前回、まだ安定して、とまでは行かないものの、製作中の倒立振子ロボットが立つ素振りを見せるようになりました。
ここはゲインチューニングをひたすらやっていけば、それこそwikipediaのチューニング方法とかを参考にしていけば、理論上は安定して立つはずですので、ブログには書かないでおこうと思います。（正直、思ったほどうまくは行きませんでしたが・・・）
ある程度安定して立つようにはなりますが、このままではとにかく立ち続けるということしかやってくれませんので、徐々にロボットの位置や向きがズレて行ってしまいます。
安定して立てば立つほど、この問題が顕在化しますので、位置制御機能というか、その場静止、ホバリング（とは言わないかもしれませんが）機能を追加しようと思います。
結論を言えば、行きたい方向に車体を意図的に傾ける 機能を追加することにより、位置制御ができるようになり、車体が一か所で安定するようになりました。
今回はその部分を記録していこうと思います。
動作状況
いきなり結論の動画ですが、このようにその場で立つようになりました。
まだホストPCはデスクトップPCを使っていますので、デスクトップPCからマイコンにUSBケーブルが伸びています。
倒立振子ロボットver2 （位置制御の導入）
位置制御のゲイン調整は、どんなに頑張ってもこれが限界でした。
おそらく、そもそもの姿勢制御のゲイン調整が不十分か、車体の性能、モータの特性、制御方式などの気付いていない問題があるのだと思います。理想はBostonDynamicsのHandleくらいに安定するロボットなのですが・・・難しいですね。
（参考）
Introducing Handle
では、実際にどのように実装したのか、概念だけでも説明しようと思います。
最初の試み
どうも倒立振子をホバリング、つまりその場に留まるように位置制御をするって、あまり表に出てこない取り組みのようで、先人の取り組みを見つけることができませんでした。
探し方が悪かったのかもしれませんが・・・
ただ、なんとなくですが、車体が傾いたら、傾いた方向に車輪を回して車体の姿勢を立て直すのが基本的な動作原理ですから、それを逆手に取り、行きたい方向に車体を意図的に傾けるようにすれば、自然と車体はその方向に移動するような気がします。
そのつもりで、基本的な制御プログラムを実装した際、（あるいはこちら）に、倒立の目標角度（当時は目標姿勢角と呼んでいたかも）を固定パラメータとせず、時々刻々更新可能な値として、ホストPCから逐次送信する形に整備しておいたのでした。
ホストPCでは、マイコンからエンコーダ情報を取得し、車輪の回転量から車体の移動量を常に計算・保持しています。
この部分は以下のサイトを参考にして、エンコーダ情報をSubscribe → ROSの /Odometry メッセージとしてPublish するROSノードとして実装しました（絶対に既存の安定したパッケージがあると思うのですが、見つけられず・・・）
参考にしたサイト
myenigma.hatenablog.com
自作したホイールオドメトリの計算パッケージ
github.com
今回作っている車輪型倒立振子は、
- 進行方向向かって左右に向かい合って配置された2つの車輪を別々に駆動する
- 左右車輪を同じ方向に回せば前進・後退、どちらかを多く回せばそれと反対方向に旋回、互いに逆方向に回せばその場旋回
ができる差動二輪とか対向二輪と言われるタイプのロボットで、オドメトリの計算は割と単純です。
ルンバとかも、基本的には同じ構造ですね（従動輪としてキャスターが付いていて、制御しなくても安定しますが）
とにかく、これで初期位置からのずれを常に記録できるようになりましたので、
- 前進・後退（並進）方向の位置ずれを取得
- 旋回方向の位置（角度）ずれを取得
という流れが完成しました。
旋回方向の制御については説明していませんが、理論上、車輪型の倒立振子をその場旋回させても、そのバランスに影響は及ぼしませんので、単純に旋回させたい場合はモータドライバに送るPWMを左右逆方向にオフセットさせる実装としました。
並進方向の制御だけ、うまくいかなかった
本当に理由はわからないんですが、上手く動きませんでした。
旋回方向は非常にうまくいって、実は前回の動画では長時間動かすとどういうわけか徐々に左に旋回していくクセがあったのですが、それがピタリと収まるようになりました。
元々この制御方式自体、直感的にこうじゃないか？ と思って実装したので、もしかしたら根本的に間違ってるのでは・・・と思いました。
第２案として実装したのは、位置がズレたら、PWM出力値にオフセットをかけ、元に戻る方向に車輪が回りやすくするという方式でした。
位置がズレている時に無理に戻そうと車輪を回してしまうと、車体が行きたい方向と反対方向に傾いてしまうので、直感的には上手くいかなさそうで、案の定、上手くいきませんでした。
戻したら、なぜか上手くいった
これまた本当に理由はわかりませんが、最初に考えていた方式に戻したところ、上手くいくようになりました。
どこも変えていないと思ったのですが・・・PID計算で正負を逆にするとか、そういう些細かつ致命的なミスがあったかもしれません。
とにかく、冒頭で動画で紹介したように、まだ完全に安定とはいかないものの安定してその場に立ってくれるようになりました。
同じ方式を使えば、遠隔操縦も容易にできると思うので、次回にでも試してみようと思います。
完全に静止しないのは、まずは位置制御以前に姿勢制御の精度が良くないか、ゲイン調整が不十分なのだろうと思っています。
ゲイン調整はこれ以上追い込める気がしなかったのですが、姿勢検出、モータ駆動等、誤差の要因は山ほどありますので・・・
それはさて置いた状態で、位置の偏差を元にPID制御を行っています。
Pゲインだけだと全く収束せず、1mくらいの、部屋内で実験をするにはあまりにも広い範囲で往復運動をし続けます。
この時点で、上述の姿勢制御の方を改良すべきなのかも知れませんが、ちょっとどこから手を付けて良いものやらわからなかったので、往復運動はDゲインを上げて無理矢理押さえ込んでいます。
ゲインを上げ過ぎると強い外乱が入った時に挙動がおかしくなるのですが、丁度良い値にすることで概ね、PCの脇で倒立振子ロボットを立たせたままブログを執筆できる程度には安定するようになりました。
おまけ機能
無邪気に倒立振子を作れば、重心が綺麗に車体の中央に位置するとは限りません。特に倒立振子が振れる車体の前後方向の重心位置のズレは、倒立の目標角度が鉛直上向きとは限らない ことに繋がります。しかも、バッテリの取り付け具合や機能追加にしたがって、毎回重心位置が一定とも限りません。
（下図では、重心が画面右方向にずれている場合を示しています）
そして位置制御により一箇所にとどまる機能を追加した場合にも、重心位置がズレていれば、留まれる位置自体も、その目標からオフセットしてしまうでしょう（下図④、⑤）
一方で制御上の中立点（この場所にいれば、位置制御のために制御入力をしなくて良い位置。下図①）では、制御入力が無いにも関わらず、重心のズレにより画面右に車体を傾け右進させる力が働いてしまいますから、理想的な状態に比べて、往復運動の幅が広い、すなわち位置制御の精度が下がる気すら致します。
そこでこの部分のおまけ機能として、重心位置キャリブレーション機能を実装しました。具体的には、
- 特定のトリガーを入れる（今回の場合、ゲームパッドで特定のボタンを押す）
- 5〜10秒間の間、往復運動の状態を記録し、往復運動の中心点を求める
- 目標位置と往復運動の中心点との差分 = 重心位置のズレの程度として、適当なゲインを乗じて倒立の目標角度をオフセットする（上図⑥）
という手順になります。
焼け石に水の感はありますが、往復運動の中心点が間違いなく目標位置と重なるようになり、往復運動の振幅も小さくなり、位置制御の精度が向上します。
まとめ
倒立振子ロボットに位置制御機能を追加し、放置しても長時間その場に留まるようになりました。
更におまけ機能として、（不完全な）位置制御による往復運動から、ロボット本体の重心のズレを検知し、倒立の目標角度をオフセットする機能を付加し、倒立の安定を図りました。
これらの機能は、車体制御ノードとして実装しています。
github.com
相当にゴチャゴチャな上、次の記事で説明する速度制御機能も入っていたりして、とてもリンクを貼る意味があるとは思えませんが・・・
ちなみに、おまけ機能は、calibrate_initial_target_angle() という関数で実装されています。"initial" とつくのは、位置制御に加えて、別の記事で説明する速度制御のために、倒立の目標角度を更に変化させる羽目になるからです。あくまでこの関数で操作している target_angle は、安定して倒立させるためのベースの角度となります。
次回は、たびたび言及している遠隔操縦の導入（速度制御の導入）の説明をします。
ktd-prototype.hatenablog.com
