ステレオカメラによるビジュアルオドメトリおよび自己位置認識 ○岩田啓明（東京工業大学），実吉敬二（東京工業大学） Visual Odometry and Localization by Stereo Vision ○Hiroaki IWATA, and Keiji SANEYOSHI (Tokyo Institute of Technology) Abstract: We have developed methods of getting an odometry and an object map, and a method of localization on the map by using only stereo vision. Odometry is computed using objects detected according to the height from the road. Then the map is made using the objects and odometry. When a robot is running autonomously, it can be localized on the map by means of matching the map and the detected object. 1. はじめに算出を行う．また特徴点を個々に対応させるのではな自律移動をするためには走行軌跡（オドメトリ）推く，データ全体の一致度を元に移動量を決定する．こ定・環境マップ生成およびマップ内における自己位置のためカメラの動きが大きくても，また移動物体が一の認識を行う必要がある．これらを実現するために部映っていても安定して移動量を求めることができる GPS・車輪・カメラ・レーザー等のセンサを組み合わせことが特長である．そしてその軌跡を用いてマップをる研究がおこなわれている．作成し，マップ‐フレーム間で同じマッチング手法を GPS は絶対位置を求めることができるものの，環境用いてマップ内における自己位置認識を行う．自律移によって結果が不安定になったり，観測不能になった動車に搭載することを想定し，2 次元的なオドメトリ算りすることがある．そのため，実際にはその他のセン出および自己位置認識を行う．サにより補助しなければならないし，また周囲環境もその他の手段で観測しなければならない．車輪は累積誤差が発生し，それを補正するためにやはりその他の手段に頼らなければならない．このようにそれぞれの欠点を補うため様々な種類のセンサを組み合わせて用いることが主流である[2]． 2. 本手法の概要本手法は事前に走行させ撮影したデータから走行軌跡とともに物体マップを作成し，自律走行時には，これらのデータを用いて現在位置を認識することを想定している．しかし我々はシンプルなシステムをめざし，ステレオカメラ[1]のみで自律走行を行うことを考えている．ステレオカメラは，測距センサとしてはレーザーレーダーと比べて高解像度・安価・安定した観測が可能であることや，パッシブセンサなので人体への危険がないという特長がある．もちろん測距センサとしてだけではなく通常のカメラとして，詳細なテクスチャ情報も得られる．カメラを用いる場合フレーム間の移動量を累積して軌跡を算出するが，累積誤差が問題になる．そこでマップを用いて自己位置を補正することでこの問題を解決する．またそのマップもステレオカメラのみで作成する．カメラを用いた移動軌跡検出（ビジュアルオドメトリ）システムとしては，画像の輝度パターンの特徴点を用いたシステムがある[3]．これに対して本手法では領域ベースマッチングで得られた高解像度の視差データから路面を認識し，路面上の立体物を路面からの高さ別に検出して特徴点とする．その特徴点をフレーム間でマッチングさせることによりカメラの走行軌跡の第11回システムインテグレーション部門講演会(SI2010)（2010年12月23日～25日・仙台） -2174- Fig . 1 The process flowchart SY0016/10/0000 - 2174 © 2010 SICE 処理の流れを Fig . 1 に示す．連続して撮影した画像からフレーム間マッチングまでの共通処理とそこから２つに分岐している処理からなる．一方は事前に走行させてマップを生成する場合，もう一方はあらかじめ生成したマップを用いて自律走行時にマップ内における自己位置を認識する場合に用いる．使用したステレオカメラは解像度 2048×2048 画素，基線長 5cm，焦点距離 5mm で補正校正処理後に領域ベースマッチングにより視差を計算している．マッチングウィンドウサイズは 4×4，探索範囲は 128 画素である．このカメラにより得られる基準画像および視差画像をそれぞれ Fig . 2 左上および左下に示す． 3. 路面平面の算出まず、路面を平面として推定する．車両に取り付けたステレオカメラは障害物と路面が見られるように車両前方に若干下に向けて設置してあるため，画像の約 Fig . 2 Disparity image and detected objects. 半分以上は必ず路面が映っている．このため手法とし Upper left: base image, left lower: disparity image, ては視差画像の下半分のデータからハフ変換で平面を Right side: From top to bottom, detected objects 求める．ハフ変換により安定して路面位置を求めるこ ranging 180cm-230cm, 100cm-150cm and 10cm-60cm とができる．ここで求めた平面に沿って物体を検出す respectively. Horizontal axis: u, vertical axis: disparity, る．路面を基準にすることでロボットの振動などに強 D. い物体の検出が可能である． 5. フレーム間の移動量の推定 4. 物体の検出ここでは路面からの 3 つのレベルの高さ帯ごとに物体を検出する．設定したレベルは路面からの高さがそれぞれ 10cm-60cm，100cm-150cm，180cm-230cm の範囲とした。そして各レベルにおいて画像を幅𝑊𝑇 で縦方向の列に区切って列ごとに物体があるかを判定し，あるならその視差を求める[1]．方法としては，視差画像における該当レベル・列内のデータに対して視差のヒストグラムをとる．その列に物体がある場合，同じような視差にデータがまとまっているので，その視差の度数が高くなり，その度数は同じ大きさの物体であれば視差に比例すると考えられる．そこで視差に比例する閾値を設定し，度数の最大値が閾値を超えたら物体があるとする。物体の視差は最大度数のグループとその隣の度数の大きい方のグループのデータの視差の平均とする. このようにして各レベルで物体データとして物体の u 座標と視差 D の組（視差空間座標）が得られる．1 レベル当たり最大（画像幅/𝑊𝑇 ）個のデータが検出される．検出した物体データを用いてフレーム間の移動量を推定する．個々の物体データをフレーム間で対応付けることはせず，フレーム間のデータ全体でのマッチング度を求め，もっともよくマッチする移動量を求める．そのために前フレームの物体データ *𝑝𝑖′ +，現フレームの物体データ *𝑝𝑖 + ，フレーム間の移動量 Θ = (∆𝑥, ∆𝑧, ∆𝜃)に対して尤度関数を以下のように定義した． 𝑦は*𝑝𝑖 +と*𝑝𝑖′ +を合わせたベクトル，𝑅Θ (∙)は与えられた視差空間の座標を実空間に変換しΘにしたがって移動後再び視差空間の座標に変換する関数とし，𝑐は𝑅Θ (𝑝𝑖′ ) が視野（観測範囲）内に入る個数とする． 1 𝑓(𝑦|Θ) = ∑ ∑ 𝑣(𝑅Θ (𝑝𝑖′ ) − 𝑝𝑖 ) 𝑐 𝑖 𝑗 |𝑥 | | 𝑣(𝑥) = {1, 𝑢 < 1.5𝑊𝑇 かつ|𝑥𝐷 < 1.5 0, 𝑜𝑡ℎ𝑒𝑟𝑤𝑖𝑠𝑒 実空間(𝑋, 𝑍)で評価する場合、距離𝑍の誤差が𝑋成分に影響を与えてしまう．評価を視差空間で行うことによこれを u 座標に対するルックアップ形式でレベル別にり，２変数間の誤差の伝播がないように分離した．ま固定長メモリ内に保存する．た，データの観測誤差が視差空間では一定になると期物体検出結果を Fig . 2 右側に示す．横軸に u 座標・待でき，処理が簡素化できる．長い側壁が写っているようなシーンでは，実際には縦軸に視差 D で検出した物体をプロットしている．壁面に沿って前に移動していても検出される壁面の物 -2175- 体データとしてはフレーム間で変化がないことになる． 5 で述べた手法からフレーム間の物体データの一方しかし c で割らない場合，カメラが移動すると視野外に（前フレームの方）をマップからのデータとすること移動するデータが発生し評価値が減少し，また壁面自でマップマッチングを行った．このようにすると 1 度体のデータ数も多くなるため，木や電柱のようなそのマップからデータを取り出すだけで済むため，マッチ他の手掛かりがあってもカメラが移動しない方が評価ング本体の処理時間はマップのデータ数によらずにフが高くなってしまうことがある．c で割ることによって，レーム間マッチングと同様に計算できる．視野外に物体が移動することがペナルティにならない実際にはまずフレーム間マッチングにより現在位置ようにして，その他の手掛かりの効果を生かせるようの予測を行い，次にマップマッチングで現在位置を修にした．正するという流れで行う．具体的には各フレームで以 𝑣(𝑥)については，ある程度のゆらぎを許容しつつ，離下のような設定で処理を行い，カメラ位置を更新する．れた場合は０を返すことでノイズに強くなるように井１）フレーム間移動量戸型の関数とした．各高さレベルごとに𝑓を求め，最も上層のレベルの値 *𝑝𝑖′ + : 前フレームで観測した物体列を重み 2，その他のレベルの値を重み 1 として合成する． *𝑝𝑖 + : 現フレームで観測した物体列最も上層のレベルの値の重みを大きくするのは，この Θ′ : 前フレームでのフレーム間移動量の値レベルは人よりも高いため移動物体が映っていないと２）マップ‐現フレーム間移動量考えられ，信頼性が高いためである． *𝑝𝑖′ + : 現在位置とマップから作成した物体列尤度関数のみでは手がかりとなる物体が少なかったりする場合等の原因で大きくミスマッチしてしまうこ *𝑝𝑖 + : 現フレームで観測した物体列とがあり，これを抑えるため移動速度一定を仮定する Θ′ : 0 事前分布の因子を付けた． 𝜋(Θ) = 𝑒 − このように，フレーム間とマップ‐現フレーム間処理の両方を行うことで相互に補完し合い，ロバスト性 (∆𝑥−∆𝑥 ′ )2 (∆𝑧−∆𝑧 ′ )2 (∆θ−∆θ′ )2 − − 2𝜎𝑥 2 2𝜎𝑧 2 2𝜎θ 2 が高まることが期待できる． ∆𝑥 ′ , ∆𝑧 ′ , ∆θ′ は前フレームの移動量の各成分とする．各 8. 実験成分とも𝜎は大きめにとり緩やかな形とした． 8.1 移動軌跡算出およびマップの生成そして MAP 推定の要領で𝑓(Θ|𝑦) ∝ 𝑓(𝑦|Θ)𝜋(Θ)を最大化するΘを求め，結果とする．求め方は移動可能な範囲内をある程度細かく全探索して求める．ステレオカメラを台車に乗せ，約 0.5m/s の速度でつくば公園内を約 160m 移動させながら，0.5fps のフレームレートで撮影し，これまでに述べた方法でカメラの移動軌跡およびマップ生成を行った． 6. マップ生成求めた軌跡を用いて，検出した物体データを統一した実座標系(𝑋, 𝑍)に記録する．この時に同じ物体が複数のフレームで観測されることは頻繁に起こるが，このような重複したデータの中で精度のよい最も接近したデータのみをできるだけ記録するようにしたい．そこで簡易的に，検出した物体の内，次のフレームで視野から消える物体を記録した．結果を Fig . 3 に示す．得られた軌跡を赤い線で，マップに記録された物体の内 10cm-60cm の物体を点で表示している．また，観測時の観測点から物体までの距離に応じて点を色分けしている．軌跡についてはほぼ走行させた経路通りに求まった．物体データはおおむね重複が除去され最も近くで観測したデータが残っているが，池の前の右カーブを過ぎた直後の所で軌跡上に物体が残っている．これはこの領域が一度視野から外 7. 自己位置認識れ再び視野に入ってきたために一度目の精度の悪い自律走行時にはマップ内における自己位置を求める必要がある． -2176- データが残ってしまったためである． Fig . 3 Result of the estimated trajectory of motion and objects recorded on the map. Red line: the estimated trajectory of motion, the points: objects recorded on the map (They are color-coded according to the distance between them and camera position at the moment of detecting them.), the photo is issued by The Geospatial Information Authority of Japan. 9. まとめステレオカメラのみを用いて路面を認識し物体を高さ別に検出し、検出した物体を用いてオドメトリ算出・マップ生成およびマップ内自己位置認識を行う手法について述べ，実際に撮影した画像を用いた実験を行った．今後これらの手法をロボットに組み込み自律走行を行う予定である．また，テクスチャ情報なども取り入れ精度を上げることも考えている．参考文献 Fig . 4 The result of localization 8.2 自己位置認識先ほど作成したマップを用いて自己位置認識のテストを行った．観測画像としてマップ作成時と同じ画像データを用いる．この場合自己位置認識手順１）のフレーム間移動量がマップ作成時と一致してしまうので，独立にガウスノイズを加えて意図的にずらす，そして，２）のマップ‐フレーム間処理時に元の軌跡の位置に戻れば自己位置が認識できたことになる．処理結果を Fig . 4 に示す．赤色がマップ作成時の軌跡，緑色が移動量にノイズを加えた軌跡，青色が自己位置認識結果を示している．この結果から，自己位置認識結果が元のマップ生成時の軌跡にほぼ一致しておりマップ‐フレーム間マッチングに成功していることがわかる． -2177- [1] K.Saneyoshi, K.Hanawa, K.Kise: "Image Recognition System for Active Drive Assist", AVEC ‘92, 280-285 (1992) [2] 大谷和彦，永谷圭司，吉田和哉："GPS およびオドメトリ機能を搭載した移動ロボットの不整地フィールドにおける位置推定実験"，第 10 回計測自動制御学会システムインテグレーション部門講演会，(2009) [3] 友納正裕："エッジ点追跡に基づくステレオカメラを用いた三次元 SLAM"，第 26 回日本ロボット学会学術講演会， (2008)