災害地の 3 次元再構築のための移動ロボット制御システム - 竹村研究室

社団法人 電子情報通信学会
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, [email protected])
あらまし 災害時,被害状況の確認や救助活動を行う専門家が現地に直行できない場合は,遠隔地からその状況
を把握することが必要である.災害地における危険な場所や狭い場所の環境情報を取得するための方法として移動
ロボットがある.この際に,画像に加えて 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, [email protected])
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.