カテゴリー : 倒立振子ロボット

倒立振子ロボットの筐体完成

筐体が完成

ステッピングモータの結線はまだだけど、取りあえずご覧のように倒立振子ロボットの筐体が完成した。

完成した倒立振子ロボット

完成した倒立振子ロボット

やっとこれで、まともにプログラムが組める。

倒立振子ロボット用Arduinoシールドの通信

ヤバイ!!やつだ

倒立振子ロボット用Arduinoシールドの半分を占めるBluetoothボードの話題。

シールドの半分を占めるHC-05

シールドの半分を占めるHC-05

HC-05は中国のショップで単価数百円で通販で購入したBluetoothモジュール。いつか使おうと大事に仕舞ってたのだが、技適をうけてないので電波を流すだけで違法!?と教えて貰って、泣く泣くお蔵入りした逸品なのだ。

未練があるばんとさんは、スマホを持ってHC-05の電波状態を確認してみたのだ。自宅のマンションの部屋から出ると見失ってしまうほど電波が弱いのが確認された。HC-05にとって自宅は電波的な暗室のようなもののようだ。

お蔵入りするのは忍びないので、実験であり且つ電波的な暗室で使うということでHC-05を利用することにした。

倒立振子ロボットをどこかでお披露目するとかだったら、そのときは技適を受けてるBluetoothデバイスに入れ替えるということにしよう。

早速の通信テスト

ジャイロセンサと加速度センサのデータからカルマン・フィルターで算出した傾斜角度をBluetoothの電波に乗せてみた。

Teratermで受信したのがこれである。

TeraTermで受信

TeraTermで受信

さらに先月号のトラ技にも記事があったが、Andriodスマホ(IS05)で受信したのがこれである。

スマホで受信

スマホで受信

ばっちりだ。

何故無線通信に拘ってるかというと

前回mbedで倒立振子ロボットを作ったとき無線(Bluetooth)の必要性を感じたのだ。というのも、倒立振子ロボットを安定して立たせる為には、姿勢制御のPIDの係数など細かい修正が必要になってくる。
有線では自由な運動が阻害されてきめ細かい設定が難しいのだ。電波で制御するのならなんでもいいのだが、Bluetoothにしとくとスマホをコントローラにすることができて都合が良い。

倒立振子ロボット用Arduinoシールド製作

シールド製作

倒立振子ロボット用Arduinoシールドを製作した。
出来上がったのはこれである。

倒立振子ロボット用シールド

倒立振子ロボット用シールド

ご覧のとおり山盛りだ。載せた部品は以下のとおり、

  • ステッピングモータ制御端子
  • ジャイロセンサ・モジュール
  • 加速度センサ・モジュール
  • 通信用Bluetoothモジュール
  • 3.3V-5Vレベルコンバータ

Arduinoシールドは意外と小さい。片面プリント基板はコンパクトに製作するのは難しいので、苦手なユニバーサル基板で製作することにした。最近は安価でArduino専用のユニバーサル基板が売り出されてるが、当方Arduinoに関して興味を失ってたので購入していない。

従来通り、秋月のCタイプのユニバーサル基板で製作した。シールドとして使える秋月Cタイプユニバーサル基板に部品を載せたとろいっぱいになってしまった。接続できないところはジュンフロン線にて結線した。

ユニバーサル基板での配線は難しい

がた老さんはユニバーサル基板での配線を楽しそうにされてるのだが、当方は苦手なのだ。 :cry:
手順は以下のとおり、配線する前には明確な配線図を準備する必要がある。

  1. 回路図を製作
  2. 配線図を製作
  3. 配線

がた老さんは手書きで配線図を作られてるが、当方はEagleを利用して配線図を製作している。

これが回線図

回路図

回路図

そして配線図

配線図

配線図

プリント基板なら半日でできるのだが、完成まで2日も掛かった。 :oops:
作っている内に苦手な理由が分かったような気がする。短気が原因なのだろうと思う。

テスト

動作テスト

動作テスト

製作に2日も掛かったシールドの動作テストである。
ジャイロ+加速度センサのスケッチはあるのでこれでデストである。一発合格である。

BluetoothモジュールHC-05である。HC-05は3.3Vのモジュールなので、コンパクトな秋月の3.3V-5Vレベルコンバータを介してArduinoと接続した。HC-05はArduinoでは使ったことがないので、これは初テストである。HC-05ライブラリィをダウンロードしてテストした。これも一発で合格である。

カルマンフィルターを移植

LPC1114FN28 mbedへ移植

Arduinoでテストした倒立振子ロボットは、モータのトルクの不足で自立できないという残念な結果になった。気分一新でもないが傾斜角度を把握するのに利用したカルマンフィルターをLPC1114FN28 mbedへ移植してみた。

Arduinoの時はジャイロセンサ・加速度センサ共にアナログのデバイスを利用したが、今回はアマゾンで安価で入手したMPU-6050というジャイロ加速度センサなのだ。

カルマンフィルタを移植

カルマンフィルタを移植

LPC1114FN28 mbed + MPU-6050版も、いい感じで傾斜角を演算してくれた。

さてどうしようか

LPC1114FN28 mbedはマイブームなので、これで倒立振子ロボットを組み立てたい気持ちもあるし、Arduino版も形にしたいところだ。いっそのこと両方作ってしまおうか。

ただArduinoのモーターシールドはちょっと荷が重そうなので、代わりにモータの駆動の実績がある赤いロボコンで製作したDC-DC電源+モータコンローラ複合ボードを利用しようかと思う。

DC-DC電源+モータコンローラ複合ボード

DC-DC電源+モータコンローラ複合ボード

カルマンフィルターとPIDで制御した倒立振子ロボットが立った

姿勢制御

これまで作った倒立振子ロボットはICHROさんの倒立振子ロボットのクローンだったので、下記の状態方程式を元に姿勢制御をしていた。

モータの加速度をα、傾斜角θ、角速度ω、位置x、速度vとして

状態方程式

状態方程式

今回も、この状態方程式を利用してもよいのだが、カルマンフィルターでかなり正確な筐体の傾斜角度が得られているという前提のもと、PID制御で姿勢制御してみた。

PID制御

PID制御とは
P…比例(Proportional)
I…積分(Integral)
D…微分(Derivative)
の頭文字をとった制御方式で、現在の偏差eに比例した修正量を出す比例動作(Proportional Action:P)と、過去の偏差の累積値を元に修正量を出す積分動作(Integral Action:I)と、偏差eの変化の速度を元に修正量を出す微分動作(Derivative)で制御をする方式で、古典的と称される制御方式である。

PID制御の方程式は以下のとおり、

PID制御方程式

PID制御方程式

今回は鉛直に立ってる姿勢からの傾斜角度を偏差eとし操作量はステッピングモータの回転速度としてプログラムを組んでみた。

一応立った

何とか立った。それがこの動画である。
You need to a flashplayer enabled browser to view this YouTube video

電源コードとUSBケーブルを刺した状態では余裕で小一時間は立つようなのだが、ケーブルがないと倒れてしまう。

モータのトルク不足のようなのだ。筐体が載った状態でステッピングモータを低速で回すときには十分なトルクが必要なのだが、モータシールドとそのライブラリィではトルク制御が出来ないのだ。

色々と工夫が必要だ

倒立振子ロボット

倒立振子ロボット

カルマンフィルタ&PID制御は、倒立振子ロボットの姿勢制御には適してるようだ。
現状は一応は立ってるという状態なので、外力などによって姿勢が乱れても修正できるように、モータ周りとプログラムを修正する必要がありそうだ。

倒立振子ロボットの筐体製作

筐体再製作

カルマンフィルターを利用した倒立振子ロボットを再製作したいと思ってるのだが遅々として進まない、当たり前である筐体ができていないのだ。

mbedコントローラーが載ったロボコン号

mbedコントローラーが載ったロボコン号

前回の赤いロボコンの筐体はアクリル板をコの字に曲げで作ったのだが、この構造では取り付ける基板によってバランスを崩しやすいので、今回カルマンフィルターで参考させてもらったTKJ ElectronicsのBalandinoのように上部に基板をスタックする筐体を真似ることにした。この構造ならバランスを大きく崩すことはないだろう。

部品取り

今のところステッピングモータとかタイヤとか予備がないので、赤いロボコンの筐体から移植することにした。

これが部品取りした残骸である。余分に部品があったら分解する必要もないのだが仕方ない。上の基板がコントローラボード、下の基板がパワー&モータボードである。STM32F3-Discoveryなどの場合は、パワー&モータボードを利用することになるだろう。

部品取りの残骸

部品取りの残骸

ステッピングモーターを赤いロボコンの筐体から取りはずすとき、黄色のワイドタイヤの軸を破損させてしまった。
タイヤが空転するので、グルーガンで固定してたのが災いしてしまったのだ。不器用さここにありなのだが、使いやすいワイドタイヤを失ってしまった。 :cry:

とりあえずのターゲットはArduino版

まずはArduino版を作ることにした。TKJ ElectronicsのBalandinoがArduinoだということもあるのだが、前回も書いたように可哀想な死蔵部品に活躍の場を与えたいのだ。ジャイロセンサや加速度センサは、手持ち部品としてMEMSのI2C版もあるのだが、あえて死蔵品のアナログのセンサを使用している。

材料は以下のとおり

  • Arduino Duemilanove
  • Adafruit MotorShield V1.1(中華製のバッタもの)
  • ステッピングモータ(SPG20-1362) X 2
  • 小型圧電振動ジャイロモジュール(AE-GYRO-SMD/ENC-03R:秋月)
  • 3軸加速度センサモジュール(KXR94-2050:秋月)
  • ナロータイヤセット(タミヤ製)
  • 筐体用アクリル板
  • ネジ・ナット

モーターシールドやアナログセンサを接続するとArduinoのピンに空きがなくなるのだ。拡張性がないのでカルマンフィルターの検証で終わりになるだろう。
検証なのでArduino版の倒立振子ロボットにはバッテリ関係の部品は省略しACアダプタで駆動することにする。
姿勢が安定したらそこがゴールだ。

出来上がった筐体

これが製作した筐体である。周波数カウンタのケース製作で余った黄色のアクリル板を曲げて、タイヤハウジングを作った。その上に基板などを積んでいく単純構造だ。

今回はArduino用の一層のスタックを載せた。

Arduino版の筐体(斜め)

Arduino版の筐体(斜め)

そしてこれが上から見たところ。

Arduino版の筐体(上)

Arduino版の筐体(上)

ジャイロセンサ・加速度センサそして超音波距離センサの専用のプリント基板を作りArduinoの横に配置した。
電源はArduinoのACプラグから配給することになる。

おっと失敗だ

超音波距離センサがあれば色々なギミックができるのだが、ご覧のように超音波距離センサがAdafruit MotorShield V1.1と干渉してうまく挿らない。サイズが中途半端な余りのアクリル板を使用したのと適当な設計&製作が原因なのだ。基板そのものを嵩上げしたら解決しそうだが、とりあえずは超音波距離センサなしだ。

干渉してうまく挿せない

干渉してうまく挿せない

Arduino版は手始め

Arduino 倒立振子ロボットのキーワードでメモたんくを訪れるひとが結構いるので、作って情報を公開すると喜ばれるひとが多いかもしれない。

しかし正直なところArduinoで作ってもあまり面白みがないのだ。ほんとはSTM32F3-Discovery版などを作りたい。

Arduinoで倒立振子ロボットが作れるかも!?

死蔵庫の奥から

死蔵庫の奥から覚えのあるArduinoのシールドが出てきた。確か一昨年前、中華ショップの誤配送で注文してないシールドが届いたあれです。メールにて誤配送で商品が届いたと慣れない英語で報告すると、これまた慣れない英語で”あなたとの取引は完了済みです”の一点張り返答のメールが返ってくるだけでコミニュケーションが取れず、仕方なく商品は当方預かりでということになった。曰くありありのブツです。

謎のシールドは

謎のシールドは、どうもAdaFruit Motor Shield V1.1の中華コピー品のようで、DCモータ・サーボモータ・ステッピングモータを駆動できる多機能モータシールド。多機能なため目いっぱいArduinoのピンを占有してるので使い道が思いつかず死蔵庫入りになったのだ。改めて確認するとArduinoのA0~A5ピンとD2ピンがフリーだった。

ん!?。 8-O
カルマンフィルターでテストしたジャイロセンサと加速度センサなら接続できそうだ。

なんか図ったように収まる

なんか図ったように収まる

死蔵品の一覧

すべて死蔵品。Arduinoは、センサ類のテストには活躍してるが、考えてみるとこれで何か作ったことはないのだ。

すべて死蔵品

すべて死蔵品

ステッピングモータと電源を何とかすれば、それなりのものが出来そうだ。
死蔵品に活躍の場を与えることができる。これで成仏!?できる。 8-)

ジャイロセンサ その2

ジャイロセンサのドリフト

倒立振子ロボットを倒れないように制御する為には傾き角度を把握する必要がある。
傾きを関知するセンサとしてはジャイロセンサと加速度センサがあるのだが、ジャイロセンサは姿勢の変化を数値に表し、加速度センサは加速度の変化を数値に表すものなのだ。

ジャイロセンサは姿勢の変化(角速度)を積分し傾き角度を算出する。一方加速度センサは、地球上で常に鉛直方向に働いてる重力(加速度)を関知して傾きを算出することができる。
ただし水準器のような静止している状態(重力以外の力が加わってない無い状態)なら正確な傾き角度を算出できるのだが、倒立振子ロボットのような常に動いている物体には重力(加速度)以外の力(加速度)が働くので傾き角度を把握するのが難しいのだ。

傾き角度を求めるにはジャイロセンサの出番となるのだが、ジャイロにはドリフトというやっかいなものあるのだ。

LPC1114FN28でSTM製のジャイロL3GD20を接続して適当にジャイロセンサを傾けた記録がこれだ。

センサ単体の環境

センサ単体の環境

角速度

角速度

角度

角度

角速度は0度を中心に振れているが角度は徐々に増えている。ジャイロが壊れている訳ではないのだ。これがドリフトなのだ。

ドリフトの補正

ネットで倒立振子ロボットを格好良く立たせている方々は、何らかの方法でドリフトの補正をしているはずだ。
倒立振子ロボットの基礎を学んだIchiroさんはドリフト補正に最小二乗法予測を用いてドリフトを予想して補正してたのだが、最近カルマンフィルターという手法があるのを知った。

今回はカルマンフィルターを利用してみようかと思う。

カルマンフィルターの検証

大学で制御工学などを学んでたらカルマンフィルターはあれか!とピンと来るかも知れないが当方は無知なので良くは知らない。

幸いにもISHIDAさんのHPに”秋月 小型圧電振動ジャイロモジュール AE-GYRO-SMD/ENC-03R の使い方“という記事があり、Arduinoの環境で小型圧電振動ジャイロモジュール(AE-GYRO-SMD/ENC-03R)
のドリフトを3軸加速度センサモジュール KXR94-2050を用いたカルマンフィルターで抑える実験と検証をされている。

手持ちの死蔵部品庫には3軸加速度センサモジュール KXR94-2050はなく、ひとつ前の3軸加速度センサモジュール KXR52-1050ならあるので、これを用いて実験してみた。

カルマンフィルターの実験

カルマンフィルターの実験

カルマンフィルター適用したデータがこれだ。

カルマンフィルター適用のデータ

カルマンフィルター適用のデータ

ジャイロのみの場合は明らかにドリフトの影響を受けているが、カルマンフィルター適用した後のデータは収束している。

カルマンフィルターの評価

ISHIDAさんも書かれてるが、ENC-03Rは細工しても個体差が結構あり扱いが難しいセンサなのだが、加速度センサを利用しているとは云え使えるデータになっている。
ジャイロセンサと加速度センサ共に載っているSTM32F3-Discoveryもあるし単体の加速度センサも入手済みなので、カルマンフィルター適用して姿勢制御をしていこう。

ジャイロセンサ その1

ジャイロセンサ付きのお得ボード

ジャイロセンサ付きのボードが数種類集まった。
元々はSTM32F3-Discovery(ジャイロ&加速度+磁気センサ付きのCortex-M4のお得ボード)でARM32の勉強がてら倒立振子ロボットを作ろうかと思ってた。昨年末FREESCALEのFRDM-KL25Zを勢いで購入したのだが、これまたジャイロセンサ&タッチセンサつきのお得ボードだった。さらにSTMF3-Discoveryを秋月で注文したとき、STMのMENSのジャイロ&加速度センサも購入しているのだ。

倒立振子ロボットが沢山作れそうな状態なのである。

倒立振子ロボットを作れそうなボード達

倒立振子ロボットを作れそうなボード達

STM32F3-Discovery

まずはSTM32F3-Discovery。これは秋月で950円で販売されてるお得ボード。256KBのフラッシュメモリに、センサもSTM製のジャイロL3GD20や加速度+磁気センサLSM303DLHCが付き、しかもCortex-M4なのでHard Floatが実装されているのだ。難点としては他のボードに比べて少し大きく、プログラムはC言語で組むことになるので少し邪魔くさいかも知れない。

STM32F3-Discovery

STM32F3-Discovery

FRDM-KL25Z

FRDM-KL25ZはFREESCALEのCortex-M0+のお得ボード。128KBのフラッシュメモリに、MMA8451Q加速度センサが付き、しかもmbedとして開発できる手頃な大きさのボード。KL25Zはインテリジェントなロボットカーなどの作例がネットにある。

FRDM-KL25Z

FRDM-KL25Z

センサ単体の環境

秋月で買ったSTM製のジャイロセンサL3GD20と加速度センサLIS3DHを利用した環境だ。写真はmbed化したLPC1114FN28での環境だ。I2Cで駆動したテストプログラムは32KBのフラッシュメモリのうち14KBぐらいなので、これを用いても倒立振子ロボットを製作が可能だろう。

センサ単体の環境

センサ単体の環境

どれか適当なボードなのだろうか…

一昨年倒立振子ロボットを作ったとき参考にさせていただいたIchiroさんのLIBRA-ZEROのPIC版。そしてmbed LPC1768版を作ったのだが、mbed LPC1768はネットワークLSIを積んでいる為か消費電力が大きく電池での駆動が難しいのだ。

倒立振子の姿勢制御だけだったら8ビットのPICやAVRでも十二分なのだが、さてさてどれを選んだらよいのだろうか悩ましいところだ。

倒立振子ロボットの制御プログラム

VC#で倒立振子ロボットの制御プログラムをでっち上げた。姿勢制御のグラフにはもう少し手を入れるが基本はこんな感じ。VC++でアプリを書いたことはあるのだがVC#は初めて。意外と簡単にできそうだ。

倒立振子ロボットの制御プログラム

倒立振子ロボットの制御プログラム

【追記(2012/8/7)】

もう少し制御プログラムに手を加えてみた。

更に手を加えた制御プログラム

更に手を加えた制御プログラム

制御プログラムで倒立振子ロボットの状態が分かるのと各種のゲイン値を設定できるというのは効率的だ。

Get Adobe Flash player