JP2007241326A - 移動体動き算出装置 - Google Patents
移動体動き算出装置 Download PDFInfo
- Publication number
- JP2007241326A JP2007241326A JP2006000592A JP2006000592A JP2007241326A JP 2007241326 A JP2007241326 A JP 2007241326A JP 2006000592 A JP2006000592 A JP 2006000592A JP 2006000592 A JP2006000592 A JP 2006000592A JP 2007241326 A JP2007241326 A JP 2007241326A
- Authority
- JP
- Japan
- Prior art keywords
- motion
- movement
- corresponding points
- camera
- moving body
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
【課題】カメラが設置された移動体について、カメラ画像を用いてその動きを求める技術において、対応点がたとえ大きな誤差を含む場合であっても、精度良く、移動体動きを算出可能にする。
【解決手段】移動体動き算出装置100において、対応点算出部101はカメラ120によって撮影された画像間の対応点を算出する。第1の動き算出部102は画像内に所定の平面を仮定して、対応点を用いて移動体の第1の動きMaを算出する。第2の動き算出部103は第1の動きMaと対応点を用いて、第2の動きMbを算出する。
【選択図】図2
【解決手段】移動体動き算出装置100において、対応点算出部101はカメラ120によって撮影された画像間の対応点を算出する。第1の動き算出部102は画像内に所定の平面を仮定して、対応点を用いて移動体の第1の動きMaを算出する。第2の動き算出部103は第1の動きMaと対応点を用いて、第2の動きMbを算出する。
【選択図】図2
Description
本発明は、移動体の動きを算出する技術に関し、特に移動体に設置されたカメラで撮影した画像を用いて、3次元空間での移動体自身の動きを算出する技術に関するものである。
車両や自走ロボットなどの移動体の動きを算出する主な方法として、車輪の回転や操舵輪の角度、加速度、角加速度などを検出するセンサを用いる方法と、移動体に設置されたカメラで撮影した移動体周辺の画像を用いる方法との2つがある。
車輪の回転や操舵輪の角度を検出するセンサを用いる方法は、平坦で乾いた路面などの一定の環境下では誤差が小さく、検出不能となる場合も少ないため、安定であるという利点がある。しかし、路面と車輪が滑る、路面が平面ではない、といった環境下では、原理的に動きの誤差が大きくなるという欠点がある。
また、加速度や角加速度を検出するセンサを用いる方法では、原理的には、路面と車輪が滑る場合や路面が平面ではない場合でも、移動体の動きを算出可能である。しかし、重力加速度に対して動きや姿勢の変化による加速度や角加速度の比率が小さい、センサで発生する雑音やドリフトが比較的大きい、などの理由により、特に路面が水平な平面でない場合や移動体が傾いている場合に、動きの誤差が大きくなるという欠点がある。
一方、移動体に設置されたカメラで撮影した移動体周辺の画像を用いる方法の場合、路面が平面でない場合や移動体が傾いている場合でも、原理的には、移動体の動きを正しく算出することが可能である。
カメラの動きを、撮影した動画像を用いて求める代表的な方法として、オプティカルフローを用いる方法が提案されている。オプティカルフローは、被写体上のある1点に対応する、時間的に連続した2つの画像上の点すなわち対応点を結ぶベクトルである。対応点とカメラ動きとの間には、幾何学的な拘束式が成り立つため、複数の対応点が一定の条件を満たす場合にカメラの動きを算出することができる。
例えば、非特許文献1では、2画像間の静止した8点以上の対応点の組からカメラ動きを算出する、いわゆる8点法と呼ばれる方法が提案されている。しかしながら、この8点法では、対応点の算出結果に誤差が含まれるために、8点以上の誤差の小さい対応点を抽出することが困難であるという課題や、カメラ動きの導出式の不安定性から、対応点の誤差が比較的小さくてもカメラ動きの誤差が大きくなることがあるという課題があることが、広く知られている。
また、8点法に限らず、実際に撮影された画像を用いてカメラの動きを算出する場合、画像処理の過程で発生する、はずれ値様の大きな誤差や、画像中の雑音や量子化誤差などの計測誤差を0にするのは、実質的に不可能といってよい。
そのため、画像を用いてカメラ動きを算出する方法では、誤差の影響を抑制することを目的として、さまざまな方法が提案されている。例えば、対応点間の拘束条件を与えることによってより少ない対応点からカメラ動きを算出する方法や、カメラ動きの算出過程における未知パラメタ数を減らしてカメラ動きを算出する方法、未知パラメタを多段階に算出する方法、カルマンフィルタなどのフィルタを用いる方法、などがある。
例えば、非特許文献2および特許文献1では、入力となる対応点は全て同一平面上に存在するものと仮定した上で、4点以上の対応点からカメラ動きと平面の式を算出する方法が提案されている。この方法は、複数の対応点がある拘束条件を満たすものと仮定して、8点より少ない対応点からカメラ動きを算出する方法の例である。
カメラ動きの算出に用いる対応点の数が少ないということは、対応点がある確率で誤差を含む場合に、算出したカメラ動きが誤差を含む確率が低くなるという効果が期待できる。さらに、4点より多くの対応点を用いた場合には、二乗誤差を最小化する方法でカメラ動きを算出することによって、対応点に含まれる計測誤差の影響を低減できるという効果が期待できる。
また例えば、特許文献2では、路面平面の対応点を選択して算出されたカメラ動きに対して、車両挙動やカメラ配置を反映したカルマンフィルタをかけることによって、誤差の小さいカメラ動きを算出する方法が提案されている。算出したカメラ動きにフィルタ処理をかけることによって、カメラ動き算出における誤差を低減することができる。
また例えば、特許文献3および非特許文献3では、カメラの動きを平面上の平行移動と回転の3パラメタに限定した上で、対応点とカメラ動き間の幾何学的拘束式と、対応点の近傍の小領域間の画素値に関する光学的拘束式を用いて、探索的な手法によってカメラの動きを算出する方法が提案されている。カメラの動きを限定して未知パラメタを減らすことによって、カメラ動き算出における誤差を低減することができる。
また例えば、非特許文献4では、対応点からカメラ動きを算出する際の未知パラメタのうち、回転と平行移動とを2段階に分けて算出することで、1回の処理で求める未知パラメタを少なくし、カメラ動き算出における誤差を低減する方法が提案されている。
国際公開第97/35161号パンフレット
特開2001−266160号公報
特表2003−515827号公報
"A computer algorithm for reconstructing a scene from two projections.", H. C. Longuet-Higgins, Nature, 293:133-135, Sept. 1981
"画像理解"金谷健一、森北出版、1990年
"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
しかしながら、上述した従来の方法では、それぞれ次のような問題があった。
まず、対応点の拘束条件を与えることによって、より少ない対応点からカメラ動きを算出する方法では、多数の対応点の中から、特定の拘束条件を満たす対応点を抽出する必要がある。しかし、この対応点を抽出する過程における誤りを0にすることは困難であり、誤って抽出された対応点からカメラ動きを算出した場合には、カメラ動きの誤差が大きくなるという別の課題がある。例えば、金谷の方法(非特許文献2)は、平面上の4点以上の対応点からカメラの動きを算出可能であるが、画像中の複数の対応点から同一平面上に存在する静止した対応点を抽出する必要がある。しかし、移動体に設置したカメラで撮影された画像に適用する場合には、同一平面上に存在する静止した対応点を誤りなく抽出するのは困難である。このため、対応点を抽出する過程における誤りによってカメラ動きの誤差が大きくなってしまう、という別の問題が生じる。
また、フィルタ処理は、カメラ動きの算出結果に含まれる誤差の影響を低減する方法として有効である。しかし、カメラ動きや誤差の原因が予め仮定したモデルに従うことが前提であるため、例えば、車両が低速で走行する場合、路面の凹凸や段差などの予測困難な外乱の影響が大きくなるため、フィルタそのものの設計が困難である。このため、フィルタのパラメタが適当でない場合に、逆に誤差が大きくなってしまうという問題がある。
また、未知パラメタを減らす方法では、実際のカメラの動きが未知パラメタを減らした関係式と近い場合は、誤差が小さい動きを算出できる。しかし、実際のカメラの動きが未知パラメタを減らした関係式から外れた場合、誤差が大きくなってしまう。例えば、Steinらの方法(特許文献3および非特許文献3)では、カメラは特定の平面上しか移動しないと仮定して、平面上の平行移動2パラメタと平面上の回転1パラメタの計3パラメタのみを算出している。このため、カメラが傾いた場合や路面が平面でない場合に、カメラ動きの誤差が大きくなるという問題がある。
さらに、未知パラメタを2段階に算出する方法では、各段階で適切な未知パラメタが選択されている場合、有効である。しかし、パラメタの選択が適当でない場合、各段での推定において、未知パラメタを減らす方法と同様に、誤差が大きくなる。例えば、Iraniらの方法(非特許文献4)は、回転と平行移動を分けて2段階で求めているため、例えば、回転と平行移動のいずれの成分も無視できない程度に含まれる場合に、各段の推定を正しく求めることができない場合がある。
前記の問題に鑑み、本発明は、カメラが設置された移動体について、カメラ画像を用いてその動きを求める技術において、対応点(オプティカルフロー)を用い、対応点がたとえ大きな誤差を含む場合であっても、精度良く、移動体動きを算出可能にすることを課題とする。
本発明は、カメラが設置された移動体について、カメラによって撮影された移動体周囲の画像を用いて動きを求める際に、撮影時刻が異なる2枚のカメラ画像から複数の対応点を求め、カメラ画像内に所定の平面を仮定した上で、求めた複数の対応点を用いて、移動体の第1の動きを求め、この第1の動きと複数の対応点とを用いて、移動体の第2の動きを求めるものである。
本発明によると、第1の動きを求める際に、所定の平面を仮定し、一部の対応点が所定の平面上に存在するという拘束条件を与えることによって、未知パラメタを減らすことができ、平面に対するおおまかな、大きな誤差のない第1の動きを求めることができる。そして、この第1の動きを用いることによって、例えば平面が実際には傾いていたような場合であっても、誤差が小さい第2の動きを求めることができる。
また、前記第1の動きの算出は、複数の対応点の中から、m(mは3以上の整数)個の対応点で構成される部分対応点をq(qは2以上の整数)組選択し、選択したq組の部分対応点からそれぞれ、平面を表す平面式を用いて、移動体の動き候補を算出し、算出したq個の動き候補を所定の評価手法によって評価し、この評価結果に従って、前記第1の動きを特定するのが好ましい。
これにより、対応点に含まれるはずれ値様の大きな誤差に影響されないで、誤差の小さい移動体の第1の動きを算出することができる。
また、前記第2の動きの算出は、第1の動きを初期値として、探索手法によって、複数の対応点に対する評価を行いつつ、第2の動きを求めるのが好ましい。これにより、より精度の高い、移動体の第2の動きを算出することができる。
本発明によると、2段階の動き算出ステップを含み、平面を仮定して第1の動きを算出した後に、この第1の動きを用いて第2の動きを算出するので、例えば、移動体と路面が傾いた状況であっても、移動体の動きを精度良く算出することができる。
本発明の第1態様では、カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて動きを求める装置として、撮影時刻が異なる2枚の前記画像から複数の対応点を求める対応点算出部と、前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて前記移動体の動きを示す第1の動きを求める第1の動き算出部と、前記第1の動き算出部によって求められた第1の動きと前記複数の対応点とを用いて、前記移動体の動きを示す第2の動きを求める第2の動き算出部とを備え、前記第1の動き算出部は、前記対応点を3個用いて前記第1の動きを求めるものを提供する。
本発明の第2態様では、カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて動きを求める装置として、撮影時刻が異なる2枚の前記画像から複数の対応点を求める対応点算出部と、前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて前記移動体の動きを示す第1の動きを求める第1の動き算出部と、前記第1の動き算出部によって求められた第1の動きと前記複数の対応点とを用いて、前記移動体の動きを示す第2の動きを求める第2の動き算出部とを備え、前記第1の動き算出部は、前記複数の対応点の中からm(mは3以上の整数)個の対応点で構成される部分対応点をq(qは2以上の整数)組選択し、選択したq組の部分対応点からそれぞれ、前記平面を表す平面式を用いて前記移動体の動き候補を算出し、算出したq個の動き候補を所定の評価手法によって評価し、この評価結果に従って前記第1の動きを特定するものを提供する。
本発明の第3態様では、前記移動体は車両であり、前記第2の動き算出部は、前記第1の動きの代わりに、この第1の動きに対応する部分対応点と前記平面を表す平面式を用い、前記平面式を初期値として、探索手法によって、前記カメラに近い車軸中心を中心として平面を傾けながら、前記複数の対応点に対する評価を行いつつ、前記第2の動きを求めるものである第2態様の移動体動き算出装置を提供する。
以下、本発明の実施の形態について、図面を参照して説明する。
(第1の実施形態)
本発明の第1の実施形態では、移動体として、カメラが設置された車両を例にとり、カメラによって撮影された車両後方の画像を用いて、車両の動きを求めるものとする。
本発明の第1の実施形態では、移動体として、カメラが設置された車両を例にとり、カメラによって撮影された車両後方の画像を用いて、車両の動きを求めるものとする。
図1は本実施形態における状況を示す図である。図1に示すように、移動体としての車両1には、その後部に、後方周辺を撮影するようにカメラ120が設置されている。カメラ120は車両後方周辺を撮影して一連の画像列を出力する。
図2は本実施形態に係る移動体動き算出装置を含む構成を示すブロック図である。図2において、100はカメラ120によって撮影された画像から車両1の動きを求める移動体動き算出装置、121はカメラ120によって撮影された画像に、移動体動き算出装置100によって求められた車両1の動きに関する情報を重畳した合成画像を生成する画像合成装置、122は画像合成装置121によって生成された合成画像を表示するディスプレイである。
移動体動き算出装置100は、カメラ120から出力された連続する2画像間から対応点(オプティカルフローベクトル)を複数個求める対応点算出部101と、対応点算出部101から出力された複数の対応点を用いて、平面を仮定して、車両1の動きを示す第1の動きMaを求める第1の動き算出部102と、対応点算出部101から出力された複数の対応点と、第1の動き算出部102から出力された第1の動きMaとを用いて、車両1の動きを示す第2の動きMbを求める第2の動き算出部103とを備えている。
図2の移動体動き算出装置100、画像合成装置121およびディスプレイ122は、例えば、車両1内に設置されるが、車両1とは異なる場所に設けることも可能である。
図3は図2の移動体動き算出装置100における第1の動き算出部102の詳細な構成を示すブロック図である。図3において、111は対応点算出部101から出力された複数の対応点から、所定数の対応点からなる部分対応点を複数組選択する部分対応点選択部、112は部分対応点選択部111によって選択された各組の部分対応点から、車両1の動き候補をそれぞれ算出する動き候補算出部、113は動き候補算出部112によって算出された動き候補を所定の評価手法によって評価し、この評価結果に従って、第1の動きMaを特定する動き評価選択部である。
また、114は第1の動きMaを算出する際に仮定する所定の平面を表す平面式を出力する平面式算出部、115は静止した対応点の比率である静止対応点率Rを出力する静止対応点率算出部、116は移動体動きの予測値を求める移動体動き予測部である。
ここでは、図1の状況において、車両1が後進した場合を例にとって、説明を行う。
図1において、世界座標系(Xw,Yw,Zw)は3次元空間中で静止しており、車両座標系(Xc,Yc,Zc)および視点座標系(X,Y,Z)は車両1に固定されているものとする。視点座標系の座標値(x,y,z)と車両座標系の座標値(xc,yc,zc)との関係は、座標系間の位置関係を表す行列Cを用いて、(数1)のように表すことができる。
世界座標系に対して静止した点が、車両座標系における車両の動きMcによって、車両座標系の座標値(xc,yc,zc)から(xc’,yc’,zc’)に移動した場合、(数2)の関係式が成り立つ。さらに、このときの視点座標系の動きがMであり、同じ点の視点座標系の座標値が(x,y,z)から(x’,y’,z’)に移動したとすると、(数3)の関係式が成り立つ。したがって、車両1の動きMcとカメラ120の動きMとの間には(数4)の関係式が成り立つ。
以上のことから、車両座標系と視点座標系の位置関係を示すCが既知のとき、車両座標系を基準にした車両1の動きMcを求めるためには、視点座標系を基準にしたカメラ120の動きMを算出して、(数4)で変換すればよい。カメラ120と車両1との相対位置が固定されている場合、カメラ120の動きと車両1の動き(移動体の動き)とは一意に変換できるため、本願明細書では、特に明示がない限り、区別はしないものとする。
図4(a),(b)は図1の状況において、車両1が後方に移動した場合にカメラ120から出力される一連の画像列の例であり、図4(a)は時刻t−1に撮影された画像、図4(b)は時刻tに撮影された画像である。また、図4(c)は、時刻t−1と時刻tの2画像間の同一点の動きである対応点(図中の矢印)を示す図である。
図5はカメラ120の3次元的な動きと画像変化との関係を示す図である。図5(a)に示すように、カメラ120が時刻t−1から時刻tの間に動きMで移動したとき、世界座標系における静止点Xが、視点座標系の座標値(x,y,z)から(x’,y’,z’)に移動したものとする。このとき、座標値(x,y,z)、(x’,y’,z’)とカメラ動きMとの間には、(数3)が成り立つ。図5(b)(c)はそれぞれ時刻t−1,tにおける画像であり、時刻t−1において視点座標(x,y,z)の点Xが画像座標(u,v)に、時刻tにおいて視点座標(x’,y’,z’)の点Xが画像座標(u’,v’)に、それぞれ対応している。このとき(u,v)と(x,y,z)、(u’,v’)と(x’,y’,z’)の関係と(数3)を用いると、(数5)の関係式が成り立つ。(数5)において、fはカメラの焦点距離を示す。
このとき、静止点X(x,y,z)が(数6)の平面式で表される平面上に存在し、かつ、カメラ120の動きM(tx,ty,tz,wx,wy,wz)が十分に小さく(数7)が成り立つとき、点Xの画像座標(u,v)、(u’,v’)とカメラ動きMとの間に(数8)の関係式が成り立つ。さらに、(数8)をカメラ動きMの要素に着目して整理すると、(数9)が得られる。1組の対応点ごとに(数9)の2つの式が成り立つことから、平面式とカメラ120の焦点距離fが既知の場合は、独立した(同一直線上ではない)対応点(u,v),(u’,v’)が3組以上与えられれば、カメラ120の動きM(tx,ty,tz,wx,wy,wz)の6変数を算出することができる。
以下、時刻t−1から時刻tの間のカメラ動きMを算出する場合を例にとって、説明を行う。なお、上述したように、カメラの動きと移動体の動きは(数4)により一意に変換できるため、カメラ動きが算出できれば、移動体の動きも実質的に算出されたことになる。
ここでは、車両座標系(Xc,Yc,Zc)および視点座標系(X,Y,Z)は図1のように配置されているものとし、車両座標系に対する視点座標系の位置関係を表すCは、予め計測されているものとする。また、カメラ120の焦点距離fも予め求められており、その値は、実座標系での焦点距離に、実座標と画像座標との変換係数を乗じたものとする。
対応点算出部101はカメラ120から出力された一連の画像列を一時的に保存し、画像列のうちの連続した2枚の画像から、対応点をn個(nは2以上の整数)検出する。ここでは、時刻t−1の画像上のn点の座標(ui,vi)(i=1,…,n)について、対応する、時刻tの画像上の座標(ui’,vi’)(i=1,…,n)をそれぞれ検出する。検出したn点の対応点の情報を、Pi(ui,vi,ui’,vi’)(i=1,…,n)と表す。対応点の個数nは、予め定められた値とする。
なお、対応点を検出する2枚の画像は、必ずしも連続している必要はなく、撮影時刻が異なっていればよい。
ここでの対応点検出は、例えば、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.を参照)。この方法は、一般に広く知られた方法であるので、ここでは詳細な説明を省略する。
第1の動き算出部102は、対応点算出部101によって検出されたn個の対応点Pi(ui,vi,ui’,vi’)(i=1,…,n)を用いて、車両1の第1の動きMaを算出する。以下、第1の動き算出部102を構成する各処理手段の動作について説明する。
平面式算出部114は、ここでは所定の平面式を出力する。ここで用いる平面式は、図1の状況における路面を平面として近似したものとする。例えば、車両が静止した状態で、水平かつ平坦な路面平面の式を予め計測しておき、平面式算出部114は、予め計測した(数6)の平面式のパラメタS(a,b,c)を出力する。なお、平面式算出部114は、平面式を更新する機能も有する。これについては、後述する。
静止対応点率算出部115は、静止対応点率Rを出力する。ここで、静止対応点率とは、好ましくは、対応点算出部101によって算出されたn個の対応点のうち、世界座標に対して静止しており、かつ、対応関係が正しい対応点の比率を表すものである。ここで、対応点の対応関係が正しいとは、対応点の座標値の誤差が所定の閾値以下であることを意味する。なお、実時間でのオンライン処理において、静止対応点率Rを事前に正確に得ることは極めて困難であり、実際には、静止対応点率Rとは、上述した比率の予測値とする。
ここでは、静止対応点率算出部115は、静止対応点率Rとして、所定の値、例えば0.5を出力する。静止対応点率R=0.5とは、「対応点算出部101によって算出されたn個の対応点に、静止した、対応関係の正しい対応点が50%含まれる」と予測していることを意味する。所定の値としては、例えば、図1と類似の走行状態において静止対応点率を予め複数回計測し、計測した中の最低値を用いればよい。なお、静止対応点率算出部115は、静止対応点率Rを新たに算出する機能も有する。これについては、後述する。
移動体動き予測部116は、車両1の動きの予測値を求める機能を有するが、ここでは、特に処理を行わないものとする。
部分対応点選択部111は、対応点算出部101から出力されたn個の対応点Pi、平面式算出部114から出力された平面式S、および静止対応点率算出部115から出力された静止対応点率Rを用いて、m個の対応点で構成される部分対応点をq組選択する。ここで、部分対応点を構成する対応点の個数mは、移動体の動きを算出するために必要となる対応点の数である。(数9)を用いてカメラ動きを算出するためには、最低3個の対応点が必要である。このため、ここでは、m=3とする。もちろん、mは3より大きい値であってもかまわない。
部分対応点選択部111は、まず、部分対応点の組数qを算出する。部分対応点の組数qは、その値が大きいほど、第1の動き算出部102での処理時間が長くなり、逆にqが小さいほど処理時間は小さくなる。そのため、例えば処理時間の制約がある場合は、その上限からqを決定しても良い。
ここでは、部分対応点の組数qを(数10)の関係式から求めるものとする。
(数10)において、Rrは路面静止対応点率、zは保証率である。
路面静止対応点率Rrは、部分対応点として選択された各対応点が、路面上に存在し、世界座標に対して静止しており、かつ対応関係が正しい対応点の確率である。したがって、対応点が路面上に存在する確率Rsと、対応点の静止対応点率Rとの間に相関がない場合、対応点が路面上に存在する確率Rsに静止対応点率Rを掛けたものが路面静止対応点率Rrとなる。また、もしも路面上に存在する対応点を正確に選択することが可能であり、かつ、選択した対応点の静止対応点率がRである場合、選択した対応点の路面対応点率Rrは、静止対応点率Rと等しくなる。ここでは、所定の値として、例えば、Rr=0.3を用いる。所定の値としては、例えば図1と類似の走行状態において、選択後の対応点が路面上に存在する確率Rsを計測しておき、これと静止対応点率Rをかけた値を用いればよい。
保証率zは、q組の部分対応点のうち、全て(m個)の対応点が路面上の静止した正しい対応点で構成された部分対応点が少なくとも1組存在することを統計的に保証する率である。ここでは、所定の値として、例えば、z=0.999を用いる。言い換えると、少なくとも1組の部分対応点が路面上の正しい対応点で構成されることを、保証率zで保証するためには、q組の部分対応点を選択すればよい。
(数10)において、qとRrが一定の場合、部分対応点を構成する対応点数mが小さいほど保証率zが大きくなり、zとRrが一定の場合は、mが小さいほどqが小さくなる。したがって、対応点の数mは小さいほど、処理時間が短く、部分対応点の少なくとも1組が路面上の正しい対応点で構成される可能性が高くなる。このことと、(数9)の算出には3点以上の対応点数が必要であることから、最良のmは3となる。
次に、部分対応点選択部111は、算出した組数qを用いて、n個の対応点から、異なるm個の対応点からなる部分対応点をq組選択して出力する。部分対応点を選択する方法として、n個の全ての対応点からランダムに選択してもよいし、特定の条件を満たす対応点を抽出した上で、そこからランダムに選択するとしてもよい。ここでは後者を用いる。すなわち、平面式Sが存在し得る画像領域内の対応点を抽出した上で、抽出した対応点から、ランダムに、異なるm個の対応点からなる部分対応点をq組選択して出力する。例えば、図4(d)において、画像中で平面Sが存在し得る領域は、水平線より下の斜線領域に限られることを示している。この方法では、斜線領域に存在する対応点を抽出した上で部分対応点を選択する場合の路面静止対応点率Rrは、n個の対応点からランダムに選択する場合の路面静止対応点率Rrに比べて、より高い値となり、その結果、部分対応点の組数qをより小さい値にすることができる。
動き候補算出部112は、部分対応点選択部111によって選択されたq組の部分対応点から、それぞれ、平面式Sを用いて、q個の動き候補を算出する。
上述したように、画像中の対応点Pi(u,v,u’,v’)が平面式S(a,b,c)上にあり、かつ、カメラの動きMが小さいと仮定した場合、(数9)の関係式が成り立つ。(数9)において、対応点と平面式が既知、カメラの動きMを未知とすると、対応点が3個以上あればカメラの動きMを算出することができる。そこで、動き候補算出部112は、3個の対応点からなるq組の部分対応点について、それぞれ、(数9)の関係式を用いて、q個のカメラの動きMj(j=1,…,q)を算出し、これを車両1の動き候補として出力する。
動き評価選択部113は、動き候補算出部112によって算出されたq個の動き候補(カメラ動きMj(j=1,…,q))を、所定の評価手法によって評価し、この評価結果に従って、第1の動きMaを特定する。ここでの評価では、対応点算出部101から出力されたn個の対応点Pi、および静止対応点率算出部115から出力された静止対応点率Rを用いる。なお、必要に応じて、移動体動き予測部116から出力された動き予測を用いてもよい。これについては、後述する。
ここでは、まず、カメラ動きMjとn個の対応点Piとの一致する度合いを評価する。具体的には、それぞれのカメラ動きMjについて、n個の対応点(ui,vi,ui’,vi’)(i=1,…,n)を(数11)に代入し、n個の対応点の3次元座標(xi,yi,zi)を算出する。ここで、(数11)は未知数4に対して独立した一次式が4つあること、および、画像から算出した対応点Pi(ui,vi,ui’,vi’)は誤差を含むと考えられることから、最小二乗法を用いて3次元座標(xi,yi,zi)を算出する。
さらに、算出した3次元座標(xi,yi,zi)とカメラ動きMjを(数11)に再び代入して、新たに対応点(uij,vij,uij’,vij’)を算出する。ここで、対応点Pi(ui,vi,ui’,vi’)およびカメラ動きMjが誤差のない正確な値である場合、Pi(ui,vi,ui’,vi’)と(uij,vij,uij’,vij’)は一致し、誤差がある場合は一致しない。そこで、(ui,vi,ui’,vi’)と(uij,vij,uij’,vij’)の誤差距離平均Ejiを、(数12)から算出する。Ejiの値は、カメラ動きMjと対応点Piが一致するときは、0になり、一致しないときは、値が大きくなるという性質をもつことから、Ejiをカメラ動きMjと対応点Piの一致度合いに関する評価値として用いる。
次に、n個の対応点Piそれぞれに対するカメラ動きMjの評価値Eji(i=1,…,n)のうち、評価の良い方すなわち値の小さい方から(n×R)番目の評価値を、カメラ動きMjの評価値Ejとして選択する。
そして、q個のカメラ動きMj(j=1,…,q)の中で、それぞれの評価値Ej(j=1,…,q)のうち評価が最も良い、すなわち値が最も小さいものに対応するカメラ動きを、第1の動きMaとして選択する。
図6はn個の対応点に対するある1つのカメラ動きMjの評価値Eji(i=1,…,n)の分布を概念的に示すグラフである。図6では、評価値Ejiを小さい順にソートしており、横軸はソート後の対応点番号(1,…,n)、縦軸は各対応点に対するカメラ動きMjの評価値Ejiの値である。
ここで、カメラ動きMjが正しく、かつ、n個の対応点のうち(n×R)個が静止した正しい対応点であるとすると、理想的には図6(a)のように、小さい方から(n×R)番目までの評価値Eijは0で、それ以降は次第に値が大きくなる。このため、評価値Ejも0となる。しかし、実際には対応点の計測誤差などの小さな誤差が存在するため、(n×R)番目までの評価値Eijは必ずしも0にはならない。
一方、カメラ動きMjが正しくない場合は、図6(b)のように、評価値Ejiの多くが大きな値となり、評価値Ejも大きな値となる。このことを言い換えると、静止対応点率Rを前提条件とした場合、カメラ動きMjの評価値Ejが小さいということは前提条件を満たすことを意味し、さらに、評価値Ejが最小となるカメラ動き(すなわち第1の動きMa)は、前提条件を最も良く満たすカメラ動きであることを意味する。
このような動作によって、第1の動き算出部102は、時刻t−1と時刻tの画像上のn個の対応点Piを用いて、車両1の動きである第1の動きMaを算出して出力する。
第2の動き算出部103は、第1の動き算出部102から出力された第1の動きMaと、対応点算出部101から出力されたn個の対応点Piとを用いて、車両1の動きを示す第2の動きMbを算出する。ここでは、第1の動きMaを初期値とした探索的手法によって、第2の動きMbを算出するものとする。
具体的には、まず、第1の動きMa(tx,ty,tz,wx,wy,wz)を初期値として、各パラメータに対して所定の微小動き(dtx,dty,dtz,dwx,dwy,dwz)をそれぞれ増減して、複数の動き候補を得る。これにより、o=3^6個の動き候補Mbk=(tx±dtx,ty±dty,tz±dtz,wx±dwx,wy±dwy,wz±dwz)(k=1,…,o)が得られる。
次に、第1の動き算出部102の動き評価選択部113と同様に、o個の動き候補Mbkについて、それぞれ、n個の対応点に対する評価値Ebki(i=1,…,n)を算出する。そして、小さい方から(n×R)番目の評価値Ebkiを動き候補Mbkの評価値Ebkとし、さらに、評価値Ebkが最小となる動き候補Mbkを選択する。
次いで、選択された動き候補Mbkを初期値として、上と同様の処理を行う。これを繰り返し実行し、値が収束したとき、または、繰り返し回数が所定回数に達したとき、処理を終了する。そして、第2の動き算出部103は、そのとき選択されている動き候補Mbkを第2の動きMbとして出力する。
このような動作により、移動体動き算出装置100は、時刻t−1から時刻tの間の車両1の動きを算出する。また、カメラ120から出力された画像列について、同様の処理を繰り返すことによって、車両1の動きを連続して算出することができる。
そして、画像合成装置121は、カメラ120によって撮影された画像と、移動体動き算出装置100から出力された第2の動きMbを入力して、車両1の動きに関する情報が重畳された合成画像を生成する。ディスプレイ122は画像合成装置121によって生成された合成画像を表示する。図7は表示された合成画像の例であり、車両1の第2の動きMbに対応する予測軌跡TRが、カメラ120で撮影された画像に重畳されている。
<本実施形態の効果>
本実施形態によると、例えば、対応点探索の誤りが多く、大きな誤差を含む場合であっても、車両の動きを精度良く算出することができる。
本実施形態によると、例えば、対応点探索の誤りが多く、大きな誤差を含む場合であっても、車両の動きを精度良く算出することができる。
対応点算出部101の動作説明で触れたKanade, Lucas and Tomasiの方法は、高い精度と少ない計算量の両面で優れた対応点探索方法として広く知られている。しかし、そのような優れた方法であっても、制約の無い環境で撮影された画像において対応点探索の誤りを0にすることは、まず不可能と言ってよい。
図8(a)は実際に車両の後方にカメラを設置して撮影した画像の例である。図8(a)のシーンで撮影した動画像について、上述した手法で対応点探索を行った。図8(b)はそのときの対応点正解率と車両の移動速度との関係を示すグラフである。ここでは、画像座標上の誤差が1画素以内であった対応点の比率を対応点正解率としている。なお、図8(b)のグラフでは、時間と移動速度の値には、適当なスケールが掛けられている。
図8(b)からも分かるように、実際に撮影された画像を用いた場合、精度が良いとされている手法であっても、対応点正解率が50%程度にまで低下する場合がある。また、はずれ値の様な大きな誤差を持つ対応点が多数含まれる場合もある。本願発明者らは、このような場合が稀ではないことを、他の実験結果からも確認している。
そして、上述したような従来の方法では、多数の誤りを排除する仕組みを持たないことから、例えば、対応点探索の正解率が大きく低下してしまった場合、精度の高い動き算出は、到底期待できない。
これに対して本実施形態では、たとえ対応点正解率が大きく低下したような場合であっても、第1の動き算出部102において、第1の動きMaを精度良く算出することができる。すなわち、本実施形態の方法の場合、q組の部分対応点の中に、正しい部分対応点が少なくとも1組含まれていれば、q個の動き候補Mjの中に少なくとも1つの正しい動きが含まれることになる。さらに、対応点探索の正解率に相当する静止対応点率Rを用いて、q個の動きMjを、n個の対応点に対する評価値のうち良い方から(n×R)番目の評価値を用いて評価することによって、正しい動きMaを算出することを可能にしている。
また、本実施形態によると、例えば路面が傾いている場合であっても、車両の動きを精度良く算出することができる。
本実施形態では、車両が静止した状態で計測して得た水平かつ平坦な路面平面の式を、所定の平面式として用いて、第1の動きMaを算出するものとした。しかし、坂道や昇降路などの路面が傾いている場合や、乗員や荷物の積載、またはハンドル操作などによって車両の姿勢が変わった場合は、予め計測した所定の平面式は、実際に車両が走行している路面平面と一致しなくなる。
図9(a)は車両が傾いた場合の画像例である。車両が傾くと、画像内の路面も傾くため、予め計測した所定の平面式と一致しなくなる。この場合、選択された部分対応点の3個の対応点が、路面上にありかつ対応関係が正しいものであったとしても、(数9)を用いて算出された動きMjは誤差を含む。このため、図9(b)に示すように、n個の対応点に対する動きMjの評価値Ejiは、平面式が正しい場合に比べて少しずつ大きな値となり、評価値Ejも大きな値となる。したがって、評価値が最も小さい第1の動きMaであっても、誤差を含むことになる。
ここで、評価値が図9(b)のようになる場合に、従来技術であるはずれ値除去方法を適用し、評価値が小さい一部の対応点を抽出して、(数9)を用いた最小二乗法によって動きを算出したとしても、誤差を小さくすることは困難である。
これに対して本実施形態では、第1の動きMaを初期値として、小さい方からn×R個の評価値を基準とした探索的手法を用いて、第2の動きMbを算出する。一般に、初期値から真の値(または最良の値)までの間で、その評価関数が滑らかであれば、探索的手法によって真の値を得ることが可能である。そして、本実施形態で用いた評価値は、真の値の近傍であれば上記条件を満たす。このため、たとえ、実際の路面が所定の平面式と一致しなくなったために第1の動きMaが誤差を含んでいる場合であっても、第2の動き算出部103によって、誤差の小さい第2の動きMbを算出することができる。
また、本実施形態において、平面式を仮定することの妥当性と効果について、実際の車両の走行環境において発生する問題を基にして、説明する。
車両周辺をカメラで監視するシステムには、次のような特徴がある。すなわち、運転者が車両周辺を監視できるように、典型的には図4のように、車両近傍の路面から水平線より上の障害物までが画像に映るように、広視野角のカメラが設置される。このように設置されたカメラの画像には、ほとんど全てのシーンにおいて路面が映ることが期待できる。さらに、仮に路面が連続した平面ではない場合であっても、多くの被写体が路面平面の延長上に存在する。例えば水平線近傍の遠方の被写体は、実際には路面に接していなくても、路面と同一の平面式上に存在すると仮定することができる。このようなケースが多いことが、本願発明者らによる実験でも確認できた。このことから、車両周辺を監視するシステムにおいて、路面を表す平面式を仮定して動きを求める方法は、妥当といえる。
また、平面式を仮定した場合、(数9)を用いて、3個の対応点から車両の動きを算出できる。これに対して、平面式を仮定しない方法では、動き(または動き候補)を求めるために、少なくとも4個以上の対応点が必要となり、このため、算出過程において誤差の影響を受けやすい。これに対して、本実施形態のように平面式を仮定することによって、誤差の影響を回避できるという効果が得られる。
さらに、単眼カメラの画像を用いてカメラの動きを算出する場合、スケール項を求めるために、画像中に大きさが既知の被写体が必要となる。本実施形態では、平面式を仮定することによって、画像中に大きさが既知の被写体が存在しなくても、スケールを考慮したカメラ動きを算出できるという効果もある。
さらに、本実施形態のように平面式を仮定することによって、画像中に他の移動体が存在する場合であっても、移動体動きを正しく算出できるという効果も得られる。
図10(a)は自車両1の後方に他の移動車両2が存在している状況を示す図である。図10(a)では、自車両1は後方(図中右)に、他車両2は前方(図中左)にそれぞれ移動している。このとき、図10(b)のような画像がカメラ120によって撮影される。
ここで、対応点が、路面と他車両2上のそれぞれに、50%ずつ算出されたとする(対応関係の誤りはないものとする)。このような状況に、従来の方法を適用すると、自車両1について、路面に対する動きと、他車両2に対する動きとの2つが算出される可能性があるため、これら2つの動きを判別する手段が別途必要になる。さらに2つの動きを判別する手段において誤りが発生すれば、自車両1の動きは誤差の大きな動きとなる。
これに対して、本実施例では、路面平面の式を仮定して自車両1の動きを算出する。このため、第1の動き算出部102において、路面平面から離れた他車両2上の対応点から得られた自車両1の動きMjは、大きな誤差を含むこととなり、その評価値Ejは大きくなる。この結果、第1の動き算出部102において、他車両2上の対応点から得られた動きが選択されることはなく、路面上の対応点から得られた路面平面に対する動きが、第1の動きMaとして選択される。以上のことから、本実施形態は、画像中に他の移動物が存在する場合であっても、移動体の動きを正しく算出できるという効果があると言える。
以上説明したように、平面式を仮定して動きを算出することによって、実際の車両の走行環境で発生する、対応点探索過程で発生する誤差、路面や車両の傾き、他の移動物の存在などといった問題に対しても、移動体動きを精度良く算出することができる、という効果が得られる。
<平面式の更新>
平面式算出部114は、過去の画像から第1の動き算出部102によって求められた第1の動きMaを用いて、平面式を更新することも可能である。
平面式算出部114は、過去の画像から第1の動き算出部102によって求められた第1の動きMaを用いて、平面式を更新することも可能である。
例えば、平面式算出部114は、時刻t=1において、予め得られた所定の初期平面式(a0,b0,c0)を出力する。第1の動き算出部102は、この初期平面式(a0,b0,c0)を用いて時刻t=0とt=1間の第1の動きMaを求める。次に、時刻t=2において、平面式算出部114は、時刻t=0とt=1間の第1の動きMaを用いて、移動体が初期平面式(a0,b0,c0)に対して第1の動きMaだけ動いたものとして、新たな平面式(a1,b1,c1)を算出して出力する。これ以降、同様の処理を順次繰り返すことによって、平面式は更新されていく。
このように、過去に算出した移動体動きから新たに平面式を算出し、次の動き算出に用いることによって、より正確な平面の位置に基づいた動き算出を実行することができる。したがって、移動体動きをより精度良く算出することができる。
なお、平面式の更新には、第1の動きMaに代えて、第2の動きMbを用いてもよいし、またその両方を用いてもかまわない。
また、平面式の更新は、必ずしも、動き算出を実行するたびに行う必要はない。例えば、動き算出を所定回数実行する毎に行ってもよいし、路面や車両の状況に変化が生じたときに行うようにしてもよい。
<静止対応点率の更新>
静止対応点率算出部115は、所定の静止対応点率を出力する代わりに、対応点算出部101から出力された複数の対応点と、過去の画像から第1の動き算出部102によって算出された第1の動きMaとを用いて、静止対応点率を算出することも可能である。
静止対応点率算出部115は、所定の静止対応点率を出力する代わりに、対応点算出部101から出力された複数の対応点と、過去の画像から第1の動き算出部102によって算出された第1の動きMaとを用いて、静止対応点率を算出することも可能である。
例えば、静止対応点率算出部115は、第1の動き算出部102によって算出された第1の動きMaについて、n個の対応点に対して、上述したような(数11)および(数12)を用いた評価を行う。そして、n個の対応点のうち評価値Eiが所定の閾値以下であるものの比率を算出し、これを新たな静止対応点率として保持する。そして、次の動き算出を実行するときに、保持していた静止対応点率を出力する。これ以降、同様の処理を順次繰り返すことによって、静止対応点率は更新されていく。
ここで、動き算出に用いられた静止対応点率が真の静止対応点率と異なる場合、算出される動きは、真の動きに対する誤差を含むことになる。この静止対応点率Rと真の静止対応点率Rtrueおよびカメラ動きの誤差EMとの関係を図11のグラフに示す。図11のグラフは本願発明者らが行った実験の結果から、その共通の傾向を模式的に表したものである。図11のグラフからも分かるように、静止対応点率Rと真の静止対応点率Rtrueが一致しているとき(比R/Rtrueが1のとき)、カメラ動きの誤差EMが最も小さくなり、静止対応点率Rが真の静止対応点率Rtrueに対して大きくなった場合でも小さくなった場合でも、カメラ動きの算出精度は低下する。
一方、動き算出の繰り返しの周期が比較的短い場合、車両周辺の画像の変化は小さく、したがって、静止対応点率の変化も小さいと考えられる。このため、時刻t−1において得られた動きを用いて算出した静止対応点率R(t−1)は、時刻tの画像における真の静止対応点率Rtrue(t)に近いと予想される。したがって、静止対応点率を更新していくことによって、誤差の小さい動きを算出することが可能になる。
なお、時刻tにおいて、時刻t−1で算出した静止対応点率R(t−1)を用いて移動体動きを算出した結果、この移動体動きから求めた静止対応点率R(t)が静止対応点率R(t−1)を下回った場合は、時刻tの移動体動きを、新たな静止対応点率R(t)を用いて再計算するようにしてもよい。
図11からわかるように、静止対応点率Rが真の静止対応点率Rtrueよりも小さい場合は、カメラ動きの誤差はあまり大きくならないのに対して、静止対応点率Rが真の静止対応点率Rtrueよりも大きい場合は、カメラ動きの誤差は極端に大きくなる。したがって、R(t)<R(t−1)の場合、静止対応点率R(t−1)を用いて算出したカメラ動きは大きな誤差を含んでいる可能性がある。このことから、R(t)<R(t−1)の場合に、新たに得られたR(t)を用いて移動体動きを再計算することによって、より精度の高い動き算出が可能になる。
なお、静止対応点率の更新には、第1の動きMaに代えて、第2の動きMbを用いてもよいし、またその両方を用いてもかまわない。
<移動体動きの予測値の利用>
移動体動き予測部116は、第1の動き算出部102によって過去に求められた第1の動きMaを用いて、車両1の動きを予測し、その動き予測値を出力することができる。この場合、動き評価選択部113は、移動体動き予測部116から出力された動き予測値を加味して、第1の動きMaを算出する。
移動体動き予測部116は、第1の動き算出部102によって過去に求められた第1の動きMaを用いて、車両1の動きを予測し、その動き予測値を出力することができる。この場合、動き評価選択部113は、移動体動き予測部116から出力された動き予測値を加味して、第1の動きMaを算出する。
ここでは例えば、移動体動き予測部116は、車両1の動きはほぼ等速・等角速度運動であると仮定して、予測を行うものとする。すなわち、時刻t−1に算出された第1の動きMaを保存しておき、時刻tの動き予測値Mrtとして出力する。動き評価選択部113は、動き候補算出部112から入力したq個のカメラ動きMj(j=1,…,q)とともに、動き予測値Mrtも併せた(q+1)個のカメラ動きに対して、上述したように(数11)(数12)を用いて評価を行う。そして、最も評価値の良いものを、時刻tにおける第1の動きMaとして選択する。
上述したように、部分対応点の組数qと保証率zとの間には(数10)の関係があり、静止対応点率R<1、かつ、qが有限である限り、保証率zは1未満となる。つまり、q組の部分対応点から第1の動きMaを求める場合、ある確率で、誤差が大きい動きが算出される。
これに対して、動き予測値Mrtも併せて評価を行い、第1の動きMaを選択することによって、q組の部分対応点からは正しい動きが得られないような場合でも、車両1の動きが等速・等角速度運動に近い場合は、動き予測値Mrtが第1の動きMaとして選択される。この結果、第1の動きMaに大きな誤差が生じるケースを回避することができる。
なお、ここでは、移動体が等速・等角速度運動していると仮定して動き予測を行うものとしたが、動き予測の方法はこれに限られるものではない。例えば、移動体が等加速・等角加速度運動をしていると仮定して動き予測を行ってもよく、この場合でも上と同様に、第1の動きMaに大きな誤差が生じるケースを回避することができる。
なお、移動体の動き予測には、第1の動きMaに代えて、第2の動きMbを用いてもよいし、またその両方を用いてもかまわない。
以上のように本実施形態によると、対応点が存在する所定の平面を仮定することによって、未知パラメタを減らすことができることから、大きな誤差の少ない第1の動きMaを求めることができる。さらに、第1の動きMaを初期値とした探索的手法を用いることによって、誤差の小さい第2の動きMbを求めることができる。
なお、本実施形態では、動き候補の評価のために、(数11)と(数12)に表された評価値Ejiを用いたが、評価手法はこれに限定されるものではなく、対応点がカメラ動きに一致するか否かを評価する値であればどのような評価値を用いてもかまわない。例えば、エピポーラ拘束や基礎行列と呼ばれる、カメラ間の位置(動き)と対応点との関係を示す評価値を用いてもよい。(数13)はその具体例である。
なお、本実施形態では、カメラ動きMjについて、n個の対応点に対する評価値Ejiのうち小さい方から(n×R)番目の評価値を、評価値Ejとして選択するものとしたが、評価値Ejの特定方法はこれに限定されるものではない。例えば、評価値Ejiの小さい方から(n×R)番目までの評価値の平均値を、評価値Ejとしてもよい。
また、静止対応点率Rの値に応じて、評価値Ejの特定方法を変えてもよい。例えば、静止対応点率Rが所定の閾値よりも大きい場合は、評価値Ejiのうちの小さい方から(n×R)番目の評価値をEjとし、一方、静止対応点率Rが所定の閾値よりも小さい場合は、評価値Ejiのうちの小さい方から(n×R)番目までの評価値の平均値をEjとするようにしてもよい。
この方法によると、静止対応点率Rが比較的小さいときは、複数の評価値の平均値をとることによって、評価値に含まれる誤差の影響を抑制することができる。その一方で、静止対応点率Rが比較的高いときは、1個の評価値を評価値Ejとして採用することによって、(n×R)番目近辺の微小な差異が平均化されずに、評価値Ejに反映される、という効果が得られる。
なお、本実施形態において、第2の動きMbの算出を、第1の動きMaを初期値とした探索的手法によって、行うものとしたが、このような方法に限定されるものではない。例えば、第1の動き算出部102が、第1の動きMaの代わりに、第1の動きMaの算出元となった部分対応点と平面式を、第2の動き算出部103に出力するものとする。そして第2の動き算出部103は、この部分対応点および平面式と、対応点算出部101から出力されたn個の対応点とを用いて、別の探索的手法により第2の動きMbを算出するとしてもよい。別の探索的手法として、例えば、平面式S(a,b,c)を初期値として、平面式の各パラメタに対して所定の微小変化(da,db,dc)をそれぞれ増減した、o=3^3個の平面式Sk(a±da,b±db,c±dc)(k=1,・・・,o)を算出する。次に、第1の動きMaの算出元となった部分対応点とo個の平面式Skから、(数9)を用いてカメラ動き候補Mbk(k=1,・・・,o)を算出する。そして、前述した手法によって、n個の対応点に対する動き候補Mbkの評価値Ebkを算出し、評価値Ebkが最小となる動き候補Mbkを選択する。さらに、選択されたMbkに対応する平面式Skを初期値として、上述した処理を繰り返し実行し、値が収束したとき、または繰り返し回数が所定回数に達したとき、処理を終了する。そして、そのとき選択されている動き候補Mbkを第2の動きMbとして出力する。この方法は、第1の動きMaに含まれる誤差の主要因が、平面式Sと実際の路面との差である場合に、実際の路面に近い平面式Sを元にして動きを再計算することが期待できるため、誤差の小さい動きを算出できると考えられる。また、この方法を用いた場合、探索空間を狭くできるため、処理が軽くなるというメリットが得られる。
また、平面式S(a,b,c)のパラメタのうち、cはスケール項であることから、cに対する探索は効果がない場合がある。このような場合は、o=3^3個の平面式Sk(a±da,b±db,c±dc)(k=1,・・・,o)用いる代わりに、o=3^2個の平面式Sk(a±da,b±db,c)(k=1,・・・,o)用いてもよい。
さらに、この方法を用いる場合、視点座標系の平面式S(a,b,c)を初期値として、一定の微小変化をそれぞれ増減した平面式Skを用いて探索を行う場合は、図12(a)のように、実際の路面の変動とは無関係な平面式を用いた探索となる。これに対して、図12(b)のように、例えば、カメラ120に近い車軸中心(図の場合は後輪軸中心)を基準に平面を傾けた場合の、所定の微小変化(da,db,dc)を増減した平面式Skを用いて探索を行う方が、車両と路面との位置変動を反映した探索であると言えることから、より少ない繰り返し回数での動き算出や、より誤差の小さい動き算出が可能になる。このような所定の微小変化としては、例えば、図12と類似の状態において、さまざまな平面式Sとその微小変化(da,db,dc)をあらかじめ計測しておき、これを用いればよい。
なお、本実施形態では、移動体動き算出装置100から出力された第2の動きMbを用いて、図7のように車両1の移動軌跡を画像に重畳するものとしたが、算出した移動体動きの利用形態は、これに限られるものではない。
例えば、画像合成装置121は、第1の動きMaの評価値Eiが小さい方から(n×R)個までの対応点について、その画像座標と3次元位置を入力する。そして、(n×R)個の対応点のうち、その3次元位置が移動体の進路上であり、かつ、所定の範囲内にあるものが存在するときは、図13のような注意メッセージを重畳合成するとともに、該当する対応点の付近を枠で囲むなどの強調表示するようにしてもよい。
また、本実施形態では、移動体動き算出装置100の出力は、画像合成装置121とディスプレイ122に供給されるものとしたが、これは、移動体動き算出装置100やその出力である移動体動きの用途を限定するものではなく、他のどのような装置と組み合わせて用いてもかまわない。
なお、本実施形態では、カメラ120は車両1の後部に設置されており、車両後方を撮影するものとしたが、カメラの位置や撮影範囲はこれに限定されるものではなく、車両の周辺が撮影されるように設置されていれば、カメラはどのような位置にあってもよい。
なお、本実施形態では、動き算出のために仮定した平面は路面であったが、仮定する平面は路面に限定されるものではなく、移動体との相対的な位置を予め与えることができる平面であれば、どのような平面であってもかまわない。
例えば、図14のような屋内においては、天井面も移動体との相対的な位置を予め与えることができるため、路面の代わりに、天井面を平面として仮定してもよい。また、移動体が壁面と所定の距離を保って移動するような場合には、路面の代わりに、壁面を平面として仮定してもよい。
なお、本実施形態では、移動体の動きの表現や、移動体の動きを表すための座標系の取り方については、特に制約はない。例えば、垂直軸が車両の非操舵輪の車軸を含む直線と直交するように、車両座標系を置き、車両の路面上の動きを、垂直軸周りの円弧運動として表してもよい。図15はいわゆるアッカーマンモデルに基づく移動体の動きの表現を示す図である。図15では、前輪が操舵輪、後輪が車体に車軸が固定された非操舵輪である車両について、非操舵輪の車軸の延長線上に、水平面上での回転成分の中心がおかれている。
このとき、図15のように、座標系の垂直軸(Yc軸)を後輪の中心位置を通るようにおくと、車両の動きのうち(txc,tzc,wyc)の間に拘束式が成り立つ。このような拘束条件をおくことによって、未知パラメタの個数を減らすことができ、より安定に解を求めることができる。
(第2の実施形態)
本発明の第2の実施形態では、第1の実施形態と同様に構成された移動体動き算出装置を、ナビゲーション装置と組み合わせて、ナビゲーションシステムを構成する。ここでのナビゲーション装置は、人工衛星からの電波を用いて現在位置を測定する機能と、現在位置と地図情報を用いて現在位置の表示や目的地への誘導表示を行う機能とを有するものとする。
本発明の第2の実施形態では、第1の実施形態と同様に構成された移動体動き算出装置を、ナビゲーション装置と組み合わせて、ナビゲーションシステムを構成する。ここでのナビゲーション装置は、人工衛星からの電波を用いて現在位置を測定する機能と、現在位置と地図情報を用いて現在位置の表示や目的地への誘導表示を行う機能とを有するものとする。
図16は本実施形態に係る移動体動き算出装置を含むナビゲーションシステムの構成図である。図16では、図2および図3と共通の構成要素には同一の符号を付しており、ここではその詳細な説明を省略する。
図16において、ナビゲーション装置130は、位置情報取得部131、位置情報算出部132、画像合成部133および地図情報記憶部134を備えている。位置情報取得部131はいわゆるGPS機能を有しており、複数の人工衛星からの電波を受信して車両の位置に関する情報を取得する。また、位置情報取得部131は、衛星からの電波が受信可能であり、GPS機能によって位置情報が取得可能な場合(GPS機能が有効な場合)は取得した位置情報を出力し、衛星からの電波が受信できないなどの理由によりGPS機能によって位置情報を取得できない場合(GPS機能が無効な場合)は、位置情報が取得不可能であることを示す情報を出力する。位置情報算出部132は位置情報取得部131からの位置情報と、移動体動き算出装置100から出力された移動体動き情報(第2の動きMb)とを入力して、車両の現在位置を算出する。地図情報記憶部134は地図座標に対応した道路や駐車場、地図の情報を保持している。画像合成部133は位置情報算出部132から現在位置を入力して、現在位置に対応する道路や地図の情報を地図情報記憶部134から読み出し、画像に変換して出力する。
図16のナビゲーションシステムの動作について、説明する。いま、車両は屋外にあり、衛星からの電波が受信可能であり、GPS機能が有効となっているものとする。このとき、位置情報取得部131はGPS機能によって得られた位置情報を出力し、位置情報算出部132は位置情報取得部131から受けた位置情報を、そのまま現在位置として出力するとともに、一時的に記憶する。画像合成部133は位置情報算出部132から現在位置を受け、地図情報記憶部134に記憶された現在位置周辺の地図情報を読み出し、地図画像に変換して出力する。この地図画像は、ディスプレイ122に表示される。
また、位置情報取得部131がGPS機能によって位置情報を取得可能であるときは、位置情報算出部132は、移動体動き算出装置102から入力された移動体動きの情報を無視する。
次に、車両が運転者の操作によって移動し、屋内駐車場などのような、衛星からの電波が受信できずGPS機能によって位置情報が取得できない場所に移動したものとする。このとき、位置情報取得部131はGPS機能が無効であることを表す情報を出力する。位置情報算出部132はこの情報を入力したとき、内部に記憶していた現在位置に、移動体動き算出装置100から出力された移動体動きを積算し、新たな現在位置として出力するとともに一時的に記憶する。画像合成部133はGPS機能が有効な場合と同様に動作し、位置情報算出部132から現在位置を受け、地図情報記憶部134に記憶された現在位置周辺の地図情報を読み出し、地図画像に変換して出力する。この地図画像は、ディスプレイ122に表示される。
また、位置情報算出部132は、現在位置を算出するとともに、必要に応じて地図情報記憶部134に記憶された駐車場の情報を読み出し、平面式算出部114に送る。平面式算出部114は、必要に応じて駐車場の情報に含まれる平面式の情報を読み出して出力する。
さらに、車両が運転者の操作によって移動し、再び電波が受信可能となり、GPS機能が有効になったとき、位置情報算出部132は、位置情報取得部131で得られた位置情報を、現在位置として出力する。
図17は屋内駐車場において、本発明に係る移動体動き算出方法を用いた実験により得られた車両の移動軌跡を示すグラフである。上述したように、本発明に係る移動体動き算出方法によると、車両に設置したカメラ画像のみを用いて移動体動きを算出することが可能であり、しかも、路面が傾いている場合であっても誤差を小さく抑えることが可能であることから、図17に示すように、屋内駐車場においても誤差の小さい移動軌跡の算出が可能となる。
ところが、図8(b)のグラフを用いてすでに説明したとおり、移動体の移動速度が速くなるほど、対応点探索の誤差が大きくなる(対応点正解率が低下する)傾向があり、対応点の誤差が大きくなりすぎた場合は、移動体動きの誤差が大きくなる。
このような問題に対して、本実施形態のナビゲーションシステムでは、移動体動き算出装置100の利用は衛星からの電波が受信できない屋内だけにとどめて、衛星からの電波が受信可能な屋外では、GPS機能によって取得した位置情報を現在位置として用いている。これにより、精度の高い現在位置の算出を可能にしている。
すなわち、本実施形態では、GPS機能によって位置情報が取得できるか否かに応じて、現在位置の算出方法を選択的に切り替えることによって、GPS機能が有効となる屋外と、電波が受信できない屋内とのいずれにおいても、常に現在位置の算出が可能となる。
なお、本実施形態では、現在位置の算出方法の切り換えを、GPS機能によって現在位置が取得できるか否かに応じて行うものとしたが、これに限られるものではなく、例えば、算出した現在位置が地図情報上で屋内駐車場の入り口近辺であるか否かに応じて、現在位置の算出方法を切り替えるようにしてもよい。
なお、本実施形態においても、第1の実施形態と同様に、動き算出のために仮定した平面は、路面に限定されるものではなく、移動体との相対的な位置を予め与えることができる平面であれば、どのような平面であってもかまわない。
例えば、屋内駐車場が図14のような構造である場合に、その天井を平面と仮定した場合の平面式を、地図情報とともに地図情報記憶部134に記憶しておく。そして、位置情報算出部132は、現在位置が図14の屋内駐車場に該当するとき、地図情報記憶部134から屋内駐車場の天井の平面式を読み出し、第1の動き算出部102の平面式算出部114に出力する。平面式算出部104は、位置情報算出部132から平面式が出力されたときは、その平面式を出力する。これにより、移動体動き算出装置100は、天井平面を基準とした移動体動きを算出する。
本発明では、カメラで撮影された画像を用いて移動体の動きを算出するため、例えば、路面が暗く、天井が明るい屋内駐車場では、路面よりも天井を平面として仮定した方が、算出精度が向上することが期待される。したがって、位置情報に対応した平面式を予め地図情報記憶部134に記憶しておき、移動体動き算出装置100が、現在位置に応じて平面式を選択して用いることによって、より精度の高い移動体動きが算出可能となる。
なお、上述の各実施形態では、移動体として車両を例にとって説明を行ったが、移動体の種類は車両に限られるものではなく、例えば、ロボットであってもよい。
なお、上述の各実施形態において、各処理手段および装置は、ハードウェアによって実現されてもよいし、ソフトウェアによって実現してもよい。例えば、図18のような、CPU141、ROM142およびRAM143と、画像の入出力機能とを備えたコンピュータを用いて、各処理手段をソフトウェアによって実現してもよい。
本発明は、カメラが設置された移動体の動きを、カメラ画像を用いて精度良く求めることができるので、例えば、車両や自走ロボットなどに適用し、その走行制御や、操作者に対する進路や位置の提示サービスなどに、利用することが可能である。
Claims (3)
- カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて、動きを求める装置であって、
撮影時刻が異なる2枚の前記画像から、複数の対応点を求める対応点算出部と、
前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて、前記移動体の動きを示す第1の動きを求める第1の動き算出部と、
前記第1の動き算出部によって求められた第1の動きと、前記複数の対応点とを用いて、前記移動体の動きを示す第2の動きを求める第2の動き算出部とを備え、
前記第1の動き算出部は、前記対応点を3個用いて、前記第1の動きを求めるものである
ことを特徴とする移動体動き算出装置。 - カメラが設置された移動体について、前記カメラによって撮影された移動体周囲の画像を用いて、動きを求める装置であって、
撮影時刻が異なる2枚の前記画像から、複数の対応点を求める対応点算出部と、
前記画像内に所定の平面を仮定し、前記対応点算出部によって求められた複数の対応点を用いて、前記移動体の動きを示す第1の動きを求める第1の動き算出部と、
前記第1の動き算出部によって求められた第1の動きと、前記複数の対応点とを用いて、前記移動体の動きを示す第2の動きを求める第2の動き算出部とを備え、
前記第1の動き算出部は、
前記複数の対応点の中から、m(mは3以上の整数)個の対応点で構成される部分対応点を、q(qは2以上の整数)組選択し、
選択したq組の部分対応点から、それぞれ、前記平面を表す平面式を用いて、前記移動体の動き候補を算出し、
算出したq個の動き候補を、所定の評価手法によって評価し、この評価結果に従って、前記第1の動きを特定する
ことを特徴とする移動体動き算出装置。 - 請求項2において、
前記移動体は、車両であり、
前記第2の動き算出部は、
前記第1の動きの代わりに、この第1の動きに対応する部分対応点と、前記平面を表す平面式を用い、
前記平面式を初期値として、探索手法によって、前記カメラに近い車軸中心を中心として平面を傾けながら、前記複数の対応点に対する評価を行いつつ、前記第2の動きを求めるものである
ことを特徴とする移動体動き算出装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2006000592A JP2007241326A (ja) | 2003-10-17 | 2006-01-05 | 移動体動き算出装置 |
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2003357729 | 2003-10-17 | ||
JP2006000592A JP2007241326A (ja) | 2003-10-17 | 2006-01-05 | 移動体動き算出装置 |
Related Parent Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2005514877A Division JP3843119B2 (ja) | 2003-10-17 | 2004-10-15 | 移動体動き算出方法および装置、並びにナビゲーションシステム |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2007241326A true JP2007241326A (ja) | 2007-09-20 |
Family
ID=38586854
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2006000592A Pending JP2007241326A (ja) | 2003-10-17 | 2006-01-05 | 移動体動き算出装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2007241326A (ja) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010134606A (ja) * | 2008-12-03 | 2010-06-17 | Nippon Telegr & Teleph Corp <Ntt> | 三次元的動き検出装置、三次元的動き検出方法及び三次元的動き検出プログラム |
WO2014076769A1 (ja) * | 2012-11-13 | 2014-05-22 | 株式会社東芝 | 検出装置、方法及びプログラム |
JP2014519444A (ja) * | 2011-06-15 | 2014-08-14 | ローベルト ボツシユ ゲゼルシヤフト ミツト ベシユレンクテル ハフツング | 駐車誘導用後付けセット |
KR101876353B1 (ko) * | 2016-11-09 | 2018-07-09 | 현대오트론 주식회사 | 움직임 추정 장치 및 방법 |
-
2006
- 2006-01-05 JP JP2006000592A patent/JP2007241326A/ja active Pending
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010134606A (ja) * | 2008-12-03 | 2010-06-17 | Nippon Telegr & Teleph Corp <Ntt> | 三次元的動き検出装置、三次元的動き検出方法及び三次元的動き検出プログラム |
JP2014519444A (ja) * | 2011-06-15 | 2014-08-14 | ローベルト ボツシユ ゲゼルシヤフト ミツト ベシユレンクテル ハフツング | 駐車誘導用後付けセット |
WO2014076769A1 (ja) * | 2012-11-13 | 2014-05-22 | 株式会社東芝 | 検出装置、方法及びプログラム |
CN103988241A (zh) * | 2012-11-13 | 2014-08-13 | 株式会社东芝 | 检测装置、方法以及程序 |
US9122936B2 (en) | 2012-11-13 | 2015-09-01 | Kabushiki Kaisha Toshiba | Detecting device, detection method, and computer program product |
JPWO2014076769A1 (ja) * | 2012-11-13 | 2016-09-08 | 株式会社東芝 | 検出装置、方法及びプログラム |
KR101876353B1 (ko) * | 2016-11-09 | 2018-07-09 | 현대오트론 주식회사 | 움직임 추정 장치 및 방법 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP3843119B2 (ja) | 移動体動き算出方法および装置、並びにナビゲーションシステム | |
CN112785702B (zh) | 一种基于2d激光雷达和双目相机紧耦合的slam方法 | |
KR101725060B1 (ko) | 그래디언트 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법 | |
Zhou et al. | Ground-plane-based absolute scale estimation for monocular visual odometry | |
US10762643B2 (en) | Method for evaluating image data of a vehicle camera | |
EP2948927B1 (en) | A method of detecting structural parts of a scene | |
US10636151B2 (en) | Method for estimating the speed of movement of a camera | |
KR101708659B1 (ko) | 이동 로봇의 맵을 업데이트하기 위한 장치 및 그 방법 | |
KR101776622B1 (ko) | 다이렉트 트래킹을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법 | |
KR101776621B1 (ko) | 에지 기반 재조정을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법 | |
KR101776620B1 (ko) | 검색 기반 상관 매칭을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법 | |
Badino | A robust approach for ego-motion estimation using a mobile stereo platform | |
US11082633B2 (en) | Method of estimating the speed of displacement of a camera | |
Vidas et al. | Real-time mobile 3D temperature mapping | |
KR101544021B1 (ko) | 3차원 지도 생성 장치 및 3차원 지도 생성 방법 | |
JP6782903B2 (ja) | 自己運動推定システム、自己運動推定システムの制御方法及びプログラム | |
US11282164B2 (en) | Depth-guided video inpainting for autonomous driving | |
KR101573576B1 (ko) | Avm 시스템의 이미지 처리 방법 | |
KR102559203B1 (ko) | 포즈 정보를 출력하는 방법 및 장치 | |
CN113137968B (zh) | 基于多传感器融合的重定位方法、重定位装置和电子设备 | |
KR20100066952A (ko) | 스테레오 비전을 이용하여 장애물을 추적하는 장치 및 방법 | |
JP2007241326A (ja) | 移動体動き算出装置 | |
JP2016162096A (ja) | 移動物体追跡装置 | |
JP6826010B2 (ja) | カメラ運動推定装置、カメラ運動推定方法及びプログラム | |
Rabe | Detection of moving objects by spatio-temporal motion analysis |