これはStudent Advent Calendar 2016の25日の記事です。
qiita.com
竹です。工業高校の三年で、専攻は電気（強電です）。ひょんなことからこんなタイトル通りのことをすることになりました。
なぜこんなこと書こうかと思ったかというと大学に進学し、高校から離れるにあたり解体することになってしまったからです。
そして私が未だに完全に作りきれなかったことの研究が出来なくなったからでもあります。
これは、今後パーツや素材を一から集めれた時の備忘録＆これからメカトロニクスや倒立振子を始めたい人のために書きました。あと残念ながら訳あってソースコードはお見せできません。すいません。ですがそのうち載せたいと思います。
ちなみに私自身元々は自称ソフト屋さんでマイコンを取り扱うことや弱電は一度たりともありませんでした。完全に電子工作及びメカトロ初心者で、工具の使い方ひとつでさえわかりませんでした。
メカトロニクスについて勉強したいなーと思う人は僕みたいなフローを辿るといいかもしれません
以下に本記事の中身を記していきます。拙い文章ですが気長にお付き合いください。
概要
大会に出すのに人の役に立つものを作りたいと考えました。
その中のアイディアとして自立移動する杖があればじじばばが楽になるのではという理由から自立する杖を作りたいと思いました（適当）
ちなみにこれをやったのは二年生の後半から3年の前でした。
倒立振子とは？
読んだ名の通り「逆立ちする振り子」です。普通の振り子は支持点が重心の上方に存在するのに対し、倒立振子は支持点が重心の下方に存在します。
つまり、箒を手のひらでおっとっと〜と支える感じです。
倒立振子が倒れないようにするには，常に支持点を重心の真下に位置させる制御技術が必要です。
つまりあれです。箒が倒れそうな方向に走っていくと倒れないアレです。
取り掛かってみる
しかし結局検索して出てきた知識を使いたくてもどうやればいいんだこれでした。全くわからん。
というわけで先行研究を調べました。
どうやら東北学院大学の熊谷教授が研究しているそうです。（地元なのに知らなかった。。。。）
論文：玉乗りロボットの開発 第一報 ロボットの実装と基本制御
https://tohoku-gakuin.repo.nii.ac.jp/index.php?action=pages_view_main&active_action=repository_action_common_download&item_id=136&item_no=1&attribute_id=22&file_no=1&page_id=34&block_id=86
（それに乗っかるつもりで）これを読み込みました。ちなみにメカトロ知識ゼロです。大学数学もわかりません。ですので並行して自動制御とメカトロについても勉強しました。
http://www.mech.tohoku-gakuin.ac.jp/rde/contents/sendai/mechatro/archive/RMSeminar_No09_s8.pdf
などがあるこれまた東北学院大学の熊谷教授の資料が初心者向けで最高です。以下のインデックスから漁ってください。
Index of /rde/contents/sendai/mechatro/archive
また
センサの基本と実用回路 (計測・制御テクノロジーシリーズ)
Amazon CAPTCHA
など難しい知識が必要な時はこういう本を読んでました。
基本方針は上で紹介した熊谷教授の公開なされているpdfを読んで、たりなきゃ考えればいいみたいなのが一番よかったと思います。
正直そんな難しいと考え込んでしまってしまっていたのが失敗なので是非楽に考えましょう。
ちなみに今玉乗りロボットは僕が始める時にはなかったこんな知識の宝庫みたいなサイトがあるみたいです。羨ましい。。。。
玉乗りロボット BallIPMini 技術情報
さてさて。どうすればいいかはなんとなくわかってきましたがそれでもモーターでさえ一つ回したことありません。
論文中にはステッピングモーターを使っています。まずは回すとこからスタートをしました。
主に使用したもの電子デバイスは
ステッピングモーター
ArduinoMega
MPU6050(６軸センサー（加速度とジャイロ））
を使いました。
また、なめらかオムニホイールと呼ばれる軌跡が一つになる特殊なオムニホイールを使用しました。
ステッピングモーターを回す
こういう記事ととかみたりどんなのあるのかメーカサイトを見たり。
ステッピングモーターの基本（前編）〜Arduinoでパーツやセンサーを使ってみよう | Device Plus - デバプラ
駆動方法を調べたりしました。ぶっちゃけモーター駆動とモーター回路が一番難しくて一番大変です。しっかりと理解しましょう。
モータードライバは細かい動きをさせるマイクロステップをやりたいが故に市販のを使いました。
akizukidenshi.com
これまた僕が回路周りめっちゃ苦労した時にあったら最高だったなというサイトがありました。
この回路設計まんまパクってもいいねというぐらいいいと思います。
1500円で作る！CNCフライス用ドライバー | 自作工房
モーターは色々使ったんですがこの辺がすごくおすすめです。
バイポーラ ステッピングモーター １７ＰＭ−Ｋ０４４−ＡＫＺ: パーツ一般 秋月電子通商 電子部品 ネット通販
ちなみに回路周りなのですが紛失しました。。。。ごめんなさい。。。（手書きなのでどこに置いたか。。。）
さてさてなんとなくモーター周りを理解したところで「倒立振子って結局技術的にはなんぞや」というところと「６軸センサー（角速度と加速度）の制御」、「機体の設計」の足場を固めながら三点に取り掛かります。
倒立振子って結局技術的にはなんぞや
論文読んでもネット見てもわからん。
今思うに、初心者には論文で使うような真面目な理論は難しすぎる。でもネットの半日でできるなんちゃらとかはの理論抜きでやりすぎてるのである程度しっかり知りたい人には全くわからない。つまりまぁ慣れろってことなんでしょうけど。
とりあえず調べていくと平然と
”まず車体へ働く力を物理的に把握することが必要です。運動方程式を立て行います。”
とか言われます。がこれらは現代制御法の場合必要です。
ですが今回は論文に沿うことにしたので私は今回はPD制御による加速度制御、簡単に言えば古典制御でのアプローチをしました。
古典制御とは
偏差（修正すべきずれ）に対して比例制御、積分制御、微分制御これらを組み合わせアプローチを行う制御。
イメージとしては、
比例制御：偏差（誤差）に対して比例させて治すアプローチ。
積分制御：偏差（誤差）がどんどん溜まった際にまとめて打ち消すアプローチ。
微分制御：起こりゆく誤差に対して起こすアプローチで、少し偏差（誤差）を打ち消したい時に行うアプローチ。ズレ始めとか強く効く。
こんな感じです。
もう少し具体的に説明をします。
距離、速度、加速度これらの関係は微分積分で変化させることができます。距離を二階微分すると加速度になり。で逆に二重積分すれば距離になります。この時微積分でのΔになる部分は”t：時間”です。つまるところ、微分で成分に対して制御をし、積分でまとめての大味な制御をすることができます。つまり時間のP,I,Dこれらは次元ごとの制御をしてるとも言えます。
さてここまで理解したところで不思議なことに気がつき、疑問を持つ方もいるかもしれません。なんでPD？PIDじゃないの？という疑問です。普通に考えたらPIDの方が全部に対してやっていて良さげですよね。ですが今回の使用しているモーターはステッピングモーターです。トルクに対する指令ができません。できるのは回転速度に対することのみです。ですので角速度と角加速度のみが制御可能なので、PD制御による加速度制御となります。
具体的な制御について
この図はロボットに用いた,センサによるロボットの姿勢角と車輪の移動量を用いる典型的なPDフィードバックのブロック図です。
制御部は以下の式で表されます
aは操作量で球の外周の加速度であり、θは各軸の方向の傾斜角x,yは車輪の移動量、vは速度である。添え字x,yはそれぞれの軸に関する状態量(距離)であること を示し,K*は実験を行い調整した定数制御ゲインである。またvとx(y)はaを数値積分することで得られました。
なおこの場合はx,y,vx,vyはワールド座標系ではなく球の回転量です。
一般的に倒立振子はトルク制御の方が有名ですが、今回はステッピングモータ を利用したというのことから加速度制御を採用したということを前述したと思います。
今回採用したPDフィードバックは、センサーノイズがダイレクトに操作に関わってくることが一般的に知られています。（よくわからない場合は熊谷教授のPDFを参照）
そこでセンサー対策として簡易的な一次デジタルローパスフィルタを作成し、センサデータをリプル状のノイズを消すようにしました。
パラメータはrのみで、入力を比率rをゲインとする特性を持っている。r=1のときは、ただの素通し、r=0のときは、y[k]=y[k-1]となるつまり、値を維持となります。r=0.1など小さい値にすれば変位値が元の値に寄っていながら増えるというものです。
実際に制御とデジタルローパスフィルタ(LPF)をソースに書き起こしたものが以下のソースです。なおKsという記述はゲインの入れ込んだ配列であり、LPFのゲインは実験的に求めたものです。
thx -= thofx; thy -= thofy; //PD制御
acxt = (Ks[0] * thx ) + //角度
(Ks[1] * thvx ) +//角速度
(Ks[2] * distance_x - xofs) ) + //目標距離
(Ks[3] * vx); //速度
acyt = (Ks[0] * thy)+
(Ks[1] * thvy)+
(Ks[2] * distance_y - yofs) +
(Ks[3] * vy );
//ローパスフィルタ //LPF:y=(1-r)y+ru(u入力、y出力、rフィルタ係数r<1)*(2^n/1) #define LPF_f(y,u) y = (LPF_A*u)+((1-LPF_A)*y)
#define LPF_A 0.05
LPF_f(acx, (acxt-acx));
LPF_f(acy, (acyt-acy));
/*積分*/
vx += acx; vy += acy;
distance_x += (vx); distance_y += (vy); MotorSpeedX = (vx);
MotorSpeedY = (vy);
このような形になります。
「６軸センサー（角速度と加速度）の制御」
今回はMPU6050と呼ばれるデバイスからセンサデータを取得し、角度を得ました。
MPU6050は少しだけ他のデバイスと異なっていて、DMP (Digital Motion Processor) という機能を使うことで、補正済みのデータを簡単に利用できるのが特徴です。一般にジャイロ（角速度センサ）の特性は温度によって変化し、ノイズも多く乗るので適切な処理が必要とされています。
私は角度を得るために今回DMPのバージョンとカルマンフィルタのバージョンを作りました。
DMPのバージョンは多くの記事があるので割愛して、初めてセンサーを触る人のために簡単に分解能についても話し、その後カルマンフィルタについて少し説明したいと思います。
分解能
このMPU6050などセンサーを調べている際にLSBという文字を目にすることがあります。LSBとは最下位のビットではなく（私が勘違いした例なんですけど）制度を論じていて、分解能という意味です.
分解能の概念は以下のサイト曰く、1メートルの物差しに1ミリメートル単位で目盛りつけた時の目盛りが、分解能だそうです。
分解能 - National Instruments
詳しくは以下のサイトなどがよくわかりやすいのかと思うので査収ください。
www.widesnow.com
カルマンフィルタ
カルマンフィルタは逐次ベイズフィルタの一種で、オンラインのフィルターです。簡単にいうと確率論をベースにしたフィルターで、昔のアポロ計画、カーナビ、今回のように角度を求めるなどなどの様々なところで利用されていて、モデルが線形である（または線形近似したもの）ことや，ノイズがガウス分布である必要があります。
基本的には
- 状態空間表現(動作モデル，観測モデル)
- 予測ステップ（事前推定値，事前誤差共分散）
- フィルタリングステップ（カルマンゲイン，事後推定値，事後誤差共分散）
の三つを基本としています。
簡単にざっくりいうと対象のデータにはホワイトノイズが乗っていてその際にはどれがどのノイズかがわかりません。
そこでデータの中間を取り、それに対してその時共分散という値の関係する度合いを用いて、誤差に寄与率加えて、センサーのノイズを薄めるという手法です。
今回の場合、なぜ使うのかというと、角度を求めるのには加速度センサーを使うことで求めることができ、止まっている時は重力加速度のみなので、十分に値が取ることができます。ですがそれだけでは並行に移動し加速（以下並進加速度と呼ぶ）している時に思わぬ値が出てしまいます。そこで、角速度センサーの値を用いて、移動している時は角速度センサーの値を用い、相補的な方策で立てて対抗をすることができます。
しかし、角速度センサーには弱点があり、ドリフトと呼ばれる値のズレが存在します。それに対してのアプローチの方法がこのフィルターなのです。
具体的には
- ジャイロにドリフトあるが、加速度センサには誤差ないとしたときがある。⇒この時は、誤差の要因は全てジャイロであり加速度センサの値を正解として補正をしたい。
- ジャイロにドリフトないが、加速度センサには誤差あるとしたとき⇒この時は、誤差の要因は全て加速度センサであり加速度センサの値は無視したい。
という判断をしていかなくてはいけません。
もし詳しく知りたい場合は以下の記事を見てください（私の説明が正解なのか怪しいので）
qiita.com
さて、以下にソースコードを示します。なお、このソースコードは以下のサイトを参考にしました。
TKJ Electronics » A practical approach to Kalman filter and how to implement it
gist.github.com
gist.github.com
このような形になります。ぜひこれはソースコードと参考サイト、上での紹介した記事をぜひ読み込んで理解してください。
機体の設計
結論としては、球乗りの機構を実現するためにオムニホイールをモータ軸につけ、等間隔に3軸を並べそこへ球に乗せる形を作りました。またギアによるバックラッシをなくすためにステッピングモータを使用し、それをダイレクトドライブにしました。それ故移動制御 がパルス分だけの移動なので確実に移動距離を把握できるようになりました。
言葉ではわかりにくいので以下の実際の球乗りの台座を写真aと 写真bを参照してください。
ちなみにbは改良を行い捻りを加えサイズの大きいモータでも十分にコンパクトな サイズにできるようにしたものです（論文中と同じですね）
さて。モーターが三つなので少し複雑になりました。そのためにx,yの制御をに変換しなくてはいけませんですので、
まずはX,Y軸の制御を変換するために球駆動のための車輪速度の算出を行いました。
車輪速度の算出
始めに球の中心を原点とする局所座標系を定義します。
(図a)この座標系はロボット本体に固定し,z軸は球の中心とロボットの質量中心を通る鉛直線として,x,y 軸はz軸に垂直に定義します。
1.球の角速度ωを得るための車輪の速度を導くために、車輪iと球の接点をPiそ の位置ベクトルをpiとする.球の角速度ω対応する,Piにおける球の周速度viは次式で得られます。
2.球の周速度vと車輪の周速度vsの関係を導出する。(図b)最初に右の図の ように速度vを車軸に平行な成分vfと垂直な成分vsに分解する。vfは車輪外周 の樽型の小さいローラの自由な回転によって生成され、vsはモータの回転によ る車輪の周速度である。このvとvsは,車輪の駆動方向単位ベクトルsによって対応付けられ、
となり、vsの正負は車輪の回転方向を示す.各車輪の位置piと方向siを定めることにより、角速度に対応する車輪速度が求まります。
3.vsの正負は車輪の回転方向を示す。各車輪の位置piと方向siを定めることにより、角速度に対応する車輪速度が求まり、車輪の速度の基本式が
とわかります。
4.3の式よりモータ3つの式を求めていく。天頂角φ、rを球の半径と置く時、位置ベクトルpiは以下となりました。
5.駆動方向siは球の俯瞰図及び車輪の上面図(図a)より
となる。
6.x軸周りに角速度ωxで回転させる時(ω=(ωx,0,0)とする時)サラスの公式を 用いて4.、5.より求めました。
なお単位ベクトルの和は
とします。
7.同様にしてy軸周りに角速度ωyで回転させる時(ω=(0,ωy,0)とする時)を求めました。
以上よりx方向とy方向へのベクトルを分解する式を得ました。
これをソースに書き起こす際は以下のようにしました。
なお以下で√3/2を近似で扱った理由としてはマイコンのスペックを考慮した故です。
//x>>1は1/2、√3/2~=222/256->√3/2*256=221.7という近似で扱っている Moter_action_1 = -((long)MotorSpeedX ) + 0;//モータ1 Moter_action_2 = ((long)MotorSpeedX >> 1) - (((long)MotorSpeedY * 222) >> 8;)//モータ2 Moter_action_3 = ((long)MotorSpeedX >> 1) + (((long)MotorSpeedY * 222) >> 8);//モータ3
これらを用いて加速度的にモーター駆動を行いました。
この時加速度的というのは自分なりでいいのでオレオレ加速度（つまり自然界の尺ではなく相対的なものでもいい）を作りモーターの駆動を行いましょう。
意味がわかりにくいと思いますが、V=V0+atみたいなことになれば良いのです。ここがかなりキモになります。
もしピンとこない場合は以下のソースコードを読んでください。モーター駆動の記述でオレオレ加速度を設定しているのがわかります。
self balancing arduino robot
機体の作成
せっかくなので、初めて旋盤やポール盤、シャーなどを使い、色々試行錯誤した後も含め見てもらいたいと思います。
１番最初は、
このような形で鉄を曲げてそれに小型のステッピングモーターを入れました。
ちなみにアルミの柱、オムニホイールのハブはアルミの廃材より全て手作りです。（ほんと苦労した。。。。）
ですがこれでは若干のトルク不足など応答性が低いということがありました。その後もこれをベースに何度か改良をするのですが、残念ながらうまくいきませんでした。
下の写真は改良の跡で、写真の上には、球を止めるための機構で、キャスターで使うような玉がついています。
改良を行った際にピラミット型にしてその表面三つにモーターを設置できないかとアプローチした後です。
その後思い切って下のベースごと変えました。
この時ねじりを加える工夫を行い、モーターも通常のステッピングモーターへと換装しました。
ちなみにモーターのブランケットも手作りで、
このような台紙をCADで作り、それを元に廃材のアルミ板を切り、曲げて、下のようになりました。
中間はまだ安定してないのでスカスカでした笑
そして上部には下からモータードライバー、マイコン、センサーと設置しました。
そうして現在の改良を行った形へとなりました。
まとめ
ここまでお付き合いいただき有り難うございました。
肝心のこれって立つのという話ですが、まだ5〜15秒ぐらいしか安定しません。最長で30でした。何が悪いのかわからんので今後の課題です。
それと、実はこのロボットの機構には続きがあって、”ステキなすてっき”という親父ギャグの効いたシステム名をつけてもらい、まさかの友達にロゴを作ってもらうなど、いろいろなことをしました。
また、スマホで動かすためにespwroomというWi-Fimoduleを繋いでいて、iOSアプリサイドのお話も実はあるのです。また、espwroomもArduino化していて、温湿度と気圧をはかり、ミスナールの式で体感温度や気圧の変動のスパンで簡単な天気予報をしてくれる機能がついている。といったまだまだ実はネタがありますが、それはぜひまたの機会にとでも思います。もし気になる方がいれば説明書を以前作ったのでぜひ見てください。
Suteki-Na-Stick/SutekiNaStick取扱説明書.pdf at master · Suteki-Na-Stick/Suteki-Na-Stick · GitHub
これを手伝ってださった先生や、関係している方々、所属してる部活動の人たちに感謝の気持ちを申し上げます。
また、まだまだ初学者の身分で偉そうなことを言っていて、これにも間違えがあるかもしれません。もしよければ優しくご指導ください。
最後になりますが、これを通じて何かお互いに拙い文章の中でも学べるものがあれば幸いと感じます。
Studentアドベントカレンダーお疲れ様でした！！！（結局当日の朝4時までかかったので遅刻（白目））
