JP6913339B2 - 移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラム - Google Patents

Description

本発明は、移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラムに関する。

近年、外界の状況をリアルタイムに判断しながら、自らの運動を自律的に制御可能な移動ロボットの開発が急速に進められている。

そしてそのための技術として、移動ロボットに外界を認識するためのレーザスキャナやカメラなどの外界センサを搭載し、外界センサから得られる情報を移動ロボット自らがリアルタイムに解析することで、外界の地図を構築しつつ、自らの運動状態を推定するＳＬＡＭ（Simultaneous Localization and Mapping）と呼ばれる技術が開発されている（例えば特許文献１及び非特許文献１参照）。

特開２００８−１９７８８４号公報

S.Thrun, W.Burgard, and D.Fox: Probabilistic Robotics, MIT Press, 2005.

ＳＬＡＭを用いて移動ロボットを制御する場合には、過去に訪れたことのある場所（再訪点）を再び訪れたことを検出することによって、移動ロボットがそれまでに算定してきた自らの移動軌跡の誤差を修正するループ閉じ込みと呼ばれる手法を用いることができる。

しかしながら、ループ閉じ込みの処理は負荷が大きい上、多くの場合、一つの再訪点に対してその前後でも同様の再訪点が複数検出されるため、ループ閉じ込みの処理を何度も実行することになる。

また移動ロボットは、様々な情報処理を実時間内に行わなければならないため、効率的に情報処理を実行しなければならない。

本発明はこのような点を鑑みてなされたものであり、移動体の再訪点をより効果的に特定し、移動体の移動軌跡を算定する際の処理負荷を軽減することが可能な移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラムを提供することを一つの目的とする。

なお本発明は、例えば自転車や自動車、人をも含む、ロボット以外の広義の移動体にも適用して、自律移動制御以外の様々な分野（例えば地図作成の支援など）への応用も可能である。

一つの側面に係る移動軌跡算定システムは、移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムであって、前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得するスキャンデータ取得部と、第１タイミングで取得したスキャンデータと、前記第１タイミングから第１所定時間後の第２タイミングで取得したスキャンデータと、を照合することにより、前記第１所定時間における前記移動体の移動ベクトルを求める移動ベクトル算出部と、前記第１所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第１所定時間毎の各通過地点の位置情報を求める位置情報算出部と、前記移動体の前記移動ベクトルの大きさを累積することにより、前記移動体の移動開始地点から前記各通過地点までのそれぞれの累積移動距離を求める累積移動距離算出部と、前記各通過地点のうち前記移動体の現在位置からの距離が所定値よりも小さな通過地点を前記移動体の再訪点として検出し、前記再訪点が複数検出された場合には、前記複数の再訪点のそれぞれの前記累積移動距離の差が所定値以内のもの同士が同一グループになるように前記複数の再訪点を分類し、前記各グループから、前記移動体の前記現在位置に最も近接する再訪点を一つ選出する再訪点検出部と、を備える。

その他、本願が開示する課題、及びその解決方法は、発明を実施するための形態の欄の記載、及び図面の記載等により明らかにされる。

本発明によれば、移動体の再訪点をより効果的に特定し、移動体の移動軌跡を算定する際の処理負荷を軽減することが可能となる。

本実施形態に係る移動体及び移動軌跡算定システムの構成を示す図である。 本実施形態に係る経路情報記憶部を示す図である。 本実施形態に係る移動体の位置情報、移動ベクトル及び累積移動距離を示す図である。 本実施形態に係る再訪点を示す図である。 本実施形態に係る再訪点の検出方法を説明するための図である。 本実施形態に係る再訪点の検出方法を説明するための図である。 本実施形態に係る移動軌跡の位置ずれを説明するための図である。 本実施形態に係るループ閉じ込みを行う際のＩＣＰの初期値を説明するための図である。 本実施形態に係る処理の流れを説明するためのフローチャートである。 本実施形態に係るスキャン点を説明するための図である。 本実施形態に係るスキャン点を説明するための図である。 本実施形態に係る地図を説明するための図である。 本実施形態に係る移動軌跡を説明するための図である。 本実施形態に係る再訪点の検出結果を説明するための図である。

本明細書および添付図面の記載により、少なくとも以下の事項が明らかとなる。

＝＝第１実施形態＝＝
本発明の第１実施形態に係る移動軌跡算定システム１００について、図面を参照しながら説明する。

本実施形態に係る移動軌跡算定システム１００は、例えば自律移動ロボットのような移動体１０００に搭載され、移動体１０００から観察される外界の存在物との相対的な位置の変化を検出することにより移動体１０００の移動軌跡を算定する機能を有する装置である。

移動軌跡とは、移動体１０００の位置姿勢の時系列を意味する。移動体１０００の位置姿勢とは、たとえば、移動体１０００が２次元平面上を移動する場合は、所定の２次元座標系における位置座標および方位角(x,y,θ)である。

なお詳細は後述するが、本実施形態に係る移動軌跡算定システム１００は、移動体１０００の移動中にリアルタイムに周囲の地図の生成も行っている。

＝移動体＝
移動体１０００は、例えば自動運転車、ヒューマノイドロボット、パーソナルモビリティなど、自らの判断で自らの運動を自律的に制御することが可能な装置である。

図１に示すように、移動体１０００は、移動軌跡算定システム１００、距離センサ２００、運動制御部４００、アクチュエータ５００を備えて構成される。

＜距離センサ＞
距離センサ２００は、外界の存在物までの距離を計測するセンサである。本実施形態に係る距離センサ２００は、レーザパルスを出射する光源と、レーザパルスの反射光を検出する検出器とを備えた２次元（２Ｄ）レーザスキャナであり、所定の方向に向けて出射したレーザパルスが往復する時間を計測することで計測点までの距離を算定する。

また本実施形態に係る距離センサ２００は、レーザパルスの光源及び検出器が所定速度で回転し、所定角度毎あるいは所定時間毎にレーザパルスを出射し、その反射光を検出する。本実施形態では、距離センサ２００は左回転に毎秒１０回転で回転し、光源が１°回転する毎にレーザパルスを出射する。これらの回転の向きや回転速度、レーザパルスの出射頻度の値は一例であり、距離センサの機種によって異なる。

そして距離センサ２００は、計測した距離及びその時のレーザパルスの出射方向を計測結果として出力する。

なお距離センサ２００は、レーザパルスが計測点まで届かなかった場合や反射光が検出できなかったような場合には、計測結果を出力しない。このため、本実施形態に係る距離センサ２００は、光源が１回転する間に、最大で３６０カ所の距離及び方向を計測結果として出力するが、計測結果が３６０個よりも少ないこともある。

本実施形態では、光源が１回転する間に得られる計測結果の集合をスキャンデータあるいはフレームと記す。また所定個数のスキャンデータごとに（例えば１０個毎に）選び出されたスキャンデータをキーフレームと記す。また距離センサ２００によって距離及び方位が計測された外界の計測点をスキャン点と記す。

また本実施形態では、光源が１回転する時間（０．１秒）を単位時間とし、この単位時間毎の離散時刻をタイミングと記す。このため、光源が１回転する間に得られた計測結果は、同じタイミングで計測されたものとして処理される。タイミングは、実際の測定時刻とは別のものであり、データの通し番号に相当する離散時刻である。

なお、距離センサ２００は、レーザ光が水平方向に沿って回転するように移動体１０００に装着される。

このような本実施形態で採用するレーザスキャナは、２次元（２Ｄ）レーザスキャナや３次元（３Ｄ）レーザスキャナを含め、レーザ光の光源を機械的に回転させて周囲の存在物までの距離を逐次的に計測するため、一般に広い視野を確保できる。

そしてこのようなレーザスキャナには、レーザ光の往復時間を計測するものの他に、レーザ光の送信信号と受信信号の位相差を計測するものや、レーザ光の出射点と受光点との間の距離に基づく三角法を用いるものがあり、距離センサ２００はそれらのどれでもよい。本実施形態では、代表として２Ｄレーザスキャナを扱うこととするが、３Ｄレーザスキャナでもよい。またレーザスキャナの他に、複数の受光素子を用いて複数地点の距離を同時に測定する距離画像カメラを用いてもよい。

＜移動軌跡算定システム＞
移動軌跡算定システム１００は、移動体１０００に搭載され、外界の存在物との相対的な位置の変化を検出することにより、移動体１０００の移動軌跡を算定する装置である。詳細は後述する。

＜運動制御部＞
運動制御部４００は、移動軌跡算定システム１００により推定された移動体１０００の所定時間毎の並進量及び回転量（移動ベクトル）や、距離センサ２００により得られるスキャンデータを元に作成される外界の地図を用いて、移動体１０００を移動させるためのアクチュエータ５００を駆動する。

＜アクチュエータ＞
アクチュエータ５００は、移動体１０００を移動させるためのモータやバルブ、エンジンなどの動力発生装置である。

＝移動軌跡算定システム＝
次に、上述した移動軌跡算定システム１００について詳細に説明する。

移動軌跡算定システム１００は、図１に示すように、スキャンデータ取得部１１０、経路情報記憶部１２０、移動ベクトル算出部１３０、位置情報算出部１４０、再訪点検出部１５０、累積移動距離算出部１６０、ループ閉じ込み処理部１７０、地図作成部１８０、地図情報記憶部１９０を備える。

なお、移動軌跡算定システム１００は、不図示のＣＰＵ（Central Processing Unit）、ＲＡＭ(Random Access Memory)やＲＯＭ(Read Only Memory)、ハードディスク装置などの記憶装置、各種のデータ入出力インタフェース等の機器を有するコンピュータにより構成される。そしてＣＰＵが記憶装置に記憶されているプログラムを実行することにより、本実施形態に係る移動軌跡算定システム１００が有する各種機能が実現される。

また移動軌跡算定システム１００は、データ入出力インタフェースを通じて、距離センサ２００などの外部機器からデータを取得する。またプログラムがＣＤＲＯＭやＤＶＤ等の記録媒体に記録されている場合には、これらの記録媒体からプログラムを記憶装置にロードすることができる。あるいはプログラムが他のコンピュータに格納されている場合には、インターネット等の通信ネットワークを介してこのコンピュータからプログラムを記憶装置にダウンロードすることができる。

＜スキャンデータ取得部＞
スキャンデータ取得部１１０は、距離センサ２００から出力される外界の各計測点（スキャン点）までの距離及び方向を示す計測結果を順次取得する。そしてスキャンデータ取得部１１０は、距離センサ２００による距離の計測方向が１回転する毎に、１回転分の計測結果をスキャンデータＰ_ｔ（フレームＰ_ｔ）としてまとめ、後述する経路情報記憶部１２０に記憶する。

このようにしてスキャンデータ取得部１１０は、距離センサ２００によって所定周期（本実施形態では0.1秒毎）で計測される外界の各点までの距離及び方位を含むスキャンデータＰ_ｔを取得する。

なお、各スキャンデータＰ_ｔ内の距離及び方向は、計測時点の移動体１０００の位置を基準にした局所座標で表わされている。

またＰ_ｔのｔは、タイミングを表す整数である。上述したように、タイミングは、距離センサ２００の計測方向が１回転する時間を単位時間とする離散時刻である。このスキャンデータＰ_ｔのタイミングを示す整数ｔは、スキャンデータ取得部１１０によって適宜、付番される。

スキャンデータ取得部１１０が取得したスキャンデータＰ_ｔを、図１０及び図１１を参照しながら説明する。上述したように、スキャンデータＰ_ｔには、外界の各計測点（スキャン点）までの距離及び方向を示すデータが含まれているが、図１０及び図１１は、それらの各計測点の位置を２次元平面上にプロットしたものである。

このとき、図１０及び図１１においてｒ_ａ、ｒ_ｂで示される点は、距離センサ２００すなわち移動体１０００の位置を表す。

なお、上述したように、距離センサ２００はレーザバルスを３６０°の全方向に回転させながら出射し、その反射光を検出しているが、図１０、図１１に示されるように、全てのレーザパルスの反射光が検出できるわけではない。

スキャンデータ取得部１１０は、このようにして取得したスキャンデータＰ_ｔを、スキャンデータＰ_ｔを取得する毎に順次、経路情報記憶部１２０に記憶する。

＜経路情報記憶部＞
つぎに、経路情報記憶部１２０を図２に示す。

経路情報記憶部１２０には、図２に示すように、各タイミング毎のスキャンデータＰ_ｔが記憶されている。図２に示す例では、スキャンデータＰ_０〜Ｐ_ｎが記憶されている。Ｐ_ｎは、直近のタイミングｎに取得したスキャンデータＰ_ｎ、つまり現在位置ｎにおけるスキャンデータＰ_ｎである。

また詳細は後述するが、経路情報記憶部１２０には、スキャンデータＰ_ｔ（ｔ＝０〜ｎ）の他に、各タイミング毎に、移動ベクトルｍ_ｔ（ｔ＝０〜ｎ）、位置情報ｒ_ｔ（ｔ＝０〜ｎ）、再訪点ｒ_ｓ、累積移動距離Ｌ_ｔ（ｔ＝０〜ｎ）、及び第２相対位置ベクトルｖ_２,ｔ（ｔ＝０〜ｎ）がそれぞれ対応づけて記憶されている。

＜移動ベクトル算出部＞
図１に戻って、移動ベクトル算出部１３０は、ある第１タイミングｉで取得したスキャンデータＰ_ｉと、この第１タイミングから第１所定時間後の第２タイミングｊで取得したスキャンデータＰ_ｊと、を照合することにより、第１所定時間における移動体１０００の並進量及び回転量、すなわち移動ベクトルｍ_ｊを求める。そして移動ベクトル算出部１３０は、第１所定時間毎に繰り返し移動ベクトルを算出する。

本実施形態では、第２タイミングｊは、第１タイミングｉの次のタイミングとしている（ｊ＝ｉ＋１）。つまり上述した第１所定時間は、距離センサ２００が１回転する時間（0.1秒）としている。そして移動ベクトル算出部１３０は、距離センサ２００が１回転する毎に、新たに取得したスキャンデータと前回取得したスキャンデータとの照合を行い、移動体１０００の移動ベクトルを算出する。

なお、移動ベクトル算出部１３０は、２タイミング以上離れたスキャンデータ同士を照合して移動ベクトルを求めるようにしても良い。このようにすると、移動ベクトルを求めるための計算量を減らすことができ、移動軌跡算定システム１００の処理負荷を軽減することが可能となる。また距離センサ２００が１回転する間に移動ベクトルの算出が間に合わないことが仮にあっても、移動軌跡算定システム１００は移動体１０００の運動状態を見失うことなく、安定して制御を続けることが可能となる。

移動ベクトル算出部１３０は、例えばＩＣＰ（Iterative Closest Points）を用いて、第１タイミングにおけるスキャンデータＰ_ｉと第２タイミングにおけるスキャンデータＰ_ｊとの照合を行う。ＩＣＰは、２つの点群データの位置合せをする方法である（ＩＣＰについては、例えばP. J. Besl and N. D. Mckay: “A Method of Registration of 3-D Shapes,” IEEE Trans. on PAMI, Vol. 14, No. 2,pp. 239-256, 1992.を参照）。

この場合、移動ベクトル算出部１３０は、各スキャンデータから得られる計測点（スキャン点）の対応付けと、移動体１０００の位置の最適化を交互に行うことで、２つの点群の位置合わせを行う。点対点のＩＣＰでは、２つの点群データの対応点間のユークリッド距離の二乗和を誤差関数とする。

また移動ベクトル算出部１３０は、各スキャンデータから得られるスキャン点の対応付けを行うために、スキャン点の最近傍探索を行う。これは、２つのスキャンデータから得られるスキャン点で最も位置の近いペアを見つける処理である。最近傍探索は、たとえば、ＦＬＡＮＮ（M. Muja and D. G. Lowe: Fast Approximate Nearest Neighbors with Automatic Algorithm Configuration, Proc. of International Conference on Computer Vision Theory and Applications (VISAPP2009)を参照）などのソフトウェアライブラリを用いて実現することができる。

そして移動ベクトル算出部１３０は、得られた点の対応付けに対して、下記の式（１）を最小化する移動ベクトルｍ_ｔを求める。式（１）を最小化する手法は種々あるが、本実施形態では準ニュートン法を用いている。

移動ベクトルｍ_ｔの初期値は、移動体１００がオドメトリを有している場合には、オドメトリで求めた予測値を用いることができる。あるいは、オドメトリを用いなくても、第１所定時間前に求めた移動ベクトル（前回求めた移動ベクトルｍ_ｔ−１）や所定の固定値を初期値として用いても良い。

移動ベクトル算出部１３０は、この点の対応づけと誤差最小化の処理を、Ｇ（ｍ_ｔ） の値が収束するまで交互に繰り返すことでＩＣＰを行う。

ここで、ｐ_ｔ，ｉは、タイミングｔ（第２タイミング）のスキャンデータから得られるスキャン点（入力スキャン点とも記す）であり、ｐ_{ｒｅｆ，ｉ}は、対応するスキャン点（第１タイミングのスキャンデータから得られるスキャン点であり、参照スキャン点とも記す）である。ｈ（ｐ_ｔ，ｉ，ｍ_ｔ）はｐ_ｔ，ｉをｍ_ｔで座標変換して得た位置である（ｐ_ｔ，ｉをｍ_ｔの回転成分で回転し、さらにｍ_ｔの並進成分を加える）。

また、ｎ_{ｒｅｆ，ｉ}は、ｐ_{ｒｅｆ，ｉ}の法線ベクトルである。この評価関数では、ｐ_{ｒｅｆ，ｉ}を通りｎ_{ｒｅｆ，ｉ}と直交する直線とｐ_ｔ，ｉとの距離を計算している。一般に、このような点対線の誤差関数の方が、上述の点対点の誤差関数よりも極小値に陥りにくい。ただし、得られるスキャンデータ内に直線が少ない環境では、点対点の誤差関数（２点間のユークリッド距離）を用いてもよい。

ρは、Ｍ推定のカーネル関数であり、誤差の大きいアウトライア（外れ値）の影響を軽減するために用いる。本実施形態では、Ｇｅｍａｎ−ＭｃＣｌｕｒｅカーネルを用いているが、他のカーネル関数でもよい。

移動ベクトル算出部１３０は、このようにして新たな移動ベクトルｍ_ｔを求める毎に、経路情報記憶部１２０に順次記憶しておく。なおｍ_０は０ベクトルである（並進０、回転０）。

＜位置情報算出部＞
つぎに位置情報算出部１４０について説明する。

位置情報算出部１４０は、第１所定時間毎に得られる移動ベクトルｍ_iを累積することにより、移動体１０００の第１所定時間毎の通過地点の位置情報ｒ_ｔを求める。

移動ベクトルｍ_iを累積するとは、移動ベクトルｍ_i−１の終点の位置に移動ベクトルｍ_iの始点を重ねることをいう。

具体的には、位置情報算出部１４０は、新たな移動ベクトルが算出される毎に、式（２）により移動ベクトルを累積し、移動体１０００の位置情報を求める。

位置情報算出部１４０が移動ベクトルｍ_iを累積することで移動体１０００の各通過地点の位置情報ｒ_ｔ(t＝０〜ｎ)を求める様子を図３に示す。なお本実施形態では、移動開始地点ｒ_０も、便宜上、通過地点と称する。

＜累積移動距離算出部＞
累積移動距離算出部１６０は、移動体１０００の移動ベクトルｍ_iの大きさ|ｍ_i|を累積することにより、移動体１０００の移動開始地点ｒ_０から各通過地点ｒ_ｔ（ｔ＝０〜ｎ）までのそれぞれの累積移動距離Ｌ_ｔを求める。

具体的には、累積移動距離算出部１６０は、新たな移動ベクトルが算出される毎に、式（３）により移動体１０００の累積移動距離Ｌ_ｔを求める。

累積移動距離算出部１６０が移動ベクトルｍ_iの大きさ|ｍ_i|を累積することにより、移動体１０００の移動開始地点ｒ_０から各通過地点ｒ_ｔ(t＝０〜ｎ)までの累積移動距離Ｌ_ｔ(t＝０〜ｎ)を求める様子を図３に示す。

＜再訪点検出部＞
次に、再訪点検出部１５０について説明する。再訪点検出部１５０は、移動体１０００の各通過地点ｒ_ｔ（ｔ＝０〜ｎ）の中から、移動体１０００の最新の通過地点である現在位置ｒ_ｎからの距離が所定値よりも小さく、かつ、極小となる通過地点を、移動体１０００の再訪点ｒ_ｓとして検出する。

図４に、再訪点検出部１５０によって現在位置ｒ_ｎに対して３つの再訪点ｒ_ｓ１、ｒ_ｓ２、ｒ_ｓ３が検出された様子を示す。なお図４において、現在位置ｒ_ｎから距離が所定値となる場所を破線の丸印で示してある。

これらの再訪点の検出方法を、図５（ａ）及び図５（ｂ）を参照しながらより詳細に説明する。

図５（ａ）は、移動体１０００の現在位置ｒ_ｎからの距離が所定値よりも小さい通過地点を黒丸で示した図である。

図５（ａ）に示すように、移動体１０００の現在位置ｒ_ｎからの距離が所定値以内の範囲（破線の範囲内）には、過去の移動体１０００の通過地点が複数存在する場合がある。

この場合、これらの通過地点を全て再訪点としてしまうと、移動軌跡算定システム１００は、現在位置ｒ_ｎと、これらの再訪点と、に対して後述するループ閉じ込みと呼ばれる処理をそれぞれ行うことになる。

ループ閉じ込みは、移動体１０００の現在位置ｒ_ｎと再訪点ｒ_ｓとのずれを修正するために、現在位置ｒ_ｎにおいて得られたスキャンデータＰ_ｎと、再訪点ｒ_ｓにおいて得られたスキャンデータＰ_ｓと、を照合する処理や、経路情報記憶部１２０に記憶されている移動体１０００の各通過地点の位置情報ｒ_ｔ（ｔ＝０〜ｎ）と地図情報１９０を更新する処理を含むため、負荷が大きい。

そのため、移動体１０００が現在位置ｒ_ｎから次の位置ｒ_ｎ＋１に移動する短時間の間に、図５（ａ）に示す全ての再訪点に対するループ閉じ込みの処理を完了できない可能性もある。

そこで本実施形態に係る再訪点検出部１５０は、移動体１０００の現在位置ｒ_ｎとの距離が極小となる通過地点を再訪点ｒ_ｓとして検出するようにして、再訪点の絞り込みを行う（第１の近傍抑制）。

図５（ｂ）は、移動体１０００の現在位置ｒ_ｎとの距離が極小となる通過地点を再訪点ｒ_ｓとして検出する際の様子を示す図である。図５（ｂ）には、説明の簡単化のために、現在位置ｒ_ｎ及び再訪点ｒ_ｓ１にそれぞれ連なる通過地点のみを示してあるが、他の再訪点ｒ_ｓ２、ｒ_ｓ３についても同様である。

まず再訪点検出部１５０は、移動体１０００の現在位置ｒ_ｎからの距離が所定値よりも小さな８つの通過地点ｒ_ｓ１−２、ｒ_ｓ１−１、ｒ_ｓ１、ｒ_ｓ１＋１、ｒ_ｓ１＋２、ｒ_ｎ、ｒ_ｎ−１、ｒ_ｎ−２のうち、現在位置ｒ_ｎからの距離が極小となる点を選ぶ。この例では、ｒ_ｎ自身とｒ_ｓ１が選ばれる。ただし、ｒ_ｎは現在位置なので再訪点には含めない。このように極小点を求めることで再訪点の個数を減らすことができる。なお、この例では、簡単のため、通過点を８個としているが、実際は任意の個数でよい。

さらに、累積移動距離を用いることで、再訪点の絞り込みを行うこともできる（第２の近傍抑制）。上述のようにして求めた極小点を中心に、該極小点との累積移動距離の差が所定値以内の通過地点を同一グループになるように分類する。図５（ｂ）に示す例では、上記８つの通過地点は、ｒ_ｓ１−２、ｒ_ｓ１−１、ｒ_ｓ１、ｒ_ｓ１＋１、ｒ_ｓ１＋２からなる第１グループと、ｒ_ｎ、ｒ_ｎ−１、ｒ_ｎ−２からなる第２グループに分類される。

グループの中心となる極小点の選び方によって、グループ構成は変わりうる。選び方としては、たとえば、現在位置ｒ_ｎからの直線距離が小さい順に極小点を選ぶ、または、現在位置ｒ_ｎから時刻を逆順にたどって極小点を選ぶ、などが考えられる。

そして次に、再訪点検出部１５０は、各グループごとに、グループ内の各通過地点と現在位置ｒ_ｎとの距離が最小となる通過地点を再訪点として一つ選ぶ。

具体的には、再訪点検出部１５０は、各グループ内で、移動体１０００の現在位置ｒ_ｎと、各通過地点との距離をそれぞれ求め、これらの距離同士を比較する。そして距離が最小となる再訪点を一つ選ぶ。

図５（ｂ）の例では、第１グループからは再訪点ｒ_ｓ１が検出される。第２グループの最小点ｒ_ｎは現在位置なので再訪点には含めない。

このようにして第１グループから選出される再訪点ｒ_ｓ１は、現在位置ｒ_ｎに最も近い本質的な再訪点であると言えるため、再訪点としても好適である。また説明は省略したが、同様にして再訪点ｒ_ｓ２及びｒ_ｓ３も検出される。

なお、１つのグループに極小点が複数含まれることがある。図６の例では、同一グループに分類された複数の通過地点ｒ_ｓ１−１〜ｒｓ_１＋５の中で、現在位置ｒ_ｎからの距離が極小となる再訪点が複数検出されている（ｒ_ｓ１とｒ_ｓ１＋４）。このような場合であっても、再訪点検出部１５０は、現在位置ｒ_ｎと通過地点ｒ_ｓ１との間の距離と、現在位置ｒ_ｎと通過地点ｒ_ｓ１＋４との間の距離と、を比較し、通過地点ｒ_ｓ１を再訪点ｒ_ｓ１として選出する。

このようにして本実施形態に係る再訪点検出部１５０によれば、移動体１０００の現在位置ｒ_ｎからの距離が所定値よりも小さく、かつ、極小となる通過地点を、移動体１０００の再訪点ｒ_ｓとして検出するようにすることにより、移動体１０００の再訪点ｒ_ｓを効果的に検出し、移動体１０００の移動軌跡を算定する際の処理負荷を軽減することが可能となる。また、同一グループ内に現在位置ｒ_ｎからの距離が極小となる再訪点が複数検出される場合でも、現在位置ｒ_ｎに最も近接する再訪点を一つ選出することで、さらに効果的に再訪点ｒ_ｓの検出を行うことが可能となる。

＜地図作成部＞
次に、図１に戻って、地図作成部１８０について説明する。 地図作成部１８０は、スキャンデータ取得部１１０が距離センサ２００から新たなスキャンデータＰ_ｔを取得する毎に、この新たなスキャンデータＰ_ｔを、移動体１０００を基準にした局所座標で表わされた値から、移動体１０００の位置に依存しない所定の共通座標で表わされた値に変換する。そして地図作成部１８０はこれらの共通座標に変換されたスキャンデータを順次、地図情報記憶部１９０に蓄積することにより、外界の地図を作成する。

例えば、図１０は、移動体１０００がｒ_ａで示される位置にいるときに、スキャンデータ取得部１１０が距離センサ２００から取得したスキャンデータＰ_ａを元に、外界の各スキャン点の位置を２次元平面状にプロットしたものであり、図１１は、移動体１０００がｒ_ｂで示される位置にいるときに、スキャンデータ取得部１１０が距離センサ２００から取得したスキャンデータＰ_ｂを元に、外界の各スキャン点の位置を２次元平面状にプロットしたものであるが、いずれのスキャンデータＰ_ａ、Ｐ_ｂも、それぞれ移動体１０００の位置ｒ_ａ、ｒ_ｂを中心とした局所座標で表現されているため、そのまま重ね合わせることはできない。

本実施形態では、これらのスキャンデータＰ_ａ、Ｐ_ｂを、共通座標における値に変換するようにしている。共通座標の原点及び座標軸の向きはどのように定めても良いが、例えば移動体１０００の移動開始時の位置及び向きを基準に定めるようにすればよい。

移動体１０００の移動開始時の位置及び向きは、経路情報記憶部１２０に記憶されている移動ベクトルを、移動開始時に遡るように順に参照することによって容易に算定できる。

もちろん実際には、１タイミング毎の移動体１０００の並進量及び回転量を蓄積しておき、局所座標から共通座標へ変換する際にはこれらの蓄積値を用いることにより、容易に局所座標から共通座標への変換を行うことができる。

このようにして各スキャンデータＰ_ｔ（ｔ＝０〜ｎ）から得られる外界の各スキャン点の位置が共通座標上で位置合わせされることによって、地図情報記憶部１９０に作成された地図の一例を図１２に示す。図１２は、地図情報記憶部１９０に蓄積された共通座標のスキャンデータを元に、外界の各スキャン点の位置を２次元平面状にプロットしたものである。このようにして地図情報記憶部１９０に作成された地図は、運動制御部４００によって用いられる。

＜ループ閉じ込み処理部＞
ループ閉じ込み処理部１７０は、移動体１０００の現在位置ｒ_ｎにおいて得られたスキャンデータＰ_ｎと、再訪点ｒ_ｓにおいて得られたスキャンデータＰ_ｓと、を照合することにより、現在位置ｒ_ｎと再訪点ｒ_ｓとの位置ずれを修正する。

ループ閉じ込み処理部１７０は、第１相対位置ベクトル算出部１７１と、第２相対位置ベクトル算出部１７２と、位置情報修正部１７３と、を有して構成されている。

第１相対位置ベクトル算出部１７１は、移動ベクトルｍ_i（i＝０〜ｎ）を累積することにより算定される移動体１０００の現在位置ｒ_ｎと、移動ベクトルｍ_i（i＝０〜ｓ）を累積することにより算定される再訪点ｒ_ｓの位置と、の間の相対位置を示す第１相対位置ベクトルｖ_１を求める。

そして第２相対位置ベクトル算出部１７２は、移動体１０００が再訪点ｒ_ｓを通過したタイミングｓ（第３タイミング）におけるスキャンデータＰ_ｓと、移動体１０００が現在位置ｒ_ｎを通過したタイミングｎ（第４タイミング）におけるスキャンデータＰ_ｎと、を照合することにより、現在位置ｒ_ｎと再訪点ｒ_ｓとの間の相対位置を示す第２相対位置ベクトルｖ_２を求める。

第２相対位置ベクトル算出部１７２は、例えばＩＣＰを用いて、タイミングｓにおけるスキャンデータＰ_ｓとタイミングｎにおけるスキャンデータＰ_ｎとの照合を行う。すなわち、第２相対位置ベクトル算出部１７２は、タイミングｎにおけるスキャンデータＰ_ｎから得られる各スキャン点の位置が、タイミングｓにおけるスキャンデータＰ_ｓから得られる各スキャン点の位置に近づくように、第２相対位置ベクトルｖ_２を求める。

第２相対位置ベクトル算出部１７２は、移動ベクトル算出部１３０と同様に、各スキャンデータＰ_ｎ、Ｐ_ｓから得られるスキャン点の対応付けと、移動体１０００の位置の最適化を交互に行うことで、２つの点群の位置合わせを行う。

そして第２相対位置ベクトル算出部１７２は、得られた点の対応付けに対して、式（４）を最小化する第２相対位置ベクトルｖ_２を求める。

式（４）は式（１）と同型であり、ｐ_ｎ，ｉは、タイミングｎ（第４タイミング） のスキャンデータＰ_ｎから得られるスキャン点であり、ｐ_ｓ，ｉは、対応するスキャン点（第３タイミングのスキャンデータから得られるスキャン点）である。ｎ_ｓ，ｉは、ｐ_ｓ，ｉの法線ベクトルである。また、ｈ（ｐ_ｎ，ｉ，ｖ_２）はｐ_ｎ，ｉをｖ_２で座標変換して得た位置である。ρは、Ｍ推定のカーネル関数である。式（４）を最小化する手法は種々あるが、本実施形態では準ニュートン法を用いている。

第２相対位置ベクトル算出部１７２は、このようにして第２相対位置ベクトルｖ_２を求める。そして第２相対位置ベクトル算出部１７２は、第２相対位置ベクトルｖ_２を経路情報記憶部１２０に記憶する。

次に、第２相対位置ベクトル算出部１７２がＩＣＰを行う際に用いる第２相対位置ベクトルｖ_２の初期値について説明する。この初期値の算出方法として、以下に第１の方法及び第２の方法を例示する。

第１の方法は、移動体１０００が現在位置ｒ_ｎを通過した第４タイミングにおけるスキャンデータＰ_ｎから得られる各スキャン点の特徴を表す記述子（後述）と、移動体１０００が再訪点ｒ_ｓを通過した第３タイミングにおけるスキャンデータＰ_ｓから得られる各スキャン点の特徴を表す記述子と、を比較し、これらの各記述子の一致度合いが所定の判定値よりも大きな第１スキャン点及び第２スキャン点を各スキャンデータＰ_ｎ、Ｐ_ｓからそれぞれ抽出し、第１スキャン点の位置が第２スキャン点の位置になるような変位ベクトルｑ（x,y,θ）を、第２相対位置ベクトルｖ_２の推定値（初期値）とする方法である。

具体的には、まず第２相対位置ベクトル算出部１７２は、現在位置ｒ_ｎにおけるスキャンデータＰ_ｎと、再訪点ｒ_ｓにおけるスキャンデータＰ_ｓについて、各スキャンデータから得られるスキャン点の局所記述子を計算する。

局所記述子は、スキャン点をハフ空間に投影して、ハフ空間での近傍点の分布を特徴ベクトルにしたものである。

次に第２相対位置ベクトル算出部１７２は、スキャンデータＰ_ｎから得られるスキャン点と、スキャンデータＰ_ｓから得られるスキャン点と、から、局所記述子の一致度が所定の判定値よりも高いペアを選ぶ。

次に第２相対位置ベクトル算出部１７２は、これらのスキャン点のペアに対して、式（５）〜（７）を用いて姿勢クラスタリングを行う。

まずスキャンデータＰ_ｎ、Ｐ_ｓにおいて正しく対応するスキャン点を、それぞれp1=(x1,y1,θ1)、p2=(x2,y2,θ2)とする。これらのスキャン点は、各スキャンデータの局所座標系での値である。なおθ1、θ2は法線方向である。このとき、スキャンデータ間の相対位置をq=(x,y,θ)とすると、仮に２つのスキャンデータＰ_ｎ、Ｐ_ｓが正しく対応していると仮定した場合には、式（５）〜式（７）が成り立つ。

θ=θ1-θ2 （５）
x=x1-(x2cosθ-y2sinθ) （６）
y=y1-(x2sinθ+y2cosθ) （７）
理想的にはこれで相対位置q=(x,y,θ)が求まるが、実際には誤対応や誤差などにより、一組のスキャン点で得た解は信頼性が低い。

そこで、対応するスキャン点の各ペアについて式（５）〜式（７）を解いて得られた相対位置q=(x,y,θ)をクラスタリングして、サイズの大きいクラスタを、第２相対位置ベクトルｖ_２の初期値とする（なお、局所記述子及び姿勢クラスタリングについては、例えば、特開２００５−１７４０６２号公報、及び、友納正裕:ユークリッド変換に不変な特徴量を用いた二次元大域スキャンマッチング方式;『日本ロボット学会誌』Vol.25, No.3, pp.66-77,(2007)を参照）。

この初期値は粗い推定であるが、式（４）のＩＣＰで詳細なマッチングを行い、スコアが最もよい解を採用することで、より正確な第２相対位置ベクトルｖ_２を算出することができる。

なお、局所記述子は、１つのタイミングのスキャンデータのみを用いて生成しても良いが、前後のタイミングを含む複数のスキャンデータを用いて生成するとなお良い。これにより局所記述子を構成する近傍点の分布を安定させることが可能となる。

また、局所記述子は他の方法によるものを用いてもよい。さらには、局所記述子を用いずに姿勢クラスタリングを行うことも可能である。その場合は、スキャンデータＰ_ｎから得られるスキャン点と、スキャンデータＰ_ｓから得られるスキャン点のすべての組について式（５）〜式（７）を解いて、姿勢クラスタリングを行う。

次に、第２の方法について説明する。図８は、第２の方法を説明するための図である。

第２の方法は、移動体１０００が現在位置ｒ_ｎを通過したタイミングｎ（第４タイミング）よりも第２所定期間前（例えば１タイミング前）のタイミングｎ−１（第５タイミング）からタイミングｎ（第４タイミング）までの移動体１０００の移動ベクトルｍ_ｒを、移動体１０００が再訪点ｒ_ｓを通過したタイミングｓ（第３タイミング）よりも１タイミング前（第２所定期間前）のタイミングｓ−１（第６タイミング）における移動体１０００の再訪点の確定した位置（ｒ’_ｓ−１＝ｒ_ｎ−１＋ｖ_{２,ｎ−１}）に加えることにより、再訪点ｒ_ｓの推定位置（ｒ’_ｓ−１＋ｍ_ｒ）を求め、移動体１０００の現在位置ｒ_ｎと、再訪点の推定位置（ｒ’_ｓ−１＋ｍ_ｒ）と、の間の変位ベクトルΔｒ_ｎを、第２相対位置ベクトルｖ_２の推定値（初期値）として用いる方法である。ただし、ｖ_{２,ｎ−１}はタイミングｎ−１での第２相対位置ベクトルである。

つまり第２の方法は、タイミングｎ−１で実施したループ閉じ込みによって確定した再訪点ｒ_ｓ−１の位置ｒ’_ｓ−１から、移動体１０００の移動ベクトルｍ_ｒを用いて再訪点ｒ_ｓの位置を推定する方法である。

なお、移動ベクトル（または相対位置ベクトル）ｕ₁=(x₁, y₁,θ₁)とｕ₁=(x₂, y₂, θ₂)の加法（u=u₁+u₂）は次のように行う。

x = x₁ + (x₂cosθ₁ - y₂sinθ₁) （８）
y = y₁ + (x₂sinθ₁ + y₂cosθ₁) （９）
θ=θ₁ + θ₂ （１０）
第２相対位置ベクトル算出部１７２は、この推定を例えば１フレームごとに行う。１フレーム分の移動体１０００の移動量ｍ_ｒは小さいので、移動体１０００が再訪点ｒ_ｓを通過したタイミングｓの１タイミング前のタイミングｓ−１において確定した再訪点の位置ｒ’_ｓ−１を基準にすることで、現在位置ｒ_ｎと再訪点ｒ_ｓとの間の第２相対位置ベクトルｖ_２,nの初期値Δｒ_ｎを効率よく求めることができる。

具体的には、まず第２相対位置ベクトル算出部１７２は、再訪点ｒ_ｓ−１の確定位置をｒ’_ｓ−１＝ｒ_ｎ−１＋ｖ_{２、ｎ−１}として求め、それに移動ベクトルｍ_ｒを加えて、次フレームの再訪点候補ｒ’_ｓ−１＋ｍ_ｒを求める。

そして第２相対位置ベクトル算出部１７２は、移動体１０００の現在位置ｒ_ｎと再訪点候補ｒ’_ｓ−１＋ｍ_ｒとの間の変位ベクトルΔｒ_ｎを第２相対位置ベクトルｖ_２,nの初期値にする。

その後第２相対位置ベクトル算出部１７２は、この初期値Δｒ_ｎを用いて、式（４）により、現在位置ｒ_ｎのスキャンデータＰ_ｎと、再訪点ｒ_ｓのスキャンデータＰ_ｓと、の間のＩＣＰを行って、両者の相対位置（第２相対位置ベクトルｖ_２,n）を求める。

＜位置情報修正部＞
次に、位置情報修正部１７３は、ループ閉じ込みを行う。具体的には、位置情報修正部１７３は、第１相対位置ベクトルｖ_１と第２相対位置ベクトルｖ_２とが求められたら、第１相対位置ベクトルｖ_１が第２相対位置ベクトルｖ_２に近づくように、移動体１０００の各通過地点の位置情報ｒ_ｔ（ｔ＝０〜ｎ）を修正する。

図７に、経路情報記憶部１２０に記録されている移動体１０００の移動ベクトルｍ_i（i＝０〜ｎ）を累積することにより得られる移動体１０００の移動軌跡の一例を示す。図７に示すように、個々の移動ベクトルｍ_iの誤差により、それらを累積することにより得られる移動体１００の移動軌跡にはずれが生じる。

位置情報修正部１７３は、このずれを修正するように、移動体１０００の各通過地点の位置情報ｒ_ｔ（ｔ＝０〜ｎ）を修正する。ループ閉じ込みを行うことによって修正された移動体１０００の移動軌跡の例を図１３に示す。

位置情報修正部１７３は、ループ閉じ込みの処理をポーズグラフを用いて行う。ポーズグラフは、移動体１０００の位置をノード、ノード間の相対位置をアークとしたグラフである。上述した移動ベクトルｍ_iは、隣接したノード間のアークとなる。また第２相対位置ベクトルｖ_２は、離れたノード間のアークであり、ループを生成するのでループアークと呼ばれる。

位置情報修正部１７３は、経路情報記憶部１２０を参照してポーズグラフを生成し、このポーズグラフに対してポーズ調整を行うことで、移動体１０００の軌跡と地図を修正する。ポーズ調整は、式（１１）で表される二乗誤差を最小化することにより行われる。

なお本実施形態では、式（１１）の最小化にＳＰＡ（Sparse Pose Adjustment）を用いる（ＳＰＡについては、K.Konolige, G.Grisetti,et al.:Efficient Sparse Pose Adjustment for 2D Mapping; Proc. of IROS, pp.22-29,(2010)を参照）。

ここで、ｃはアーク集合、ｕｉ，ｊはアーク（ｉ，ｊ）∈ｃの観測値（すなわち、移動ベクトルｍｉ、あるいは、第２相対位置ベクトルｖ２）、関数ｇ（ｒｉ，ｒｊ）は移動体１０００の位置ｒｉとｒｊから計算で得た相対位置（すなわち第１相対位置ベクトルｖ１）である。またｗｉ，ｊは、ｕｉ，ｊの誤差共分散である。

以上のようにして、ループ閉じ込み処理部１７０はループ閉じ込み処理を行う。

このように、移動体１０００が再訪点ｒ_ｓを通過した際のスキャンデータＰ_ｓを現在位置ｒ_ｎのスキャンデータＰ_ｎと比較することで移動体１０００の移動軌跡を修正することにより、移動体１０００の位置及び運動状態をより正確に把握することが可能となる。

そして本実施形態に係る移動軌跡算定システム１０００は、上述したように、移動体１０００の現在位置ｒ_ｎに対して本質的な再訪点ｒ_ｓを特定することができるため、ループ閉じ込みの処理を無駄なく行い、処理負荷の軽減を図ることが可能となる。

なお、複数の再訪点、例えば２つの再訪点ｒ_ｓ１、ｒ_ｓ２検出された場合には、ループ閉じ込み処理部１７０は、各再訪点ｒ_ｓ１、ｒ_ｓ２に対してそれぞれループ閉じ込みの処理を行う。

＝移動軌跡算定システムの制御方法＝
次に、移動軌跡算定システム１００による処理の流れを、図９を参照しながら説明する。

まず、スキャンデータ取得部１１０は、距離センサ２００から出力される外界の各スキャン点までの距離及び方向を示す計測結果を順次取得する。そしてスキャンデータ取得部１１０は、距離センサ２００による距離の計測方向が１回転する毎に、１回転分の計測結果をスキャンデータＰ_ｔ（フレームＰ_ｔ）としてまとめ、経路情報記憶部１２０に記憶する（S1000）。

そして移動ベクトル算出部１３０は、新たにスキャンデータを取得する毎に、このスキャンデータを前回取得したスキャンデータと照合することにより、第１所定時間毎の移動体１０００の移動ベクトルを求め、経路情報記憶部１２０に記憶する（S1010）。

そして位置情報算出部１４０は、上記第１所定時間毎に得られる移動ベクトルｍ_i（i=０〜t）を累積することにより、移動体１０００の第１所定時間毎の通過地点の位置情報ｒ_ｔを求め、経路情報記憶部１２０に記憶する（S1020）。

また累積移動距離算出部１６０は、移動体１０００の移動ベクトルｍ_iの大きさ|ｍ_i|（i=０〜t）を累積することにより、移動体１０００の移動開始地点から各通過地点までのそれぞれの累積移動距離Ｌ_ｔを求め、経路情報記憶部１２０に記憶する（S1030）。

そして再訪点検出部１５０は、移動体１０００の各通過地点の中から、移動体１０００の最新の通過地点である現在位置ｒ_ｎからの距離が所定値よりも小さく、かつ、極小となる通過地点を、移動体１０００の再訪点ｒ_ｓとして検出する（S1040）。

本実施形態に係る移動軌跡算定システム１０００は、このような処理を行うことによって、移動体１０００の現在位置ｒ_ｎに対して本質的な再訪点ｒ_ｓを特定することができるようになるため、ループ閉じ込みの処理を無駄なく行い、移動体１０００の移動軌跡を算定する際の処理負荷の軽減を図ることが可能となる。

その後、ループ閉じ込み処理部１７０は、再訪点ｒ_ｓが検出された場合には、移動体１０００の現在位置ｒ_ｎにおいて得られたスキャンデータＰ_ｎと、再訪点ｒ_ｓにおいて得られたスキャンデータＰ_ｓと、を照合することにより、現在位置ｒ_ｎと再訪点ｒｓとの位置ずれを修正するループ閉じ込みを行う（S1050）。

また、図９に示すフローチャートには含まれていないが、地図作成部１８０は、スキャンデータ取得部１１０が距離センサ２００から新たなスキャンデータＰ_ｔを取得する毎に、この新たなスキャンデータＰ_ｔを、移動体１０００を基準にした局所座標で表わされた値から、移動体１０００の位置に依存しない共通座標で表わされた値に変換し、そして、これらの共通座標に変換されたスキャンデータを順次、地図情報記憶部１９０に蓄積する。これにより、地図作成部１８０は、外界の地図を作成する。

移動軌跡算定システム１００は、このようにして作成された地図を用いることにより、移動体１０００の自律的な運動を適切に行うことが可能となる。

＝＝第２実施形態＝＝
次に、第２実施形態に係る移動軌跡算定システム１００について説明する。

第１実施形態では、再訪点検出部１５０は、スキャンデータ取得部１１０が距離センサ２００からスキャンデータＰ_ｔを取得する毎に再訪点ｒ_ｓの検出を行っているが、第２実施形態に係る再訪点検出部１５０は、キーフレームごとに再訪点ｒ_ｓの検出を行う。キーフレームは、所定枚数毎のスキャンデータから選出されるスキャンデータであり、例えば距離センサ２００のレーザパルスが１０回転するごとに抽出されるスキャンデータ（フレーム）である。

このような態様により、再訪点検出処理（S1040）及びループ閉じ込み処理（S1050）の実行頻度を減らし、移動軌跡算出システム１００の処理負荷をさらに軽減することが可能となる。そしてこれにより、処理速度の高速化を図ることができる。

＝＝第３実施形態＝＝
次に、第３実施形態に係る移動軌跡算定システム１００について説明する。

第１実施形態及び第２実施形態では、第２相対位置ベクトル算出部１７２は、経路情報記憶部１２０に記憶されている現在位置ｒ_ｎにおけるスキャンデータＰ_ｎと、再訪点ｒ_ｓにおけるスキャンデータＰ_ｓと、を用いて第２相対位置ベクトルｖ_２を算出したが、第３実施形態では、地図情報記憶部１９０に記憶されている地図を用いて第２相対位置ベクトルｖ_２を算出する。以下に詳しく説明する。

図１０や図１１に示したように、１個のスキャンデータでは、周囲の環境形状を十分に表現できないことがある。たとえば、距離センサ２００が視野の狭いレーザスキャナである場合は、全周の形状が得られないし、レーザスキャナのレンジが短い場合は、近距離にある存在物の形状しかわからない。

そこで第３実施形態では、移動体１０００の現在位置ｒ_ｎと再訪点ｒ_ｓとの間の第２相対位置ベクトルｖ_２を求める際に、経路情報記憶部１２０に記憶されたスキャンデータを用いるのではなく、地図情報記憶部１９０に記憶されている地図を用いることにする。

つまり、地図情報記憶部１９０には、外界の各スキャン点までの距離及び方位を示すスキャンデータが共通座標に変換された形で移動体１０００の移動開始時点から蓄積されているが、第３実施形態に係る第２相対位置ベクトル算出部１７２は、この地図情報記憶部１９０に蓄積されているスキャンデータを、移動体１０００の現在位置ｒ_ｎ及び再訪点ｒ_ｓを起点とする局所座標での距離及び方位を示すデータ（仮想スキャンデータと呼ぶ）にそれぞれ変換し、それらの仮想スキャンデータを照合することにより、現在位置ｒ_ｎと再訪点ｒ_ｓとの相対位置を求める。

第２相対位置ベクトル算出部１７２は、移動体１０００の位置（現在位置ｒ_ｎ、再訪点ｒ_ｓ）を中心として半径Ｒの範囲にあるスキャン点を地図情報記憶部１９０に記憶されている全体地図から取り出すことで、仮想スキャンデータを生成する。

より詳しくは、第２相対位置ベクトル算出部１７２は、まず、地図情報記憶部１９０に蓄積されている共通座標で表わされたスキャンデータを、移動体１０００の現在位置ｒ_ｎを基準とした局所座標上での値に変換することにより、現在位置ｒ_ｎから外界の各スキャン点までの距離及び方位を示す第１仮想スキャンデータを生成する。

また同様に、第２相対位置ベクトル算出部１７２は、地図情報記憶部１９０に蓄積されている共通座標で表されたスキャンデータを、再訪点ｒ_ｓを基準とした局所座標上での値に変換することにより、再訪点ｒ_ｓから外界の各スキャン点までの距離及び方位を示す第２仮想スキャンデータを生成する。

そして第２相対位置ベクトル算出部１７２は、第１仮想スキャンデータと第２仮想スキャンデータとを照合することにより、現在位置ｒ_ｎと再訪点ｒ_ｓとの間の相対位置を示す第２相対位置ベクトルｖ_２を求める。

このような態様により、移動体１０００の現在位置ｒ_ｎと再訪点ｒ_ｓとの相対位置である第２相対位置ベクトルｖ_２を、より多くのスキャン点を照合することで、より正確に求めることが可能となる。

また本実施形態に係る第２相対位置ベクトル算出部１７２は、地図情報記憶部１９０に全体地図として蓄積されている共通座標で表されたスキャンデータから仮想スキャンデータを生成する際に、移動体１０００が再訪点ｒ_ｓを通過した際の局所座標で表されたスキャンデータＰ_ｓに由来するスキャンデータと、移動体１０００が現在位置ｒ_ｎを通過した際の局所座標で表されたスキャンデータＰ_ｎに由来するスキャンデータとを混在しないようにしている。

たとえば、図７において、多重ループをなす部分軌跡ｔｒ_１、ｔｒ_２は、それぞれ地図上においてスキャン点群ｓ_１、ｓ_２を作るが、ループ閉じ込みを行っていない場合には、移動体１０００の位置情報の誤差が修正されていないために、これらのスキャン点群はきれいに重ならない。これらのスキャン点群を混在したまま地図から仮想スキャンデータを作ると、その形状は正しくないので、現在位置ｒ_ｎと再訪点ｒ_ｓのマッチング誤差が大きくなる。

そこで、本実施形態に係る第２相対位置ベクトル算出部１７２は、累積走行距離Ｌ_ｔを用いて部分軌跡を分離し、それぞれで仮想スキャンデータを生成するようにしている。

つまり、第２相対位置ベクトル算出部１７２は、まず、第１仮想スキャンデータを生成する際には、移動体１０００が現在位置ｒ_ｎを通過したタイミングｎ（第４タイミング）から所定タイミングα以内（第３所定期間α以内。ｎ−α〜ｎ）に距離センサ２００から取得したスキャンデータＰ_ｎ−α〜Ｐ_ｎに由来して地図に蓄積されたスキャンデータを用いる。

また同様に、第２相対位置ベクトル算出部１７２は、第２仮想スキャンデータを生成する際には、移動体１０００が再訪点ｒ_ｓを通過したタイミングｓ（第３タイミング）から所定タイミングα以内（第３所定期間α以内。ｓ−α〜ｓ＋α）に距離センサ２００から取得したスキャンデータＰ_ｓ−α〜Ｐ_ｓ＋αに由来して地図に蓄積されたスキャンデータを用いる。

より具体的には、まず、第２相対位置ベクトル算出部１７２は、経路情報記憶部１２０から、再訪点ｒ_ｓとの累積走行距離の差および直線距離が閾値より小さい通過地点ｒ_ｓ−α〜ｒ_ｓ＋αを部分軌跡として抽出する。

そして第２相対位置ベクトル算出部１７２は、経路情報記憶部１２０から部分軌跡として抽出された各通過地点の累積走行距離の最小値atdm（Ｌ_ｓ−α）と、最大値atdM（Ｌ_ｓ＋α）と、を読み出す。

次に第２相対位置ベクトル算出部１７２は、地図情報記憶部１９０に記憶されているスキャンデータから、累積走行距離がatdm（Ｌ_ｓ−α）とatdM（Ｌ_ｓ＋α）の範囲内にあるスキャンデータＰ_ｓ−α〜Ｐ_ｓ＋αに由来する共通座標のスキャンデータを抽出し、その中で再訪点ｒ_ｓの近傍のスキャンデータ、つまり、再訪点ｒ_ｓとの直線距離が閾値より小さいものを選び、第２仮想スキャンデータとする。

第１仮想スキャンデータも同様に抽出する。ただし、第１仮想スキャンデータを抽出する場合は、最新のスキャンデータはＰ_ｎなので、Ｐ_ｎ−α〜Ｐ_ｎに由来する共通座標のスキャンデータを地図から抽出し、その中で現在位置ｒ_ｎの近傍のスキャンデータ、つまり、現在位置ｒ_ｎとの直線距離が閾値より小さいものを選ぶ。

その後第２相対位置ベクトル算出部１７２は、第１実施形態と同様に、例えばＩＣＰを用いて第１仮想スキャンデータと第２仮想スキャンデータとの照合を行い、第１仮想スキャンデータから得られる各スキャン点の位置が、第２仮想スキャンデータから得られる各スキャン点の位置に近づくように、第２相対位置ベクトルｖ_２を求める。

この第３実施形態では、多重ループをなす各部分軌跡において、累積走行距離を用いて仮想スキャンデータを抽出しているため、再訪点ｒ_ｓが仮想スキャンデータを作る部分軌跡のほぼ中心にできる。つまり、再訪点ｒ_ｓの仮想スキャンデータは、Ｐ_ｓ−αからＰ_ｓ＋αの範囲のスキャンデータを用いて作成することができる。

なお、ＳＬＡＭの最中に部分地図を定期的に保存しておき、それを仮想スキャンデータにするようにしても良い。

以上のような態様によっても、第１実施形態や第２実施形態と同様に、移動体１０００の現在位置ｒ_ｎに対して本質的な再訪点ｒ_ｓを特定することができるため、ループ閉じ込みの処理を無駄なく行え、処理負荷の軽減を図ることが可能となる。

＝＝第４実施形態＝＝
第１実施形態から第３実施形態では、スキャンデータ取得部１１０は、距離センサ２００から取得した各スキャン点までの距離及び方位の計測結果をそのままスキャンデータＰ_ｔとして経路情報記憶部１２０に記憶したが、第４実施形態に係るスキャンデータ取得部１１０は、距離センサ２００から各スキャン点までの距離及び方位の計測結果を取得した際に、隣り合うスキャン点の間隔が全体として平準化されるように、各計測結果を補正する。

例えば距離センサ２００が回転型である場合は、スキャン点までの計測距離が遠い場合と近い場合とを比べると、距離が遠いスキャン点であるほど隣り合うスキャン点の間隔が離れるが、本実施形態では、例えば計測距離がより近いスキャン点ほどより多くのスキャン点を間引くように補正することで、スキャンデータＰ_ｔ全体として隣り合うスキャン点の間隔を平準化するようにする。

本実施形態ではスキャン点の平準化を以下のように行っている。なお、スキャン点を平準化して得られる点をマップ点と称する。

スキャンデータ取得部１１０は、マップ点をその近傍内にある複数のスキャン点から生成する。スキャンデータ取得部１１０は、まず、あるスキャン点を中心にして、そのスキャン点から所定距離ｄ以内にあるスキャン点を集める。

次にスキャンデータ取得部１１０は、そのスキャン点集合の重心を中心にして再び所定距離ｄ以内のスキャン点を集め、重心が収束するまで同じ処理を繰り返す。そして重心が収束した点をマップ点とする。このため、本実施形態に係るスキャンデータＰ_ｔを構成するマップ点は、３６０よりも少なくなる。

また、本実施形態に係るスキャンデータ取得部１１０は、マップ点を構成するスキャン点集合を主成分分析して、その最小固有ベクトルをマップ点の法線方向とする。そしてこの法線を用いて、第１実施形態において説明した各スキャン点の特徴を表す局所記述子を定義するようにしても良い。

また本実施形態では、スキャンデータ取得部１１０は、各マップ点に、そのマップ点の生成に用いたスキャン点を得たときの移動体１０００の累積走行距離Ｌ_ｔを記録しておく。

また地図作成部１８０が各タイミングｔのスキャンデータＰ_ｔを共通座標の地図に追加していく際、Ｐ_ｔ内のスキャン点の共通座標系での位置の所定距離ｄ内に累積移動距離の差が閾値Ｄ以内のマップ点があれば、そのうちの最近傍のマップ点にそのスキャン点を統合する。このような処理により、地図を構成するマップ点の密度をほぼ一定に保つことが可能となる。累積移動距離によってマップ点を分離するのは、第３実施形態で述べたように、多重ループをなす各部分軌跡の仮想スキャンデータを抽出する際に、マップ点を累積移動距離で区別できるようにするためである。

なお、本実施形態に係るスキャンデータ取得部１１０は、マップ点を生成する際に、スキャン点集合の大きさが閾値に満たない場合は、マップ点を生成せずにそのスキャン点を削除するようにしても良い。これにより、スキャン点における小さなノイズを除去することができる。

このようにマップ点を用いてスキャンデータＰ_ｔを生成するためには、近傍内にあるスキャン点を頻繁に検索する必要があるため、高速な最近傍探索が必要であるが、本実施形態に係る移動軌跡算定システム１００は、FLANNなどの最近傍探索ライブラリを用いている。

なお、図１０及び図１１に示した各スキャン点は、このようにして平準化が行われている。本実施形態に係る移動軌跡算定システム１００のような態様によって、複数のスキャンデータを照合する際に、対応づける点の密度にムラが少なくなり、計測距離の影響を受けにくくできるため、精度良く照合を行うことが可能となる。また、マップ点はスキャン点よりも個数が少なくなるため、メモリ効率も向上する。

＝＝第５実施形態＝＝
第５実施形態に係る移動軌跡算定システム１００は、第２実施形態から第４実施形態でそれぞれ説明した構成をすべて含む形態である。

そして本実施形態に係る移動軌跡算定システム１００を用いて実験を行った結果を、図１２〜図１４に示す。なお本実験では、１０フレームおきにキーフレームをとり、キーフレーム単位でループ閉じ込みを行った。

図１２は、本実施形態に係る移動軌跡算定システム１００により作成された外界の地図を示す。図１３は移動体１０００の移動軌跡であり、主な地点でのキーフレーム番号を付加している。図１３の移動軌跡は、移動体１０００の前方をｘ軸、横方向をｙ軸とした座標系で表した位置の列である。図１４は、各キーフレームで異なる再訪点が何個検出されたかを示すグラフである。横軸がキーフレーム番号、縦軸が再訪点数である。

図１４を見ると、キーフレーム１から進むにつれて再訪点が増えていく様子がわかる。図１３の拡大部分Ａには。軌跡が５本ある。このため、再訪点は４個得られるはずであるが、図１４のグラフのキーフレーム８１７付近の区間では、再訪点数は４個であり、すべて検出できていることがわかる。

同様に、図１３の拡大部分Ｂには、軌跡が６本あり、再訪点は５個得られるはずである。図１４のグラフのキーフレーム番号８３４付近の区間では、再訪点数は４〜５個であり、ほぼすべての再訪点が得られていることが分かる。このように、図１２から図１４の結果から、本実施形態に係る移動軌跡算定システム１００によって、多くの再訪点が正しく検出できていることが確かめられる。

以上、第１〜第５に係る実施形態の移動軌跡算定システム１００について説明したが、いずれの移動軌跡算定システム１００においても、移動体１０００の再訪点ｒ_ｓをより効果的に特定し、移動体１０００の移動軌跡を算定する際の処理負荷を軽減することが可能となる。

また本実施形態に係る移動軌跡算定システム１００は、ＩＣＰや局所記述子において法線を使うため、壁などが多く形状が安定している屋内環境に適している。

さらに、第４実施形態及び第５実施形態に係る移動軌跡算定システム１００は、スキャン点を直接使うのではなく、その集合であるマップ点の法線を主成分分析で得ているため、形状に多少の凹凸があっても安定して処理を行うことができる。

なお上述した実施の形態は本発明の理解を容易にするためのものであり、本発明を限定して解釈するためのものではない。本発明はその趣旨を逸脱することなく変更、改良され得るとともに、本発明にはその等価物も含まれる。

例えば、移動体１０００は、自らの判断で自らの運動を自律的に制御することが可能な装置の他、人間や他の制御装置からの操作に従って移動する装置でも良い。さらには人間が２Ｄレーザスキャナ（距離センサ２００）及び移動軌跡算定システム１００を携帯して移動することで、人間の移動軌跡を推定する形態であってもよい。

さらに移動軌跡算定システム１０００は、第２実施形態から第４実施形態に示した構成を任意の組み合わせで備える構成であっても良い。

１００ 移動軌跡算定システム
１１０ スキャンデータ取得部
１２０ 経路情報記憶部
１３０ 移動ベクトル算出部
１４０ 位置情報算出部
１５０ 再訪点検出部
１６０ 累積移動距離算出部
１７０ ループ閉じ込み処理部
１７１ 第１相対位置ベクトル算出部
１７２ 第２相対位置ベクトル算出部
１７３ 位置情報修正部
１８０ 地図作成部
１９０ 地図情報記憶部
２００ 距離センサ
４００ 運動制御部
５００ アクチュエータ
１０００ 移動体

Claims (9)

1. 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムであって、
   前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得するスキャンデータ取得部と、
   第１タイミングで取得したスキャンデータと、前記第１タイミングから第１所定時間後の第２タイミングで取得したスキャンデータと、を照合することにより、前記第１所定時間における前記移動体の移動ベクトルを求める移動ベクトル算出部と、
   前記第１所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第１所定時間毎の各通過地点の位置情報を求める位置情報算出部と、
   前記移動体の前記移動ベクトルの大きさを累積することにより、前記移動体の移動開始地点から前記各通過地点までのそれぞれの累積移動距離を求める累積移動距離算出部と、
   前記各通過地点のうち前記移動体の現在位置からの距離が所定値よりも小さな通過地点を前記移動体の再訪点として検出し、前記再訪点が複数検出された場合には、前記複数の再訪点のそれぞれの前記累積移動距離の差が所定値以内のもの同士が同一グループになるように前記複数の再訪点を分類し、前記各グループから、前記移動体の前記現在位置に最も近接する再訪点を一つ選出する再訪点検出部と、
   を備えることを特徴とする移動軌跡算定システム。
2. 請求項１に記載の移動軌跡算定システムであって、
   前記移動体の現在位置の位置情報と、前記再訪点の位置情報と、に基づいて、前記現在位置と前記再訪点との間の相対位置を示す第１相対位置ベクトルを求める第１相対位置ベクトル算出部と、
   前記移動体が前記再訪点を通過した第３タイミングに取得したスキャンデータと、前記移動体が前記現在位置を通過した第４タイミングに取得したスキャンデータと、を照合することにより、前記現在位置と前記再訪点との間の相対位置を示す第２相対位置ベクトルを求める第２相対位置ベクトル算出部と、
   前記第１相対位置ベクトルが前記第２相対位置ベクトルに近づくように、前記移動体の前記各通過地点の位置情報を修正する位置情報修正部と、
   を備えることを特徴とする移動軌跡算定システム。
3. 請求項２に記載の移動軌跡算定システムであって、
   前記第２相対位置ベクトル算出部は、前記第４タイミングに取得したスキャンデータから得られる外界の各計測点の位置が、前記第３タイミングに取得したスキャンデータから得られる外界の各計測点の位置に近づくように、前記第２相対位置ベクトルを求める
   ことを特徴とする移動軌跡算定システム。
4. 請求項３に記載の移動軌跡算定システムであって、
   前記第２相対位置ベクトル算出部は、前記第４タイミングに取得したスキャンデータから得られる外界の各計測点の特徴を表す記述子と、前記第３タイミングに取得したスキャンデータから得られる外界の各計測点の特徴を表す記述子と、を比較し、前記各記述子の一致度合いが所定の判定値よりも大きな第１計測点及び第２計測点を前記各スキャンデータからそれぞれ抽出し、前記第１計測点の位置が前記第２計測点になるような変位ベクトルを、前記第２相対位置ベクトルの推定値として用いる
   ことを特徴とする移動軌跡算定システム。
5. 請求項３に記載の移動軌跡算定システムであって、
   前記第２相対位置ベクトル算出部は、前記移動体が前記現在位置を通過した前記第４タイミングよりも第２所定期間前の第５タイミングから前記第４タイミングまでの前記移動体の移動ベクトルを、前記移動体が前記再訪点を通過した前記第３タイミングよりも前記第２所定期間前の第６タイミングにおける前記移動体の通過地点の位置情報に加えることにより、前記再訪点の推定位置を求め、前記移動体の前記現在位置と、前記再訪点の前記推定位置と、の間の変位ベクトルを、前記第２相対位置ベクトルの推定値として用いる
   ことを特徴とする移動軌跡算定システム。
6. 請求項２〜５のいずれかに記載の移動軌跡算定システムであって、
   前記スキャンデータ取得部が前記距離センサから新たなスキャンデータを取得する毎に、前記新たなスキャンデータを、前記移動体を基準にした局所座標で表わされた値から、前記移動体に依存しない所定の共通座標で表わされた値に変換して蓄積することにより、外界の地図を作成する地図作成部と、
   をさらに備え、
   前記第２相対位置ベクトル算出部は、前記地図に蓄積されている前記共通座標で表わされたスキャンデータを、前記移動体の前記現在位置を基準とした局所座標上での値に変換することにより、前記現在位置から外界の各計測点までの距離及び方位を示す第１仮想スキャンデータを生成すると共に、前記地図に蓄積されている前記共通座標で表されたスキャンデータを、前記再訪点を基準とした局所座標上での値に変換することにより、前記再訪点から外界の各計測点までの距離及び方位を示す第２仮想スキャンデータを生成し、前記第１仮想スキャンデータ及び前記第２仮想スキャンデータを、それぞれ、前記第３タイミングにおける前記スキャンデータ及び前記第４タイミングにおける前記スキャンデータであるものとして照合することにより、前記現在位置と前記再訪点との間の位置差を示す前記第２相対位置ベクトルを求める
   ことを特徴とする移動軌跡算定システム。
7. 請求項６に記載の移動軌跡算定システムであって、
   前記第２相対位置ベクトル算出部は、前記第１仮想スキャンデータを生成する際には、前記移動体が前記現在位置を通過した前記第４タイミングから第３所定期間内に前記距離センサから取得したスキャンデータに由来して前記地図に蓄積されたスキャンデータを用いると共に、前記第２仮想スキャンデータを生成する際には、前記移動体が前記再訪点を通過した前記第３タイミングから前記第３所定期間内に前記距離センサから取得したスキャンデータに由来して前記地図に蓄積されたスキャンデータを用いる
   ことを特徴とする移動軌跡算定システム。
8. 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムの制御方法であって、
   前記移動軌跡算定システムが、前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得し、
   前記移動軌跡算定システムが、第１タイミングで取得したスキャンデータと、前記第１タイミングから第１所定時間後の第２タイミングで取得したスキャンデータと、を照合することにより、前記第１所定時間における前記移動体の移動ベクトルを求め、
   前記移動軌跡算定システムが、前記第１所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第１所定時間毎の各通過地点の位置情報を求め、
   前記移動軌跡算定システムが、前記移動体の前記移動ベクトルの大きさを累積することにより、前記移動体の移動開始地点から前記各通過地点までのそれぞれの累積移動距離を求め、
   前記移動軌跡算定システムが、前記各通過地点のうち前記移動体の現在位置からの距離が所定値よりも小さな通過地点を前記移動体の再訪点として検出し、前記再訪点が複数検出された場合には、前記複数の再訪点のそれぞれの前記累積移動距離の差が所定値以内のもの同士が同一グループになるように前記複数の再訪点を分類し、前記各グループから、前記移動体の前記現在位置に最も近接する再訪点を一つ選出することを特徴とする移動軌跡算定システムの制御方法。
9. 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムに、
   前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得する手順と、
   第１タイミングで取得したスキャンデータと、前記第１タイミングから第１所定時間後の第２タイミングで取得したスキャンデータと、を照合することにより、前記第１所定時間における前記移動体の移動ベクトルを求める手順と、
   前記第１所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第１所定時間毎の各通過地点の位置情報を求める手順と、
   前記移動体の前記移動ベクトルの大きさを累積することにより、前記移動体の移動開始地点から前記各通過地点までのそれぞれの累積移動距離を求める手順と、
   前記各通過地点のうち前記移動体の現在位置からの距離が所定値よりも小さな通過地点を前記移動体の再訪点として検出し、前記再訪点が複数検出された場合には、前記複数の再訪点のそれぞれの前記累積移動距離の差が所定値以内のもの同士が同一グループになるように前記複数の再訪点を分類し、前記各グループから、前記移動体の前記現在位置に最も近接する再訪点を一つ選出する手順と、
   を実行させるためのプログラム。

Images