社団法人電子情報通信学会 THE INSTITUTE OF ELECTRONICS, INFORMATION AND COMMUNICATION ENGINEERS 信学技報 TECHNICAL REPORT OF IEICE 災害地の 3 次元再構築のための移動ロボット制御システムの開発河合克哉† 中澤篤志†‡ 清川清†‡ 竹村治雄†‡ †大阪大学大学院情報科学研究科〒565-0871 大阪府吹田市山田丘 1-5 ‡大阪大学サイバーメディアセンター〒560-0043 大阪府豊中市待兼山町 1-32 E-mail: †[email protected], ‡{nakazawa, kiyo, takemura}@cmc.osaka-u.ac.jp) あらまし災害時，被害状況の確認や救助活動を行う専門家が現地に直行できない場合は，遠隔地からその状況を把握することが必要である．災害地における危険な場所や狭い場所の環境情報を取得するための方法として移動ロボットがある．この際に，画像に加えて 3 次元形状を計測することが有用であると考えられる．本研究では，全方位レンジセンサと全方位カメラ及び姿勢センサを搭載した移動ロボットを用いて自己位置の推定と形状情報の取得を行うシステムを開発する．全方位レンジセンサはライン型のレーザスキャナと回転ステージによって実現する．自己位置の推定には車輪の回転量と姿勢センサによる計測値を用いる．また，複数の位置で取得された形状データに対して，ICP アルゴリズムによる位置合わせを行い，自己位置推定の補正値としてフィードバックする．試作したシステムを用いて実際に動作実験を行い，自己位置推定精度の評価を行った．キーワード災害地，3 次元形状，自己位置推定，位置合わせ Development of a Control System of a Mobile Robot for 3D Reconstruction of a Disaster Area Katsuya KAWAI† Atsushi NAKAZAWA†‡ Kiyoshi KIYOKAWA†‡ and Haruo TAKEMURA†‡ †Graduate School of Information Science, Osaka University 1-5 Yamadaoka, Suita, Osaka 565-0871 Japan ‡Cybermedia Center, Osaka University 1-32 Machikaneyama, Toyonaka, Osaka 560-0043 Japan E-mail: †[email protected], ‡{nakazawa, kiyo, takemura}@cmc.osaka-u.ac.jp) Abstract Three dimensional geometry information, as well as live video image, of a disaster site is useful for damage investigation and rescue planning. In this report, presented is an information gathering mobile robot that acquires geometry information of a disaster site. After moving to every location directed by a remote operator, the robot acquires omnidirectional range data by using an omnidirectional range sensor, and estimates its position and orientation from the amount of wheels' rotation and an orientation sensor. Using the estimated location as the initial value, current and past range data are registered by the ICP algorithm. Indoor experiments have shown that the self-localization error was within five percents. Keyword disaster site，Three dimensional geometry，self-localization，registration 1. はじめに近年，自己位置を推定しながら実環境の情報を取得する移動ロボットの研究が数多くなされている災害地実環境遠隔地 [1][2][3]．災害地においては，危険な場所や狭い場所といった人間が入れない場所が多く存在するが，移動ロボットはそのような場所でも情報を得ることが出来る．取得した環境情報を現地に直行できない専門家に対して提示することで，被害状況の確認や救助活動に遠隔操縦システム各センサ駆動部各データ役立てることが出来る．我々の研究グループでは，情報収集ロボットの遠隔操縦者計算機操縦システムに関する研究を行っている．このシステムでは，広範囲にわたる環境情報を取得するための情情報収集ロボット移動指示画像・形状位置・姿勢 VR環境計算機報収集ロボットを遠隔操縦システムによって操作する．図 1 システムの構成図環境情報として，画像に加えて形状情報を取得するこ推定する．とで， VR 環境を構築することができる．計測ロボッ示することにより，遠隔地の状況把握およびロボットに対する的確な指示が可能となる [4]．本システムの構初期地点トから受信した環境情報を統合して VR 提示装置で表形状取得次の計測地点の指定旋回・直進成を図 1 に示す．本稿ではこのうち，情報収集ロボットの制御システムを試作した．ロボットは環境中を移動し，複数の地点で全方位形状と全方位画像を計測する．取得した距離データを位置合わせして統合し， 1 つのモデルとして操縦者に提示する．各計測地点の位形状取得障害物次の計測地点の指定旋回・直進いて把握できるが，誤差を含んでいるため，最終的に × は形状モデル同士の位置合わせアルゴリズムを用いる．ここで得た推定値により，車輪の回転量と姿勢センサの計測値による推定誤差を修正する．形状取得次の計測地点の指定図 3 2. 情報収集ロボットの構成旋回・直進ロボットの行動計画の概要 2.1. システムのシステムの構成移動ロボットは図 2 のように， 2 台の計算機を用いて電動台車と各センサを制御する．一方の計算機では，全方位形状の取得電動台車，姿勢センサ及び全方位カメラを制御する．また，もう一方の計算機では，レーザスキャナと回転ステージを制御する．次の計測地点の指定推定誤差の補正全方位カメラレーザスキャナ姿勢センサ全方位画像距離値回転ステージステージの回転角姿勢旋回・自己位置推定直進・自己位置推定計算機車輪の回転量旋回角度と直進距離の計算形状モデルの位置合わせ全方位形状の取得図 4 ロボットの行動計画の流れ図環境情報電動台車図 2 ロボットの構成 3. 環境情報の取得 3.1. 全方位形状の取得実環境の 3 次元形状情報を取得する方法はこれまで 2.2. ロボットのロボットの行動計画ロボットが目標地点に到達するために，図 3 及び図 4 に示すように旋回・直進と形状取得というステップを繰り返す．最初に，初期地点で全方位形状を取得する．そして，操縦者の指定に従って，次の計測地点への移動を開始する．ロボットは，まず移動方向を向くようにその場で旋回し，指示された移動距離だけ直進する．移動が終了すると，その地点での全方位形状の取得を行う．ロボットは，常時計測する運動センサと定点ごとに計測する距離センサの値から，自己位置をにも研究されている [5][6]が，本システムでは，ラインセンサ型のレーザスキャナを回転ステージの上に設置した計測システムを用いる．計測のイメージを図 5 に示す．スキャナはレーザの投光角度を変化させることにより，縦方向 (経線に沿った方向 )180 度の範囲の距離値 r を得る．更に，ラインの角度を回転ステージで制御することで横方向 (緯線に沿った方向 )360 度に渡ってラインデータを得る．各点におけるラインの角度φ は ,ステージの回転速度とスキャナの計測タイミングを考慮して計算する．これらのデータを極座標系から目標地点形状取得置・姿勢は，車輪の回転量と姿勢センサの計測値を用直交座標系へ変換し，全方位の 3 次元形状を獲得する．実際に計測した全方位形状モデルを図 6 に示す．計測ライン計測点ステージの回転軸 (鉛直方向) r(θ θ,φ φ) θ φ x= X⋅ f ⋅ y =Y ⋅ f ⋅ (b2 − c 2 ) (b2 + c 2 )Z − 2bc X 2 +Y 2 + Z 2 (b2 − c 2 ) (b2 + c 2 )Z − 2bc X 2 +Y 2 + Z 2        (1) ここでの 3 次元座標はカメラ座標系で表されているので，形状モデル中の点との対応を求めるにはスキャナとカメラの位置関係を求める必要がある．カメラ座標系での点 P C (X C ,Y C ,Z C ) とスキャナ座標系での点 P S (X S ,Y S ,Z S )との間の変換は定数 H C を用いて式 2 のように表される．図 5 全方位形状計測のイメージ  HC X C   XS       H C YC   YS   H Z  = C Z   C C  S  1   H  C     (2) 変換行列 C は，画像中の特徴点と形状モデル中の点との対応から推定する [8]．こうして求めた値を用いて，図 6 実環境 (左 )と形状モデル (右 ) 3 次元モデル中の各点に対応する色情報を画像から取り出して貼り付ける．実際に形状モデルに画像を貼り付けた様子を図 8 に示す． 3.2. 全方位画像の取得本システムでは ,鉛直下向きの双曲面ミラーとその下方のカメラによって周囲の画像を一度に取得する画像センサを搭載する．得られた全方位画像は図 7 のようなパノラマ画像や任意方向の透視投影画像に変換できる [7]．ここで得られた映像を形状データと統合する．3 次元環境中の点 P(X, Y, Z)に対応する全方位画像中の点 p(x,y)は式 1 によって表される．ただし， f はカメラの焦点距離であり，a と b 及び c は双曲面を定義する定数であり， c = a 2 + b 2 図 8 形状モデル（左）と画像貼り付け結果（右）である． 4. 移動ロボットの自己位置推定 4.1. 車輪の回転量を用いたいたデッドレコニングデッドレコニングデッドレコニングとは，ロボットの内界センサを用いて自己の位置姿勢を推定する方法である [9]．駆動部として用いる電動台車から車輪の回転量を取得し，このデータから台車の位置推定を行う．図 9 のような運動モデルを考える．l w は車輪の半径， la は車軸の半径である．並進距離 d と旋回角度 φ は左右それぞれの車輪の回転角度θl とθr を用いて式 3 のような線形変換で表される． d   l w / 2 φ  = l / 2l   w a 図 7 全方位画像（上）とパノラマ画像（下） l w / 2  θ r  − l w / 2l a  θ l  (3) θ r= θ l= θ の時に台車は直進し，直進距離は θ lw (cm)となる．また， θ r = － θ l = θ の時に台車はその場 4.3. 形状モデルのモデルのレジストレーションで旋回し，旋回角度は θ l w /l a (度 )となる．移動ロボットの位置姿勢に依存する．複数視点から取ある視点で形状データを取得したとき，その原点は得した形状データを統合するには，視点間の相対的な位置姿勢を用いて座標変換すればよい．しかし，車輪 d θl の回転量と姿勢データを用いた推定には誤差が含まれ θr るため，形状データのマッチングによって移動量を推 φ 定することを考える．形状データの特徴から対応関係を求めて位置合わせを行う方法として， ICP アルゴリ lw la ズムがある [10]．このアルゴリズムは，まず両者の点群から対応する点の組み合わせを決定し，その間の誤差が最も小さくなるような変換行列を反復して求めるものである．一方で， ICP では初期位置合わせの必要図 9 運動モデルがあるため，車輪の回転量と姿勢データを用いた推定この方法は，車輪や車軸の半径の誤差，走行時のス値を ICP の初期移動量として用いる． ICP によって得リップ等の誤差が累積するという問題がある．た変換行列は，姿勢データを用いた推定値の誤差が累 4.2. 姿勢センサのセンサの利用から取得した形状に ICP アルゴリズムを適用した結果積されるのを防ぐための補正に用いる．実際に 2 視点車輪の回転量のみを用いたデッドレコニングで推を図 11 に示す．この図は実験室の形状を上方から見た定できるのは 2 次元上の位置姿勢であり，坂道や瓦礫ものであり，下側に示されている壁や左上側に示されの上を通る場合の 3 次元的な位置姿勢を推定することている平面等のずれが ICP によって補正されているこは出来ない．そこで，姿勢センサを用い，車輪の回転とが確認できる．量と姿勢データから 3 次元位置姿勢を推定する．姿勢センサの計測値をオイラー角 (yaw, pitch, roll)か → → → ら回転行列 R=(r x , r y , r z )に変換する．ここでは台車の進行方向を z 軸方向としており，進行方向の各軸成分 → は r z で表される (式 4)．  sin( yaw ) cos( roll ) + cos( yaw ) sin( pitch ) sin( roll )   (4) r  rz =  sin( yaw ) sin( roll ) − cos( yaw ) sin( pitch ) sin( roll )    cos( yaw ) cos( pitch )   その場で旋回する際は姿勢センサの値のみを利用する．直進時は，車輪の回転量と同時に姿勢データを取得す図 11 2 視点から取得した形状モデル (左 )とる．時刻 t から t+ ⊿ t までの回転角を θ (t)と書くと， ICP アルゴリズムの適用結果 (右 ) この区間での移動距離は θ (t)l w となる．図 10 のよう → に，直進移動開始地点から終了時地点までの移動量m は式 5 で表される． r r m = ∫ rz (t )θ (t )l w dt (5) t 移動開始地点 θ (t + ⊿ rz(t) 試作システムでは，電動台車としてワコー技研の EMU-S を用いる．搭載するラインセンサ型のレーザス rz(t+⊿t) )lw θ(t 5. 試作システムによる実験 5.1. 試作システムのシステムの実装キャナとしては， SICK の LMS200 を用いる．これは， t)lw 移動終了地点 m 図 10 姿勢データを用いた自己位置推定図 12 に示すタイミングで 1 ラインを 26.64ms かけて計測する．これを中央精機の回転ステージ ARS-136-HP の上に載せ，同社のコントローラ QT-CD1 を用いて回転させる．双曲面ミラーを用いた全方位カメラとしては，末陰産業の SOIOS-55Cam を用いる．このカメラの有効画角は双曲面ミラー焦点を中心に方位角 360 度，仰角 12 度，俯角 50 度である．姿勢センサとしては， 0 2 … 358 00.00ms(θ=0°) 360 1 … 06.66ms(θ=180°) … 359 357 13.32ms … 19.98ms 26.64ms 図 12 レンジセンサによる計測タイミング InterSense の InertiaCube3 を用いる．これはジャイロで直進時も旋回時も姿勢センサを用いた推定値の実計測した角速度の積分から姿勢データを求め，加速度測値に対する誤差は ±5％以内であり，姿勢センサが有計とコンパスの値によってドリフトを補正するセンサ効に使われていると判断することが出来る．しかし，である．ただし，コンパスは移動ロボットの発する電平坦で空回りの無い屋内環境での実験であったため移場の影響を受けるため，補正には用いなかった．これ動中に鉛直軸回りの姿勢成分しか変わらず，車輪の回らを制御する計算機として東芝の Libretto U100 を 2 台転量のみから推定した場合と姿勢センサを用いた場合用いる．計算機と電動台車はそれぞれの内部のバッテの有意差は見られなかった．また， ICP アルゴリズムリによって駆動することが出来るが，その他の機器はによる位置合わせを行うと，直進時には誤差が大きく外部からの電力供給が必要である．具体的には，レーなっている．これは，視点の移動によって重なり部分ザスキャナと回転ステージがそれぞれ 24V，全方位カが小さくなり，対応点の決定が困難になることが原因メラが 12V，姿勢センサが 6V の直流電圧を必要とすであると考えられる．解決方法として，重なり部分がる．そこで， 12V の鉛蓄電池 2 つと 1.5V の乾電池 4 大きくなるように代表点を抽出することが考えられる．つを搭載する．実際に試作した移動ロボットの概観を図 13 に示す． 800 直進時 750 推定値(cm) 700 650 600 車輪の回転量のみ 550 姿勢データを併用 500 ICPを適用 450 実測値 400 350 400 500 600 700 800 実測値(cm) 旋回時 90 図 13 移動ロボットの概観 5.2. 自己位置推定手法の精度評価まず，車輪の回転量のみを用いた推定と姿勢センサ推定値(度) 80 70 姿勢データのみ 50 を用いた推定の精度比較を行った．直進と旋回それぞ 40 れの場合について実験を行っている．ロボットに目標 30 値を指定した移動命令を与え，実測値と推定距離を比較する．直進時の距離は手動で実測し，旋回時の角度車輪の回転量のみ 60 ICPを適用実測値 30 40 50 60 70 80 90 実測値(度) 図 14 自己位置推定の実験結果は 3rdTech の HiBall センサシステムを用いて測っている．このシステムは，対象の位置・姿勢を高精度 (位置 5.3. 連続移動時の自己位置推定軌跡の評価 0.4mm，姿勢 0.02 度 )で計測するトラッカであり，今回次に，直進と旋回を繰り返した時の，車輪の回転量の実験の真値として用いるのに十分である．また，と姿勢センサを用いた自己位置推定の軌跡を評価する ICP アルゴリズムを用いた位置合わせも行い，補正値実験を行った．経路は， 120cm 直進した後左に 135 度を得た．直進距離を 400cm， 600cm， 800cm とした時旋回し，さらに 120cm 直進，最後に右に 135 度旋回しの実験結果と旋回角度を 30 度， 60 度， 90 度とした時て 120cm 直進するというものである．実測値は前述のの実験結果を図 14 に示す． HiBall を利用して計測した値を用いている．実験の結果を図 15 に示す．終了時の実測値と推定値のずれは約 4cm であり，総移動距離の約 1%である．平坦な場所での自己位置推定は概ね正しい結果が得られていると言える． 100 80 60 (cm) 推定値 40 真値 20 0 0 20 40 60 80 100 120 140 160 180 -20 (cm) 図 15 自己位置推定の実験結果 6. おわりに本稿では，自己位置を推定しながら多視点からの全方位の映像と 3 次元形状を計測する移動ロボットを開発した．また，開発したシステムの位置推定精度について実験し，実際に屋外環境のモデリングを行った．今後の課題としては，屋外の不整地走行における自己位置推定の精度評価，操縦者の指示に応じたパスプランニング方式の検討，操縦者に対するモデルの提示方式の検討が挙げられる．文献 [1] A. Nüchter, H. Surmann, K. Lingemann, J. Hertzberg, and S. Thrun. “6D SLAM with Application in Autonomous Mine Mapping,” Proc. ICRA, pp.1998-2003, 2004. [2] J. Folkesson, H. I. Christensen. “Graphical SLAM A Self-Correcting Map,” Proc. ICRA, pp. 791-798, 2004. [3] Hiroshi Ishida, Keiji Nagatani, Yutaka Tanaka. “Three-Dimensional Localization and Mapping for a Crawler-type Mobile Robot in an Occluded Area Using the Scan Matching Method,” Proc. IEEE/RSJ Int. Conference on Intelligent Robots and Systems, pp. 449-454, 2004. [4] 齋藤研作 , 町田貴史 , 清川清 , 竹村治雄 , “移動ロボットの遠隔操縦のための三次元情報提示システムの提案 ”, 電子情報通信学会総合大会講演論文集 , D-12, pp. 128, 2005. [5] K. Ikeuchi, A. Nakazawa, K. Hasegawa, T. Ohishi, “The Great Buddha Project: Modeling Cultural Heritage for VR Systems through Observation,” IEEE ISMAR03, pp.7-16, 2003. [6] 山本和彦 , 棚橋英樹 , 桑島茂純 , 丹羽義典 , “実環境センシングのための全方向ステレオシステム（ SOS）” , 電気学会論文誌 C（電子・情報・システム部門誌） Vol. 121-C, No. 5, pp. 876-881, 2001． [7] 山澤一誠 , 八木康史 , 谷内田正彦 , “移動ロボットのナビゲーションのための全方位視覚系 HyperOmni Vision の提案 ”, 電子情報通信学会論文誌 , Vol.J79-D-II, No.5, pp.698-707, 1996. [8] 井口征士，佐藤宏介，三次元画像計測，昭晃堂， 1990． [9] 吉川毅 , 本間稔規 , 大村功 , 鈴木慎一 , 高橋裕之 , 橋場参生 , 澤山一博 , 長尾信一 , “視覚システム搭載型自律走行ロボットの開発（第 2 報）－機能モジュールの開発と自律走行試験－” , 北海道立工業試験場報告 , No.296, pp.113-121, 1997. [10] P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Trans. PAMI, Vol. 14, No. 2, pp. 239-256, 1992.