ARマーカーでドローン制御

ドローンを自動制御してみたくなりました。
いきさつを言うと、とある大学でドローン競技があり、その中に人が操作せず空中に5秒間ホバーリングさせるという種目があるのを知りました。私が競技に出場する訳ではないのですがこれをやってみたくなりました。

まず空中に静止させるには考えられる方法が色々あります。GPSで座標を取得してフィードバックするというのが一番ありそうですが、世の中に既にあるし屋外で実験するのも中々大変です。できればコアレスモーターの軽いやつを使って室内で試したいのです。そこでARToolkitなるものを使ってみようと思います。

前に作った「どどーん」を使いたい。

ARToolkitはARマーカーという図形を印刷しておき、これをカメラで映すと画像に3Dの物体が合成されて現れるアレです。詳細はこちらを参照ください。
ARToolkit本家→ http://www.hitl.washington.edu/artoolkit/
工学ナビさんの説明→ http://kougaku-navi.net/ARToolKit/

そしてマーカーはこんな感じの図形です。

ARToolkitについてきたサンプルマーカーをそのまま使用

ARToolkit自体はCのライブラリなのでVisualC++から呼び出します。
ARToolkitに画像を渡しマーカーを認識しすると、マーカー座標からカメラ座標への変換行列が得られます。 ARtoolkitの一般的な使い方ではこれを元に図形を描画するのですが、この行列にはマーカーの位置が3次元的にどっちにあるか、またどの方向に傾いているかという情報が含まれるので、今回はこれを元にドローンを制御し、常に同じ位置にマーカーが見える様にしてやればホバーリングできるはず・・・という目論みです。

最初はこんな感じでやろうと思っていました。

ARマーカーは壁等に貼っておきます。

そしてコントローラーはこれ。Arduionoでサーボを制御して送信機のスティックを動かすという無理やりな構造。

やはりこのコントローラーは無理がありました。最初は手動でホバーリングさせてから自動に切り替えたいのですが、この切替がうまくできません。また縦軸を動かすと横軸が微妙に動くという問題もあります。 制御パラメーターの微調整が必要になる筈ですが、不安定なコントローラーでは訳が分からなくなりそうなので、まずはPCが考える値を確実にドローン側のフライトコントローラーに伝えるため、以前作ったTWE-Liteを使った送受信機を使う事にします。

またドローンに搭載したVTXから送られてくる画像では画質が今イチな上に振動の影響なのかARマーカーの認識が不安定でした。そこでARマーカーをドローンにぶら下げ、三脚に固定したカメラで検出する方法に変更しました。これでもマーカーが揺れますがカメラが固定なのでこっちの方がマシなのです。

カメラは固定に変更。ドローン側にARマーカーをぶら下げます。
こんな感じで糸でぶら下げました。

固定カメラで見たマーカーの位置を元に制御量を決め、FT232RL→TWE-Liteを経由してドローンのフライトコントローラーに伝えます。
また最初に手動で浮かせてから自動に切り替えるのでRC送信機→受信機→HOI-Link→PCの経路で送信機の操作量をそのままドローンにスルーすることもできます。HOI-Linkはこれまた先日作成したS-BUS→HIDコンバーターで、ラジコン受信機が出すS-BUS信号をPCからジョイスティックに見える信号に変換してPCに入力します。
なお手動と自動は送信機のスイッチで切り替える様にしました。

こんな感じで何枚もペラを折ってしまいながら調整を繰り返しました。
スロットルの制御が一番難しいです。まあ自分が操縦しても高度を維持するのが結構難しいので自動制御でも同じなのでしょう。特にロール、ピッチ、ヨーの制御だったらニュートラル位置がだいたい決まっているのでそこを中心に上下してやればよいのですがスロットルだけはこの値にすれば上下しない位置というのが決まっていないしバッテリーの残量によっても変わってくるので飛びながら調整していく必要があります。

そんな感じで何とかホバーリングが10秒以上は 持続するようになったのが次の動画です。まだ上下動が大きいのでもう少しパラメーターを調節したいところ。またバッテリーが消耗してくると何故かYaw軸が右に回ってきて制御で打ち消せなくなってきます

追記・・・

その後パラメータをイジってかなり安定になりました。
またYaw軸が回ってしまうのは機体の特性っぽいですが、これを制御で抑えられない原因が判りました。ドローンが右に回った時、回転行列から取ってくる角度が本来ならマイナスの値になる筈なのにプラスの値になる場合があります。この時自分の目で見てもマーカーは左に回っている様に見えるのです。この原因はARマーカーを印刷した紙が風で曲がっている為だと思います。ARToolkitはマーカーの四隅の頂点位置から角度を算出しますが、マーカーの角が曲がると正しい角度を割り出せないんですね。そこでマーカーの曲がり易そうな箇所を裏からスチレンペーパーで補強したところ、イイ感じになっています。
これでバッテリーが無くなるまでホバーリングを続ける様になりました。
安定化後の動画 ↓