3.2.5 小型移動ロボット用遠隔操縦・視覚情報取得用インテリジェントセンサの開発早稲田大学理工学総合研究所橋詰匠栄野隼一荒木政和清田光政三菱電機株式会社瀧口純一 (1) 目的本研究は被災現場で使用する情報収集システムに用いるためのインテリジェントセンサの開発を目的としている．図 1 にインテリジェントセンサ用途を示す． (a) 遠隔操縦用リアルタイム半球画像本センサに搭載している光学系は，リアルタイムに取得できる収差の少ない前方半球視野を持つため，モニタへ映し出し，遠隔操縦に使用する． (b) 自己位置標定装置 IMU などを用いた自己位置標定装置により，移動体姿勢角，自己位置を高精度に算出する． (c) 環境地図ロボットの真上や真横を撮影し，パノラマ状に展開した後，自己位置に合わせ張り合わせることで，通常の光学系では作成不可能な歪みのない移動軌跡上の実写地図を作成する． (d) GIS データベースへの統合ロボットの周囲 360°の距離情報と画像テクスチャを統合してローカルな三次元地図を作成し，さらに基地局で GPS データを加えることにより，防災シミュレータ等で用いる GIS データベースの補足データの収集が可能となる．図１インテリジェントセンサ用途 - 159 - (2) 年次実施計画平成14年度センサヘッドとセンサ制御器の製作及び単体評価試験主要目標性能：①センサヘッド直径Φ50ｍｍ以下 ②目標位置標定精度１ｍ以下連続移動100ｍ ③二次元フィールドでの地図更新レート0.2Hz以上 ※移動体の速度変化及び旋回方向を考慮した地図作成機能の達成平成15年度センサ信号処理の高速化、及び試験用移動プラットフォームを用いたテスト，フィールドでの総合試験・評価主要目標性能：①地図更新レート１Hz以上 ②視野角；前方視野55°以上，側方視野角45°以上 ③三次元空間での画像空間安定化 ※地面傾斜や凹凸への対応，高低情報を含む三次元地図 (平成16年度) センサヘッド小型化，自己位置標定精度向上 (平成17年度) センサ制御機器とIMUの小型化，三次元地図生成 (平成18年度) 移動機構との統合，GISデータベースへの三次元データ統合 (3) 前年度までの成果要約平成１４年度では，前方半球視野を持つ ODV を移動体に装備することで，上下左右を含む視野情報が得られるため，被災者の早期発見や走行不能領域の予測など，移動ロボットの遠隔操縦に有効であることを確認した．また，二次元フィールドにおいて，磁気方位計と加速度計による IMU での姿勢・位置情報を取得し，自己位置標定を行った．ODV の画像を展開し得られるパノラマ画像を，移動体の移動軌跡にしたがって重ね合わせるパノラマ環境地図を作成した．得られたパノラマ環境地図は，被災者位置確認や救助隊の救出プラン策定，ロボット等の回収に有効であることを確認した． (4) 平成 15 年度の目的平成１５年度は，平成１４年度で達成できなかった自己位置精度向上と三次元空間での環境地図生成の二つを目的とする．自己位置標定精度向上では，ODV画像から移動体の速度情報を抽出するアルゴリズムの開発を行い，得られた三次元相対速度とキャタピラ回転速度を観測量とするカルマンフィルタにより，加速度計やジャイロのバイアスを補正し，位置標定精度向上を図る．環境地図では二次元平面から三次元空間に拡張するため，ロール角，ピッチ角の画像補正を行う画像スタビライザ機能付きパノラマ環境地図生成手法を提案する．さらに，得られた自己位置と姿勢からモーションステレオ視により撮影対象物の距離を測定し，画像テクスチャを貼り付ける三次元地図生成手法を開発し，GIS データ拡張用として防災基地局とのシステムアップを目指す． - 160 - (5) 平成 15 年度の成果表 1 に当初目標と試作機の性能比較を示す．当初目標に対してセンサヘッド径と自己位置標定精度は満足できなかったものの，改良に向けての技術的目処をつけることができた．表１主要性能一覧項目目標性能試作 1 号機性能センサヘッド直径 φ50mm 以下 φ160mm ODV 直径 φ40mm 以下 φ60mm パノラマ環境地図更新レート 1Hz 以上約 1Hz(ただし画像処理のみ) 画像スタビライザピッチ角 5°以上ロール角 5°以上ピッチ角 10°以下ロール角 360° ◎ ODV 画角 (直射 /反射ハイブリット光学系構成) 前方 55°以上側方 45°以上同左 ◎ 自己位置標定精度 1ｍ以下（100m 走行時）三次元地図 10ｍ以下（100m 走行時）デモ環境下で VRML 出力可能備考イルミネータの小型化で対処可能来年度小型化予定最適化，PC の処理能力向上により対処可能次年度性能改善実施予定評価 × △ ○ △ ○ (a) センサヘッド部図 2 に ODV(OmniDirectional Vision)光学系原理図を示す． ODV は，主鏡，副鏡，レンズ，CCD カメラ，アクリルカバーから構成されている．主鏡と副鏡は，その反射面の動径方向の断面形状は凹曲面であり，周方向の断面形状も凹曲面なので，非点収差の発生は抑えられ，かつ２枚の反射鏡で像収束点を CCD 面に一様に置くことができ，低照度下において図２ ODV 原理図図３試作機外観も焦点の合ったシャープな映像が得られる．なお，本試作 ODV では，反射／直射投影比率を大きく取り，パノラマ展開画像の分解能を高くしている．図 3 に今回試作したセンサの外観図を示す． (b) パノラマ環境地図生成部本システムは前方と側方の視野を持つ ODV によって撮像した画像をパノラマ展開し，加速度計とジャイロ等により構成される MEMS(Micro Electro Mechanical Systems) IMU(Inertial Measurement Unit) によって，姿勢と直線加速度を計測する．そして画像取得タイミング毎の移動ベクタから画像の重ね合わせを行い，継ぎ目の無い連続パノラマ画像による環境地図を生成する． - 161 - 1) ロール姿勢変化補正図 4 にロール姿勢変化時の取得画像の補正結果を示す．パノラマ展開する際の基準点を IMU により計測したロール角分ずらすことによって，常に水平面基準のパノラマ展開画像を得られることがわかる． (a) 基準画像図４ (b) ロール姿勢変化画像図５ロール補正パノラマ画像ロール姿勢変化補正図5に図4に対応するロール補正による展開画像を示す．図5より重力方向である道路が画像中心に位置しており，ロール姿勢変化補正が有効であることが確認できる． 2) ピッチ姿勢変化補正図 6 に姿勢変化における座標系の定義図を示す． ODV 基準直交座標系を ( X o , Yo , Z o ) ，水 ( ) 平面基準直交座標系を X p , Yp , Z p とし，ロール，ピッチ，ヨーの順にオイラー角を (φ , θ ,ψ ) とすると，ODV 基準直交座標系と水平面基準直交座標系の変換式は式(1)で表せる． ⎛ X p ⎞ ⎛ cos θ ⎟ ⎜ ⎜ ⎜ Yp ⎟ = ⎜ 0 ⎜ Z ⎟ ⎜ − sin θ ⎝ p⎠ ⎝ sin θ ⎞ ⎟ 0 ⎟ 0 cos θ ⎟⎠ 0 1 ⎛ Xo ⎞ ⎟ ⎜ ⎜ Yo ⎟ ⎜Z ⎟ ⎝ o⎠ (1) 式(1)の座標変換式より図 7 のように水平面基準へ展開画像を変換するものとする．図６座標系の定義図７ピッチ姿勢変化の各座標系図 8 に ODV 水平設置時の取得パノラマ画像，図 9 にピッチ角 10°姿勢変化時の取得画像を水平面上に補正した画像を示す．図 8，図 9 より物体の位置及び傾きが同様の傾向を示しており，本補正が有効であることがわかる．図８基準パノラマ画像図９ - 162 - ピッチ補正パノラマ画像 3) 画像スタビライズ結果図 10 にピッチ，ロール動揺による姿勢変化をともなった際の環境地図を示す．図中の赤いラインは画像歪の比較のため，あらかじめ壁，床に引いたものである．図 10 は直線的な通路を走行しているにも関わらず，環境地図上ではロール回転の影響により曲線的な通路となっている．また，ピッチ回転の影響により，物体の傾きに歪みが生じていることがわかる．図 11 にロール，ピッチ補正を実行した最終的な環境地図を示す．補正前後での赤いラインの歪みをみると，図 10 の補正前画像では歪んでいたラインが，図 11 の補正後画像では直線的になっていることがわかる．また，ロール変化による影響も補正され，直線的な通路が再現されていることがわかる．これにより，ロール，ピッチ変化時の画像補正が有効であることが確認でき，不整地等での姿勢変化時においても視覚的に状況把握の容易な水平面基準の環境地図を作成できることが確認できた．図１０姿勢変化補正なしパノラマ環境地図図１１水平面基準パノラマ環境地図 (c) ODV 画像を用いた移動速度推定部 (b)項において，センサの回転に関しては補正ができたため，以下において周囲環境を基準に画像情報から車速推定を試みる．まず ODV 画像から床上の特徴点 P を抽出し，ODV特徴点の位置関係を計算する．そして次フレームの画像から対応する特徴点の位置関係を計算することで，移動距離を推定する．ここでは，ODV 画像上のある点と ODV の光学系を基準とした直交座標系との位置関係を求める．通路上の床と壁の境界線とドアなどの垂直エッジの交点を特徴点として抽出し，ENU座標系におけるODVの移動速度をODV画像上の特徴点の追尾を行うことで推定する．座標系の定義を図 12 に示し，ODVのドーナツ画像との対応を図 13 に示す．ODV座標は２項のように水平面に空間安定化されているためENU系に対してh 0 の高さに平行と仮定する． ODV座標系における特徴点Pの座標(X P ,Y P ,Z P )は，以下のように表される． XP = tan θ g × h0 cos(π − θ r ) =− tan θ g × h0 (2) cos(θ r ) (3) YP = h0 × tan(π − θ r ) = −h0 × tan(θ r ) (4) Z P = h0 以後，この点 P の時系列的な移動ベクトルを計算することで慣性基準の移動速度が推定できる． - 163 - θr Y U h0 Z P 図 12 θr X θg E θg (X P , Y P , Z P ) N P ODV 空間座標と特徴点図 13 ODV 画像上での対応点 (d) カルマンフィルタによる自己位置補正画像推定による速度は，いつでも算出できるわけでなく，キャタピラ回転速度もキャタピラが滑ったり，足場が崩れたりした場合には速度に誤差が生じるため，常に計測可能な IMU 出力を，画像処理による推定速度とキャタピラ回転速度により補正するカルマンフィルタを作成し，自己位置標定精度向上を図る．システム入力信号にIMUのセンサ座標基準加速度(a x , a y , a z )とオイラー角 (φ , θ ,ψ ) を，観測信号には，３項で求めた画像推定による速度と慣性基準に変換したキャタピラ回転速度の重み付平均(v x , v y , v z )を用いる．状態方程式は， x k +1 = Fk x k + G kWk (5) 観測方程式は y k = H K xk + q k ｋ:離散時刻 x k ;状態ベクトル y k :観測ベクトル F k : 状態行列 H k :観測行列 G k: 駆動行列 W k: プロセスノイズ (6) で表される．尚，観測信号により停車中と判断した場合，画像処理により速度を算出できない場合およびキャタピラが滑る場合等は，速度平均演算における重みを変えたり観測ノイズを変化させることでカルマンゲインをリアルタイムに変化させる．実際に 3.2[m]×3.2[m]の L 字型のコースを走行し，この際の IMU と画像推定速度，及び車軸エンコーダからとったキャタピラ回転速度の時系列データよりポスト処理にて計算した結果を図 14 に示す．IMU による純慣性出力のみで求めた移動経路と比べ，カルマンフィルタ出力は，良好な補正結果を得られ，真の移動経路に近づいていることがわかる． - 164 - 緑：真の移動経路赤：IMU 出力のみにより求めた移動経路青：カルマンフィルタによる移動経路図１４移動軌跡出力結果 (e) ODV／IMU 複合モーションステレオによる三次元地図生成大規模災害発生時における各地域の詳細な被災状況の把握は，住民の避難誘導，救難隊の派遣，二次災害の防止等を行うための情報として重要である．特に GIS データベースとリンクした被災地の三次元地図データの迅速な収集／データベース化は，救助隊の派遣優先度決定，救難計画立案，救助隊の詳細誘導に有効である．以下において，三次元データの取得方法について詳述する．本センサをレスキューロボットに搭載することにより，IMU を用いた INS (Inertial Navigation System)機能と画像によるモーションステレオの原理により周辺環境の距離を計測するとともに，周囲環境の画像を同期させて取得することが可能である．本センサによるモーションステレオは画像間の対応を取る為に通常カメラの 2 眼ステレオのようにエピポーラ拘束を使用するが，ODV という特殊な全周光学系を用いるため，エピポーラ拘束算出過程は通常の中心射影のカメラと比べて複雑になる．図 15 にエピポーラ線算出図を示す．図 15 においてカメラ①，カメ ⎡ M ′⎤ ⎡ R T ⎤ ⎡ M ⎤ ⎢ 1 ⎥ = ⎢0 1⎥ ⎢ 1 ⎥ ⎦⎣ ⎦ ⎣ ⎦ ⎣ R：カメラ間相対回転行列 (7) T：カメラ間相対移動ベクトル行ラ②座標系原点と空間上の注視点の 3 点からなる平面をエピポーラ平面とし，またカメラ ①座標系における空間上の注視点方向の直線をカメラ②仮想画像面に投影したものをエピポーラ線とする．カメラ①座標系における注視点 M のカメラ②座標系 M’ への座標変換式は式(7)になる．式(7)の座標変換式により，カメラ②座標系におけるエピポーラ平面方程式を算出し，ODV 仮想画像面との交線を求め，デジタル画像に投影することによりエピポーラ線を算出している． - 165 - 図１５エピポーラ線算出図図１６エピポーラ線抽出例図 16 にエピポーラ線抽出例を示す．提案手法はこのエピポーラ拘束を移動体の時系列的な位置・姿勢偏差から幾何的に求めるものであり，エピポーラ拘束を求めるに当り画像自体を使わないため，複雑な環境においても 2 枚の画像の対応点探索過程のロバスト性が高いという特徴がある．エピポーラ線の抽出後，両画像間の ActiveCell という小領域内においてノルム比較をすることにより適合度を算出し，最適な対応点を決定する．図 17 の実験装置を示す．本実験装置は ODV，イルミネータ，IMU から構成されている．図 18 に図 17 の実験装置を用いてモーションステレオを行った距離画像結果を示す．本センサにより取得した時系列的な距離データとテクスチャをレンダリングし，三次元環境を復元させている．図 19 に VRML 表示例を示す．図１７実験装置図１８距離画像図１９ VRML 表示 (6) 平成 15 年度の成果発表等 (a) 論文掲載 T.Hasizume, et al .,”A Study of Autonomous Mobile System in Outdoor Environment (Part 21 A Development of Intelligent Vision Sensor for a Remote-control Mobile Robot)”, JSME, N2A1-2F-B3, 2003.5.25. 荒木政和:屋外環境下で用いる自律移動システムに関する研究, ロボティクス・メカトロニクス講演会'03, ロボティクス・メカトロニクス講演会'03 講演論文集, 2A1-2F-B3, 2003 - 166 - 廣瀬直輝:屋外環境下で用いる自律移動システムに関する研究第 23 報二組のミラーを持つ反射型全周撮像器のステレオ視による高精度な距離画像生成, SI2003, 計測自動制御学会廣瀬システムインテグレーション部門講演会論文集, 1B3-6, 2003 直輝:屋外環境下で用いる自律移動システムに関する研究第 22 報半球視野を持つビジョンセンサによるパノラマ環境地図生成, SI2003, 計測自動制御学会システムインテグレーション部門講演会論文集, 3J3-1, 2003 (b) 特許「画像生成装置」特願 2002−356090 (7) 参考文献 [1] T. Hasizume, et al.: “A Study of Autonomous Mobile System in Outdoor Environment (Part 14 Development of the self-positioning system with an ODV),” JSME, No.001-4 pp.98-99, 2000.3.11. [2] T. Hasizume, et al.: “A Study of Autonomous Mobile System in Outdoor Environment (Part 15 Evaluation of the Self-positioning System with an ODV),” JSME,2P2-46-061,200.5.14. [3] T. Hasizume, et al.: “A Study of Autonomous Mobile System in Outdoor Environment (Part 19 Autonomous Mobile System Based on the Environmental Map Generation),” JSME, 1P1-G07, 2002.6.8. [4] T. Hasizume, et al.: “A Study of Autonomous Mobile System in Outdoor Environment (Part 20 Outline of a Forward-looking Hemispheric Vision for Reconnaissance & Surveillance Operation) ,” JSME, pp259-260, 2002.12.19. [5] J. Takiguchi, et al.: “A Self-positioning System Using an Omnidirectional Vision,” Transactions of the Japan Society of Mechanical Engineers(Part C), JSME, No68-673, pp.206-213, 2002. [6] J. Takiguchi, et al.: “High Precision Range Estimation from an Omnidirectional Stereo System,” Proc. IEEE Int. Workshop Intelligent Robots & Systems, Switzerland, pp.263-268, 2002. - 167 -