(57)【特許請求の範囲】
【請求項１】
ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する方法であって、
前記センサから出力された点群から特徴部をフレーム毎に抽出し、
所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、
１周目の所定のフレームから当該フレームに対応する２周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
前記１周目の所定のフレームに含まれる前記特徴部の位置と当該フレームに対応する前記２周目のフレームに含まれる前記特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
前記２周目の所定のフレームに関する前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び前記蓄積回転行例と前記直接回転行列の差を示す推定誤差行列を前記２周目の全てのフレームについてそれぞれ算出し、
所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を前記２周目の全てのフレームについてそれぞれ算出し、
前記２周目の最後のフレームの推定誤差ベクトルに、前記２周目の全てのフレームについての前記推定誤差変化量の総和に対する前記２周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記２周目の全てのフレームについてそれぞれ求め、
前記２周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した第１の補正並進ベクトルを前記２周目の全てのフレーム間についてそれぞれ算出し、
前記第１の補正並進ベクトル及び前記回転行列を使用して前記２周目の軌跡を推定する
ことを含むことを特徴とする軌跡推定方法。
【請求項２】
前記特徴部は、平面であり、
前記特徴部を抽出することは、
前記センサから出力された点群をフレーム毎に取得し、
前記取得された点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、
前記平面点群により形成される前記平面を抽出することを含む、請求項１に記載の軌跡推定方法。
【請求項３】
前記２周目の各フレームにおける推定誤差変化量を、当該フレームに対応付けられた前記１周目の各フレームに対応付け、
前記１周目の最後のフレームの推定誤差ベクトルに、前記１周目の全てのフレームに対応付けられた前記推定誤差変化量の総和に対する前記１周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記１周目の全てのフレームについてそれぞれ求め、
前記１周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した第２の補正並進ベクトルを前記１周目の全てのフレーム間についてそれぞれ算出し、
前記第２の補正並進ベクトル及び前記回転行列を使用して前記１周目の軌跡を推定することを更に含む、請求項１又は２に記載の軌跡推定方法。
【請求項４】
ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する方法であって、
前記センサから出力された点群から特徴部をフレーム毎に抽出し、
所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、
１周目の所定のフレームから当該フレームに対応する２周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
前記１周目の所定のフレームに含まれる前記特徴部の位置と当該フレームに対応する前記２周目のフレームに含まれる前記特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
前記２周目の所定のフレームに関する前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び前記蓄積回転行例と前記直接回転行列の差を示す推定誤差行列を前記２周目の全てのフレームについてそれぞれ算出し、
所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を前記２周目の全てのフレームについてそれぞれ算出し、
前記２周目の各フレームにおける推定誤差変化量を、当該フレームに対応付けられた前記１周目の各フレームに対応付け、
前記１周目の最後のフレームの推定誤差ベクトルに、前記１周目の全てのフレームに対応付けられた前記推定誤差変化量の総和に対する前記１周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記１周目の全てのフレームについてそれぞれ求め、
前記１周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した補正並進ベクトルを前記１周目の全てのフレーム間についてそれぞれ算出し、
前記補正並進ベクトル及び前記回転行列を使用して前記１周目の軌跡を推定する
ことを含むことを特徴とする軌跡推定方法。
【請求項５】
ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する軌跡推定装置であって、
前記センサから出力された点群から特徴部をフレーム毎に抽出する抽出部と、
所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出する算出部と、
１周目の各フレームから当該フレームに対応する２周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記２周目のフレーム毎に算出し、
前記１周目の各フレームに含まれる前記特徴部の位置と当該フレームに対応する前記２周目のフレームに含まれる前記特徴部の位置とから、前記１周目の各フレームに含まれる前記特徴部と前記２周目の対応するフレームに含まれる前記特徴部との間の位置関係を示す直接並進ベクトル及び直接回転行列を前記２周目のフレーム毎に算出し、
前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び推定誤差行列を前記２周目のフレーム毎に算出し、
所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとの間の距離を示す推定誤差変化量を前記２周目のフレーム毎に算出し、
前記２周目の最後のフレームの推定誤差ベクトルに、各フレームの推定誤差変化量に応じた比率を乗じて算出された補正ベクトルに基づいて、前記２周目の前記並進ベクトルを補正した補正並進ベクトルを算出し、
前記補正並進ベクトル及び前記回転行列を使用して前記２周目の軌跡を推定する推定部と
を備えることを特徴とする軌跡推定装置。
【請求項６】
ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する軌跡推定プログラムであって、
前記センサから出力された点群から特徴部をフレーム毎に抽出し、
所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、
１周目の所定のフレームから当該フレームに対応する２周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
前記１周目の所定のフレームに含まれる前記特徴部の位置と当該フレームに対応する前記２周目のフレームに含まれる前記特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
前記２周目の所定のフレームに関する前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び前記蓄積回転行例と前記直接回転行列の差を示す推定誤差行列を前記２周目の全てのフレームについてそれぞれ算出し、
所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を前記２周目の全てのフレームについてそれぞれ算出し、
前記２周目の最後のフレームの推定誤差ベクトルに、前記２周目の全てのフレームについての前記推定誤差変化量の総和に対する前記２周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記２周目の全てのフレームについてそれぞれ求め、
前記２周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した補正並進ベクトルを前記２周目の全てのフレーム間についてそれぞれ算出し、
前記補正並進ベクトル及び前記回転行列を使用して前記２周目の軌跡を推定する
ことをコンピュータに実行させることを特徴とする軌跡推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、軌跡推定方法、軌跡推定装置及び軌跡推定プログラムに関する。
【背景技術】
【０００２】
軌跡推定方法として、レーザセンサ又は撮像装置等のセンサを用いて周囲の地物の位置を測定し、測定された周囲の地物の位置に基づいて自己位置を推定する軌跡推定方法が知られている。このような軌跡推定方法では、測定中における各センサの位置及び姿勢が推定され、推定された位置及び姿勢と周囲の地物の位置とに基づいて自己位置が推定される。
【０００３】
例えば、非特許文献１には、移動ロボットに搭載されたカメラによって撮影された複数の画像に含まれる複数の特徴点座標に基づいてカメラの位置姿勢が推定され、推定された位置姿勢から移動ロボットの自己位置が推定される軌跡推定方法が記載されている。この軌跡推定方法では、撮影された最初の画像から特徴点が抽出され、特徴点追跡手法によって移動後の画像に含まれる特徴点が追跡され、各画像における特徴点に基づいて並進ベクトル及び回転行列が算出される。そして、算出された複数の並進ベクトル及び回転行列に基づいて当該カメラの移動軌跡が推定される。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】川西亮輔, 外２名, “全方位カメラを用いた周囲環境測定のための特徴点フローモデルによる位置姿勢推定”, 映像情報メディア学会技術報告, ２００９年２月, Vol.33, No.11, pp.65-68
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来の軌跡推定方法においては、複数の並進ベクトル及び回転行列のそれぞれにおいて誤差が生じているため、カメラの移動軌跡は、各並進ベクトル及び回転行列の誤差の蓄積を含むものとなっていた。例えば、カメラを搭載した移動ロボットが、出発した地点に再び戻ってくるような場合、推定された移動軌跡上の出発地点と推定された移動軌跡上の到着地点との間に大きなずれが生じるという問題があった。
【０００６】
本発明は、このような課題を解決すべくなされたものであり、出発地点に再び戻る移動を行う移動体の自己位置推定の精度を向上させることを可能とする軌跡推定方法、軌跡推定装置及び軌跡推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明に係る軌跡推定方法は、ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する方法であって、センサから出力された点群から特徴部をフレーム毎に抽出し、所定のフレームに含まれる特徴部の位置と、次のフレームに含まれる特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、１周目の所定のフレームから当該フレームに対応する２周目のフレームまでの並進ベクトル及び回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を２周目の全てのフレームについてそれぞれ算出し、１周目の所定のフレームに含まれる特徴部の位置と当該フレームに対応する２周目のフレームに含まれる特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を２周目の全てのフレームについてそれぞれ算出し、２周目の所定のフレームに関する蓄積並進ベクトル及び蓄積回転行列と直接並進ベクトル及び直接回転行列とから、蓄積並進ベクトルと直接並進ベクトルとの差を示す推定誤差ベクトル及び蓄積回転行例と直接回転行列の差を示す推定誤差行列を２周目の全てのフレームについてそれぞれ算出し、所定のフレームにおける推定誤差ベクトルと次のフレームにおける推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を２周目の全てのフレームについてそれぞれ算出し、２周目の最後のフレームの推定誤差ベクトルに、２周目の全てのフレームについての推定誤差変化量の総和に対する２周目の所定のフレームの推定誤差変化量の比率を乗じて算出された補正ベクトルを、２周目の全てのフレームについてそれぞれ求め、２周目の所定のフレーム間についての並進ベクトルを対応する補正ベクトルに基づいて補正した補正並進ベクトルを２周目の全てのフレーム間についてそれぞれ算出し、補正並進ベクトル及び回転行列を使用して２周目の軌跡を推定することを含む。
【０００８】
また、本発明に係る軌跡推定方法において、特徴部は、平面であり、特徴部を抽出することは、センサから出力された点群をフレーム毎に取得し、取得された点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、平面点群により形成される平面を抽出することを含むことが好ましい。
【０００９】
また、本発明に係る軌跡推定方法において、２周目の各フレームにおける推定誤差変化量を、当該フレームに対応付けられた１周目の各フレームに対応付け、１周目の最後のフレームの推定誤差ベクトルに、１周目の全てのフレームに対応付けられた推定誤差変化量の総和に対する１周目の所定のフレームの推定誤差変化量の比率を乗じて算出された補正ベクトルを、１周目の全てのフレームについてそれぞれ求め、１周目の所定のフレーム間についての並進ベクトルを対応する補正ベクトルに基づいて補正した第２の補正並進ベクトルを１周目の全てのフレーム間についてそれぞれ算出し、第２の補正並進ベクトル及び回転行列を使用して１周目の軌跡を推定することを更に含むことが好ましい。
【００１０】
本発明に係る他の軌跡推定方法は、ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する方法であって、センサから出力された点群から特徴部をフレーム毎に抽出し、所定のフレームに含まれる特徴部の位置と、次のフレームに含まれる特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、１周目の所定のフレームから当該フレームに対応する２周目のフレームまでの並進ベクトル及び回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を２周目の全てのフレームについてそれぞれ算出し、１周目の所定のフレームに含まれる特徴部の位置と当該フレームに対応する２周目のフレームに含まれる特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を２周目の全てのフレームについてそれぞれ算出し、２周目の所定のフレームに関する蓄積並進ベクトル及び蓄積回転行列と直接並進ベクトル及び直接回転行列とから、蓄積並進ベクトルと直接並進ベクトルとの差を示す推定誤差ベクトル及び蓄積回転行例と直接回転行列の差を示す推定誤差行列を２周目の全てのフレームについてそれぞれ算出し、所定のフレームにおける推定誤差ベクトルと次のフレームにおける推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を２周目の全てのフレームについてそれぞれ算出し、２周目の各フレームにおける推定誤差変化量を、当該フレームに対応付けられた１周目の各フレームに対応付け、１周目の最後のフレームの推定誤差ベクトルに、１周目の全てのフレームに対応付けられた推定誤差変化量の総和に対する１周目の所定のフレームの推定誤差変化量の比率を乗じて算出された補正ベクトルを、１周目の全てのフレームについてそれぞれ求め、１周目の所定のフレーム間についての並進ベクトルを対応する補正ベクトルに基づいて補正した第２の補正並進ベクトルを１周目の全てのフレーム間についてそれぞれ算出し、第２の補正並進ベクトル及び回転行列を使用して１周目の軌跡を推定することを含む。
【００１１】
本発明に係る軌跡推定装置は、ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する軌跡推定装置であって、センサから出力された点群から特徴部をフレーム毎に抽出する抽出部と、所定のフレームに含まれる特徴部の位置と、次のフレームに含まれる特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出する算出部と、１周目の各フレームから当該フレームに対応する２周目のフレームまでの並進ベクトル及び回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を２周目のフレーム毎に算出し、１周目の各フレームに含まれる特徴部の位置と当該フレームに対応する２周目のフレームに含まれる特徴部の位置とから、１周目の各フレームに含まれる特徴部と２周目の対応するフレームに含まれる特徴部との間の位置関係を示す直接並進ベクトル及び直接回転行列を２周目のフレーム毎に算出し、蓄積並進ベクトル及び蓄積回転行列と直接並進ベクトル及び直接回転行列とから、蓄積並進ベクトルと直接並進ベクトルとの差を示す推定誤差ベクトル及び推定誤差行列を２周目のフレーム毎に算出し、所定のフレームにおける推定誤差ベクトルと次のフレームにおける推定誤差ベクトルとの間の距離を示す推定誤差変化量を２周目のフレーム毎に算出し、２周目の最後のフレームの推定誤差ベクトルに、各フレームの推定誤差変化量に応じた比率を乗じて算出された補正ベクトルに基づいて、２周目の並進ベクトルを補正した第２補正並進ベクトルを算出し、第２補正並進ベクトル及び回転行列を使用して２周目の軌跡を推定する推定部とを備える。
【００１２】
本発明に係る軌跡推定プログラムは、ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する軌跡推定プログラムであって、
センサから出力された点群から特徴部をフレーム毎に抽出し、所定のフレームに含まれる特徴部の位置と、次のフレームに含まれる特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、１周目の所定のフレームから当該フレームに対応する２周目のフレームまでの並進ベクトル及び回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を２周目の全てのフレームについてそれぞれ算出し、１周目の所定のフレームに含まれる特徴部の位置と当該フレームに対応する２周目のフレームに含まれる特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を２周目の全てのフレームについてそれぞれ算出し、２周目の所定のフレームに関する蓄積並進ベクトル及び蓄積回転行列と直接並進ベクトル及び直接回転行列とから、蓄積並進ベクトルと直接並進ベクトルとの差を示す推定誤差ベクトル及び蓄積回転行例と直接回転行列の差を示す推定誤差行列を２周目の全てのフレームについてそれぞれ算出し、所定のフレームにおける推定誤差ベクトルと次のフレームにおける推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を２周目の全てのフレームについてそれぞれ算出し、２周目の最後のフレームの推定誤差ベクトルに、２周目の全てのフレームについての推定誤差変化量の総和に対する２周目の所定のフレームの推定誤差変化量の比率を乗じて算出された補正ベクトルを、２周目の全てのフレームについてそれぞれ求め、２周目の所定のフレーム間についての並進ベクトルを対応する補正ベクトルに基づいて補正した補正並進ベクトルを２周目の全てのフレーム間についてそれぞれ算出し、補正並進ベクトル及び回転行列を使用して２周目の軌跡を推定する
ことをコンピュータに実行させる。
【発明の効果】
【００１３】
本発明に係る軌跡推定方法、軌跡推定装置及び軌跡推定プログラムでは、出発地点に再び戻る移動を行う移動体の自己位置推定の精度を向上させることが可能となる。
【図面の簡単な説明】
【００１４】
【図１】位置推定システムの概要を説明するための模式図である。
【図２】位置推定システム１の概略構成の一例を示す図である。
【図３】位置推定装置２の概略構成の一例を示す図である。
【図４】点群テーブルのデータ構造の一例を示す図である。
【図５】位置推定処理の動作フローの一例を示す図である。
【図６】１フレームの点群をマッピングした３次元空間の一例を示す模式図である。
【図７】平面抽出処理の動作フローの一例を示す図である。
【図８】特定の１つのレイヤーにおける各セグメントをマッピングした２次元空間の一例を示す模式図である。
【図９】特定の１つのレイヤーにおける各直線状のセグメントをマッピングした２次元空間の一例を示す模式図である。
【図１０】特定の１つのフレームにおける各直線状のセグメントをマッピングした３次元空間の一例を示す模式図である。
【図１１】直線状のセグメントにおける法線ベクトルの一例を説明するための模式図である。
【図１２】特定の１つのフレームにおける各直線状のセグメントの法線ベクトルをマッピングした３次元空間の一例を示す模式図である。
【図１３】特定の１つのフレームにおける平面点群をマッピングした３次元空間の一例を示す模式図である。
【図１４】並進ベクトル・回転行列算出処理の動作フローの一例を示す図である。
【図１５】隣り合う２つのフレームの平面点群のマッチングを説明するための模式図である。
【図１６】（ａ）平面点群の平面への射影を説明するための模式図である。（ｂ）マッチング候補の平面点群の平面への射影を説明するための模式図である。
【図１７】Ｃｏｎｖｅｘ  ｈｕｌｌを説明するための模式図である。
【図１８】最適化処理の動作フローの一例を示す図である。
【図１９】（ａ）平面点群の実際の平面及びマッチング平面を説明するための模式図である。（ｂ）平面点群の最適化を説明するための模式図である。
【図２０】推定処理の動作フローの一例を示す図である。
【図２１】（ａ）蓄積並進ベクトル及び蓄積回転行列及び直接並進ベクトル及び直接回転行列の一例を説明するための模式図である。（ｂ）推定誤差ベクトルと推定誤差変化量の一例を説明するための模式図である。
【発明を実施するための形態】
【００１５】
以下、図面を参照しつつ、本発明の様々な実施形態について説明する。ただし、本発明の技術的範囲はそれらの実施形態に限定されず、特許請求の範囲に記載された発明とその均等物に及ぶ点に留意されたい。
【００１６】
１．本実施形態の概略
図１は、本実施形態の位置推定システムの概要を説明するための模式図である。本実施形態の位置推定システムでは、複数の地物の位置を所定の測定時間毎に測定し、測定した地物の複数のセンサ座標系の位置を示す点群ＰＴを出力するセンサを搭載した移動体の軌跡が推定される。位置推定装置は、コンピュータであるが、本発明が適用可能であればどのような形態でもよい。なお、位置推定装置は、軌跡推定装置の一例である。例えば、位置推定装置は、パーソナルコンピュータ、サーバ、ノートＰＣ、多機能携帯電話機（所謂「スマートフォン」）、携帯情報端末（Personal Digital Assistant, PDA）、タブレット端末、タブレットＰＣ、携帯型ゲーム機等でもよい。移動体は、自動車、原動機付自転車、又は軽車両（自転車等）等である。
【００１７】
センサは、周囲にある地物等の物体までの距離及び角度を連続して測定し、測定された距離及び角度に基づいてセンサ座標系の３次元位置を出力する。センサは、ＬｉＤＡＲ（Light Detection and Ranging）又はレーザレンジファインダ等のレーザ測距装置である。レーザ測距装置のレーザ照射部分は、パルス状に発光するレーザを照射して散乱光を測定し、照射から検出までの時間に基づいて反射した位置を測定する。レーザ照射部分は、垂直視野角約４０度の範囲を測定できるように３２個のレーザ送受信センサを備え、接地面と平行面上に３６０度回転する。このレーザ測距装置では、垂直視野角約４０度の範囲内において３２本のレーザが同時に照射され、レーザ照射部分の回転により、周囲３６０度の地物の位置が測定される。なお、センサとして、レーダ測距装置、撮像装置等が用いられてもよい。センサとして撮像装置が用いられる場合、例えば、２地点において、撮像装置がセンサの周囲にある同一物体を撮像することにより、三角測量によって物体の位置が測定される。
【００１８】
図１（１）に示すとおり、位置推定装置が位置推定処理を実行する前に、センサによる測定が行われる。まず、センサが移動体に搭載され、移動体が移動を開始する。移動体は、少なくともループ状の経路を２周移動する。すなわち、移動体は、予め定められた道路上の設定地点から出発し、所定のループ状の経路上を移動して設定地点に戻り、再び同じ所定のループ状の経路上を移動して設定地点に戻る。以下、移動体が２周移動した所定のループ状の経路において、最初に位置した設定地点を１周目の始点と称し、２度目に位置した設定地点を１周目の終点又は２周目の始点と称し、３度目に位置した設定地点を２周目の終点と称する。そして、移動体が移動を続けながら、センサは、所定の測定時間毎（例えば、０．１秒毎）にレーザ照射部分を１回転させながら、移動体周囲の地物までの距離及び角度を測定し、地物のセンサ座標系の３次元位置を複数出力する。以上により、センサによる測定が終了する。
【００１９】
次に、位置推定装置は、センサから出力された点群のうち１周目の点群を所定数のフレーム毎に取得するとともに、２周目の点群を所定数のフレーム毎に取得する。位置推定装置は、出力された複数の地物の３次元位置を示す点群ＰＴを、レーザ照射部分が１回転するたびに１フレームずつ順次取得する。以下、センサのレーザ照射部分が１回転されることによって取得された点群を１フレームの点群と称する。なお、点群ＰＴに含まれる各点は、各フレームにおいてセンサによって測定された測距値及び角度に基づく複数のセンサ座標系の３次元位置（ｘ、ｙ、ｚ）で表される。図１（２）に示すとおり、特定のフレームにおいて点群ＰＴによって示される位置は、センサ周囲の地物の表面上の位置である。
【００２０】
次に、位置推定装置は、取得した点群ＰＴから、特徴部をフレーム毎に抽出する。特徴部は、例えば、平面を形成する点群である。以下、平面を形成する点群を平面点群と称する。なお、特徴部は、平面点群に限定されず、建築物の角等の地物の特徴点等でもよい。
【００２１】
次に、位置推定装置は、所定のフレームに含まれる特徴部の位置と、次のフレームに含まれる特徴部の位置との関係からフレーム間の並進ベクトルＴ及び回転行列Ｒを算出する。まず、位置推定装置は、複数のフレーム毎に、各フレームにおける特徴部と次のフレームにおける特徴部とのマッチングを行う。そして、図１（２）に示すとおり、位置推定装置は、マッチングが成立したフレーム間の並進ベクトルＴ及び回転行列Ｒを算出する。なお、次のフレームは、所定のフレームの次に取得されたフレームである。
【００２２】
次に、位置推定装置は、１周目の所定のフレームから当該フレームに対応する２周目のフレームまでの並進ベクトル及び回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を２周目の全てのフレームについてそれぞれ算出する。図１（３）に示すとおり、蓄積並進ベクトル及び蓄積回転行列は、１周目の各フレームを取得した時のセンサ座標系を基準とした並進ベクトル及び回転行列で表される。また、蓄積並進ベクトル及び蓄積回転行列は、１周目の各フレームを取得した位置に対する、２周目の対応するフレームを取得したと推定される位置の相対位置を示す。フレーム間の並進ベクトル及び回転行列は誤差を含むため、蓄積並進ベクトル及び蓄積回転行列は、各フレーム間の並進ベクトル及び回転行列の誤差の蓄積を含むものである。なお、蓄積並進ベクトル及び蓄積回転行列は、対応する２周目のフレーム毎に対応付けられる。
【００２３】
次に、位置推定装置は、１周目の所定のフレームに含まれる特徴部の位置と当該フレームに対応する２周目のフレームに含まれる特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を２周目の全てのフレームについてそれぞれ算出する。図１（３）に示すとおり、直接並進ベクトル及び直接回転行列は、１周目の各フレームを取得した位置に対する、２周目の対応するフレームを取得したと推定される位置の相対位置を示す。先に算出された蓄積並進ベクトル及び蓄積回転行列は複数のフレーム間の並進ベクトル及び回転行列の誤差が蓄積されるのに対し、ここで算出される直接並進ベクトル及び直接回転行列は誤差が蓄積されるおそれがない。したがって、直接並進ベクトル及び直接回転行列の誤差は、蓄積並進ベクトル及び蓄積回転行列の誤差よりも小さいと推定される。なお、直接並進ベクトル及び直接回転行列は、対応する２周目のフレーム毎に対応付けられる。
【００２４】
次に、位置推定装置は、２周目の所定のフレームに関する蓄積並進ベクトル及び蓄積回転行列と直接並進ベクトル及び直接回転行列とから、蓄積並進ベクトルと直接並進ベクトルとの差を示す推定誤差ベクトル及び蓄積回転行例と直接回転行列の差を示す推定誤差行列を２周目の全てのフレームについてそれぞれ算出する。上述したとおり、直接並進ベクトル及び直接回転行列は、１周目の各フレームに含まれる特徴部の位置と２周目の対応するフレームに含まれる特徴部の位置とから直接算出された並進ベクトル及び回転行列である。一方、蓄積並進ベクトル及び蓄積回転行列は、１周目の各フレームから２周目の対応するフレームまでの間において算出された並進ベクトル及び回転行列の蓄積である。したがって、図１（３）に示す蓄積並進ベクトル及び蓄積回転行列と直接並進ベクトル及び直接回転行列との差である推定誤差ベクトル及び推定誤差行列は、１周目の各フレームから２周目の対応するフレームまでの間において算出された並進ベクトル及び回転行列のそれぞれの誤差の蓄積である。
【００２５】
次に、位置推定装置は、所定のフレームにおける推定誤差ベクトルと次のフレームにおける推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を２周目の全てのフレームについてそれぞれ算出する。推定誤差ベクトルは、１周目の各フレームから２周目の対応するフレームまでの間において算出された並進ベクトルのそれぞれの誤差の蓄積である。したがって、図１（４）に示すとおり、各フレーム間の並進ベクトルの誤差は、当該フレームの推定誤差ベクトルと前の推定誤差ベクトルとの間の距離に対応する。
【００２６】
次に、位置推定装置は、２周目の最後のフレームの推定誤差ベクトルに、２周目の全てのフレームについての推定誤差変化量の総和に対する２周目の所定のフレームの推定誤差変化量の比率を乗じて算出された補正ベクトルを、２周目の全てのフレームについてそれぞれ求め、２周目の所定のフレーム間についての並進ベクトルを対応する補正ベクトルに基づいて補正した補正並進ベクトルを２周目の全てのフレーム間についてそれぞれ算出する。２周目の最後のフレームにおける推定誤差ベクトルは、１周目の終点すなわち２周目の始点から２周目の終点までの並進ベクトルのそれぞれの誤差の蓄積である。各フレーム間の並進ベクトルの誤差の比率は、各フレームの推定誤差変化量の比率に近似すると推定される。したがって、２周目の最後のフレームにおける推定誤差ベクトルに、２周目の各フレームの推定誤差変化量の比率を乗ずることにより算出された補正ベクトルが、各フレーム間の並進ベクトルを補正するベクトルとなる。
【００２７】
そして、位置推定装置は、図１（５）に示すとおり、補正並進ベクトル及び回転行列を使用して２周目の軌跡を推定する。
【００２８】
これにより、本実施形態の軌跡推定装置は、誤差が蓄積される並進ベクトルを、誤差が蓄積されるおそれが少ない直接並進ベクトルに近づけることができるため、並進ベクトル及び回転行列を蓄積することによって推定される移動体の移動軌跡を、並進ベクトルのそれぞれの誤差を排除した移動体の移動軌跡に補正することができる。また、本実施形態の軌跡推定装置は、出発地点に再び戻る移動を行う移動体の自己位置推定の精度を向上させることが可能となる。
【００２９】
２．位置推定システム１の構成
図２は、位置推定システム１の概略構成の一例を示す図である。
【００３０】
位置推定システム１は、位置推定装置２、センサ３、センサ３が搭載された移動体４を有する。センサ３は、測定された３次元位置の情報を出力するための出力インターフェース（図示せず）を備える。位置推定装置２は、センサ３と有線又は無線接続され、センサ３の出力インターフェース経由でセンサ３から出力された点群の情報を取得する。
【００３１】
なお、センサ３は、出力インターフェースを介して、ＣＤ−ＲＯＭ（compact disk read only memory）、ＤＶＤ−ＲＯＭ（digital versatile disk read only memory）等のコンピュータ読み取り可能な可搬型記録媒体の記録装置と接続されてもよい。この場合、可搬型記録媒体の記録装置は、センサ３の出力インターフェースを介して入力された点群の置の情報を、可搬型記録媒体に記録する。そして、位置推定装置２は、点群の情報が記録された可搬型記録媒体を読み込むことにより、当該点群の情報を取得する。
【００３２】
２．１．位置推定装置２の構成
図３は、位置推定装置２の概略構成の一例を示す図である。また、図４は、記憶部２１が記憶する点群テーブルのデータ構造の一例を示す図である。
【００３３】
図３に示すように、位置推定装置２は、記憶部２１と、入力部２２と、出力部２３と、処理部２４とを備える。位置推定装置２は、センサから出力された点群を取得し、取得された点群に基づいてセンサを搭載した移動体の軌跡を推定する。
【００３４】
記憶部２１は、例えば、磁気テープ装置、磁気ディスク装置、又は光ディスク装置のうちの少なくとも一つを備える。記憶部２１は、処理部２４での処理に用いられるオペレーティングシステムプログラム、ドライバプログラム、アプリケーションプログラム、データ等を記憶する。例えば、記憶部２１は、アプリケーションプログラムとして、点群を取得する処理、取得された点群に基づく移動体の位置推定処理、及び位置推定処理の結果に係る表示データを作成する処理等を、処理部２４に実行させるための位置推定プログラム等を記憶する。位置推定プログラムは、例えばＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ等のコンピュータ読み取り可能な可搬型記録媒体から、公知のセットアッププログラム等を用いて記憶部２１にインストールされてもよい。
【００３５】
また、記憶部２１は、データとして、取得した点群を管理するための点群テーブル等を記憶する。さらに、記憶部２１は、所定の処理に係る一時的なデータを一時的に記憶してもよい。
【００３６】
図４は、点群テーブルのデータ構造の一例を示す図である。
【００３７】
点群テーブルには、垂直視野角約４０度の範囲内において３２本のレーザを同時に照射し、レーザ照射部分の回転により周囲３６０度の地物の位置を測定するセンサによる測定結果のデータが格納される。図４に示すように、点群テーブルには、レーザＩＤ毎に、フレーム番号と、レーザの照射によって測定された測距値及び各レーザに固有の垂直視野角に基づくセンサ座標系の３次元位置（ｘ、ｙ、ｚ）とが、測定時間に従って順次関連付けられて記憶される。なお、１回のレーザの照射によって測定された測距値及び垂直視野角に基づくセンサ座標系の３次元位置（ｘ、ｙ、ｚ）が、１レコードとして記憶され、各レコードは、測定された順番に、レコード１、レコード２、レコード３、・・・として記憶される。レーザＩＤは、３２本のレーザをそれぞれ識別するための識別情報である。フレーム番号は、フレームを識別するための番号であり、レーザ照射部分が１回転するたびに時系列順に付与される。
【００３８】
図３に戻り、入力部２２は、データの入力が可能であればどのようなデバイスでもよく、例えば、タッチパネル、キーボタン等である。操作者は、入力部２２を用いて、文字、数字、記号等を入力することができる。入力部２２は、操作者により操作されると、その操作に対応する信号を生成する。そして、生成された信号は、操作者の指示として、処理部２４に供給される。
【００３９】
出力部２３も、映像や画像等の表示が可能であればどのようなデバイスでもよく、例えば、液晶ディスプレイや有機ＥＬ（Electro−Luminescence）ディスプレイ等である。出力部２３は、処理部２４から供給された映像データに応じた映像や、画像データに応じた画像等を表示する。また、出力部２３は、紙などの表示媒体に、映像、画像又は文字等を印刷する出力装置であってもよい。
【００４０】
処理部２４は、一又は複数個のプロセッサ及びその周辺回路を有する。処理部２４は、位置推定装置２の全体的な動作を統括的に制御するものであり、例えば、ＣＰＵである。処理部２４は、記憶部２１に記憶されているプログラム（ドライバプログラム、オペレーティングシステムプログラム、アプリケーションプログラム等）に基づいて処理を実行する。また、処理部２４は、複数のプログラム（アプリケーションプログラム等）を並列に実行できる。
【００４１】
２．２．処理部２４の構成
処理部２４は、取得部２４１、抽出部２４２、平面特定部２４３、算出部２４４、最適化部２４５、及び推定部２４６を備える。これらの各部は、処理部２４が備えるプロセッサで実行されるプログラムにより実現される機能モジュールである。あるいは、これらの各部は、ファームウェアとして位置推定装置２に実装されてもよい。
【００４２】
図５は、位置推定処理の動作フローの一例を示す図である。この動作フローは、予め記憶部２１に記憶されているプログラムに基づいて、主に処理部２４により、位置推定装置２の各要素と協働して実行される。
【００４３】
まず、センサが、移動体が移動している間に回転されるレーザ照射部分によって測定された地物の位置を示す点群ＰＴを出力し、位置推定装置２の取得部２４１は、出力された点群ＰＴを１回転毎に１フレームずつ順次取得する（ステップＳ１０１）。取得部２４１は、取得した点群ＰＴを記憶部２１の点群テーブルに記憶する。
【００４４】
図６は、１フレームの点群をマッピングした３次元空間の一例を示す模式図である。図６に示すように、１フレームの点群は、３２本のレーザを同時に照射するセンサのレーザ照射部分の回転によって測定された、周囲３６０度の複数の地物のセンサ座標系の３次元位置である。以下、１フレームにおけるセンサ１本分の点群を１レイヤーの点群と称する。
【００４５】
図５に戻り、次に、位置推定装置２の抽出部２４２及び平面特定部２４３は、取得部２４１によって点群ＰＴが取得されると、平面を抽出するための平面抽出処理を実行する（ステップＳ１０２）。平面抽出処理の詳細については後述する。
【００４６】
次に、位置推定装置２の算出部２４４は、抽出部２４２及び平面特定部２４３によって平面抽出処理が行われると、並進ベクトル・回転行列算出処理を実行する（ステップＳ１０３）。並進ベクトル・回転行列算出処理の詳細については後述する。
【００４７】
次に、位置推定装置２の最適化部２４５は、算出部２４４によって並進ベクトル・回転行列算出処理が実行されると、最適化処理を実行する（ステップＳ１０４）。最適化処理の詳細については後述する。
【００４８】
そして、位置推定装置２の推定部２４６は、最適化部２４５によって最適化処理が実行されると、推定処理を実行し（ステップＳ１０５）、一連のステップを終了する。なお、位置推定処理の一連のステップが終了すると、操作者の指示に応じて、位置推定装置２の処理部２４は、出力部２３に、位置推定処理の結果（移動体の移動軌跡等）を表示する。
【００４９】
図７は、位置推定装置２の抽出部２４２及び平面特定部２４３による平面抽出処理の動作フローの一例を示す図である。図７に示す平面抽出処理は、図５のステップＳ１０２において実行される。
【００５０】
まず、抽出部２４２は、点群テーブルに記憶された複数のフレームの点群のうち、１つのフレームの点群を取得する（ステップＳ２０１）。以下のステップＳ２０２〜Ｓ２０７は、取得された１つのフレームの点群に対して実行される。
【００５１】
次に、抽出部２４２は、レイヤー毎に、取得した点群のうちの所定の点と次の点との間の距離を算出する（ステップＳ２０２）。すなわち、抽出部２４２は、レーザＩＤ毎に、所定のレコードの３次元位置（ｘ、ｙ、ｚ）と、当該レコードの次のレコードの３次元位置（ｘ、ｙ、ｚ）とを特定し、特定した両位置を端点とする線分の距離を算出する。以下、算出された距離を点間距離と称する。
【００５２】
次に、抽出部２４２は、各レイヤーの点群を点間距離に基づいてセグメントに分類する（ステップＳ２０３）。すなわち、抽出部２４２は、レーザＩＤ毎に、レコードの順番に従って各点間距離を累積加算した値が閾値を超えた場合、当該閾値を超える前までに累積加算した全ての点間距離における端点を示す各点を１つのセグメントとする。なお、以下、セグメントに含まれる全ての点を、セグメントに含まれる点群と称する場合がある。図８は、特定の１つのレイヤーにおける全てのセグメントをマッピングした２次元空間の一例を示す模式図である。
【００５３】
次に、抽出部２４２は、Ｓ２０３において分割したセグメントのうち、セグメントに含まれる点群の数が所定数以上であり且つセグメントに含まれる点群の形状が直線状であるセグメントを検出する（ステップＳ２０４）。ステップＳ２０４では、まず、抽出部２４２は、セグメントに含まれる点群の数が所定数以上であるか否か判断する。次に、抽出部２４２は、点群の数が所定数以上であるセグメントに含まれる全て点群に対して、主成分分析を行う。次に、抽出部２４２は、第１主成分の分散が第２主成分の分散及び第３主成分の分散よりも所定比率以上大きい場合，主成分分析を行った点群を含むセグメントを直線状のセグメントとして検出する。これにより、セグメントに含まれる点群によって形成される形状が、第１主成分を軸とする直線状であると判定される。
【００５４】
図９は、特定の１つのレイヤーにおける各直線状のセグメントをマッピングした２次元空間の一例を示す模式図であり、図１０は、特定の１つのフレームにおける各直線状のセグメントをマッピングした３次元空間の一例を示す模式図である。
【００５５】
次に、抽出部２４２は、Ｓ２０４において特定された直線状のセグメントの法線ベクトルを算出する（ステップＳ２０５）。ステップＳ２０５では、まず、抽出部２４２は、Ｓ２０４において特定された全ての直線状のセグメントを取得する。次に、抽出部２４２は、取得した全ての直線状のセグメントのそれぞれに対して、各直線状のセグメントを中心とした所定範囲内の他の直線状のセグメントを検出する。次に、抽出部２４２は、各直線状のセグメントと各直線状のセグメントに対して検出された他の直線状のセグメントとの内積のなす角が所定閾値以下の他の直線状のセグメントを近傍セグメントとして検出する。次に、抽出部２４２は、各直線状のセグメントと各直線状のセグメントに対して検出された近傍セグメントとに含まれる点群に対して主成分分析を行う。そして、抽出部２４２は、第１主成分の分散及び第２主成分の分散が第３主成分の分散よりも一定比率以上（例えば、４００倍以上）大きい場合、第１主成分及び第２主成分を軸とした平面に対する法線ベクトルを、各直線状のセグメントの法線ベクトルとして算出する。以下、図１１（ａ）及び（ｂ）を参照して、ステップＳ２０５の処理について具体的に説明する。
【００５６】
図１（ａ）は、直線状のセグメントにおける法線ベクトルの一例を説明するための模式図である。レイヤーＡ、Ｂ、Ｃ，Ｄ，及びＥ毎に、ステップＳ２０４において検出された直線状のセグメントが特定されている。例えば、セグメントＳ15は、レイヤーＡにおける直線状のセグメントである。また、セグメントＳ11、Ｓ12、Ｓ16は、レイヤーＢにおける直線状のセグメントである。また、セグメントＳ13、Ｓ14は、レイヤーＣにおける直線状のセグメントである。なお、図１１（ａ）及び（ｂ）において示される全てのセグメントは、直線状のセグメントである。図１１（ａ）に示すように、抽出部２４２は、セグメントＳ11を中心とした所定範囲内にある他のセグメントＳ12〜Ｓ16を検出する。次に、抽出部２４２は、セグメントＳ11とセグメントＳ12〜Ｓ16のそれぞれとの内積のなす角度が所定閾値（例えば、６０度）以下であるか否か判断する。図１１（ａ）では、セグメントＳ12〜Ｓ16の全てにおいて、セグメントＳ11との内積のなす角度が所定閾値以下であるので、抽出部２４２は、セグメントＳ11〜Ｓ16に含まれる点群に基づいて法線ベクトルを算出し、算出した法線ベクトルをＳ１１の法線ベクトルとして特定する。
【００５７】
図１１（ｂ）は、直線状のセグメントにおける法線ベクトルの他の一例を説明するための模式図である。レイヤーＡ、Ｂ、Ｃ，Ｄ，及びＥは、図１１（ａ）と同様である。セグメントＳ24、Ｓ25は、レイヤーＣにおける直線状のセグメントである。また、セグメントＳ21、Ｓ23、Ｓ26は、レイヤーＤにおける直線状のセグメントである。また、セグメントＳ22は、レイヤーＥにおける直線状のセグメントである。図１１（ｂ）に示すように、抽出部２４２は、セグメントＳ21を中心とした所定範囲内にある他のセグメントＳ22〜Ｓ26を検出する。次に、抽出部２４２は、セグメントＳ21とセグメントＳ22〜Ｓ26のそれぞれとの内積のなす角度が所定閾値（例えば、６０度）以下であるか否か判断する。図１１（ｂ）では、セグメントＳ22〜Ｓ25のそれぞれとセグメントＳ21との内積のなす角度は、それぞれ所定閾値以下であり、セグメントＳ26とセグメントＳ21との内積のなす角度は所定閾値よりも大きい。したがって、抽出部２４２は、セグメントＳ21〜Ｓ25に含まれる点群に基づいて法線ベクトルを算出し、算出した法線ベクトルをＳ21の法線ベクトルとして特定する。
【００５８】
図１２は、特定の１つのフレームにおける各直線状のセグメントの法線ベクトルをマッピングした３次元空間の一例を示す模式図である。
【００５９】
図７に戻り、次に、抽出部２４２及び平面特定部２４３は、平面を形成する点群を平面点群として抽出する（ステップＳ２０６）。ステップＳ２０６では、まず、抽出部２４２は、各直線状のセグメントを無向グラフの節点ｖkとして扱い、法線ベクトル間の類似度が高い２つの直線状のセグメントに対応する２つの節点ｖi、ｖjに対し、無向辺が存在すると判定する。例えば、抽出部２４２は、２つの直線状のセグメントの法線ベクトルの内積が所定閾値（例えば、１０度）以内である場合、法線ベクトル間の類似度が高いと判定する。抽出部２４２は、無向辺が存在する場合、ａij＝１とし、無向辺が存在しない場合、ａij＝０とする隣接行列Ａ＝［ａij］を作成する。なお、各直線状のセグメントの数がｎである場合、ｉ＝１，２，３，・・・，ｎであり、ｊ＝１，２，３，・・・，ｎであり、隣接行列Ａ＝［ａij］は、ｎ×ｎの正方行列である。このようにして、各直線状のセグメントの法線ベクトル間の類似度を示す隣接行列Ａ＝［ａij］が作成される。
【００６０】
次に、抽出部２４２は、隣接行列Ａの作成処理と同様に、各直線状のセグメントを無向グラフの節点ｖkとして扱い、各直線状のセグメントの位置間の類似度が高い２つの直線状のセグメントに対応する２つの節点ｖi、ｖjに対し、無向辺が存在すると判定する。例えば、抽出部２４２は、特定の直線状のセグメントと他の直線状のセグメントとの間の距離を算出し、特定の直線状のセグメントとの距離が短い所定数（例えば、５つ）の他の直線状のセグメントを特定する。そして、特定の直線状のセグメントと、特定の直線状のセグメントとの距離が短い所定数の他の直線状のセグメントとの間において、位置間の類似度が高いと判定する。抽出部２４２は、無向辺が存在する場合、ｂij＝１とし、無向辺が存在しない場合、ｂij＝０とする隣接行列Ｂ＝［ｂij］を作成する。このようにして、各直線状のセグメントの位置間の類似度を示す隣接行列Ｂ＝［ｂij］が作成される。
【００６１】
次に、抽出部２４２は、次の式（１）によって、隣接行列Ａ及び隣接行列Ｂの要素積Ｃを算出する。
【００６２】
【数１】
0006525148.tif 000002
【００６３】
次に、抽出部２４２は、次の式（２）によって、ベキ乗行列のブール和を算出する。
【００６４】
【数２】
0006525148.tif 000003
【００６５】
次に、抽出部２４２は、次の式（３）に示すとおり、ベキ乗行列のブール和ＣkとＣk+1とが変わらなくなる場合におけるＣk（又はＣk+1）を可到達行列Ｄとして算出する。
【００６６】
【数３】
0006525148.tif 000004
【００６７】
次に、抽出部２４２は、可到達行列Ｄに対して、ガウスの消去法の前進消去を行うことにより重複する行を削除し、Ｄの上三角行列を算出する。次に、抽出部２４２は、Ｄの上三角行列において、要素が０でない行を１つの平面点群であると判定し、列に各平面点群に属する直線状のセグメントを抽出する。なお、行列のランクは、当該フレームにおける平面点群の総数である。
【００６８】
そして、平面特定部２４３は、抽出された各平面点群に属する直線状のセグメントの点群に対し主成分分析を行う。平面特定部２４３は、主成分分析によって算出された第１主成分及び第２主成分に基づき各平面点群の平面の式を算出することにより、平面の位置を特定する。以上により、ステップＳ２０６の処理が終了する。図１３は、特定の１つのフレームにおいて、同一の平面を形成する平面点群をマッピングした３次元空間の一例を示す模式図である。
【００６９】
図７に戻り、次に、平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームであるか否かを判定する（ステップＳ２０７）。平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームでない場合（ステップＳ２０７−Ｎｏ）、ステップＳ２０１に処理を戻す。
【００７０】
平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームである場合（ステップＳ２０７−Ｙｅｓ）、一連のステップを終了する。
【００７１】
図１４は、位置推定装置２の算出部２４４による並進ベクトル・回転行列算出処理の動作フローの一例を示す図である。図１４に示す並進ベクトル・回転行列算出処理は、図５のステップＳ１０３において実行される。
【００７２】
まず、算出部２４４は、抽出部２４２がステップＳ２０６において抽出した複数のフレームの平面点群のうち、特定のフレームの平面点群及び特定のフレームの次のフレームの平面点群を取得する（ステップＳ３０１）。以下、特定のフレームの次のフレームを、単に次のフレームと称する場合がある。
【００７３】
まず、算出部２４４は、ステップＳ３０１で取得した特定のフレームの平面点群と次のフレームの平面点群とのマッチング候補を検出する（ステップＳ３０２）。ステップＳ３０２では、まず、算出部２４４は、特定のフレームの各平面点群の重心位置及び次のフレームの各平面点群の重心位置を算出し、特定のフレームの各平面点群の重心位置と次のフレームの各平面点群の重心位置との重心間距離を算出する。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトル及び次のフレームの各平面点群の法線ベクトルを算出する。なお、算出部２４４が実行する法線ベクトルの算出処理は、抽出部２４２がステップＳ２０５において実行する法線ベクトルの算出処理と同様の処理である。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと、特定のフレームの各平面点群との重心間距離が一番短い次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であるか否かを判定する。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であると判定された場合、当該特定のフレームの平面点群と当該次のフレームの平面点群とを互いにマッチング候補として関連付ける。以上により、ステップＳ３０２の処理が終了する。
【００７４】
算出部２４４が実行するステップＳ３０２の処理により、特定のフレームの平面点群と次のフレームの平面点群との誤マッチングを防止することができる。以下、図１５を参照して、隣り合う２つのフレームの平面点群のマッチングについて具体的に説明する。
【００７５】
図１５は、隣り合う２つのフレームの平面点群のマッチングを説明するための模式図である。図１５では、平面点群Ａ１及びＡ２は、特定のフレームの平面点群であり、平面点群Ｂ１及びＢ２は、次のフレームの平面点群である。センサ３を搭載した移動体４の移動方向は、図１５に示す矢印のとおりであり、特定のフレームの平面点群Ａ１が位置する実際の平面は、次のフレームの平面点群Ｂ１が位置する平面であり。特定のフレームの平面点群Ａ２が位置する実際の平面は、次のフレームの平面点群Ｂ２が位置する平面である。
【００７６】
特定のフレームの各平面点群に対して、各平面点群の法線ベクトルとの重心間距離が一番短い次のフレームの平面点群をマッチング候補とした場合、図１５に示す模式図では、平面点群Ｂ２が平面点群Ａ１のマッチング候補となる。このような誤マッチングを防止するため、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと、各平面点群との重心間距離が一番短い次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であるか否かを判定する。これにより、平面点群Ａ１の法線ベクトルと平面点群Ｂ２の法線ベクトルとの誤マッチングを防止することができる。
【００７７】
図１４に戻り、次に、算出部２４４は、特定のフレームのマッチング候補の平面点群と次のフレームのマッチング候補の平面点群とのマッチングを判定する（ステップＳ３０３）。
【００７８】
ステップＳ３０３では、まず、算出部２４４は、マッチング候補として互いに関連付けられた特定のフレームの平面点群及び次のフレームの平面点群を取得し、特定のフレームの平面点群に対して主成分分析を行う。算出部２４４は、図１６（ａ）に示すように、主成分分析によって算出された第１主成分及び第２主成分を軸とした２次元平面を算出し、算出された２次元平面に、特定のフレームのマッチング候補の平面点群を射影することにより次元圧縮を行う。次に、算出部２４４は、図１６（ｂ）に示すように、特定のフレームのマッチング候補の平面点群に対して算出された２次元平面に、次のフレームのマッチング候補の平面点群を射影する。次に、算出部２４４は、図１７に示すように、次元圧縮された特定のフレームのマッチング候補の平面点群に対してドロネー三角形分割を求め、求められたドロネー三角形分割に基づいてＣｏｎｖｅｘ  ｈｕｌｌを作成する。次に、算出部２４４は、作成したＣｏｎｖｅｘ  ｈｕｌｌの内部に、射影された次のフレームのマッチング候補の平面点群が包含される包含率を算出し、算出された包含率が所定の閾値（例えば、６０％）以上である場合は、マッチングが成立したと判定する。以上により、ステップＳ３０３の処理が終了する。
【００７９】
図１４に戻り、次に、算出部２４４は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群に基づいてフレーム間の並進ベクトルＴ及び回転行列Ｒを算出する（ステップＳ３０４）。
【００８０】
ステップＳ３０４では、まず、算出部２４４は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群のそれぞれに対する平面の式を算出する。以下、前者を、特定のフレームのマッチング平面と称し、後者を、次のフレームのマッチング平面と称する。なお、算出部２４４が実行する平面の式の算出処理は、平面特定部２４３がステップＳ２０６において実行する平面の式の算出処理と同様である。次に、算出部２４４は、特定のフレームのマッチング平面の法線ベクトルＮ1と次のフレームのマッチング平面の法線ベクトルＮ2とにより次の式（４）に示す目的関数を最小二乗法を用いて最小化する行列Ｒ1を算出する。
【００８１】
【数４】
0006525148.tif 000005
【００８２】
次に、算出部２４４は、行列のｐｏｌａｒ  ｄｅｃｏｍｐｏｓｉｔｉｏｎを利用して、次の式（５）及び（６）によってフレーム間の回転行列Ｒを算出する。
【００８３】
【数５】
0006525148.tif 000006
【００８４】
【数６】
0006525148.tif 000007
【００８５】
次に、算出部２４４は、特定のフレームのマッチング平面の垂線の足からなるベクトルｄ1と次のフレームのマッチング平面の垂線の足からなるベクトルｄ2とから、次の式（７）によってフレーム間の並進ベクトルＴを算出する。以上により、ステップＳ３０４の処理が終了する。
【００８６】
【数７】
0006525148.tif 000008
【００８７】
そして、算出部２４４は、ステップＳ３０１〜Ｓ３０４において実行された次のフレームが最後のフレームでない場合（ステップＳ３０５−Ｎｏ）、ステップＳ３０１に処理を戻す。また、算出部２４４は、ステップＳ３０１〜Ｓ３０４において実行された次のフレームが最後のフレームである場合（ステップＳ３０５−Ｙｅｓ）、一連のステップを終了する。
【００８８】
図１８は、位置推定装置２の最適化部２４５による最適化処理の動作フローの一例を示す図である。図１８に示す最適化処理は、図５のステップＳ１０４において実行される。以下、図１９（ａ）及び（ｂ）を参照して、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群について説明する。
【００８９】
図１９（ａ）は、平面点群の真の平面及びマッチング平面を説明するための模式図である。図１９（ａ）に示すように、特定のフレームの平面点群の真の平面（実際の平面）と特定のフレームのマッチング平面との間に誤差が生じている場合がある。同様に、次のフレームの平面点群の真の平面（実際の平面）と次のフレームのマッチング平面との間に誤差が生じている場合がある。
【００９０】
図１９（ｂ）は、平面点群の最適化を説明するための模式図である。特定のフレームの平面点群の真の平面と特定のフレームのマッチング平面との間に誤差が生じている場合や、次のフレームの平面点群の真の平面（実際の平面）と次のフレームのマッチング平面との誤差が生じている場合、フレーム間の並進ベクトルＴ及び回転行列Ｒを用いて、マッチングが成立した特定のフレームの平面点群を移動させると、特定のフレームのマッチング平面と次のフレームのマッチング平面との間にぶれが生じる。
【００９１】
最適化部２４５は、特定のフレームのマッチング平面と次のフレームのマッチング平面との間にぶれが生じることが無いように、フレーム間の並進ベクトルＴ及び回転行列Ｒを最適化する。
【００９２】
まず、最適化部２４５は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群、並びに、特定のフレーム及び次のフレームのフレーム間の並進ベクトルＴ及び回転行列Ｒを取得する（ステップＳ４０１）。
【００９３】
次に、最適化部２４５は、フレーム間の並進ベクトルＴ及び回転行列Ｒの最適化処理を行う（ステップＳ４０２）。ステップ４０２では、最適化部２４５は、まず、フレーム間の並進ベクトルＴ及び回転行列Ｒを用いて、マッチングが成立した特定のフレームの平面点群を移動させ、マッチングが成立した特定のフレームの平面点群とマッチングが成立した次のフレームの平面点群とを重ねる。次に、最適化部２４５は、マッチングが成立した全ての特定のフレームの平面点群及び次のフレームの平面点群の法線方向の分散が最小となるような目的関数を設定し、レーベンバーグ・マルカート法によって、並進ベクトルＴ及び回転行列Ｒの最適化を行う。なお、レーベンバーグ・マルカート法による最適化は、並進ベクトルＴ及び回転行列Ｒを交互に最適化し、並進ベクトルＴに対して最適化を行っている場合は、回転行列Ｒを固定し、回転行列Ｒに対して最適化を行っている場合は、並進ベクトルＴを固定する。以上により、ステップＳ４０２の処理が終了する。
【００９４】
そして、最適化部２４５は、ステップＳ４０１〜Ｓ４０２において実行された次のフレームが最後のフレームでない場合（ステップＳ４０３−Ｎｏ）、ステップＳ４０１に処理を戻す。また、最適化部２４５は、ステップＳ４０１〜Ｓ４０２において実行された次のフレームが最後のフレームである場合（ステップＳ４０３−Ｙｅｓ）、一連のステップを終了する。
【００９５】
図２０は、位置推定装置２の推定部２４６による推定処理の動作フローの一例を示す図である。図２０に示す推定処理は、図５のステップＳ１０５において実行される。
【００９６】
まず、位置推定装置２の推定部２４６は、最適化部２４５によって最適化された並進ベクトルＴ及び回転行列Ｒに基づいて、１周目に所定のループ状の経路を移動した移動体の軌跡及び２周目に所定のループ状の経路を移動した移動体の軌跡を算出（ステップＳ５０１）する。以下、１周目に所定のループ状の経路を移動した移動体の軌跡を１周目の軌跡と称し、２周目に所定のループ状の経路を移動した移動体の軌跡を２周目の軌跡と称する。
【００９７】
次に、推定部２４６は、１周目の軌跡及び２周目の軌跡によって、１周目の各フレームと２周目の各フレームを互いに対応付ける（ステップＳ５０２）。まず、推定部２４６は、１周目の各フレームの点群を取得した移動体（センサ）のフレーム取得位置を１周目の軌跡から算出し、２周目の各フレームの点群を取得した移動体（センサ）のフレーム取得位置を２周目の軌跡から算出する。そして、推定部２４６は、１周目のフレーム毎に、１周目の各フレーム取得位置から最も近い２周目の各フレーム所得位置のフレームを抽出し、１周目の各フレームと抽出した２周目のフレームとを対応付ける。
【００９８】
次に、推定部２４６は、蓄積並進ベクトルＴacc,i及び蓄積回転行列Ｒacc,iを算出する（ステップＳ５０３）。蓄積並進ベクトルＴacc,i及び蓄積回転行列Ｒacc,iは、１周目のｉ番目のフレームＦ1,iと１周目のｉ＋１番目のフレームＦ1,i+1との間の並進ベクトルＴi及び回転行列Ｒiから、２周目のｉ−１番目のフレームＦ2,i-1と２周目のｉ番目のフレームＦ2,iとの間の並進ベクトルＴi-1及び回転行列Ｒi-1までの全ての並進ベクトルＴ及び回転行列Ｒを蓄積したものである。
【００９９】
ここで、並進ベクトルＴ及び回転行列Ｒの蓄積処理について説明する。１周目のｉ番目のフレームＦ1,iと１周目のｉ＋１番目のフレームＦ1,i+1との間の並進ベクトルＴi及び回転行列Ｒiは、センサがフレームＦ1,iの点群を取得した際のセンサ座標系の並進ベクトル及び回転行列である。すなわち、並進ベクトルＴi及び回転行列Ｒiは、センサがフレームＦ1,iの点群を取得した際のセンサ（移動体）の位置に対する、センサがフレームＦ1,i+1の点群を取得した際のセンサ（移動体）の相対的な推定位置を示す。同様に、並進ベクトルＴi+1及び回転行列Ｒi+1は、センサがフレームＦ1,i+1の点群を取得した際のセンサの位置に対する、センサがフレームＦ1,i+2の点群を取得した際のセンサの相対的な推定位置を示す。並進ベクトルＴi及び回転行列Ｒiと並進ベクトルＴi+1及び回転行列Ｒi+1とにより、センサがフレームＦ1,iの点群を取得した際のセンサの位置に対する、センサがフレームＦ1,i+2の点群を取得した際のセンサの相対的な推定位置が算出される。したがって、１周目のｉ番目の並進ベクトルＴi及び回転行列Ｒiから、２周目のｉ−１番目の並進ベクトルＴi-1及び回転行列Ｒi-1までの全ての並進ベクトルＴ及び回転行列Ｒによって、センサがフレームＦ1,iの点群を取得した際のセンサの位置に対する、センサがフレームＦ2,iの点群を取得した際のセンサの相対的な推定位置が算出される。
【０１００】
次に、推定部２４６は、１周目の各フレームに含まれる特徴部の位置と当該フレームに対応する２周目のフレームに含まれる特徴部の位置とから、１周目の各フレームと２周目対応するフレームとの間の直接並進ベクトルＴdir,i及び直接回転行列Ｒdir,iを算出する（ステップＳ５０４）。直接並進ベクトルＴdir,i及び直接回転行列Ｒdir,iの算出処理は、図１４の位置推定装置２の算出部２４４による並進ベクトル・回転行列算出処理の動作フローと同様の処理によって実行される。
【０１０１】
次に、推定部２４６は、蓄積並進ベクトルＴacc,i及び蓄積回転行列Ｒacc,iと直接並進ベクトルＴdir,i及び直接回転行列Ｒdir,iとの差を示す推定誤差ベクトルＴerror,iと推定誤差行列Ｒerror,iを、次の式（８）によって算出する（ステップＳ５０５）。なお、ＲTaccは、Ｒaccの転置行列を示す。
【０１０２】
【数８】
0006525148.tif 000009
【０１０３】
また、図２１（ａ）は、蓄積並進ベクトルＴacc,i及び蓄積回転行列Ｒacc,i及び直接並進ベクトルＴdir,i及び直接回転行列Ｒdir,iの一例を説明するための模式図である。図２１（ａ）に示すとおり、蓄積並進ベクトルＴacc,i及び蓄積回転行列Ｒacc,iは、フレームＦ1,iを取得した時のセンサ座標系を基準とした並進ベクトル及び回転行列で表される。また、蓄積並進ベクトル及び蓄積回転行列は、フレームＦ1,iを取得した位置に対する、フレームＦ1,iからフレームＦ2,iまでのフレーム間の並進ベクトル及び回転行列の蓄積によって推定されるフレームＦ2,iの位置の相対位置を示す。フレーム間の並進ベクトルＴ及び回転行列Ｒは誤差を含むため、蓄積並進ベクトル及び蓄積回転行列は、フレームＦ1,iからフレームＦ2,iまでの各フレーム間の並進ベクトルＴ及び回転行列Ｒの誤差の蓄積を含むものである。
【０１０４】
また、図２１（ａ）に示すとおり、直接並進ベクトルＴdir,i及び直接回転行列Ｒdir,iは、１周目のフレームＦ1,iに含まれる特徴部の位置と当該フレームＦ1,iに対応する２周目のフレームＦ2,iに含まれる特徴部の位置とから、直接求めた並進ベクトル及び回転行列である。直接並進ベクトルＴdir,i及び直接回転行列Ｒdir,iによって算出される、フレームＦ1,iにおけるセンサの位置に対するフレームＦ2,iにおけるセンサの相対的な推定位置には、誤差が蓄積されるおそれがないため、直接並進ベクトルＴdir,i及び直接回転行列Ｒdir,iによって算出される推定位置が、フレームＦ2,iにおけるセンサの相対的な位置として、正確な位置であると仮定される。
【０１０５】
図２１（ａ）に示すとおり、蓄積並進ベクトルＴacc,i及び蓄積回転行列Ｒacc,iと直接並進ベクトルＴdir,i及び直接回転行列Ｒdir,iとの差を示す推定誤差ベクトルＴerror,iと推定誤差行列Ｒerror,iは、蓄積並進ベクトルＴacc,i及び蓄積回転行列Ｒacc,iによって算出される推定位置から、直接並進ベクトルＴdir,i及び直接回転行列Ｒdir,iによって算出される推定位置との誤差を示す。
【０１０６】
次に、推定部２４６は、フレームＦ1,i及びフレームＦ2,iにおける推定誤差ベクトルＴerror,iとフレームＦ1,i+1及びフレームＦ2,i+1における推定誤差ベクトルＴerror,i+1との間の推定誤差の差を示す推定誤差変化ベクトルから、所定のフレームにおける推定誤差変化量Ｅiを算出する（ステップＳ５０６）。推定誤差変化量Ｅiは、フレームＦ1,i-1及びフレームＦ2,i-1における推定誤差ベクトルＴerror,i-1までの影響を受けないため、並進ベクトルＴ1,iのみの誤差に影響される量である（図２１（ｂ））。
【０１０７】
次に、推定部２４６は、推定誤差変化量ＥiをフレームＦ1,i及びフレームＦ2,iのいずれか一方に振り分ける（ステップＳ５０７）。まず、推定部２４６は、１周目の各並進ベクトルＴ1,iに推定誤差変化ベクトルを除算した各第２並進ベクトルＴ'1,iを算出するとともに、２周目の各並進ベクトルＴ2,iに推定誤差変化ベクトルを加算した各第２並進ベクトルＴ'2,iを算出する。次に、推定部２４６は、第２並進ベクトルＴ'1,i及び第２並進ベクトルＴ'2,iを用いて、フレームＦ1,i+1及びフレームＦ2,i+1における平面を、フレームＦ1,i及びフレームＦ2,iにおけるセンサ座標系に座標変換し、座標変換された平面を用いて、フレームＦ1,i及びフレームＦ1,i+1における第３並進ベクトルＴ"1,iとフレームＦ2,i及びフレームＦ2,i+1における第３並進ベクトルＴ"2,iとを求める。
【０１０８】
次に、推定部２４６は、次の式（９）によって、第３並進ベクトルＴ"1,iと第３並進ベクトルＴ"2,iとのＬ１ノルム差を示すＤiを算出する。
【数９】
0006525148.tif 000010
【０１０９】
１周目の各並進ベクトルＴ1,iの誤差が小さい場合、１周目の各並進ベクトルＴ1,iに推定誤差変化ベクトルを除算した各第２並進ベクトルＴ'1,iは、１周目の各並進ベクトルＴ1,iよりも大きくなる。同様に、２周目の各並進ベクトルＴ2,iの誤差が小さい場合、２周目の各並進ベクトルＴ2,iに推定誤差変化ベクトルを加算した各第２並進ベクトルＴ'2,iは、各並進ベクトルＴ2,iよりも大きくなる。したがって、１周目の第３並進ベクトルＴ"1,iのＬ１ノルムと２周目の第３並進ベクトルＴ"2,iのＬ１ノルム差との差であるＤiが負の場合すなわち２周目の第３並進ベクトルＴ"2,iのＬ１ノルムの方が大きい場合は、１周目の各並進ベクトルＴ1,iの方が、誤差が大きかったことになるため、推定誤差変化量Ｅiを１周目のフレームＦ1,iに振り分ける。また、１周目の第３並進ベクトルＴ"1,iのＬ１ノルムと２周目の第３並進ベクトルＴ"2,iのＬ１ノルム差との差であるＤiが正の場合すなわち１周目の第３並進ベクトルＴ"1,iのＬ１ノルムの方が大きい場合は、２周目の各並進ベクトルＴ2,iの方が、誤差が大きかったことになるため、推定誤差変化量Ｅiを２周目のフレームＦ2,iに振り分ける。以上により、ステップＳ５０７の処理が終了する。なお、推定部２４６は、上述したステップＳ５０７を実行せずに、全ての推定誤差変化量Ｅiを、１周目のフレームＦ1,i及び２周目のフレームＦ2,iの両方／いずれか一方に振り分けるようにしてもよい。
【０１１０】
次に、推定部２４６は、１周目の終点のフレームＦ1,nの推定誤差ベクトルＴ1,allerrorと、１周目に振り分けられた推定誤差変化量Ｅiとによって補正ベクトルＣ1,iを算出する（ステップＳ５０８）。すなわち、推定部２４６は、１周目に振り分けられた全ての推定誤差変化量Ｅiの総和に対する各推定誤差変化量Ｅiの比率を算出する。そして、推定部２４６は、推定誤差ベクトルＴ1,allerrorに、各フレームの推定誤差変化量Ｅiの比率を乗ずることにより、各フレームの補正ベクトルＣ1,iを算出する。
【０１１１】
次に、推定部２４６は、２周目の終点のフレームＦ2,nの推定誤差ベクトルＴ2,allerrorと、２周目に振り分けられた推定誤差変化量Ｅiとによって補正ベクトルＣ2,iを算出する（ステップＳ５０９）。すなわち、推定部２４６は、２周目に振り分けられた全ての推定誤差変化量Ｅiの総和に対する各推定誤差変化量Ｅiの比率を算出する。そして、推定部２４６は、推定誤差ベクトルＴ2,allerrorに、各フレームの推定誤差変化量Ｅiの比率を乗ずることにより、各フレームの補正ベクトルＣ2,iを算出する。
【０１１２】
次に、推定部２４６は、１周目のフレームＦ1,iの並進ベクトルＴ1,iに補正ベクトルＣ1,iを加算することにより、並進ベクトルＴ1,iを補正する。また、推定部２４６は、２周目のフレームＦ2,iの並進ベクトルＴ2,iに補正ベクトルＣ2,iを加算することにより、並進ベクトルＴ2,iを補正する。そして、推定部２４６は、補正された並進ベクトルＴ1,iと回転行列1,iとによって、１周目において移動体が移動した軌跡を推定するとともに、補正された並進ベクトルＴ2,iと回転行列2,iとによって、２周目において移動体が移動した軌跡を推定する（ステップＳ５１０）。
【０１１３】
以上説明してきたように、位置推定装置２は、誤差が蓄積される並進ベクトルを、誤差が蓄積されるおそれが少ない直接並進ベクトルに近づけることができるため、並進ベクトル及び回転行列を蓄積することによって推定される移動体の移動軌跡を、並進ベクトルのそれぞれの誤差を排除した移動体の移動軌跡に補正することが可能となる。また、位置推定装置２は、出発地点に再び戻る移動を行う移動体の自己位置推定の精度を向上させることが可能となる。
【０１１４】
当業者は、本発明の精神及び範囲から外れることなく、様々な変更、置換、及び修正をこれに加えることが可能であることを理解されたい。
【符号の説明】
【０１１５】
１    位置推定システム
２    位置推定装置
２１    記憶部
２２    入力部
２３    出力部
２４    処理部
２４１    取得部
２４２    抽出部
２４３    平面特定部
２４４    算出部
２４５    最適化部
２４６    推定部
３    センサ
４    移動体
【図１】
0006525148.tif 000011
【図２】
0006525148.tif 000012
【図３】
0006525148.tif 000013
【図４】
0006525148.tif 000014
【図５】
0006525148.tif 000015
【図６】
0006525148.tif 000016
【図７】
0006525148.tif 000017
【図８】
0006525148.tif 000018
【図９】
0006525148.tif 000019
【図１０】
0006525148.tif 000020
【図１１】
0006525148.tif 000021
【図１２】
0006525148.tif 000022
【図１３】
0006525148.tif 000023
【図１４】
0006525148.tif 000024
【図１５】
0006525148.tif 000025
【図１６】
0006525148.tif 000026
【図１７】
0006525148.tif 000027
【図１８】
0006525148.tif 000028
【図１９】
0006525148.tif 000029
【図２０】
0006525148.tif 000030
【図２１】
0006525148.tif 000031