移動体動き算出装置
【課題】カメラが設置された移動体について、カメラ画像を用いてその動きを求める技術において、対応点がたとえ大きな誤差を含む場合であっても、精度良く、移動体動きを算出可能にする。
【解決手段】移動体動き算出装置１００において、対応点算出部１０１はカメラ１２０によって撮影された画像間の対応点を算出する。第１の動き算出部１０２は画像内に所定の平面を仮定して、対応点を用いて移動体の第１の動きＭａを算出する。第２の動き算出部１０３は第１の動きＭａと対応点を用いて、第２の動きＭｂを算出する。
【解決手段】移動体動き算出装置１００において、対応点算出部１０１はカメラ１２０によって撮影された画像間の対応点を算出する。第１の動き算出部１０２は画像内に所定の平面を仮定して、対応点を用いて移動体の第１の動きＭａを算出する。第２の動き算出部１０３は第１の動きＭａと対応点を用いて、第２の動きＭｂを算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の動きを算出する技術に関し、特に移動体に設置されたカメラで撮影した画像を用いて、３次元空間での移動体自身の動きを算出する技術に関するものである。
【背景技術】
【０００２】
車両や自走ロボットなどの移動体の動きを算出する主な方法として、車輪の回転や操舵輪の角度、加速度、角加速度などを検出するセンサを用いる方法と、移動体に設置されたカメラで撮影した移動体周辺の画像を用いる方法との２つがある。
【０００３】
車輪の回転や操舵輪の角度を検出するセンサを用いる方法は、平坦で乾いた路面などの一定の環境下では誤差が小さく、検出不能となる場合も少ないため、安定であるという利点がある。しかし、路面と車輪が滑る、路面が平面ではない、といった環境下では、原理的に動きの誤差が大きくなるという欠点がある。
【０００４】
また、加速度や角加速度を検出するセンサを用いる方法では、原理的には、路面と車輪が滑る場合や路面が平面ではない場合でも、移動体の動きを算出可能である。しかし、重力加速度に対して動きや姿勢の変化による加速度や角加速度の比率が小さい、センサで発生する雑音やドリフトが比較的大きい、などの理由により、特に路面が水平な平面でない場合や移動体が傾いている場合に、動きの誤差が大きくなるという欠点がある。
【０００５】
一方、移動体に設置されたカメラで撮影した移動体周辺の画像を用いる方法の場合、路面が平面でない場合や移動体が傾いている場合でも、原理的には、移動体の動きを正しく算出することが可能である。
【０００６】
カメラの動きを、撮影した動画像を用いて求める代表的な方法として、オプティカルフローを用いる方法が提案されている。オプティカルフローは、被写体上のある１点に対応する、時間的に連続した２つの画像上の点すなわち対応点を結ぶベクトルである。対応点とカメラ動きとの間には、幾何学的な拘束式が成り立つため、複数の対応点が一定の条件を満たす場合にカメラの動きを算出することができる。
【０００７】
例えば、非特許文献１では、２画像間の静止した８点以上の対応点の組からカメラ動きを算出する、いわゆる８点法と呼ばれる方法が提案されている。しかしながら、この８点法では、対応点の算出結果に誤差が含まれるために、８点以上の誤差の小さい対応点を抽出することが困難であるという課題や、カメラ動きの導出式の不安定性から、対応点の誤差が比較的小さくてもカメラ動きの誤差が大きくなることがあるという課題があることが、広く知られている。
【０００８】
また、８点法に限らず、実際に撮影された画像を用いてカメラの動きを算出する場合、画像処理の過程で発生する、はずれ値様の大きな誤差や、画像中の雑音や量子化誤差などの計測誤差を０にするのは、実質的に不可能といってよい。
【０００９】
そのため、画像を用いてカメラ動きを算出する方法では、誤差の影響を抑制することを目的として、さまざまな方法が提案されている。例えば、対応点間の拘束条件を与えることによってより少ない対応点からカメラ動きを算出する方法や、カメラ動きの算出過程における未知パラメタ数を減らしてカメラ動きを算出する方法、未知パラメタを多段階に算出する方法、カルマンフィルタなどのフィルタを用いる方法、などがある。
【００１０】
例えば、非特許文献２および特許文献１では、入力となる対応点は全て同一平面上に存在するものと仮定した上で、４点以上の対応点からカメラ動きと平面の式を算出する方法が提案されている。この方法は、複数の対応点がある拘束条件を満たすものと仮定して、８点より少ない対応点からカメラ動きを算出する方法の例である。
【００１１】
カメラ動きの算出に用いる対応点の数が少ないということは、対応点がある確率で誤差を含む場合に、算出したカメラ動きが誤差を含む確率が低くなるという効果が期待できる。さらに、４点より多くの対応点を用いた場合には、二乗誤差を最小化する方法でカメラ動きを算出することによって、対応点に含まれる計測誤差の影響を低減できるという効果が期待できる。
【００１２】
また例えば、特許文献２では、路面平面の対応点を選択して算出されたカメラ動きに対して、車両挙動やカメラ配置を反映したカルマンフィルタをかけることによって、誤差の小さいカメラ動きを算出する方法が提案されている。算出したカメラ動きにフィルタ処理をかけることによって、カメラ動き算出における誤差を低減することができる。
【００１３】
また例えば、特許文献３および非特許文献３では、カメラの動きを平面上の平行移動と回転の３パラメタに限定した上で、対応点とカメラ動き間の幾何学的拘束式と、対応点の近傍の小領域間の画素値に関する光学的拘束式を用いて、探索的な手法によってカメラの動きを算出する方法が提案されている。カメラの動きを限定して未知パラメタを減らすことによって、カメラ動き算出における誤差を低減することができる。
【００１４】
また例えば、非特許文献４では、対応点からカメラ動きを算出する際の未知パラメタのうち、回転と平行移動とを２段階に分けて算出することで、１回の処理で求める未知パラメタを少なくし、カメラ動き算出における誤差を低減する方法が提案されている。
【特許文献１】国際公開第９７／３５１６１号パンフレット
【特許文献２】特開２００１−２６６１６０号公報
【特許文献３】特表２００３−５１５８２７号公報
【非特許文献１】“A computer algorithm for reconstructing a scene from two projections.”, H. C. Longuet-Higgins, Nature, 293:133-135, Sept. 1981
【非特許文献２】“画像理解”金谷健一、森北出版、１９９０年
【非特許文献３】“A Robust Method for Computing Vehicle Ego-motion”, G.P. Stein, O. Mano and A. Shashua, IEEE Intelligent Vehicles Symposium (IV2000),Oct. 2000
【非特許文献４】“Recovery of Ego-Motion Using Image Stabilization”, M. Irani, B. Rousso and S. Peleg, ICVPR, pp454-460, March, 1994
【発明の開示】
【発明が解決しようとする課題】
【００１５】
しかしながら、上述した従来の方法では、それぞれ次のような問題があった。
【００１６】
まず、対応点の拘束条件を与えることによって、より少ない対応点からカメラ動きを算出する方法では、多数の対応点の中から、特定の拘束条件を満たす対応点を抽出する必要がある。しかし、この対応点を抽出する過程における誤りを０にすることは困難であり、誤って抽出された対応点からカメラ動きを算出した場合には、カメラ動きの誤差が大きくなるという別の課題がある。例えば、金谷の方法（非特許文献２）は、平面上の４点以上の対応点からカメラの動きを算出可能であるが、画像中の複数の対応点から同一平面上に存在する静止した対応点を抽出する必要がある。しかし、移動体に設置したカメラで撮影された画像に適用する場合には、同一平面上に存在する静止した対応点を誤りなく抽出するのは困難である。このため、対応点を抽出する過程における誤りによってカメラ動きの誤差が大きくなってしまう、という別の問題が生じる。
【００１７】
また、フィルタ処理は、カメラ動きの算出結果に含まれる誤差の影響を低減する方法として有効である。しかし、カメラ動きや誤差の原因が予め仮定したモデルに従うことが前提であるため、例えば、車両が低速で走行する場合、路面の凹凸や段差などの予測困難な外乱の影響が大きくなるため、フィルタそのものの設計が困難である。このため、フィルタのパラメタが適当でない場合に、逆に誤差が大きくなってしまうという問題がある。
【００１８】
また、未知パラメタを減らす方法では、実際のカメラの動きが未知パラメタを減らした関係式と近い場合は、誤差が小さい動きを算出できる。しかし、実際のカメラの動きが未知パラメタを減らした関係式から外れた場合、誤差が大きくなってしまう。例えば、Steinらの方法（特許文献３および非特許文献３）では、カメラは特定の平面上しか移動しないと仮定して、平面上の平行移動２パラメタと平面上の回転１パラメタの計３パラメタのみを算出している。このため、カメラが傾いた場合や路面が平面でない場合に、カメラ動きの誤差が大きくなるという問題がある。
【００１９】
さらに、未知パラメタを２段階に算出する方法では、各段階で適切な未知パラメタが選択されている場合、有効である。しかし、パラメタの選択が適当でない場合、各段での推定において、未知パラメタを減らす方法と同様に、誤差が大きくなる。例えば、Iraniらの方法（非特許文献４）は、回転と平行移動を分けて２段階で求めているため、例えば、回転と平行移動のいずれの成分も無視できない程度に含まれる場合に、各段の推定を正しく求めることができない場合がある。
【００２０】
前記の問題に鑑み、本発明は、カメラが設置された移動体について、カメラ画像を用いてその動きを求める技術において、対応点（オプティカルフロー）を用い、対応点がたとえ大きな誤差を含む場合であっても、精度良く、移動体動きを算出可能にすることを課題とする。
【課題を解決するための手段】
【００２１】
本発明は、カメラが設置された移動体について、カメラによって撮影された移動体周囲の画像を用いて動きを求める際に、撮影時刻が異なる２枚のカメラ画像から複数の対応点を求め、カメラ画像内に所定の平面を仮定した上で、求めた複数の対応点を用いて、移動体の第１の動きを求め、この第１の動きと複数の対応点とを用いて、移動体の第２の動きを求めるものである。
【００２２】
本発明によると、第１の動きを求める際に、所定の平面を仮定し、一部の対応点が所定の平面上に存在するという拘束条件を与えることによって、未知パラメタを減らすことができ、平面に対するおおまかな、大きな誤差のない第１の動きを求めることができる。そして、この第１の動きを用いることによって、例えば平面が実際には傾いていたような場合であっても、誤差が小さい第２の動きを求めることができる。
【００２３】
また、前記第１の動きの算出は、複数の対応点の中から、ｍ（ｍは３以上の整数）個の対応点で構成される部分対応点をｑ（ｑは２以上の整数）組選択し、選択したｑ組の部分対応点からそれぞれ、平面を表す平面式を用いて、移動体の動き候補を算出し、算出したｑ個の動き候補を所定の評価手法によって評価し、この評価結果に従って、前記第１の動きを特定するのが好ましい。
【００２４】
これにより、対応点に含まれるはずれ値様の大きな誤差に影響されないで、誤差の小さい移動体の第１の動きを算出することができる。
【００２５】
また、前記第２の動きの算出は、第１の動きを初期値として、探索手法によって、複数の対応点に対する評価を行いつつ、第２の動きを求めるのが好ましい。これにより、より精度の高い、移動体の第２の動きを算出することができる。
【発明の効果】
【００２６】
本発明によると、２段階の動き算出ステップを含み、平面を仮定して第１の動きを算出した後に、この第１の動きを用いて第２の動きを算出するので、例えば、移動体と路面が傾いた状況であっても、移動体の動きを精度良く算出することができる。
【発明を実施するための最良の形態】
【００２７】
本発明の第１態様では、カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて動きを求める装置として、撮影時刻が異なる２枚の前記画像から複数の対応点を求める対応点算出部と、前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて前記移動体の動きを示す第１の動きを求める第１の動き算出部と、前記第１の動き算出部によって求められた第１の動きと前記複数の対応点とを用いて、前記移動体の動きを示す第２の動きを求める第２の動き算出部とを備え、前記第１の動き算出部は、前記対応点を３個用いて前記第１の動きを求めるものを提供する。
【００２８】
本発明の第２態様では、カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて動きを求める装置として、撮影時刻が異なる２枚の前記画像から複数の対応点を求める対応点算出部と、前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて前記移動体の動きを示す第１の動きを求める第１の動き算出部と、前記第１の動き算出部によって求められた第１の動きと前記複数の対応点とを用いて、前記移動体の動きを示す第２の動きを求める第２の動き算出部とを備え、前記第１の動き算出部は、前記複数の対応点の中からｍ（ｍは３以上の整数）個の対応点で構成される部分対応点をｑ（ｑは２以上の整数）組選択し、選択したｑ組の部分対応点からそれぞれ、前記平面を表す平面式を用いて前記移動体の動き候補を算出し、算出したｑ個の動き候補を所定の評価手法によって評価し、この評価結果に従って前記第１の動きを特定するものを提供する。
【００２９】
本発明の第３態様では、前記移動体は車両であり、前記第２の動き算出部は、前記第１の動きの代わりに、この第１の動きに対応する部分対応点と前記平面を表す平面式を用い、前記平面式を初期値として、探索手法によって、前記カメラに近い車軸中心を中心として平面を傾けながら、前記複数の対応点に対する評価を行いつつ、前記第２の動きを求めるものである第２態様の移動体動き算出装置を提供する。
【００３０】
以下、本発明の実施の形態について、図面を参照して説明する。
【００３１】
（第１の実施形態）
本発明の第１の実施形態では、移動体として、カメラが設置された車両を例にとり、カメラによって撮影された車両後方の画像を用いて、車両の動きを求めるものとする。
【００３２】
図１は本実施形態における状況を示す図である。図１に示すように、移動体としての車両１には、その後部に、後方周辺を撮影するようにカメラ１２０が設置されている。カメラ１２０は車両後方周辺を撮影して一連の画像列を出力する。
【００３３】
図２は本実施形態に係る移動体動き算出装置を含む構成を示すブロック図である。図２において、１００はカメラ１２０によって撮影された画像から車両１の動きを求める移動体動き算出装置、１２１はカメラ１２０によって撮影された画像に、移動体動き算出装置１００によって求められた車両１の動きに関する情報を重畳した合成画像を生成する画像合成装置、１２２は画像合成装置１２１によって生成された合成画像を表示するディスプレイである。
【００３４】
移動体動き算出装置１００は、カメラ１２０から出力された連続する２画像間から対応点（オプティカルフローベクトル）を複数個求める対応点算出部１０１と、対応点算出部１０１から出力された複数の対応点を用いて、平面を仮定して、車両１の動きを示す第１の動きＭａを求める第１の動き算出部１０２と、対応点算出部１０１から出力された複数の対応点と、第１の動き算出部１０２から出力された第１の動きＭａとを用いて、車両１の動きを示す第２の動きＭｂを求める第２の動き算出部１０３とを備えている。
【００３５】
図２の移動体動き算出装置１００、画像合成装置１２１およびディスプレイ１２２は、例えば、車両１内に設置されるが、車両１とは異なる場所に設けることも可能である。
【００３６】
図３は図２の移動体動き算出装置１００における第１の動き算出部１０２の詳細な構成を示すブロック図である。図３において、１１１は対応点算出部１０１から出力された複数の対応点から、所定数の対応点からなる部分対応点を複数組選択する部分対応点選択部、１１２は部分対応点選択部１１１によって選択された各組の部分対応点から、車両１の動き候補をそれぞれ算出する動き候補算出部、１１３は動き候補算出部１１２によって算出された動き候補を所定の評価手法によって評価し、この評価結果に従って、第１の動きＭａを特定する動き評価選択部である。
【００３７】
また、１１４は第１の動きＭａを算出する際に仮定する所定の平面を表す平面式を出力する平面式算出部、１１５は静止した対応点の比率である静止対応点率Ｒを出力する静止対応点率算出部、１１６は移動体動きの予測値を求める移動体動き予測部である。
【００３８】
ここでは、図１の状況において、車両１が後進した場合を例にとって、説明を行う。
【００３９】
図１において、世界座標系（Ｘｗ，Ｙｗ，Ｚｗ）は３次元空間中で静止しており、車両座標系（Ｘｃ，Ｙｃ，Ｚｃ）および視点座標系（Ｘ，Ｙ，Ｚ）は車両１に固定されているものとする。視点座標系の座標値（ｘ，ｙ，ｚ）と車両座標系の座標値（ｘｃ，ｙｃ，ｚｃ）との関係は、座標系間の位置関係を表す行列Ｃを用いて、（数１）のように表すことができる。
【数１】
【００４０】
世界座標系に対して静止した点が、車両座標系における車両の動きＭｃによって、車両座標系の座標値（ｘｃ，ｙｃ，ｚｃ）から（ｘｃ’，ｙｃ’，ｚｃ’）に移動した場合、（数２）の関係式が成り立つ。さらに、このときの視点座標系の動きがＭであり、同じ点の視点座標系の座標値が（ｘ，ｙ，ｚ）から（ｘ’，ｙ’，ｚ’）に移動したとすると、（数３）の関係式が成り立つ。したがって、車両１の動きＭｃとカメラ１２０の動きＭとの間には（数４）の関係式が成り立つ。
【数２】
【数３】
【数４】
【００４１】
以上のことから、車両座標系と視点座標系の位置関係を示すＣが既知のとき、車両座標系を基準にした車両１の動きＭｃを求めるためには、視点座標系を基準にしたカメラ１２０の動きＭを算出して、（数４）で変換すればよい。カメラ１２０と車両１との相対位置が固定されている場合、カメラ１２０の動きと車両１の動き（移動体の動き）とは一意に変換できるため、本願明細書では、特に明示がない限り、区別はしないものとする。
【００４２】
図４（ａ），（ｂ）は図１の状況において、車両１が後方に移動した場合にカメラ１２０から出力される一連の画像列の例であり、図４（ａ）は時刻ｔ−１に撮影された画像、図４（ｂ）は時刻ｔに撮影された画像である。また、図４（ｃ）は、時刻ｔ−１と時刻ｔの２画像間の同一点の動きである対応点（図中の矢印）を示す図である。
【００４３】
図５はカメラ１２０の３次元的な動きと画像変化との関係を示す図である。図５（ａ）に示すように、カメラ１２０が時刻ｔ−１から時刻ｔの間に動きＭで移動したとき、世界座標系における静止点Ｘが、視点座標系の座標値（ｘ，ｙ，ｚ）から（ｘ’，ｙ’，ｚ’）に移動したものとする。このとき、座標値（ｘ，ｙ，ｚ）、（ｘ’，ｙ’，ｚ’）とカメラ動きＭとの間には、（数３）が成り立つ。図５（ｂ）（ｃ）はそれぞれ時刻ｔ−１，ｔにおける画像であり、時刻ｔ−１において視点座標（ｘ，ｙ，ｚ）の点Ｘが画像座標（ｕ，ｖ）に、時刻ｔにおいて視点座標（ｘ’，ｙ’，ｚ’）の点Ｘが画像座標（ｕ’，ｖ’）に、それぞれ対応している。このとき（ｕ，ｖ）と（ｘ，ｙ，ｚ）、（ｕ’，ｖ’）と（ｘ’，ｙ’，ｚ’）の関係と（数３）を用いると、（数５）の関係式が成り立つ。（数５）において、ｆはカメラの焦点距離を示す。
【数５】
【００４４】
このとき、静止点Ｘ（ｘ，ｙ，ｚ）が（数６）の平面式で表される平面上に存在し、かつ、カメラ１２０の動きＭ（ｔｘ，ｔｙ，ｔｚ，ｗｘ，ｗｙ，ｗｚ）が十分に小さく（数７）が成り立つとき、点Ｘの画像座標（ｕ，ｖ）、（ｕ’，ｖ’）とカメラ動きＭとの間に（数８）の関係式が成り立つ。さらに、（数８）をカメラ動きＭの要素に着目して整理すると、（数９）が得られる。１組の対応点ごとに（数９）の２つの式が成り立つことから、平面式とカメラ１２０の焦点距離ｆが既知の場合は、独立した（同一直線上ではない）対応点（ｕ，ｖ），（ｕ’，ｖ’）が３組以上与えられれば、カメラ１２０の動きＭ（ｔｘ，ｔｙ，ｔｚ，ｗｘ，ｗｙ，ｗｚ）の６変数を算出することができる。
【数６】
【数７】
【数８】
【数９】
【００４５】
以下、時刻ｔ−１から時刻ｔの間のカメラ動きＭを算出する場合を例にとって、説明を行う。なお、上述したように、カメラの動きと移動体の動きは（数４）により一意に変換できるため、カメラ動きが算出できれば、移動体の動きも実質的に算出されたことになる。
【００４６】
ここでは、車両座標系（Ｘｃ，Ｙｃ，Ｚｃ）および視点座標系（Ｘ，Ｙ，Ｚ）は図１のように配置されているものとし、車両座標系に対する視点座標系の位置関係を表すＣは、予め計測されているものとする。また、カメラ１２０の焦点距離ｆも予め求められており、その値は、実座標系での焦点距離に、実座標と画像座標との変換係数を乗じたものとする。
【００４７】
対応点算出部１０１はカメラ１２０から出力された一連の画像列を一時的に保存し、画像列のうちの連続した２枚の画像から、対応点をｎ個（ｎは２以上の整数）検出する。ここでは、時刻ｔ−１の画像上のｎ点の座標（ｕｉ，ｖｉ）（ｉ＝１，…，ｎ）について、対応する、時刻ｔの画像上の座標（ｕｉ’，ｖｉ’）（ｉ＝１，…，ｎ）をそれぞれ検出する。検出したｎ点の対応点の情報を、Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）（ｉ＝１，…，ｎ）と表す。対応点の個数ｎは、予め定められた値とする。
【００４８】
なお、対応点を検出する２枚の画像は、必ずしも連続している必要はなく、撮影時刻が異なっていればよい。
【００４９】
ここでの対応点検出は、例えば、Kanade, Lucas and Tomasiの対応点探索方法によって行えばよい（例えば、“Detection and Tracking of Point Features”, Carlo Tomasi and Takeo Kanade, Carnegie Mellon University Technical Report, CMU-CS-91-132, April 1991.を参照）。この方法は、一般に広く知られた方法であるので、ここでは詳細な説明を省略する。
【００５０】
第１の動き算出部１０２は、対応点算出部１０１によって検出されたｎ個の対応点Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）（ｉ＝１，…，ｎ）を用いて、車両１の第１の動きＭａを算出する。以下、第１の動き算出部１０２を構成する各処理手段の動作について説明する。
【００５１】
平面式算出部１１４は、ここでは所定の平面式を出力する。ここで用いる平面式は、図１の状況における路面を平面として近似したものとする。例えば、車両が静止した状態で、水平かつ平坦な路面平面の式を予め計測しておき、平面式算出部１１４は、予め計測した（数６）の平面式のパラメタＳ（ａ，ｂ，ｃ）を出力する。なお、平面式算出部１１４は、平面式を更新する機能も有する。これについては、後述する。
【００５２】
静止対応点率算出部１１５は、静止対応点率Ｒを出力する。ここで、静止対応点率とは、好ましくは、対応点算出部１０１によって算出されたｎ個の対応点のうち、世界座標に対して静止しており、かつ、対応関係が正しい対応点の比率を表すものである。ここで、対応点の対応関係が正しいとは、対応点の座標値の誤差が所定の閾値以下であることを意味する。なお、実時間でのオンライン処理において、静止対応点率Ｒを事前に正確に得ることは極めて困難であり、実際には、静止対応点率Ｒとは、上述した比率の予測値とする。
【００５３】
ここでは、静止対応点率算出部１１５は、静止対応点率Ｒとして、所定の値、例えば０．５を出力する。静止対応点率Ｒ＝０．５とは、「対応点算出部１０１によって算出されたｎ個の対応点に、静止した、対応関係の正しい対応点が５０％含まれる」と予測していることを意味する。所定の値としては、例えば、図１と類似の走行状態において静止対応点率を予め複数回計測し、計測した中の最低値を用いればよい。なお、静止対応点率算出部１１５は、静止対応点率Ｒを新たに算出する機能も有する。これについては、後述する。
【００５４】
移動体動き予測部１１６は、車両１の動きの予測値を求める機能を有するが、ここでは、特に処理を行わないものとする。
【００５５】
部分対応点選択部１１１は、対応点算出部１０１から出力されたｎ個の対応点Ｐｉ、平面式算出部１１４から出力された平面式Ｓ、および静止対応点率算出部１１５から出力された静止対応点率Ｒを用いて、ｍ個の対応点で構成される部分対応点をｑ組選択する。ここで、部分対応点を構成する対応点の個数ｍは、移動体の動きを算出するために必要となる対応点の数である。（数９）を用いてカメラ動きを算出するためには、最低３個の対応点が必要である。このため、ここでは、ｍ＝３とする。もちろん、ｍは３より大きい値であってもかまわない。
【００５６】
部分対応点選択部１１１は、まず、部分対応点の組数ｑを算出する。部分対応点の組数ｑは、その値が大きいほど、第１の動き算出部１０２での処理時間が長くなり、逆にｑが小さいほど処理時間は小さくなる。そのため、例えば処理時間の制約がある場合は、その上限からｑを決定しても良い。
【００５７】
ここでは、部分対応点の組数ｑを（数１０）の関係式から求めるものとする。
【数１０】
（数１０）において、Ｒｒは路面静止対応点率、ｚは保証率である。
【００５８】
路面静止対応点率Ｒｒは、部分対応点として選択された各対応点が、路面上に存在し、世界座標に対して静止しており、かつ対応関係が正しい対応点の確率である。したがって、対応点が路面上に存在する確率Ｒｓと、対応点の静止対応点率Ｒとの間に相関がない場合、対応点が路面上に存在する確率Ｒｓに静止対応点率Ｒを掛けたものが路面静止対応点率Ｒｒとなる。また、もしも路面上に存在する対応点を正確に選択することが可能であり、かつ、選択した対応点の静止対応点率がＲである場合、選択した対応点の路面対応点率Ｒｒは、静止対応点率Ｒと等しくなる。ここでは、所定の値として、例えば、Ｒｒ＝０．３を用いる。所定の値としては、例えば図１と類似の走行状態において、選択後の対応点が路面上に存在する確率Ｒｓを計測しておき、これと静止対応点率Ｒをかけた値を用いればよい。
【００５９】
保証率ｚは、ｑ組の部分対応点のうち、全て（ｍ個）の対応点が路面上の静止した正しい対応点で構成された部分対応点が少なくとも１組存在することを統計的に保証する率である。ここでは、所定の値として、例えば、ｚ＝０．９９９を用いる。言い換えると、少なくとも１組の部分対応点が路面上の正しい対応点で構成されることを、保証率ｚで保証するためには、ｑ組の部分対応点を選択すればよい。
【００６０】
（数１０）において、ｑとＲｒが一定の場合、部分対応点を構成する対応点数ｍが小さいほど保証率ｚが大きくなり、ｚとＲｒが一定の場合は、ｍが小さいほどｑが小さくなる。したがって、対応点の数ｍは小さいほど、処理時間が短く、部分対応点の少なくとも１組が路面上の正しい対応点で構成される可能性が高くなる。このことと、（数９）の算出には３点以上の対応点数が必要であることから、最良のｍは３となる。
【００６１】
次に、部分対応点選択部１１１は、算出した組数ｑを用いて、ｎ個の対応点から、異なるｍ個の対応点からなる部分対応点をｑ組選択して出力する。部分対応点を選択する方法として、ｎ個の全ての対応点からランダムに選択してもよいし、特定の条件を満たす対応点を抽出した上で、そこからランダムに選択するとしてもよい。ここでは後者を用いる。すなわち、平面式Ｓが存在し得る画像領域内の対応点を抽出した上で、抽出した対応点から、ランダムに、異なるｍ個の対応点からなる部分対応点をｑ組選択して出力する。例えば、図４（ｄ）において、画像中で平面Ｓが存在し得る領域は、水平線より下の斜線領域に限られることを示している。この方法では、斜線領域に存在する対応点を抽出した上で部分対応点を選択する場合の路面静止対応点率Ｒｒは、ｎ個の対応点からランダムに選択する場合の路面静止対応点率Ｒｒに比べて、より高い値となり、その結果、部分対応点の組数ｑをより小さい値にすることができる。
【００６２】
動き候補算出部１１２は、部分対応点選択部１１１によって選択されたｑ組の部分対応点から、それぞれ、平面式Ｓを用いて、ｑ個の動き候補を算出する。
【００６３】
上述したように、画像中の対応点Ｐｉ（ｕ，ｖ，ｕ’，ｖ’）が平面式Ｓ（ａ，ｂ，ｃ）上にあり、かつ、カメラの動きＭが小さいと仮定した場合、（数９）の関係式が成り立つ。（数９）において、対応点と平面式が既知、カメラの動きＭを未知とすると、対応点が３個以上あればカメラの動きＭを算出することができる。そこで、動き候補算出部１１２は、３個の対応点からなるｑ組の部分対応点について、それぞれ、（数９）の関係式を用いて、ｑ個のカメラの動きＭｊ（ｊ＝１，…，ｑ）を算出し、これを車両１の動き候補として出力する。
【００６４】
動き評価選択部１１３は、動き候補算出部１１２によって算出されたｑ個の動き候補（カメラ動きＭｊ（ｊ＝１，…，ｑ））を、所定の評価手法によって評価し、この評価結果に従って、第１の動きＭａを特定する。ここでの評価では、対応点算出部１０１から出力されたｎ個の対応点Ｐｉ、および静止対応点率算出部１１５から出力された静止対応点率Ｒを用いる。なお、必要に応じて、移動体動き予測部１１６から出力された動き予測を用いてもよい。これについては、後述する。
【００６５】
ここでは、まず、カメラ動きＭｊとｎ個の対応点Ｐｉとの一致する度合いを評価する。具体的には、それぞれのカメラ動きＭｊについて、ｎ個の対応点（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）（ｉ＝１，…，ｎ）を（数１１）に代入し、ｎ個の対応点の３次元座標（ｘｉ，ｙｉ，ｚｉ）を算出する。ここで、（数１１）は未知数４に対して独立した一次式が４つあること、および、画像から算出した対応点Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）は誤差を含むと考えられることから、最小二乗法を用いて３次元座標（ｘｉ，ｙｉ，ｚｉ）を算出する。
【数１１】
【００６６】
さらに、算出した３次元座標（ｘｉ，ｙｉ，ｚｉ）とカメラ動きＭｊを（数１１）に再び代入して、新たに対応点（ｕｉｊ，ｖｉｊ，ｕｉｊ’，ｖｉｊ’）を算出する。ここで、対応点Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）およびカメラ動きＭｊが誤差のない正確な値である場合、Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）と（ｕｉｊ，ｖｉｊ，ｕｉｊ’，ｖｉｊ’）は一致し、誤差がある場合は一致しない。そこで、（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）と（ｕｉｊ，ｖｉｊ，ｕｉｊ’，ｖｉｊ’）の誤差距離平均Ｅｊｉを、（数１２）から算出する。Ｅｊｉの値は、カメラ動きＭｊと対応点Ｐｉが一致するときは、０になり、一致しないときは、値が大きくなるという性質をもつことから、Ｅｊｉをカメラ動きＭｊと対応点Ｐｉの一致度合いに関する評価値として用いる。
【数１２】
【００６７】
次に、ｎ個の対応点Ｐｉそれぞれに対するカメラ動きＭｊの評価値Ｅｊｉ（ｉ＝１，…，ｎ）のうち、評価の良い方すなわち値の小さい方から（ｎ×Ｒ）番目の評価値を、カメラ動きＭｊの評価値Ｅｊとして選択する。
【００６８】
そして、ｑ個のカメラ動きＭｊ（ｊ＝１，…，ｑ）の中で、それぞれの評価値Ｅｊ（ｊ＝１，…，ｑ）のうち評価が最も良い、すなわち値が最も小さいものに対応するカメラ動きを、第１の動きＭａとして選択する。
【００６９】
図６はｎ個の対応点に対するある１つのカメラ動きＭｊの評価値Ｅｊｉ（ｉ＝１，…，ｎ）の分布を概念的に示すグラフである。図６では、評価値Ｅｊｉを小さい順にソートしており、横軸はソート後の対応点番号（１，…，ｎ）、縦軸は各対応点に対するカメラ動きＭｊの評価値Ｅｊｉの値である。
【００７０】
ここで、カメラ動きＭｊが正しく、かつ、ｎ個の対応点のうち（ｎ×Ｒ）個が静止した正しい対応点であるとすると、理想的には図６（ａ）のように、小さい方から（ｎ×Ｒ）番目までの評価値Ｅｉｊは０で、それ以降は次第に値が大きくなる。このため、評価値Ｅｊも０となる。しかし、実際には対応点の計測誤差などの小さな誤差が存在するため、（ｎ×Ｒ）番目までの評価値Ｅｉｊは必ずしも０にはならない。
【００７１】
一方、カメラ動きＭｊが正しくない場合は、図６（ｂ）のように、評価値Ｅｊｉの多くが大きな値となり、評価値Ｅｊも大きな値となる。このことを言い換えると、静止対応点率Ｒを前提条件とした場合、カメラ動きＭｊの評価値Ｅｊが小さいということは前提条件を満たすことを意味し、さらに、評価値Ｅｊが最小となるカメラ動き（すなわち第１の動きＭａ）は、前提条件を最も良く満たすカメラ動きであることを意味する。
【００７２】
このような動作によって、第１の動き算出部１０２は、時刻ｔ−１と時刻ｔの画像上のｎ個の対応点Ｐｉを用いて、車両１の動きである第１の動きＭａを算出して出力する。
【００７３】
第２の動き算出部１０３は、第１の動き算出部１０２から出力された第１の動きＭａと、対応点算出部１０１から出力されたｎ個の対応点Ｐｉとを用いて、車両１の動きを示す第２の動きＭｂを算出する。ここでは、第１の動きＭａを初期値とした探索的手法によって、第２の動きＭｂを算出するものとする。
【００７４】
具体的には、まず、第１の動きＭａ（ｔｘ，ｔｙ，ｔｚ，ｗｘ，ｗｙ，ｗｚ）を初期値として、各パラメータに対して所定の微小動き（ｄｔｘ，ｄｔｙ，ｄｔｚ，ｄｗｘ，ｄｗｙ，ｄｗｚ）をそれぞれ増減して、複数の動き候補を得る。これにより、ｏ＝３＾６個の動き候補Ｍｂｋ＝（ｔｘ±ｄｔｘ，ｔｙ±ｄｔｙ，ｔｚ±ｄｔｚ，ｗｘ±ｄｗｘ，ｗｙ±ｄｗｙ，ｗｚ±ｄｗｚ）（ｋ＝１，…，ｏ）が得られる。
【００７５】
次に、第１の動き算出部１０２の動き評価選択部１１３と同様に、ｏ個の動き候補Ｍｂｋについて、それぞれ、ｎ個の対応点に対する評価値Ｅｂｋｉ（ｉ＝１，…，ｎ）を算出する。そして、小さい方から（ｎ×Ｒ）番目の評価値Ｅｂｋｉを動き候補Ｍｂｋの評価値Ｅｂｋとし、さらに、評価値Ｅｂｋが最小となる動き候補Ｍｂｋを選択する。
【００７６】
次いで、選択された動き候補Ｍｂｋを初期値として、上と同様の処理を行う。これを繰り返し実行し、値が収束したとき、または、繰り返し回数が所定回数に達したとき、処理を終了する。そして、第２の動き算出部１０３は、そのとき選択されている動き候補Ｍｂｋを第２の動きＭｂとして出力する。
【００７７】
このような動作により、移動体動き算出装置１００は、時刻ｔ−１から時刻ｔの間の車両１の動きを算出する。また、カメラ１２０から出力された画像列について、同様の処理を繰り返すことによって、車両１の動きを連続して算出することができる。
【００７８】
そして、画像合成装置１２１は、カメラ１２０によって撮影された画像と、移動体動き算出装置１００から出力された第２の動きＭｂを入力して、車両１の動きに関する情報が重畳された合成画像を生成する。ディスプレイ１２２は画像合成装置１２１によって生成された合成画像を表示する。図７は表示された合成画像の例であり、車両１の第２の動きＭｂに対応する予測軌跡ＴＲが、カメラ１２０で撮影された画像に重畳されている。
【００７９】
＜本実施形態の効果＞
本実施形態によると、例えば、対応点探索の誤りが多く、大きな誤差を含む場合であっても、車両の動きを精度良く算出することができる。
【００８０】
対応点算出部１０１の動作説明で触れたKanade, Lucas and Tomasiの方法は、高い精度と少ない計算量の両面で優れた対応点探索方法として広く知られている。しかし、そのような優れた方法であっても、制約の無い環境で撮影された画像において対応点探索の誤りを０にすることは、まず不可能と言ってよい。
【００８１】
図８（ａ）は実際に車両の後方にカメラを設置して撮影した画像の例である。図８（ａ）のシーンで撮影した動画像について、上述した手法で対応点探索を行った。図８（ｂ）はそのときの対応点正解率と車両の移動速度との関係を示すグラフである。ここでは、画像座標上の誤差が１画素以内であった対応点の比率を対応点正解率としている。なお、図８（ｂ）のグラフでは、時間と移動速度の値には、適当なスケールが掛けられている。
【００８２】
図８（ｂ）からも分かるように、実際に撮影された画像を用いた場合、精度が良いとされている手法であっても、対応点正解率が５０％程度にまで低下する場合がある。また、はずれ値の様な大きな誤差を持つ対応点が多数含まれる場合もある。本願発明者らは、このような場合が稀ではないことを、他の実験結果からも確認している。
【００８３】
そして、上述したような従来の方法では、多数の誤りを排除する仕組みを持たないことから、例えば、対応点探索の正解率が大きく低下してしまった場合、精度の高い動き算出は、到底期待できない。
【００８４】
これに対して本実施形態では、たとえ対応点正解率が大きく低下したような場合であっても、第１の動き算出部１０２において、第１の動きＭａを精度良く算出することができる。すなわち、本実施形態の方法の場合、ｑ組の部分対応点の中に、正しい部分対応点が少なくとも１組含まれていれば、ｑ個の動き候補Ｍｊの中に少なくとも１つの正しい動きが含まれることになる。さらに、対応点探索の正解率に相当する静止対応点率Ｒを用いて、ｑ個の動きＭｊを、ｎ個の対応点に対する評価値のうち良い方から（ｎ×Ｒ）番目の評価値を用いて評価することによって、正しい動きＭａを算出することを可能にしている。
【００８５】
また、本実施形態によると、例えば路面が傾いている場合であっても、車両の動きを精度良く算出することができる。
【００８６】
本実施形態では、車両が静止した状態で計測して得た水平かつ平坦な路面平面の式を、所定の平面式として用いて、第１の動きＭａを算出するものとした。しかし、坂道や昇降路などの路面が傾いている場合や、乗員や荷物の積載、またはハンドル操作などによって車両の姿勢が変わった場合は、予め計測した所定の平面式は、実際に車両が走行している路面平面と一致しなくなる。
【００８７】
図９（ａ）は車両が傾いた場合の画像例である。車両が傾くと、画像内の路面も傾くため、予め計測した所定の平面式と一致しなくなる。この場合、選択された部分対応点の３個の対応点が、路面上にありかつ対応関係が正しいものであったとしても、（数９）を用いて算出された動きＭｊは誤差を含む。このため、図９（ｂ）に示すように、ｎ個の対応点に対する動きＭｊの評価値Ｅｊｉは、平面式が正しい場合に比べて少しずつ大きな値となり、評価値Ｅｊも大きな値となる。したがって、評価値が最も小さい第１の動きＭａであっても、誤差を含むことになる。
【００８８】
ここで、評価値が図９（ｂ）のようになる場合に、従来技術であるはずれ値除去方法を適用し、評価値が小さい一部の対応点を抽出して、(数９）を用いた最小二乗法によって動きを算出したとしても、誤差を小さくすることは困難である。
【００８９】
これに対して本実施形態では、第１の動きＭａを初期値として、小さい方からｎ×Ｒ個の評価値を基準とした探索的手法を用いて、第２の動きＭｂを算出する。一般に、初期値から真の値（または最良の値）までの間で、その評価関数が滑らかであれば、探索的手法によって真の値を得ることが可能である。そして、本実施形態で用いた評価値は、真の値の近傍であれば上記条件を満たす。このため、たとえ、実際の路面が所定の平面式と一致しなくなったために第１の動きＭａが誤差を含んでいる場合であっても、第２の動き算出部１０３によって、誤差の小さい第２の動きＭｂを算出することができる。
【００９０】
また、本実施形態において、平面式を仮定することの妥当性と効果について、実際の車両の走行環境において発生する問題を基にして、説明する。
【００９１】
車両周辺をカメラで監視するシステムには、次のような特徴がある。すなわち、運転者が車両周辺を監視できるように、典型的には図４のように、車両近傍の路面から水平線より上の障害物までが画像に映るように、広視野角のカメラが設置される。このように設置されたカメラの画像には、ほとんど全てのシーンにおいて路面が映ることが期待できる。さらに、仮に路面が連続した平面ではない場合であっても、多くの被写体が路面平面の延長上に存在する。例えば水平線近傍の遠方の被写体は、実際には路面に接していなくても、路面と同一の平面式上に存在すると仮定することができる。このようなケースが多いことが、本願発明者らによる実験でも確認できた。このことから、車両周辺を監視するシステムにおいて、路面を表す平面式を仮定して動きを求める方法は、妥当といえる。
【００９２】
また、平面式を仮定した場合、（数９）を用いて、３個の対応点から車両の動きを算出できる。これに対して、平面式を仮定しない方法では、動き（または動き候補）を求めるために、少なくとも４個以上の対応点が必要となり、このため、算出過程において誤差の影響を受けやすい。これに対して、本実施形態のように平面式を仮定することによって、誤差の影響を回避できるという効果が得られる。
【００９３】
さらに、単眼カメラの画像を用いてカメラの動きを算出する場合、スケール項を求めるために、画像中に大きさが既知の被写体が必要となる。本実施形態では、平面式を仮定することによって、画像中に大きさが既知の被写体が存在しなくても、スケールを考慮したカメラ動きを算出できるという効果もある。
【００９４】
さらに、本実施形態のように平面式を仮定することによって、画像中に他の移動体が存在する場合であっても、移動体動きを正しく算出できるという効果も得られる。
【００９５】
図１０（ａ）は自車両１の後方に他の移動車両２が存在している状況を示す図である。図１０（ａ）では、自車両１は後方（図中右）に、他車両２は前方（図中左）にそれぞれ移動している。このとき、図１０（ｂ）のような画像がカメラ１２０によって撮影される。
【００９６】
ここで、対応点が、路面と他車両２上のそれぞれに、５０％ずつ算出されたとする（対応関係の誤りはないものとする）。このような状況に、従来の方法を適用すると、自車両１について、路面に対する動きと、他車両２に対する動きとの２つが算出される可能性があるため、これら２つの動きを判別する手段が別途必要になる。さらに２つの動きを判別する手段において誤りが発生すれば、自車両１の動きは誤差の大きな動きとなる。
【００９７】
これに対して、本実施例では、路面平面の式を仮定して自車両１の動きを算出する。このため、第１の動き算出部１０２において、路面平面から離れた他車両２上の対応点から得られた自車両１の動きＭｊは、大きな誤差を含むこととなり、その評価値Ｅｊは大きくなる。この結果、第１の動き算出部１０２において、他車両２上の対応点から得られた動きが選択されることはなく、路面上の対応点から得られた路面平面に対する動きが、第１の動きＭａとして選択される。以上のことから、本実施形態は、画像中に他の移動物が存在する場合であっても、移動体の動きを正しく算出できるという効果があると言える。
【００９８】
以上説明したように、平面式を仮定して動きを算出することによって、実際の車両の走行環境で発生する、対応点探索過程で発生する誤差、路面や車両の傾き、他の移動物の存在などといった問題に対しても、移動体動きを精度良く算出することができる、という効果が得られる。
【００９９】
＜平面式の更新＞
平面式算出部１１４は、過去の画像から第１の動き算出部１０２によって求められた第１の動きＭａを用いて、平面式を更新することも可能である。
【０１００】
例えば、平面式算出部１１４は、時刻ｔ＝１において、予め得られた所定の初期平面式（ａ０，ｂ０，ｃ０）を出力する。第１の動き算出部１０２は、この初期平面式（ａ０，ｂ０，ｃ０）を用いて時刻ｔ＝０とｔ＝１間の第１の動きＭａを求める。次に、時刻ｔ＝２において、平面式算出部１１４は、時刻ｔ＝０とｔ＝１間の第１の動きＭａを用いて、移動体が初期平面式（ａ０，ｂ０，ｃ０）に対して第１の動きＭａだけ動いたものとして、新たな平面式（ａ１，ｂ１，ｃ１）を算出して出力する。これ以降、同様の処理を順次繰り返すことによって、平面式は更新されていく。
【０１０１】
このように、過去に算出した移動体動きから新たに平面式を算出し、次の動き算出に用いることによって、より正確な平面の位置に基づいた動き算出を実行することができる。したがって、移動体動きをより精度良く算出することができる。
【０１０２】
なお、平面式の更新には、第１の動きＭａに代えて、第２の動きＭｂを用いてもよいし、またその両方を用いてもかまわない。
【０１０３】
また、平面式の更新は、必ずしも、動き算出を実行するたびに行う必要はない。例えば、動き算出を所定回数実行する毎に行ってもよいし、路面や車両の状況に変化が生じたときに行うようにしてもよい。
【０１０４】
＜静止対応点率の更新＞
静止対応点率算出部１１５は、所定の静止対応点率を出力する代わりに、対応点算出部１０１から出力された複数の対応点と、過去の画像から第１の動き算出部１０２によって算出された第１の動きＭａとを用いて、静止対応点率を算出することも可能である。
【０１０５】
例えば、静止対応点率算出部１１５は、第１の動き算出部１０２によって算出された第１の動きＭａについて、ｎ個の対応点に対して、上述したような（数１１）および（数１２）を用いた評価を行う。そして、ｎ個の対応点のうち評価値Ｅｉが所定の閾値以下であるものの比率を算出し、これを新たな静止対応点率として保持する。そして、次の動き算出を実行するときに、保持していた静止対応点率を出力する。これ以降、同様の処理を順次繰り返すことによって、静止対応点率は更新されていく。
【０１０６】
ここで、動き算出に用いられた静止対応点率が真の静止対応点率と異なる場合、算出される動きは、真の動きに対する誤差を含むことになる。この静止対応点率Ｒと真の静止対応点率Ｒtrueおよびカメラ動きの誤差ＥＭとの関係を図１１のグラフに示す。図１１のグラフは本願発明者らが行った実験の結果から、その共通の傾向を模式的に表したものである。図１１のグラフからも分かるように、静止対応点率Ｒと真の静止対応点率Ｒtrueが一致しているとき（比Ｒ／Ｒtrueが１のとき）、カメラ動きの誤差ＥＭが最も小さくなり、静止対応点率Ｒが真の静止対応点率Ｒtrueに対して大きくなった場合でも小さくなった場合でも、カメラ動きの算出精度は低下する。
【０１０７】
一方、動き算出の繰り返しの周期が比較的短い場合、車両周辺の画像の変化は小さく、したがって、静止対応点率の変化も小さいと考えられる。このため、時刻ｔ−１において得られた動きを用いて算出した静止対応点率Ｒ（ｔ−１）は、時刻ｔの画像における真の静止対応点率Ｒtrue（ｔ）に近いと予想される。したがって、静止対応点率を更新していくことによって、誤差の小さい動きを算出することが可能になる。
【０１０８】
なお、時刻ｔにおいて、時刻ｔ−１で算出した静止対応点率Ｒ（ｔ−１）を用いて移動体動きを算出した結果、この移動体動きから求めた静止対応点率Ｒ（ｔ）が静止対応点率Ｒ（ｔ−１）を下回った場合は、時刻ｔの移動体動きを、新たな静止対応点率Ｒ（ｔ）を用いて再計算するようにしてもよい。
【０１０９】
図１１からわかるように、静止対応点率Ｒが真の静止対応点率Ｒｔｒｕｅよりも小さい場合は、カメラ動きの誤差はあまり大きくならないのに対して、静止対応点率Ｒが真の静止対応点率Ｒｔｒｕｅよりも大きい場合は、カメラ動きの誤差は極端に大きくなる。したがって、Ｒ（ｔ）＜Ｒ（ｔ−１）の場合、静止対応点率Ｒ（ｔ−１）を用いて算出したカメラ動きは大きな誤差を含んでいる可能性がある。このことから、Ｒ（ｔ）＜Ｒ（ｔ−１）の場合に、新たに得られたＲ（ｔ）を用いて移動体動きを再計算することによって、より精度の高い動き算出が可能になる。
【０１１０】
なお、静止対応点率の更新には、第１の動きＭａに代えて、第２の動きＭｂを用いてもよいし、またその両方を用いてもかまわない。
【０１１１】
＜移動体動きの予測値の利用＞
移動体動き予測部１１６は、第１の動き算出部１０２によって過去に求められた第１の動きＭａを用いて、車両１の動きを予測し、その動き予測値を出力することができる。この場合、動き評価選択部１１３は、移動体動き予測部１１６から出力された動き予測値を加味して、第１の動きＭａを算出する。
【０１１２】
ここでは例えば、移動体動き予測部１１６は、車両１の動きはほぼ等速・等角速度運動であると仮定して、予測を行うものとする。すなわち、時刻ｔ−１に算出された第１の動きＭａを保存しておき、時刻ｔの動き予測値Ｍｒｔとして出力する。動き評価選択部１１３は、動き候補算出部１１２から入力したｑ個のカメラ動きＭｊ（ｊ＝１，…，ｑ）とともに、動き予測値Ｍｒｔも併せた（ｑ＋１）個のカメラ動きに対して、上述したように（数１１）（数１２）を用いて評価を行う。そして、最も評価値の良いものを、時刻ｔにおける第１の動きＭａとして選択する。
【０１１３】
上述したように、部分対応点の組数ｑと保証率ｚとの間には（数１０）の関係があり、静止対応点率Ｒ＜１、かつ、ｑが有限である限り、保証率ｚは１未満となる。つまり、ｑ組の部分対応点から第１の動きＭａを求める場合、ある確率で、誤差が大きい動きが算出される。
【０１１４】
これに対して、動き予測値Ｍｒｔも併せて評価を行い、第１の動きＭａを選択することによって、ｑ組の部分対応点からは正しい動きが得られないような場合でも、車両１の動きが等速・等角速度運動に近い場合は、動き予測値Ｍｒｔが第１の動きＭａとして選択される。この結果、第１の動きＭａに大きな誤差が生じるケースを回避することができる。
【０１１５】
なお、ここでは、移動体が等速・等角速度運動していると仮定して動き予測を行うものとしたが、動き予測の方法はこれに限られるものではない。例えば、移動体が等加速・等角加速度運動をしていると仮定して動き予測を行ってもよく、この場合でも上と同様に、第１の動きＭａに大きな誤差が生じるケースを回避することができる。
【０１１６】
なお、移動体の動き予測には、第１の動きＭａに代えて、第２の動きＭｂを用いてもよいし、またその両方を用いてもかまわない。
【０１１７】
以上のように本実施形態によると、対応点が存在する所定の平面を仮定することによって、未知パラメタを減らすことができることから、大きな誤差の少ない第１の動きＭａを求めることができる。さらに、第１の動きＭａを初期値とした探索的手法を用いることによって、誤差の小さい第２の動きＭｂを求めることができる。
【０１１８】
なお、本実施形態では、動き候補の評価のために、（数１１）と（数１２）に表された評価値Ｅｊｉを用いたが、評価手法はこれに限定されるものではなく、対応点がカメラ動きに一致するか否かを評価する値であればどのような評価値を用いてもかまわない。例えば、エピポーラ拘束や基礎行列と呼ばれる、カメラ間の位置（動き）と対応点との関係を示す評価値を用いてもよい。（数１３）はその具体例である。
【数１３】
【０１１９】
なお、本実施形態では、カメラ動きＭｊについて、ｎ個の対応点に対する評価値Ｅｊｉのうち小さい方から（ｎ×Ｒ）番目の評価値を、評価値Ｅｊとして選択するものとしたが、評価値Ｅｊの特定方法はこれに限定されるものではない。例えば、評価値Ｅｊｉの小さい方から（ｎ×Ｒ）番目までの評価値の平均値を、評価値Ｅｊとしてもよい。
【０１２０】
また、静止対応点率Ｒの値に応じて、評価値Ｅｊの特定方法を変えてもよい。例えば、静止対応点率Ｒが所定の閾値よりも大きい場合は、評価値Ｅｊｉのうちの小さい方から（ｎ×Ｒ）番目の評価値をＥｊとし、一方、静止対応点率Ｒが所定の閾値よりも小さい場合は、評価値Ｅｊｉのうちの小さい方から（ｎ×Ｒ）番目までの評価値の平均値をＥｊとするようにしてもよい。
【０１２１】
この方法によると、静止対応点率Ｒが比較的小さいときは、複数の評価値の平均値をとることによって、評価値に含まれる誤差の影響を抑制することができる。その一方で、静止対応点率Ｒが比較的高いときは、１個の評価値を評価値Ｅｊとして採用することによって、（ｎ×Ｒ）番目近辺の微小な差異が平均化されずに、評価値Ｅｊに反映される、という効果が得られる。
【０１２２】
なお、本実施形態において、第２の動きＭｂの算出を、第１の動きＭａを初期値とした探索的手法によって、行うものとしたが、このような方法に限定されるものではない。例えば、第１の動き算出部１０２が、第１の動きＭａの代わりに、第１の動きＭａの算出元となった部分対応点と平面式を、第２の動き算出部１０３に出力するものとする。そして第２の動き算出部１０３は、この部分対応点および平面式と、対応点算出部１０１から出力されたｎ個の対応点とを用いて、別の探索的手法により第２の動きＭｂを算出するとしてもよい。別の探索的手法として、例えば、平面式Ｓ（ａ，ｂ，ｃ）を初期値として、平面式の各パラメタに対して所定の微小変化（ｄａ，ｄｂ，ｄｃ）をそれぞれ増減した、ｏ＝３＾３個の平面式Ｓｋ（ａ±ｄａ，ｂ±ｄｂ，ｃ±ｄｃ）（ｋ＝１，・・・，ｏ）を算出する。次に、第１の動きＭａの算出元となった部分対応点とｏ個の平面式Ｓｋから、（数９）を用いてカメラ動き候補Ｍｂｋ（ｋ＝１，・・・，ｏ）を算出する。そして、前述した手法によって、ｎ個の対応点に対する動き候補Ｍｂｋの評価値Ｅｂｋを算出し、評価値Ｅｂｋが最小となる動き候補Ｍｂｋを選択する。さらに、選択されたＭｂｋに対応する平面式Ｓｋを初期値として、上述した処理を繰り返し実行し、値が収束したとき、または繰り返し回数が所定回数に達したとき、処理を終了する。そして、そのとき選択されている動き候補Ｍｂｋを第２の動きＭｂとして出力する。この方法は、第１の動きＭａに含まれる誤差の主要因が、平面式Ｓと実際の路面との差である場合に、実際の路面に近い平面式Ｓを元にして動きを再計算することが期待できるため、誤差の小さい動きを算出できると考えられる。また、この方法を用いた場合、探索空間を狭くできるため、処理が軽くなるというメリットが得られる。
【０１２３】
また、平面式Ｓ（ａ，ｂ，ｃ）のパラメタのうち、ｃはスケール項であることから、ｃに対する探索は効果がない場合がある。このような場合は、ｏ＝３＾３個の平面式Ｓｋ（ａ±ｄａ，ｂ±ｄｂ，ｃ±ｄｃ）（ｋ＝１，・・・，ｏ）用いる代わりに、ｏ＝３＾２個の平面式Ｓｋ（ａ±ｄａ，ｂ±ｄｂ，ｃ）（ｋ＝１，・・・，ｏ）用いてもよい。
【０１２４】
さらに、この方法を用いる場合、視点座標系の平面式Ｓ（ａ，ｂ，ｃ）を初期値として、一定の微小変化をそれぞれ増減した平面式Ｓｋを用いて探索を行う場合は、図１２（ａ）のように、実際の路面の変動とは無関係な平面式を用いた探索となる。これに対して、図１２（ｂ）のように、例えば、カメラ１２０に近い車軸中心（図の場合は後輪軸中心）を基準に平面を傾けた場合の、所定の微小変化（ｄａ，ｄｂ，ｄｃ）を増減した平面式Ｓｋを用いて探索を行う方が、車両と路面との位置変動を反映した探索であると言えることから、より少ない繰り返し回数での動き算出や、より誤差の小さい動き算出が可能になる。このような所定の微小変化としては、例えば、図１２と類似の状態において、さまざまな平面式Ｓとその微小変化（ｄａ，ｄｂ，ｄｃ）をあらかじめ計測しておき、これを用いればよい。
【０１２５】
なお、本実施形態では、移動体動き算出装置１００から出力された第２の動きＭｂを用いて、図７のように車両１の移動軌跡を画像に重畳するものとしたが、算出した移動体動きの利用形態は、これに限られるものではない。
【０１２６】
例えば、画像合成装置１２１は、第１の動きＭａの評価値Ｅｉが小さい方から（ｎ×Ｒ）個までの対応点について、その画像座標と３次元位置を入力する。そして、（ｎ×Ｒ）個の対応点のうち、その３次元位置が移動体の進路上であり、かつ、所定の範囲内にあるものが存在するときは、図１３のような注意メッセージを重畳合成するとともに、該当する対応点の付近を枠で囲むなどの強調表示するようにしてもよい。
【０１２７】
また、本実施形態では、移動体動き算出装置１００の出力は、画像合成装置１２１とディスプレイ１２２に供給されるものとしたが、これは、移動体動き算出装置１００やその出力である移動体動きの用途を限定するものではなく、他のどのような装置と組み合わせて用いてもかまわない。
【０１２８】
なお、本実施形態では、カメラ１２０は車両１の後部に設置されており、車両後方を撮影するものとしたが、カメラの位置や撮影範囲はこれに限定されるものではなく、車両の周辺が撮影されるように設置されていれば、カメラはどのような位置にあってもよい。
【０１２９】
なお、本実施形態では、動き算出のために仮定した平面は路面であったが、仮定する平面は路面に限定されるものではなく、移動体との相対的な位置を予め与えることができる平面であれば、どのような平面であってもかまわない。
【０１３０】
例えば、図１４のような屋内においては、天井面も移動体との相対的な位置を予め与えることができるため、路面の代わりに、天井面を平面として仮定してもよい。また、移動体が壁面と所定の距離を保って移動するような場合には、路面の代わりに、壁面を平面として仮定してもよい。
【０１３１】
なお、本実施形態では、移動体の動きの表現や、移動体の動きを表すための座標系の取り方については、特に制約はない。例えば、垂直軸が車両の非操舵輪の車軸を含む直線と直交するように、車両座標系を置き、車両の路面上の動きを、垂直軸周りの円弧運動として表してもよい。図１５はいわゆるアッカーマンモデルに基づく移動体の動きの表現を示す図である。図１５では、前輪が操舵輪、後輪が車体に車軸が固定された非操舵輪である車両について、非操舵輪の車軸の延長線上に、水平面上での回転成分の中心がおかれている。
【０１３２】
このとき、図１５のように、座標系の垂直軸（Ｙｃ軸）を後輪の中心位置を通るようにおくと、車両の動きのうち（ｔｘｃ，ｔｚｃ，ｗｙｃ）の間に拘束式が成り立つ。このような拘束条件をおくことによって、未知パラメタの個数を減らすことができ、より安定に解を求めることができる。
【０１３３】
（第２の実施形態）
本発明の第２の実施形態では、第１の実施形態と同様に構成された移動体動き算出装置を、ナビゲーション装置と組み合わせて、ナビゲーションシステムを構成する。ここでのナビゲーション装置は、人工衛星からの電波を用いて現在位置を測定する機能と、現在位置と地図情報を用いて現在位置の表示や目的地への誘導表示を行う機能とを有するものとする。
【０１３４】
図１６は本実施形態に係る移動体動き算出装置を含むナビゲーションシステムの構成図である。図１６では、図２および図３と共通の構成要素には同一の符号を付しており、ここではその詳細な説明を省略する。
【０１３５】
図１６において、ナビゲーション装置１３０は、位置情報取得部１３１、位置情報算出部１３２、画像合成部１３３および地図情報記憶部１３４を備えている。位置情報取得部１３１はいわゆるＧＰＳ機能を有しており、複数の人工衛星からの電波を受信して車両の位置に関する情報を取得する。また、位置情報取得部１３１は、衛星からの電波が受信可能であり、ＧＰＳ機能によって位置情報が取得可能な場合（ＧＰＳ機能が有効な場合）は取得した位置情報を出力し、衛星からの電波が受信できないなどの理由によりＧＰＳ機能によって位置情報を取得できない場合（ＧＰＳ機能が無効な場合）は、位置情報が取得不可能であることを示す情報を出力する。位置情報算出部１３２は位置情報取得部１３１からの位置情報と、移動体動き算出装置１００から出力された移動体動き情報（第２の動きＭｂ）とを入力して、車両の現在位置を算出する。地図情報記憶部１３４は地図座標に対応した道路や駐車場、地図の情報を保持している。画像合成部１３３は位置情報算出部１３２から現在位置を入力して、現在位置に対応する道路や地図の情報を地図情報記憶部１３４から読み出し、画像に変換して出力する。
【０１３６】
図１６のナビゲーションシステムの動作について、説明する。いま、車両は屋外にあり、衛星からの電波が受信可能であり、ＧＰＳ機能が有効となっているものとする。このとき、位置情報取得部１３１はＧＰＳ機能によって得られた位置情報を出力し、位置情報算出部１３２は位置情報取得部１３１から受けた位置情報を、そのまま現在位置として出力するとともに、一時的に記憶する。画像合成部１３３は位置情報算出部１３２から現在位置を受け、地図情報記憶部１３４に記憶された現在位置周辺の地図情報を読み出し、地図画像に変換して出力する。この地図画像は、ディスプレイ１２２に表示される。
【０１３７】
また、位置情報取得部１３１がＧＰＳ機能によって位置情報を取得可能であるときは、位置情報算出部１３２は、移動体動き算出装置１０２から入力された移動体動きの情報を無視する。
【０１３８】
次に、車両が運転者の操作によって移動し、屋内駐車場などのような、衛星からの電波が受信できずＧＰＳ機能によって位置情報が取得できない場所に移動したものとする。このとき、位置情報取得部１３１はＧＰＳ機能が無効であることを表す情報を出力する。位置情報算出部１３２はこの情報を入力したとき、内部に記憶していた現在位置に、移動体動き算出装置１００から出力された移動体動きを積算し、新たな現在位置として出力するとともに一時的に記憶する。画像合成部１３３はＧＰＳ機能が有効な場合と同様に動作し、位置情報算出部１３２から現在位置を受け、地図情報記憶部１３４に記憶された現在位置周辺の地図情報を読み出し、地図画像に変換して出力する。この地図画像は、ディスプレイ１２２に表示される。
【０１３９】
また、位置情報算出部１３２は、現在位置を算出するとともに、必要に応じて地図情報記憶部１３４に記憶された駐車場の情報を読み出し、平面式算出部１１４に送る。平面式算出部１１４は、必要に応じて駐車場の情報に含まれる平面式の情報を読み出して出力する。
【０１４０】
さらに、車両が運転者の操作によって移動し、再び電波が受信可能となり、ＧＰＳ機能が有効になったとき、位置情報算出部１３２は、位置情報取得部１３１で得られた位置情報を、現在位置として出力する。
【０１４１】
図１７は屋内駐車場において、本発明に係る移動体動き算出方法を用いた実験により得られた車両の移動軌跡を示すグラフである。上述したように、本発明に係る移動体動き算出方法によると、車両に設置したカメラ画像のみを用いて移動体動きを算出することが可能であり、しかも、路面が傾いている場合であっても誤差を小さく抑えることが可能であることから、図１７に示すように、屋内駐車場においても誤差の小さい移動軌跡の算出が可能となる。
【０１４２】
ところが、図８（ｂ）のグラフを用いてすでに説明したとおり、移動体の移動速度が速くなるほど、対応点探索の誤差が大きくなる（対応点正解率が低下する）傾向があり、対応点の誤差が大きくなりすぎた場合は、移動体動きの誤差が大きくなる。
【０１４３】
このような問題に対して、本実施形態のナビゲーションシステムでは、移動体動き算出装置１００の利用は衛星からの電波が受信できない屋内だけにとどめて、衛星からの電波が受信可能な屋外では、ＧＰＳ機能によって取得した位置情報を現在位置として用いている。これにより、精度の高い現在位置の算出を可能にしている。
【０１４４】
すなわち、本実施形態では、ＧＰＳ機能によって位置情報が取得できるか否かに応じて、現在位置の算出方法を選択的に切り替えることによって、ＧＰＳ機能が有効となる屋外と、電波が受信できない屋内とのいずれにおいても、常に現在位置の算出が可能となる。
【０１４５】
なお、本実施形態では、現在位置の算出方法の切り換えを、ＧＰＳ機能によって現在位置が取得できるか否かに応じて行うものとしたが、これに限られるものではなく、例えば、算出した現在位置が地図情報上で屋内駐車場の入り口近辺であるか否かに応じて、現在位置の算出方法を切り替えるようにしてもよい。
【０１４６】
なお、本実施形態においても、第１の実施形態と同様に、動き算出のために仮定した平面は、路面に限定されるものではなく、移動体との相対的な位置を予め与えることができる平面であれば、どのような平面であってもかまわない。
【０１４７】
例えば、屋内駐車場が図１４のような構造である場合に、その天井を平面と仮定した場合の平面式を、地図情報とともに地図情報記憶部１３４に記憶しておく。そして、位置情報算出部１３２は、現在位置が図１４の屋内駐車場に該当するとき、地図情報記憶部１３４から屋内駐車場の天井の平面式を読み出し、第１の動き算出部１０２の平面式算出部１１４に出力する。平面式算出部１０４は、位置情報算出部１３２から平面式が出力されたときは、その平面式を出力する。これにより、移動体動き算出装置１００は、天井平面を基準とした移動体動きを算出する。
【０１４８】
本発明では、カメラで撮影された画像を用いて移動体の動きを算出するため、例えば、路面が暗く、天井が明るい屋内駐車場では、路面よりも天井を平面として仮定した方が、算出精度が向上することが期待される。したがって、位置情報に対応した平面式を予め地図情報記憶部１３４に記憶しておき、移動体動き算出装置１００が、現在位置に応じて平面式を選択して用いることによって、より精度の高い移動体動きが算出可能となる。
【０１４９】
なお、上述の各実施形態では、移動体として車両を例にとって説明を行ったが、移動体の種類は車両に限られるものではなく、例えば、ロボットであってもよい。
【０１５０】
なお、上述の各実施形態において、各処理手段および装置は、ハードウェアによって実現されてもよいし、ソフトウェアによって実現してもよい。例えば、図１８のような、ＣＰＵ１４１、ＲＯＭ１４２およびＲＡＭ１４３と、画像の入出力機能とを備えたコンピュータを用いて、各処理手段をソフトウェアによって実現してもよい。
【産業上の利用可能性】
【０１５１】
本発明は、カメラが設置された移動体の動きを、カメラ画像を用いて精度良く求めることができるので、例えば、車両や自走ロボットなどに適用し、その走行制御や、操作者に対する進路や位置の提示サービスなどに、利用することが可能である。
【図面の簡単な説明】
【０１５２】
【図１】本発明の第１の実施形態における状況を示す図である。
【図２】本発明の第１の実施形態に係る構成を示すブロック図である。
【図３】図２における第１の動き算出部の詳細な構成を示すブロック図である。
【図４】（ａ），（ｂ）は図１の状況で撮影された画像の例、（ｃ）はその対応点を示す図である。
【図５】カメラの動きと画像変化との関係を示す図である。
【図６】各対応点に対する動きＭｊの評価値Ｅｊｉの分布を概念的に示すグラフである。
【図７】算出した動きに対応する予測軌跡が重畳された合成画像の例である。
【図８】（ａ）は車両に設置したカメラによる実際の画像の例、（ｂ）は（ａ）の状況における車両の移動速度と対応点正解率との関係を示すグラフである。
【図９】（ａ）は車両が傾いた場合の画像例、（ｂ）は（ａ）の状況における各対応点に対する動きＭｊの評価値Ｅｊｉの分布である。
【図１０】（ａ）は車両後方に他の車両が存在している状況を示す図、（ｂ）は（ａ）の状況において撮影された画像の例である。
【図１１】静止対応点率Ｒと真の静止対応点率Ｒtrueとの比（Ｒ／Ｒtrue）とカメラ動きの誤差ＥＭとの関係を示すグラフである。
【図１２】（ａ），（ｂ）は平面パラメタを用いた探索を概念的に示す図である。
【図１３】注意メッセージなどが重畳された合成画像の例である。
【図１４】屋内駐車場における撮影画像の例である。
【図１５】アッカーマンモデルに基づく移動体動きの表現を示す図である。
【図１６】本発明の第２の実施形態に係るナビゲーションシステムの構成図である。
【図１７】屋内駐車場における実験により得られた車両の移動軌跡を示すグラフである。
【図１８】本発明を実施するための他の構成を示す図である。
【技術分野】
【０００１】
本発明は、移動体の動きを算出する技術に関し、特に移動体に設置されたカメラで撮影した画像を用いて、３次元空間での移動体自身の動きを算出する技術に関するものである。
【背景技術】
【０００２】
車両や自走ロボットなどの移動体の動きを算出する主な方法として、車輪の回転や操舵輪の角度、加速度、角加速度などを検出するセンサを用いる方法と、移動体に設置されたカメラで撮影した移動体周辺の画像を用いる方法との２つがある。
【０００３】
車輪の回転や操舵輪の角度を検出するセンサを用いる方法は、平坦で乾いた路面などの一定の環境下では誤差が小さく、検出不能となる場合も少ないため、安定であるという利点がある。しかし、路面と車輪が滑る、路面が平面ではない、といった環境下では、原理的に動きの誤差が大きくなるという欠点がある。
【０００４】
また、加速度や角加速度を検出するセンサを用いる方法では、原理的には、路面と車輪が滑る場合や路面が平面ではない場合でも、移動体の動きを算出可能である。しかし、重力加速度に対して動きや姿勢の変化による加速度や角加速度の比率が小さい、センサで発生する雑音やドリフトが比較的大きい、などの理由により、特に路面が水平な平面でない場合や移動体が傾いている場合に、動きの誤差が大きくなるという欠点がある。
【０００５】
一方、移動体に設置されたカメラで撮影した移動体周辺の画像を用いる方法の場合、路面が平面でない場合や移動体が傾いている場合でも、原理的には、移動体の動きを正しく算出することが可能である。
【０００６】
カメラの動きを、撮影した動画像を用いて求める代表的な方法として、オプティカルフローを用いる方法が提案されている。オプティカルフローは、被写体上のある１点に対応する、時間的に連続した２つの画像上の点すなわち対応点を結ぶベクトルである。対応点とカメラ動きとの間には、幾何学的な拘束式が成り立つため、複数の対応点が一定の条件を満たす場合にカメラの動きを算出することができる。
【０００７】
例えば、非特許文献１では、２画像間の静止した８点以上の対応点の組からカメラ動きを算出する、いわゆる８点法と呼ばれる方法が提案されている。しかしながら、この８点法では、対応点の算出結果に誤差が含まれるために、８点以上の誤差の小さい対応点を抽出することが困難であるという課題や、カメラ動きの導出式の不安定性から、対応点の誤差が比較的小さくてもカメラ動きの誤差が大きくなることがあるという課題があることが、広く知られている。
【０００８】
また、８点法に限らず、実際に撮影された画像を用いてカメラの動きを算出する場合、画像処理の過程で発生する、はずれ値様の大きな誤差や、画像中の雑音や量子化誤差などの計測誤差を０にするのは、実質的に不可能といってよい。
【０００９】
そのため、画像を用いてカメラ動きを算出する方法では、誤差の影響を抑制することを目的として、さまざまな方法が提案されている。例えば、対応点間の拘束条件を与えることによってより少ない対応点からカメラ動きを算出する方法や、カメラ動きの算出過程における未知パラメタ数を減らしてカメラ動きを算出する方法、未知パラメタを多段階に算出する方法、カルマンフィルタなどのフィルタを用いる方法、などがある。
【００１０】
例えば、非特許文献２および特許文献１では、入力となる対応点は全て同一平面上に存在するものと仮定した上で、４点以上の対応点からカメラ動きと平面の式を算出する方法が提案されている。この方法は、複数の対応点がある拘束条件を満たすものと仮定して、８点より少ない対応点からカメラ動きを算出する方法の例である。
【００１１】
カメラ動きの算出に用いる対応点の数が少ないということは、対応点がある確率で誤差を含む場合に、算出したカメラ動きが誤差を含む確率が低くなるという効果が期待できる。さらに、４点より多くの対応点を用いた場合には、二乗誤差を最小化する方法でカメラ動きを算出することによって、対応点に含まれる計測誤差の影響を低減できるという効果が期待できる。
【００１２】
また例えば、特許文献２では、路面平面の対応点を選択して算出されたカメラ動きに対して、車両挙動やカメラ配置を反映したカルマンフィルタをかけることによって、誤差の小さいカメラ動きを算出する方法が提案されている。算出したカメラ動きにフィルタ処理をかけることによって、カメラ動き算出における誤差を低減することができる。
【００１３】
また例えば、特許文献３および非特許文献３では、カメラの動きを平面上の平行移動と回転の３パラメタに限定した上で、対応点とカメラ動き間の幾何学的拘束式と、対応点の近傍の小領域間の画素値に関する光学的拘束式を用いて、探索的な手法によってカメラの動きを算出する方法が提案されている。カメラの動きを限定して未知パラメタを減らすことによって、カメラ動き算出における誤差を低減することができる。
【００１４】
また例えば、非特許文献４では、対応点からカメラ動きを算出する際の未知パラメタのうち、回転と平行移動とを２段階に分けて算出することで、１回の処理で求める未知パラメタを少なくし、カメラ動き算出における誤差を低減する方法が提案されている。
【特許文献１】国際公開第９７／３５１６１号パンフレット
【特許文献２】特開２００１−２６６１６０号公報
【特許文献３】特表２００３−５１５８２７号公報
【非特許文献１】“A computer algorithm for reconstructing a scene from two projections.”, H. C. Longuet-Higgins, Nature, 293:133-135, Sept. 1981
【非特許文献２】“画像理解”金谷健一、森北出版、１９９０年
【非特許文献３】“A Robust Method for Computing Vehicle Ego-motion”, G.P. Stein, O. Mano and A. Shashua, IEEE Intelligent Vehicles Symposium (IV2000),Oct. 2000
【非特許文献４】“Recovery of Ego-Motion Using Image Stabilization”, M. Irani, B. Rousso and S. Peleg, ICVPR, pp454-460, March, 1994
【発明の開示】
【発明が解決しようとする課題】
【００１５】
しかしながら、上述した従来の方法では、それぞれ次のような問題があった。
【００１６】
まず、対応点の拘束条件を与えることによって、より少ない対応点からカメラ動きを算出する方法では、多数の対応点の中から、特定の拘束条件を満たす対応点を抽出する必要がある。しかし、この対応点を抽出する過程における誤りを０にすることは困難であり、誤って抽出された対応点からカメラ動きを算出した場合には、カメラ動きの誤差が大きくなるという別の課題がある。例えば、金谷の方法（非特許文献２）は、平面上の４点以上の対応点からカメラの動きを算出可能であるが、画像中の複数の対応点から同一平面上に存在する静止した対応点を抽出する必要がある。しかし、移動体に設置したカメラで撮影された画像に適用する場合には、同一平面上に存在する静止した対応点を誤りなく抽出するのは困難である。このため、対応点を抽出する過程における誤りによってカメラ動きの誤差が大きくなってしまう、という別の問題が生じる。
【００１７】
また、フィルタ処理は、カメラ動きの算出結果に含まれる誤差の影響を低減する方法として有効である。しかし、カメラ動きや誤差の原因が予め仮定したモデルに従うことが前提であるため、例えば、車両が低速で走行する場合、路面の凹凸や段差などの予測困難な外乱の影響が大きくなるため、フィルタそのものの設計が困難である。このため、フィルタのパラメタが適当でない場合に、逆に誤差が大きくなってしまうという問題がある。
【００１８】
また、未知パラメタを減らす方法では、実際のカメラの動きが未知パラメタを減らした関係式と近い場合は、誤差が小さい動きを算出できる。しかし、実際のカメラの動きが未知パラメタを減らした関係式から外れた場合、誤差が大きくなってしまう。例えば、Steinらの方法（特許文献３および非特許文献３）では、カメラは特定の平面上しか移動しないと仮定して、平面上の平行移動２パラメタと平面上の回転１パラメタの計３パラメタのみを算出している。このため、カメラが傾いた場合や路面が平面でない場合に、カメラ動きの誤差が大きくなるという問題がある。
【００１９】
さらに、未知パラメタを２段階に算出する方法では、各段階で適切な未知パラメタが選択されている場合、有効である。しかし、パラメタの選択が適当でない場合、各段での推定において、未知パラメタを減らす方法と同様に、誤差が大きくなる。例えば、Iraniらの方法（非特許文献４）は、回転と平行移動を分けて２段階で求めているため、例えば、回転と平行移動のいずれの成分も無視できない程度に含まれる場合に、各段の推定を正しく求めることができない場合がある。
【００２０】
前記の問題に鑑み、本発明は、カメラが設置された移動体について、カメラ画像を用いてその動きを求める技術において、対応点（オプティカルフロー）を用い、対応点がたとえ大きな誤差を含む場合であっても、精度良く、移動体動きを算出可能にすることを課題とする。
【課題を解決するための手段】
【００２１】
本発明は、カメラが設置された移動体について、カメラによって撮影された移動体周囲の画像を用いて動きを求める際に、撮影時刻が異なる２枚のカメラ画像から複数の対応点を求め、カメラ画像内に所定の平面を仮定した上で、求めた複数の対応点を用いて、移動体の第１の動きを求め、この第１の動きと複数の対応点とを用いて、移動体の第２の動きを求めるものである。
【００２２】
本発明によると、第１の動きを求める際に、所定の平面を仮定し、一部の対応点が所定の平面上に存在するという拘束条件を与えることによって、未知パラメタを減らすことができ、平面に対するおおまかな、大きな誤差のない第１の動きを求めることができる。そして、この第１の動きを用いることによって、例えば平面が実際には傾いていたような場合であっても、誤差が小さい第２の動きを求めることができる。
【００２３】
また、前記第１の動きの算出は、複数の対応点の中から、ｍ（ｍは３以上の整数）個の対応点で構成される部分対応点をｑ（ｑは２以上の整数）組選択し、選択したｑ組の部分対応点からそれぞれ、平面を表す平面式を用いて、移動体の動き候補を算出し、算出したｑ個の動き候補を所定の評価手法によって評価し、この評価結果に従って、前記第１の動きを特定するのが好ましい。
【００２４】
これにより、対応点に含まれるはずれ値様の大きな誤差に影響されないで、誤差の小さい移動体の第１の動きを算出することができる。
【００２５】
また、前記第２の動きの算出は、第１の動きを初期値として、探索手法によって、複数の対応点に対する評価を行いつつ、第２の動きを求めるのが好ましい。これにより、より精度の高い、移動体の第２の動きを算出することができる。
【発明の効果】
【００２６】
本発明によると、２段階の動き算出ステップを含み、平面を仮定して第１の動きを算出した後に、この第１の動きを用いて第２の動きを算出するので、例えば、移動体と路面が傾いた状況であっても、移動体の動きを精度良く算出することができる。
【発明を実施するための最良の形態】
【００２７】
本発明の第１態様では、カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて動きを求める装置として、撮影時刻が異なる２枚の前記画像から複数の対応点を求める対応点算出部と、前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて前記移動体の動きを示す第１の動きを求める第１の動き算出部と、前記第１の動き算出部によって求められた第１の動きと前記複数の対応点とを用いて、前記移動体の動きを示す第２の動きを求める第２の動き算出部とを備え、前記第１の動き算出部は、前記対応点を３個用いて前記第１の動きを求めるものを提供する。
【００２８】
本発明の第２態様では、カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて動きを求める装置として、撮影時刻が異なる２枚の前記画像から複数の対応点を求める対応点算出部と、前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて前記移動体の動きを示す第１の動きを求める第１の動き算出部と、前記第１の動き算出部によって求められた第１の動きと前記複数の対応点とを用いて、前記移動体の動きを示す第２の動きを求める第２の動き算出部とを備え、前記第１の動き算出部は、前記複数の対応点の中からｍ（ｍは３以上の整数）個の対応点で構成される部分対応点をｑ（ｑは２以上の整数）組選択し、選択したｑ組の部分対応点からそれぞれ、前記平面を表す平面式を用いて前記移動体の動き候補を算出し、算出したｑ個の動き候補を所定の評価手法によって評価し、この評価結果に従って前記第１の動きを特定するものを提供する。
【００２９】
本発明の第３態様では、前記移動体は車両であり、前記第２の動き算出部は、前記第１の動きの代わりに、この第１の動きに対応する部分対応点と前記平面を表す平面式を用い、前記平面式を初期値として、探索手法によって、前記カメラに近い車軸中心を中心として平面を傾けながら、前記複数の対応点に対する評価を行いつつ、前記第２の動きを求めるものである第２態様の移動体動き算出装置を提供する。
【００３０】
以下、本発明の実施の形態について、図面を参照して説明する。
【００３１】
（第１の実施形態）
本発明の第１の実施形態では、移動体として、カメラが設置された車両を例にとり、カメラによって撮影された車両後方の画像を用いて、車両の動きを求めるものとする。
【００３２】
図１は本実施形態における状況を示す図である。図１に示すように、移動体としての車両１には、その後部に、後方周辺を撮影するようにカメラ１２０が設置されている。カメラ１２０は車両後方周辺を撮影して一連の画像列を出力する。
【００３３】
図２は本実施形態に係る移動体動き算出装置を含む構成を示すブロック図である。図２において、１００はカメラ１２０によって撮影された画像から車両１の動きを求める移動体動き算出装置、１２１はカメラ１２０によって撮影された画像に、移動体動き算出装置１００によって求められた車両１の動きに関する情報を重畳した合成画像を生成する画像合成装置、１２２は画像合成装置１２１によって生成された合成画像を表示するディスプレイである。
【００３４】
移動体動き算出装置１００は、カメラ１２０から出力された連続する２画像間から対応点（オプティカルフローベクトル）を複数個求める対応点算出部１０１と、対応点算出部１０１から出力された複数の対応点を用いて、平面を仮定して、車両１の動きを示す第１の動きＭａを求める第１の動き算出部１０２と、対応点算出部１０１から出力された複数の対応点と、第１の動き算出部１０２から出力された第１の動きＭａとを用いて、車両１の動きを示す第２の動きＭｂを求める第２の動き算出部１０３とを備えている。
【００３５】
図２の移動体動き算出装置１００、画像合成装置１２１およびディスプレイ１２２は、例えば、車両１内に設置されるが、車両１とは異なる場所に設けることも可能である。
【００３６】
図３は図２の移動体動き算出装置１００における第１の動き算出部１０２の詳細な構成を示すブロック図である。図３において、１１１は対応点算出部１０１から出力された複数の対応点から、所定数の対応点からなる部分対応点を複数組選択する部分対応点選択部、１１２は部分対応点選択部１１１によって選択された各組の部分対応点から、車両１の動き候補をそれぞれ算出する動き候補算出部、１１３は動き候補算出部１１２によって算出された動き候補を所定の評価手法によって評価し、この評価結果に従って、第１の動きＭａを特定する動き評価選択部である。
【００３７】
また、１１４は第１の動きＭａを算出する際に仮定する所定の平面を表す平面式を出力する平面式算出部、１１５は静止した対応点の比率である静止対応点率Ｒを出力する静止対応点率算出部、１１６は移動体動きの予測値を求める移動体動き予測部である。
【００３８】
ここでは、図１の状況において、車両１が後進した場合を例にとって、説明を行う。
【００３９】
図１において、世界座標系（Ｘｗ，Ｙｗ，Ｚｗ）は３次元空間中で静止しており、車両座標系（Ｘｃ，Ｙｃ，Ｚｃ）および視点座標系（Ｘ，Ｙ，Ｚ）は車両１に固定されているものとする。視点座標系の座標値（ｘ，ｙ，ｚ）と車両座標系の座標値（ｘｃ，ｙｃ，ｚｃ）との関係は、座標系間の位置関係を表す行列Ｃを用いて、（数１）のように表すことができる。
【数１】
【００４０】
世界座標系に対して静止した点が、車両座標系における車両の動きＭｃによって、車両座標系の座標値（ｘｃ，ｙｃ，ｚｃ）から（ｘｃ’，ｙｃ’，ｚｃ’）に移動した場合、（数２）の関係式が成り立つ。さらに、このときの視点座標系の動きがＭであり、同じ点の視点座標系の座標値が（ｘ，ｙ，ｚ）から（ｘ’，ｙ’，ｚ’）に移動したとすると、（数３）の関係式が成り立つ。したがって、車両１の動きＭｃとカメラ１２０の動きＭとの間には（数４）の関係式が成り立つ。
【数２】
【数３】
【数４】
【００４１】
以上のことから、車両座標系と視点座標系の位置関係を示すＣが既知のとき、車両座標系を基準にした車両１の動きＭｃを求めるためには、視点座標系を基準にしたカメラ１２０の動きＭを算出して、（数４）で変換すればよい。カメラ１２０と車両１との相対位置が固定されている場合、カメラ１２０の動きと車両１の動き（移動体の動き）とは一意に変換できるため、本願明細書では、特に明示がない限り、区別はしないものとする。
【００４２】
図４（ａ），（ｂ）は図１の状況において、車両１が後方に移動した場合にカメラ１２０から出力される一連の画像列の例であり、図４（ａ）は時刻ｔ−１に撮影された画像、図４（ｂ）は時刻ｔに撮影された画像である。また、図４（ｃ）は、時刻ｔ−１と時刻ｔの２画像間の同一点の動きである対応点（図中の矢印）を示す図である。
【００４３】
図５はカメラ１２０の３次元的な動きと画像変化との関係を示す図である。図５（ａ）に示すように、カメラ１２０が時刻ｔ−１から時刻ｔの間に動きＭで移動したとき、世界座標系における静止点Ｘが、視点座標系の座標値（ｘ，ｙ，ｚ）から（ｘ’，ｙ’，ｚ’）に移動したものとする。このとき、座標値（ｘ，ｙ，ｚ）、（ｘ’，ｙ’，ｚ’）とカメラ動きＭとの間には、（数３）が成り立つ。図５（ｂ）（ｃ）はそれぞれ時刻ｔ−１，ｔにおける画像であり、時刻ｔ−１において視点座標（ｘ，ｙ，ｚ）の点Ｘが画像座標（ｕ，ｖ）に、時刻ｔにおいて視点座標（ｘ’，ｙ’，ｚ’）の点Ｘが画像座標（ｕ’，ｖ’）に、それぞれ対応している。このとき（ｕ，ｖ）と（ｘ，ｙ，ｚ）、（ｕ’，ｖ’）と（ｘ’，ｙ’，ｚ’）の関係と（数３）を用いると、（数５）の関係式が成り立つ。（数５）において、ｆはカメラの焦点距離を示す。
【数５】
【００４４】
このとき、静止点Ｘ（ｘ，ｙ，ｚ）が（数６）の平面式で表される平面上に存在し、かつ、カメラ１２０の動きＭ（ｔｘ，ｔｙ，ｔｚ，ｗｘ，ｗｙ，ｗｚ）が十分に小さく（数７）が成り立つとき、点Ｘの画像座標（ｕ，ｖ）、（ｕ’，ｖ’）とカメラ動きＭとの間に（数８）の関係式が成り立つ。さらに、（数８）をカメラ動きＭの要素に着目して整理すると、（数９）が得られる。１組の対応点ごとに（数９）の２つの式が成り立つことから、平面式とカメラ１２０の焦点距離ｆが既知の場合は、独立した（同一直線上ではない）対応点（ｕ，ｖ），（ｕ’，ｖ’）が３組以上与えられれば、カメラ１２０の動きＭ（ｔｘ，ｔｙ，ｔｚ，ｗｘ，ｗｙ，ｗｚ）の６変数を算出することができる。
【数６】
【数７】
【数８】
【数９】
【００４５】
以下、時刻ｔ−１から時刻ｔの間のカメラ動きＭを算出する場合を例にとって、説明を行う。なお、上述したように、カメラの動きと移動体の動きは（数４）により一意に変換できるため、カメラ動きが算出できれば、移動体の動きも実質的に算出されたことになる。
【００４６】
ここでは、車両座標系（Ｘｃ，Ｙｃ，Ｚｃ）および視点座標系（Ｘ，Ｙ，Ｚ）は図１のように配置されているものとし、車両座標系に対する視点座標系の位置関係を表すＣは、予め計測されているものとする。また、カメラ１２０の焦点距離ｆも予め求められており、その値は、実座標系での焦点距離に、実座標と画像座標との変換係数を乗じたものとする。
【００４７】
対応点算出部１０１はカメラ１２０から出力された一連の画像列を一時的に保存し、画像列のうちの連続した２枚の画像から、対応点をｎ個（ｎは２以上の整数）検出する。ここでは、時刻ｔ−１の画像上のｎ点の座標（ｕｉ，ｖｉ）（ｉ＝１，…，ｎ）について、対応する、時刻ｔの画像上の座標（ｕｉ’，ｖｉ’）（ｉ＝１，…，ｎ）をそれぞれ検出する。検出したｎ点の対応点の情報を、Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）（ｉ＝１，…，ｎ）と表す。対応点の個数ｎは、予め定められた値とする。
【００４８】
なお、対応点を検出する２枚の画像は、必ずしも連続している必要はなく、撮影時刻が異なっていればよい。
【００４９】
ここでの対応点検出は、例えば、Kanade, Lucas and Tomasiの対応点探索方法によって行えばよい（例えば、“Detection and Tracking of Point Features”, Carlo Tomasi and Takeo Kanade, Carnegie Mellon University Technical Report, CMU-CS-91-132, April 1991.を参照）。この方法は、一般に広く知られた方法であるので、ここでは詳細な説明を省略する。
【００５０】
第１の動き算出部１０２は、対応点算出部１０１によって検出されたｎ個の対応点Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）（ｉ＝１，…，ｎ）を用いて、車両１の第１の動きＭａを算出する。以下、第１の動き算出部１０２を構成する各処理手段の動作について説明する。
【００５１】
平面式算出部１１４は、ここでは所定の平面式を出力する。ここで用いる平面式は、図１の状況における路面を平面として近似したものとする。例えば、車両が静止した状態で、水平かつ平坦な路面平面の式を予め計測しておき、平面式算出部１１４は、予め計測した（数６）の平面式のパラメタＳ（ａ，ｂ，ｃ）を出力する。なお、平面式算出部１１４は、平面式を更新する機能も有する。これについては、後述する。
【００５２】
静止対応点率算出部１１５は、静止対応点率Ｒを出力する。ここで、静止対応点率とは、好ましくは、対応点算出部１０１によって算出されたｎ個の対応点のうち、世界座標に対して静止しており、かつ、対応関係が正しい対応点の比率を表すものである。ここで、対応点の対応関係が正しいとは、対応点の座標値の誤差が所定の閾値以下であることを意味する。なお、実時間でのオンライン処理において、静止対応点率Ｒを事前に正確に得ることは極めて困難であり、実際には、静止対応点率Ｒとは、上述した比率の予測値とする。
【００５３】
ここでは、静止対応点率算出部１１５は、静止対応点率Ｒとして、所定の値、例えば０．５を出力する。静止対応点率Ｒ＝０．５とは、「対応点算出部１０１によって算出されたｎ個の対応点に、静止した、対応関係の正しい対応点が５０％含まれる」と予測していることを意味する。所定の値としては、例えば、図１と類似の走行状態において静止対応点率を予め複数回計測し、計測した中の最低値を用いればよい。なお、静止対応点率算出部１１５は、静止対応点率Ｒを新たに算出する機能も有する。これについては、後述する。
【００５４】
移動体動き予測部１１６は、車両１の動きの予測値を求める機能を有するが、ここでは、特に処理を行わないものとする。
【００５５】
部分対応点選択部１１１は、対応点算出部１０１から出力されたｎ個の対応点Ｐｉ、平面式算出部１１４から出力された平面式Ｓ、および静止対応点率算出部１１５から出力された静止対応点率Ｒを用いて、ｍ個の対応点で構成される部分対応点をｑ組選択する。ここで、部分対応点を構成する対応点の個数ｍは、移動体の動きを算出するために必要となる対応点の数である。（数９）を用いてカメラ動きを算出するためには、最低３個の対応点が必要である。このため、ここでは、ｍ＝３とする。もちろん、ｍは３より大きい値であってもかまわない。
【００５６】
部分対応点選択部１１１は、まず、部分対応点の組数ｑを算出する。部分対応点の組数ｑは、その値が大きいほど、第１の動き算出部１０２での処理時間が長くなり、逆にｑが小さいほど処理時間は小さくなる。そのため、例えば処理時間の制約がある場合は、その上限からｑを決定しても良い。
【００５７】
ここでは、部分対応点の組数ｑを（数１０）の関係式から求めるものとする。
【数１０】
（数１０）において、Ｒｒは路面静止対応点率、ｚは保証率である。
【００５８】
路面静止対応点率Ｒｒは、部分対応点として選択された各対応点が、路面上に存在し、世界座標に対して静止しており、かつ対応関係が正しい対応点の確率である。したがって、対応点が路面上に存在する確率Ｒｓと、対応点の静止対応点率Ｒとの間に相関がない場合、対応点が路面上に存在する確率Ｒｓに静止対応点率Ｒを掛けたものが路面静止対応点率Ｒｒとなる。また、もしも路面上に存在する対応点を正確に選択することが可能であり、かつ、選択した対応点の静止対応点率がＲである場合、選択した対応点の路面対応点率Ｒｒは、静止対応点率Ｒと等しくなる。ここでは、所定の値として、例えば、Ｒｒ＝０．３を用いる。所定の値としては、例えば図１と類似の走行状態において、選択後の対応点が路面上に存在する確率Ｒｓを計測しておき、これと静止対応点率Ｒをかけた値を用いればよい。
【００５９】
保証率ｚは、ｑ組の部分対応点のうち、全て（ｍ個）の対応点が路面上の静止した正しい対応点で構成された部分対応点が少なくとも１組存在することを統計的に保証する率である。ここでは、所定の値として、例えば、ｚ＝０．９９９を用いる。言い換えると、少なくとも１組の部分対応点が路面上の正しい対応点で構成されることを、保証率ｚで保証するためには、ｑ組の部分対応点を選択すればよい。
【００６０】
（数１０）において、ｑとＲｒが一定の場合、部分対応点を構成する対応点数ｍが小さいほど保証率ｚが大きくなり、ｚとＲｒが一定の場合は、ｍが小さいほどｑが小さくなる。したがって、対応点の数ｍは小さいほど、処理時間が短く、部分対応点の少なくとも１組が路面上の正しい対応点で構成される可能性が高くなる。このことと、（数９）の算出には３点以上の対応点数が必要であることから、最良のｍは３となる。
【００６１】
次に、部分対応点選択部１１１は、算出した組数ｑを用いて、ｎ個の対応点から、異なるｍ個の対応点からなる部分対応点をｑ組選択して出力する。部分対応点を選択する方法として、ｎ個の全ての対応点からランダムに選択してもよいし、特定の条件を満たす対応点を抽出した上で、そこからランダムに選択するとしてもよい。ここでは後者を用いる。すなわち、平面式Ｓが存在し得る画像領域内の対応点を抽出した上で、抽出した対応点から、ランダムに、異なるｍ個の対応点からなる部分対応点をｑ組選択して出力する。例えば、図４（ｄ）において、画像中で平面Ｓが存在し得る領域は、水平線より下の斜線領域に限られることを示している。この方法では、斜線領域に存在する対応点を抽出した上で部分対応点を選択する場合の路面静止対応点率Ｒｒは、ｎ個の対応点からランダムに選択する場合の路面静止対応点率Ｒｒに比べて、より高い値となり、その結果、部分対応点の組数ｑをより小さい値にすることができる。
【００６２】
動き候補算出部１１２は、部分対応点選択部１１１によって選択されたｑ組の部分対応点から、それぞれ、平面式Ｓを用いて、ｑ個の動き候補を算出する。
【００６３】
上述したように、画像中の対応点Ｐｉ（ｕ，ｖ，ｕ’，ｖ’）が平面式Ｓ（ａ，ｂ，ｃ）上にあり、かつ、カメラの動きＭが小さいと仮定した場合、（数９）の関係式が成り立つ。（数９）において、対応点と平面式が既知、カメラの動きＭを未知とすると、対応点が３個以上あればカメラの動きＭを算出することができる。そこで、動き候補算出部１１２は、３個の対応点からなるｑ組の部分対応点について、それぞれ、（数９）の関係式を用いて、ｑ個のカメラの動きＭｊ（ｊ＝１，…，ｑ）を算出し、これを車両１の動き候補として出力する。
【００６４】
動き評価選択部１１３は、動き候補算出部１１２によって算出されたｑ個の動き候補（カメラ動きＭｊ（ｊ＝１，…，ｑ））を、所定の評価手法によって評価し、この評価結果に従って、第１の動きＭａを特定する。ここでの評価では、対応点算出部１０１から出力されたｎ個の対応点Ｐｉ、および静止対応点率算出部１１５から出力された静止対応点率Ｒを用いる。なお、必要に応じて、移動体動き予測部１１６から出力された動き予測を用いてもよい。これについては、後述する。
【００６５】
ここでは、まず、カメラ動きＭｊとｎ個の対応点Ｐｉとの一致する度合いを評価する。具体的には、それぞれのカメラ動きＭｊについて、ｎ個の対応点（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）（ｉ＝１，…，ｎ）を（数１１）に代入し、ｎ個の対応点の３次元座標（ｘｉ，ｙｉ，ｚｉ）を算出する。ここで、（数１１）は未知数４に対して独立した一次式が４つあること、および、画像から算出した対応点Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）は誤差を含むと考えられることから、最小二乗法を用いて３次元座標（ｘｉ，ｙｉ，ｚｉ）を算出する。
【数１１】
【００６６】
さらに、算出した３次元座標（ｘｉ，ｙｉ，ｚｉ）とカメラ動きＭｊを（数１１）に再び代入して、新たに対応点（ｕｉｊ，ｖｉｊ，ｕｉｊ’，ｖｉｊ’）を算出する。ここで、対応点Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）およびカメラ動きＭｊが誤差のない正確な値である場合、Ｐｉ（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）と（ｕｉｊ，ｖｉｊ，ｕｉｊ’，ｖｉｊ’）は一致し、誤差がある場合は一致しない。そこで、（ｕｉ，ｖｉ，ｕｉ’，ｖｉ’）と（ｕｉｊ，ｖｉｊ，ｕｉｊ’，ｖｉｊ’）の誤差距離平均Ｅｊｉを、（数１２）から算出する。Ｅｊｉの値は、カメラ動きＭｊと対応点Ｐｉが一致するときは、０になり、一致しないときは、値が大きくなるという性質をもつことから、Ｅｊｉをカメラ動きＭｊと対応点Ｐｉの一致度合いに関する評価値として用いる。
【数１２】
【００６７】
次に、ｎ個の対応点Ｐｉそれぞれに対するカメラ動きＭｊの評価値Ｅｊｉ（ｉ＝１，…，ｎ）のうち、評価の良い方すなわち値の小さい方から（ｎ×Ｒ）番目の評価値を、カメラ動きＭｊの評価値Ｅｊとして選択する。
【００６８】
そして、ｑ個のカメラ動きＭｊ（ｊ＝１，…，ｑ）の中で、それぞれの評価値Ｅｊ（ｊ＝１，…，ｑ）のうち評価が最も良い、すなわち値が最も小さいものに対応するカメラ動きを、第１の動きＭａとして選択する。
【００６９】
図６はｎ個の対応点に対するある１つのカメラ動きＭｊの評価値Ｅｊｉ（ｉ＝１，…，ｎ）の分布を概念的に示すグラフである。図６では、評価値Ｅｊｉを小さい順にソートしており、横軸はソート後の対応点番号（１，…，ｎ）、縦軸は各対応点に対するカメラ動きＭｊの評価値Ｅｊｉの値である。
【００７０】
ここで、カメラ動きＭｊが正しく、かつ、ｎ個の対応点のうち（ｎ×Ｒ）個が静止した正しい対応点であるとすると、理想的には図６（ａ）のように、小さい方から（ｎ×Ｒ）番目までの評価値Ｅｉｊは０で、それ以降は次第に値が大きくなる。このため、評価値Ｅｊも０となる。しかし、実際には対応点の計測誤差などの小さな誤差が存在するため、（ｎ×Ｒ）番目までの評価値Ｅｉｊは必ずしも０にはならない。
【００７１】
一方、カメラ動きＭｊが正しくない場合は、図６（ｂ）のように、評価値Ｅｊｉの多くが大きな値となり、評価値Ｅｊも大きな値となる。このことを言い換えると、静止対応点率Ｒを前提条件とした場合、カメラ動きＭｊの評価値Ｅｊが小さいということは前提条件を満たすことを意味し、さらに、評価値Ｅｊが最小となるカメラ動き（すなわち第１の動きＭａ）は、前提条件を最も良く満たすカメラ動きであることを意味する。
【００７２】
このような動作によって、第１の動き算出部１０２は、時刻ｔ−１と時刻ｔの画像上のｎ個の対応点Ｐｉを用いて、車両１の動きである第１の動きＭａを算出して出力する。
【００７３】
第２の動き算出部１０３は、第１の動き算出部１０２から出力された第１の動きＭａと、対応点算出部１０１から出力されたｎ個の対応点Ｐｉとを用いて、車両１の動きを示す第２の動きＭｂを算出する。ここでは、第１の動きＭａを初期値とした探索的手法によって、第２の動きＭｂを算出するものとする。
【００７４】
具体的には、まず、第１の動きＭａ（ｔｘ，ｔｙ，ｔｚ，ｗｘ，ｗｙ，ｗｚ）を初期値として、各パラメータに対して所定の微小動き（ｄｔｘ，ｄｔｙ，ｄｔｚ，ｄｗｘ，ｄｗｙ，ｄｗｚ）をそれぞれ増減して、複数の動き候補を得る。これにより、ｏ＝３＾６個の動き候補Ｍｂｋ＝（ｔｘ±ｄｔｘ，ｔｙ±ｄｔｙ，ｔｚ±ｄｔｚ，ｗｘ±ｄｗｘ，ｗｙ±ｄｗｙ，ｗｚ±ｄｗｚ）（ｋ＝１，…，ｏ）が得られる。
【００７５】
次に、第１の動き算出部１０２の動き評価選択部１１３と同様に、ｏ個の動き候補Ｍｂｋについて、それぞれ、ｎ個の対応点に対する評価値Ｅｂｋｉ（ｉ＝１，…，ｎ）を算出する。そして、小さい方から（ｎ×Ｒ）番目の評価値Ｅｂｋｉを動き候補Ｍｂｋの評価値Ｅｂｋとし、さらに、評価値Ｅｂｋが最小となる動き候補Ｍｂｋを選択する。
【００７６】
次いで、選択された動き候補Ｍｂｋを初期値として、上と同様の処理を行う。これを繰り返し実行し、値が収束したとき、または、繰り返し回数が所定回数に達したとき、処理を終了する。そして、第２の動き算出部１０３は、そのとき選択されている動き候補Ｍｂｋを第２の動きＭｂとして出力する。
【００７７】
このような動作により、移動体動き算出装置１００は、時刻ｔ−１から時刻ｔの間の車両１の動きを算出する。また、カメラ１２０から出力された画像列について、同様の処理を繰り返すことによって、車両１の動きを連続して算出することができる。
【００７８】
そして、画像合成装置１２１は、カメラ１２０によって撮影された画像と、移動体動き算出装置１００から出力された第２の動きＭｂを入力して、車両１の動きに関する情報が重畳された合成画像を生成する。ディスプレイ１２２は画像合成装置１２１によって生成された合成画像を表示する。図７は表示された合成画像の例であり、車両１の第２の動きＭｂに対応する予測軌跡ＴＲが、カメラ１２０で撮影された画像に重畳されている。
【００７９】
＜本実施形態の効果＞
本実施形態によると、例えば、対応点探索の誤りが多く、大きな誤差を含む場合であっても、車両の動きを精度良く算出することができる。
【００８０】
対応点算出部１０１の動作説明で触れたKanade, Lucas and Tomasiの方法は、高い精度と少ない計算量の両面で優れた対応点探索方法として広く知られている。しかし、そのような優れた方法であっても、制約の無い環境で撮影された画像において対応点探索の誤りを０にすることは、まず不可能と言ってよい。
【００８１】
図８（ａ）は実際に車両の後方にカメラを設置して撮影した画像の例である。図８（ａ）のシーンで撮影した動画像について、上述した手法で対応点探索を行った。図８（ｂ）はそのときの対応点正解率と車両の移動速度との関係を示すグラフである。ここでは、画像座標上の誤差が１画素以内であった対応点の比率を対応点正解率としている。なお、図８（ｂ）のグラフでは、時間と移動速度の値には、適当なスケールが掛けられている。
【００８２】
図８（ｂ）からも分かるように、実際に撮影された画像を用いた場合、精度が良いとされている手法であっても、対応点正解率が５０％程度にまで低下する場合がある。また、はずれ値の様な大きな誤差を持つ対応点が多数含まれる場合もある。本願発明者らは、このような場合が稀ではないことを、他の実験結果からも確認している。
【００８３】
そして、上述したような従来の方法では、多数の誤りを排除する仕組みを持たないことから、例えば、対応点探索の正解率が大きく低下してしまった場合、精度の高い動き算出は、到底期待できない。
【００８４】
これに対して本実施形態では、たとえ対応点正解率が大きく低下したような場合であっても、第１の動き算出部１０２において、第１の動きＭａを精度良く算出することができる。すなわち、本実施形態の方法の場合、ｑ組の部分対応点の中に、正しい部分対応点が少なくとも１組含まれていれば、ｑ個の動き候補Ｍｊの中に少なくとも１つの正しい動きが含まれることになる。さらに、対応点探索の正解率に相当する静止対応点率Ｒを用いて、ｑ個の動きＭｊを、ｎ個の対応点に対する評価値のうち良い方から（ｎ×Ｒ）番目の評価値を用いて評価することによって、正しい動きＭａを算出することを可能にしている。
【００８５】
また、本実施形態によると、例えば路面が傾いている場合であっても、車両の動きを精度良く算出することができる。
【００８６】
本実施形態では、車両が静止した状態で計測して得た水平かつ平坦な路面平面の式を、所定の平面式として用いて、第１の動きＭａを算出するものとした。しかし、坂道や昇降路などの路面が傾いている場合や、乗員や荷物の積載、またはハンドル操作などによって車両の姿勢が変わった場合は、予め計測した所定の平面式は、実際に車両が走行している路面平面と一致しなくなる。
【００８７】
図９（ａ）は車両が傾いた場合の画像例である。車両が傾くと、画像内の路面も傾くため、予め計測した所定の平面式と一致しなくなる。この場合、選択された部分対応点の３個の対応点が、路面上にありかつ対応関係が正しいものであったとしても、（数９）を用いて算出された動きＭｊは誤差を含む。このため、図９（ｂ）に示すように、ｎ個の対応点に対する動きＭｊの評価値Ｅｊｉは、平面式が正しい場合に比べて少しずつ大きな値となり、評価値Ｅｊも大きな値となる。したがって、評価値が最も小さい第１の動きＭａであっても、誤差を含むことになる。
【００８８】
ここで、評価値が図９（ｂ）のようになる場合に、従来技術であるはずれ値除去方法を適用し、評価値が小さい一部の対応点を抽出して、(数９）を用いた最小二乗法によって動きを算出したとしても、誤差を小さくすることは困難である。
【００８９】
これに対して本実施形態では、第１の動きＭａを初期値として、小さい方からｎ×Ｒ個の評価値を基準とした探索的手法を用いて、第２の動きＭｂを算出する。一般に、初期値から真の値（または最良の値）までの間で、その評価関数が滑らかであれば、探索的手法によって真の値を得ることが可能である。そして、本実施形態で用いた評価値は、真の値の近傍であれば上記条件を満たす。このため、たとえ、実際の路面が所定の平面式と一致しなくなったために第１の動きＭａが誤差を含んでいる場合であっても、第２の動き算出部１０３によって、誤差の小さい第２の動きＭｂを算出することができる。
【００９０】
また、本実施形態において、平面式を仮定することの妥当性と効果について、実際の車両の走行環境において発生する問題を基にして、説明する。
【００９１】
車両周辺をカメラで監視するシステムには、次のような特徴がある。すなわち、運転者が車両周辺を監視できるように、典型的には図４のように、車両近傍の路面から水平線より上の障害物までが画像に映るように、広視野角のカメラが設置される。このように設置されたカメラの画像には、ほとんど全てのシーンにおいて路面が映ることが期待できる。さらに、仮に路面が連続した平面ではない場合であっても、多くの被写体が路面平面の延長上に存在する。例えば水平線近傍の遠方の被写体は、実際には路面に接していなくても、路面と同一の平面式上に存在すると仮定することができる。このようなケースが多いことが、本願発明者らによる実験でも確認できた。このことから、車両周辺を監視するシステムにおいて、路面を表す平面式を仮定して動きを求める方法は、妥当といえる。
【００９２】
また、平面式を仮定した場合、（数９）を用いて、３個の対応点から車両の動きを算出できる。これに対して、平面式を仮定しない方法では、動き（または動き候補）を求めるために、少なくとも４個以上の対応点が必要となり、このため、算出過程において誤差の影響を受けやすい。これに対して、本実施形態のように平面式を仮定することによって、誤差の影響を回避できるという効果が得られる。
【００９３】
さらに、単眼カメラの画像を用いてカメラの動きを算出する場合、スケール項を求めるために、画像中に大きさが既知の被写体が必要となる。本実施形態では、平面式を仮定することによって、画像中に大きさが既知の被写体が存在しなくても、スケールを考慮したカメラ動きを算出できるという効果もある。
【００９４】
さらに、本実施形態のように平面式を仮定することによって、画像中に他の移動体が存在する場合であっても、移動体動きを正しく算出できるという効果も得られる。
【００９５】
図１０（ａ）は自車両１の後方に他の移動車両２が存在している状況を示す図である。図１０（ａ）では、自車両１は後方（図中右）に、他車両２は前方（図中左）にそれぞれ移動している。このとき、図１０（ｂ）のような画像がカメラ１２０によって撮影される。
【００９６】
ここで、対応点が、路面と他車両２上のそれぞれに、５０％ずつ算出されたとする（対応関係の誤りはないものとする）。このような状況に、従来の方法を適用すると、自車両１について、路面に対する動きと、他車両２に対する動きとの２つが算出される可能性があるため、これら２つの動きを判別する手段が別途必要になる。さらに２つの動きを判別する手段において誤りが発生すれば、自車両１の動きは誤差の大きな動きとなる。
【００９７】
これに対して、本実施例では、路面平面の式を仮定して自車両１の動きを算出する。このため、第１の動き算出部１０２において、路面平面から離れた他車両２上の対応点から得られた自車両１の動きＭｊは、大きな誤差を含むこととなり、その評価値Ｅｊは大きくなる。この結果、第１の動き算出部１０２において、他車両２上の対応点から得られた動きが選択されることはなく、路面上の対応点から得られた路面平面に対する動きが、第１の動きＭａとして選択される。以上のことから、本実施形態は、画像中に他の移動物が存在する場合であっても、移動体の動きを正しく算出できるという効果があると言える。
【００９８】
以上説明したように、平面式を仮定して動きを算出することによって、実際の車両の走行環境で発生する、対応点探索過程で発生する誤差、路面や車両の傾き、他の移動物の存在などといった問題に対しても、移動体動きを精度良く算出することができる、という効果が得られる。
【００９９】
＜平面式の更新＞
平面式算出部１１４は、過去の画像から第１の動き算出部１０２によって求められた第１の動きＭａを用いて、平面式を更新することも可能である。
【０１００】
例えば、平面式算出部１１４は、時刻ｔ＝１において、予め得られた所定の初期平面式（ａ０，ｂ０，ｃ０）を出力する。第１の動き算出部１０２は、この初期平面式（ａ０，ｂ０，ｃ０）を用いて時刻ｔ＝０とｔ＝１間の第１の動きＭａを求める。次に、時刻ｔ＝２において、平面式算出部１１４は、時刻ｔ＝０とｔ＝１間の第１の動きＭａを用いて、移動体が初期平面式（ａ０，ｂ０，ｃ０）に対して第１の動きＭａだけ動いたものとして、新たな平面式（ａ１，ｂ１，ｃ１）を算出して出力する。これ以降、同様の処理を順次繰り返すことによって、平面式は更新されていく。
【０１０１】
このように、過去に算出した移動体動きから新たに平面式を算出し、次の動き算出に用いることによって、より正確な平面の位置に基づいた動き算出を実行することができる。したがって、移動体動きをより精度良く算出することができる。
【０１０２】
なお、平面式の更新には、第１の動きＭａに代えて、第２の動きＭｂを用いてもよいし、またその両方を用いてもかまわない。
【０１０３】
また、平面式の更新は、必ずしも、動き算出を実行するたびに行う必要はない。例えば、動き算出を所定回数実行する毎に行ってもよいし、路面や車両の状況に変化が生じたときに行うようにしてもよい。
【０１０４】
＜静止対応点率の更新＞
静止対応点率算出部１１５は、所定の静止対応点率を出力する代わりに、対応点算出部１０１から出力された複数の対応点と、過去の画像から第１の動き算出部１０２によって算出された第１の動きＭａとを用いて、静止対応点率を算出することも可能である。
【０１０５】
例えば、静止対応点率算出部１１５は、第１の動き算出部１０２によって算出された第１の動きＭａについて、ｎ個の対応点に対して、上述したような（数１１）および（数１２）を用いた評価を行う。そして、ｎ個の対応点のうち評価値Ｅｉが所定の閾値以下であるものの比率を算出し、これを新たな静止対応点率として保持する。そして、次の動き算出を実行するときに、保持していた静止対応点率を出力する。これ以降、同様の処理を順次繰り返すことによって、静止対応点率は更新されていく。
【０１０６】
ここで、動き算出に用いられた静止対応点率が真の静止対応点率と異なる場合、算出される動きは、真の動きに対する誤差を含むことになる。この静止対応点率Ｒと真の静止対応点率Ｒtrueおよびカメラ動きの誤差ＥＭとの関係を図１１のグラフに示す。図１１のグラフは本願発明者らが行った実験の結果から、その共通の傾向を模式的に表したものである。図１１のグラフからも分かるように、静止対応点率Ｒと真の静止対応点率Ｒtrueが一致しているとき（比Ｒ／Ｒtrueが１のとき）、カメラ動きの誤差ＥＭが最も小さくなり、静止対応点率Ｒが真の静止対応点率Ｒtrueに対して大きくなった場合でも小さくなった場合でも、カメラ動きの算出精度は低下する。
【０１０７】
一方、動き算出の繰り返しの周期が比較的短い場合、車両周辺の画像の変化は小さく、したがって、静止対応点率の変化も小さいと考えられる。このため、時刻ｔ−１において得られた動きを用いて算出した静止対応点率Ｒ（ｔ−１）は、時刻ｔの画像における真の静止対応点率Ｒtrue（ｔ）に近いと予想される。したがって、静止対応点率を更新していくことによって、誤差の小さい動きを算出することが可能になる。
【０１０８】
なお、時刻ｔにおいて、時刻ｔ−１で算出した静止対応点率Ｒ（ｔ−１）を用いて移動体動きを算出した結果、この移動体動きから求めた静止対応点率Ｒ（ｔ）が静止対応点率Ｒ（ｔ−１）を下回った場合は、時刻ｔの移動体動きを、新たな静止対応点率Ｒ（ｔ）を用いて再計算するようにしてもよい。
【０１０９】
図１１からわかるように、静止対応点率Ｒが真の静止対応点率Ｒｔｒｕｅよりも小さい場合は、カメラ動きの誤差はあまり大きくならないのに対して、静止対応点率Ｒが真の静止対応点率Ｒｔｒｕｅよりも大きい場合は、カメラ動きの誤差は極端に大きくなる。したがって、Ｒ（ｔ）＜Ｒ（ｔ−１）の場合、静止対応点率Ｒ（ｔ−１）を用いて算出したカメラ動きは大きな誤差を含んでいる可能性がある。このことから、Ｒ（ｔ）＜Ｒ（ｔ−１）の場合に、新たに得られたＲ（ｔ）を用いて移動体動きを再計算することによって、より精度の高い動き算出が可能になる。
【０１１０】
なお、静止対応点率の更新には、第１の動きＭａに代えて、第２の動きＭｂを用いてもよいし、またその両方を用いてもかまわない。
【０１１１】
＜移動体動きの予測値の利用＞
移動体動き予測部１１６は、第１の動き算出部１０２によって過去に求められた第１の動きＭａを用いて、車両１の動きを予測し、その動き予測値を出力することができる。この場合、動き評価選択部１１３は、移動体動き予測部１１６から出力された動き予測値を加味して、第１の動きＭａを算出する。
【０１１２】
ここでは例えば、移動体動き予測部１１６は、車両１の動きはほぼ等速・等角速度運動であると仮定して、予測を行うものとする。すなわち、時刻ｔ−１に算出された第１の動きＭａを保存しておき、時刻ｔの動き予測値Ｍｒｔとして出力する。動き評価選択部１１３は、動き候補算出部１１２から入力したｑ個のカメラ動きＭｊ（ｊ＝１，…，ｑ）とともに、動き予測値Ｍｒｔも併せた（ｑ＋１）個のカメラ動きに対して、上述したように（数１１）（数１２）を用いて評価を行う。そして、最も評価値の良いものを、時刻ｔにおける第１の動きＭａとして選択する。
【０１１３】
上述したように、部分対応点の組数ｑと保証率ｚとの間には（数１０）の関係があり、静止対応点率Ｒ＜１、かつ、ｑが有限である限り、保証率ｚは１未満となる。つまり、ｑ組の部分対応点から第１の動きＭａを求める場合、ある確率で、誤差が大きい動きが算出される。
【０１１４】
これに対して、動き予測値Ｍｒｔも併せて評価を行い、第１の動きＭａを選択することによって、ｑ組の部分対応点からは正しい動きが得られないような場合でも、車両１の動きが等速・等角速度運動に近い場合は、動き予測値Ｍｒｔが第１の動きＭａとして選択される。この結果、第１の動きＭａに大きな誤差が生じるケースを回避することができる。
【０１１５】
なお、ここでは、移動体が等速・等角速度運動していると仮定して動き予測を行うものとしたが、動き予測の方法はこれに限られるものではない。例えば、移動体が等加速・等角加速度運動をしていると仮定して動き予測を行ってもよく、この場合でも上と同様に、第１の動きＭａに大きな誤差が生じるケースを回避することができる。
【０１１６】
なお、移動体の動き予測には、第１の動きＭａに代えて、第２の動きＭｂを用いてもよいし、またその両方を用いてもかまわない。
【０１１７】
以上のように本実施形態によると、対応点が存在する所定の平面を仮定することによって、未知パラメタを減らすことができることから、大きな誤差の少ない第１の動きＭａを求めることができる。さらに、第１の動きＭａを初期値とした探索的手法を用いることによって、誤差の小さい第２の動きＭｂを求めることができる。
【０１１８】
なお、本実施形態では、動き候補の評価のために、（数１１）と（数１２）に表された評価値Ｅｊｉを用いたが、評価手法はこれに限定されるものではなく、対応点がカメラ動きに一致するか否かを評価する値であればどのような評価値を用いてもかまわない。例えば、エピポーラ拘束や基礎行列と呼ばれる、カメラ間の位置（動き）と対応点との関係を示す評価値を用いてもよい。（数１３）はその具体例である。
【数１３】
【０１１９】
なお、本実施形態では、カメラ動きＭｊについて、ｎ個の対応点に対する評価値Ｅｊｉのうち小さい方から（ｎ×Ｒ）番目の評価値を、評価値Ｅｊとして選択するものとしたが、評価値Ｅｊの特定方法はこれに限定されるものではない。例えば、評価値Ｅｊｉの小さい方から（ｎ×Ｒ）番目までの評価値の平均値を、評価値Ｅｊとしてもよい。
【０１２０】
また、静止対応点率Ｒの値に応じて、評価値Ｅｊの特定方法を変えてもよい。例えば、静止対応点率Ｒが所定の閾値よりも大きい場合は、評価値Ｅｊｉのうちの小さい方から（ｎ×Ｒ）番目の評価値をＥｊとし、一方、静止対応点率Ｒが所定の閾値よりも小さい場合は、評価値Ｅｊｉのうちの小さい方から（ｎ×Ｒ）番目までの評価値の平均値をＥｊとするようにしてもよい。
【０１２１】
この方法によると、静止対応点率Ｒが比較的小さいときは、複数の評価値の平均値をとることによって、評価値に含まれる誤差の影響を抑制することができる。その一方で、静止対応点率Ｒが比較的高いときは、１個の評価値を評価値Ｅｊとして採用することによって、（ｎ×Ｒ）番目近辺の微小な差異が平均化されずに、評価値Ｅｊに反映される、という効果が得られる。
【０１２２】
なお、本実施形態において、第２の動きＭｂの算出を、第１の動きＭａを初期値とした探索的手法によって、行うものとしたが、このような方法に限定されるものではない。例えば、第１の動き算出部１０２が、第１の動きＭａの代わりに、第１の動きＭａの算出元となった部分対応点と平面式を、第２の動き算出部１０３に出力するものとする。そして第２の動き算出部１０３は、この部分対応点および平面式と、対応点算出部１０１から出力されたｎ個の対応点とを用いて、別の探索的手法により第２の動きＭｂを算出するとしてもよい。別の探索的手法として、例えば、平面式Ｓ（ａ，ｂ，ｃ）を初期値として、平面式の各パラメタに対して所定の微小変化（ｄａ，ｄｂ，ｄｃ）をそれぞれ増減した、ｏ＝３＾３個の平面式Ｓｋ（ａ±ｄａ，ｂ±ｄｂ，ｃ±ｄｃ）（ｋ＝１，・・・，ｏ）を算出する。次に、第１の動きＭａの算出元となった部分対応点とｏ個の平面式Ｓｋから、（数９）を用いてカメラ動き候補Ｍｂｋ（ｋ＝１，・・・，ｏ）を算出する。そして、前述した手法によって、ｎ個の対応点に対する動き候補Ｍｂｋの評価値Ｅｂｋを算出し、評価値Ｅｂｋが最小となる動き候補Ｍｂｋを選択する。さらに、選択されたＭｂｋに対応する平面式Ｓｋを初期値として、上述した処理を繰り返し実行し、値が収束したとき、または繰り返し回数が所定回数に達したとき、処理を終了する。そして、そのとき選択されている動き候補Ｍｂｋを第２の動きＭｂとして出力する。この方法は、第１の動きＭａに含まれる誤差の主要因が、平面式Ｓと実際の路面との差である場合に、実際の路面に近い平面式Ｓを元にして動きを再計算することが期待できるため、誤差の小さい動きを算出できると考えられる。また、この方法を用いた場合、探索空間を狭くできるため、処理が軽くなるというメリットが得られる。
【０１２３】
また、平面式Ｓ（ａ，ｂ，ｃ）のパラメタのうち、ｃはスケール項であることから、ｃに対する探索は効果がない場合がある。このような場合は、ｏ＝３＾３個の平面式Ｓｋ（ａ±ｄａ，ｂ±ｄｂ，ｃ±ｄｃ）（ｋ＝１，・・・，ｏ）用いる代わりに、ｏ＝３＾２個の平面式Ｓｋ（ａ±ｄａ，ｂ±ｄｂ，ｃ）（ｋ＝１，・・・，ｏ）用いてもよい。
【０１２４】
さらに、この方法を用いる場合、視点座標系の平面式Ｓ（ａ，ｂ，ｃ）を初期値として、一定の微小変化をそれぞれ増減した平面式Ｓｋを用いて探索を行う場合は、図１２（ａ）のように、実際の路面の変動とは無関係な平面式を用いた探索となる。これに対して、図１２（ｂ）のように、例えば、カメラ１２０に近い車軸中心（図の場合は後輪軸中心）を基準に平面を傾けた場合の、所定の微小変化（ｄａ，ｄｂ，ｄｃ）を増減した平面式Ｓｋを用いて探索を行う方が、車両と路面との位置変動を反映した探索であると言えることから、より少ない繰り返し回数での動き算出や、より誤差の小さい動き算出が可能になる。このような所定の微小変化としては、例えば、図１２と類似の状態において、さまざまな平面式Ｓとその微小変化（ｄａ，ｄｂ，ｄｃ）をあらかじめ計測しておき、これを用いればよい。
【０１２５】
なお、本実施形態では、移動体動き算出装置１００から出力された第２の動きＭｂを用いて、図７のように車両１の移動軌跡を画像に重畳するものとしたが、算出した移動体動きの利用形態は、これに限られるものではない。
【０１２６】
例えば、画像合成装置１２１は、第１の動きＭａの評価値Ｅｉが小さい方から（ｎ×Ｒ）個までの対応点について、その画像座標と３次元位置を入力する。そして、（ｎ×Ｒ）個の対応点のうち、その３次元位置が移動体の進路上であり、かつ、所定の範囲内にあるものが存在するときは、図１３のような注意メッセージを重畳合成するとともに、該当する対応点の付近を枠で囲むなどの強調表示するようにしてもよい。
【０１２７】
また、本実施形態では、移動体動き算出装置１００の出力は、画像合成装置１２１とディスプレイ１２２に供給されるものとしたが、これは、移動体動き算出装置１００やその出力である移動体動きの用途を限定するものではなく、他のどのような装置と組み合わせて用いてもかまわない。
【０１２８】
なお、本実施形態では、カメラ１２０は車両１の後部に設置されており、車両後方を撮影するものとしたが、カメラの位置や撮影範囲はこれに限定されるものではなく、車両の周辺が撮影されるように設置されていれば、カメラはどのような位置にあってもよい。
【０１２９】
なお、本実施形態では、動き算出のために仮定した平面は路面であったが、仮定する平面は路面に限定されるものではなく、移動体との相対的な位置を予め与えることができる平面であれば、どのような平面であってもかまわない。
【０１３０】
例えば、図１４のような屋内においては、天井面も移動体との相対的な位置を予め与えることができるため、路面の代わりに、天井面を平面として仮定してもよい。また、移動体が壁面と所定の距離を保って移動するような場合には、路面の代わりに、壁面を平面として仮定してもよい。
【０１３１】
なお、本実施形態では、移動体の動きの表現や、移動体の動きを表すための座標系の取り方については、特に制約はない。例えば、垂直軸が車両の非操舵輪の車軸を含む直線と直交するように、車両座標系を置き、車両の路面上の動きを、垂直軸周りの円弧運動として表してもよい。図１５はいわゆるアッカーマンモデルに基づく移動体の動きの表現を示す図である。図１５では、前輪が操舵輪、後輪が車体に車軸が固定された非操舵輪である車両について、非操舵輪の車軸の延長線上に、水平面上での回転成分の中心がおかれている。
【０１３２】
このとき、図１５のように、座標系の垂直軸（Ｙｃ軸）を後輪の中心位置を通るようにおくと、車両の動きのうち（ｔｘｃ，ｔｚｃ，ｗｙｃ）の間に拘束式が成り立つ。このような拘束条件をおくことによって、未知パラメタの個数を減らすことができ、より安定に解を求めることができる。
【０１３３】
（第２の実施形態）
本発明の第２の実施形態では、第１の実施形態と同様に構成された移動体動き算出装置を、ナビゲーション装置と組み合わせて、ナビゲーションシステムを構成する。ここでのナビゲーション装置は、人工衛星からの電波を用いて現在位置を測定する機能と、現在位置と地図情報を用いて現在位置の表示や目的地への誘導表示を行う機能とを有するものとする。
【０１３４】
図１６は本実施形態に係る移動体動き算出装置を含むナビゲーションシステムの構成図である。図１６では、図２および図３と共通の構成要素には同一の符号を付しており、ここではその詳細な説明を省略する。
【０１３５】
図１６において、ナビゲーション装置１３０は、位置情報取得部１３１、位置情報算出部１３２、画像合成部１３３および地図情報記憶部１３４を備えている。位置情報取得部１３１はいわゆるＧＰＳ機能を有しており、複数の人工衛星からの電波を受信して車両の位置に関する情報を取得する。また、位置情報取得部１３１は、衛星からの電波が受信可能であり、ＧＰＳ機能によって位置情報が取得可能な場合（ＧＰＳ機能が有効な場合）は取得した位置情報を出力し、衛星からの電波が受信できないなどの理由によりＧＰＳ機能によって位置情報を取得できない場合（ＧＰＳ機能が無効な場合）は、位置情報が取得不可能であることを示す情報を出力する。位置情報算出部１３２は位置情報取得部１３１からの位置情報と、移動体動き算出装置１００から出力された移動体動き情報（第２の動きＭｂ）とを入力して、車両の現在位置を算出する。地図情報記憶部１３４は地図座標に対応した道路や駐車場、地図の情報を保持している。画像合成部１３３は位置情報算出部１３２から現在位置を入力して、現在位置に対応する道路や地図の情報を地図情報記憶部１３４から読み出し、画像に変換して出力する。
【０１３６】
図１６のナビゲーションシステムの動作について、説明する。いま、車両は屋外にあり、衛星からの電波が受信可能であり、ＧＰＳ機能が有効となっているものとする。このとき、位置情報取得部１３１はＧＰＳ機能によって得られた位置情報を出力し、位置情報算出部１３２は位置情報取得部１３１から受けた位置情報を、そのまま現在位置として出力するとともに、一時的に記憶する。画像合成部１３３は位置情報算出部１３２から現在位置を受け、地図情報記憶部１３４に記憶された現在位置周辺の地図情報を読み出し、地図画像に変換して出力する。この地図画像は、ディスプレイ１２２に表示される。
【０１３７】
また、位置情報取得部１３１がＧＰＳ機能によって位置情報を取得可能であるときは、位置情報算出部１３２は、移動体動き算出装置１０２から入力された移動体動きの情報を無視する。
【０１３８】
次に、車両が運転者の操作によって移動し、屋内駐車場などのような、衛星からの電波が受信できずＧＰＳ機能によって位置情報が取得できない場所に移動したものとする。このとき、位置情報取得部１３１はＧＰＳ機能が無効であることを表す情報を出力する。位置情報算出部１３２はこの情報を入力したとき、内部に記憶していた現在位置に、移動体動き算出装置１００から出力された移動体動きを積算し、新たな現在位置として出力するとともに一時的に記憶する。画像合成部１３３はＧＰＳ機能が有効な場合と同様に動作し、位置情報算出部１３２から現在位置を受け、地図情報記憶部１３４に記憶された現在位置周辺の地図情報を読み出し、地図画像に変換して出力する。この地図画像は、ディスプレイ１２２に表示される。
【０１３９】
また、位置情報算出部１３２は、現在位置を算出するとともに、必要に応じて地図情報記憶部１３４に記憶された駐車場の情報を読み出し、平面式算出部１１４に送る。平面式算出部１１４は、必要に応じて駐車場の情報に含まれる平面式の情報を読み出して出力する。
【０１４０】
さらに、車両が運転者の操作によって移動し、再び電波が受信可能となり、ＧＰＳ機能が有効になったとき、位置情報算出部１３２は、位置情報取得部１３１で得られた位置情報を、現在位置として出力する。
【０１４１】
図１７は屋内駐車場において、本発明に係る移動体動き算出方法を用いた実験により得られた車両の移動軌跡を示すグラフである。上述したように、本発明に係る移動体動き算出方法によると、車両に設置したカメラ画像のみを用いて移動体動きを算出することが可能であり、しかも、路面が傾いている場合であっても誤差を小さく抑えることが可能であることから、図１７に示すように、屋内駐車場においても誤差の小さい移動軌跡の算出が可能となる。
【０１４２】
ところが、図８（ｂ）のグラフを用いてすでに説明したとおり、移動体の移動速度が速くなるほど、対応点探索の誤差が大きくなる（対応点正解率が低下する）傾向があり、対応点の誤差が大きくなりすぎた場合は、移動体動きの誤差が大きくなる。
【０１４３】
このような問題に対して、本実施形態のナビゲーションシステムでは、移動体動き算出装置１００の利用は衛星からの電波が受信できない屋内だけにとどめて、衛星からの電波が受信可能な屋外では、ＧＰＳ機能によって取得した位置情報を現在位置として用いている。これにより、精度の高い現在位置の算出を可能にしている。
【０１４４】
すなわち、本実施形態では、ＧＰＳ機能によって位置情報が取得できるか否かに応じて、現在位置の算出方法を選択的に切り替えることによって、ＧＰＳ機能が有効となる屋外と、電波が受信できない屋内とのいずれにおいても、常に現在位置の算出が可能となる。
【０１４５】
なお、本実施形態では、現在位置の算出方法の切り換えを、ＧＰＳ機能によって現在位置が取得できるか否かに応じて行うものとしたが、これに限られるものではなく、例えば、算出した現在位置が地図情報上で屋内駐車場の入り口近辺であるか否かに応じて、現在位置の算出方法を切り替えるようにしてもよい。
【０１４６】
なお、本実施形態においても、第１の実施形態と同様に、動き算出のために仮定した平面は、路面に限定されるものではなく、移動体との相対的な位置を予め与えることができる平面であれば、どのような平面であってもかまわない。
【０１４７】
例えば、屋内駐車場が図１４のような構造である場合に、その天井を平面と仮定した場合の平面式を、地図情報とともに地図情報記憶部１３４に記憶しておく。そして、位置情報算出部１３２は、現在位置が図１４の屋内駐車場に該当するとき、地図情報記憶部１３４から屋内駐車場の天井の平面式を読み出し、第１の動き算出部１０２の平面式算出部１１４に出力する。平面式算出部１０４は、位置情報算出部１３２から平面式が出力されたときは、その平面式を出力する。これにより、移動体動き算出装置１００は、天井平面を基準とした移動体動きを算出する。
【０１４８】
本発明では、カメラで撮影された画像を用いて移動体の動きを算出するため、例えば、路面が暗く、天井が明るい屋内駐車場では、路面よりも天井を平面として仮定した方が、算出精度が向上することが期待される。したがって、位置情報に対応した平面式を予め地図情報記憶部１３４に記憶しておき、移動体動き算出装置１００が、現在位置に応じて平面式を選択して用いることによって、より精度の高い移動体動きが算出可能となる。
【０１４９】
なお、上述の各実施形態では、移動体として車両を例にとって説明を行ったが、移動体の種類は車両に限られるものではなく、例えば、ロボットであってもよい。
【０１５０】
なお、上述の各実施形態において、各処理手段および装置は、ハードウェアによって実現されてもよいし、ソフトウェアによって実現してもよい。例えば、図１８のような、ＣＰＵ１４１、ＲＯＭ１４２およびＲＡＭ１４３と、画像の入出力機能とを備えたコンピュータを用いて、各処理手段をソフトウェアによって実現してもよい。
【産業上の利用可能性】
【０１５１】
本発明は、カメラが設置された移動体の動きを、カメラ画像を用いて精度良く求めることができるので、例えば、車両や自走ロボットなどに適用し、その走行制御や、操作者に対する進路や位置の提示サービスなどに、利用することが可能である。
【図面の簡単な説明】
【０１５２】
【図１】本発明の第１の実施形態における状況を示す図である。
【図２】本発明の第１の実施形態に係る構成を示すブロック図である。
【図３】図２における第１の動き算出部の詳細な構成を示すブロック図である。
【図４】（ａ），（ｂ）は図１の状況で撮影された画像の例、（ｃ）はその対応点を示す図である。
【図５】カメラの動きと画像変化との関係を示す図である。
【図６】各対応点に対する動きＭｊの評価値Ｅｊｉの分布を概念的に示すグラフである。
【図７】算出した動きに対応する予測軌跡が重畳された合成画像の例である。
【図８】（ａ）は車両に設置したカメラによる実際の画像の例、（ｂ）は（ａ）の状況における車両の移動速度と対応点正解率との関係を示すグラフである。
【図９】（ａ）は車両が傾いた場合の画像例、（ｂ）は（ａ）の状況における各対応点に対する動きＭｊの評価値Ｅｊｉの分布である。
【図１０】（ａ）は車両後方に他の車両が存在している状況を示す図、（ｂ）は（ａ）の状況において撮影された画像の例である。
【図１１】静止対応点率Ｒと真の静止対応点率Ｒtrueとの比（Ｒ／Ｒtrue）とカメラ動きの誤差ＥＭとの関係を示すグラフである。
【図１２】（ａ），（ｂ）は平面パラメタを用いた探索を概念的に示す図である。
【図１３】注意メッセージなどが重畳された合成画像の例である。
【図１４】屋内駐車場における撮影画像の例である。
【図１５】アッカーマンモデルに基づく移動体動きの表現を示す図である。
【図１６】本発明の第２の実施形態に係るナビゲーションシステムの構成図である。
【図１７】屋内駐車場における実験により得られた車両の移動軌跡を示すグラフである。
【図１８】本発明を実施するための他の構成を示す図である。
【特許請求の範囲】
【請求項１】
カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて、動きを求める装置であって、
撮影時刻が異なる２枚の前記画像から、複数の対応点を求める対応点算出部と、
前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて、前記移動体の動きを示す第１の動きを求める第１の動き算出部と、
前記第１の動き算出部によって求められた第１の動きと、前記複数の対応点とを用いて、前記移動体の動きを示す第２の動きを求める第２の動き算出部とを備え、
前記第１の動き算出部は、前記対応点を３個用いて、前記第１の動きを求めるものである
ことを特徴とする移動体動き算出装置。
【請求項２】
カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて、動きを求める装置であって、
撮影時刻が異なる２枚の前記画像から、複数の対応点を求める対応点算出部と、
前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて、前記移動体の動きを示す第１の動きを求める第１の動き算出部と、
前記第１の動き算出部によって求められた第１の動きと、前記複数の対応点とを用いて、前記移動体の動きを示す第２の動きを求める第２の動き算出部とを備え、
前記第１の動き算出部は、
前記複数の対応点の中から、ｍ（ｍは３以上の整数）個の対応点で構成される部分対応点を、ｑ（ｑは２以上の整数）組選択し、
選択したｑ組の部分対応点から、それぞれ、前記平面を表す平面式を用いて、前記移動体の動き候補を算出し、
算出したｑ個の動き候補を、所定の評価手法によって評価し、この評価結果に従って、前記第１の動きを特定する
ことを特徴とする移動体動き算出装置。
【請求項３】
請求項２において、
前記移動体は、車両であり、
前記第２の動き算出部は、
前記第１の動きの代わりに、この第１の動きに対応する部分対応点と、前記平面を表す平面式を用い、
前記平面式を初期値として、探索手法によって、前記カメラに近い車軸中心を中心として平面を傾けながら、前記複数の対応点に対する評価を行いつつ、前記第２の動きを求めるものである
ことを特徴とする移動体動き算出装置。
【請求項１】
カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて、動きを求める装置であって、
撮影時刻が異なる２枚の前記画像から、複数の対応点を求める対応点算出部と、
前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて、前記移動体の動きを示す第１の動きを求める第１の動き算出部と、
前記第１の動き算出部によって求められた第１の動きと、前記複数の対応点とを用いて、前記移動体の動きを示す第２の動きを求める第２の動き算出部とを備え、
前記第１の動き算出部は、前記対応点を３個用いて、前記第１の動きを求めるものである
ことを特徴とする移動体動き算出装置。
【請求項２】
カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて、動きを求める装置であって、
撮影時刻が異なる２枚の前記画像から、複数の対応点を求める対応点算出部と、
前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて、前記移動体の動きを示す第１の動きを求める第１の動き算出部と、
前記第１の動き算出部によって求められた第１の動きと、前記複数の対応点とを用いて、前記移動体の動きを示す第２の動きを求める第２の動き算出部とを備え、
前記第１の動き算出部は、
前記複数の対応点の中から、ｍ（ｍは３以上の整数）個の対応点で構成される部分対応点を、ｑ（ｑは２以上の整数）組選択し、
選択したｑ組の部分対応点から、それぞれ、前記平面を表す平面式を用いて、前記移動体の動き候補を算出し、
算出したｑ個の動き候補を、所定の評価手法によって評価し、この評価結果に従って、前記第１の動きを特定する
ことを特徴とする移動体動き算出装置。
【請求項３】
請求項２において、
前記移動体は、車両であり、
前記第２の動き算出部は、
前記第１の動きの代わりに、この第１の動きに対応する部分対応点と、前記平面を表す平面式を用い、
前記平面式を初期値として、探索手法によって、前記カメラに近い車軸中心を中心として平面を傾けながら、前記複数の対応点に対する評価を行いつつ、前記第２の動きを求めるものである
ことを特徴とする移動体動き算出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図８】
【公開番号】特開２００７−２４１３２６（Ｐ２００７−２４１３２６Ａ）
【公開日】平成１９年９月２０日（２００７．９．２０）
【国際特許分類】
【出願番号】特願２００６−５９２（Ｐ２００６−５９２）
【出願日】平成１８年１月５日（２００６．１．５）
【分割の表示】特願２００５−５１４８７７（Ｐ２００５−５１４８７７）の分割
【原出願日】平成１６年１０月１５日（２００４．１０．１５）
【出願人】（０００００５８２１）松下電器産業株式会社 (73,050)
【Ｆターム（参考）】
【公開日】平成１９年９月２０日（２００７．９．２０）
【国際特許分類】
【出願日】平成１８年１月５日（２００６．１．５）
【分割の表示】特願２００５−５１４８７７（Ｐ２００５−５１４８７７）の分割
【原出願日】平成１６年１０月１５日（２００４．１０．１５）
【出願人】（０００００５８２１）松下電器産業株式会社 (73,050)
【Ｆターム（参考）】
[ Back to top ]