小型無人移動体を用いた
3
次元地図の自動生成
尾
崎
宏
樹
†1渡
邉
賢
†2長
尾
確
†2 3次元地図は複雑な建物での情報提示などに適しているが, 作成には非常に高いコ ストがかかり, 運用が難しいという問題がある. 自律走行可能な小型無人移動体を用 いて環境内のデータを自動的に収集し, 収集したデータから 3 次元地図の生成を行う ことで, 3 次元地図の自動生成を行う仕組みを実現した. また, 位置推定のための環境 地図と対応付けられた 3 次元地図の生成を行うことで, 3 次元地図をナビゲーション などの応用に容易に利用することが可能となる.Automatic 3D Map Generation
by Using a Small Unmanned Vehicle
Hiroki Osaki,
†1Ken Watanabe
†2and Katashi Nagao
†2While 3D maps are useful to visualize complicated shapes of objects such as buildings, it is very costly to create and maintain them. We developed a mech-anism to automatically generate indoor 3D maps by using a Small Unmanned Vehicle (SUV, for short). SUV is capable of autonomous run and collection of 3D data captured by an RGB-D (RGB and Depth) camera. SUV also has a function of generation of a 2D environmental map for localization of current position. Our method can make a precise alignment between the 3D maps and the 2D environmental maps so that our generated 3D maps can be applied to highly informative indoor navigations.
†1 名古屋大学 工学部 電気電子・情報工学科
Department of Electrical and Electronic Engineering and Information Engineering, School of Engineering, Nagoya University
†2 名古屋大学 大学院情報科学研究科
Graduate School of Information Science, Nagoya University
1.
は じ め に
人は普段から3次元空間の中で生活しているため,従来の2次元地図を用いて周囲の環境 を認識する場合,慣れない2次元の地図と3次元の実環境を対応付けて判断しなければなら ない. 一方, 3次元地図は,実環境と同様の3次元空間であるため,より直感的に周囲の環境 を認識できる. 特に,大きな駅やショッピングモールなどの複雑で大きな建物では, 2次元地 図よりも3次元地図による情報提示が適していると思われる. しかし, 3次元地図の作成に は非常に高いコストがかかり, 3次元地図を利用したシステムの運用は困難となっている. 3次元地図の生成手法は,レーザーレンジセンサーを用いるもの1)や,カメラ画像を用い るもの2)が提案されていが,前者は平面をセンシングするため重ね合わせが難しく,後者は 暗い場所や画像特徴点の少ない環境では正確に3次元地図を生成する事ができないという問 題がある. また, RGB-Dカメラを用いた手法として, 3次元の点群と画像特徴点の重ね合わ せを行うことで精密な3次元地図を生成するRGBD-ICP3)が提案されている. しかし,こ の手法ではRGB-Dカメラのデータのみで重ね合わせを行うため,生成した3次元地図を利 用する場合,実世界の位置と3次元地図座標のキャリブレーションを行う必要がある. 位置 推定のための環境の2次元地図が既に存在する場合には, 2次元地図と対応付けられた3次 元地図を生成することで,既存の位置推定の仕組みをそのまま利用する事ができる. そこで 本研究では,自己位置推定が可能な小型無人移動体(SUV: Small Unmanned Vehicle)を用 いてデータの収集を行い,環境地図と対応付けられた3次元地図を生成する手法を提案する. SUVは,我々の研究室で研究・開発を行っている自律走行可能な無人の小型ロボットであ る. SUVは,事前に環境内を走行することで2次元環境地図を生成する機能を持ち,環境地 図を利用して位置推定や自律走行を行うことができる. そこで, SUVにRGB-Dカメラを 搭載し,環境地図を基にデータ収集経路を生成することで,自動的にデータを収集する仕組 みを実現する. また,データ収集時の位置情報を利用して, RGBD-ICPを3次元の点群と画 像特徴点だけでなく,環境地図とも重ね合わせを行うように拡張することで,環境地図と対 応の取れた3次元地図を生成する.2.
小型無人移動体 (SUV)
SUVは,自身を取り巻く環境を認識して人間に情報を提示することができ,また,環境内 を探査することで様々なデータを蓄積し,そのデータを再利用することで高度なサービスを 提供することを目的とした,無人で自律走行可能な小型のロボットである.SUVの外観は図1のようになっており，対向二輪型の移動機構を持つiRobot Create⋆1に
PCとレーザーレンジセンサーとRGB-Dカメラを搭載した構成となっている. レーザーレ ンジセンサーは物体にレーザー光をあて，その反射光を受光して物体までの距離を算出し， これを一定角度間隔で測定することで，平面上にセンサーから物体までの距離を知ることが できる．SUVは,このセンサーを利用して環境地図を生成し,自己位置推定を行う.
SUVはRGB-DカメラとしてKinect⋆2を搭載している. KinectはRGBカメラに加え て，カメラから見た物体までの距離が計測できる深度センサーを持つデバイスである．平面 上の障害物までの距離が計測できるレーザーレンジセンサーと異なり，物体の3次元形状の 復元を行うことが可能である. 実際に3次元形状復元したものを図2に示す. 本研究ではこ の機能を利用して3次元地図の生成を行っている. 詳しくは第3章で述べる. RGB-Dカメラ 図 1 SUV の構成 Fig. 1 Configuration of SUV
図 2 3 次元形状の復元 Fig. 2 Reconstruction of 3D shape
2.1 環境地図生成 現在地を把握したり，目的地を決めその場所まで自律走行するためには走行する環境の地 図が必要である．SUVは事前に環境を走行することで，レーザーレンジセンサーの計測記 録から2次元の占有格子地図を作る機能を持つ．占有格子地図とは，等間隔の格子に配置 された確率変数で表現される地図のことであり，確率変数の値が高いほどその領域が物体に よって占められている可能性が高いことを意味する. 実際にSUVが生成した占有格子地図
⋆1 iRobot Createは米国 iRobot Corporation の米国およびその他の国における登録商標です.
⋆2 Kinectは米国 Microsoft Corporation の米国およびその他の国における登録商標です.
を図3に示す．画像の各画素の色の濃さが占有格子地図の各格子の確率変数の値の大きさを 表す．黒は障害物有り，白は障害物無し，灰色が初期状態を表している．
図 3 生成した占有格子地図 Fig. 3 Example of occupancy grid map
2.2 自己位置推定 設計した経路に沿った柔軟な自律走行を行うために, SUVは環境地図上での自身の位置 を常に知っていなければならない. ここでいう位置とは, 2次元のグローバルな座標系,つ まり地図の座標系において,移動体の姿勢at = (xt, yt, θt)のことである. 姿勢は移動体の 位置x, yと向きθによって決まる値である. SUVは,環境地図と現在のレーザーレンジセ ンサーの値を比較することで,環境地図上での位置を推定することができ,実時間で位置推 定を行うことで任意の経路に沿った自律走行が可能となる. 図4は,リモコンによるマニュ アル操作で同じコースを6周したときの位置推定のなし・ありでの軌跡を表しており，図5 は1週ごとの出発地点との位置の誤差を表している． 位置推定なし 位置推定あり 図 4 位置推定なし・ありの場合のそれぞれの移動軌跡
位置推定 あり 位置推定 なし
図 5 周回後の現在地情報と出発点との距離
Fig. 5 Distance between initial location and current location
3. 3
次元地図生成
本研究における3次元地図生成は,以下の4ステップから成る. ( 1 ) データ収集経路の生成 ( 2 ) データの収集 ( 3 ) センサーデータの重ね合わせ ( 4 ) 3次元地図のポリゴン化 (1)はSUVを用いてデータを自動収集するため, SUVがデータ収集時に走行する経路を 環境地図から自動的に生成する機能である. これについて3.1節で詳しく説明する. (2)は (1)で生成した経路に沿って実際にSUVを走行させ, RGB-Dカメラのデータとその時の位 置情報を記録する. (3)は収集したデータの重ね合わせを行うことで3次元地図を生成する. 重ね合わせにはフレーム間の3次元の点群と画像特徴点のそれぞれの対応点と,環境地図と の対応点を利用することで,環境地図と対応付けられた3次元地図を生成する. これについ て3.2節で詳しく説明する. センサーデータの重ね合わせによって生成した3次元地図は膨 大な数の点の集合であり,データ量が大きく扱うのが非常に困難である.そこで, (4)では建 物の内装を構成する主な要素である壁や床といった平面に着目し, 3次元地図中の平面を抽 出しポリゴン化することで3次元地図を単純化する. これについて3.3節で説明する. 3.1 データ収集経路の生成 センサーデータの重ね合わせには画像特徴点を利用するため,収集するデータには画像特 徴点が多く含まれることが理想的である. 屋内では,特徴となるような領域は壁沿いに集中 し,床ではあまり検出されないことが多い.そこで,基本的には壁沿いを走行し,必要に応じ て通路の中央などのデータを収集する経路を生成する. また,生成した経路を一周するだけ では物陰になってしまいデータが取れない領域ができてしまう可能性があるため、経路を一 周した後、経路を逆向きにもう一周することでくまなくデータを収集する。 具体的には,環境地図から基準となるノードを生成し,ノード同士を接続することによっ てグラフ構造を作る. さらに,生成されたグラフ構造を利用してデータ収集経路を生成する. 3.1.1 ノードとグラフ構造の生成 壁沿いを走行する経路を生成するため, 環境地図の走行可能領域の壁からの距離を基に ノードを生成する. 図6に走行可能領域を壁からの距離で色分けした様子を示す. 壁から近 いほど濃く,遠くなるにつれて薄い青になる. 最近点の計算にはk-d木を用いた. 色が変わ る境界線上に一定間隔でノードを生成する. ここで生成するノードをノードAとし,図6に 赤色の点で示す. ノードAが生成されないような狭い通路では,通路の中央にノードを生成 する. 最近点までの距離と距離がほぼ等しい近傍点が,最近点から離れた位置に存在する場 合,その走行可能領域を通路の中央であると判断する. 狭い通路の中央を図6に赤色の線で 示し,その線上に生成されたノードをノードBとして緑色の点で示す. ノードAとノード Bの接続点として,壁からの距離が一定距離で通路の中央と判断される位置にノードCを生 成する. ノードCを黄色の点で示す. 生成したノード同士を接続しグラフ構造を生成する. ここでは,ノード間の関係を隣接す るノードと到達可能なノードの2種類定義する. ノードAおよびノードCで同じ境界線上 に生成されたノード同士,もしくはノードBおよびノードCで隣り合ったノード同士を隣 接するノードとし,その接続を図7に赤色の線で示す. また,隣接したノード以外で一定距 離以内にあり,間に障害物のないノード同士を到達可能なノードとして,青色の線で示す. 図 6 ノード生成の様子 Fig. 6 State generated by the node図 7 ノード間のグラフ構造 Fig. 7 Graph structure between the nodes
3.1.2 経路生成のアルゴリズム ( 1 ) スタート地点を現在地から最も近いノードAまたはノードCに設定する ( 2 ) 指定されたノードから隣接するノードをたどって1周する経路Rを生成する この時通過したノードRi(0≤ i ≤ n)に到達可能ノードNmが存在し,ノードNmが 未探索の場合, Ri→ Nmを探索候補に追加していく. また, Nj= Riとなる探索候 補が存在する場合それを削除する. ( 3 ) 未探索のノードを通る経路R′を生成し経路Rに挿入する 探索候補のうち, Ri→ Nmの距離が最も短くなるものを選び,ノードNmをスター ト地点として手順2と同様の操作で経路R′を生成する. 経路Rのi + 1番目に経路 R′を挿入する. このとき,滑らかな経路を生成するためにRi−1→ R′1が到達可能で あればR′0を削除する. ( 4 ) 探索候補がなくなるまで手順3を繰り返し実行する ( 5 ) 経路上のすべてのノードCに対して手順6を実行する ( 6 ) 狭い通路を通る経路R′′を生成し経路Rに挿入する ノードCからノードBへ進む経路R′′を生成する. 経路R′′の経路Rへの挿入方法 は経路R′′の終端であるノードR′′nによって3つの場合に分かれる. それぞれの場 合について以下で説明する. ( a ) R′′nがノードBの場合 経路R′′を反転し経路の0番目とn番目を削除したものを経路R′′の末尾に 追加し,経路Rのi番目に経路R′′を挿入する. ( b ) R′′nが未探索のノードCの場合 R′′nをスタート地点として手順2-4を実行し経路R′′′を生成する. 経路R′′の 0番目とn番目を削除し反転したものを経路R′′′の末尾に追加し,経路R′′の n番目を削除したものを経路R′′′の先頭に挿入する. 経路Rのi番目に経路 R′′′を挿入する. ( c ) R′′nが探索済みのノードCの場合 経路Rijを反転し,経路R′′の0番目とn番目を削除したものを経路Rのj 番目に挿入し,経路Rのi番目に経路R′′を挿入する. ただし,経路Rのi番 目からj番目までの経路を経路Rijとし, Rj= R′′nとする. ( 7 ) 経路Rを反転したものを経路Rの末尾に追加する 以上のアルゴリズムを用いて、実際に生成された経路を図8に示す. 図 8 生成された経路 Fig. 8 Example of route
3.2 センサーデータの重ね合わせ 3.2.1 床の検出と位置推定 センサーデータの重ね合わせを行う際に初期位置としてSUVの位置情報を用いることで, 環境地図とある程度対応の取れた位置にセンサーデータを配置する事ができ,環境地図と重 ね合わせを行うことができる. しかし, SUVが取得できる位置情報はx,y座標および左右 方向の向き(ヨーθy)のみで, 3次元的な位置および角度情報を取得する事ができず,環境地 図は2次元の地図であり3次元的な重ね合わせを行うことができないため,センサーが水平 に固定されていなければ正確に重ね合わせることができない. しかし,実際にはSUVは移 動しながらデータを収集するため,センサーが常に水平に固定されているとは言えない. そ こで,取得したセンサーデータから床の検出を行い,床が水平面と一致するようなz座標お よび上下方向の向きと進行方向に対しての回転(ピッチθp,ロールθr)の推定を行うことに よってデータ記録時のSUVの3次元的な位置推定を行う. 床の検出はRGB-Dカメラが床に対してある程度水平になっていることを前提とする. 床 は,面の方程式ax + by + cz− 1 = 0で表すことができるので, RANSAC (Random Sample Consensus)を用いて尤も床らしいパラメータa, b, cを求める. 全てのデータからランダム に3点を取り出し,取り出した3点からa, b, cを求める. このとき, 3点を含む面がある程 度水平でなければその面は床でないと見なす. 求めたパラメータに全データを当てはめ観測 されたデータとの誤差を計算し,誤差が許容範囲内であればそのパラメータに投票を行う. これを繰り返し行い得票数が最も多いパラメータに投票したデータを抽出し,抽出したデー タから再度パラメータを求めることで尤も床らしい面を検出する. データ中に床の領域があ まり含まれず床が正しく検出されない場合,そのデータは重ね合わせには利用しない. 本研
究ではセンサーデータからサンプリングした1000個の点を全データとし,繰り返し回数を 1000回としてアルゴリズムを適用した 3次元的な位置情報は,床上の点pを水平面上の点p′ に変換するp′ = R· p + Tとなる 回転行列Rと平行移動行列Tから計算することができる. 回転行列Rおよび平行移動行 列Tは水平面と検出した床との対応点3点から求める. 3.2.2 センサーデータの重ね合わせ方法 フレーム間の3次元の点群と画像特徴点の対応点と,環境地図との対応関係を利用してセ ンサーデータの重ね合わせを行うことで,環境地図と対応付けられた3次元地図の生成を行 う. 重ね合わせを行うには回転および平行移動を行う剛体変換行列Aを求める必要がある. 変換行列Aは次式によってSUVの位置情報から計算することができる. ただしx, y, z軸 それぞれに対する回転行列をRx, Ry, Rzとする. A = TRz(θy)Rx(θp)Ry(θr) 本研究では,床が水平になるようにSUVの位置情報を修正しているのでz座標およびピッ チθpとロールθrは修正する必要がない。そのためSUVのxy座標と左右方向の向きθyの みを重ね合わせにより修正する. ( 1 ) 点の選択 重ね合わせを行う入力フレームの点群から特徴点Fs,環境地図とのマッチングに用い る点Ms,点群のマッチングに用いる点Psを抽出する. FsはSURF (Speeded Up Robust Features) によりRGB画像の局所特徴量を計算して抽出する。抽出した特 徴点と対応する点が点群中に存在しない場合,その点は無視する. Msには点群のう ち変換後のz座標がSUVのレーザーレンジセンサーの高さあたりになる点を選択す る. Psは点群からサンプリングを行うことで求める. 本研究では、処理時間とマッ チング精度を考慮して、元の点群のおよそ4分の1の点を一様にサンプリングした. ( 2 ) 重ね合わせ先となるデータの選択 入力フレームの直前の数フレームのうち,入力フレームと画像特徴点が最も多くマッ チングするフレームをターゲットフレームとする. マッチングするフレームが存在し ない場合は入力フレームの直前のフレームをターゲットフレームとする. ( 3 ) マッチングと重み付け 手順1で選択した点Fs,Ms,Psと対応する点をそれぞれ求め, それらの集合をそれ ぞれAf, Am, Apとする. Af は入力フレームとターゲットフレームの画像特徴点 のマッチングを計算することで求める.このとき点fsに対応する点ftが存在しない 場合は点fsをFsから削除する. 変換後の座標は,センサーデータ上の点p,剛体変 換行列をtとして、t· pで求めることができる. Amは環境地図上の点を3次元座標 に変換したもののうち点t· msに最も近い点を点mtとして求める. Apはターゲッ トフレームの点群から点t· psに最も近い点を点ptとして求める. MsおよびPs の 最近傍の探索はk-d木を用いて行い,重みは全て均一とした. ( 4 ) 外れ値の除去 外れ値が存在すると誤差量の最小化に悪影響が出るためAf, Am, Apからそれぞれ 外れ値の除去を行う. AfではRANSACアルゴリズムを用いて正解データを抽出し, それ以外の点を外れ値として除去する. AmおよびApでは一定距離以上離れた点を 外れ値として除去する. また, Apでは深度画像上で点ptに隣接するピクセルにデー タが存在しない場合,その点を3次元モデルの境界として除去する. ( 5 ) 誤差量の計算と最小化 Afは2点間の距離の2乗、ApおよびAmは点と平面の距離の2乗から誤差量を求め る.誤差量は次式によって計算する.法線ベクトルは点pt周辺の点群および点mt周辺 の環境地図上の点の主成分分析によって求める. 本論文ではα = 0.3, β = 0.3, γ = 0.4 とした. e = α( 1 |Af|
∑
i∈Af wi|t(fsi)− f i t| 2 ) + β( 1 |Ap|∑
j∈Ap wj|(t(pjs)− p j t)· nj|2) +γ( 1 |Am|∑
k∈Am wk|(t(mks)− m k t)· nk|2) (α + β + γ = 1) w: 重み, t: 剛体変換行列, n: 法線ベクトル 本研究ではLevenberg-Marquardt法4),5)を用いて誤差量の最小化を行った. 手順3-5を誤差量が十分小さくなるか繰り返し回数が一定回数を超えるまで繰り返し行う. 3.2.3 環境地図との対応付け 3次元の点群と画像特徴点の対応だけでなく,環境地図との対応関係も利用してセンサー データの重ね合わせを行うことで,環境地図と対応の取れた3次元地図の生成を行った. 実 際に生成された3次元地図を図9に示す. 3次元地図がどの程度正確であるか検証するため, 3次元地図上のSUVのレーザーレンジセンサーの高さ付近のデータを環境地図にマッピン グしたものを図10に示す. 赤色の点が3次元地図上の点を環境地図にマッピングした点で ある. 生成した3次元地図が環境地図と正確に対応付けられていることが分かる.図 9 生成された 3 次元地図 Fig. 9 Example of 3D map
図 10 環境地図と生成された 3 次元地図の対応付け Fig. 10 3D map overlaid on a environmental map
3.3 3次元地図のポリゴン化 前節で生成した3次元地図は膨大な数の点の集合であり,データ量が非常に大きく扱うの が困難である. そこで,建物の内装を構成する主な要素である壁や床といった平面に着目し, 点の集合である3次元地図から平面を抽出しポリゴン化することで3次元地図の構造の単 純化を行う. 3次元地図全体から直接面を抽出することは困難であるため,一つ一つのセン サーデータから面を抽出し,抽出した面の重ね合わせを行うことで3次元地図のポリゴン化 を行う. 面の検出は3.2.1項の床の検出と同様のアルゴリズムで行う. ただし,ここでは面 の向きを指定しない. また,複数の面の検出するため,点群から検出された面に含まれる点 を取り除き,面が検出されなくなるまで繰り返し面の検出を行う. ポリゴンを描画するため, 面のテクスチャを生成し描画範囲を指定する必要がある. そこで,面に含まれる点を平面上 にマッピングし画像の平滑化を行うことでテクスチャ画像を生成し,その輪郭線と凸包を求 めることで描画範囲を指定する. 面の重ね合わせを行うため,面の法線の向きと重心の位置 から一致する面を検出し,一致する面が存在する場合には,入力面と一致した面の法線ベク トルをそれぞれの面の面積で重み付けした平均を求め,それを重ね合わせてできる面の法線 ベクトルとする. 入力面と一致した面からそれぞれ基準となる3点を重ね合わせてできる面 上にマッピングし,テクスチャ上の2次元アフィン変換を求めることで新しいテクスチャを 生成する. 図9の点群から成る3次元地図をポリゴン化した地図を図11に示す. 点群から成る3次 元地図は約1500万点の頂点データから構成され,約800MBのデータサイズであったのに 対し,ポリゴン化した3次元地図では面の枚数が34枚,データサイズは約30MBとなりか なり単純化することができた. 図 11 ポリゴン化した 3 次元地図 Fig. 11 Example of 3D polygon map
4.
お わ り に
本研究では,自律走行可能な小型無人移動体SUVにRGB-Dカメラを搭載し,データ収 集経路を生成することで自動的にデータの収集を行い,収集したデータから環境地図と対応 の取れた3次元地図を生成する仕組みを実現した. これにより,既存の位置推定の仕組みを そのまま利用して, 3次元地図をナビゲーションシステムなどに応用する事ができる. 本研究では,平面だけを抽出しポリゴン化を行ったが,実際の環境では机やいすなどのオブ ジェクトが存在する事が考えられるため,それらの3次元オブジェクトのポリゴン化を行う 必要がある. また,生成した3次元地図を利用した応用システムの開発が今後の課題である.参 考 文 献
1) Thrun, S., Thayer, S., Whittaker, W., Baker, C., Burgard, W., Ferguson, D., Hah-nel, D., Montemerlo, D., Morris, A., Omohundro, Z., Reverte, C. and Whittaker, W.: Autonomous exploration and mapping of abandoned mines, IEEE Robotics and
Automation Magazine, Vol.11, No.4, pp.79–91 (2004).
2) J., D.A.: MonoSLAM : real-time single camera SLAM, IEEE Trans. Pattern Anal.
Mach. Intell., Vol.6, pp.1052–1067 (2007).
3) Henry, P., Krainin, M., Herbst, E., Ren, X. and Fox, D.: RGB-D Mapping: Using Depth Cameras for Dense 3D Modeling of Indoor Environments, Proc. of the ISER (2010).
4) Levenberg, K.: A method for the solution of certain non-linear problems in least squares, Quarterly Journal of Applied Mathmatics, Vol.2, No.2, pp.164–168 (1944). 5) Marquardt, D.W.: An Algorithm for Least-Squares Estimation of Nonlinear
