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はマーカーの四隅の頂点位置から角度を算出しますが、マーカーの角が曲がると正しい角度を割り出せないんですね。そこでマーカーの曲がり易そうな箇所を裏からスチレンペーパーで補強したところ、イイ感じになっています。
これでバッテリーが無くなるまでホバーリングを続ける様になりました。
安定化後の動画 ↓

ArduinoでPICマイコンに書込む~AE-PIC18F14K50編~

先日HOI-Linkを製作した時、秋月電子のPIC18F14K50基板「AE-PIC18F14K50」Arduiono UNOを使ってブートローダーを書き込みました。その際以前書いた「ArduinoでPICマイコンに書込む~OpenStickLite(PIC18F14K50)編~」とは若干変更する必要がありました。
またブートローダーだけでなくHOI-Linkのファーム全体を書いてみました。
なおArduinoに入れる書込み用スケッチにミスっていた箇所があったので修正版を掲載しています。

AE-PIC18F14K50基板は秋月電子で¥800 (現時点) で売られています。DIPパッケージ単体では¥220ですが発振子やUSBコネクタも買う事を考えると基板もそんなに割高ではありません。
以前DIP版のPIC18F14K50にArduinoからブートローダーを書き込んだのと同様にこの基板にも書込んでみました。

前回DIPパッケージ版と今回のAE-PIC18F14K50とで異なるところ

前回の回路図はこれ。この時マイコンのVDDにはArduinoから3.3Vを供給し、VUSB端子もVDDに直結していました。

PIC14K50WriteByUNOsch
ArduinoUNOでPIC18F14K50に書き込んでみた回路図。

一方AE_PIC18F14K50基板はVUSB端子が外部に出ていません。VUSBはチップ内にレギュレータを持っていて3.3Vが出力されるのでVDDに少し高い電圧(まあ5Vですよね)を入れてやればVUSBには外から印加する必要はありません。PICkit3でもそうなっていますしね。

という事で次の様な回路で上手くいきました。
上と違うのは電源を5Vに変えたのとVUSBには接続していない事。
なおVUSB端子はGNDとの間にコンデンサを接続する必要がありますがAE-PIC18F14K50は基板内に0.22μFを内蔵しています。

AE-PIC18F14K50基板に書き込む配線

ブートローダーだけでなくファームウェア本体も書いてみる。

といつもはブートローダーだけを書き込み、そのブートローダーから本体のファームウェアを書き込んでいました。今回ちょっと試しにブートローダー+HOI-Linkのファーム全体を書き込んでみます。

まずその前に書き込みデータを作ります。PICkit3を接続してファーム書込み済のマイコンからプログラムを読み出し、HEXファイルに書きだしました。このファイルにはフラッシュに書かれたプログラムとコンフィグデータ以外にEEPROMの内容も含んでいます。しかしArduino上の書込みスケッチをEEPROMには対応させていないのでテキストエディタでEEPROMに関する行を消しておきます。

そして出来たのがこのファイル→HOI-LinkAndBootloader.zip
解凍すると次のHEXファイルが出てきます。
HOI-LinkAndBootloader.hex
これをメモ帳で開いてTeratermにコピペする事でAE-PIC18F14K50に書き込みます。このあたりの手順は前回の記事に書いています。

そしてファーム全体を書き込むと1分程度で何事もなく終わりました。
因みにブートローダーだけだと約10秒です。Teratermの設定で速度を下げているので専用PICライタに比べると時間が掛かりますが、これは 書き込みデータ全てを Arduinoのメモリーに保存できないのでシリアル通信でPCから順次送りながら書き込んでいる為です。この時PCとArduinoの間でフロー制御できればよいのですが、Arduinoの素の状態ではこれが出来ないので送り込み速度の方を落としているのです。

続いてベリファイをしてみると・・・なんだかエラーが出ます。調べてみるとArduino上の書込みスケッチのベリファイルーチンでチェックサムの計算をミスっていたので、これを修正するとベリファイもエラー無く終了しました。
なお書込みルーチンでもチェックサムを計算していますがこちらは正常です。
書き込みとベリファイは殆ど同じ処理なので上手くサブルーチンで共有すればいいのに面倒になってコピペで作成した為、デバッグの時にベリファイ側の修正を忘れていた様です。

修正版のArduino UNO用書込みスケッチ→PICWrite18F14K50_190621.zip

という事で・・・

秋月電子のAE-PIC18F14K50基板に専用PICライタを使わずファームを書き込めるようになりました(Arduinoは必要ですけど)。 直接ファームを書いても良し、ブートローダーを書いてUSB経由でファームを送り込むのも良しでお手軽にUSB工作が楽しめます。

HOI-Link:S-BUS→PC接続 完成

前回の続きです。前回は秋月電子のPIC18F14K50搭載基板をブレッドボードに挿して実験していました。今回は完成版として基板に収めるのでDIPパッケージで製作します。

まずKi-cadで回路図を書いて・・・

基板のパターンを作って・・・

片面基板なので表パターン(赤の配線)は実装時に導線で接続します。

CNCで削って基板を作ります。

FRISKケース(何年か前から大きくなった120%Booster版)に入れてみました。

寸法を合わせて作ったので当然ですがピッタリです。

ところでFRISKケースの場合ソケットを使ってマイコンを実装すると高すぎて蓋が閉まらなくなります。なので基板に直付けする前にブートローダーを書き込んでおきます(後からでも書けるけどブレッドボードに載せられる実装前の方がやりやすいので)。前に書いた9V電池書きこみ方式で・・・

006Pの9V電池書き込み方式

そして部品取り付け。
私のコントローラーはフタバのFASST方式ですが息子はFHSS方式なので受信機を取り替えられる様にします。最初はXHコネクタの3ピンを使うつもりでしたがL字型のポストが部品箱にありません。L字型でないとコネクタを刺す方向が縦になっておさまりが悪いのです。
代りにL字のピンヘッダがあったのでこれを基板にとりつけ、ここにRCサーボ等で使う3pinコネクタ(QIとかデュポンコネクタとか呼ぶやつ)で接続します。
但し逆刺しができてしまうのは要注意。

完成の図。

そして当初の目的であったドローンシミュレータ実行中。

参考のためマイコンに書いたファームウェアを載せておきます(例によってソースは試行錯誤の後が残ったままで汚いです)。コンパイルはMPLAB-IDEのバージョン8で行っていますが今となってはかなり古い環境だと思います。最新の環境でビルドできるかどうかは確かめていません。
また本来USB機器には固有のVID,PIDを書き込んでおく必要がありますがこのプログラムには適当な値を書いてあるのでそのまま使われる場合は自己責任でお願いします( 確率は低いですがもしVID,PIDがダブった機器を接続した場合に上手くつながらなくなる可能性あり)。
なおOpenStickLiteで使ったのと同じブートローダーから書き込む仕様でビルドしています。

HOI-Link:S-BUS→PC接続

PC上でドローンシミューレータを動かすとき、今までこんなコントローラーを使っていました。

10年くらい前、ムサシノ模型のモスキートモス号を作った時に練習用に買ったコントローラー

私レベルだとこれで問題ありません。しかし息子によると実物の送信機とは感覚が違うのでレースの練習はやりにくいとの事。
そこでトレーナーコネクタからPCに接続するインターフェースの購入も検討したのですが、その前に手元にあるものでなんとかできないか考えました。

まずトレーナーコネクタというのは送信機の裏についているこういう四角いコネクタ。ここからPWM信号が出ているらしいです。

右上の四角いコネクタがトレーナーポートです。

まずこのコネクタをどこで入手できるかというところから検討しなければなりません。またここから信号を取出せたとしてパルス幅をチャンネル数分カウントし、HIDデバイスとしてUSBに伝える処理も必要で何となく面倒です。

そこでRC受信機が出すS-BUS信号をマイコンで取り込んでUSBに伝えるのなら、OpenStickのファームウェアを少し改造して実現できそうです。しかもシリアル通信なのでパルス幅を数えるよりも精度が良い筈。但し受信機が一つ余分に必要なのと電波が出っぱなしになるのがちょっと気が引けますが、たぶんトレーナーコネクタから取り出す方式でも電波は出ているんではないですかね?(想像ですが)

こんな構造にしようと思います。

受信機はSBUSが出せれば何でもよいので、まずはこれで試してみます。

REDCON FT4X
ケースは外していました。

マイコンは秋月電子で買って放置していたPIC18C14K50搭載ボードです。このマイコンはOpenStickLiteでも採用しているので馴染みがあります。

そしてRAM領域の不足に悩まされましたが何とか動作し始めました。S-BUSは18チャンネル分のデータが含まれていますが私のT6EXで出せる6CHまでしか確認していません。あとで息子のT10Jで10chを試してみようと思います。


T6EXで実験中。ちょっと古いFASST方式ですが現役です。

実物のコントローラーだしケーブルをつなぐ必要もなく操作性は良いです。
何か他の事にも使えそうな気がします・・・今すぐ思いつきませんが・・ラジコン送信機でPCを操作する何か・・・
またPCの画面の前にカメラを置いてVTXで画像を飛ばし、ゴーグルで見ながらシミュレーターをやったらもっとリアルになるかも(かなりオタクだなぁ)。

現状はブレッドボードですが、この後ちゃんとした基板に載せ、ケースにも入れて完成させたいと思います。FRISKのケースあたりが良いかな?