JP6525148B2 - 軌跡推定方法、軌跡推定装置及び軌跡推定プログラム - Google Patents

Description

本発明は、軌跡推定方法、軌跡推定装置及び軌跡推定プログラムに関する。

軌跡推定方法として、レーザセンサ又は撮像装置等のセンサを用いて周囲の地物の位置を測定し、測定された周囲の地物の位置に基づいて自己位置を推定する軌跡推定方法が知られている。このような軌跡推定方法では、測定中における各センサの位置及び姿勢が推定され、推定された位置及び姿勢と周囲の地物の位置とに基づいて自己位置が推定される。

例えば、非特許文献１には、移動ロボットに搭載されたカメラによって撮影された複数の画像に含まれる複数の特徴点座標に基づいてカメラの位置姿勢が推定され、推定された位置姿勢から移動ロボットの自己位置が推定される軌跡推定方法が記載されている。この軌跡推定方法では、撮影された最初の画像から特徴点が抽出され、特徴点追跡手法によって移動後の画像に含まれる特徴点が追跡され、各画像における特徴点に基づいて並進ベクトル及び回転行列が算出される。そして、算出された複数の並進ベクトル及び回転行列に基づいて当該カメラの移動軌跡が推定される。

川西亮輔, 外２名, "全方位カメラを用いた周囲環境測定のための特徴点フローモデルによる位置姿勢推定", 映像情報メディア学会技術報告, ２００９年２月, 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）ディスプレイ等である。出力部２３は、処理部２４から供給された映像データに応じた映像や、画像データに応じた画像等を表示する。また、出力部２３は、紙などの表示媒体に、映像、画像又は文字等を印刷する出力装置であってもよい。

処理部２４は、一又は複数個のプロセッサ及びその周辺回路を有する。処理部２４は、位置推定装置２の全体的な動作を統括的に制御するものであり、例えば、ＣＰＵである。処理部２４は、記憶部２１に記憶されているプログラム（ドライバプログラム、オペレーティングシステムプログラム、アプリケーションプログラム等）に基づいて処理を実行する。また、処理部２４は、複数のプログラム（アプリケーションプログラム等）を並列に実行できる。

２．２．処理部２４の構成
処理部２４は、取得部２４１、抽出部２４２、平面特定部２４３、算出部２４４、最適化部２４５、及び推定部２４６を備える。これらの各部は、処理部２４が備えるプロセッサで実行されるプログラムにより実現される機能モジュールである。あるいは、これらの各部は、ファームウェアとして位置推定装置２に実装されてもよい。

図５は、位置推定処理の動作フローの一例を示す図である。この動作フローは、予め記憶部２１に記憶されているプログラムに基づいて、主に処理部２４により、位置推定装置２の各要素と協働して実行される。

まず、センサが、移動体が移動している間に回転されるレーザ照射部分によって測定された地物の位置を示す点群ＰＴを出力し、位置推定装置２の取得部２４１は、出力された点群ＰＴを１回転毎に１フレームずつ順次取得する（ステップＳ１０１）。取得部２４１は、取得した点群ＰＴを記憶部２１の点群テーブルに記憶する。

図６は、１フレームの点群をマッピングした３次元空間の一例を示す模式図である。図６に示すように、１フレームの点群は、３２本のレーザを同時に照射するセンサのレーザ照射部分の回転によって測定された、周囲３６０度の複数の地物のセンサ座標系の３次元位置である。以下、１フレームにおけるセンサ１本分の点群を１レイヤーの点群と称する。

図５に戻り、次に、位置推定装置２の抽出部２４２及び平面特定部２４３は、取得部２４１によって点群ＰＴが取得されると、平面を抽出するための平面抽出処理を実行する（ステップＳ１０２）。平面抽出処理の詳細については後述する。

次に、位置推定装置２の算出部２４４は、抽出部２４２及び平面特定部２４３によって平面抽出処理が行われると、並進ベクトル・回転行列算出処理を実行する（ステップＳ１０３）。並進ベクトル・回転行列算出処理の詳細については後述する。

次に、位置推定装置２の最適化部２４５は、算出部２４４によって並進ベクトル・回転行列算出処理が実行されると、最適化処理を実行する（ステップＳ１０４）。最適化処理の詳細については後述する。

そして、位置推定装置２の推定部２４６は、最適化部２４５によって最適化処理が実行されると、推定処理を実行し（ステップＳ１０５）、一連のステップを終了する。なお、位置推定処理の一連のステップが終了すると、操作者の指示に応じて、位置推定装置２の処理部２４は、出力部２３に、位置推定処理の結果（移動体の移動軌跡等）を表示する。

図７は、位置推定装置２の抽出部２４２及び平面特定部２４３による平面抽出処理の動作フローの一例を示す図である。図７に示す平面抽出処理は、図５のステップＳ１０２において実行される。

まず、抽出部２４２は、点群テーブルに記憶された複数のフレームの点群のうち、１つのフレームの点群を取得する（ステップＳ２０１）。以下のステップＳ２０２〜Ｓ２０７は、取得された１つのフレームの点群に対して実行される。

次に、抽出部２４２は、レイヤー毎に、取得した点群のうちの所定の点と次の点との間の距離を算出する（ステップＳ２０２）。すなわち、抽出部２４２は、レーザＩＤ毎に、所定のレコードの３次元位置（ｘ、ｙ、ｚ）と、当該レコードの次のレコードの３次元位置（ｘ、ｙ、ｚ）とを特定し、特定した両位置を端点とする線分の距離を算出する。以下、算出された距離を点間距離と称する。

次に、抽出部２４２は、各レイヤーの点群を点間距離に基づいてセグメントに分類する（ステップＳ２０３）。すなわち、抽出部２４２は、レーザＩＤ毎に、レコードの順番に従って各点間距離を累積加算した値が閾値を超えた場合、当該閾値を超える前までに累積加算した全ての点間距離における端点を示す各点を１つのセグメントとする。なお、以下、セグメントに含まれる全ての点を、セグメントに含まれる点群と称する場合がある。図８は、特定の１つのレイヤーにおける全てのセグメントをマッピングした２次元空間の一例を示す模式図である。

次に、抽出部２４２は、Ｓ２０３において分割したセグメントのうち、セグメントに含まれる点群の数が所定数以上であり且つセグメントに含まれる点群の形状が直線状であるセグメントを検出する（ステップＳ２０４）。ステップＳ２０４では、まず、抽出部２４２は、セグメントに含まれる点群の数が所定数以上であるか否か判断する。次に、抽出部２４２は、点群の数が所定数以上であるセグメントに含まれる全て点群に対して、主成分分析を行う。次に、抽出部２４２は、第１主成分の分散が第２主成分の分散及び第３主成分の分散よりも所定比率以上大きい場合，主成分分析を行った点群を含むセグメントを直線状のセグメントとして検出する。これにより、セグメントに含まれる点群によって形成される形状が、第１主成分を軸とする直線状であると判定される。

図９は、特定の１つのレイヤーにおける各直線状のセグメントをマッピングした２次元空間の一例を示す模式図であり、図１０は、特定の１つのフレームにおける各直線状のセグメントをマッピングした３次元空間の一例を示す模式図である。

次に、抽出部２４２は、Ｓ２０４において特定された直線状のセグメントの法線ベクトルを算出する（ステップＳ２０５）。ステップＳ２０５では、まず、抽出部２４２は、Ｓ２０４において特定された全ての直線状のセグメントを取得する。次に、抽出部２４２は、取得した全ての直線状のセグメントのそれぞれに対して、各直線状のセグメントを中心とした所定範囲内の他の直線状のセグメントを検出する。次に、抽出部２４２は、各直線状のセグメントと各直線状のセグメントに対して検出された他の直線状のセグメントとの内積のなす角が所定閾値以下の他の直線状のセグメントを近傍セグメントとして検出する。次に、抽出部２４２は、各直線状のセグメントと各直線状のセグメントに対して検出された近傍セグメントとに含まれる点群に対して主成分分析を行う。そして、抽出部２４２は、第１主成分の分散及び第２主成分の分散が第３主成分の分散よりも一定比率以上（例えば、４００倍以上）大きい場合、第１主成分及び第２主成分を軸とした平面に対する法線ベクトルを、各直線状のセグメントの法線ベクトルとして算出する。以下、図１１（ａ）及び（ｂ）を参照して、ステップＳ２０５の処理について具体的に説明する。

図１（ａ）は、直線状のセグメントにおける法線ベクトルの一例を説明するための模式図である。レイヤーＡ、Ｂ、Ｃ，Ｄ，及びＥ毎に、ステップＳ２０４において検出された直線状のセグメントが特定されている。例えば、セグメントＳ₁₅は、レイヤーＡにおける直線状のセグメントである。また、セグメントＳ₁₁、Ｓ₁₂、Ｓ₁₆は、レイヤーＢにおける直線状のセグメントである。また、セグメントＳ₁₃、Ｓ₁₄は、レイヤーＣにおける直線状のセグメントである。なお、図１１（ａ）及び（ｂ）において示される全てのセグメントは、直線状のセグメントである。図１１（ａ）に示すように、抽出部２４２は、セグメントＳ₁₁を中心とした所定範囲内にある他のセグメントＳ₁₂〜Ｓ₁₆を検出する。次に、抽出部２４２は、セグメントＳ₁₁とセグメントＳ₁₂〜Ｓ₁₆のそれぞれとの内積のなす角度が所定閾値（例えば、６０度）以下であるか否か判断する。図１１（ａ）では、セグメントＳ₁₂〜Ｓ₁₆の全てにおいて、セグメントＳ₁₁との内積のなす角度が所定閾値以下であるので、抽出部２４２は、セグメントＳ₁₁〜Ｓ₁₆に含まれる点群に基づいて法線ベクトルを算出し、算出した法線ベクトルをＳ１１の法線ベクトルとして特定する。

図１１（ｂ）は、直線状のセグメントにおける法線ベクトルの他の一例を説明するための模式図である。レイヤーＡ、Ｂ、Ｃ，Ｄ，及びＥは、図１１（ａ）と同様である。セグメントＳ₂₄、Ｓ₂₅は、レイヤーＣにおける直線状のセグメントである。また、セグメントＳ₂₁、Ｓ₂₃、Ｓ₂₆は、レイヤーＤにおける直線状のセグメントである。また、セグメントＳ₂₂は、レイヤーＥにおける直線状のセグメントである。図１１（ｂ）に示すように、抽出部２４２は、セグメントＳ₂₁を中心とした所定範囲内にある他のセグメントＳ₂₂〜Ｓ₂₆を検出する。次に、抽出部２４２は、セグメントＳ₂₁とセグメントＳ₂₂〜Ｓ₂₆のそれぞれとの内積のなす角度が所定閾値（例えば、６０度）以下であるか否か判断する。図１１（ｂ）では、セグメントＳ₂₂〜Ｓ₂₅のそれぞれとセグメントＳ₂₁との内積のなす角度は、それぞれ所定閾値以下であり、セグメントＳ₂₆とセグメントＳ₂₁との内積のなす角度は所定閾値よりも大きい。したがって、抽出部２４２は、セグメントＳ₂₁〜Ｓ₂₅に含まれる点群に基づいて法線ベクトルを算出し、算出した法線ベクトルをＳ₂₁の法線ベクトルとして特定する。

図１２は、特定の１つのフレームにおける各直線状のセグメントの法線ベクトルをマッピングした３次元空間の一例を示す模式図である。

図７に戻り、次に、抽出部２４２及び平面特定部２４３は、平面を形成する点群を平面点群として抽出する（ステップＳ２０６）。ステップＳ２０６では、まず、抽出部２４２は、各直線状のセグメントを無向グラフの節点ｖ_kとして扱い、法線ベクトル間の類似度が高い２つの直線状のセグメントに対応する２つの節点ｖ_i、ｖ_jに対し、無向辺が存在すると判定する。例えば、抽出部２４２は、２つの直線状のセグメントの法線ベクトルの内積が所定閾値（例えば、１０度）以内である場合、法線ベクトル間の類似度が高いと判定する。抽出部２４２は、無向辺が存在する場合、ａ_ij＝１とし、無向辺が存在しない場合、ａ_ij＝０とする隣接行列Ａ＝［ａ_ij］を作成する。なお、各直線状のセグメントの数がｎである場合、ｉ＝１，２，３，・・・，ｎであり、ｊ＝１，２，３，・・・，ｎであり、隣接行列Ａ＝［ａ_ij］は、ｎ×ｎの正方行列である。このようにして、各直線状のセグメントの法線ベクトル間の類似度を示す隣接行列Ａ＝［ａ_ij］が作成される。

次に、抽出部２４２は、隣接行列Ａの作成処理と同様に、各直線状のセグメントを無向グラフの節点ｖ_kとして扱い、各直線状のセグメントの位置間の類似度が高い２つの直線状のセグメントに対応する２つの節点ｖ_i、ｖ_jに対し、無向辺が存在すると判定する。例えば、抽出部２４２は、特定の直線状のセグメントと他の直線状のセグメントとの間の距離を算出し、特定の直線状のセグメントとの距離が短い所定数（例えば、５つ）の他の直線状のセグメントを特定する。そして、特定の直線状のセグメントと、特定の直線状のセグメントとの距離が短い所定数の他の直線状のセグメントとの間において、位置間の類似度が高いと判定する。抽出部２４２は、無向辺が存在する場合、ｂ_ij＝１とし、無向辺が存在しない場合、ｂ_ij＝０とする隣接行列Ｂ＝［ｂ_ij］を作成する。このようにして、各直線状のセグメントの位置間の類似度を示す隣接行列Ｂ＝［ｂ_ij］が作成される。

次に、抽出部２４２は、次の式（１）によって、隣接行列Ａ及び隣接行列Ｂの要素積Ｃを算出する。

次に、抽出部２４２は、次の式（２）によって、ベキ乗行列のブール和を算出する。

次に、抽出部２４２は、次の式（３）に示すとおり、ベキ乗行列のブール和Ｃ_kとＣ_k+1とが変わらなくなる場合におけるＣ_k（又はＣ_k+1）を可到達行列Ｄとして算出する。

次に、抽出部２４２は、可到達行列Ｄに対して、ガウスの消去法の前進消去を行うことにより重複する行を削除し、Ｄの上三角行列を算出する。次に、抽出部２４２は、Ｄの上三角行列において、要素が０でない行を１つの平面点群であると判定し、列に各平面点群に属する直線状のセグメントを抽出する。なお、行列のランクは、当該フレームにおける平面点群の総数である。

そして、平面特定部２４３は、抽出された各平面点群に属する直線状のセグメントの点群に対し主成分分析を行う。平面特定部２４３は、主成分分析によって算出された第１主成分及び第２主成分に基づき各平面点群の平面の式を算出することにより、平面の位置を特定する。以上により、ステップＳ２０６の処理が終了する。図１３は、特定の１つのフレームにおいて、同一の平面を形成する平面点群をマッピングした３次元空間の一例を示す模式図である。

図７に戻り、次に、平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームであるか否かを判定する（ステップＳ２０７）。平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームでない場合（ステップＳ２０７−Ｎｏ）、ステップＳ２０１に処理を戻す。

平面特定部２４３は、ステップＳ２０１〜Ｓ２０６において実行されたフレームが点群テーブルに記憶された最後のフレームである場合（ステップＳ２０７−Ｙｅｓ）、一連のステップを終了する。

図１４は、位置推定装置２の算出部２４４による並進ベクトル・回転行列算出処理の動作フローの一例を示す図である。図１４に示す並進ベクトル・回転行列算出処理は、図５のステップＳ１０３において実行される。

まず、算出部２４４は、抽出部２４２がステップＳ２０６において抽出した複数のフレームの平面点群のうち、特定のフレームの平面点群及び特定のフレームの次のフレームの平面点群を取得する（ステップＳ３０１）。以下、特定のフレームの次のフレームを、単に次のフレームと称する場合がある。

まず、算出部２４４は、ステップＳ３０１で取得した特定のフレームの平面点群と次のフレームの平面点群とのマッチング候補を検出する（ステップＳ３０２）。ステップＳ３０２では、まず、算出部２４４は、特定のフレームの各平面点群の重心位置及び次のフレームの各平面点群の重心位置を算出し、特定のフレームの各平面点群の重心位置と次のフレームの各平面点群の重心位置との重心間距離を算出する。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトル及び次のフレームの各平面点群の法線ベクトルを算出する。なお、算出部２４４が実行する法線ベクトルの算出処理は、抽出部２４２がステップＳ２０５において実行する法線ベクトルの算出処理と同様の処理である。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと、特定のフレームの各平面点群との重心間距離が一番短い次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であるか否かを判定する。次に、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であると判定された場合、当該特定のフレームの平面点群と当該次のフレームの平面点群とを互いにマッチング候補として関連付ける。以上により、ステップＳ３０２の処理が終了する。

算出部２４４が実行するステップＳ３０２の処理により、特定のフレームの平面点群と次のフレームの平面点群との誤マッチングを防止することができる。以下、図１５を参照して、隣り合う２つのフレームの平面点群のマッチングについて具体的に説明する。

図１５は、隣り合う２つのフレームの平面点群のマッチングを説明するための模式図である。図１５では、平面点群Ａ１及びＡ２は、特定のフレームの平面点群であり、平面点群Ｂ１及びＢ２は、次のフレームの平面点群である。センサ３を搭載した移動体４の移動方向は、図１５に示す矢印のとおりであり、特定のフレームの平面点群Ａ１が位置する実際の平面は、次のフレームの平面点群Ｂ１が位置する平面であり。特定のフレームの平面点群Ａ２が位置する実際の平面は、次のフレームの平面点群Ｂ２が位置する平面である。

特定のフレームの各平面点群に対して、各平面点群の法線ベクトルとの重心間距離が一番短い次のフレームの平面点群をマッチング候補とした場合、図１５に示す模式図では、平面点群Ｂ２が平面点群Ａ１のマッチング候補となる。このような誤マッチングを防止するため、算出部２４４は、特定のフレームの各平面点群の法線ベクトルと、各平面点群との重心間距離が一番短い次のフレームの平面点群の法線ベクトルとのなす角が、閾値以下であるか否かを判定する。これにより、平面点群Ａ１の法線ベクトルと平面点群Ｂ２の法線ベクトルとの誤マッチングを防止することができる。

図１４に戻り、次に、算出部２４４は、特定のフレームのマッチング候補の平面点群と次のフレームのマッチング候補の平面点群とのマッチングを判定する（ステップＳ３０３）。

ステップＳ３０３では、まず、算出部２４４は、マッチング候補として互いに関連付けられた特定のフレームの平面点群及び次のフレームの平面点群を取得し、特定のフレームの平面点群に対して主成分分析を行う。算出部２４４は、図１６（ａ）に示すように、主成分分析によって算出された第１主成分及び第２主成分を軸とした２次元平面を算出し、算出された２次元平面に、特定のフレームのマッチング候補の平面点群を射影することにより次元圧縮を行う。次に、算出部２４４は、図１６（ｂ）に示すように、特定のフレームのマッチング候補の平面点群に対して算出された２次元平面に、次のフレームのマッチング候補の平面点群を射影する。次に、算出部２４４は、図１７に示すように、次元圧縮された特定のフレームのマッチング候補の平面点群に対してドロネー三角形分割を求め、求められたドロネー三角形分割に基づいてＣｏｎｖｅｘ ｈｕｌｌを作成する。次に、算出部２４４は、作成したＣｏｎｖｅｘ ｈｕｌｌの内部に、射影された次のフレームのマッチング候補の平面点群が包含される包含率を算出し、算出された包含率が所定の閾値（例えば、６０％）以上である場合は、マッチングが成立したと判定する。以上により、ステップＳ３０３の処理が終了する。

図１４に戻り、次に、算出部２４４は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群に基づいてフレーム間の並進ベクトルＴ及び回転行列Ｒを算出する（ステップＳ３０４）。

ステップＳ３０４では、まず、算出部２４４は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群のそれぞれに対する平面の式を算出する。以下、前者を、特定のフレームのマッチング平面と称し、後者を、次のフレームのマッチング平面と称する。なお、算出部２４４が実行する平面の式の算出処理は、平面特定部２４３がステップＳ２０６において実行する平面の式の算出処理と同様である。次に、算出部２４４は、特定のフレームのマッチング平面の法線ベクトルＮ₁と次のフレームのマッチング平面の法線ベクトルＮ₂とにより次の式（４）に示す目的関数を最小二乗法を用いて最小化する行列Ｒ₁を算出する。

次に、算出部２４４は、行列のｐｏｌａｒ ｄｅｃｏｍｐｏｓｉｔｉｏｎを利用して、次の式（５）及び（６）によってフレーム間の回転行列Ｒを算出する。

次に、算出部２４４は、特定のフレームのマッチング平面の垂線の足からなるベクトルｄ₁と次のフレームのマッチング平面の垂線の足からなるベクトルｄ₂とから、次の式（７）によってフレーム間の並進ベクトルＴを算出する。以上により、ステップＳ３０４の処理が終了する。

そして、算出部２４４は、ステップＳ３０１〜Ｓ３０４において実行された次のフレームが最後のフレームでない場合（ステップＳ３０５−Ｎｏ）、ステップＳ３０１に処理を戻す。また、算出部２４４は、ステップＳ３０１〜Ｓ３０４において実行された次のフレームが最後のフレームである場合（ステップＳ３０５−Ｙｅｓ）、一連のステップを終了する。

図１８は、位置推定装置２の最適化部２４５による最適化処理の動作フローの一例を示す図である。図１８に示す最適化処理は、図５のステップＳ１０４において実行される。以下、図１９（ａ）及び（ｂ）を参照して、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群について説明する。

図１９（ａ）は、平面点群の真の平面及びマッチング平面を説明するための模式図である。図１９（ａ）に示すように、特定のフレームの平面点群の真の平面（実際の平面）と特定のフレームのマッチング平面との間に誤差が生じている場合がある。同様に、次のフレームの平面点群の真の平面（実際の平面）と次のフレームのマッチング平面との間に誤差が生じている場合がある。

図１９（ｂ）は、平面点群の最適化を説明するための模式図である。特定のフレームの平面点群の真の平面と特定のフレームのマッチング平面との間に誤差が生じている場合や、次のフレームの平面点群の真の平面（実際の平面）と次のフレームのマッチング平面との誤差が生じている場合、フレーム間の並進ベクトルＴ及び回転行列Ｒを用いて、マッチングが成立した特定のフレームの平面点群を移動させると、特定のフレームのマッチング平面と次のフレームのマッチング平面との間にぶれが生じる。

最適化部２４５は、特定のフレームのマッチング平面と次のフレームのマッチング平面との間にぶれが生じることが無いように、フレーム間の並進ベクトルＴ及び回転行列Ｒを最適化する。

まず、最適化部２４５は、マッチングが成立した特定のフレームの平面点群及び次のフレームの平面点群、並びに、特定のフレーム及び次のフレームのフレーム間の並進ベクトルＴ及び回転行列Ｒを取得する（ステップＳ４０１）。

次に、最適化部２４５は、フレーム間の並進ベクトルＴ及び回転行列Ｒの最適化処理を行う（ステップＳ４０２）。ステップ４０２では、最適化部２４５は、まず、フレーム間の並進ベクトルＴ及び回転行列Ｒを用いて、マッチングが成立した特定のフレームの平面点群を移動させ、マッチングが成立した特定のフレームの平面点群とマッチングが成立した次のフレームの平面点群とを重ねる。次に、最適化部２４５は、マッチングが成立した全ての特定のフレームの平面点群及び次のフレームの平面点群の法線方向の分散が最小となるような目的関数を設定し、レーベンバーグ・マルカート法によって、並進ベクトルＴ及び回転行列Ｒの最適化を行う。なお、レーベンバーグ・マルカート法による最適化は、並進ベクトルＴ及び回転行列Ｒを交互に最適化し、並進ベクトルＴに対して最適化を行っている場合は、回転行列Ｒを固定し、回転行列Ｒに対して最適化を行っている場合は、並進ベクトルＴを固定する。以上により、ステップＳ４０２の処理が終了する。

そして、最適化部２４５は、ステップＳ４０１〜Ｓ４０２において実行された次のフレームが最後のフレームでない場合（ステップＳ４０３−Ｎｏ）、ステップＳ４０１に処理を戻す。また、最適化部２４５は、ステップＳ４０１〜Ｓ４０２において実行された次のフレームが最後のフレームである場合（ステップＳ４０３−Ｙｅｓ）、一連のステップを終了する。

図２０は、位置推定装置２の推定部２４６による推定処理の動作フローの一例を示す図である。図２０に示す推定処理は、図５のステップＳ１０５において実行される。

まず、位置推定装置２の推定部２４６は、最適化部２４５によって最適化された並進ベクトルＴ及び回転行列Ｒに基づいて、１周目に所定のループ状の経路を移動した移動体の軌跡及び２周目に所定のループ状の経路を移動した移動体の軌跡を算出（ステップＳ５０１）する。以下、１周目に所定のループ状の経路を移動した移動体の軌跡を１周目の軌跡と称し、２周目に所定のループ状の経路を移動した移動体の軌跡を２周目の軌跡と称する。

次に、推定部２４６は、１周目の軌跡及び２周目の軌跡によって、１周目の各フレームと２周目の各フレームを互いに対応付ける（ステップＳ５０２）。まず、推定部２４６は、１周目の各フレームの点群を取得した移動体（センサ）のフレーム取得位置を１周目の軌跡から算出し、２周目の各フレームの点群を取得した移動体（センサ）のフレーム取得位置を２周目の軌跡から算出する。そして、推定部２４６は、１周目のフレーム毎に、１周目の各フレーム取得位置から最も近い２周目の各フレーム所得位置のフレームを抽出し、１周目の各フレームと抽出した２周目のフレームとを対応付ける。

次に、推定部２４６は、蓄積並進ベクトルＴ_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を、次の式（８）によって算出する（ステップＳ５０５）。なお、Ｒ^T _accは、Ｒ_accの転置行列を示す。

また、図２１（ａ）は、蓄積並進ベクトルＴ_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を算出する。

１周目の各並進ベクトルＴ_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とによって、２周目において移動体が移動した軌跡を推定する（ステップＳ５１０）。

以上説明してきたように、位置推定装置２は、誤差が蓄積される並進ベクトルを、誤差が蓄積されるおそれが少ない直接並進ベクトルに近づけることができるため、並進ベクトル及び回転行列を蓄積することによって推定される移動体の移動軌跡を、並進ベクトルのそれぞれの誤差を排除した移動体の移動軌跡に補正することが可能となる。また、位置推定装置２は、出発地点に再び戻る移動を行う移動体の自己位置推定の精度を向上させることが可能となる。

当業者は、本発明の精神及び範囲から外れることなく、様々な変更、置換、及び修正をこれに加えることが可能であることを理解されたい。

１ 位置推定システム
２ 位置推定装置
２１ 記憶部
２２ 入力部
２３ 出力部
２４ 処理部
２４１ 取得部
２４２ 抽出部
２４３ 平面特定部
２４４ 算出部
２４５ 最適化部
２４６ 推定部
３ センサ
４ 移動体

Claims (6)

1. ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する方法であって、
   前記センサから出力された点群から特徴部をフレーム毎に抽出し、
   所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、
   １周目の所定のフレームから当該フレームに対応する２周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
   前記１周目の所定のフレームに含まれる前記特徴部の位置と当該フレームに対応する前記２周目のフレームに含まれる前記特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
   前記２周目の所定のフレームに関する前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び前記蓄積回転行例と前記直接回転行列の差を示す推定誤差行列を前記２周目の全てのフレームについてそれぞれ算出し、
   所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を前記２周目の全てのフレームについてそれぞれ算出し、
   前記２周目の最後のフレームの推定誤差ベクトルに、前記２周目の全てのフレームについての前記推定誤差変化量の総和に対する前記２周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記２周目の全てのフレームについてそれぞれ求め、
   前記２周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した第１の補正並進ベクトルを前記２周目の全てのフレーム間についてそれぞれ算出し、
   前記第１の補正並進ベクトル及び前記回転行列を使用して前記２周目の軌跡を推定する
   ことを含むことを特徴とする軌跡推定方法。
2. 前記特徴部は、平面であり、
   前記特徴部を抽出することは、
   前記センサから出力された点群をフレーム毎に取得し、
   前記取得された点群から、平面を形成する点群を平面点群としてフレーム毎に抽出し、
   前記平面点群により形成される前記平面を抽出することを含む、請求項１に記載の軌跡推定方法。
3. 前記２周目の各フレームにおける推定誤差変化量を、当該フレームに対応付けられた前記１周目の各フレームに対応付け、
   前記１周目の最後のフレームの推定誤差ベクトルに、前記１周目の全てのフレームに対応付けられた前記推定誤差変化量の総和に対する前記１周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記１周目の全てのフレームについてそれぞれ求め、
   前記１周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した第２の補正並進ベクトルを前記１周目の全てのフレーム間についてそれぞれ算出し、
   前記第２の補正並進ベクトル及び前記回転行列を使用して前記１周目の軌跡を推定することを更に含む、請求項１又は２に記載の軌跡推定方法。
4. ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する方法であって、
   前記センサから出力された点群から特徴部をフレーム毎に抽出し、
   所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、
   １周目の所定のフレームから当該フレームに対応する２周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
   前記１周目の所定のフレームに含まれる前記特徴部の位置と当該フレームに対応する前記２周目のフレームに含まれる前記特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
   前記２周目の所定のフレームに関する前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び前記蓄積回転行例と前記直接回転行列の差を示す推定誤差行列を前記２周目の全てのフレームについてそれぞれ算出し、
   所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を前記２周目の全てのフレームについてそれぞれ算出し、
   前記２周目の各フレームにおける推定誤差変化量を、当該フレームに対応付けられた前記１周目の各フレームに対応付け、
   前記１周目の最後のフレームの推定誤差ベクトルに、前記１周目の全てのフレームに対応付けられた前記推定誤差変化量の総和に対する前記１周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記１周目の全てのフレームについてそれぞれ求め、
   前記１周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した補正並進ベクトルを前記１周目の全てのフレーム間についてそれぞれ算出し、
   前記補正並進ベクトル及び前記回転行列を使用して前記１周目の軌跡を推定する
   ことを含むことを特徴とする軌跡推定方法。
5. ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する軌跡推定装置であって、
   前記センサから出力された点群から特徴部をフレーム毎に抽出する抽出部と、
   所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出する算出部と、
   １周目の各フレームから当該フレームに対応する２周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記２周目のフレーム毎に算出し、
   前記１周目の各フレームに含まれる前記特徴部の位置と当該フレームに対応する前記２周目のフレームに含まれる前記特徴部の位置とから、前記１周目の各フレームに含まれる前記特徴部と前記２周目の対応するフレームに含まれる前記特徴部との間の位置関係を示す直接並進ベクトル及び直接回転行列を前記２周目のフレーム毎に算出し、
   前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び推定誤差行列を前記２周目のフレーム毎に算出し、
   所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとの間の距離を示す推定誤差変化量を前記２周目のフレーム毎に算出し、
   前記２周目の最後のフレームの推定誤差ベクトルに、各フレームの推定誤差変化量に応じた比率を乗じて算出された補正ベクトルに基づいて、前記２周目の前記並進ベクトルを補正した補正並進ベクトルを算出し、
   前記補正並進ベクトル及び前記回転行列を使用して前記２周目の軌跡を推定する推定部と
   を備えることを特徴とする軌跡推定装置。
6. ループ状の経路を２周移動する間に測定された複数の地物の位置を所定の測定時間毎に測定し、測定した地物のセンサ座標系の位置を示す点群を出力するセンサを搭載した移動体の軌跡を推定する軌跡推定プログラムであって、
   前記センサから出力された点群から特徴部をフレーム毎に抽出し、
   所定のフレームに含まれる前記特徴部の位置と、次のフレームに含まれる前記特徴部の位置との関係からフレーム間の並進ベクトル及び回転行列を算出し、
   １周目の所定のフレームから当該フレームに対応する２周目のフレームまでの前記並進ベクトル及び前記回転行列を蓄積した蓄積並進ベクトル及び蓄積回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
   前記１周目の所定のフレームに含まれる前記特徴部の位置と当該フレームに対応する前記２周目のフレームに含まれる前記特徴部の位置関係を示す直接並進ベクトル及び直接回転行列を前記２周目の全てのフレームについてそれぞれ算出し、
   前記２周目の所定のフレームに関する前記蓄積並進ベクトル及び前記蓄積回転行列と前記直接並進ベクトル及び前記直接回転行列とから、前記蓄積並進ベクトルと前記直接並進ベクトルとの差を示す推定誤差ベクトル及び前記蓄積回転行例と前記直接回転行列の差を示す推定誤差行列を前記２周目の全てのフレームについてそれぞれ算出し、
   所定のフレームにおける前記推定誤差ベクトルと次のフレームにおける前記推定誤差ベクトルとに基づいた当該所定のフレームについての推定誤差変化量を前記２周目の全てのフレームについてそれぞれ算出し、
   前記２周目の最後のフレームの推定誤差ベクトルに、前記２周目の全てのフレームについての前記推定誤差変化量の総和に対する前記２周目の所定のフレームの前記推定誤差変化量の比率を乗じて算出された補正ベクトルを、前記２周目の全てのフレームについてそれぞれ求め、
   前記２周目の所定のフレーム間についての前記並進ベクトルを対応する前記補正ベクトルに基づいて補正した補正並進ベクトルを前記２周目の全てのフレーム間についてそれぞれ算出し、
   前記補正並進ベクトル及び前記回転行列を使用して前記２周目の軌跡を推定する
   ことをコンピュータに実行させることを特徴とする軌跡推定プログラム。