人の多い混雑な環境下での SLAM による移動ロボットのナビゲーション○森岡博史 李想揆 Tongprasit Noppharit 長谷川修 (東工大)

Visual SLAM in Crowded Environments and Mobile Robot Navigation*Hiroshi MORIOKA, Sangkyu YI, Noppharit TONGPRASIT, Osamu HASEGAWA (TokyoInstitute of Technology)
Abstract— Existing methods of SLAM are not enough for robots needed to live in crowded environments
such as stations and shopping mall. In this paper, we propose a SLAM and navigation method which isrobust in the crowded environments
Key Words: SLAM, Mobile Robot, Monocular camera, Crowded environments, Hybrid maps

1.

はじめに近年のロボット産業の発展に伴い，警備ロボットや介護ロボットなどの，自律移動ロボットの導入が期待されている．一般に，移動ロボットが自律的にナビゲーションを行うためには，障害物の位置情報などを表した地図が必要となる．未知の環境で移動ロボットが自律的に地図構築・自己位置推定を行う問題はSLAM(Simultaneously Localization And Mapping) と呼ばれており，従来から広く研究されてきた．しかし従来のシステムでは，駅構内やショッピングモールのようにたくさんの人が入り乱れている環境下では，安定した地図構築・自己位置推定ができなかった．
そこで本研究では，複数の画像データから安定した特徴点を取り出すことで，たくさんの人が存在する混雑な環境下でオンライン・追加的に地図構築・自己位置推定を行い，人間が教えた道順通りに自律的に移動することを目的としている．

2.

先行研究

一般的な SLAM の手法では，環境は静的であると仮定して処理を行う．そのため，それらの手法を混雑な環境下で用いた場合，誤って人間のような動いている物をランドマークとして認識してしまうことによって，地図構築・自己位置推定に誤りが生じ，アルゴリズムが破綻してしまうという問題が起こる．
このような問題に対応するためには，大きく分けて2 種類の方法が考えられる．一つ目は，動いている物体を陽に扱う方法．二つ目は動いている物体を外れ値として取り除いて処理する方法である．
動いている物体を陽に扱う方法としては，例えば [9]では，最小二乗法と EM アルゴリズムを用いることで過去の観測点に対するデータ対応付けのやり直しを可能とし，動的な物体と静的な物体とを同じ枠組みで扱う手法が提案されている．
また，動いている物を外れ値として取り除く方法としては，例えば [4] ではエントロピーフィルタを用い，すでに知っている物と，人のように動いている障害物を分離することで，美術館という人が多い環境下で自Fig.1 人の多い混雑な環境の例．実験においてロボットが取得した画像であり，常に多くの人が出入りしている

己位置推定を行うツアーロボットが提案されたが，事前に正確な地図が与えられている必要があった．[5][6]では動く物体をトラッキングし，SLAM の処理から除外することで，精度の向上を行っている．しかしこれらの研究においては，環境の表現方法や，動いている物と止まっている物の分離方法など，未解決の問題が残されている．[7] では，事前に画像データから木などの静的なものを学習しておき，それをランドマークとして SLAM に利用することで，人や車などの，事前に学習したランドマーク以外の物体による誤差を抑えるという手法がとられている．しかし，事前に画像からランドマークを学習しておく必要があるため，ロボッ
トを違う環境に適応させる際，新しくランドマークを覚えなおす必要がある．[8] では，障害物の配置の変化など，長い時間尺度での環境の変化による新しい観測データと，過去に訪れた際の観測データとのトレードオフを考え，徐々に地図を修正する手法がとられている．しかし，動いている人のような，短い時間尺度での環境の変化に対するロバスト性については考慮されていなかった．
本研究では，動いている物体を取り除いて処理する方法として，画像から混雑な環境下でも安定な 3 次元上の特徴点を抽出し，その特徴点を用いることで，ロバストに SLAM・ナビゲーションを行う手法を提案する．

Fig.2 本研究で提案するシステム．連続画像とオドメトリから 3D-PIRF を抽出することでハイブリッドマップを構築し，ナビゲーションを行う

3.

提案手法

本研究ではセンサとして単眼全方位カメラと車輪のエンコーダを用いて地図構築を行う．また環境として駅構内やショッピングモールのような，たくさんの人が入り乱れている混雑な環境を想定する．そのような混雑な環境下では人などが常に動いているため，特徴点の対応付け・位置計測が難しく，SLAM を行うのは極めて困難である．
そこで本研究ではカメラから得られる特徴量として，人の多い混雑な環境下でも安定して得られる 3 次元上の特徴点を用いることで，混雑な環境下でロバストにSLAM・ナビゲーションを行う手法を提案する．本研
究で提案するシステムを Fig.2 に示す．以下，各処理の詳細を述べる．

3·1

Fig.3 連続画像から PIRF[1] を抽出．PIRF は各画像から抽出された SIFT 特徴点のうち，すべての隣り合った画像間でマッチングがとれた特徴点として抽出されるため，見た目の変化の大きい部分の特徴点を除外できる

Fig.4 連続画像の SIFT とオドメトリから 3D-PIRF を抽出．本論文で提案する 3D-PIRF は PIRF 特徴点の拡張であり，PIRF 特徴量とその 3 次元位置で表される

混雑な環境下でも安定な 3 次元上の特徴点の抽出

画像を用いた SLAM において多く用いられている特徴点として，SIFT やコーナー点がある．一般に SIFTやコーナー点などの局所特徴点は対応付けが取りやす
く，画像を用いた SLAM に適している．しかし，多くの人が入り乱れる混雑な環境下でロボットが取得した画像から SIFT を抽出すると，ロボットが少し移動しただけで消えてしまうような弱い特徴点や，動いている人物からの特徴点が多く抽出されてしまうという問題があった．そのような不安定な特徴点が多いと，無駄に処理時間が延びるだけでなく，地図構築に悪影響を及ぼすという問題が起こる．
そこで近年，PIRF(Position Invariant Robust Feature) という特徴量が提案された [1]．PIRF は複数枚の連続画像から SIFT を抽出して連続的にマッチングを取り，全ての画像でマッチングが取れた特徴点のみを抽出する手法である (Fig.3)．PIRF はロボットの移動に対して見た目の変化の小さい部分を特徴として抽出することができ，それにともなって，動いている人物などから抽出される見た目の変化の大きい部分の特徴点を除外できるという特徴がある．
しかし PIRF は画像から抽出される画像上の特徴点であり，直接 SLAM に適用することはできない．そこで本研究では，連続画像の SIFT にオドメトリの情報を組み合わせることで，PIRF を 3 次元空間にマッピングする (3D-PIRF) 手法を提案する．Fig.4 は 3D-PIRFの概念図を表している．3D-PIRF は複数のステップにおいてロボットが取得した連続画像と，各ステップ間のオドメトリの情報から，3 次元上の特徴点として抽出される．
ここで，3D-PIRF を抽出するためには特徴点の 3 次元位置を計算する必要があるが，3D-PIRF は過去数ステップの観測データを用いるため，ロボットの現在の状態しか保持しないカルマンフィルタなどの手法と組み合わせることができないという問題がある．
そこで本研究では SWF(Sliding Window Filter)[2]を用いる．SWF はカルマンフィルタなどのオンライン型の手法と，過去全てのデータを用いるバッチ型の手法との中間的な手法であり，一定期間よりも古い姿勢・特徴点を消去していくことで，計算時間を一定に保つことができるという特徴がある．これを用いることで，3D-PIRF の 3 次元位置を計算することができ，連続画像とオドメトリの情報から 3D-PIRF を抽出することが可能となる．
このように 3 次元上の特徴点である 3D-PIRF を用いることで，人の多い混雑な環境下でもロバストに，かつ特徴点の位置情報を用いた SLAM・ナビゲーションを行うことが可能となる．

3·2 局所地図を用いたハイブリッド地図の構築
SLAM には大きく分けて，地図を連続的な空間配置で表すメトリカルな手法と，地図が離散的なグラフ構造で表されるトポロジカルな手法，それらを組み合わせたハイブリッドな手法がある．ハイブリッドな手法
では，各ノードが局所的なメトリカル地図の情報を持つようなトポロジカルな地図が構築される．
本研究では，ハイブリッドな手法を用いる．ハイブ
リッドな手法はグラフ構造を有するため経路計画が容易であり，距離情報も用いることが可能であるため，最短経路の探索など，より高度な経路計画を行うことが可能であるという特徴がある．
本手法で構築するハイブリッド地図は Fig.2 に示すものである．ロボットの軌跡はノードとエッジによるグラフで表され，各ノードはノード位置周辺で観測した
3D-PIRF の位置 mx と分散 Πx の情報を持ち，各エッジはノード間の相対位置 v の情報を持つ．
3·3 Loop-Closing による軌跡の修正
一般的に SLAM においては，移動距離が長くなるほど誤差が蓄積していくという問題がある．地図の誤差修正を行うためには，現在いる場所が過去に訪れたことのある場所かどうかを認識する必要があるが，そのための手法としてオンラインでの認識が可能な [2] を用いる．この手法では，混雑した環境下においてオンラインで過去に訪れたことのある場所かどうかを認識(Loop-Closing 検出) することが可能であるという特徴がある．
Loop-Closing が検出されると，Loop-Closing による情報と各時刻のロボットの姿勢 x，ノード間の相対位置 v から計算される評価関数を最適化するよう軌跡が修正される (Fig.7)．また，Loop-Closing が検出された
位置では，前に訪れた際にノードの情報として保存した局所地図における特徴点の位置と共分散の情報を用いることで，過去の局所マップを考慮した地図構築を行う．このように過去の局所地図と現在の観測データを組み合わせることで，局所地図の精度の向上を行うことが可能となる．
3·4 ナビゲーション
本研究で目標とする移動ロボットの自律移動には学習フェーズとナビゲーションフェーズがある．
学習フェーズは，人がロボットを操縦することでロボットに通ってほしい道順を教える段階であり，ロボットは移動しながら，前節までで述べた手法によりハイブリッド地図を構築する．
次のナビゲーションフェーズでは，学習フェーズで教えられた道順通りにロボットが自律的に移動する．移動ロボットは学習フェーズと同様にハイブリッド地図を構築しながら移動するが，ナビゲーションフェーズでは，教えられた道順通りに移動するために，学習フェーズで学習したハイブリッド地図と現在のハイブリッド地図それぞれの軌跡と局所マップを比較することで，学習した軌跡に追従するように行動計画を行う．

4.

実験結果

実験は 20[m]×20[m] 程度の屋内環境 (Fig.1) で行った．この環境は食堂であり，常に多くの人が出入りし，Fig.5 学習フェーズで移動させた道順．実験は Fig.1 の環境で行い，常に多くの人が出入りしている．以下，Fig.6 から Fig.8 まで同様の環境での実験である

混雑している．このような常に人がいる環境の場合，従来の手法では動いている人から抽出される特徴点が悪影響を与え，ロバストに SLAM・ナビゲーションを行うことができなかった．一方本手法では，混雑な環境下でもロバストに SLAM・ナビゲーションを行えるという特徴がある．
まず学習フェーズでは，全方位カメラとエンコーダセンサを搭載したロボットが人間の操縦によって，提案手法を用いて自己位置推定と地図構築を行いながら移動する．Fig.5 は環境の真値地図と，学習フェーズでロボットを移動させた道順を表しており，まず左下の赤い実線で示した小さいループを 2 週，次に右上に移動し，そこから青の点線で示した大きなループを 2 週．
最後に右上の緑の一点鎖線で示した小さいループを 2週させた．
Fig.6 は以上の経路に対してカメラの情報を使わずオドメトリのみから計算したロボットの軌跡である．図において，青色の丸はスタート地点，水色の丸はゴール地点を表しており，ルートはスタート地点から徐々に色が薄くなるように描かれている．この図から，オドメトリのみから計測した場合には移動距離が延びるほど自己位置推定の誤差が溜まり，正しい軌跡が学習出来ていないことがわかる．
一方 Fig.7 は提案手法によりロボットが学習した軌跡である．図において，赤い丸は過去に訪れたことがあると認識 (Loop-Closing 検出) された位置を表している．この図から，移動距離が延びても誤差が溜まっておらず，ルートを正しく学習出来ていることが分かる．
なお，現在は学習した軌跡の正しさを，スタートからゴールまでの軌跡の重なり度合いと，環境の真値地図との比較から定性的に評価しているが，定量的な評価ができておらず，今後の課題である．
次にナビゲーションフェーズでは，学習フェーズで学習した軌跡に沿うように，ロボットが自律的に移動する．今回は左下から右上まで移動するように経路計画を行った．Fig.8 がロボットが自律的に移動した軌跡である．図において，学習フェーズで学習した軌跡を

Fig.6 オドメトリのみから計算した軌跡．徐々に誤差が蓄積している

Fig.8 ナビゲーションフェーズでのロボットの軌跡．ス
タート地点からゴール地点まで自律的に移動出来ている．

マノイドなど，様々なものに適用することが可能であり，広い応用可能性がある．
今後の課題としては，3D-PIRF のより洗練された抽出方法の検討．また，最短経路探索などナビゲーションフェーズでのより高度な経路計画などが挙げられる．
謝辞
本研究は NEDO 産業技術研究助成事業から支援を頂きました．記して感謝致します．
参考文献

Fig.7 提案手法により学習した軌跡であり，Fig.6 から改善されているグレーで，ロボットが移動した軌跡を青で示しており，学習フェーズで学習した軌跡に対して Loop-Closing 検出がされた位置を赤い丸で示している．この図から，ロボットは学習フェーズで学習した左下から右上に至る経路に沿うように自律的に移動し，ゴール地点まで到達できていることがわかる．

5.

まとめ

本 稿 で は ，人 の 多 い 混 雑 な 環 境 下 で ロ バ ス ト にSLAM・ナビゲーションを行う手法として，画像から得られる，人の多い混雑な環境下でも安定な特徴点を用いて SLAM・ナビゲーションを行う手法を提案し，多くの人が常に映りこんでいる状況下でも安定して SLAM・ナビゲーションが行えることを確認した．
この技術は将来的にロボットを実環境に適応させ，人と共存させるためには必要不可欠な要素であり，ロボットの実用化に大きく貢献することが期待できる．また，本手法は 2 輪の移動ロボットだけでなく，車やヒュー[1] A.Kawewong, et al.:”Online Incremental Appearancebased SLAM in Highly Dynamic Environments”, International Journal of Robotics Research (IJRR),2010[2] G.Sibley, et al.:”Constant time sliding window filter
slam as a basis for metric visual perception”, IEEE International Conference on Robotics and Automation(ICRA) Workshop ,2007[3] N.Tongprasit, et al.:”A Fast Online Incremental
Loop-Closure Detection for Appearance-based SLAMin Dynamic Crowded Environment”, MIRU ,2010[4] W.Burgard, et al.:”Experiences with an interactive
museum tour-guide robot”, Artificial Intelligence,
114(1-2), 3-55, 1999
[5] C.C.Wang: ”Simultaneous Localization, Mappingand Moving Object Tracking”, The International
Journal of Robotics Research (IJRR),2007[6] D.Wolf, G.S.Sukhatme: ”Online Simultaneous Localization and Mapping in Dynamic Environments”,
Proc. of ICRA, 2004
[7] F.T.Ramos, et al.:”Recognising and Modelling Landmarks to Close Loops in Outdoor SLAM”, Proc. ofICRA, 2007
[8] P.Biber, T.Duckett: ”Dynamic Maps for LongTerm Operation of Mobile Service Robots”, Proc. ofRobotics: Science and Systems (RSS), 2005
[9] C.Bibby, I.Reid: ”Simultaneous localisation and mapping in dynamic environments (SLAMIDE) with reversible data association”, Proc. of Robotics: Scienceand Systems (RSS), 2007