群飛行ロボットの協調位置推定

概要

自律ロボットの構成において、不確実性を含む状況下で，自己位置を推論する技術は重要である．しかし，飛行ロボットでは，ペイロードの制約から，位置推定に必要なセンサ群を，十分に搭載することができない．そこで本研究では，群飛行ロボットによる協調位置推定システムを提案する．複数台の飛行ロボットがお互いの環境情報を共有することで，位置推定の向上を図る．位置推定手法には，確率論的なアプローチであるMonte Carlo Localizationを応用し，飛行ロボットに適用する．

システム構成

図1:システム構成図

本研究では群飛行ロボットの自律移動のための自己位置推定システムを構築する．システムの基本構成は図1に示すように，主に飛行ロボット，カメラ画像受信機，制御信号送信機，ホストコンピュータの４つから構成される．飛行ロボットにはカメラが搭載されおり，カメラから撮影された画像をカメラ画像受信機で受信し，A/D変換を通してホストコンピュータに送れらる．飛行ロボットには，市販の屋内用ラジコン飛行船(株)タカラドリームフォース02 スカイシップ）を用いる．ラジコン飛行船は浮力を得るためのエンベロープ部とモータ，プロペラ，制御信号受信機，バッテリ（ニッケル水素電池）を統合した駆動部から構成される．さらに外界の情報を得るために，小型ワイヤレスカメラ（カメラ部）を搭載する．カメラはカメラモジュールと画像送信機が一体になっている．

位置推定手法

本研究では，カメラ座標系でのlandmarkの位置情報から，ワールド座標系での飛行ロボットの位置（カメラの位置）を推定する．一般的にロボットは，行動に伴って自己位置推定の数値計算などに誤差を蓄積していく．この誤差が蓄積されていくと，ロボットは自身の状態を正しく判断できなくなってしまう．そこで，本研究では，確率的な位置推定手法として，Monte Carlo Localization（MCL）を用いる． MCLはロボットの状態をサンプルセットによる確率分布で表し，センサデータに基づき，この確率分布（サンプルセット）を更新していくことで，ロボットの位置を推定する．本研究では，ロボットの位置をlandmarkを中心とした円の円周近傍に分布するサンプルで表現する．従ってサンプルセットは図2に示すような円環状の分布になる．

図2:サンプルセット

観測データが得られた時に， $(x^{(i)},y^{(i)},z^{(i)})$ に位置するサンプル $i$ の重要度 $w$ は次式で更新される． $w(x^{(i)},y^{(i)},z^{(i)}) = P(r - r_{\mathrm{o}},z^{(i)}-z_{\mathrm{o}})$
$P$ は， $(0,0)$ を平均とする確率密度関数である. 一般的には，2次元ガウス密度関数 $N$ を用いる．
$r,r_{\mathrm{o}},z_{\mathrm{o}}$ は以下の観測データを用いて求められる．
観測データ

ワールド座標系のlandmarkの位置 $(x_{mw},y_{mw},z_{mw})$
カメラ座標系のlandmarkの位置 $(x_{mc},y_{mc},z_{mc})$

$r,r_{\mathrm{o}}$ は
$r = \sqrt{(x^{(i)}-x_{mw})^{2} + (y^{(i)} - y_{mw})^{2}} r_{\mathrm{o}} = \sqrt{x_{mc}^{2} + y_{mc}^{2}}$
として与えられる．

図3:各サンプルとLandmarkの位置関係（XY平面）

図4:各サンプルとLandmarkの位置関係（XZ平面）

また $z_{\mathrm{o}}$ に関しては図4の位置関係になり，
$z_{\mathrm{o}} = z_{mw} - z_{mc}$
として与えられる．
観測データに従って $\theta_{\mathrm{z}}$ も更新する．
$\theta_{\mathrm{z}} = \pi- \psi+ \phi$
ここで $\psi,\phi$ は
$\displaystyle \phi= \tan^{-1} \frac{y_{mc}}{x_{mc}} \psi= \tan^{-1} \frac{y^{(i)}-y_{mw}}{x^{(i)}-x_{mw}}$
として与えられる．よって各サンプルは，その位置に応じたヨー角 $\theta_{\mathrm{z}}$ が与えられる．（図5参照）．

図5:各サンプルのヨー角

さらに，各サンプルはそのサンプルがどの円環に属するかの情報を保つため，円環の中心座標を保存しておく．
つまり最後に観測したワールド座標系でのlandmarkの位置を記憶しておく．

$C(X_{cw},Y_{cw}) = (x_{mw},y_{mw})$

この情報は後の協調位置推定で用いる．

協調位置推定手法

協調位置推定では，それぞれの位置の確率分布を共有するだけでなく，他の飛行ロボットを観測したときの相対位置関係を利用する．
相対位置関係を確率分布として与え，それぞれの飛行ロボットの存在確率分布と照らし合わせることで，飛行ロボットの位置を推定する．
具体的な処理の流れは以下の通りである．ここでは，2台の飛行ロボットを用いた場合について述べる．
2台の飛行ロボットの場合の協調位置推定の手順は，以下の通りである．

飛行ロボット１の自己位置推定
飛行ロボット２の自己位置推定
飛行ロボット１の観測による飛行ロボット２の位置推定
飛行ロボット２の観測による飛行ロボット１の位置推定

図6:他の飛行ロボットを観測した場合

飛行ロボット1が飛行ロボット2を観測した場合（図6参照)，飛行ロボット1と飛行ロボット2の相対距離 $d_{b1-b2}$ は次式で求められる．
$d_{b1-b2} = \sqrt{x_{b12c}^{2}+y_{b12c}^{2}}$
ここで， $(x_{b1c},y_{b1c})$ は飛行ロボット1のカメラ座標系における飛行ロボット2の座標を表す．
また，このとき飛行ロボット1のカメラ方向ベクトルに対して，飛行ロボット2が存在する方向ベクトルとのなす角 $\phi$ は
$\displaystyle \phi= \tan^{-1} \frac{y_{b12c}}{x_{b12c}}$

である（飛行ロボット1のカメラ方向ベクトルを基準とする）．

以上の飛行ロボット1における飛行ロボット2の観測情報と，観測した時点での飛行ロボット1の状態（位置 $(x_{b1},y_{b1},z_{b1})$ とヨー角 $\theta_{z1}$ ）を用いて，ワールド座標系における飛行ロボット2の位置を記述すると
$x_{b2} = d_{b1-b2} \cos(\phi+ \theta_{z1}) + x_{b1}$
$y_{b2} = d_{b1-b2} \sin(\phi+ \theta_{z1}) + y_{b1}$
$z_{b2} = z_{b12c} + z_{b1} - h_{2}$
となる．
このようにして，飛行ロボット1の観測データから飛行ロボット2の座標が求まる．
各飛行ロボットの位置は確率分布によって表現されているので，（飛行ロボット1による飛行ロボット2の）観測データに基づいた飛行ロボット2の位置も確率分布になる．

図7:他の飛行ロボットを観測した場合

この観測に基づく他の飛行ロボットの存在確率分布を，次の例を用いて求める．
図7のように飛行ロボット1がlandmark1と飛行ロボットを同時に観測したとする．
この場合，（飛行ロボット1の観測データから得られる）飛行ロボット2の存在確率分布は次のように求めることができる．

先ずlandmark1の観測結果から飛行ロボット1の自己位置推定を行い，飛行ロボット1の状態を表すサンプルセットをつくる．
この中から任意のサンプル $i$ を選び，サンプル $i$ に基づく飛行ロボット2の位置 $(x_{b2}^{(i)},y_{b2}^{(i)},z_{b2}^{(i)})$ を求める．
次に飛行ロボット2とlandmark1の距離を次式で計算する．
$d_{b2-m1} = \sqrt{(x_{b2}^{(i)} - x_{mw1})^{2} + (y_{b2}^{(i)} - y_{mw1})^{2}}$

図8:他の飛行ロボットを観測した場合

ここで $(x_{mw1},y_{mw1})$ はlandmark1のワールド座標である．
$r_{b2-m1}$ は他のサンプルについて計算しても，ほぼ同じ値が得られる（landmark1のサンプルの分布によって誤差が決まる）．
つまり飛行ロボット2の存在確率分布は，landmark1を中心に半径 $r_{b2-m1}$ によって作られる円を円環の中心線とした確率分布になる．
この確率分布を用いて飛行ロボット2のサンプルセットの重要度を更新する．
飛行ロボット2が正しい自己位置推定を行えた場合，飛行ロボット1が飛行ロボット2を観測することで，飛行ロボット1自身の存在確率分布も正しい位置へと収束させることができる．なぜなら飛行ロボット1は飛行ロボット2を介してlandmark2の情報を得ているからである（図8参照）．
ここで飛行ロボット2の位置が定まった場合，飛行ロボット1の位置は，飛行ロボット2を中心として半径 $d_{b1-b2}$ の円を中心線とする円環状の分布になる．
この確率分布により飛行ロボット1のサンプルセットの重要度を更新することで，飛行ロボット1の位置の不確かさを減少させることができる．