運動推定装置及びプログラム
【課題】簡易な構成で、安定して移動体の運動を精度よく推定することができるようにする。
【解決手段】画像撮像部１２によって異なるタイミングで複数の画像を撮像し、運動推定候補算出部２０によって、撮像した複数の画像に基づいて、自車両の運動推定候補を複数算出する。また、ジャイロセンサ１４によって、自車両のヨー角速度を計測する。信頼度判定部２２によって、算出された複数の運動推定候補の各々について、信頼度を判定する。運動検証部２６によって、ジャイロセンサ１４によって計測されたヨー角速度、及び信頼度判定部２２によって判定された複数の運動推定候補の各々の信頼度に基づいて、複数の運動推定候補のうちの何れか１つを、自車両の運動の推定結果として選択する。
【解決手段】画像撮像部１２によって異なるタイミングで複数の画像を撮像し、運動推定候補算出部２０によって、撮像した複数の画像に基づいて、自車両の運動推定候補を複数算出する。また、ジャイロセンサ１４によって、自車両のヨー角速度を計測する。信頼度判定部２２によって、算出された複数の運動推定候補の各々について、信頼度を判定する。運動検証部２６によって、ジャイロセンサ１４によって計測されたヨー角速度、及び信頼度判定部２２によって判定された複数の運動推定候補の各々の信頼度に基づいて、複数の運動推定候補のうちの何れか１つを、自車両の運動の推定結果として選択する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、運動推定装置及びプログラムに係り、特に、移動体の運動を推定する運動推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、画像のオプティカルフローを検出して、移動体の運動及び移動量を推定する自己位置認識システムが知られている（例えば、特許文献１）。このシステムでは、ステレオカメラを用いて、移動体の運動と移動量とを推定する。また、遠方用のステレオカメラと近傍用のステレオカメラとを用意し、遠方用のステレオカメラを用いて回転を求め、近傍用のステレオカメラを用いて並進を求める。また、２枚の画像間のオプティカルフローに基づいて、移動体の運動を検出する。
【特許文献１】特開２００３−２４７８２４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記の特許文献１に記載の技術では、４台のカメラが必要になるため、装置にコストがかかると共に、画像処理に時間がかかる、という問題がある。また、通常のカメラを用いている限りは、周辺の照明環境の影響を受けやすいため、安定して運動を推定することができない、という問題がある。
【０００４】
本発明は、上記の問題点を解決するためになされたもので、簡易な構成で、安定して移動体の運動を精度よく推定することができる運動推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記の目的を達成するために本発明に係る運動推定装置は、移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の推定候補を複数算出する候補算出手段と、前記移動体の運動を計測する運動計測手段と、前記候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段と、前記運動計測手段によって計測された前記移動体の運動、及び前記信頼度算出手段によって算出された前記複数の運動の推定候補の各々の信頼度に基づいて、前記複数の運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段とを含んで構成されている。
【０００６】
本発明に係るプログラムは、コンピュータを、移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の推定候補を複数算出する候補算出手段、前記候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段、及び前記移動体の運動を計測する運動計測手段によって計測された前記移動体の運動、及び前記信頼度算出手段によって算出された前記複数の運動の推定候補の各々の信頼度に基づいて、前記複数の運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段として機能させるためのプログラムである。
【０００７】
本発明によれば、候補算出手段によって、移動体の外部を撮像した複数の画像に基づいて、移動体の運動の推定候補を複数算出する。また、運動計測手段によって、移動体の運動を計測する。
【０００８】
そして、信頼度算出手段によって、候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する。運動推定手段によって、運動計測手段によって計測された移動体の運動、及び信頼度算出手段によって算出された複数の運動の推定候補の各々の信頼度に基づいて、複数の運動の推定候補のうちの何れか１つを、移動体の運動の推定結果として選択する。
【０００９】
このように、複数の運動の推定候補の各々の信頼度と計測された移動体の運動とに基づいて、移動体の運動を推定することにより、簡易な構成で、安定して移動体の運動を精度よく推定することができる。
【００１０】
本発明に係る運動推定手段は、運動計測手段によって計測された移動体の運動との差分の絶対値が閾値未満となる運動の推定候補のうち、算出された信頼度が最も高い運動の推定候補を選択する。これによって、移動体の運動を精度よく推定することができる。
【００１１】
本発明に係る運動推定手段は、複数の運動の推定候補の全てについて、計測された移動体の運動との差分の絶対値が閾値以上となる場合、又は、算出された信頼度が信頼度に関する閾値未満である場合、計測された移動体の運動を、移動体の運動の推定結果とすることができる。これによって、安定して移動体の運動を推定することができる。
【００１２】
上記の運動計測手段は、ジャイロセンサを用いて移動体の運動を計測することができる。また、本発明の運動推定装置は、ジャイロセンサを用いて計測される移動体の運動の計測値のオフセット量を補正するオフセット補正手段を更に含むことができる。これによって、移動体の運動を精度よく計測することができ、移動体の運動をより精度よく推定することができる。
【００１３】
上記のオフセット補正手段は、運動計測手段による移動体の運動の計測値と、運動推定手段による移動体の運動の推定結果との差分に基づいて、オフセット量を補正することができる。
【００１４】
上記の候補算出手段は、複数の画像の各々から、複数の画像間で対応した特徴点を検索する検索手段と、検索された対応した特徴点の組み合わせを繰り返し選択し、繰り返し選択された対応した特徴点の組み合わせに基づいて、複数の画像の各々を撮像したときの移動体の位置及び姿勢の相対関係を表わす運動を繰り返し算出する運動算出手段を備え、運動算出手段によって繰り返し算出された運動を、複数の運動の推定候補とすることができる。
【発明の効果】
【００１５】
以上説明したように、本発明の運動推定装置及びプログラムによれば、複数の運動の推定候補の各々の信頼度と計測された移動体の運動とに基づいて、移動体の運動を推定することにより、簡易な構成で、安定して移動体の運動を精度よく推定することができる、という効果が得られる。
【発明を実施するための最良の形態】
【００１６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載された運動推定装置に本発明を適用した場合を例に説明する。
【００１７】
図１に示すように、第１の実施の形態に係る運動推定装置１０は、自車両の前方の画像を撮像する単眼のカメラで構成される画像撮像部１２と、自車両の運動として、ヨー角速度を計測するジャイロセンサ１４と、画像撮像部１２によって撮像された画像、及びジャイロセンサ１４によって計測されたヨー角速度に基づいて、自車両の運動を推定して外部装置（図示省略）に出力するコンピュータ１６とを備えている。なお、ジャイロセンサ１４が、運動計測手段の一例である。
【００１８】
本実施の形態では、単眼のカメラを自車両の前方に設置して、自車両の前方の画像を取得する場合を例に説明するが、自車両の外部の画像を撮像すればよく、例えば、単眼のカメラを自車両の後方に設置し、自車両の後方を撮像してもよい。また、単眼のカメラは、通常の画角４０度程度のカメラでもよいし、広角のカメラでもよいし、全方位カメラでもよい。また、撮像する画像の波長について、種類は問わない。運動量を適切に推定できる場所であれば、設置場所、画角、波長、及び設置個数について問わない。
【００１９】
コンピュータ１６では、自車両の運動として、自車両の３軸角速度と並進方向を示す成分とを推定する。コンピュータ１６は、ＣＰＵと、ＲＡＭと、後述する運動推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１６は、画像撮像部１２によって撮像された複数の画像を取得する画像入力部１８と、複数の画像に基づいて、自車両の運動推定候補を複数算出する運動推定候補算出部２０と、算出された複数の運動推定候補の各々について信頼度を判定する信頼度判定部２２と、ジャイロセンサ１４によって計測された計測値のオフセット量を補正するオフセット補正部２４と、算出された複数の運動推定候補の各々に対する信頼度、及びオフセット補正されたヨー角速度の計測値に基づいて、複数の運動推定候補を検証して、自車両の運動を推定する運動検証部２６とを備えている。なお、運動検証部２６が、運動推定手段の一例である。
【００２０】
運動推定候補算出部２０は、図２に示すように、画像撮像部１２により得られた複数の画像の各々から、画像上で追跡しやすい特徴点を複数抽出する特徴点抽出部３０と、特徴点抽出部３０により得られた２つの画像の各々における特徴点から、２つの画像の間で対応する特徴点（以下、対応点と称する）を検索する対応点検索部３２と、対応点検索部３２で得られた対応点から、８組の対応点を複数種類の組み合わせで選択する特徴点選択部３４と、選択された８組の対応点の各種組み合わせについて、当該組み合わせの各対応点における各画像の画像座標を入力として、対応点が検索された一方の画像を撮像したときの画像撮像部１２の位置姿勢を基準とした、対応点が検索された他方の画像を撮像したときの画像撮像部１２の位置姿勢への変化（位置姿勢の相対関係）を、画像撮像部１２の運動のＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量として算出する運動算出部３６とを備えている。
【００２１】
特徴点抽出部３０は、画像撮像部１２から得られる異なる時刻に撮像した２枚の画像から、それぞれ特徴点を抽出する。特徴点とは、周囲の点と区別でき、異なる画像間で対応関係を求めることが容易な点のことを指す。特徴点は、２次元的に濃淡変化の勾配値が大きくなる画素を検出する方法（例えばＴｏｍａｓｉ−Ｋａｎａｄｅの方法や、Ｈａｒｒｉｓオペレータなど）を用いて、自動的に抽出される。特徴点の数としては、全画像に対して３０〜５００点程度を想定すると良いが、状況に応じて、固定であってもよいし、可変であってもよい。
【００２２】
Ｈａｒｒｉｓオペレータを用いる方法では、以下に説明するように、特徴点を抽出する。まず、画像の点（ｕ，ｖ）の輝度をＩ（ｕ，ｖ）として、以下の（１）式によって、行列Ｍを計算する。
【００２３】
【数１】
【００２４】
ただし、Ｉｕ，Ｉｖはそれぞれ水平方向、垂直方向の微分、Ｇσは標準偏差σのガウス分布による平滑化を表す。
【００２５】
そして、上記（１）式で計算された行列Ｍの固有値λ１、λ２を用いて、以下の（２）式によりコーナー強度を計算する。
【００２６】
【数２】
【００２７】
ただし、ｋは予め設定される定数であって、０．０４〜０．０６の値が一般に用いられる。Ｈａｒｒｉｓオペレータを用いる方法では、このコーナー強度がしきい値以上でかつ極大となる点を選択し、選択された点を特徴点として抽出する。
【００２８】
対応点検索部３２は、特徴点抽出部３０において２つの画像の各々から抽出された特徴点について、２つの画像間での対応付けを行って、２つの画像間の対応点を検索する。画像間での特徴点の対応付けでは、対応する点が同一ならば、画像１と画像２で対応する点とその周囲の点の輝度は殆ど変化しないと仮定し、この仮定に基づいて各特徴点の対応付けを行う。例えば、特徴点周辺に設定した小領域での輝度分布が似ている点の組を選択し、選択された点の組を対応点とする。特徴点の対応付け（特徴点追跡）の周知の手法として、ルーカスカナデ法（Ｌｕｃａｓ−Ｋａｎａｄｅ法）などが挙げられる。
【００２９】
特徴点選択部３４は、特徴点の対応付け（追跡）が完了した全対応点の中から、運動推定に用いる対応点の組を任意に選択する。ここで選択した対応点の組み合わせによって、運動算出部３６で算出される運動推定候補が決定される。なお、各種組み合わせについて、対応点は８組以上選択するものとする。
【００３０】
運動算出部３６は、特徴点選択部３４より選択された対応点の複数種類の組み合わせの各々について、当該組み合わせにおける少なくとも８組の対応点の画像座標から、２つの画像の各々が撮像されたときの画像撮像部１２の位置及び姿勢の変化（ＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量）を計算する。位置姿勢の変化は、図３に示すように、第１の画像から第２の画像への回転行列Ｒ（Ｘ軸を基準とする回転量、Ｙ軸を基準とする回転量、Ｚ軸を基準とする回転量）と、並進ベクトルｔ（Ｘ軸方向の移動量ｔｘ、Ｙ軸方向の移動量ｔｙ、Ｚ軸方向の移動量ｔｚ）との６要素から構成される運動である。なお、回転行列Ｒ及び並進ベクトルｔの要素は、２つの画像間の画像座標の変換を表す物理量である。
【００３１】
ここで、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとの計算方法について説明する。第１の画像におけるｎ点の対応点の画像座標ｘ１と第２の画像におけるｎ点の対応点の画像座標ｘ２とについて（ｎ≧８）、対応点が正しくて誤差がなければ、以下の（３）式を満たす３×３行列Ｆが存在する。
【００３２】
【数３】
【００３３】
ここで、８組以上の対応点ｘ１、ｘ２があれば、上記の基礎行列Ｆを算出することができる。
【００３４】
上述したように基礎行列Ｆが計算でき、また、画像撮像部１２のキャリブレーション行列Ｋが既知である場合には、カメラの撮像特性に依る画像の歪みを補正するキャリブレーション行列Ｋ、及び基礎行列Ｆを用いて、以下の（４）式、（５）式より、回転行列Ｒと並進ベクトルｔとを計算することができる．
【００３５】
【数４】
【００３６】
ここで算出した運動を運動推定候補とする。特徴点選択部３４による特徴点の選択と運動算出部３６による算出とをＮ回繰り返すことにより、Ｎ個の運動推定候補が得られる。
【００３７】
また、信頼度判定部２２は、図４に示すように、複数の運動推定候補の各々について、当該運動推定候補に対し動きが整合する特徴点を選別する特徴点選別部４０と、各運動推定候補について選別された特徴点の数から運動推定候補の信頼度を算出する信頼度評価部４２と、運動推定候補の信頼度に基づいて、運動推定候補を並び替えると共に選定する運動推定候補選定部４４とを備えている。
【００３８】
特徴点選別部４０は、複数の運動推定候補の各々について、当該運動推定候補を算出する際に用いられた複数の特徴点のうち、当該運動推定候補に対し動きが整合する特徴点を選別する。
【００３９】
例えば、図５に示すように、画像１で座標ｘ１に位置する特徴点Ｐが、基礎行列Ｆで表わされる運動に従って動く時、画像２での特徴点Ｐの座標はＦｘ１で表わされる線（エピポーラ線）上に位置する。すなわち、画像２における対応する特徴点の位置とエピポーラ線の距離が近い程、その特徴点の動きは運動推定候補に対する整合性が高いと言える。この性質を利用して、当該運動推定候補を算出する際に用いられた複数の特徴点のうち、対応するエピポーラ線との距離が閾値以下となる特徴点を選別する。
【００４０】
信頼度評価部４２は、複数の運動推定候補の各々について、特徴点選別部４０で選別された、当該運動推定候補に対し動きが整合する特徴点の数から、当該運動推定候補の信頼度を算出する。特徴点の数が大きいほど、高い信頼度が算出される。また、信頼度の指標として、選別された特徴点の数をそのまま用いてもよいし、特徴点の数に任意の関数を適用した結果を用いてもよい。
【００４１】
運動推定候補選定部４４は、Ｎ個の運動推定候補について、算出された信頼度が高い順に並び替えると共に、信頼度が高い方から順にＭ個選定する。これによって、信頼度によって順位付けされた運動推定候補Ｃ１〜ＣＭが出力される。なお、Ｎ個全ての運動推定候補を出力するようにしてもよいし、任意に設定した閾値よりも信頼度が高い運動推定候補だけを出力するようにしてもよい。
【００４２】
オフセット補正部２４は、ジャイロセンサ１４によって計測されたヨー角速度の計測値を取得し、ジャイロセンサ１４の計測値のオフセット（定常的な偏差）を補正した値を、運動計測結果Ｙｇとして出力する。運動検証部２６の出力値とジャイロセンサ１４の計測値とについて、現時刻から過去一定期間の平均値の差を算出し、オフセット補正値として設定する。また、オフセット補正値はタイムステップ毎に算出される。ただし、運動検証部２６において、運動推定候補算出部２０により算出された運動推定候補が全て不採用であった場合、その時刻の入力を除いて平均値を算出する。
【００４３】
運動検証部２６は、信頼度判定部２２から信頼度と共に出力された運動推定候補Ｃ１〜ＣＭを、オフセット補正部２４からの出力値Ｙｇと比較すると共に、信頼度を参照して、適切な運動推定候補を決定する。
【００４４】
比較項目は、運動推定候補に含まれる運動の回転３成分（ｐｉｔｃｈ、ｙａｗ、ｒｏｌｌ）のうちのヨー角速度とし、まず、運動推定候補Ｃ１から得られたヨー角速度Ｙ１と、オフセット補正部２４から得られたヨー角速度Ｙｇとを比較する。ヨー角速度に関する閾値をＹｔｈとして、｜Ｙ１−Ｙｇ｜＜ＹｔｈであればＣ１を運動推定結果として出力する。｜Ｙ１−Ｙｇ｜≧Ｙｔｈである場合には、Ｃ１と同様の方法でＣ２について検証を行う。以下同様に、出力値が得られるまで検証を繰り返し、ＣＭまで全て検証が終わった時点で出力値が得られない場合は、Ｙｇを運動の推定結果として出力する。
【００４５】
次に、第１の実施の形態に係る運動推定装置１０の作用について説明する。なお、運動推定装置１０を搭載した自車両が走行しているときに、自車両の運動を推定する場合を例に説明する。
【００４６】
まず、運動推定装置１０において、図６に示す運動推定処理ルーチンが実行される。ステップ１００において、異なるタイミングで画像撮像部１２によって自車両前方を撮像した第１の画像及び第２の画像を取得すると共に、第１の画像及び第２の画像を撮像する間にジャイロセンサ１４によって計測されたヨー角速度を取得する。そして、ステップ１０２において、上記ステップ１００で取得された第１の画像及び第２の画像に基づいて、複数の運動推定候補を算出する。
【００４７】
上記ステップ１０２は、図７に示す運動推定候補算出処理ルーチンによって実現される。まず、ステップ１２０において、第１の画像及び第２の画像から、それぞれ所定の数だけ特徴点を抽出する。そして、ステップ１２２において、上記ステップ１２０で抽出された第１の画像における特徴点の各々を、第２の画像において追跡し、第２の画像から、対応する各特徴点をそれぞれ検索する。
【００４８】
そして、ステップ１２４において、算出された運動推定候補の数を示す変数ｎを初期値の１に設定し、ステップ１２６において、上記ステップ１２２で検索された複数組の対応点から、８組の対応点をランダムに選択する。ステップ１２８では、上記ステップ１２６で選択された８組の対応点（２時刻間で互いに対応する特徴点の組）から、２つの撮像時刻間における画像撮像部１２の相対的な位置関係、即ち、画像撮像部１２を搭載した自車両のその２時刻間における運動を推定する。
【００４９】
そして、ステップ１３０では、変数ｎが、算出する運動推定候補の数を表わす定数Ｎ未満であるか否かを判定し、Ｎ個の運動推定候補を算出していない場合には、ステップ１３２において、変数ｎをインクリメントしてステップ１２６へ戻るが、一方、変数ｎがＮに達した場合には、運動推定候補算出処理ルーチンを終了する。以上のように、運動推定候補算出処理ルーチンが実行されると、Ｎ個の運動推定候補が出力される。
【００５０】
そして、運動推定処理ルーチンのステップ１０４では、上記ステップ１０２で算出されたＮ個の運動推定候補の各々について、信頼度を判定する。上記ステップ１０４は、図８に示す信頼度判定処理ルーチンによって実現される。
【００５１】
まず、ステップ１４０において、判定対象の運動推定候補を設定し、ステップ１４２において、判定対象の運動推定候補を算出する際に用いられた複数の特徴点の各々について、第２の画像におけるエピポーラ線を求める。
【００５２】
そして、ステップ１４４において、判定対象の運動推定候補を算出する際に用いられた複数の特徴点から、第２の画像における対応する特徴点とエピポーラ線との距離が閾値以下となる特徴点を選別する。次のステップ１４６では、上記ステップ１４４で選別された特徴点の数に基づいて、判定対象の運動推定候補の信頼度を判定する。
【００５３】
そして、ステップ１４８では、全ての運動推定候補について信頼度の判定が行われたか否かを判定し、信頼度が判定されていない運動推定候補がある場合には、ステップ１４０へ戻るが、一方、全ての運動推定候補について信頼度の判定が行われた場合には、ステップ１５０へ移行する。
【００５４】
ステップ１５０では、各運動推定候補について判定された信頼度に基づいて、Ｎ個の運動推定候補を、信頼度の高い順に並び替えると共に、信頼度の高い方から順にＭ個の運動推定候補を選定して、信頼度判定処理ルーチンを終了する。
【００５５】
そして、運動推定処理ルーチンのステップ１０６では、後述するステップ１１２で前回算出されたオフセット補正値を用いて、上記ステップ１００で取得したヨー角速度のオフセット量を補正する。次のステップ１０８では、上記ステップ１０４で選定された運動推定候補のヨー角速度を、信頼度の高い方から順に、上記ステップ１０６で補正されたヨー角速度と比較して検証し、補正されたヨー角速度との差分の絶対値が閾値未満となる運動推定候補のうち、最も信頼度が高い運動推定候補を、自車両の運動の推定結果として出力する。また、全ての運動推定候補のヨー角速度について、補正されたヨー角速度との差分の絶対値が閾値以上であった場合には、補正されたヨー角速度を、自車両の運動の推定結果として出力する。
【００５６】
そして、ステップ１１０において、過去一定期間で推定された運動のヨー角速度の平均値と、過去一定期間でジャイロセンサ１４より計測されたヨー角速度の平均値とを算出し、ステップ１１２において、上記ステップ１１０で算出されたヨー角速度の平均値の差分を、オフセット補正値として算出して、上記ステップ１００へ戻る。
【００５７】
上記のように、運動推定処理ルーチンが実行されると、撮像画像による運動推定によって、図９に示すように、複数の運動推定候補が算出され、ジャイロセンサ１４の計測値を用いた運動検証によって最適な運動推定候補が選択される。また、図１０に示すように、複数の運動推定候補に対して運動検証を行うため、いくつかの運動推定候補が不採用と判断されても、適切な運動推定候補が選択され、適切な運動推定結果が安定して得られる。
【００５８】
以上説明したように、第１の実施の形態に係る運動推定装置によれば、複数の運動推定候補の各々の信頼度と、ジャイロセンサによって計測されたヨー角速度とに基づいて、自車両の運動を推定することにより、簡易な構成で、安定して自車両の運動を精度よく推定することができる。
【００５９】
また、ジャイロセンサによる計測値のオフセット量を補正することにより、自車両のヨー角速度を精度よく計測することができ、これにより、自車両の運動をより精度よく推定することができる。
【００６０】
また、撮像画像に基づく運動推定において、複数の運動推定候補を算出し、ジャイロセンサの計測値を用いた運動検証によって最適な運動推定候補を選択することで、適切な運動推定結果を安定的に取得することができる。
【００６１】
また、適切な運動推定結果を安定的に得ることで、時刻と共に変動するジャイロセンサのオフセット量を適時補正することが可能となり、ジャイロセンサのオフセット補正後の計測値の精度が安定する。更に、ジャイロセンサの計測値を用いた運動検証の妥当性が向上する。
【００６２】
なお、上記の実施の形態では、全ての運動推定候補のヨー角速度について、計測されたヨー角速度との差の絶対値が閾値以上である場合に、計測されたヨー角速度を、自車両の２時刻間の運動の推定結果として出力する場合を例に説明したが、これに限定されるものではなく、全ての運動推定候補の信頼度が、信頼度に関する閾値未満である場合に、計測されたヨー角速度を、運動の推定結果として出力するようにしてもよい。
【００６３】
次に、第２の実施の形態について説明する。なお、第２の実施の形態に係る運動推定装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【００６４】
第２の実施の形態では、運動推定候補の算出と信頼度の判定とを行う順序が、第１の実施の形態と異なっている。
【００６５】
第２の実施の形態に係る運動推定装置では、運動推定候補算出部２０によって、運動推定候補を１つ算出すると、信頼度判定部２２によって、算出された運動推定候補の信頼度を判定する。また、上記の運動推定候補算出部２０による算出及び信頼度判定部２２による判定が、逐次的に繰り返され、複数の運動推定候補と、各運動推定候補の信頼度とが得られる。
【００６６】
なお、第２の実施の形態に係る運動推定装置の他の構成及び処理については、第１の実施の形態と同様であるため、説明を省略する。
【００６７】
次に、第３の実施の形態について説明する。なお、第３の実施の形態に係る運動推定装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【００６８】
第３の実施の形態では、運動推定候補を算出する毎に、運動検証を行っている点が、主に第１の実施の形態と異なっている。
【００６９】
第３の実施の形態に係る運動推定装置の運動推定候補算出部２０では、運動推定候補を１つ算出し、信頼度判定部２２によって、算出された運動推定候補の信頼度を判定し、信頼度が閾値以上である運動推定候補を算出する。
【００７０】
また、運動検証部２６では、信頼度判定部２２によって出力された運動推定候補Ｃ１のヨー角速度Ｙ１と、オフセット補正部２４から出力されたヨー角速度Ｙｇとを比較し、｜Ｙ１−Ｙｇ｜＜Ｙｔｈとなった場合、運動推定候補Ｃ１を、運動の推定結果として出力する。一方、｜Ｙ１−Ｙｇ｜≧Ｙｔｈとなった場合に、運動推定候補算出部２０で新たに運動推定候補を１つ算出し、信頼度判定部２２によって、算出された運動推定候補の信頼度を判定する。
【００７１】
このように、オフセット補正部２４から出力されたヨー角速度Ｙｇとの差の絶対値が閾値未満となるヨー角速度を含む運動推定候補が得られるまで、運動推定候補算出部２０による算出、信頼度判定部２２による判定、及び運動検証部２６による運動検証を逐次的に繰り返し行う。
【００７２】
なお、第３の実施の形態に係る運動推定装置の他の構成及び処理については、第１の実施の形態と同様であるため、説明を省略する。
【００７３】
また、上記の第１の実施の形態〜第３の実施の形態では、ジャイロセンサによってヨー角速度を計測する場合を例に説明したが、これに限定されるものではなく、ジャイロセンサによって３軸角速度を計測するように構成してもよく、ヨー角速度、ピッチ角速度、及びロール角速度の少なくとも１つを計測するように構成してもよい。この場合、運動推定候補との比較項目は、運動の回転３成分（ｐｉｔｃｈ、ｙａｗ、ｒｏｌｌ）のうち、１成分でもよいし、複数成分でもよいし、複数成分の組み合わせでもよい。また、センサによって並進３成分（ｔｘ，ｔｙ，ｔｚ）の少なくとも１つを計測するようにしてもよく、この場合に、運動推定候補の並進成分と比較するようにすればよい。
【００７４】
また、運動推定候補のヨー角速度とジャイロセンサによって計測されたヨー角速度との差分の絶対値に対する閾値判定で用いる閾値Ｙｔｈが、一定値である場合を例に説明したが、これに限定されるものではなく、例えば、ジャイロセンサの誤差変動が大きければ、閾値Ｙｔｈを高めの値に設定するなど、ジャイロセンサの誤差の特性に合わせて可変の値を設定するようにしてもよい。
【００７５】
本発明のプログラムは、記憶媒体に格納して提供するようにしてもよい。
【図面の簡単な説明】
【００７６】
【図１】本発明の第１の実施の形態に係る運動推定装置を示すブロック図である。
【図２】本発明の第１の実施の形態に係る運動推定装置の運動推定候補算出部の構成を示すブロック図である。
【図３】抽出された特徴点と移動体の運動との関係を説明するための図である。
【図４】本発明の第１の実施の形態に係る運動推定装置の信頼度判定部の構成を示すブロック図である。
【図５】エピポーラ線を説明するためのイメージ図である。
【図６】本発明の第１の実施の形態に係る運動推定装置における運動推定処理ルーチンの内容を示すフローチャートである。
【図７】本発明の第１の実施の形態に係る運動推定装置における運動推定候補算出処理ルーチンの内容を示すフローチャートである。
【図８】本発明の第１の実施の形態に係る運動推定装置における信頼度判定処理ルーチンの内容を示すフローチャートである。
【図９】本発明の第１の実施の形態に係る運動推定装置の処理の流れを示すイメージ図である。
【図１０】複数の運動推定候補から、適切な運動推定候補を選択する処理を示すイメージ図である。
【符号の説明】
【００７７】
１０ 運動推定装置
１２ 画像撮像部
１４ ジャイロセンサ
１６ コンピュータ
２０ 運動推定候補算出部
２２ 信頼度判定部
２４ オフセット補正部
２６ 運動検証部
３０ 特徴点抽出部
３２ 対応点検索部
３４ 特徴点選択部
３６ 運動算出部
４０ 特徴点選別部
４２ 信頼度評価部
４４ 運動推定候補選定部
【技術分野】
【０００１】
本発明は、運動推定装置及びプログラムに係り、特に、移動体の運動を推定する運動推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、画像のオプティカルフローを検出して、移動体の運動及び移動量を推定する自己位置認識システムが知られている（例えば、特許文献１）。このシステムでは、ステレオカメラを用いて、移動体の運動と移動量とを推定する。また、遠方用のステレオカメラと近傍用のステレオカメラとを用意し、遠方用のステレオカメラを用いて回転を求め、近傍用のステレオカメラを用いて並進を求める。また、２枚の画像間のオプティカルフローに基づいて、移動体の運動を検出する。
【特許文献１】特開２００３−２４７８２４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、上記の特許文献１に記載の技術では、４台のカメラが必要になるため、装置にコストがかかると共に、画像処理に時間がかかる、という問題がある。また、通常のカメラを用いている限りは、周辺の照明環境の影響を受けやすいため、安定して運動を推定することができない、という問題がある。
【０００４】
本発明は、上記の問題点を解決するためになされたもので、簡易な構成で、安定して移動体の運動を精度よく推定することができる運動推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記の目的を達成するために本発明に係る運動推定装置は、移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の推定候補を複数算出する候補算出手段と、前記移動体の運動を計測する運動計測手段と、前記候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段と、前記運動計測手段によって計測された前記移動体の運動、及び前記信頼度算出手段によって算出された前記複数の運動の推定候補の各々の信頼度に基づいて、前記複数の運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段とを含んで構成されている。
【０００６】
本発明に係るプログラムは、コンピュータを、移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の推定候補を複数算出する候補算出手段、前記候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段、及び前記移動体の運動を計測する運動計測手段によって計測された前記移動体の運動、及び前記信頼度算出手段によって算出された前記複数の運動の推定候補の各々の信頼度に基づいて、前記複数の運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段として機能させるためのプログラムである。
【０００７】
本発明によれば、候補算出手段によって、移動体の外部を撮像した複数の画像に基づいて、移動体の運動の推定候補を複数算出する。また、運動計測手段によって、移動体の運動を計測する。
【０００８】
そして、信頼度算出手段によって、候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する。運動推定手段によって、運動計測手段によって計測された移動体の運動、及び信頼度算出手段によって算出された複数の運動の推定候補の各々の信頼度に基づいて、複数の運動の推定候補のうちの何れか１つを、移動体の運動の推定結果として選択する。
【０００９】
このように、複数の運動の推定候補の各々の信頼度と計測された移動体の運動とに基づいて、移動体の運動を推定することにより、簡易な構成で、安定して移動体の運動を精度よく推定することができる。
【００１０】
本発明に係る運動推定手段は、運動計測手段によって計測された移動体の運動との差分の絶対値が閾値未満となる運動の推定候補のうち、算出された信頼度が最も高い運動の推定候補を選択する。これによって、移動体の運動を精度よく推定することができる。
【００１１】
本発明に係る運動推定手段は、複数の運動の推定候補の全てについて、計測された移動体の運動との差分の絶対値が閾値以上となる場合、又は、算出された信頼度が信頼度に関する閾値未満である場合、計測された移動体の運動を、移動体の運動の推定結果とすることができる。これによって、安定して移動体の運動を推定することができる。
【００１２】
上記の運動計測手段は、ジャイロセンサを用いて移動体の運動を計測することができる。また、本発明の運動推定装置は、ジャイロセンサを用いて計測される移動体の運動の計測値のオフセット量を補正するオフセット補正手段を更に含むことができる。これによって、移動体の運動を精度よく計測することができ、移動体の運動をより精度よく推定することができる。
【００１３】
上記のオフセット補正手段は、運動計測手段による移動体の運動の計測値と、運動推定手段による移動体の運動の推定結果との差分に基づいて、オフセット量を補正することができる。
【００１４】
上記の候補算出手段は、複数の画像の各々から、複数の画像間で対応した特徴点を検索する検索手段と、検索された対応した特徴点の組み合わせを繰り返し選択し、繰り返し選択された対応した特徴点の組み合わせに基づいて、複数の画像の各々を撮像したときの移動体の位置及び姿勢の相対関係を表わす運動を繰り返し算出する運動算出手段を備え、運動算出手段によって繰り返し算出された運動を、複数の運動の推定候補とすることができる。
【発明の効果】
【００１５】
以上説明したように、本発明の運動推定装置及びプログラムによれば、複数の運動の推定候補の各々の信頼度と計測された移動体の運動とに基づいて、移動体の運動を推定することにより、簡易な構成で、安定して移動体の運動を精度よく推定することができる、という効果が得られる。
【発明を実施するための最良の形態】
【００１６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載された運動推定装置に本発明を適用した場合を例に説明する。
【００１７】
図１に示すように、第１の実施の形態に係る運動推定装置１０は、自車両の前方の画像を撮像する単眼のカメラで構成される画像撮像部１２と、自車両の運動として、ヨー角速度を計測するジャイロセンサ１４と、画像撮像部１２によって撮像された画像、及びジャイロセンサ１４によって計測されたヨー角速度に基づいて、自車両の運動を推定して外部装置（図示省略）に出力するコンピュータ１６とを備えている。なお、ジャイロセンサ１４が、運動計測手段の一例である。
【００１８】
本実施の形態では、単眼のカメラを自車両の前方に設置して、自車両の前方の画像を取得する場合を例に説明するが、自車両の外部の画像を撮像すればよく、例えば、単眼のカメラを自車両の後方に設置し、自車両の後方を撮像してもよい。また、単眼のカメラは、通常の画角４０度程度のカメラでもよいし、広角のカメラでもよいし、全方位カメラでもよい。また、撮像する画像の波長について、種類は問わない。運動量を適切に推定できる場所であれば、設置場所、画角、波長、及び設置個数について問わない。
【００１９】
コンピュータ１６では、自車両の運動として、自車両の３軸角速度と並進方向を示す成分とを推定する。コンピュータ１６は、ＣＰＵと、ＲＡＭと、後述する運動推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１６は、画像撮像部１２によって撮像された複数の画像を取得する画像入力部１８と、複数の画像に基づいて、自車両の運動推定候補を複数算出する運動推定候補算出部２０と、算出された複数の運動推定候補の各々について信頼度を判定する信頼度判定部２２と、ジャイロセンサ１４によって計測された計測値のオフセット量を補正するオフセット補正部２４と、算出された複数の運動推定候補の各々に対する信頼度、及びオフセット補正されたヨー角速度の計測値に基づいて、複数の運動推定候補を検証して、自車両の運動を推定する運動検証部２６とを備えている。なお、運動検証部２６が、運動推定手段の一例である。
【００２０】
運動推定候補算出部２０は、図２に示すように、画像撮像部１２により得られた複数の画像の各々から、画像上で追跡しやすい特徴点を複数抽出する特徴点抽出部３０と、特徴点抽出部３０により得られた２つの画像の各々における特徴点から、２つの画像の間で対応する特徴点（以下、対応点と称する）を検索する対応点検索部３２と、対応点検索部３２で得られた対応点から、８組の対応点を複数種類の組み合わせで選択する特徴点選択部３４と、選択された８組の対応点の各種組み合わせについて、当該組み合わせの各対応点における各画像の画像座標を入力として、対応点が検索された一方の画像を撮像したときの画像撮像部１２の位置姿勢を基準とした、対応点が検索された他方の画像を撮像したときの画像撮像部１２の位置姿勢への変化（位置姿勢の相対関係）を、画像撮像部１２の運動のＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量として算出する運動算出部３６とを備えている。
【００２１】
特徴点抽出部３０は、画像撮像部１２から得られる異なる時刻に撮像した２枚の画像から、それぞれ特徴点を抽出する。特徴点とは、周囲の点と区別でき、異なる画像間で対応関係を求めることが容易な点のことを指す。特徴点は、２次元的に濃淡変化の勾配値が大きくなる画素を検出する方法（例えばＴｏｍａｓｉ−Ｋａｎａｄｅの方法や、Ｈａｒｒｉｓオペレータなど）を用いて、自動的に抽出される。特徴点の数としては、全画像に対して３０〜５００点程度を想定すると良いが、状況に応じて、固定であってもよいし、可変であってもよい。
【００２２】
Ｈａｒｒｉｓオペレータを用いる方法では、以下に説明するように、特徴点を抽出する。まず、画像の点（ｕ，ｖ）の輝度をＩ（ｕ，ｖ）として、以下の（１）式によって、行列Ｍを計算する。
【００２３】
【数１】
【００２４】
ただし、Ｉｕ，Ｉｖはそれぞれ水平方向、垂直方向の微分、Ｇσは標準偏差σのガウス分布による平滑化を表す。
【００２５】
そして、上記（１）式で計算された行列Ｍの固有値λ１、λ２を用いて、以下の（２）式によりコーナー強度を計算する。
【００２６】
【数２】
【００２７】
ただし、ｋは予め設定される定数であって、０．０４〜０．０６の値が一般に用いられる。Ｈａｒｒｉｓオペレータを用いる方法では、このコーナー強度がしきい値以上でかつ極大となる点を選択し、選択された点を特徴点として抽出する。
【００２８】
対応点検索部３２は、特徴点抽出部３０において２つの画像の各々から抽出された特徴点について、２つの画像間での対応付けを行って、２つの画像間の対応点を検索する。画像間での特徴点の対応付けでは、対応する点が同一ならば、画像１と画像２で対応する点とその周囲の点の輝度は殆ど変化しないと仮定し、この仮定に基づいて各特徴点の対応付けを行う。例えば、特徴点周辺に設定した小領域での輝度分布が似ている点の組を選択し、選択された点の組を対応点とする。特徴点の対応付け（特徴点追跡）の周知の手法として、ルーカスカナデ法（Ｌｕｃａｓ−Ｋａｎａｄｅ法）などが挙げられる。
【００２９】
特徴点選択部３４は、特徴点の対応付け（追跡）が完了した全対応点の中から、運動推定に用いる対応点の組を任意に選択する。ここで選択した対応点の組み合わせによって、運動算出部３６で算出される運動推定候補が決定される。なお、各種組み合わせについて、対応点は８組以上選択するものとする。
【００３０】
運動算出部３６は、特徴点選択部３４より選択された対応点の複数種類の組み合わせの各々について、当該組み合わせにおける少なくとも８組の対応点の画像座標から、２つの画像の各々が撮像されたときの画像撮像部１２の位置及び姿勢の変化（ＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量）を計算する。位置姿勢の変化は、図３に示すように、第１の画像から第２の画像への回転行列Ｒ（Ｘ軸を基準とする回転量、Ｙ軸を基準とする回転量、Ｚ軸を基準とする回転量）と、並進ベクトルｔ（Ｘ軸方向の移動量ｔｘ、Ｙ軸方向の移動量ｔｙ、Ｚ軸方向の移動量ｔｚ）との６要素から構成される運動である。なお、回転行列Ｒ及び並進ベクトルｔの要素は、２つの画像間の画像座標の変換を表す物理量である。
【００３１】
ここで、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとの計算方法について説明する。第１の画像におけるｎ点の対応点の画像座標ｘ１と第２の画像におけるｎ点の対応点の画像座標ｘ２とについて（ｎ≧８）、対応点が正しくて誤差がなければ、以下の（３）式を満たす３×３行列Ｆが存在する。
【００３２】
【数３】
【００３３】
ここで、８組以上の対応点ｘ１、ｘ２があれば、上記の基礎行列Ｆを算出することができる。
【００３４】
上述したように基礎行列Ｆが計算でき、また、画像撮像部１２のキャリブレーション行列Ｋが既知である場合には、カメラの撮像特性に依る画像の歪みを補正するキャリブレーション行列Ｋ、及び基礎行列Ｆを用いて、以下の（４）式、（５）式より、回転行列Ｒと並進ベクトルｔとを計算することができる．
【００３５】
【数４】
【００３６】
ここで算出した運動を運動推定候補とする。特徴点選択部３４による特徴点の選択と運動算出部３６による算出とをＮ回繰り返すことにより、Ｎ個の運動推定候補が得られる。
【００３７】
また、信頼度判定部２２は、図４に示すように、複数の運動推定候補の各々について、当該運動推定候補に対し動きが整合する特徴点を選別する特徴点選別部４０と、各運動推定候補について選別された特徴点の数から運動推定候補の信頼度を算出する信頼度評価部４２と、運動推定候補の信頼度に基づいて、運動推定候補を並び替えると共に選定する運動推定候補選定部４４とを備えている。
【００３８】
特徴点選別部４０は、複数の運動推定候補の各々について、当該運動推定候補を算出する際に用いられた複数の特徴点のうち、当該運動推定候補に対し動きが整合する特徴点を選別する。
【００３９】
例えば、図５に示すように、画像１で座標ｘ１に位置する特徴点Ｐが、基礎行列Ｆで表わされる運動に従って動く時、画像２での特徴点Ｐの座標はＦｘ１で表わされる線（エピポーラ線）上に位置する。すなわち、画像２における対応する特徴点の位置とエピポーラ線の距離が近い程、その特徴点の動きは運動推定候補に対する整合性が高いと言える。この性質を利用して、当該運動推定候補を算出する際に用いられた複数の特徴点のうち、対応するエピポーラ線との距離が閾値以下となる特徴点を選別する。
【００４０】
信頼度評価部４２は、複数の運動推定候補の各々について、特徴点選別部４０で選別された、当該運動推定候補に対し動きが整合する特徴点の数から、当該運動推定候補の信頼度を算出する。特徴点の数が大きいほど、高い信頼度が算出される。また、信頼度の指標として、選別された特徴点の数をそのまま用いてもよいし、特徴点の数に任意の関数を適用した結果を用いてもよい。
【００４１】
運動推定候補選定部４４は、Ｎ個の運動推定候補について、算出された信頼度が高い順に並び替えると共に、信頼度が高い方から順にＭ個選定する。これによって、信頼度によって順位付けされた運動推定候補Ｃ１〜ＣＭが出力される。なお、Ｎ個全ての運動推定候補を出力するようにしてもよいし、任意に設定した閾値よりも信頼度が高い運動推定候補だけを出力するようにしてもよい。
【００４２】
オフセット補正部２４は、ジャイロセンサ１４によって計測されたヨー角速度の計測値を取得し、ジャイロセンサ１４の計測値のオフセット（定常的な偏差）を補正した値を、運動計測結果Ｙｇとして出力する。運動検証部２６の出力値とジャイロセンサ１４の計測値とについて、現時刻から過去一定期間の平均値の差を算出し、オフセット補正値として設定する。また、オフセット補正値はタイムステップ毎に算出される。ただし、運動検証部２６において、運動推定候補算出部２０により算出された運動推定候補が全て不採用であった場合、その時刻の入力を除いて平均値を算出する。
【００４３】
運動検証部２６は、信頼度判定部２２から信頼度と共に出力された運動推定候補Ｃ１〜ＣＭを、オフセット補正部２４からの出力値Ｙｇと比較すると共に、信頼度を参照して、適切な運動推定候補を決定する。
【００４４】
比較項目は、運動推定候補に含まれる運動の回転３成分（ｐｉｔｃｈ、ｙａｗ、ｒｏｌｌ）のうちのヨー角速度とし、まず、運動推定候補Ｃ１から得られたヨー角速度Ｙ１と、オフセット補正部２４から得られたヨー角速度Ｙｇとを比較する。ヨー角速度に関する閾値をＹｔｈとして、｜Ｙ１−Ｙｇ｜＜ＹｔｈであればＣ１を運動推定結果として出力する。｜Ｙ１−Ｙｇ｜≧Ｙｔｈである場合には、Ｃ１と同様の方法でＣ２について検証を行う。以下同様に、出力値が得られるまで検証を繰り返し、ＣＭまで全て検証が終わった時点で出力値が得られない場合は、Ｙｇを運動の推定結果として出力する。
【００４５】
次に、第１の実施の形態に係る運動推定装置１０の作用について説明する。なお、運動推定装置１０を搭載した自車両が走行しているときに、自車両の運動を推定する場合を例に説明する。
【００４６】
まず、運動推定装置１０において、図６に示す運動推定処理ルーチンが実行される。ステップ１００において、異なるタイミングで画像撮像部１２によって自車両前方を撮像した第１の画像及び第２の画像を取得すると共に、第１の画像及び第２の画像を撮像する間にジャイロセンサ１４によって計測されたヨー角速度を取得する。そして、ステップ１０２において、上記ステップ１００で取得された第１の画像及び第２の画像に基づいて、複数の運動推定候補を算出する。
【００４７】
上記ステップ１０２は、図７に示す運動推定候補算出処理ルーチンによって実現される。まず、ステップ１２０において、第１の画像及び第２の画像から、それぞれ所定の数だけ特徴点を抽出する。そして、ステップ１２２において、上記ステップ１２０で抽出された第１の画像における特徴点の各々を、第２の画像において追跡し、第２の画像から、対応する各特徴点をそれぞれ検索する。
【００４８】
そして、ステップ１２４において、算出された運動推定候補の数を示す変数ｎを初期値の１に設定し、ステップ１２６において、上記ステップ１２２で検索された複数組の対応点から、８組の対応点をランダムに選択する。ステップ１２８では、上記ステップ１２６で選択された８組の対応点（２時刻間で互いに対応する特徴点の組）から、２つの撮像時刻間における画像撮像部１２の相対的な位置関係、即ち、画像撮像部１２を搭載した自車両のその２時刻間における運動を推定する。
【００４９】
そして、ステップ１３０では、変数ｎが、算出する運動推定候補の数を表わす定数Ｎ未満であるか否かを判定し、Ｎ個の運動推定候補を算出していない場合には、ステップ１３２において、変数ｎをインクリメントしてステップ１２６へ戻るが、一方、変数ｎがＮに達した場合には、運動推定候補算出処理ルーチンを終了する。以上のように、運動推定候補算出処理ルーチンが実行されると、Ｎ個の運動推定候補が出力される。
【００５０】
そして、運動推定処理ルーチンのステップ１０４では、上記ステップ１０２で算出されたＮ個の運動推定候補の各々について、信頼度を判定する。上記ステップ１０４は、図８に示す信頼度判定処理ルーチンによって実現される。
【００５１】
まず、ステップ１４０において、判定対象の運動推定候補を設定し、ステップ１４２において、判定対象の運動推定候補を算出する際に用いられた複数の特徴点の各々について、第２の画像におけるエピポーラ線を求める。
【００５２】
そして、ステップ１４４において、判定対象の運動推定候補を算出する際に用いられた複数の特徴点から、第２の画像における対応する特徴点とエピポーラ線との距離が閾値以下となる特徴点を選別する。次のステップ１４６では、上記ステップ１４４で選別された特徴点の数に基づいて、判定対象の運動推定候補の信頼度を判定する。
【００５３】
そして、ステップ１４８では、全ての運動推定候補について信頼度の判定が行われたか否かを判定し、信頼度が判定されていない運動推定候補がある場合には、ステップ１４０へ戻るが、一方、全ての運動推定候補について信頼度の判定が行われた場合には、ステップ１５０へ移行する。
【００５４】
ステップ１５０では、各運動推定候補について判定された信頼度に基づいて、Ｎ個の運動推定候補を、信頼度の高い順に並び替えると共に、信頼度の高い方から順にＭ個の運動推定候補を選定して、信頼度判定処理ルーチンを終了する。
【００５５】
そして、運動推定処理ルーチンのステップ１０６では、後述するステップ１１２で前回算出されたオフセット補正値を用いて、上記ステップ１００で取得したヨー角速度のオフセット量を補正する。次のステップ１０８では、上記ステップ１０４で選定された運動推定候補のヨー角速度を、信頼度の高い方から順に、上記ステップ１０６で補正されたヨー角速度と比較して検証し、補正されたヨー角速度との差分の絶対値が閾値未満となる運動推定候補のうち、最も信頼度が高い運動推定候補を、自車両の運動の推定結果として出力する。また、全ての運動推定候補のヨー角速度について、補正されたヨー角速度との差分の絶対値が閾値以上であった場合には、補正されたヨー角速度を、自車両の運動の推定結果として出力する。
【００５６】
そして、ステップ１１０において、過去一定期間で推定された運動のヨー角速度の平均値と、過去一定期間でジャイロセンサ１４より計測されたヨー角速度の平均値とを算出し、ステップ１１２において、上記ステップ１１０で算出されたヨー角速度の平均値の差分を、オフセット補正値として算出して、上記ステップ１００へ戻る。
【００５７】
上記のように、運動推定処理ルーチンが実行されると、撮像画像による運動推定によって、図９に示すように、複数の運動推定候補が算出され、ジャイロセンサ１４の計測値を用いた運動検証によって最適な運動推定候補が選択される。また、図１０に示すように、複数の運動推定候補に対して運動検証を行うため、いくつかの運動推定候補が不採用と判断されても、適切な運動推定候補が選択され、適切な運動推定結果が安定して得られる。
【００５８】
以上説明したように、第１の実施の形態に係る運動推定装置によれば、複数の運動推定候補の各々の信頼度と、ジャイロセンサによって計測されたヨー角速度とに基づいて、自車両の運動を推定することにより、簡易な構成で、安定して自車両の運動を精度よく推定することができる。
【００５９】
また、ジャイロセンサによる計測値のオフセット量を補正することにより、自車両のヨー角速度を精度よく計測することができ、これにより、自車両の運動をより精度よく推定することができる。
【００６０】
また、撮像画像に基づく運動推定において、複数の運動推定候補を算出し、ジャイロセンサの計測値を用いた運動検証によって最適な運動推定候補を選択することで、適切な運動推定結果を安定的に取得することができる。
【００６１】
また、適切な運動推定結果を安定的に得ることで、時刻と共に変動するジャイロセンサのオフセット量を適時補正することが可能となり、ジャイロセンサのオフセット補正後の計測値の精度が安定する。更に、ジャイロセンサの計測値を用いた運動検証の妥当性が向上する。
【００６２】
なお、上記の実施の形態では、全ての運動推定候補のヨー角速度について、計測されたヨー角速度との差の絶対値が閾値以上である場合に、計測されたヨー角速度を、自車両の２時刻間の運動の推定結果として出力する場合を例に説明したが、これに限定されるものではなく、全ての運動推定候補の信頼度が、信頼度に関する閾値未満である場合に、計測されたヨー角速度を、運動の推定結果として出力するようにしてもよい。
【００６３】
次に、第２の実施の形態について説明する。なお、第２の実施の形態に係る運動推定装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【００６４】
第２の実施の形態では、運動推定候補の算出と信頼度の判定とを行う順序が、第１の実施の形態と異なっている。
【００６５】
第２の実施の形態に係る運動推定装置では、運動推定候補算出部２０によって、運動推定候補を１つ算出すると、信頼度判定部２２によって、算出された運動推定候補の信頼度を判定する。また、上記の運動推定候補算出部２０による算出及び信頼度判定部２２による判定が、逐次的に繰り返され、複数の運動推定候補と、各運動推定候補の信頼度とが得られる。
【００６６】
なお、第２の実施の形態に係る運動推定装置の他の構成及び処理については、第１の実施の形態と同様であるため、説明を省略する。
【００６７】
次に、第３の実施の形態について説明する。なお、第３の実施の形態に係る運動推定装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。
【００６８】
第３の実施の形態では、運動推定候補を算出する毎に、運動検証を行っている点が、主に第１の実施の形態と異なっている。
【００６９】
第３の実施の形態に係る運動推定装置の運動推定候補算出部２０では、運動推定候補を１つ算出し、信頼度判定部２２によって、算出された運動推定候補の信頼度を判定し、信頼度が閾値以上である運動推定候補を算出する。
【００７０】
また、運動検証部２６では、信頼度判定部２２によって出力された運動推定候補Ｃ１のヨー角速度Ｙ１と、オフセット補正部２４から出力されたヨー角速度Ｙｇとを比較し、｜Ｙ１−Ｙｇ｜＜Ｙｔｈとなった場合、運動推定候補Ｃ１を、運動の推定結果として出力する。一方、｜Ｙ１−Ｙｇ｜≧Ｙｔｈとなった場合に、運動推定候補算出部２０で新たに運動推定候補を１つ算出し、信頼度判定部２２によって、算出された運動推定候補の信頼度を判定する。
【００７１】
このように、オフセット補正部２４から出力されたヨー角速度Ｙｇとの差の絶対値が閾値未満となるヨー角速度を含む運動推定候補が得られるまで、運動推定候補算出部２０による算出、信頼度判定部２２による判定、及び運動検証部２６による運動検証を逐次的に繰り返し行う。
【００７２】
なお、第３の実施の形態に係る運動推定装置の他の構成及び処理については、第１の実施の形態と同様であるため、説明を省略する。
【００７３】
また、上記の第１の実施の形態〜第３の実施の形態では、ジャイロセンサによってヨー角速度を計測する場合を例に説明したが、これに限定されるものではなく、ジャイロセンサによって３軸角速度を計測するように構成してもよく、ヨー角速度、ピッチ角速度、及びロール角速度の少なくとも１つを計測するように構成してもよい。この場合、運動推定候補との比較項目は、運動の回転３成分（ｐｉｔｃｈ、ｙａｗ、ｒｏｌｌ）のうち、１成分でもよいし、複数成分でもよいし、複数成分の組み合わせでもよい。また、センサによって並進３成分（ｔｘ，ｔｙ，ｔｚ）の少なくとも１つを計測するようにしてもよく、この場合に、運動推定候補の並進成分と比較するようにすればよい。
【００７４】
また、運動推定候補のヨー角速度とジャイロセンサによって計測されたヨー角速度との差分の絶対値に対する閾値判定で用いる閾値Ｙｔｈが、一定値である場合を例に説明したが、これに限定されるものではなく、例えば、ジャイロセンサの誤差変動が大きければ、閾値Ｙｔｈを高めの値に設定するなど、ジャイロセンサの誤差の特性に合わせて可変の値を設定するようにしてもよい。
【００７５】
本発明のプログラムは、記憶媒体に格納して提供するようにしてもよい。
【図面の簡単な説明】
【００７６】
【図１】本発明の第１の実施の形態に係る運動推定装置を示すブロック図である。
【図２】本発明の第１の実施の形態に係る運動推定装置の運動推定候補算出部の構成を示すブロック図である。
【図３】抽出された特徴点と移動体の運動との関係を説明するための図である。
【図４】本発明の第１の実施の形態に係る運動推定装置の信頼度判定部の構成を示すブロック図である。
【図５】エピポーラ線を説明するためのイメージ図である。
【図６】本発明の第１の実施の形態に係る運動推定装置における運動推定処理ルーチンの内容を示すフローチャートである。
【図７】本発明の第１の実施の形態に係る運動推定装置における運動推定候補算出処理ルーチンの内容を示すフローチャートである。
【図８】本発明の第１の実施の形態に係る運動推定装置における信頼度判定処理ルーチンの内容を示すフローチャートである。
【図９】本発明の第１の実施の形態に係る運動推定装置の処理の流れを示すイメージ図である。
【図１０】複数の運動推定候補から、適切な運動推定候補を選択する処理を示すイメージ図である。
【符号の説明】
【００７７】
１０ 運動推定装置
１２ 画像撮像部
１４ ジャイロセンサ
１６ コンピュータ
２０ 運動推定候補算出部
２２ 信頼度判定部
２４ オフセット補正部
２６ 運動検証部
３０ 特徴点抽出部
３２ 対応点検索部
３４ 特徴点選択部
３６ 運動算出部
４０ 特徴点選別部
４２ 信頼度評価部
４４ 運動推定候補選定部
【特許請求の範囲】
【請求項１】
移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の推定候補を複数算出する候補算出手段と、
前記移動体の運動を計測する運動計測手段と、
前記候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段と、
前記運動計測手段によって計測された前記移動体の運動、及び前記信頼度算出手段によって算出された前記複数の運動の推定候補の各々の信頼度に基づいて、前記複数の運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段と、
を含む運動推定装置。
【請求項２】
前記運動推定手段は、前記運動計測手段によって計測された前記移動体の運動との差分の絶対値が閾値未満となる前記運動の推定候補のうち、算出された信頼度が最も高い運動の推定候補を選択する請求項１記載の運動推定装置。
【請求項３】
前記運動推定手段は、前記複数の運動の推定候補の全てについて、計測された前記移動体の運動との差分の絶対値が閾値以上となる場合、又は、算出された信頼度が信頼度に関する閾値未満である場合、計測された前記移動体の運動を、前記移動体の運動の推定結果とする請求項１又は２記載の運動推定装置。
【請求項４】
前記運動計測手段は、ジャイロセンサを用いて前記移動体の運動を計測する請求項１〜請求項３の何れか１項記載の運動推定装置。
【請求項５】
前記ジャイロセンサを用いて計測される前記移動体の運動の計測値のオフセット量を補正するオフセット補正手段を更に含む請求項４記載の運動推定装置。
【請求項６】
前記オフセット補正手段は、前記運動計測手段による前記移動体の運動の計測値と、前記運動推定手段による前記移動体の運動の推定結果との差分に基づいて、前記オフセット量を補正する請求項５記載の運動推定装置。
【請求項７】
前記候補算出手段は、前記複数の画像の各々から、前記複数の画像間で対応した特徴点を検索する検索手段と、前記検索された対応した特徴点の組み合わせを繰り返し選択し、前記繰り返し選択された前記対応した特徴点の組み合わせに基づいて、前記複数の画像の各々を撮像したときの前記移動体の位置及び姿勢の相対関係を表わす運動を繰り返し算出する運動算出手段を備え、前記運動算出手段によって繰り返し算出された前記運動を、前記複数の運動の推定候補とする請求項１〜請求項６の何れか１項記載の運動推定装置。
【請求項８】
コンピュータを、
移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の推定候補を複数算出する候補算出手段、
前記候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段、及び
前記移動体の運動を計測する運動計測手段によって計測された前記移動体の運動、及び前記信頼度算出手段によって算出された前記複数の運動の推定候補の各々の信頼度に基づいて、前記複数の運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段
として機能させるためのプログラム。
【請求項１】
移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の推定候補を複数算出する候補算出手段と、
前記移動体の運動を計測する運動計測手段と、
前記候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段と、
前記運動計測手段によって計測された前記移動体の運動、及び前記信頼度算出手段によって算出された前記複数の運動の推定候補の各々の信頼度に基づいて、前記複数の運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段と、
を含む運動推定装置。
【請求項２】
前記運動推定手段は、前記運動計測手段によって計測された前記移動体の運動との差分の絶対値が閾値未満となる前記運動の推定候補のうち、算出された信頼度が最も高い運動の推定候補を選択する請求項１記載の運動推定装置。
【請求項３】
前記運動推定手段は、前記複数の運動の推定候補の全てについて、計測された前記移動体の運動との差分の絶対値が閾値以上となる場合、又は、算出された信頼度が信頼度に関する閾値未満である場合、計測された前記移動体の運動を、前記移動体の運動の推定結果とする請求項１又は２記載の運動推定装置。
【請求項４】
前記運動計測手段は、ジャイロセンサを用いて前記移動体の運動を計測する請求項１〜請求項３の何れか１項記載の運動推定装置。
【請求項５】
前記ジャイロセンサを用いて計測される前記移動体の運動の計測値のオフセット量を補正するオフセット補正手段を更に含む請求項４記載の運動推定装置。
【請求項６】
前記オフセット補正手段は、前記運動計測手段による前記移動体の運動の計測値と、前記運動推定手段による前記移動体の運動の推定結果との差分に基づいて、前記オフセット量を補正する請求項５記載の運動推定装置。
【請求項７】
前記候補算出手段は、前記複数の画像の各々から、前記複数の画像間で対応した特徴点を検索する検索手段と、前記検索された対応した特徴点の組み合わせを繰り返し選択し、前記繰り返し選択された前記対応した特徴点の組み合わせに基づいて、前記複数の画像の各々を撮像したときの前記移動体の位置及び姿勢の相対関係を表わす運動を繰り返し算出する運動算出手段を備え、前記運動算出手段によって繰り返し算出された前記運動を、前記複数の運動の推定候補とする請求項１〜請求項６の何れか１項記載の運動推定装置。
【請求項８】
コンピュータを、
移動体の外部を撮像した複数の画像に基づいて、前記移動体の運動の推定候補を複数算出する候補算出手段、
前記候補算出手段によって算出された複数の運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段、及び
前記移動体の運動を計測する運動計測手段によって計測された前記移動体の運動、及び前記信頼度算出手段によって算出された前記複数の運動の推定候補の各々の信頼度に基づいて、前記複数の運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段
として機能させるためのプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１０−１４５２１９（Ｐ２０１０−１４５２１９Ａ）
【公開日】平成２２年７月１日（２０１０．７．１）
【国際特許分類】
【出願番号】特願２００８−３２２２８３（Ｐ２００８−３２２２８３）
【出願日】平成２０年１２月１８日（２００８．１２．１８）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２２年７月１日（２０１０．７．１）
【国際特許分類】
【出願日】平成２０年１２月１８日（２００８．１２．１８）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]