外部からの位置計測によるホバリング成功

バッテリ残量低下検出と緊急降下機能[1]、ラジコン操縦およびトリム調整UI、半自動の離陸、位置検出と可視化等を整備して、実験環境が整ってきました。それらを活用して、本機の動作の基本となる自動ホバリング機能を達成しました。 

#inrof に向けて開発中のクワッドコプター型ロボットです。
外部からの位置計測とフィードバック制御によるホバリングに成功しました。※ロボット本体には高度計やカメラは非搭載
先に一通り作るのを優先したので、計測も制御もまだ仮の簡易的なものです。今後改善していきます。#ESP32 #WROVER pic.twitter.com/2epDF7YcVs

— 山口辰久 (@qzy13700) February 8, 2021

空間の１点を目標として設定し、機体の重心位置をそこへ近づけるようなフィードバック制御を行います。これによりホバリングができるわけですが、それだけではありません。この目標地点を徐々に動かすと、空間を自由に移動することが出来ます。（但し、ゆっくりで良い場合に限る）

位置計測のためのセンサーとしては、Intel RealSense D435を用いました。ラップトップPCで点群を処理して位置を計測し、無線LANでロボット（本体）に操縦指令を送ります。

この操縦指令と子機側での制御は、以前書いたラジコンとして飛行させるために作った機能そのものです。ロボットは搭載したIMUにより機体姿勢（回転角）を把握しており、操縦指令のうちのロール角とピッチ角を保つようなフィードバック制御をローカルで行っています。

位置計測のための点群処理は、仮に単純なものを実装したところです。飛行想定範囲内にある点群の重心位置を子機の位置としています。クラスタリング等の処理を入れると処理の遅延が大きくなるため、まずは最もパフォーマンスが出るように、単純なものから試しています。現状では物体検出部分は16fps程度で行われています。カメラと競技台の位置関係といった概念を扱うプログラムもまだ作成していないため、実験ではカメラの座標系（光軸がZ軸、等）で目標地点を与え、そのまま制御に使っています。

なお初期の検討時問題と考えていた機体の回転方向の検出については、おそらく不要であろうということになりました。離陸時の姿勢を基準としてヨー回転を抑えながら飛ぶため、数分程度であればIMUの情報だけで最初の向きを維持できるのではないかと予想しています。

位置フィードバック制御の方も、まずは最も単純な方法ということで、とりあえず３軸方向独立に比例微分制御を実装したものです。X、Y、Z軸それぞれの方向について独立に、ロール軸目標角度、ピッチ軸目標角度、スロットル、という操作量にそれぞれ反映するというものです。フィードバックゲインは実験しながら手動で調整しました。

実際はこの３つの操作は独立ではありません。しかし、原点近く[2]ではお互いへの影響は十分小さい[3]から独立と思って制御してもある程度うまくいく、という理屈ですが、実際は必ずしも微小量とはいえないため、相当なごまかしを含んでいます。

回転をロールとピッチで表している時点でもすでに互いへの影響がありますし、機体を傾けるなら鉛直方向の分力が減るわけなのでその分推力を上げる必要があります。このような特性を反映しての制御もそのうち試します。

更に、スムーズに移動するためには位置ではなく速度の方を制御対処とする必要があるかもしれません。

なお、強調しておきたい点としては、このように外界から信頼できる観測が行えると、逆にロボットの特性の方を校正することができます。この場合で言えば、PID制御などでホバリングを維持したときの操作量から逆に、ラジコンで言うところのトリム調整を自動で行えるはずです。実験の再現性や作業性に大きくプラスになると思われるので、これも近いうちに追加します。

[1] 姿勢（機体の回転）の制御だけ残したままでスロットルをホバリングより少し低い値に固定して、2秒ぐらいしたらスロットルを切る、という内容。映像で着陸しているのもほぼ同じ方法。高度と降下速度は子機内部から計測できないため制御不能で、軟着陸とまではいかない。

[2] この場合は、水平に近い姿勢かつ重力とぴったり釣り合うスロットル出力、のこと

[3] 三角関数(主にcos)の原点近くの微小量での挙動に由来