JP6782903B2 - 自己運動推定システム、自己運動推定システムの制御方法及びプログラム - Google Patents

Description

本発明は、自己運動推定システム、自己運動推定システムの制御方法及びプログラムに関する。

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

そしてこのための技術として、ロボットに外界を認識するためのカメラなどのセンサを搭載し、センサから得られる情報をロボット自らがリアルタイムに解析することで、ロボットを取り巻く外界の３次元地図を作製しつつ自らの運動状態を推定する技術が開発されている（例えば特許文献１、非特許文献１、非特許文献２参照）。

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

J.Zhang and S.Singh:Visual-lidar Odometry and Mapping, Low-drift, Robust, and Fast, ICRA2015,pp.2174-2181,2015. M.Tomono:Robust 3DSLAM with a Stereo Camera Based on an Edge-Point ICP Algorithm, Proc. of ICRA2009,pp.4306-4311,2009.

しかしながらこの場合、ロボットは休みなく膨大な情報を取り込んで高速に処理する必要があるため、これらの膨大な情報をいかに効率的に処理できるかが重要であり、しかも、いかに正確に外界の状況及び自己の運動状態を推定できるかが、より高度な自律移動制御を実現するための鍵となる。

またこのような技術は、例えば自転車や自動車、人をも含むロボット以外の広義の移動体に適用して、自律移動制御以外の様々な分野（例えば地図作成の支援など）への応用も期待されている。

本発明はこのような点を鑑みてなされたものであり、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定可能な自己運動推定システムを提供することを一つの目的とする。

一つの側面に係る自己運動推定システムは、移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより、前記移動体の運動状態を推定する自己運動推定システムであって、前記移動体に搭載される画像センサによって所定周期で撮影される外界の明暗画像を取得する明暗画像取得部と、前記移動体に搭載される距離センサによって所定周期で計測される、前記画像センサの撮影範囲に含まれる外界の各点までの複数の距離情報を有する距離画像を取得する距離画像取得部と、前記明暗画像からエッジを検出するエッジ検出部と、第１タイミングで取得した前記距離画像と前記明暗画像とを照合し、前記明暗画像におけるエッジの位置に対応する前記距離画像内の距離情報を、エッジ位置距離情報として特定するエッジ位置距離情報特定部と、前記第１タイミングにおける前記エッジ位置距離情報と、前記第１タイミングから第２タイミングまでの間の所定時間における前記移動体の推定移動量と、を用いて算出される、前記エッジ位置距離情報の前記第２タイミングにおける明暗画像内での透視変換による投影位置が、前記第２タイミングで取得される前記明暗画像から検出されるエッジの位置に近づくように、前記移動体の前記推定移動量を算出する移動量算出部と、を備える。

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

本発明によれば、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定可能な自己運動推定システムを提供することが可能となる。

本実施形態に係る移動体及び自己運動推定システムの構成を示す図である。 本実施形態に係る明暗画像を示す図である。 本実施形態に係るエッジ画像を示す図である。 本実施形態に係る距離画像を示す図である。 本実施形態に係るＲＧＢＤ特徴ペアを示す図である。 本実施形態に係る自己運動推定システムの処理の流れを示すフローチャートである。

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

本発明の実施形態に係る自己運動推定システム１００について、図面を参照しながら説明する。

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

ここでの運動とは、移動体１０００の位置姿勢の時系列を意味する。移動体１０００の位置姿勢とは、たとえば、移動体１０００が３次元空間を移動する場合は、所定の３次元座標系での位置座標および方位角である。

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

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

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

画像センサ３００は、外界の光を検出するセンサである。本実施形態に係る画像センサ３００は、レンズを通して集光した外界の光を、２次元平面状に配置した複数の受光素子に照射し、画素ごとにＲＧＢ(Red,Green,Blue)の各色の明るさを検出することによりカラー画像を取得可能なカメラである。もちろん画像センサ３００は単色（白黒）のカメラであっても良いし、赤外線や紫外線などの可視光以外の電磁波を検知するカメラであっても良い。画像センサ３００は、所定周期（例えば0.1秒周期ないし0.01秒周期）に外界の様子を撮影する。なお周期は移動体１０００の移動速度に応じて選べばよい。例えば0.1秒より遅い場合も0.01秒より速い場合もありうる。画像センサ３００がタイミングｔで外界を撮影して得られた画像を明暗画像Ｉｔとも記す。明暗画像Ｉｔの一例を図２に模式的に示す。

距離センサ２００は、計測範囲内（計測対象となる視野の範囲内）に存在する複数の地点までの距離をレーザ光により計測するセンサである。本実施形態に係る距離センサ２００は、一例として、計測対象地点に向けて出射したレーザパルスが往復する時間を計測することで計測対象地点までの距離を計測し、計測した距離をレーザパルスの出射方向を示す情報と共に出力可能な３Ｄレーザスキャナである。レーザスキャナはレーザ光を機械的に回転させて複数の計測対象地点までの距離を逐次的に計測するため、一般に、広い視野を確保できる。レーザスキャナには、往復時間計測を用いるものの他に、レーザ光の送信信号と受信信号の位相差を用いたり、レーザ光の出射点と受光点間の距離に基づく三角法を用いるものがあり、距離センサ２００はそれらのどれでもよい。またレーザスキャナの他に、複数の受光素子を用いて複数地点の距離を同時に測定する距離画像カメラを用いてもよい。ここでは、代表としてレーザスキャナを扱うこととする。

そして本実施形態に係る距離センサ２００は、距離の計測範囲（視野の範囲）が画像センサ３００の撮影範囲（視野の範囲）の一部あるいは全部を含むように取り付け位置が調整されて移動体１０００に装着されている。つまり本実施形態に係る距離センサ２００は、画像センサ３００の撮影範囲に含まれる外界の各点までの距離が計測されるような向きに移動体１０００に装着されている。

そして、画像センサ３００及び距離センサ２００は、画像センサ３００が撮影するタイミングと、その撮影範囲内の各点までの距離を距離センサ２００が計測するタイミングとが揃うように、撮影タイミング及び計測タイミングが調整されている。ここでのタイミングは、実際の測定時刻とは別のものであり、データの通し番号に相当する離散時刻である。なお、各データの測定時刻は自己運動推定の要求精度に応じて合せる必要があるが、高精度が要求されない場合は厳密に合せなくてもよい。実際の測定時刻は別途記録しておき、データの歪み補正（後述）に用いてもよい。

自己運動推定システム１００は、移動体１０００に搭載され、外界の存在物との相対的な位置の変化を所定時間ごとに検出することにより、移動体１０００の運動状態を移動体１０００の移動中に推定する。詳細は後述する。

運動制御部４００は、自己運動推定システム１００により推定された移動体１０００の所定時間毎の移動量や、距離センサ２００により得られる距離測定データ（後述）から作成される３次元地図を用いて、移動体１０００を移動させるためのアクチュエータ５００を駆動する。

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

＜自己運動推定システムの構成＞
自己運動推定システム１００は、図１に示すように、距離画像取得部１１１、距離測定データ記憶部１１２、明暗画像取得部１２１、エッジ検出部１２２、特徴ペア生成部１３１、特徴ペア記憶部１３２、移動量算出部１４１、移動量記憶部１４２、３次元地図生成部１５０を備える。

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

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

距離画像取得部１１１は、移動体１０００に搭載される距離センサ２００によって所定周期（例えば0.1秒毎）で計測される物理量（レーザ光の往復時間や位相差、角度など）から算出される、画像センサ３００の撮影範囲（視野）に含まれる外界の各計測点までの距離情報とレーザ光の向きからなる距離測定データを取得し、この距離測定データから距離画像Ｐｔを生成する。距離画像Ｐｔは、たとえば、レーザ光の向きから得られる計測点の方位角(θ,φ)を画像上の画素位置に対応づけ、距離センサ２００から計測点までの直線距離を画素値として生成する。この方位角と距離情報から計測点の３次元位置の座標値(x,y,z)も計算できる。以下では、このような３次元位置で表された計測点（３Ｄ点）の集合も便宜的に距離画像と呼ぶ。このような距離画像では、同じ画素位置（方位角）に距離の異なる複数の３Ｄ点が登録されてもよい。なお、距離センサ２００が出力する計測点の距離、方位角、および、そこから得られる位置は、すべて距離センサ２００に付随した３次元座標系での値であり、自己運動を推定する３次元座標系（世界座標系）とは異なる。

距離画像Ｐｔの計測タイミングは、距離測定データの測定時刻を用いて、距離画像取得部１１１によって適宜生成される。例えば距離センサ２００が３６０度回転可能なセンサである場合、０度の位置から３６０度の位置までの１回転の間は同じ計測タイミングとなるように、生成される。つまり、１周分または１回の撮影分のデータを同じ離散時刻で扱う。離散時刻は順序を表す整数であり、添え字のtである。

なお以下の説明では、距離画像Ｐｔに含まれる各点を３次元座標値(x,y,z)で表したものを３Ｄ点と記し，この３Ｄ点を用いて処理の詳細を説明する。ここで、時刻ｔにおけるi番目の３Ｄ点をp_tiと表す。pは３次元座標値であり、添え字のtは計測タイミングを表し、添え字のiは識別番号である。距離画像Ｐｔの一例を図４に示す。図４に示す距離画像Ｐｔは、図２に示した明暗画像Ｉｔを撮影したタイミングｔと同じタイミングｔで距離を計測した結果を表す。なお図４において、各点のドットの大きさは各点までの距離を表しており、距離センサ２００からの距離が近いほど、ドットのサイズが大きくなるように表現されている。ただし、各点までの距離をドットの大きさで表すこの表現はここでの便宜上のものである。

距離測定データ記憶部１１２は、距離センサ２００が計測した距離測定データを、計測タイミングと対応付けて記憶する。詳細は後述するが、記憶測定データ記憶部１１２が記憶する距離測定データは、３次元地図生成部１５０が３次元地図を作成する際に用いられる。

明暗画像取得部１２１は、移動体１０００に搭載される画像センサ３００によって所定周期（例えば0.1秒毎）で撮影される外界の明暗画像Ｉｔを取得する。明暗画像Ｉｔには、撮影タイミングを表す情報と、明暗画像Ｉｔ内の各点の２次元座標を表す情報と、各点の明るさを示す情報と、が含まれる。撮影タイミングは、明暗画像取得部１２１が、例えば時刻情報を用いて生成する。

距離画像取得部１１１が生成する計測タイミングと、明暗画像取得部１２１が生成する撮影タイミングは、同じタイミングであれば同じ値になるように適宜生成される。

ここで、本実施形態に係る自己運動推定システム１００の他の構成要素及び処理内容について順に説明する前に、説明を補足する。

まず、以下の説明では、簡単のため、距離センサ２００の座標系と、移動体１０００の座標系を同一視する。そしてその座標系は、x軸が移動体１０００の前方、y軸が左方、z軸が上方とする。これは説明の都合上定めたものであり、実際は、全体で一貫性がとれていれば座標軸をどのように定めてもよい。

また明暗画像Ｉｔの内部校正、および距離センサ２００と画像センサ３００との間の外部校正はできているとする。

内部校正とレンズの歪み補正を合わせた行列をＡとする。また距離センサ２００と画像センサ３００の相対位置は固定であり、距離センサ２００の座標系から画像センサ３００の座標系への変換行列をＴ_ｌｃ＝[Ｒ|Ｔ]とする（Ｒは回転行列、Ｔは並進ベクトル）。

また、時刻tの距離画像をＰｔ、明暗画像をＩｔとする。そして時刻tの距離画像Ｐｔと明暗画像Ｉｔとの組を、フレームＦｔ＝(Ｐｔ,Ｉｔ)と呼ぶ。時刻tの移動体１０００の姿勢をrtとする。また距離画像Ｐｔと画像エッジ点（後述）を対応づけたものをＲＧＢＤ特徴ペアと呼ぶ。

以下、自己運動推定システム１００の他の構成要素について説明する。

エッジ検出部１２２は、明暗画像Ｉｔからエッジを検出する。具体的には、エッジ検出部１２２は、例えばCanny法（詳しくは、J. Canny: “A Computational Approach to Edge Detection,” IEEE Trans. on PAMI, Vol. 8, No. 6, pp.679-698 (1986)を参照）を用いて明暗画像Ｉｔからエッジを抽出する。そしてエッジ検出部１２２は、画像の勾配方向（エッジの法線方向）で微分値の絶対値が極大になる点を求めることで、太さ１画素のエッジ画像Ｅｔを生成する。エッジ画像Ｅｔは、エッジのある画素ｑ_ｔ，ｉが１、エッジのない画素ｑ_ｔ，ｉが０の値をとる。図２に示した明暗画像Ｉｔから生成されたエッジ画像Ｅｔの例を図３に示す。なお、明暗画像Ｉｔから抽出したエッジ点ｑ_ｔ，ｉの集合をＱｔとする。各エッジ点は画像の勾配方向から求めた法線ベクトルをもつ。

特徴ペア生成部１３１は、自己運動推定システム１００が同一のタイミング（第１タイミング）で取得した距離画像Ｐｔとエッジ画像Ｅｔとを照合し、エッジ画像Ｅｔにおけるエッジの位置に対応する距離画像Ｐｔ内の３Ｄ点を、エッジ位置距離情報として特定する。

具体的には、特徴ペア生成部１３１は、３Ｄ点ｐ_ｔ，ｉ∈Ｐｔを、エッジ画像Ｅｔに式（１）で透視投影し、対応する画素ｑ_ｔ，ｉを求める。ここで、各点は同次座標で表し、ｐ_ｔ，ｉ＝（Ｘ，Ｙ，Ｚ，１）^Ｔ、ｑ_ｔ，ｉ＝（ｘ，ｙ，１）^Ｔである。

ｗ_ｉｑ_ｔ，ｉ＝ＡＴ_ｌｃｐ_ｔ，ｉ （１）
なおｗ_ｉはスケールである。そして、エッジ画像Ｅｔのｑ_ｔ，ｉでの画素値が１ならば、ｐ_ｔ，ｉとｑ_ｔ，ｉを対応づけ、ＲＧＢＤ特徴ペアとする。その画素値が０ならば何もしない。時刻ｔにおけるＲＧＢＤ特徴ペアの集合をＣｔとする。

図３に示すｑ_ｔ，ｉと図４に示すｐ_ｔ，ｉがＲＧＢＤ特徴ペアとして対応する。ＲＧＢＤ特徴ペアの集合の一例を図５に示す。３Ｄ点ｐ_ｔ，ｉがエッジ位置距離情報である。

そして特徴ペア生成部１３１は、ＲＧＢＤ特徴ペアの集合Ｃｔを特徴ペア記憶部１３２に記憶する。

なお、距離センサ２００が回転型の場合は、３Ｄ点群が時々刻々と逐次的にとれる。このため、移動体１０００が移動しながら３Ｄ点群を取得すると、その形状は歪むが、この歪みの補正は公知の技術より補正することができる。

移動量算出部１４１は、上記第１タイミングにおけるエッジ位置距離情報と、第１タイミングから第２タイミングまでの間の所定時間における移動体１０００の推定移動量と、を用いて算出される、エッジ位置距離情報の第２タイミングにおける明暗画像内での透視変換による投影位置が、第２タイミングで取得される明暗画像Ｉｔから検出されるエッジの位置に近づくように、移動体１０００の推定移動量を算出する。

具体的には、移動量算出部１４１は、時刻ｔ-１（第１タイミング）におけるＲＧＢＤ特徴ペアの集合をＣｔ-１とし、時刻ｔ-１（第１タイミング）から時刻ｔ（第２タイミング）までの移動体１０００の推定移動量をｍｔ＝［Ｒｔ｜Ｔｔ］としたときに、この推定移動量ｍｔを用いてＣｔ-１の各３Ｄ点ｐ_{ｔ-１，ｉ}を、式（２）によってエッジ画像Ｅｔに投影する。

なお、図１に記載するように、移動量算出部１４１は、特徴ペア記憶部１３２からＣｔ−１を読み出して利用する。

ｗ’_ｉｑ_ｔ，ｉ＝ＡＴ_ｌｃ［Ｒｔ｜Ｔｔ］ｐ_{ｔ-１，ｉ} （２）
そして移動量算出部１４１は、Ｅｔの中でｑ_ｔ，ｉに最も近いエッジ点ｅ_ｔ，ｉを、ｐ_{ｔ-１，ｉ}に対応づける。ただしこの際、対応づけの精度を上げるために、原画像Ｉ_ｔ-１とＩ_ｔにおいて、ｑ_{ｔ-１，ｉ}とｅ_ｔ，ｉをそれぞれ中心とする局所画像のコサイン相関あるいは正規化相関を計算して、相関値が閾値を超えた場合に、ｅ_ｔ，ｉをｐ_{ｔ-１，ｉ}に対応づけると良い。

そして移動量算出部１４１は、こうして求めた対応づけに対して、式（３）を最小化する推定移動量ｍｔを求める。このとき移動量算出部１４１は、ｍｔの初期値を０として計算する。

なお、ｎ_ｔ，ｉはｅ_ｔ，ｉの法線ベクトルである。Ｇ_t(mt)の最小化は、準ニュートン法やレーベンバーグ・マーカート法、ガウス・ニュートン法、共役勾配法など種々の方法を用いて行うことが可能である。

移動量算出部１４１は、この対応づけと投影誤差最小化の処理を、Ｇ_t(mt)の値が所定値以下に収束するまで交互に繰り返す。これは、３Ｄ点−２Ｄ点対応に基づいて点対線のＩＣＰ(Iterative Closest Point)を画像空間で行うことに相当する。

ＩＣＰは２つの点群データの位置合せをする方法である。点対点のＩＣＰでは、２つの点群データの対応点間のユークリッド距離の二乗和を誤差関数とする。点対線のＩＣＰでは、どちらかの点が法線ベクトルをもつ場合に適用でき、対応点間の垂線距離の和を誤差関数とする。疎らな点群では対応点の位置が一致することはまれであり、滑らかな形状をもつ環境ならば、垂線距離を用いた誤差関数の方が安定性が高く、最小化計算において極値に陥らずによい解に到達しやすい利点がある（ＩＣＰについては、例えば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.を参照）。

そして移動量算出部１４１は、推定移動量ｍｔを移動量記憶部１４２に記憶する。

移動量記憶部１４２には、上記のようにして所定時間毎に算出された移動体１０００の推定移動量が記憶される。

自己運動推定システム１００は、これらの推定移動量を順次積算することにより、移動体１０００の移動経路を求めることが可能となる。また推定移動量を第１タイミングから第２タイミングまでの間の時間（所定時間）で除算することにより、移動体１０００の速度も求めることが可能となる。

３次元地図生成部１５０は、移動体記憶部１４２に順次蓄積されていく移動体１０００の推定移動量を、所定時間毎に積算することで、各時点における移動体１０００の位置を特定しつつ、各位置で取得した距離測定データあるいは距離画像Ｐｔを取得距離情報記憶部１１２から読み出して、移動体１０００の位置姿勢に基づいて座標変換をしながら重ね合わせることにより、３次元地図を生成する（数式としては、後述する式（８）座標変換の式と同じ）。

そして、運動制御部４００は、移動体１０００が自律移動する場合は、移動量記憶部１４２に所定時間毎に順次蓄積されていく推定移動量や、所定時間毎に完成していく３次元地図を用いて、移動体１０００の自律走行を行う。

このように本実施形態に係る自己運動推定システム１００は、距離センサ２００から得られる３Ｄ点群と、画像センサ３００から得られる明暗画像Ｉｔから抽出した画像エッジ点群と、を対応づけ、画像空間でＩＣＰを行うことにより、リアルタイムで移動体１０００の運動状態を推定することができる。このような態様によって、自己運動推定システム１００によれば、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定することが可能となる。

例えば、本実施形態に係る自己運動推定システム１００は、距離画像Ｐｔと明暗画像Ｉｔのエッジ点を用いて、上記のようにＩＣＰを行っている。多くの場合、１枚の明暗画像Ｉｔの中からは数多くのエッジ点を抽出できるため、非特許文献１のように画像のコーナ点を用いる一般的な方法に比べて、処理が格段に安定する。また画像エッジ点は法線をもつので、点対線のＩＣＰが可能となり安定性も増す。

上述したように、本実施形態に係る自己運動推定システム１００は、明暗画像Ｉｔから特徴抽出（エッジ検出）を行うようにし、３Ｄ点群からは特徴抽出を行わないので、処理が単純で効率がよい。また、リアルタイムで使える距離センサ２００は一般に解像度が低いので、遠方での計測点は疎らになり、そのような計測点からの特徴抽出は不安定になりやすいが、本実施形態に係る自己運動推定システム１００は、３Ｄ特徴（３Ｄ点群から抽出した特徴）を用いないので、遠方で３Ｄ点群が疎になって特徴抽出が不安定になるような問題も発生しない。

また自己運動推定システム１００は、３Ｄ点と２Ｄ点の対応しか用いないので、レーザセンサのみを用いる方法の場合に必要になる３Ｄ点同士の対応づけのためのデータ管理も不要にできる。

さらに自己運動推定システム１００は、平面上のテクスチャなどの画像特徴は得られても３Ｄ特徴が得られない場合であっても、問題なく動作することが可能である。

なお、本実施形態に係る自己運動推定システム１００は、距離センサ２００として３Ｄレーザスキャナを用いて距離画像Ｐｔを得ている。そのため、遠方の地形まで精度よく計測できるため、とくに屋外での大規模な地図構築に適している。

＜自己運動推定システムの処理の流れ＞
次に、本実施形態に係る自己運動推定システム１００の処理の流れを、図６に示すフローチャートに従って説明する。

まず、自己運動推定システム１００は、距離画像Ｐｔ及び明暗画像Ｉｔを取得する（S1000、S1010）。図６では、便宜上、距離画像Ｐｔを明暗画像Ｉｔよりも先に取得するように記載しているが、自己運動推定システム１００は、距離画像Ｐｔと明暗画像Ｉｔを並列して取得する。

次に、自己運動推定システム１００は、明暗画像Ｉｔからエッジ点を検出する（S1020）。エッジ点の検出は、公知のアルゴリズムを用いることが可能であり、本実施形態では、たとえば、Canny法を用いる。

そして自己運動推定システム１００は、距離画像Ｐｔの各３Ｄ点ｐ_ｔ，ｉを、エッジ画像Ｅｔに式（１）で透視投影することで、対応する画素ｑ_ｔ，ｉを求め、ＲＧＢＤ特徴ペアの集合Ｃｔを求める（S1030）。

そして自己運動推定システム１００は、時刻ｔ-１から時刻ｔまでの移動体１０００の推定移動量をｍｔとしたときに、この推定移動量ｍｔを所定の初期値から順次更新しながら、式（２）によって、時刻ｔ−１の３Ｄ点ｐ_{ｔ-１，ｉ}をＥｔに投影し、式（３）の値を最小にするような推定移動量ｍｔを求める（S1040）。

以上の処理を繰り返し実行することによって、本実施形態に係る自己運動推定システム１００は、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定することができる。

＝＝その他の実施形態＝＝
以下に、自己運動推定システム１００のその他の実施形態を説明する。以下に記載する各態様により、より一層効率的かつ正確に自己運動状態を推定することが可能となる。

＜取得周期が異なる場合＞
上述したように、時刻tの距離画像Ｐｔと明暗画像Ｉｔとの組を、フレームＦｔ＝(Ｐｔ,Ｉｔ)と呼ぶが、一般的に画像センサ３００の方が距離センサ２００よりも取得周期が短い。この場合、明暗画像取得部１２１が第１時間毎に明暗画像Ｉｔを取得するとすると、距離画像取得部１１１は、第１時間の整数倍（ｎ倍）の第２時間毎に距離画像Ｐｔを取得する。

このような場合、距離画像Ｐｔが空のフレームが生じるが、本実施形態に係る自己運動推定システム１００は以下のような処理を行うことにより、同じ３Ｄ点群を複数の明暗画像Ｉｔと位置合わせできる。

つまり、本実施形態に係る自己運動推定システム１００は、第１実施形態の処理において、明暗画像Ｉ_ｔ−１及び距離画像Ｐ_ｔ−１の双方を取得するタイミングを第１タイミングとすると共に、新たな明暗画像Ｉ_{ｔ＋ｋ−１}（k=0,…,n-1）を取得する第１時間毎に到来するタイミングを第２タイミングとして、第１時間毎に、推定移動量を算出する。

またこのとき自己運動推定システム１００は、明暗画像取得部１２１が明暗画像Ｉ_{ｔ＋ｋ−１}を取得する上記第１時間毎に、第１タイミングｔ−１で取得した距離画像Ｐ_ｔ−１と、第１タイミングｔ−１から第２タイミングｔ＋ｋ−１までの上記推定移動量の累積値とを用いて算出される、第２タイミングｔ＋ｋ−１における距離画像Ｐ_ｔ−１の透視変換による投影位置と、第２タイミングにおける明暗画像Ｉ_{ｔ＋ｋ−１}と、を照合し、明暗画像Ｉ_{ｔ＋ｋ−１}におけるエッジの位置に対応する距離画像Ｐ_ｔ−１内の距離情報を、エッジ位置距離情報として特定する
より具体的には、画像センサ３００の取得周期が距離センサ２００の取得周期の１／ｎ倍の場合、１つの３Ｄ点群Ｐｔ-１に対して、ｎ枚の明暗画像Ｉ_ｔ-１，Ｉ_ｔ…Ｉ_{ｔ＋ｎ-２}が対応するが、自己運動推定システム１００は、Ｉ_{ｔ＋ｋ-１}（k=0,…,n-1）までの推定移動量が得られているとして、Ｉ_ｔ＋ｋの位置合せを次のように行う。

まず自己運動推定システム１００は、時刻ｔ-１からｔ＋ｋまでの推定移動量をｍ_ｔ＋ｋ＝［Ｒ_ｔ＋ｋ｜Ｔ_ｔ＋ｋ］とした場合に、ｍ_{ｔ＋ｋ-１}を用いて、式（４）のように、Ｐ_ｔ-１の３Ｄ点ｐ_{ｔ-１，ｉ}をエッジ画像Ｅ_{ｔ＋ｋ-１}に投影する。

そして、自己運動推定システム１００は、第１実施形態と同様に、Ｅ_{ｔ＋ｋ-１}の画素値に基づいて、ｑ_{ｔ＋ｋ-１，ｉ}がエッジ点ならば、ｐ_{ｔ-１，ｉ}に対応づけてＲＧＢＤ特徴ペアとする。

次に、自己運動推定システム１００は、ｍ_ｔ＋ｋを用いて、式（５）のように、ｐ_{ｔ-１，ｉ}をＥ_ｔ＋ｋに投影する。

そして自己運動推定システム１００は、Ｅ_ｔ＋ｋの中でｑ_{ｔ＋ｋ，ｉ}に最も近いエッジ点ｅ_{ｔ＋ｋ，ｉ}をｐ_{ｔ-１，ｉ}に対応づける。
この際自己運動推定システム１００は、第１実施形態と同様に、原画像Ｉ_{ｔ＋ｋ-１}とＩ_ｔ＋ｋにおいて、ｑ_{ｔ＋ｋ-１，ｉ}とｅ_ｔ，ｉをそれぞれ中心とする局所画像のコサイン相関あるいは正規化相関を計算して相関値が閾値を超えた場合に、ｅ_{ｔ＋ｋ，ｉ}をｐ_{ｔ-１，ｉ}に対応づける。なお相関計算にはｑ_{ｔ-１，ｉ}を用いてもよいが、ｑ_{ｔ＋ｋ-１，ｉ}の方が視点変化による画像の歪みを小さくできる。

次に自己運動推定システム１００は、第１実施形態と同様に、式（６）を最小化する移動量ｍ_ｔ＋ｋを求める。ｍ_ｔ＋ｋの初期値はｍ_{ｔ＋ｋ-１}とする。

このような、対応づけと投影誤差の最小化の処理を、自己運動推定システム１００は交互に繰り返し行う。

このような態様によって、同じ３Ｄ点群に対して複数の明暗画像を位置合せできるようになるため、画像センサ３００の取得周期が距離センサ２００の取得周期より短くても対応することが可能となる。これらの特長により、取得周期が長い、低性能で安価な３Ｄレーザスキャナを用いることも可能となる。

＜画像の階層化＞
次に、本実施形態に係る自己運動推定システム１００は、明暗画像Ｉｔを階層化して処理することも可能である。

この場合、自己運動推定システム１００は、処理の効率化とＩＣＰの極値回避のために、明暗画像Ｉｔを階層化して位置合せを行う。階層の数は画像サイズを考慮して適宜決めればよいが、ここでは、例えば１００万画素程度の明暗画像Ｉｔを２段階で処理することとし、１／４縮小画像（解像度が相対的に低い第１明暗画像）と原画像（解像度が相対的に高い第２明暗画像）の２つを用いる。

具体的には、自己運動推定システム１００は、まず、１／４縮小画像に対して、第１実施形態に記載した方法で位置合せを行うことで、推定移動量ｍｔ（あるいはｍｔ＋ｋ）を求める。

そして、この１／４縮小画像を用いて算出した推定移動量ｍｔ（あるいはｍｔ＋ｋ）を、ＩＣＰの初期値として、原画像を用いて、推定移動量ｍ’ｔ（あるいはｍ’ｔ＋ｋ）を求める。

より具体的に説明すると、エッジ検出部１２２は、１／４縮小画像である第１明暗画像、及び原画像である第２明暗画像のそれぞれについてエッジを検出する。

そして特徴ペア生成部１３１は、時刻ｔ−１（第１タイミング）で取得した距離画像と第１明暗画像とを照合し、第１明暗画像におけるエッジの位置に対応する距離画像内の３Ｄ点を、第１エッジ位置距離情報として特定する。

そして移動量算出部１４１は、第１タイミングにおける第１エッジ位置距離情報と、時刻ｔ−１（第１タイミング）から時刻ｔ（第２タイミング）までの所定時間における移動体１０００の推定移動量ｍｔと、を用いて算出される、第１エッジ位置距離情報の時刻ｔ（第２タイミング）における明暗画像内での透視変換による投影位置が、時刻ｔ（第２タイミング）で取得される第１明暗画像におけるエッジの位置に近づくように、推定移動量ｍｔを算出する。

そして次に、特徴ペア生成部１３１は、距離画像と第２明暗画像とを照合し、第２明暗画像におけるエッジの位置に対応する距離画像内の３Ｄ点を、第２エッジ位置距離情報として特定する。

そして移動量算出部１４１は、上記第１明暗画像を用いて算出された推定移動量ｍｔを初期値として、第２エッジ位置距離情報と、時刻ｔ−１（第１タイミング）から時刻ｔ（第２タイミング）までの所定時間における移動体１０００の推定移動量ｍ’ｔと、を用いて算出される、第２エッジ位置距離情報の時刻ｔ（第２タイミング）における明暗画像内での透視変換による投影位置が、時刻ｔ（第２タイミング）で取得される第２明暗画像におけるエッジの位置に近づくように、推定移動量ｍ’ｔを算出する。

この方法は、いわゆるピラミッド探索に似ているが、対応点を求めるだけでなく縮小画像でＩＣＰ全体を行う点が異なる。その利点は、縮小画像では画素数が少ないので対応点の探索範囲が小さくて済み、また原画像では移動量のよい初期値が縮小画像により得られているので、対応点の探索範囲も移動量の探索範囲も非常に小さくて済むことである。これにより処理効率と極値回避性を向上することができる。

なお、階層の数が３以上の場合は、同様の処理を繰り返し行えばよい。例えば１／８縮小画像と１／４縮小画像と原画像との３つを用いて３段階で処理する場合は、初めに解像度が最も低い１／８縮小画像（第１明暗画像）を用いて推定移動量を算出し、次いで解像度が２番目に低い１／４縮小画像（第２明暗画像）を用いて推定移動量を算出し、最後に解像度が最も高い原画像（第３明暗画像）を用いて推定移動量を算出する。このように、Ｎ（Ｎは２以上の自然数）階層で処理する場合は、各階層の明暗画像を解像度が低い順に用いて、推定移動量をＮ回算出する。

＜移動量の予測＞
次に、本実施形態に係る自己運動推定システム１００は、移動量を推定する処理を以下のように改良することも可能である。つまり、過去のフレームで算出した推定移動量を用いて、ＩＣＰの探索範囲を狭める。

具体的には、移動量算出部１４１は、所定時間毎に推定移動量を算出する際に、直近に算出した移動体１０００の推定移動量を予測値として用いるようにする。

このような態様によって、例えば明暗画像Ｉｔ中に、隣接する走行レーンを走る車などの移動物体が写り込んでいる場合や、環境内に目印となる大きな物体（ランドマーク）が少なく、また、繰り返し構造（同じ形状が繰り返し続く構造物）が続く場合であっても、ＩＣＰの探索範囲を狭めることが可能となり、処理効率を向上させ、また対応誤りを減らすことが可能である。

つまり、ＩＣＰの探索には、対応点の探索と移動量の探索の２つがあるが、まず、対応点の探索範囲は次のように狭められる。

すなわち、移動量の予測値によって、移動体１０００の次の位置を予測する。その予測位置に基づいて、３Ｄ点群を次フレームの明暗画像に投影して、その近傍で対応点候補を探索する。予測によって３Ｄ点は真の対応点の近くに投影されるので、予測値がない場合よりもはるかに狭い範囲を探索すればすむ。

一方、移動量の探索では、予測値を初期値として、式（３）または式（６）の最適化を行う。予測により初期値が真の解に近づくので、極値に陥る可能性が低くなる。

移動量の予測により、移動物体の問題は次のように解決される。

つまり、移動体１０００と静止物体および移動体１０００と移動物体の相対速度の差が一定量より大きければ、移動物体の対応点は予測による探索範囲からはずれるため、対応点候補とならない。そのため、誤った対応づけで位置がずれることを防ぐことができる。

また繰り返し構造の問題は以下のように解決される。予測による対応点の探索範囲が繰り返し間隔より小さければ、誤対応点は探索範囲からはずれるため、対応点候補とならない。そのため、誤った対応づけで位置がずれることを防ぐことができる。

＜複数フレームの統合＞
次に、本実施形態に係る自己運動推定システム１００は、以下のようにして過去Nフレームの距離画像を用いて距離画像Ｐｔを強化することで、よりロバストに運動推定を行うことも可能である。

つまり、距離画像取得部１１１は、距離センサ２００から新たな距離画像Ｐｔを取得した際に、過去に取得した距離画像Ｐｔ−ｋ（k=1,…,N）と、過去の距離画像Ｐｔ−ｋを取得した時点から現在までの移動体１０００の推定移動量と、を用いて、過去の距離画像Ｐｔ−ｋに含まれる３Ｄ点の位置を現在の距離センサ２００の座標系に変換し、変換後の３Ｄ点を新たな距離画像Ｐｔに追加するようにする。

このような態様によって、よいランドマークがなく位置合せの手がかりとなる３Ｄ点が少ししかとれない環境の中で、３Ｄ点群の解像度が低い距離センサ２００を用いるような場合であっても、複数フレームを統合することにより、そのような３Ｄ点を増やすことが可能となる。

そのため、時刻ｔ-１のフレームの３Ｄ点群だけでなく、過去のいくつかの３Ｄ点群を統合して用いる。

これにより距離画像Ｐｔの点密度が高くなり、ランドマークが小さかったり、遠方で疎な場合でも、そこに当たる３Ｄ点の数が増えて、ＩＣＰの誤差を小さくしたり、極値を回避する可能性が高くなる。

このために、式（７）のようにして、過去Ｎフレームの３Ｄ点群を統合した局所地図（過去の３Ｄ点を追加した距離画像Ｐ^Ｎ _ｔ-１）を生成し、これを用いてＩＣＰを行う。まず、世界座標系での局所地図Ｐ’^Ｎ _ｔ-１を、普通の地図と同様に３Ｄ点群を移動体１０００の姿勢に沿って配置して生成する。

さらに、式（９）のようにして、Ｐ’^Ｎ _ｔ-１を時刻ｔ−１の距離センサ２００の座標系に変換し、ＩＣＰを行う。

本実施形態ではＮ＝５としているが、この限りではなく、移動体１０００の速度などを考慮して決めればよい。

以上、本実施形態に係る自己運動推定システム１００について説明したが、本実施形態に係る自己運動推定システム１００によれば、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定することが可能となる。

例えば、本実施形態に係る自己運動推定システム１００は、３Ｄ特徴（距離画像Ｐｔから抽出されるエッジ点などの特徴点）を抽出することなく、３Ｄ点群と画像エッジ点群の位置合せによって運動推定する。

このため、例えば低密度・低速な３Ｄレーザスキャナ（距離センサ２００）を用いた場合には利用できる３Ｄ点が少なくなるが、このような場合であっても、位置合せの精度を低下させずに済む。

また本実施形態に係る自己運動推定システム１００は、レーザスキャナ（距離センサ２００）とカメラ（画像センサ３００）を組合せて用いているが、このことは、取得周期の観点からも重要である。

つまり、機械駆動の３Ｄスキャナでは、カメラほど取得周期を短くすることは一般的に困難である。そのため、仮に、３Ｄ点群だけを用いて制御を行う場合には、取得周期が長いとフレーム間の移動距離が増えてしまい、データの対応づけが難しくなる。
とくに、移動体１０００が単調な環境を走行する場合のように、目立ったランドマークが少なく、また、繰り返し構造があるような場合に顕著である。フレーム間の移動距離が大きいほど予測誤差は大きくなり、はずれる可能性も高くなる。

しかしながら、本実施形態に係る自己運動推定システム１００は、レーザスキャナ（距離センサ２００）に加えて、カメラ（画像センサ３００）を組合せて用いることにより、取得周期を短く保ち、正確に位置合せをすることが可能である。さらに本実施形態に係る自己運動推定システム１００は、移動量の予測を取り入れることで、より正確な位置合わせを行うこともできる。

本実施形態に係る自己運動推定システム１００は、３Ｄレーザスキャナ（距離センサ２００）とカメラ（画像センサ３００）を用いて、３次元６自由度の運動推定を行い、３Ｄ 地図を構築する。この自己運動推定システム１００では、まず、同一フレームにおいて３Ｄレーザスキャナの３Ｄ点群と画像エッジ点群を対応づけてＲＧＢＤ特徴ペアを生成し、次に、時刻ｔ-１のＲＧＢＤ特徴ペアと時刻ｔの画像エッジ点を対応づけて、画像空間でＩＣＰを行うことにより、ロボット（移動体１０００）の移動量を求める。さらに、移動量の予測値によって探索範囲を狭め、また、複数フレームの点群を統合して点群密度を増やすことで、移動物体や単調環境・繰り返し構造による移動量推定の障害に対処することもできる。

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

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

また移動体１０００が、複数の画像センサ３００を備える場合には、明暗画像取得部１２１は、これらの画像センサ３００により撮影された明暗画像をそれぞれ保持するようにしても良い。

例えば移動体１０００が第１画像センサ３００Ａ及び第２画像センサ３００Ｂの２つの画像センサ３００を備える場合には、明暗画像取得部１２１は、第１画像センサ３００Ａにより撮影された第１撮影範囲の第１明暗画像と、第２画像センサ３００Ｂにより撮影された第２撮影範囲の第２明暗画像と、を持つようにする。

この場合エッジ検出部１２２は、第１明暗画像及び第２明暗画像それぞれから第１エッジ画像Ｅ１ｔ及び第２エッジ画像Ｅ２ｔを生成し、特徴ペア生成部１３１は、距離画像Ｐｔの各３Ｄ点ｐ_ｔ，ｉを、第１画像センサ３００Ａと距離センサ２００との相対位置で決まる変換行列T_ｌｃ１及び第２画像センサ３００Ｂと距離センサ２００との相対位置で決まる変換行列T_ｌｃ２に基づいて、これらのエッジ画像Ｅ１ｔ及びＥ２ｔに透視投影することで、対応する画素ｑ_ｔ，ｉを求め、ＲＧＢＤ特徴ペアの集合Ｃｔを求めるようにする。さらに、移動量算出部１４１は、距離画像Ｐｔ−１の各３Ｄ点ｐ_{ｔ−１，ｉ}を、エッジ画像Ｅ１ｔ及びＥ２ｔに透視投影する際にこれらの変換行列T_ｌｃ１及びT_ｌｃ２を用いるようにする。この際、式（３）および式（６）は、エッジ画像Ｅ１ｔ及びＥ２ｔに対して計算した値を合計すればよい。

このような態様によって、自己運動推定システム１００は、より広範囲の外界の存在物の位置変化に基づいて自己の運動状態を推定することができるため、運動状態の推定精度を向上させることが可能となる。

１００ 自己運動推定システム
１１１ 距離画像取得部
１１２ 距離測定データ記憶部
１２１ 明暗画像取得部
１２２ エッジ検出部
１３１ 特徴ペア生成部
１３２ 特徴ペア記憶部
１４１ 移動量算出部
１４２ 移動量記憶部
１５０ ３次元地図生成部
２００ 距離センサ
３００ 画像センサ
４００ 運動制御部
５００ アクチュエータ
１０００ 移動体

Claims (8)

1. 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより、前記移動体の運動状態を推定する自己運動推定システムであって、
   前記移動体に搭載される画像センサによって所定周期で撮影される外界の明暗画像を取得する明暗画像取得部と、
   前記移動体に搭載される距離センサによって所定周期で計測される、前記画像センサの撮影範囲に含まれる外界の各点までの複数の距離情報を有する距離画像を取得する距離画像取得部と、
   前記明暗画像からエッジを検出するエッジ検出部と、
   第１タイミングで取得した前記距離画像と前記明暗画像とを照合し、前記明暗画像におけるエッジの位置に対応する前記距離画像内の距離情報を、エッジ位置距離情報として特定するエッジ位置距離情報特定部と、
   前記第１タイミングにおける前記エッジ位置距離情報と、前記第１タイミングから第２タイミングまでの間の所定時間における前記移動体の推定移動量と、を用いて算出される、前記エッジ位置距離情報の前記第２タイミングにおける明暗画像内での透視変換による投影位置が、前記第２タイミングで取得される前記明暗画像から検出されるエッジの位置に近づくように、前記移動体の前記推定移動量を算出する移動量算出部と、
   を備えることを特徴とする自己運動推定システム。
2. 請求項１に記載の自己運動推定システムであって、
   前記明暗画像取得部は、第１時間毎に前記明暗画像を取得し、
   前記距離画像取得部は、前記第１時間の２以上の整数倍の第２時間毎に前記距離画像を取得し、
   前記移動量算出部は、前記明暗画像及び前記距離画像の双方を取得するタイミングを前記第１タイミングとすると共に、新たな前記明暗画像を取得する前記第１時間毎に到来するタイミングを前記第２タイミングとして、前記第１時間毎に、前記推定移動量を算出し、
   前記エッジ位置距離情報特定部は、前記明暗画像取得部が前記明暗画像を取得する前記第１時間毎に、前記第１タイミングで取得した前記距離画像と、前記第１タイミングから前記第２タイミングまでの前記推定移動量の累積値とを用いて算出される、前記距離画像の透視変換による前記第２タイミングにおける投影位置と、前記第２タイミングにおける前記明暗画像と、を照合し、前記明暗画像におけるエッジの位置に対応する前記距離画像内の距離情報を、エッジ位置距離情報として特定する
   ことを特徴とする自己運動推定システム。
3. 請求項１又は２に記載の自己運動推定システムであって、
   前記明暗画像には、解像度が相対的に低い第１明暗画像と、解像度が相対的に高い第２明暗画像と、が含まれ、
   前記エッジ検出部は、前記第１明暗画像及び前記第２明暗画像のそれぞれについてエッジを検出し、
   前記エッジ位置距離情報特定部は、前記第１タイミングで取得した前記距離画像と前記第１明暗画像とを照合し、前記第１明暗画像におけるエッジの位置に対応する前記距離画像内の距離情報を第１エッジ位置距離情報として特定すると共に、前記第１タイミングで取得した前記距離画像と前記第２明暗画像とを照合し、前記第２明暗画像におけるエッジの位置に対応する前記距離画像内の距離情報を第２エッジ位置距離情報として特定し、
   前記移動量算出部は、前記第１タイミングにおける前記第１エッジ位置距離情報と、前記第１タイミングから前記第２タイミングまでの前記所定時間における前記移動体の推定移動量と、を用いて算出される、前記第１エッジ位置距離情報の前記第２タイミングにおける明暗画像内での透視変換による投影位置が、前記第２タイミングで取得される第１明暗画像におけるエッジの位置に近づくように、前記推定移動量を算出した後に、前記算出した推定移動量を初期値として、前記第１タイミングにおける前記第２エッジ位置距離情報と、前記第１タイミングから前記第２タイミングまでの前記所定時間における前記移動体の推定移動量と、を用いて算出される、前記第２エッジ位置距離情報の前記第２タイミングにおける明暗画像内での透視変換による投影位置が、前記第２タイミングで取得される第２明暗画像におけるエッジの位置に近づくように、前記推定移動量を算出する
   ことを特徴とする自己運動推定システム。
4. 請求項１〜３のいずれかに記載の自己運動推定システムであって、
   前記移動量算出部は、前記所定時間毎に前記推定移動量を算出する際に、直近に算出した前記移動体の推定移動量を予測値として用いる
   ことを特徴とする自己運動推定システム。
5. 請求項１〜４のいずれかに記載の自己運動推定システムであって、
   前記距離画像取得部は、前記距離センサから新たな距離画像を取得した際に、過去に取得した距離画像と、前記過去の距離画像を取得した時点から現在までの前記移動体の推定移動量と、を用いて、前記過去の距離画像に含まれる距離情報の位置を現在の位置に投影し、投影後の前記距離情報を前記新たな距離画像に追加する
   ことを特徴とする自己運動推定システム。
6. 請求項１〜５のいずれかに記載の自己運動推定システムであって、
   前記画像センサは、第１撮影範囲の撮影を行う第１画像センサと、第２撮影範囲の撮影を行う第２画像センサと、を有して構成され、
   前記明暗画像取得部が取得する前記明暗画像には、前記第１画像センサにより撮影された第１明暗画像と、前記第２画像センサにより撮影された第２明暗画像と、が含まれる
   ことを特徴とする自己運動推定システム。
7. 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより、前記移動体の運動状態を推定する自己運動推定システムの制御方法であって、
   前記自己運動推定システムは、前記移動体に搭載される画像センサによって所定周期で撮影される外界の明暗画像を取得し、
   前記自己運動推定システムは、前記移動体に搭載される距離センサによって所定周期で計測される、前記画像センサの撮影範囲に含まれる外界の各点までの複数の距離情報を有する距離画像を取得し、
   前記自己運動推定システムは、前記明暗画像からエッジを検出し、
   前記自己運動推定システムは、第１タイミングで取得した前記距離画像と前記明暗画像とを照合し、前記明暗画像におけるエッジの位置に対応する前記距離画像内の距離情報を、エッジ位置距離情報として特定し、
   前記自己運動推定システムは、前記第１タイミングにおける前記エッジ位置距離情報と、前記第１タイミングから第２タイミングまでの間の所定時間における前記移動体の推定移動量と、を用いて算出される、前記エッジ位置距離情報の前記第２タイミングにおける明暗画像内での透視変換による投影位置が、前記第２タイミングで取得される前記明暗画像から検出されるエッジの位置に近づくように、前記移動体の前記推定移動量を算出する
   ことを特徴とする自己運動推定システムの制御方法。
8. 移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより、前記移動体の運動状態を推定するコンピュータからなる自己運動推定システムに、
   前記移動体に搭載される画像センサによって所定周期で撮影される外界の明暗画像を取得する手順と、
   前記移動体に搭載される距離センサによって所定周期で計測される、前記画像センサの撮影範囲に含まれる外界の各点までの複数の距離情報を有する距離画像を取得する手順と、
   前記明暗画像からエッジを検出する手順と、
   第１タイミングで取得した前記距離画像と前記明暗画像とを照合し、前記明暗画像におけるエッジの位置に対応する前記距離画像内の距離情報を、エッジ位置距離情報として特定する手順と、
   前記第１タイミングにおける前記エッジ位置距離情報と、前記第１タイミングから第２タイミングまでの間の所定時間における前記移動体の推定移動量と、を用いて算出される、前記エッジ位置距離情報の前記第２タイミングにおける明暗画像内での透視変換による投影位置が、前記第２タイミングで取得される前記明暗画像から検出されるエッジの位置に近づくように、前記移動体の前記推定移動量を算出する手順と、
   を実行させるためのプログラム。