前回は製作中の倒立振子ロボットに位置制御機能を追加し、長時間倒立させても安定して立つように再調整したところでした。
このままだとただ立っているだけで面白くないのと、そもそも自分でも忘れていましたがホームロボットを作りたいという欲望があったような気がしますので、最低限部屋内を自由に動き回れる必要があります。
ホームロボットであれば自律走行ができなければ始まらない気がしますが、そんなこと言われてもいきなりそんなものは作れませんので、とりあえずは遠隔操縦によって自由に動き回れるようにしてやろうと思います。
どうやって操縦指令を与えるか
これについては悩む必要は特にありません。ROS純正（？）でジョイスティック（ゲームパッド）を使えるパッケージがありますから、そちらと、対応するゲームパッドを使います。これについては、近藤科学のサーボモータをROSのシステムから動かせるようにした際もデモ用に使っていたゲームパッドを再利用します。
gaming.logicool.co.jp
以前、920MHｚ帯の方が通信距離が長いということで、920MHz無線通信評価用モジュールを用いたロボット遠隔操縦用リモコンを自作したこともありますが、楽に用意できるものが一番です。
当時はモジュールから送られてくる改行コード（？）か何かの処理が当初わからず、通信はできているのにその受信処理→操縦指令として解釈・・・の部分が上手く行かず苦労しました。1ヶ月くらいずっとやってた気がします。
www.interplan.co.jp
ゲームパッドの左右のスティックについて、上下左右、合計4軸の入力を-1.0〜+1.0で取得できますので、こちらに適宜ゲインを乗じて、操縦指令としていきます。
制御はどうするか？ 位置制御の拡張？
前回位置制御を導入したところでしたので、これをこのまま拡張すれば良いのではないか？ というのが最初の考えでした。
大抵のロボットは遠隔操縦の時は速度を目標値に与えるのでは？ ROSでも/cmd_vel みたいなメッセージがあったような気がするし・・・という考えも頭のどこかにはあったのですが、実行には移しませんでした。
位置制御機能では、現在位置を目標位置としてその場にとどまろうとする制御をしていました。（移動しないので、(x, y, θ) = (0, 0, 0) が目標になりますが）
従って、ジョイスティックの信号に合わせて、この目標値自体を動かしてしまえば、一応遠隔操縦機能が実装できる気がしました。
（ここから先、実験結果がどうだったか正直記憶が曖昧なのですが）
最初に実装した時は、ジョイスティックで目標位置を変えてその方向に動くところまではなんとか上手く行ったんですが、どうも動きが鈍い。
しかも遠隔操縦中に目標位置を新しい位置に更新するのを忘れていたため、ジョイスティックから手を離してしまうとロボットが一目散に元の位置に戻る始末。
その辺りを修正している時に気がついてしまったんですが、旋回はどうするんだ？そして斜め方向に移動したい場合はどうしたらいいんだ？ という問題でした。
この右側のような目標位置を与えられた時に困るわけですね。点線の矢印の経路をどう辿るのかを真面目に考えると多分経路計画問題みたいのが出てくると思うのですが、まだやったことがありません。やったことがないので、若干心理的ハードルがあります（ちょっと勉強中ですが）
経路計画を勉強することを保留にするとすると、それ以外でパッと思いつくのは、目標位置を与えられた瞬間に、
- 車体の方向
- 現在位置と目標位置を結ぶ直線
の為す角度を求め、その角度分だけ旋回し、旋回が終わったら正面方向に直進するというやりかたですが、ハッキリ言って面倒ですし、時々刻々計算をやり直していたら直感的に破綻しそうな気もします。
冷静に考えれば旋回は、前回導入した位置制御においても、旋回方向のドリフトを抑えるためにモータドライバへのPWMをオフセットさせることで制御していますから、そちらを拡張すれば簡単にできそうです。
が、やはり
- 旋回を終わらせる、その後直進する というマクロな2ステップ
- ミクロな（例えば100Hzの）制御周期
を融合すると考えると、なんとなくコスパが悪い気がしたのです。
というわけで、紆余曲折ありましたが普通に速度制御を導入することにしました。
速度制御の導入
ホイールオドメトリで車体の並進、旋回方向の速度も見ていますので、速度制御の導入は（概念的には）簡単です。
- 普段は前回導入した位置制御でその場に留まる制御
- ジョイスティックの信号が入ったら位置制御を解除し速度制御モードに
- 並進・旋回それぞれをPIDによる速度フィードバック制御。最終出力は位置制御と同様、並進方向は倒立の目標角度、旋回方向はPWMのオフセット値。
- ジョイスティックの信号がなくなったら、ロボットの現在地を新しい目標位置と更新し、位置制御を再開
という流れで、速度制御を位置制御と共存させる形にしました。
その場に留まる制御も速度フィードバックにしてしまえば、１種類の制御で全て完結するのですが、結局それだと操作していない時に少しずつ位置がドリフトしてしまう気がしましたので、今は見送って位置フィードバックとしています。
意図せぬ移動を避けるため、ジョイスティック以外に特定のボタン（確かL1ボタンを割り当てました）を押さないと遠隔操縦ができないようにしました。
- 当該ボタンが押されているか
- そもそもジョイスティックに入力があるか
と、速度制御を始めるために２つの条件があり、それぞれフラグで管理していました。しかも実装において、実際に速度制御を行う関数と、ジョイスティックの入力を見る関数が別なので、グローバル変数が増えます。ただでさえコーディングの腕が無いのに、かなり複雑なコードになりました。
（貼ってもあまり意味無いですが）
github.com
/cmd_vel 型のメッセージは、今回はわざわざ導入するまでもないかと思い、ジョイスティック入力に適当なゲインを乗じて、直接速度指令として扱っています。
自律走行を導入したり、他の人に使ってもらうようなことがあれば、
- /joyメッセージのSubscribeとゲイン乗算
- あるいは自律走行機能による操縦指令Subscribe
- /cmd_velメッセージの生成・Publish
の部分だけを別ノードにしたらいいと思いますが、今回は見送りました。
ホストPCからの独立
自由に動き回るようになったわけですから、今まで紹介したホイールオドメトリや位置制御・速度制御等を担っていたホストPCもロボット本体に搭載し、有線接続から解放する必要があります。
ROSを動かすため、Ubuntuが入るシングルボードコンピュータとして、前回も紹介しましたがUPBoardを採用しました。
ktd-prototype.hatenablog.com
up-board.org
今までデスクトップPC⇔ESP32マイコンでやりとりしていたIMUや倒立目標角度等の情報を、UPBoard⇔ESP32マイコンで通信するようにします。接続は今まで通りUSBケーブルです。
UPBoardはシングルボードコンピュータの中でも画像処理等ができるハイスペック寄りの性能となっていますが、その分、5V、4A（だったかな）とかなりの大電力を要求してきます。
あまり大電流に対応するDCDCコンバータやレギュレータ（同じ意味か？）は多くないのですが、共立エレショップでちょうどいいのを見つけたので、そちらを使用しています。大元の電源はモータ駆動電源と共通で、3セル、基準電圧11.1Vのリチウムイオンポリマーバッテリーです。しかしこのDCDCコンバータ、高かった・・・
eleshop.jp
UPBoardへの電源供給をいきなり遮断するのはよくない気がしたので、スイッチのON/OFFではモータドライバへの電源供給のみを制御し、緊急時もUPBoard自体は落ちないような構成としました。UPBoardのシャットダウンは、デスクトップPCからのリモートログインにより実施しています。
ソフト面ですが、基本的にはすべてホストPC用の部分をUPBoardに移植しますが、ゲイン調整だけはGUIにより実施したいので、こちらのみデスクトップPCに残し、デスクトップPCとUPBoardによる分散システムを構築しました。ROSは分散システムの構築にも適しているそうで、以下の書籍を参考に、ゲイン調整のみデスクトップPC ⇔（無線） 車体制御は車載UPBoard ⇔（有線） 下位の制御・センシングは車載ESP32 という構成にしました。
books.rakuten.co.jp
遠隔操縦の実現
最後になってしまいましたが、動作状況はこんな感じです（再掲）。割と意のままに制御できるようになりました。
倒立振子ロボットver2 速度制御（遠隔操縦）の導入
段差乗り越えについては特別な制御をしていないので、基本的には勢いが付いていれば乗り越えられる、程度のものになっています。
速度フィードバック制御に積分制御が入っているので、大きな段差に遭遇した場合も、速度指令を入力し続ければ、モータ出力がどんどん増していって、（スペックが許せば）いつかは乗り越えられるような気がするのですが、そこまで姿勢制御が安定しているわけでもないので、実際はあまりうまくは行っていません・・・
初代倒立振子ロボットを作った際に、段差乗り越えに必要なトルクを計算したことがありましたが、絵にかいた餅な要求仕様となってしまいました。
