More Related Content More from harmonylab (20) 7. カメラによる位置推定(2/2)
1. 事前準備
1. 内部パラメータ(歪み、画角等)推定
Zhangの手法[1] ： キャリブレーション器具を撮影
ズームやフォーカスを変えないかぎり不変
2. 外部パラメータ(位置、姿勢)推定
既知位置にマーカーを置き撮影、取得した画像中のマーカー座標と
マーカーの座標を入力することで3次元-2次元の対応が得られる
4組以上からPnP問題をLM法[2][3]によって解き外部パラメータを推定
2. 飛行中
1. カメラ画像の明るい領域の重心を計算
2値化 → 膨張×5回 → 領域解析 → 重心計算
2. 画角とカメラ姿勢でバルーンへ向かうベクトルを計算
(画像中心からの乖離)×(画角)=(カメラ向きからの角度)
3. ベクトルの交点(2乗距離が最小になる点)がバルーンの位置
取得した画像 歪み補正済み
重心
[1] Z. Zhang : "A flexible new technique for camera calibration“, 2000.
[2] K. Levenberg : "A Method for the Solution of Certain Non-Linear Problems in Least Squares“, 1944.
[3] D. Marquardt : "An Algorithm for Least-Squares Estimation of Nonlinear Parameters“, 1963.
OpenCVを用いて処理
9. 管制部
• 各バルーンに司令を送る
– 飛行準備、編隊飛行開始、緊急停止等
• カメラの画像を取得し座標を推定
• 推定した座標をバルーンに送信
• バルーンの位置、状態等を表示
カメラ映像確認画面
センサ情報確認画面
PC : DELL Alienware15
OS : Ubuntu Linux
開発言語 : C++
使用ライブラリ：
OpenCV
OpenGL(gtkglextmm)
GTK+(gtkmm)
11. プロペラユニット
• 正転/逆転で同じ特性
• 反トルク相殺
• PWMによる出力制御
– 𝑓 = 50Hz, 𝑇𝑂𝑁 = 1.0~2.0[ms]
-200
-150
-100
-50
0
50
100
150
200
-100% -50% 0% 50% 100%
推力
[gf]
出力[%]
モーター:CT2211-2200
ドライバ:HK-30A3Dプリンタ製外装
プロペラの正回転の方向 作成した両回転プロペラ
𝑚𝑎𝑥 ≅ ±180[gf] 制御信号線(制御部へ)
バッテリー：Turginy 2S 1000mAh
13. 制御部
• Raspberry Pi 2 Model B
• 管制PCとの通信(USB-WiFiドングル)
• プロペラモジュール、IRマーカー、カラーLED制御
• センサフュージョン
– カメラの位置推定約6Hz+センサ10Hz
センサ基板
MPU6050
3軸加速度・ジャイロ
HMC5883L
3軸地磁気
(BMP180
気圧・温度)
サンプリング間隔Δ𝑡 = 0.1[sec]
バルーン座標𝑃 𝑡 、速度𝑣 𝑡 、ヨー角𝜃 𝑡
センサによる推定座標𝑃𝑆 𝑡 、速度𝑣 𝑆 𝑡 、ヨー角𝜃𝑆 𝑡 、ヨー角速度Δ𝜃 𝐺𝑦
𝜃𝑆 𝑡 = 𝛼 𝑀𝑎𝑔 𝜃 𝑀𝑎𝑔 𝑡 + 1 − 𝛼 𝑀𝑎𝑔 𝜃𝑆 𝑡 − Δ𝑡 + Δ𝜃 𝐺𝑦
加速度センサ値𝑆 𝐴𝑐𝑐𝐵(バルーン座標系)、𝑆 𝐴𝑐𝑐𝑊 実世界座標系
𝑆 𝐴𝑐𝑐𝑊(𝑡) =
cos 𝜃𝑆 𝑘 0 sin 𝜃𝑆 𝑘
0 1 0
− sin 𝜃𝑆 𝑘 0 cos 𝜃𝑆 𝑘
𝑆 𝐴𝑐𝑐𝐵 𝑘
𝑃𝑆 𝑡 = 𝑆 𝐴𝑐𝑐𝑊 𝑡 Δ𝑡
カメラでの推定座標𝑃𝐶 𝑡 = 𝑥 𝐶 𝑡 , 𝑦 𝐶 𝑡 , 𝑧 𝐶 𝑡 を受信している場合:
𝑃 𝑡 = 𝛼 𝐶𝑎𝑚𝑃 𝑃𝐶 𝑡 + 1 − 𝛼 𝐶𝑎𝑚𝑃 𝑃𝑆
′
𝑡
𝑃𝑆
′
𝑡 = 𝑃 𝑡 − Δ𝑡 +
Δ𝑡(𝑣 𝑆 𝑡−Δ𝑡 +𝑣 𝑆
′
(𝑡))
2
𝑃𝑆 𝑡 から𝑣 𝑆 𝑡 , aS(t)を再計算
𝑃𝐶 𝑡 を受信していない場合：
𝑃 𝑡 = 𝑃𝑆 𝑡
𝛼 𝐶𝑎𝑚𝑃: カメラの推定の重み(0.95)
𝛼 𝑀𝑎𝑔: 地磁気センサの重み(0.95)
マイコン: Raspberry Pi 2 Model B
OS : Raspbian ”Wheezy” Linux
WiFi : PLANEX GW-450D KATANA
開発言語 : C++
制御部
14. バルーンの位置制御
サンプリング間隔Δ𝑡 = 0.1[sec]、軌跡先読み時間Δ𝑡 𝑣 = 0.5[sec]
バルーンの座標𝑝 t = 𝑥 𝑡 , 𝑦 𝑡 , 𝑧 𝑡 、ヨー角𝜃 𝑡
バルーンの速度𝑣 𝑡 、目標座標𝑝∗
𝑡
目標速度𝑣∗ 𝑡 =
𝑝∗ 𝑡+Δ𝑡 𝑣 −𝑝 𝑡
Δ𝑡 𝑣
、目標ヨー角𝜃∗ 𝑡
目標との差𝐸 𝑡 = 𝑣∗
𝑡 − 𝑣 𝑡 、𝐸 𝜃 𝑡 = 𝜃∗
𝑡 − 𝜃 𝑡
バルーンの出力(実世界座標系)𝑓𝑥 𝑡 , 𝑓𝑦 𝑡 , 𝑓𝑧 𝑡 , 𝑓𝜃 𝑡
𝐾 𝑃, 𝑇𝐼, 𝑇 𝐷: 𝑃𝐼𝐷制御ゲイン
𝑓𝑧 𝑡
𝑓𝑥 𝑡
X
Z
Y
𝑧 𝑡
𝑥 𝑡
𝜃 𝑡
𝑓𝜃 𝑡
X
Z
Y
𝑚0 𝑡
𝑚1 𝑡
𝑚2 𝑡
𝑚3 𝑡𝑚4 𝑡
𝑚4 𝑡
モーター出力𝑚0(𝑡), 𝑚1 𝑡 , 𝑚2 𝑡 , 𝑚3 𝑡 , 𝑚4 𝑡
𝑚0 𝑡 = −𝑓𝐵𝑥 𝑡 + 𝑓𝜃 𝑡
𝑚1 𝑡 = 𝑓𝐵𝑧 𝑡 − 𝑓𝜃 𝑡
𝑚2 𝑡 = −𝑓𝐵𝑥 𝑡 − 𝑓𝜃 𝑡
𝑚3 𝑡 = 𝑓𝐵𝑧 𝑡 + 𝑓𝜃 𝑡
𝑚4 𝑡 = 𝑓𝑦 𝑡
バルーンの水平方向出力(バルーン座標系)𝑓𝐵𝑥 𝑡 , 𝑓𝐵𝑧 𝑡
𝑓𝐵𝑥 𝑡
𝑓𝐵𝑧 𝑡
=
cos 𝜃 𝑡 − sin 𝜃 𝑡
sin 𝜃 𝑡 cos 𝜃 𝑡
𝑓𝑥 𝑡
𝑓𝑧(𝑡)
𝑓 𝑡 =
𝐾 𝑃 𝐸 𝑦 𝑡 + 𝑇𝐼 𝐸 𝑦 𝑡 Δ𝑡 + 𝑇 𝐷
𝐸 𝑦 𝑡 −𝐸 𝑦(𝑡−Δ𝑡)
Δ𝑡
垂直方向
𝐾 𝑃 𝐸 𝑡 + 𝑇 𝐷
𝐸 𝑡 −𝐸 𝑡−Δ𝑡
Δ𝑡
水平方向、ヨー回転
上から見た図
Editor's Notes 研究背景としては、
イベント会場では天井から風船を投下する演出がよく行われていますが、
単に落とすだけではなく、それぞれが自律的に移動し、
編隊飛行を行えたら面白い、ということで始まりました。
また、体育館等での実験だけでなく実際にライブで飛ばしてもらえるようなモノを作るということを目標に行ってきました。 開発に先立って、バルーンロボットシステムにはこれらの性能が必要であると考えました。 次に、考えたバルーンロボットシステムについて説明します。
開発したシステムのシステム図はこのようになっています。
まずはカメラによる位置推定についてです。 イベント会場では激しいライティングの変化が予想されるため、
カメラは可視光線ではなく、赤外線を撮影できるものを用意し、
バルーンの赤外線マーカーを撮影します。
赤外線マーカーはバルーンロボットの下部に取り付けるのですが、
どの方向からでも認識できるように高輝度、広角な赤外線ledを8個使っています。
ドームのような広い空間での位置推定となるため、カメラは高解像度で広角であることが求められます。
これらの要求を満たすため、市販のwebカメラなどではなく、産業用のカメラを用いることにしました。
このカメラは70メートル先で1ピクセルが51.3mmに相当する解像度を持っています。
カメラはバルーン同士の重なりによる影響を減らすためと、
広いエリアをカバーするために6台使っています。
画像の取得はイーサネットケーブルによって行うのですが、
転送速度の上限により、取得頻度が6.6fpsになってしまっています。 位置推定を行うにあたってまずはカメラの素性を知らなければなりません。
カメラの投影行列や歪み特性を推定するのにはチャンの手法と呼ばれるアルゴリズムを用いています。
これは、あらかじめサイズのわかっているチェスボード柄の器具を撮影することでこれらのパラメータを推定するというものです。
次にカメラの位置や方向がわからないと位置推定を行えないため、
カメラ自身の位置と方向を推定します。
これは3次元位置がわかっている場所に赤外線マーカーを置き撮影することで得られる3次元2次元対応を4組以上もちいて
投影誤差が最小になるような姿勢を求めることで行っています。
ここでの推定が間違っているとこの後のバルーン位置推定が破綻してしまうため、推定された位置姿勢が正しそうかどうかチェックしています。
バルーンロボットの位置推定ですが、
まずはカメラから得た画像中の明るい領域を探しだし、その重心を求めます。
カメラの画角と位置・向きはわかっているので画像の中心からのズレをもとに3次元の直線を求めます。
複数のカメラからは複数の直線が得られるので、これらの交点を計算することによって赤外線マーカーの位置を推定することができます。
次に、管制部についてご説明いたします。 管制部の役割はこれらのとおりです。
先ほどのカメラ画像による位置推定に関わる計算はここで行っています。
このようにバルーンからの情報を画面に表示したり、
バルーンに対する司令を送る事ができます。 次にバルーンロボットのプロペラユニットについてご説明します。
プロペラユニットはバルーンの移動に必要なのですが、
飛行機などとは違って前後両方に推力を発生させられることが求められます。
本システムではブラシレスモーターをラジコンカー用ドライバで駆動することでプロペラの正転逆転を実現しています。
また、一般的なプロペラには向きがあり、前後での特性が違うためため、
正転用、逆転用のプロペラを2枚逆向きに貼り合わせることによって前後で同じ特性になるようにしています。
このプロペラユニット1つで最大出力で180グラム重の推力を発生できます。
モーターの出力は制御部からのpwm信号によって制御しています。
次に、バルーンロボットの制御についてご説明します。 バルーンロボットにはraspberry piを搭載しており、
ここで管制部との無線通信やプロペラ、演出用カラーLED等の制御を行っています。
カメラによる位置情報は1秒に6回程度届くのですが、
イベント会場での無線環境は良くないため通信が途切れてしまうことがあります。
そういった状況に対応するため、バルーンロボット自体もセンサによって自身の移動を認識できるようにしてあります。
搭載しているセンサは加速度、ジャイロ、地磁気センサの3つです。
気圧センサも搭載しているのですが、不調の為使わないことにしました。
カメラからの位置情報とセンサによる位置情報の統合はこのように行っています。
まずはバルーンのヨー角を推定します。
これは、地磁気センサとジャイロセンサの値に一定の重みをつけて統合することで行っています。
次に、加速度センサによるバルーンの実世界座標系での位置、速度を推定します。
ヨー角はわかっているので加速度センサの値を回転行列によって実世界座標系に変換し、
積分することで速度と位置を計算します。
カメラからの位置情報がある場合は、センサによって推定された位置と一定の重みをつけ足しあわせています。
このとき、速度をカメラの位置情報から計算した速度で上書きします。
通信が途絶えたり、フレームレートを補完するタイミングのときは、
センサで推定した位置をそのままバルーンの位置として用います。 バルーンを決められた軌道に沿って飛行させるための位置制御についてご説明します。
決められた軌道を0.5秒先読みし、現在出しているべきである速度を計算し、
現在の速度をそれに合わせるような制御を行っています。
速度差が小さくなるような制御でプロペラの出力を決定しています。
バルーンは浮力と重力のバランスによって常に一定方向への力を受けるのでその影響を減らすために
垂直方向(y軸方向)の出力を決定するのにはPID制御を使っています。
水平方向とヨー角にはそのような一定の力はかからないためPD制御を使って出力を決定しています。
まずは実世界座標系での出力を決定し、
そこにバルーンのヨー角に対する回転行列を掛け、バルーン座標系に直し、モータードライバに伝えています。
ヨー角の出力と水平方向の出力は単純に足すことによって決定しています。 浦安舞スタジオという実際のイベント会場と同様の設備を備える会場にて
開発したバルーンロボットシステムの実証実験を行いました。
バルーンロボットは3台で、空調を切っての飛行を行いました。
(右のグラフ)これらのグラフは各バルーンの実際に飛行した軌跡と指定しておいた軌道を比較したものです。
Y軸の動きが少々ずれてしまっている部分もありますが、おおむね軌道通りに飛行できることがわかりました。
こちらがその飛行の様子です。
まとめと今後の展望です。
屋内イベント会場でのエンターテイメント飛行を目的としたバルーンロボットシステムを開発し、実際にうまくいきそうであることがわかりました。
今後の展望としては、
今回、PID制御のパラメータをトライアンドエラーで決定したのですが
これらをニューラルネットワークによって決定できるという研究があるので
これを参考に制御を見直したいと思います。
また、実際のイベント会場での飛行を想定して
安全性などを向上させていきたいです。
以上です。ありがとうございました。
