(57)【要約】【課題】移動体の再訪点をより効率的に特定し、移動体の移動軌跡を算定する際の処理負荷を軽減する。【解決手段】移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより移動体の移動軌跡を算定する移動軌跡算定システムであって、移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得するスキャンデータ取得部と、第１タイミングで取得したスキャンデータと第２タイミングで取得したスキャンデータとを照合することにより移動体の移動ベクトルを求める移動ベクトル算出部と、移動ベクトルを累積することにより移動体の各通過地点の位置情報を求める位置情報算出部と、各通過地点の中から移動体の現在位置からの距離が所定値よりも小さくかつ極小となる通過地点を移動体の再訪点として検出する再訪点検出部と、を備える。【選択図】図１
2018120460.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムであって、
前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得するスキャンデータ取得部と、
第１タイミングで取得したスキャンデータと、前記第１タイミングから第１所定時間後の第２タイミングで取得したスキャンデータと、を照合することにより、前記第１所定時間における前記移動体の移動ベクトルを求める移動ベクトル算出部と、
前記第１所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第１所定時間毎の各通過地点の位置情報を求める位置情報算出部と、
前記各通過地点の中から、前記移動体の現在位置からの距離が所定値よりも小さく、かつ、極小となる通過地点を、前記移動体の再訪点として検出する再訪点検出部と、
を備えることを特徴とする移動軌跡算定システム。
【請求項２】
請求項１に記載の移動軌跡算定システムであって、
前記移動体の前記移動ベクトルの大きさを累積することにより、前記移動体の移動開始地点から前記各通過地点までのそれぞれの累積移動距離を求める累積移動距離算出部と、
をさらに備え、
前記再訪点検出部は、前記再訪点が複数検出された場合には、前記複数の再訪点のそれぞれの累積移動距離の差が所定値以内のもの同士が同一グループになるように、前記複数の再訪点を分類し、前記各グループから一つの再訪点を選出する
ことを特徴とする移動軌跡算定システム。
【請求項３】
請求項２に記載の移動軌跡算定システムであって、
前記再訪点検出部は、前記各グループから一つの再訪点を選出する際には、前記各グループから、前記移動体の前記現在位置に最も近接する再訪点を一つ選出する
ことを特徴とする移動軌跡算定システム。
【請求項４】
請求項１〜３のいずれかに記載の移動軌跡算定システムであって、
前記移動体の現在位置の位置情報と、前記再訪点の位置情報と、に基づいて、前記現在位置と前記再訪点との間の相対位置を示す第１相対位置ベクトルを求める第１相対位置ベクトル算出部と、
前記移動体が前記再訪点を通過した第３タイミングに取得したスキャンデータと、前記移動体が前記現在位置を通過した第４タイミングに取得したスキャンデータと、を照合することにより、前記現在位置と前記再訪点との間の相対位置を示す第２相対位置ベクトルを求める第２相対位置ベクトル算出部と、
前記第１相対位置ベクトルが前記第２相対位置ベクトルに近づくように、前記移動体の前記各通過地点の位置情報を修正する位置情報修正部と、
を備えることを特徴とする移動軌跡算定システム。
【請求項５】
請求項４に記載の移動軌跡算定システムであって、
前記第２相対位置ベクトル算出部は、前記第４タイミングに取得したスキャンデータから得られる外界の各計測点の位置が、前記第３タイミングに取得したスキャンデータから得られる外界の各計測点の位置に近づくように、前記第２相対位置ベクトルを求める
ことを特徴とする移動軌跡算定システム。
【請求項６】
請求項５に記載の移動軌跡算定システムであって、
前記第２相対位置ベクトル算出部は、前記第４タイミングに取得したスキャンデータから得られる外界の各計測点の特徴を表す記述子と、前記第３タイミングに取得したスキャンデータから得られる外界の各計測点の特徴を表す記述子と、を比較し、前記各記述子の一致度合いが所定の判定値よりも大きな第１計測点及び第２計測点を前記各スキャンデータからそれぞれ抽出し、前記第１計測点の位置が前記第２計測点になるような変位ベクトルを、前記第２相対位置ベクトルの推定値として用いる
ことを特徴とする移動軌跡算定システム。
【請求項７】
請求項５に記載の移動軌跡算定システムであって、
前記第２相対位置ベクトル算出部は、前記移動体が前記現在位置を通過した前記第４タイミングよりも第２所定期間前の第５タイミングから前記第４タイミングまでの前記移動体の移動ベクトルを、前記移動体が前記再訪点を通過した前記第３タイミングよりも前記第２所定期間前の第６タイミングにおける前記移動体の通過地点の位置情報に加えることにより、前記再訪点の推定位置を求め、前記移動体の前記現在位置と、前記再訪点の前記推定位置と、の間の変位ベクトルを、前記第２相対位置ベクトルの推定値として用いる
ことを特徴とする移動軌跡算定システム。
【請求項８】
請求項４〜７のいずれかに記載の移動軌跡算定システムであって、
前記スキャンデータ取得部が前記距離センサから新たなスキャンデータを取得する毎に、前記新たなスキャンデータを、前記移動体を基準にした局所座標で表わされた値から、前記移動体に依存しない所定の共通座標で表わされた値に変換して蓄積することにより、外界の地図を作成する地図作成部と、
をさらに備え、
前記第２相対位置ベクトル算出部は、前記地図に蓄積されている前記共通座標で表わされたスキャンデータを、前記移動体の前記現在位置を基準とした局所座標上での値に変換することにより、前記現在位置から外界の各計測点までの距離及び方位を示す第１仮想スキャンデータを生成すると共に、前記地図に蓄積されている前記共通座標で表されたスキャンデータを、前記再訪点を基準とした局所座標上での値に変換することにより、前記再訪点から外界の各計測点までの距離及び方位を示す第２仮想スキャンデータを生成し、前記第１仮想スキャンデータ及び前記第２仮想スキャンデータを、それぞれ、前記第３タイミングにおける前記スキャンデータ及び前記第４タイミングにおける前記スキャンデータであるものとして照合することにより、前記現在位置と前記再訪点との間の位置差を示す前記第２相対位置ベクトルを求める
ことを特徴とする移動軌跡算定システム。
【請求項９】
請求項８に記載の移動軌跡算定システムであって、
前記第２相対位置ベクトル算出部は、前記第１仮想スキャンデータを生成する際には、前記移動体が前記現在位置を通過した前記第４タイミングから第３所定期間内に前記距離センサから取得したスキャンデータに由来して前記地図に蓄積されたスキャンデータを用いると共に、前記第２仮想スキャンデータを生成する際には、前記移動体が前記再訪点を通過した前記第３タイミングから前記第３所定期間内に前記距離センサから取得したスキャンデータに由来して前記地図に蓄積されたスキャンデータを用いる
ことを特徴とする移動軌跡算定システム。
【請求項１０】
移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムの制御方法であって、
前記移動軌跡算定システムが、前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得し、
前記移動軌跡算定システムが、第１タイミングで取得したスキャンデータと、前記第１タイミングから第１所定時間後の第２タイミングで取得したスキャンデータと、を照合することにより、前記第１所定時間における前記移動体の移動ベクトルを求め、
前記移動軌跡算定システムが、前記第１所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第１所定時間毎の各通過地点の位置情報を求め、
前記移動軌跡算定システムが、前記各通過地点の中から、前記移動体の現在位置からの距離が所定値よりも小さく、かつ、極小となる通過地点を、前記移動体の再訪点として検出する
ことを特徴とする移動軌跡算定システムの制御方法。
【請求項１１】
移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより前記移動体の移動軌跡を算定する移動軌跡算定システムに、
前記移動体に搭載される距離センサによって計測される外界の各点までの距離及び方位を含むスキャンデータを所定周期で取得する手順と、
第１タイミングで取得したスキャンデータと、前記第１タイミングから第１所定時間後の第２タイミングで取得したスキャンデータと、を照合することにより、前記第１所定時間における前記移動体の移動ベクトルを求める手順と、
前記第１所定時間毎に求めた移動ベクトルを累積することにより、前記移動体の前記第１所定時間毎の各通過地点の位置情報を求める手順と、
前記各通過地点の中から、前記移動体の現在位置からの距離が所定値よりも小さく、かつ、極小となる通過地点を、前記移動体の再訪点として検出する手順と、
を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動軌跡算定システム、移動軌跡算定システムの制御方法及びプログラムに関する。
【背景技術】
【０００２】
近年、外界の状況をリアルタイムに判断しながら、自らの運動を自律的に制御可能な移動ロボットの開発が急速に進められている。
【０００３】
そしてそのための技術として、移動ロボットに外界を認識するためのレーザスキャナやカメラなどの外界センサを搭載し、外界センサから得られる情報を移動ロボット自らがリアルタイムに解析することで、外界の地図を構築しつつ、自らの運動状態を推定するＳＬＡＭ（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)を参照）などのソフトウェアライブラリを用いて実現することができる。
【００５５】
そして移動ベクトル算出部１３０は、得られた点の対応付けに対して、下記の式（１）を最小化する移動ベクトルｍｔを求める。式（１）を最小化する手法は種々あるが、本実施形態では準ニュートン法を用いている。
【００５６】
移動ベクトルｍｔの初期値は、移動体１００がオドメトリを有している場合には、オドメトリで求めた予測値を用いることができる。あるいは、オドメトリを用いなくても、第１所定時間前に求めた移動ベクトル（前回求めた移動ベクトルｍｔ−１）や所定の固定値を初期値として用いても良い。
【００５７】
移動ベクトル算出部１３０は、この点の対応づけと誤差最小化の処理を、Ｇ（ｍｔ） の値が収束するまで交互に繰り返すことでＩＣＰを行う。
【００５８】
【数１】
2018120460.tif 000003
ここで、ｐｔ，ｉは、タイミングｔ（第２タイミング）のスキャンデータから得られるスキャン点（入力スキャン点とも記す）であり、ｐｒｅｆ，ｉは、対応するスキャン点（第１タイミングのスキャンデータから得られるスキャン点であり、参照スキャン点とも記す）である。ｈ（ｐｔ，ｉ，ｍｔ）はｐｔ，ｉをｍｔで座標変換して得た位置である（ｐｔ，ｉをｍｔの回転成分で回転し、さらにｍｔの並進成分を加える）。
【００５９】
また、ｎｒｅｆ，ｉは、ｐｒｅｆ，ｉの法線ベクトルである。この評価関数では、ｐｒｅｆ，ｉを通りｎｒｅｆ，ｉと直交する直線とｐｔ，ｉとの距離を計算している。一般に、このような点対線の誤差関数の方が、上述の点対点の誤差関数よりも極小値に陥りにくい。ただし、得られるスキャンデータ内に直線が少ない環境では、点対点の誤差関数（２点間のユークリッド距離）を用いてもよい。
【００６０】
ρは、Ｍ推定のカーネル関数であり、誤差の大きいアウトライア（外れ値）の影響を軽減するために用いる。本実施形態では、Ｇｅｍａｎ−ＭｃＣｌｕｒｅカーネルを用いているが、他のカーネル関数でもよい。
【００６１】
移動ベクトル算出部１３０は、このようにして新たな移動ベクトルｍｔを求める毎に、経路情報記憶部１２０に順次記憶しておく。なおｍ０は０ベクトルである（並進０、回転０）。
【００６２】
＜位置情報算出部＞
つぎに位置情報算出部１４０について説明する。
【００６３】
位置情報算出部１４０は、第１所定時間毎に得られる移動ベクトルｍiを累積することにより、移動体１０００の第１所定時間毎の通過地点の位置情報ｒｔを求める。
【００６４】
移動ベクトルｍiを累積するとは、移動ベクトルｍi−１の終点の位置に移動ベクトルｍiの始点を重ねることをいう。
【００６５】
具体的には、位置情報算出部１４０は、新たな移動ベクトルが算出される毎に、式（２）により移動ベクトルを累積し、移動体１０００の位置情報を求める。
【００６６】
【数２】
2018120460.tif 000004
位置情報算出部１４０が移動ベクトルｍiを累積することで移動体１０００の各通過地点の位置情報ｒｔ(t＝０〜ｎ)を求める様子を図３に示す。なお本実施形態では、移動開始地点ｒ０も、便宜上、通過地点と称する。
【００６７】
＜累積移動距離算出部＞
累積移動距離算出部１６０は、移動体１０００の移動ベクトルｍiの大きさ|ｍi|を累積することにより、移動体１０００の移動開始地点ｒ０から各通過地点ｒｔ（ｔ＝０〜ｎ）までのそれぞれの累積移動距離Ｌｔを求める。
【００６８】
具体的には、累積移動距離算出部１６０は、新たな移動ベクトルが算出される毎に、式（３）により移動体１０００の累積移動距離Ｌｔを求める。
【００６９】
【数３】
2018120460.tif 000005
累積移動距離算出部１６０が移動ベクトルｍiの大きさ|ｍi|を累積することにより、移動体１０００の移動開始地点ｒ０から各通過地点ｒｔ(t＝０〜ｎ)までの累積移動距離Ｌｔ(t＝０〜ｎ)を求める様子を図３に示す。
【００７０】
＜再訪点検出部＞
次に、再訪点検出部１５０について説明する。再訪点検出部１５０は、移動体１０００の各通過地点ｒｔ（ｔ＝０〜ｎ）の中から、移動体１０００の最新の通過地点である現在位置ｒｎからの距離が所定値よりも小さく、かつ、極小となる通過地点を、移動体１０００の再訪点ｒｓとして検出する。
【００７１】
図４に、再訪点検出部１５０によって現在位置ｒｎに対して３つの再訪点ｒｓ１、ｒｓ２、ｒｓ３が検出された様子を示す。なお図４において、現在位置ｒｎから距離が所定値となる場所を破線の丸印で示してある。
【００７２】
これらの再訪点の検出方法を、図５（ａ）及び図５（ｂ）を参照しながらより詳細に説明する。
【００７３】
図５（ａ）は、移動体１０００の現在位置ｒｎからの距離が所定値よりも小さい通過地点を黒丸で示した図である。
【００７４】
図５（ａ）に示すように、移動体１０００の現在位置ｒｎからの距離が所定値以内の範囲（破線の範囲内）には、過去の移動体１０００の通過地点が複数存在する場合がある。
【００７５】
この場合、これらの通過地点を全て再訪点としてしまうと、移動軌跡算定システム１００は、現在位置ｒｎと、これらの再訪点と、に対して後述するループ閉じ込みと呼ばれる処理をそれぞれ行うことになる。
【００７６】
ループ閉じ込みは、移動体１０００の現在位置ｒｎと再訪点ｒｓとのずれを修正するために、現在位置ｒｎにおいて得られたスキャンデータＰｎと、再訪点ｒｓにおいて得られたスキャンデータＰｓと、を照合する処理や、経路情報記憶部１２０に記憶されている移動体１０００の各通過地点の位置情報ｒｔ（ｔ＝０〜ｎ）と地図情報１９０を更新する処理を含むため、負荷が大きい。
【００７７】
そのため、移動体１０００が現在位置ｒｎから次の位置ｒｎ＋１に移動する短時間の間に、図５（ａ）に示す全ての再訪点に対するループ閉じ込みの処理を完了できない可能性もある。
【００７８】
そこで本実施形態に係る再訪点検出部１５０は、移動体１０００の現在位置ｒｎとの距離が極小となる通過地点を再訪点ｒｓとして検出するようにして、再訪点の絞り込みを行う（第１の近傍抑制）。
【００７９】
図５（ｂ）は、移動体１０００の現在位置ｒｎとの距離が極小となる通過地点を再訪点ｒｓとして検出する際の様子を示す図である。図５（ｂ）には、説明の簡単化のために、現在位置ｒｎ及び再訪点ｒｓ１にそれぞれ連なる通過地点のみを示してあるが、他の再訪点ｒｓ２、ｒｓ３についても同様である。
【００８０】
まず再訪点検出部１５０は、移動体１０００の現在位置ｒｎからの距離が所定値よりも小さな８つの通過地点ｒｓ１−２、ｒｓ１−１、ｒｓ１、ｒｓ１＋１、ｒｓ１＋２、ｒｎ、ｒｎ−１、ｒｎ−２のうち、現在位置ｒｎからの距離が極小となる点を選ぶ。この例では、ｒｎ自身とｒｓ１が選ばれる。ただし、ｒｎは現在位置なので再訪点には含めない。このように極小点を求めることで再訪点の個数を減らすことができる。なお、この例では、簡単のため、通過点を８個としているが、実際は任意の個数でよい。
【００８１】
さらに、累積移動距離を用いることで、再訪点の絞り込みを行うこともできる（第２の近傍抑制）。上述のようにして求めた極小点を中心に、該極小点との累積移動距離の差が所定値以内の通過地点を同一グループになるように分類する。図５（ｂ）に示す例では、上記８つの通過地点は、ｒｓ１−２、ｒｓ１−１、ｒｓ１、ｒｓ１＋１、ｒｓ１＋２からなる第１グループと、ｒｎ、ｒｎ−１、ｒｎ−２からなる第２グループに分類される。
【００８２】
グループの中心となる極小点の選び方によって、グループ構成は変わりうる。選び方としては、たとえば、現在位置ｒｎからの直線距離が小さい順に極小点を選ぶ、または、現在位置ｒｎから時刻を逆順にたどって極小点を選ぶ、などが考えられる。
【００８３】
そして次に、再訪点検出部１５０は、各グループごとに、グループ内の各通過地点と現在位置ｒｎとの距離が最小となる通過地点を再訪点として一つ選ぶ。
【００８４】
具体的には、再訪点検出部１５０は、各グループ内で、移動体１０００の現在位置ｒｎと、各通過地点との距離をそれぞれ求め、これらの距離同士を比較する。そして距離が最小となる再訪点を一つ選ぶ。
【００８５】
図５（ｂ）の例では、第１グループからは再訪点ｒｓ１が検出される。第２グループの最小点ｒｎは現在位置なので再訪点には含めない。
【００８６】
このようにして第１グループから選出される再訪点ｒｓ１は、現在位置ｒｎに最も近い本質的な再訪点であると言えるため、再訪点としても好適である。また説明は省略したが、同様にして再訪点ｒｓ２及びｒｓ３も検出される。
【００８７】
なお、１つのグループに極小点が複数含まれることがある。図６の例では、同一グループに分類された複数の通過地点ｒｓ１−１〜ｒｓ１＋５の中で、現在位置ｒｎからの距離が極小となる再訪点が複数検出されている（ｒｓ１とｒｓ１＋４）。このような場合であっても、再訪点検出部１５０は、現在位置ｒｎと通過地点ｒｓ１との間の距離と、現在位置ｒｎと通過地点ｒｓ１＋４との間の距離と、を比較し、通過地点ｒｓ１を再訪点ｒｓ１として選出する。
【００８８】
このようにして本実施形態に係る再訪点検出部１５０によれば、移動体１０００の現在位置ｒｎからの距離が所定値よりも小さく、かつ、極小となる通過地点を、移動体１０００の再訪点ｒｓとして検出するようにすることにより、移動体１０００の再訪点ｒｓを効果的に検出し、移動体１０００の移動軌跡を算定する際の処理負荷を軽減することが可能となる。また、同一グループ内に現在位置ｒｎからの距離が極小となる再訪点が複数検出される場合でも、現在位置ｒｎに最も近接する再訪点を一つ選出することで、さらに効果的に再訪点ｒｓの検出を行うことが可能となる。
【００８９】
＜地図作成部＞
次に、図１に戻って、地図作成部１８０について説明する。 地図作成部１８０は、スキャンデータ取得部１１０が距離センサ２００から新たなスキャンデータＰｔを取得する毎に、この新たなスキャンデータＰｔを、移動体１０００を基準にした局所座標で表わされた値から、移動体１０００の位置に依存しない所定の共通座標で表わされた値に変換する。そして地図作成部１８０はこれらの共通座標に変換されたスキャンデータを順次、地図情報記憶部１９０に蓄積することにより、外界の地図を作成する。
【００９０】
例えば、図１０は、移動体１０００がｒａで示される位置にいるときに、スキャンデータ取得部１１０が距離センサ２００から取得したスキャンデータＰａを元に、外界の各スキャン点の位置を２次元平面状にプロットしたものであり、図１１は、移動体１０００がｒｂで示される位置にいるときに、スキャンデータ取得部１１０が距離センサ２００から取得したスキャンデータＰｂを元に、外界の各スキャン点の位置を２次元平面状にプロットしたものであるが、いずれのスキャンデータＰａ、Ｐｂも、それぞれ移動体１０００の位置ｒａ、ｒｂを中心とした局所座標で表現されているため、そのまま重ね合わせることはできない。
【００９１】
本実施形態では、これらのスキャンデータＰａ、Ｐｂを、共通座標における値に変換するようにしている。共通座標の原点及び座標軸の向きはどのように定めても良いが、例えば移動体１０００の移動開始時の位置及び向きを基準に定めるようにすればよい。
【００９２】
移動体１０００の移動開始時の位置及び向きは、経路情報記憶部１２０に記憶されている移動ベクトルを、移動開始時に遡るように順に参照することによって容易に算定できる。
【００９３】
もちろん実際には、１タイミング毎の移動体１０００の並進量及び回転量を蓄積しておき、局所座標から共通座標へ変換する際にはこれらの蓄積値を用いることにより、容易に局所座標から共通座標への変換を行うことができる。
【００９４】
このようにして各スキャンデータＰｔ（ｔ＝０〜ｎ）から得られる外界の各スキャン点の位置が共通座標上で位置合わせされることによって、地図情報記憶部１９０に作成された地図の一例を図１２に示す。図１２は、地図情報記憶部１９０に蓄積された共通座標のスキャンデータを元に、外界の各スキャン点の位置を２次元平面状にプロットしたものである。このようにして地図情報記憶部１９０に作成された地図は、運動制御部４００によって用いられる。
【００９５】
＜ループ閉じ込み処理部＞
ループ閉じ込み処理部１７０は、移動体１０００の現在位置ｒｎにおいて得られたスキャンデータＰｎと、再訪点ｒｓにおいて得られたスキャンデータＰｓと、を照合することにより、現在位置ｒｎと再訪点ｒｓとの位置ずれを修正する。
【００９６】
ループ閉じ込み処理部１７０は、第１相対位置ベクトル算出部１７１と、第２相対位置ベクトル算出部１７２と、位置情報修正部１７３と、を有して構成されている。
【００９７】
第１相対位置ベクトル算出部１７１は、移動ベクトルｍi（i＝０〜ｎ）を累積することにより算定される移動体１０００の現在位置ｒｎと、移動ベクトルｍi（i＝０〜ｓ）を累積することにより算定される再訪点ｒｓの位置と、の間の相対位置を示す第１相対位置ベクトルｖ１を求める。
【００９８】
そして第２相対位置ベクトル算出部１７２は、移動体１０００が再訪点ｒｓを通過したタイミングｓ（第３タイミング）におけるスキャンデータＰｓと、移動体１０００が現在位置ｒｎを通過したタイミングｎ（第４タイミング）におけるスキャンデータＰｎと、を照合することにより、現在位置ｒｎと再訪点ｒｓとの間の相対位置を示す第２相対位置ベクトルｖ２を求める。
【００９９】
第２相対位置ベクトル算出部１７２は、例えばＩＣＰを用いて、タイミングｓにおけるスキャンデータＰｓとタイミングｎにおけるスキャンデータＰｎとの照合を行う。すなわち、第２相対位置ベクトル算出部１７２は、タイミングｎにおけるスキャンデータＰｎから得られる各スキャン点の位置が、タイミングｓにおけるスキャンデータＰｓから得られる各スキャン点の位置に近づくように、第２相対位置ベクトルｖ２を求める。
【０１００】
第２相対位置ベクトル算出部１７２は、移動ベクトル算出部１３０と同様に、各スキャンデータＰｎ、Ｐｓから得られるスキャン点の対応付けと、移動体１０００の位置の最適化を交互に行うことで、２つの点群の位置合わせを行う。
【０１０１】
そして第２相対位置ベクトル算出部１７２は、得られた点の対応付けに対して、式（４）を最小化する第２相対位置ベクトルｖ２を求める。
【０１０２】
【数４】
2018120460.tif 000006
式（４）は式（１）と同型であり、ｐｎ，ｉは、タイミングｎ（第４タイミング） のスキャンデータＰｎから得られるスキャン点であり、ｐｓ，ｉは、対応するスキャン点（第３タイミングのスキャンデータから得られるスキャン点）である。ｎｓ，ｉは、ｐｓ，ｉの法線ベクトルである。また、ｈ（ｐｎ，ｉ，ｖ２）はｐｎ，ｉをｖ２で座標変換して得た位置である。ρは、Ｍ推定のカーネル関数である。式（４）を最小化する手法は種々あるが、本実施形態では準ニュートン法を用いている。
【０１０３】
第２相対位置ベクトル算出部１７２は、このようにして第２相対位置ベクトルｖ２を求める。そして第２相対位置ベクトル算出部１７２は、第２相対位置ベクトルｖ２を経路情報記憶部１２０に記憶する。
【０１０４】
次に、第２相対位置ベクトル算出部１７２がＩＣＰを行う際に用いる第２相対位置ベクトルｖ２の初期値について説明する。この初期値の算出方法として、以下に第１の方法及び第２の方法を例示する。
【０１０５】
第１の方法は、移動体１０００が現在位置ｒｎを通過した第４タイミングにおけるスキャンデータＰｎから得られる各スキャン点の特徴を表す記述子（後述）と、移動体１０００が再訪点ｒｓを通過した第３タイミングにおけるスキャンデータＰｓから得られる各スキャン点の特徴を表す記述子と、を比較し、これらの各記述子の一致度合いが所定の判定値よりも大きな第１スキャン点及び第２スキャン点を各スキャンデータＰｎ、Ｐｓからそれぞれ抽出し、第１スキャン点の位置が第２スキャン点の位置になるような変位ベクトルｑ（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)を参照）。
【０１１３】
この初期値は粗い推定であるが、式（４）のＩＣＰで詳細なマッチングを行い、スコアが最もよい解を採用することで、より正確な第２相対位置ベクトルｖ２を算出することができる。
【０１１４】
なお、局所記述子は、１つのタイミングのスキャンデータのみを用いて生成しても良いが、前後のタイミングを含む複数のスキャンデータを用いて生成するとなお良い。これにより局所記述子を構成する近傍点の分布を安定させることが可能となる。
【０１１５】
また、局所記述子は他の方法によるものを用いてもよい。さらには、局所記述子を用いずに姿勢クラスタリングを行うことも可能である。その場合は、スキャンデータＰｎから得られるスキャン点と、スキャンデータＰｓから得られるスキャン点のすべての組について式（５）〜式（７）を解いて、姿勢クラスタリングを行う。
【０１１６】
次に、第２の方法について説明する。図８は、第２の方法を説明するための図である。
【０１１７】
第２の方法は、移動体１０００が現在位置ｒｎを通過したタイミングｎ（第４タイミング）よりも第２所定期間前（例えば１タイミング前）のタイミングｎ−１（第５タイミング）からタイミングｎ（第４タイミング）までの移動体１０００の移動ベクトルｍｒを、移動体１０００が再訪点ｒｓを通過したタイミングｓ（第３タイミング）よりも１タイミング前（第２所定期間前）のタイミングｓ−１（第６タイミング）における移動体１０００の再訪点の確定した位置（ｒ’ｓ−１＝ｒｎ−１＋ｖ２,ｎ−１）に加えることにより、再訪点ｒｓの推定位置（ｒ’ｓ−１＋ｍｒ）を求め、移動体１０００の現在位置ｒｎと、再訪点の推定位置（ｒ’ｓ−１＋ｍｒ）と、の間の変位ベクトルΔｒｎを、第２相対位置ベクトルｖ２の推定値（初期値）として用いる方法である。ただし、ｖ２,ｎ−１はタイミングｎ−１での第２相対位置ベクトルである。
【０１１８】
つまり第２の方法は、タイミングｎ−１で実施したループ閉じ込みによって確定した再訪点ｒｓ−１の位置ｒ’ｓ−１から、移動体１０００の移動ベクトルｍｒを用いて再訪点ｒｓの位置を推定する方法である。
【０１１９】
なお、移動ベクトル（または相対位置ベクトル）ｕ1=(x1, y1,θ1)とｕ1=(x2, y2, θ2)の加法（u=u1+u2）は次のように行う。
【０１２０】
x = x1 + (x2cosθ1 - y2sinθ1)     （８）
y = y1 + (x2sinθ1 + y2cosθ1)     （９）
θ=θ1 + θ2                      （１０）
第２相対位置ベクトル算出部１７２は、この推定を例えば１フレームごとに行う。１フレーム分の移動体１０００の移動量ｍｒは小さいので、移動体１０００が再訪点ｒｓを通過したタイミングｓの１タイミング前のタイミングｓ−１において確定した再訪点の位置ｒ’ｓ−１を基準にすることで、現在位置ｒｎと再訪点ｒｓとの間の第２相対位置ベクトルｖ２,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)を参照）。
【０１３０】
【数５】
2018120460.tif 000007
ここで、ｃはアーク集合、ｕｉ，ｊはアーク（ｉ，ｊ）∈ｃの観測値（すなわち、移動ベクトルｍｉ、あるいは、第２相対位置ベクトルｖ２）、関数ｇ（ｒｉ，ｒｊ）は移動体１０００の位置ｒｉとｒｊから計算で得た相対位置（すなわち第１相対位置ベクトルｖ１）である。またｗｉ，ｊは、ｕｉ，ｊの誤差共分散である。
【０１３１】
以上のようにして、ループ閉じ込み処理部１７０はループ閉じ込み処理を行う。
【０１３２】
このように、移動体１０００が再訪点ｒｓを通過した際のスキャンデータＰｓを現在位置ｒｎのスキャンデータＰｎと比較することで移動体１０００の移動軌跡を修正することにより、移動体１０００の位置及び運動状態をより正確に把握することが可能となる。
【０１３３】
そして本実施形態に係る移動軌跡算定システム１０００は、上述したように、移動体１０００の現在位置ｒｎに対して本質的な再訪点ｒｓを特定することができるため、ループ閉じ込みの処理を無駄なく行い、処理負荷の軽減を図ることが可能となる。
【０１３４】
なお、複数の再訪点、例えば２つの再訪点ｒｓ１、ｒｓ２検出された場合には、ループ閉じ込み処理部１７０は、各再訪点ｒｓ１、ｒｓ２に対してそれぞれループ閉じ込みの処理を行う。
【０１３５】
＝移動軌跡算定システムの制御方法＝
次に、移動軌跡算定システム１００による処理の流れを、図９を参照しながら説明する。
【０１３６】
まず、スキャンデータ取得部１１０は、距離センサ２００から出力される外界の各スキャン点までの距離及び方向を示す計測結果を順次取得する。そしてスキャンデータ取得部１１０は、距離センサ２００による距離の計測方向が１回転する毎に、１回転分の計測結果をスキャンデータＰｔ（フレームＰｔ）としてまとめ、経路情報記憶部１２０に記憶する（S1000）。
【０１３７】
そして移動ベクトル算出部１３０は、新たにスキャンデータを取得する毎に、このスキャンデータを前回取得したスキャンデータと照合することにより、第１所定時間毎の移動体１０００の移動ベクトルを求め、経路情報記憶部１２０に記憶する（S1010）。
【０１３８】
そして位置情報算出部１４０は、上記第１所定時間毎に得られる移動ベクトルｍi（i=０〜t）を累積することにより、移動体１０００の第１所定時間毎の通過地点の位置情報ｒｔを求め、経路情報記憶部１２０に記憶する（S1020）。
【０１３９】
また累積移動距離算出部１６０は、移動体１０００の移動ベクトルｍiの大きさ|ｍi|（i=０〜t）を累積することにより、移動体１０００の移動開始地点から各通過地点までのそれぞれの累積移動距離Ｌｔを求め、経路情報記憶部１２０に記憶する（S1030）。
【０１４０】
そして再訪点検出部１５０は、移動体１０００の各通過地点の中から、移動体１０００の最新の通過地点である現在位置ｒｎからの距離が所定値よりも小さく、かつ、極小となる通過地点を、移動体１０００の再訪点ｒｓとして検出する（S1040）。
【０１４１】
本実施形態に係る移動軌跡算定システム１０００は、このような処理を行うことによって、移動体１０００の現在位置ｒｎに対して本質的な再訪点ｒｓを特定することができるようになるため、ループ閉じ込みの処理を無駄なく行い、移動体１０００の移動軌跡を算定する際の処理負荷の軽減を図ることが可能となる。
【０１４２】
その後、ループ閉じ込み処理部１７０は、再訪点ｒｓが検出された場合には、移動体１０００の現在位置ｒｎにおいて得られたスキャンデータＰｎと、再訪点ｒｓにおいて得られたスキャンデータＰｓと、を照合することにより、現在位置ｒｎと再訪点ｒｓとの位置ずれを修正するループ閉じ込みを行う（S1050）。
【０１４３】
また、図９に示すフローチャートには含まれていないが、地図作成部１８０は、スキャンデータ取得部１１０が距離センサ２００から新たなスキャンデータＰｔを取得する毎に、この新たなスキャンデータＰｔを、移動体１０００を基準にした局所座標で表わされた値から、移動体１０００の位置に依存しない共通座標で表わされた値に変換し、そして、これらの共通座標に変換されたスキャンデータを順次、地図情報記憶部１９０に蓄積する。これにより、地図作成部１８０は、外界の地図を作成する。
【０１４４】
移動軌跡算定システム１００は、このようにして作成された地図を用いることにより、移動体１０００の自律的な運動を適切に行うことが可能となる。
【０１４５】
＝＝第２実施形態＝＝
次に、第２実施形態に係る移動軌跡算定システム１００について説明する。
【０１４６】
第１実施形態では、再訪点検出部１５０は、スキャンデータ取得部１１０が距離センサ２００からスキャンデータＰｔを取得する毎に再訪点ｒｓの検出を行っているが、第２実施形態に係る再訪点検出部１５０は、キーフレームごとに再訪点ｒｓの検出を行う。キーフレームは、所定枚数毎のスキャンデータから選出されるスキャンデータであり、例えば距離センサ２００のレーザパルスが１０回転するごとに抽出されるスキャンデータ（フレーム）である。
【０１４７】
このような態様により、再訪点検出処理（S1040）及びループ閉じ込み処理（S1050）の実行頻度を減らし、移動軌跡算出システム１００の処理負荷をさらに軽減することが可能となる。そしてこれにより、処理速度の高速化を図ることができる。
【０１４８】
＝＝第３実施形態＝＝
次に、第３実施形態に係る移動軌跡算定システム１００について説明する。
【０１４９】
第１実施形態及び第２実施形態では、第２相対位置ベクトル算出部１７２は、経路情報記憶部１２０に記憶されている現在位置ｒｎにおけるスキャンデータＰｎと、再訪点ｒｓにおけるスキャンデータＰｓと、を用いて第２相対位置ベクトルｖ２を算出したが、第３実施形態では、地図情報記憶部１９０に記憶されている地図を用いて第２相対位置ベクトルｖ２を算出する。以下に詳しく説明する。
【０１５０】
図１０や図１１に示したように、１個のスキャンデータでは、周囲の環境形状を十分に表現できないことがある。たとえば、距離センサ２００が視野の狭いレーザスキャナである場合は、全周の形状が得られないし、レーザスキャナのレンジが短い場合は、近距離にある存在物の形状しかわからない。
【０１５１】
そこで第３実施形態では、移動体１０００の現在位置ｒｎと再訪点ｒｓとの間の第２相対位置ベクトルｖ２を求める際に、経路情報記憶部１２０に記憶されたスキャンデータを用いるのではなく、地図情報記憶部１９０に記憶されている地図を用いることにする。
【０１５２】
つまり、地図情報記憶部１９０には、外界の各スキャン点までの距離及び方位を示すスキャンデータが共通座標に変換された形で移動体１０００の移動開始時点から蓄積されているが、第３実施形態に係る第２相対位置ベクトル算出部１７２は、この地図情報記憶部１９０に蓄積されているスキャンデータを、移動体１０００の現在位置ｒｎ及び再訪点ｒｓを起点とする局所座標での距離及び方位を示すデータ（仮想スキャンデータと呼ぶ）にそれぞれ変換し、それらの仮想スキャンデータを照合することにより、現在位置ｒｎと再訪点ｒｓとの相対位置を求める。
【０１５３】
第２相対位置ベクトル算出部１７２は、移動体１０００の位置（現在位置ｒｎ、再訪点ｒｓ）を中心として半径Ｒの範囲にあるスキャン点を地図情報記憶部１９０に記憶されている全体地図から取り出すことで、仮想スキャンデータを生成する。
【０１５４】
より詳しくは、第２相対位置ベクトル算出部１７２は、まず、地図情報記憶部１９０に蓄積されている共通座標で表わされたスキャンデータを、移動体１０００の現在位置ｒｎを基準とした局所座標上での値に変換することにより、現在位置ｒｎから外界の各スキャン点までの距離及び方位を示す第１仮想スキャンデータを生成する。
【０１５５】
また同様に、第２相対位置ベクトル算出部１７２は、地図情報記憶部１９０に蓄積されている共通座標で表されたスキャンデータを、再訪点ｒｓを基準とした局所座標上での値に変換することにより、再訪点ｒｓから外界の各スキャン点までの距離及び方位を示す第２仮想スキャンデータを生成する。
【０１５６】
そして第２相対位置ベクトル算出部１７２は、第１仮想スキャンデータと第２仮想スキャンデータとを照合することにより、現在位置ｒｎと再訪点ｒｓとの間の相対位置を示す第２相対位置ベクトルｖ２を求める。
【０１５７】
このような態様により、移動体１０００の現在位置ｒｎと再訪点ｒｓとの相対位置である第２相対位置ベクトルｖ２を、より多くのスキャン点を照合することで、より正確に求めることが可能となる。
【０１５８】
また本実施形態に係る第２相対位置ベクトル算出部１７２は、地図情報記憶部１９０に全体地図として蓄積されている共通座標で表されたスキャンデータから仮想スキャンデータを生成する際に、移動体１０００が再訪点ｒｓを通過した際の局所座標で表されたスキャンデータＰｓに由来するスキャンデータと、移動体１０００が現在位置ｒｎを通過した際の局所座標で表されたスキャンデータＰｎに由来するスキャンデータとを混在しないようにしている。
【０１５９】
たとえば、図７において、多重ループをなす部分軌跡ｔｒ１、ｔｒ２は、それぞれ地図上においてスキャン点群ｓ１、ｓ２を作るが、ループ閉じ込みを行っていない場合には、移動体１０００の位置情報の誤差が修正されていないために、これらのスキャン点群はきれいに重ならない。これらのスキャン点群を混在したまま地図から仮想スキャンデータを作ると、その形状は正しくないので、現在位置ｒｎと再訪点ｒｓのマッチング誤差が大きくなる。
【０１６０】
そこで、本実施形態に係る第２相対位置ベクトル算出部１７２は、累積走行距離Ｌｔを用いて部分軌跡を分離し、それぞれで仮想スキャンデータを生成するようにしている。
【０１６１】
つまり、第２相対位置ベクトル算出部１７２は、まず、第１仮想スキャンデータを生成する際には、移動体１０００が現在位置ｒｎを通過したタイミングｎ（第４タイミング）から所定タイミングα以内（第３所定期間α以内。ｎ−α〜ｎ）に距離センサ２００から取得したスキャンデータＰｎ−α〜Ｐｎに由来して地図に蓄積されたスキャンデータを用いる。
【０１６２】
また同様に、第２相対位置ベクトル算出部１７２は、第２仮想スキャンデータを生成する際には、移動体１０００が再訪点ｒｓを通過したタイミングｓ（第３タイミング）から所定タイミングα以内（第３所定期間α以内。ｓ−α〜ｓ＋α）に距離センサ２００から取得したスキャンデータＰｓ−α〜Ｐｓ＋αに由来して地図に蓄積されたスキャンデータを用いる。
【０１６３】
より具体的には、まず、第２相対位置ベクトル算出部１７２は、経路情報記憶部１２０から、再訪点ｒｓとの累積走行距離の差および直線距離が閾値より小さい通過地点ｒｓ−α〜ｒｓ＋αを部分軌跡として抽出する。
【０１６４】
そして第２相対位置ベクトル算出部１７２は、経路情報記憶部１２０から部分軌跡として抽出された各通過地点の累積走行距離の最小値atdm（Ｌｓ−α）と、最大値atdM（Ｌｓ＋α）と、を読み出す。
【０１６５】
次に第２相対位置ベクトル算出部１７２は、地図情報記憶部１９０に記憶されているスキャンデータから、累積走行距離がatdm（Ｌｓ−α）とatdM（Ｌｓ＋α）の範囲内にあるスキャンデータＰｓ−α〜Ｐｓ＋αに由来する共通座標のスキャンデータを抽出し、その中で再訪点ｒｓの近傍のスキャンデータ、つまり、再訪点ｒｓとの直線距離が閾値より小さいものを選び、第２仮想スキャンデータとする。
【０１６６】
第１仮想スキャンデータも同様に抽出する。ただし、第１仮想スキャンデータを抽出する場合は、最新のスキャンデータはＰｎなので、Ｐｎ−α〜Ｐｎに由来する共通座標のスキャンデータを地図から抽出し、その中で現在位置ｒｎの近傍のスキャンデータ、つまり、現在位置ｒｎとの直線距離が閾値より小さいものを選ぶ。
【０１６７】
その後第２相対位置ベクトル算出部１７２は、第１実施形態と同様に、例えばＩＣＰを用いて第１仮想スキャンデータと第２仮想スキャンデータとの照合を行い、第１仮想スキャンデータから得られる各スキャン点の位置が、第２仮想スキャンデータから得られる各スキャン点の位置に近づくように、第２相対位置ベクトルｖ２を求める。
【０１６８】
この第３実施形態では、多重ループをなす各部分軌跡において、累積走行距離を用いて仮想スキャンデータを抽出しているため、再訪点ｒｓが仮想スキャンデータを作る部分軌跡のほぼ中心にできる。つまり、再訪点ｒｓの仮想スキャンデータは、Ｐｓ−αからＰｓ＋αの範囲のスキャンデータを用いて作成することができる。
【０１６９】
なお、ＳＬＡＭの最中に部分地図を定期的に保存しておき、それを仮想スキャンデータにするようにしても良い。
【０１７０】
以上のような態様によっても、第１実施形態や第２実施形態と同様に、移動体１０００の現在位置ｒｎに対して本質的な再訪点ｒｓを特定することができるため、ループ閉じ込みの処理を無駄なく行え、処理負荷の軽減を図ることが可能となる。
【０１７１】
＝＝第４実施形態＝＝
第１実施形態から第３実施形態では、スキャンデータ取得部１１０は、距離センサ２００から取得した各スキャン点までの距離及び方位の計測結果をそのままスキャンデータＰｔとして経路情報記憶部１２０に記憶したが、第４実施形態に係るスキャンデータ取得部１１０は、距離センサ２００から各スキャン点までの距離及び方位の計測結果を取得した際に、隣り合うスキャン点の間隔が全体として平準化されるように、各計測結果を補正する。
【０１７２】
例えば距離センサ２００が回転型である場合は、スキャン点までの計測距離が遠い場合と近い場合とを比べると、距離が遠いスキャン点であるほど隣り合うスキャン点の間隔が離れるが、本実施形態では、例えば計測距離がより近いスキャン点ほどより多くのスキャン点を間引くように補正することで、スキャンデータＰｔ全体として隣り合うスキャン点の間隔を平準化するようにする。
【０１７３】
本実施形態ではスキャン点の平準化を以下のように行っている。なお、スキャン点を平準化して得られる点をマップ点と称する。
【０１７４】
スキャンデータ取得部１１０は、マップ点をその近傍内にある複数のスキャン点から生成する。スキャンデータ取得部１１０は、まず、あるスキャン点を中心にして、そのスキャン点から所定距離ｄ以内にあるスキャン点を集める。
【０１７５】
次にスキャンデータ取得部１１０は、そのスキャン点集合の重心を中心にして再び所定距離ｄ以内のスキャン点を集め、重心が収束するまで同じ処理を繰り返す。そして重心が収束した点をマップ点とする。このため、本実施形態に係るスキャンデータＰｔを構成するマップ点は、３６０よりも少なくなる。
【０１７６】
また、本実施形態に係るスキャンデータ取得部１１０は、マップ点を構成するスキャン点集合を主成分分析して、その最小固有ベクトルをマップ点の法線方向とする。そしてこの法線を用いて、第１実施形態において説明した各スキャン点の特徴を表す局所記述子を定義するようにしても良い。
【０１７７】
また本実施形態では、スキャンデータ取得部１１０は、各マップ点に、そのマップ点の生成に用いたスキャン点を得たときの移動体１０００の累積走行距離Ｌｔを記録しておく。
【０１７８】
また地図作成部１８０が各タイミングｔのスキャンデータＰｔを共通座標の地図に追加していく際、Ｐｔ内のスキャン点の共通座標系での位置の所定距離ｄ内に累積移動距離の差が閾値Ｄ以内のマップ点があれば、そのうちの最近傍のマップ点にそのスキャン点を統合する。このような処理により、地図を構成するマップ点の密度をほぼ一定に保つことが可能となる。累積移動距離によってマップ点を分離するのは、第３実施形態で述べたように、多重ループをなす各部分軌跡の仮想スキャンデータを抽出する際に、マップ点を累積移動距離で区別できるようにするためである。
【０１７９】
なお、本実施形態に係るスキャンデータ取得部１１０は、マップ点を生成する際に、スキャン点集合の大きさが閾値に満たない場合は、マップ点を生成せずにそのスキャン点を削除するようにしても良い。これにより、スキャン点における小さなノイズを除去することができる。
【０１８０】
このようにマップ点を用いてスキャンデータＰｔを生成するためには、近傍内にあるスキャン点を頻繁に検索する必要があるため、高速な最近傍探索が必要であるが、本実施形態に係る移動軌跡算定システム１００は、FLANNなどの最近傍探索ライブラリを用いている。
【０１８１】
なお、図１０及び図１１に示した各スキャン点は、このようにして平準化が行われている。本実施形態に係る移動軌跡算定システム１００のような態様によって、複数のスキャンデータを照合する際に、対応づける点の密度にムラが少なくなり、計測距離の影響を受けにくくできるため、精度良く照合を行うことが可能となる。また、マップ点はスキャン点よりも個数が少なくなるため、メモリ効率も向上する。
【０１８２】
＝＝第５実施形態＝＝
第５実施形態に係る移動軌跡算定システム１００は、第２実施形態から第４実施形態でそれぞれ説明した構成をすべて含む形態である。
【０１８３】
そして本実施形態に係る移動軌跡算定システム１００を用いて実験を行った結果を、図１２〜図１４に示す。なお本実験では、１０フレームおきにキーフレームをとり、キーフレーム単位でループ閉じ込みを行った。
【０１８４】
図１２は、本実施形態に係る移動軌跡算定システム１００により作成された外界の地図を示す。図１３は移動体１０００の移動軌跡であり、主な地点でのキーフレーム番号を付加している。図１３の移動軌跡は、移動体１０００の前方をｘ軸、横方向をｙ軸とした座標系で表した位置の列である。図１４は、各キーフレームで異なる再訪点が何個検出されたかを示すグラフである。横軸がキーフレーム番号、縦軸が再訪点数である。
【０１８５】
図１４を見ると、キーフレーム１から進むにつれて再訪点が増えていく様子がわかる。図１３の拡大部分Ａには。軌跡が５本ある。このため、再訪点は４個得られるはずであるが、図１４のグラフのキーフレーム８１７付近の区間では、再訪点数は４個であり、すべて検出できていることがわかる。
【０１８６】
同様に、図１３の拡大部分Ｂには、軌跡が６本あり、再訪点は５個得られるはずである。図１４のグラフのキーフレーム番号８３４付近の区間では、再訪点数は４〜５個であり、ほぼすべての再訪点が得られていることが分かる。このように、図１２から図１４の結果から、本実施形態に係る移動軌跡算定システム１００によって、多くの再訪点が正しく検出できていることが確かめられる。
【０１８７】
以上、第１〜第５に係る実施形態の移動軌跡算定システム１００について説明したが、いずれの移動軌跡算定システム１００においても、移動体１０００の再訪点ｒｓをより効果的に特定し、移動体１０００の移動軌跡を算定する際の処理負荷を軽減することが可能となる。
【０１８８】
また本実施形態に係る移動軌跡算定システム１００は、ＩＣＰや局所記述子において法線を使うため、壁などが多く形状が安定している屋内環境に適している。
【０１８９】
さらに、第４実施形態及び第５実施形態に係る移動軌跡算定システム１００は、スキャン点を直接使うのではなく、その集合であるマップ点の法線を主成分分析で得ているため、形状に多少の凹凸があっても安定して処理を行うことができる。
【０１９０】
なお上述した実施の形態は本発明の理解を容易にするためのものであり、本発明を限定して解釈するためのものではない。本発明はその趣旨を逸脱することなく変更、改良され得るとともに、本発明にはその等価物も含まれる。
【０１９１】
例えば、移動体１０００は、自らの判断で自らの運動を自律的に制御することが可能な装置の他、人間や他の制御装置からの操作に従って移動する装置でも良い。さらには人間が２Ｄレーザスキャナ（距離センサ２００）及び移動軌跡算定システム１００を携帯して移動することで、人間の移動軌跡を推定する形態であってもよい。
【０１９２】
さらに移動軌跡算定システム１０００は、第２実施形態から第４実施形態に示した構成を任意の組み合わせで備える構成であっても良い。
【符号の説明】
【０１９３】
１００  移動軌跡算定システム
１１０  スキャンデータ取得部
１２０  経路情報記憶部
１３０  移動ベクトル算出部
１４０  位置情報算出部
１５０  再訪点検出部
１６０  累積移動距離算出部
１７０  ループ閉じ込み処理部
１７１  第１相対位置ベクトル算出部
１７２  第２相対位置ベクトル算出部
１７３  位置情報修正部
１８０  地図作成部
１９０  地図情報記憶部
２００  距離センサ
４００  運動制御部
５００  アクチュエータ
１０００  移動体
【図１】
2018120460.tif 000008
【図２】
2018120460.tif 000009
【図３】
2018120460.tif 000010
【図４】
2018120460.tif 000011
【図５】
2018120460.tif 000012
【図６】
2018120460.tif 000013
【図７】
2018120460.tif 000014
【図８】
2018120460.tif 000015
【図９】
2018120460.tif 000016
【図１０】
2018120460.tif 000017
【図１１】
2018120460.tif 000018
【図１２】
2018120460.tif 000019
【図１３】
2018120460.tif 000020
【図１４】
2018120460.tif 000021