JP5609667B2

JP5609667B2 - 運動推定装置及びプログラム

Info

Publication number: JP5609667B2
Application number: JP2011008829A
Authority: JP
Inventors: 洪二郎武山
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2011-01-19
Filing date: 2011-01-19
Publication date: 2014-10-22
Anticipated expiration: 2031-01-19
Also published as: JP2012150655A

Description

本発明は、運動推定装置及びプログラムに係り、特に、移動体の運動を推定する運動推定装置及びプログラムに関する。

従来より、ＲＡＮＳＡＣ（ＲａｎｄｏｍＳａｍｐｌｉｎｇＣｏｎｓｅｎｓｕｓ法）を用いて、最も多くの特徴点の動き（オプティカルフロー）に従う運動を推定することで、少数の誤ったオプティカルフローの影響を排除し、ロバストな運動推定を行う運動計測装置が知られている（例えば、特許文献１）。

特開２００６−３５０８９７号公報

しかしながら、上記の特許文献１に記載の技術では、移動物上のオプティカルフローが多いシーンなどにおいて、多数のオプティカルフローに誤りが生じているため、多くのオプティカルフローに従う運動を推定することで誤差が生じてしまう、という問題がある。

本発明は、上記の問題点を解決するためになされたもので、移動体の運動を精度よく推定することができる運動推定装置及びプログラムを提供することを目的とする。

上記の目的を達成するために本発明に係る運動推定装置は、移動体の外部を撮像した複数の画像の各々から、前記複数の画像間で対応した特徴点を検索する検索手段と、前記検索手段によって検索された前記対応した特徴点の全ての組から、前記対応した特徴点の複数の組を繰り返し選択し、前記繰り返し選択された前記対応した特徴点の複数の組に基づいて、前記複数の画像の各々を撮像したときの前記移動体の位置及び姿勢の相対関係を表わす運動を、前記移動体の運動の推定候補として各々算出する候補算出手段と、前記候補算出手段によって算出された前記運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段と、前記候補算出手段によって算出された前記運動の推定候補の各々に基づいて、前記運動の推定候補のばらつき度合いを算出し、前記運動の推定候補のばらつき度合いに基づいて、前記運動の推定候補の使用可否を判定する使用可否判定手段と、前記使用可否判定手段によって前記運動の推定候補が使用可能であると判定された場合に、前記信頼度算出手段によって算出された前記運動の推定候補の各々の信頼度に基づいて、前記運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段と、を含んで構成されている。

本発明に係るプログラムは、コンピュータを、移動体の外部を撮像した複数の画像の各々から、前記複数の画像間で対応した特徴点を検索する検索手段、前記検索手段によって検索された前記対応した特徴点の全ての組から、前記対応した特徴点の複数の組を繰り返し選択し、前記繰り返し選択された前記対応した特徴点の複数の組に基づいて、前記複数の画像の各々を撮像したときの前記移動体の位置及び姿勢の相対関係を表わす運動を、前記移動体の運動の推定候補として各々算出する候補算出手段、前記候補算出手段によって算出された前記運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段、前記候補算出手段によって算出された前記運動の推定候補の各々に基づいて、前記運動の推定候補のばらつき度合いを算出し、前記運動の推定候補のばらつき度合いに基づいて、前記運動の推定候補の使用可否を判定する使用可否判定手段、及び前記使用可否判定手段によって前記運動の推定候補が使用可能であると判定された場合に、前記信頼度算出手段によって算出された前記運動の推定候補の各々の信頼度に基づいて、前記運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段として機能させるためのプログラムである。

本発明によれば、検索手段によって、移動体の外部を撮像した複数の画像の各々から、前記複数の画像間で対応した特徴点を検索する。候補算出手段によって、検索手段によって検索された対応した特徴点の全ての組から、対応した特徴点の複数の組を繰り返し選択し、繰り返し選択された対応した特徴点の複数の組に基づいて、複数の画像の各々を撮像したときの移動体の位置及び姿勢の相対関係を表わす運動を、移動体の運動の推定候補として各々算出する。

そして、信頼度算出手段によって、候補算出手段によって算出された運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する。使用可否判定手段によって、候補算出手段によって算出された運動の推定候補の各々に基づいて、運動の推定候補のばらつき度合いを算出し、運動の推定候補のばらつき度合いに基づいて、運動の推定候補の使用可否を判定する。

そして、運動推定手段によって、使用可否判定手段によって運動の推定候補が使用可能であると判定された場合に、信頼度算出手段によって算出された運動の推定候補の各々の信頼度に基づいて、運動の推定候補のうちの何れか１つを、移動体の運動の推定結果として選択する。また、使用可否判定手段によって運動の推定候補が使用不可能であると判定された場合には、運動推定手段は、移動体の運動の推定結果の出力を行わない。

このように、運動の推定候補のばらつき度合いに基づいて、運動の推定候補が使用不可能であると判定された場合には、運動推定結果の出力を行わず、運動の推定候補のばらつき度合いに基づいて、運動の推定候補が使用可能であると判定された場合に、運動の推定候補の各々の信頼度に基づいて、運動の推定候補のうちの何れか１つを、移動体の運動の推定結果として選択することにより、移動体の運動を精度よく推定することができる。

本発明に係る候補算出手段は、検索手段によって検索された対応した特徴点の複数の組をランダムに繰り返し選択して、移動体の運動の推定候補を各々算出するようにすることができる。これによって、運動の推定方向の各々に誤差が分散し、運動の推定候補のばらつき度合いに反映されるため、移動体の運動をより精度よく推定することができる。

本発明に係る運動推定装置は、ジャイロセンサを用いて前記移動体の運動を計測する運動計測手段と、運動計測手段による移動体の運動の計測値と、運動推定手段による移動体の運動の推定結果との差分に基づいて、ジャイロセンサを用いて計測される移動体の運動の計測値のオフセット量を補正するオフセット補正手段とを更に含むようにすることができる。これによって、移動体の運動を精度よく計測することができ、移動体の運動をより精度よく推定することができる。

以上説明したように、本発明の運動推定装置及びプログラムによれば、運動の推定候補のばらつき度合いに基づいて、運動の推定候補が使用不可能であると判定された場合には、運動推定結果の出力を行わず、運動の推定候補のばらつき度合いに基づいて、運動の推定候補が使用可能であると判定された場合に、運動の推定候補の各々の信頼度に基づいて、運動の推定候補のうちの何れか１つを、移動体の運動の推定結果として選択することにより、移動体の運動を精度よく推定することができる、という効果が得られる。

本発明の第１の実施の形態に係る運動推定装置を示すブロック図である。本発明の第１の実施の形態に係る運動推定装置の運動推定候補算出部の構成を示すブロック図である。抽出された特徴点と移動体の運動との関係を説明するための図である。本発明の第１の実施の形態に係る運動推定装置の運動推定部の構成を示すブロック図である。エピポーラ線を説明するためのイメージ図である。運動推定候補の分散値を説明するための図である。オプティカルフローの誤差とシーンの信頼度との関係を示すグラフである。本発明の第１の実施の形態に係る運動推定装置における運動推定処理ルーチンの内容を示すフローチャートである。本発明の第１の実施の形態に係る運動推定装置における運動推定候補算出処理ルーチンの内容を示すフローチャートである。本発明の第１の実施の形態に係る運動推定装置における運動候補選定処理ルーチンの内容を示すフローチャートである。（Ａ）遠方の物体上の特徴点のオプティカルフローを示す図、及び（Ｂ）遠方の物体上の特徴点のオプティカルフローを用いて運動推定候補を算出する様子を示す図である。（Ａ）近くの物体上の特徴点のオプティカルフローを示す図、及び（Ｂ）近くの物体上の特徴点のオプティカルフローを用いて運動推定候補を算出する様子を示す図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、車両に搭載された運動推定装置に本発明を適用した場合を例に説明する。

図１に示すように、第１の実施の形態に係る運動推定装置１０は、自車両の前方の画像を撮像する単眼のカメラで構成される画像撮像部１２と、自車両の運動として、ヨー角速度を計測するジャイロセンサ１４と、画像撮像部１２によって撮像された画像、及びジャイロセンサ１４によって計測されたヨー角速度に基づいて、自車両の運動を推定して外部装置（図示省略）に出力するコンピュータ１６とを備えている。なお、ジャイロセンサ１４が、運動計測手段の一例である。

本実施の形態では、単眼のカメラを自車両の前方に設置して、自車両の前方の画像を取得する場合を例に説明するが、自車両の外部の画像を撮像すればよく、例えば、単眼のカメラを自車両の後方に設置し、自車両の後方を撮像してもよい。また、単眼のカメラは、通常の画角４０度程度のカメラでもよいし、広角のカメラでもよいし、全方位カメラでもよい。また、撮像する画像の波長について、種類は問わない。運動量を適切に推定できる場所であれば、設置場所、画角、波長、及び設置個数について問わない。

コンピュータ１６では、自車両の運動として、自車両の３軸角速度と並進方向を示す成分とを推定する。コンピュータ１６は、ＣＰＵと、ＲＡＭと、後述する運動推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１６は、画像撮像部１２によって撮像された複数の画像を取得する画像入力部１８と、複数の画像に基づいて、自車両の運動推定候補を複数算出する運動推定候補算出部２０と、算出された複数の運動推定候補に基づいて、自車両の運動を推定する運動推定部２２と、ジャイロセンサ１４によって計測された計測値のオフセット量を補正するオフセット補正部２４と、推定された自車両の運動又はオフセット補正されたヨー角速度の計測値を、自車両の運動の推定結果として出力する運動推定結果出力部２６とを備えている。

運動推定候補算出部２０は、図２に示すように、画像撮像部１２により得られた複数の画像の各々から、画像上で追跡しやすい特徴点を複数抽出する特徴点抽出部３０と、特徴点抽出部３０により得られた２つの画像の各々における特徴点から、２つの画像の間で対応する特徴点（以下、対応点と称する）を検索し、対応点に基づくオプティカルフローを算出する対応点検索部３２と、対応点検索部３２で得られたオプティカルフローから、８つのオプティカルフローを複数種類の組み合わせで選択するオプティカルフローグルーピング部３４と、選択されたオプティカルフローの組み合わせの各種類について、当該オプティカルフローの組み合わせを入力として、対応点が検索された一方の画像を撮像したときの画像撮像部１２の位置姿勢を基準とした、対応点が検索された他方の画像を撮像したときの画像撮像部１２の位置姿勢への変化（位置姿勢の相対関係）を、画像撮像部１２の運動のＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量として算出する運動算出部３６とを備えている。

特徴点抽出部３０は、画像撮像部１２から得られる異なる時刻に撮像した２枚の画像から、それぞれ特徴点を抽出する。特徴点とは、周囲の点と区別でき、異なる画像間で対応関係を求めることが容易な点のことを指す。特徴点は、２次元的に濃淡変化の勾配値が大きくなる画素を検出する方法（例えばＴｏｍａｓｉ−Ｋａｎａｄｅの方法や、Ｈａｒｒｉｓオペレータなど）を用いて、自動的に抽出される。特徴点の数としては、全画像に対して３０〜５００点程度を想定すると良いが、状況に応じて、固定であってもよいし、可変であってもよい。適切な抽出方法であればその手法は上記の限りではない。

Ｈａｒｒｉｓオペレータを用いる方法では、以下に説明するように、特徴点を抽出する。まず、画像の点（ｕ，ｖ）の輝度をＩ（ｕ，ｖ）として、以下の（１）式によって、行列Ｍを計算する。

ただし、Ｉ_ｕ，Ｉ_ｖはそれぞれ水平方向、垂直方向の微分、Ｇ_σは標準偏差σのガウス分布による平滑化を表す。

そして、上記（１）式で計算された行列Ｍの固有値λ_１、λ_２を用いて、以下の（２）式によりコーナー強度を計算する。

ただし、ｋは予め設定される定数であって、０．０４〜０．０６の値が一般に用いられる。Ｈａｒｒｉｓオペレータを用いる方法では、このコーナー強度がしきい値以上でかつ極大となる点を選択し、選択された点を特徴点として抽出する。

対応点検索部３２は、特徴点抽出部３０において２つの画像の各々から抽出された特徴点について、２つの画像間での対応付けを行って、２つの画像間の対応点を検索する。画像間での特徴点の対応付けでは、対応する点が同一ならば、画像１と画像２で対応する点とその周囲の点の輝度は殆ど変化しないと仮定し、この仮定に基づいて各特徴点の対応付けを行う。例えば、特徴点周辺に設定した小領域での輝度分布が似ている点の組を選択し、選択された点の組を対応点とする。特徴点の対応付け（特徴点追跡）の周知の手法として、ルーカスカナデ法（Ｌｕｃａｓ−Ｋａｎａｄｅ法）などが挙げられる。また、対応点検索部３２は、検索した対応点の各々について、オプティカルフローを算出する。

オプティカルフローグルーピング部３４は、特徴点の対応付け（追跡）が完了した全対応点について算出された全オプティカルフローの中から、運動推定に用いる８つのオプティカルフローを任意に選択する。ここで選択したオプティカルフローの組み合わせによって、運動算出部３６で算出される運動推定候補が決定される。オプティカルフローグルーピング部３４は、８つのオプティカルフローの選択をＮ回繰り返し、Ｎ種類のオプティカルフローの組み合わせを生成する。なお、オプティカルフローの選び方はランダムであってもよいし、任意の方法でもよいが、オプティカルフローに含まれる誤差が分散するようにランダムに選択することが好ましい。また、８つ以上のオプティカルフローを選択するようにしてもよい。

運動算出部３６は、オプティカルフローグルーピング部３４より選択された８つのオプティカルフローの複数種類の組み合わせの各々について、当該組み合わせにおける８つのオプティカルフローから、２つの画像の各々が撮像されたときの画像撮像部１２の位置及び姿勢の変化（ＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量）を計算する。位置姿勢の変化は、図３に示すように、第１の画像から第２の画像への回転行列Ｒ（Ｘ軸を基準とする回転量、Ｙ軸を基準とする回転量、Ｚ軸を基準とする回転量）と、並進ベクトルｔ（Ｘ軸方向の移動量ｔ_ｘ、Ｙ軸方向の移動量ｔ_ｙ、Ｚ軸方向の移動量ｔ_ｚ）との６要素から構成される運動である。なお、回転行列Ｒ及び並進ベクトルｔの要素は、２つの画像間の画像座標の変換を表す物理量である。

ここで、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとの計算方法について説明する。第１の画像におけるｎ点の対応点の画像座標ｘ_１と第２の画像におけるｎ点の対応点の画像座標ｘ_２（オプティカルフローの始点と終点）について（ｎ≧８）、対応点が正しくて誤差がなければ、以下の（３）式を満たす３×３行列Ｆが存在する。

ここで、８組以上の対応点ｘ_１、ｘ_２があれば、上記の基礎行列Ｆを算出することができる。

上述したように基礎行列Ｆが計算でき、また、画像撮像部１２のキャリブレーション行列Ｋが既知である場合には、カメラの撮像特性に依る画像の歪みを補正するキャリブレーション行列Ｋ、及び基礎行列Ｆを用いて、以下の（４）式、（５）式より、回転行列Ｒと並進ベクトルｔとを計算することができる．

ここで算出した運動を運動推定候補とする。オプティカルフローグルーピング部３４によるオプティカルフローの選択と運動算出部３６による算出とをＮ回繰り返すことにより、Ｎ個の運動推定候補が得られる。

また、運動推定部２２は、図４に示すように、複数の運動推定候補の各々について、当該運動推定候補に対し動きが整合する特徴点を選別する特徴点選別部４０と、各運動推定候補について選別された特徴点の数から運動推定候補の信頼度を判定する信頼度判定部４２と、運動推定候補の信頼度に基づいて、運動推定候補を並び替えると共に選定する運動候補選定部４４と、複数の運動推定候補に基づいて、画像中のオプティカルフロー群に対する信頼度を、シーンの信頼度として判定するシーン信頼度判定部４６と、シーン信頼度判定部４６で判定したシーンの信頼度に基づいて、運動候補選定部４４で選定した運動推定候補の出力の可否を判定する使用判定部４８とを備えている。

特徴点選別部４０は、複数の運動推定候補の各々について、当該運動推定候補を算出する際に用いられた複数の特徴点のうち、当該運動推定候補に対し動きが整合する特徴点を選別する。

例えば、図５に示すように、画像１で座標ｘ_１に位置する特徴点Ｐが、基礎行列Ｆで表わされる運動に従って動く時、画像２での特徴点Ｐの座標はＦｘ_１で表わされる線（エピポーラ線）上に位置する。すなわち、画像２における対応する特徴点の位置とエピポーラ線の距離が近い程、その特徴点の動きは運動推定候補に対する整合性が高いと言える。この性質を利用して、当該運動推定候補を算出する際に用いられた複数の特徴点のうち、対応するエピポーラ線との距離が閾値以下となる特徴点を選別する。

信頼度判定部４２は、複数の運動推定候補の各々について、特徴点選別部４０で選別された、当該運動推定候補に対し動きが整合する特徴点の数に基づいて、当該運動推定候補の信頼度を判定する。特徴点の数が大きいほど、高い信頼度が判定される。また、信頼度の指標として、選別された特徴点の数をそのまま用いてもよいし、特徴点の数に任意の関数を適用した結果を用いてもよい。

運動候補選定部４４は、Ｎ個の運動推定候補について、算出された信頼度が高い順に並び替え、信頼度によって順位付けされた運動推定候補Ｍ_１〜Ｎ_Ｎのうち一番順位が高い運動推定候補が出力される。

シーン信頼度判定部４６は、運動推定候補算出部２０で得たＮ個の運動推定候補Ｍ_１〜Ｍ_Nに基づき、画像中のオプティカルフロー群に対する信頼度をシーンの信頼度として算出する。まず、図６に示すように、運動推定候補のばらつき度合いとして、運動推定候補Ｍ_１〜Ｍ_Nの分散値σを算出する。次に、その分散値σに基づいて、シーンの信頼度の算出を行う。例えば、以下の（６）式に従って、シーンの信頼度を算出する。
Ｒ=ｌｏｇ（１＋１／σ）・・・（６）
上記（６）式によって、オプティカルフロー誤差と反比例の関係にあるシーンの信頼度が得られる（図７参照）。なお、図７では、各オプティカルフローに対してランダムに誤差を与えたときの誤差の平均値［ｐｉｘｅｌ］と、シーンの信頼度との関係を示している。

分散値の算出では、運動推定候補Ｍ_１〜Ｍ_Nのうち、全て用いてもよいし、ジャイロセンサ１４を用いた計測結果との比較などによって選別した一部を用いてもよい。また、運動の回転３成分（ｐｉｔｃｈ、ｙａｗ、ｒｏｌｌ）と並進３成分（ｔｘ，ｔｙ，ｔｚ）のうち、分散値の算出に用いる運動成分は１成分でもよいし、複数成分でもよいし、複数成分の組み合わせでもよい。

使用判定部４８は、シーン信頼度判定部４６で判定したシーンの信頼度に基づき、運動候補選定部４４で選定した運動推定結果の出力の可否を判定する。例えば、θをシーンの信頼度Ｒに関する閾値とした場合、Ｒ≧θを満たす時のみ、選定した運動推定結果を出力可能であると判定する。これによって、誤差の値が一定値以下の運動推定結果のみが出力される。一方、Ｒ＜θの時は、使用判定部４８は、運動推定結果が使用不可であると判定する。例えば、誤ったオプティカルフローが多いシーンでは、シーンの信頼度Ｒが低くなるため、運動推定結果を出力しない。

オフセット補正部２４は、ジャイロセンサ１４によって計測されたヨー角速度の計測値を取得し、ジャイロセンサ１４の計測値のオフセット（定常的な偏差）を補正した値を、運動計測結果Ｙ_ｇとして出力する。運動推定部２２の出力値とジャイロセンサ１４の計測値とについて、現時刻から過去一定期間の平均値の差を算出し、オフセット補正値として設定する。また、オフセット補正値はタイムステップ毎に算出される。ただし、運動推定部２２において、運動推定候補算出部２０により算出された運動推定候補が全て不採用であった場合、その時刻の入力を除いて平均値を算出する。

運動推定結果出力部２６は、運動推定部２２から運動推定候補が出力された場合には、当該運動推定候補を、運動推定結果として出力し、運動推定部２２から運動推定候補が出力されなかった場合には、オフセット補正部２４から出力されたＹ_ｇを運動の推定結果として出力する。また、運動推定結果出力部２６は、運動推定部２２から運動推定候補が出力されなかった場合に、適当な文字列を出力するようにしてもよいし、運動推定値として通常取り得ない大きな値などを出力するようにしてもよい。

次に、第１の実施の形態に係る運動推定装置１０の作用について説明する。なお、運動推定装置１０を搭載した自車両が走行しているときに、自車両の運動を推定する場合を例に説明する。

まず、運動推定装置１０において、図８に示す運動推定処理ルーチンが実行される。ステップ１００において、異なるタイミングで画像撮像部１２によって自車両前方を撮像した第１の画像及び第２の画像を取得すると共に、第１の画像及び第２の画像を撮像する間にジャイロセンサ１４によって計測されたヨー角速度を取得する。そして、ステップ１０２において、上記ステップ１００で取得された第１の画像及び第２の画像に基づいて、複数の運動推定候補を算出する。

上記ステップ１０２は、図９に示す運動推定候補算出処理ルーチンによって実現される。まず、ステップ１２０において、第１の画像及び第２の画像から、それぞれ所定の数だけ特徴点を抽出する。そして、ステップ１２２において、上記ステップ１２０で抽出された第１の画像における特徴点の各々を、第２の画像において追跡し、第２の画像から、対応する各特徴点をそれぞれ検索し、検索された対応点のオプティカルフローを各々算出する。

そして、ステップ１２４において、算出された運動推定候補の数を示す変数ｎを初期値の１に設定し、ステップ１２６において、上記ステップ１２２で算出された複数のオプティカルフローから、８つのオプティカルフローの組み合わせをランダムに選択する。ステップ１２８では、上記ステップ１２６で選択された８つのオプティカルフローから、２つの撮像時刻間における画像撮像部１２の相対的な位置関係、即ち、画像撮像部１２を搭載した自車両のその２時刻間における運動を推定する。

そして、ステップ１３０では、変数ｎが、算出する運動推定候補の数を表わす定数Ｎ未満であるか否かを判定し、Ｎ個の運動推定候補を算出していない場合には、ステップ１３２において、変数ｎをインクリメントしてステップ１２６へ戻るが、一方、変数ｎがＮに達した場合には、運動推定候補算出処理ルーチンを終了する。以上のように、運動推定候補算出処理ルーチンが実行されると、Ｎ個の運動推定候補が出力される。

そして、運動推定処理ルーチンのステップ１０４では、上記ステップ１０２で算出されたＮ個の運動推定候補から、１つの運動推定候補を選定する。上記ステップ１０４は、図１０に示す運動候補選定処理ルーチンによって実現される。

まず、ステップ１４０において、判定対象の運動推定候補を設定し、ステップ１４２において、判定対象の運動推定候補を算出する際に用いられた複数の特徴点の各々について、第２の画像におけるエピポーラ線を求める。

そして、ステップ１４４において、判定対象の運動推定候補を算出する際に用いられた複数の特徴点から、第２の画像における対応する特徴点とエピポーラ線との距離が閾値以下となる特徴点を選別する。次のステップ１４６では、上記ステップ１４４で選別された特徴点の数に基づいて、判定対象の運動推定候補の信頼度を判定する。

そして、ステップ１４８では、全ての運動推定候補について信頼度の判定が行われたか否かを判定し、信頼度が判定されていない運動推定候補がある場合には、ステップ１４０へ戻るが、一方、全ての運動推定候補について信頼度の判定が行われた場合には、ステップ１５０へ移行する。

ステップ１５０では、各運動推定候補について判定された信頼度に基づいて、Ｎ個の運動推定候補を、信頼度の高い順に並び替え、最も信頼度の高い運動推定候補を選定して、運動候補選定処理ルーチンを終了する。

そして、運動推定処理ルーチンのステップ１０６では、上記ステップ１０２で算出されたＮ個の運動推定候補に基づいて、シーンの信頼度を判定する。次のステップ１０８では、上記ステップ１０６で判定されたシーンの信頼度と閾値とを比較して、上記ステップ１０４で選定された運動推定候補が、使用可能であるか否かを判定する。シーンの信頼度が閾値以上である場合には、選定された運動推定候補が、使用可能であると判定され、ステップ１１０において、上記ステップ１０４で選定された運動推定候補を、運動の推定結果として出力する。

そして、ステップ１１２において、過去一定期間で推定された運動のヨー角速度の平均値と、過去一定期間でジャイロセンサ１４より計測されたヨー角速度の平均値とを算出し、ステップ１１４において、上記ステップ１１２で算出されたヨー角速度の平均値の差分を、オフセット補正値として算出して、上記ステップ１００へ戻る。

一方、上記ステップ１０８で、シーンの信頼度が閾値未満であり、選定された運動推定候補が、使用不可であると判定された場合には、ステップ１１６において、上記ステップ１１４で前回算出されたオフセット補正値を用いて、上記ステップ１００で取得したヨー角速度のオフセット量を補正する。次のステップ１１８では、上記ステップ１１６で補正されたヨー角速度を、自車両の運動の推定結果として出力し、上記ステップ１００へ戻る。

以上説明したように、第１の実施の形態に係る運動推定装置によれば、運動推定候補の分散値に基づくシーンの信頼度に基づいて、運動推定候補が使用不可能であると判定された場合には、運動推定結果の出力を行わず、運動推定候補の分散値に基づくシーンの信頼度に基づいて、運動推定候補が使用可能であると判定された場合に、運動推定候補の各々の信頼度に基づいて、運動推定候補のうちの何れか１つを、移動体の運動の推定結果として選択することにより、自車両の運動を精度よく推定することができる。

また、照明条件の変化や移動物の移動などによって多数の特徴点が誤った動き（自車両の運動とは異なる動きを含む）をする時、ランダムに生成したオプティカルフローのグループから算出する運動推定候補は、それぞれランダムに誤差を含むため、複数の運動推定候補の分散値は増加する。よって、運動推定候補の分散値を指標として用いることで、誤った動きの特徴点が多いシーン（大きな運動推定誤差が生じる可能性が高いシーン）を区別することができ、運動推定誤差が生じやすいシーンか否かを判定することができる。また、この判定を行うことで、算出された運動推定候補から信頼度が高い運動のみを抽出することができる。

一般に、画像中の遠方の物体は、近くの物体に比べて、解像度（物体領域を表現する画素数）が低下する。このため、遠方の物体では近くの物体に比べて１画素あたりの誤差の影響が大きくなる。例えば、画像中のオプティカルフローが一律に１画素の誤差を有する場合、図１１（Ａ）に示すような近くの物体上のオプティカルフローを用いて自車運動推定を行うよりも、図１２（Ａ）に示すような遠方の物体上のオプティカルフローを用いて自車運動推定を行う方が推定精度の劣化が大きい。

本実施の形態では、オプティカルフロー群からオプティカルフローを複数個選択して、複数種類の組み合わせを作り、それぞれの組み合わせから運動推定候補を算出し、その分散値に基づきオプティカルフロー群全体の信頼性を評価している。従って、各オプティカルフローの画素の誤差ではなく、選択したオプティカルフローから算出した運動推定候補の分散値を評価することで、上述した遠近の差による誤差の影響の違いを反映することができる。すなわち、図１１（Ｂ）に示すように、選択したオプティカルフローが近くの物体上のオプティカルフローであれば、オプティカルフローの誤差が小さくなり、運動候補の分散が小さくなるため、シーンの信頼度が高くなる。一方、図１２（Ｂ）に示すように、選択したオプティカルフローが遠くの物体上のオプティカルフローであれば、オプティカルフローの誤差が大きくなり、運動候補の分散が大きくなるため、シーンの信頼度が低くなる。よって、上述した遠近の差による誤差の影響の違いを反映して、自車両の運動を精度良く推定することができる。

また、ジャイロセンサによる計測値のオフセット量を補正することにより、自車両のヨー角速度を精度よく計測することができ、これにより、自車両の運動をより精度よく推定することができる。

また、撮像画像に基づく運動推定において、複数の運動推定候補を算出し、運動推定候補が使用不可であると判定された場合、ジャイロセンサの計測値を運動推定結果として用いることで、適切な運動推定結果を安定的に取得することができる。

次に、第２の実施の形態について説明する。なお、第２の実施の形態に係る運動推定装置は、第１の実施の形態と同様の構成となっているため、同一符号を付して説明を省略する。

第２の実施の形態では、ジャイロセンサを用いた計測値との差分が一定範囲内となる運動推定候補から、運動推定候補を選定している点が、第１の実施の形態と異なっている。

第２の実施の形態に係る運動推定装置では、運動候補選定部４４によって、ジャイロセンサ１４によって計測されたヨー角速度の計測値を取得し、Ｎ個の運動推定候補について、ヨー角速度の計測値との差分が一定範囲内となる運動推定候補を選択する。また、運動候補選定部４４は、選択した運動推定候補（例えば、Ｌ個の運動推定候補とする）を、算出された信頼度が高い順に並び替え、信頼度によって順位付けされた運動推定候補Ｍ_１〜Ｍ_Ｌの（Ｌ≦Ｎ）うち一番順位が高い運動推定候補を選定して出力する。

なお、第２の実施の形態に係る運動推定装置の他の構成及び処理については、第１の実施の形態と同様であるため、説明を省略する。

また、上記の第１の実施の形態〜第２の実施の形態では、選別された特徴点の数に基づいて、運動推定候補に対する信頼度を判定する場合を例に説明したが、これに限定されるものではなく、運動推定候補を算出する際に用いられた複数の特徴点の各々とエピポーラ線との距離の平均値を用いて、当該運動推定候補に対する信頼度を判定してもよい。

また、算出した複数の運動推定候補のうち何れかを選定した後に、運動推定候補の使用可否を判定する場合を例に説明したが、これに限定されるものではなく、運動推定候補の使用可否を判定し、運動推定候補が使用可能であると判定された場合に、算出した複数の運動推定候補のうち何れかを選定するようにしてもよい。

また、８つのオプティカルフローの組み合わせを選択するたびに、運動推定候補を算出する場合を例に説明したが、これに限定されるものではなく、８つのオプティカルフローの組み合わせからなるグループを複数生成してから、各グループについて、運動推定候補を算出するようにしてもよい。

また、検索された対応点についてオプティカルフローを算出する場合を例に説明したが、これに限定されるものではなく、オプティカルフローを算出せずに、検索された全ての対応点から８つの対応点を選択して、選択された８つの対応点について、運動推定候補を算出するようにしてもよい。

また、ジャイロセンサによってヨー角速度を計測する場合を例に説明したが、これに限定されるものではなく、ジャイロセンサによって３軸角速度を計測するように構成してもよく、ヨー角速度、ピッチ角速度、及びロール角速度の少なくとも１つを計測するように構成してもよい。また、センサによって並進３成分の少なくとも１つを計測するようにしてもよい。

本発明のプログラムは、記憶媒体に格納して提供するようにしてもよい。

１０運動推定装置
１２画像撮像部
１４ジャイロセンサ
１６コンピュータ
２０運動推定候補算出部
２２運動推定部
２４オフセット補正部
２６運動推定結果出力部
３０特徴点抽出部
３２対応点検索部
３４オプティカルフローグルーピング部
３６運動算出部
４０特徴点選別部
４２信頼度判定部
４４運動候補選定部
４６シーン信頼度判定部
４８使用判定部

Claims

移動体の外部を撮像した複数の画像の各々から、前記複数の画像間で対応した特徴点を検索する検索手段と、
前記検索手段によって検索された前記対応した特徴点の全ての組から、前記対応した特徴点の複数の組を繰り返し選択し、前記繰り返し選択された前記対応した特徴点の複数の組に基づいて、前記複数の画像の各々を撮像したときの前記移動体の位置及び姿勢の相対関係を表わす運動を、前記移動体の運動の推定候補として各々算出する候補算出手段と、
前記候補算出手段によって算出された前記運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段と、
前記候補算出手段によって算出された前記運動の推定候補の各々に基づいて、前記運動の推定候補のばらつき度合いを算出し、前記運動の推定候補のばらつき度合いに基づいて、前記運動の推定候補の使用可否を判定する使用可否判定手段と、
前記使用可否判定手段によって前記運動の推定候補が使用可能であると判定された場合に、前記信頼度算出手段によって算出された前記運動の推定候補の各々の信頼度に基づいて、前記運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段と、
を含む運動推定装置。
前記候補算出手段は、前記検索手段によって検索された前記対応した特徴点の複数の組をランダムに繰り返し選択して、前記移動体の運動の推定候補を各々算出する請求項１記載の運動推定装置。
ジャイロセンサを用いて前記移動体の運動を計測する運動計測手段と、
前記運動計測手段による前記移動体の運動の計測値と、前記運動推定手段による前記移動体の運動の推定結果との差分に基づいて、前記ジャイロセンサを用いて計測される前記移動体の運動の計測値のオフセット量を補正するオフセット補正手段とを更に含む請求項１又は２記載の運動推定装置。
コンピュータを、
移動体の外部を撮像した複数の画像の各々から、前記複数の画像間で対応した特徴点を検索する検索手段、
前記検索手段によって検索された前記対応した特徴点の全ての組から、前記対応した特徴点の複数の組を繰り返し選択し、前記繰り返し選択された前記対応した特徴点の複数の組に基づいて、前記複数の画像の各々を撮像したときの前記移動体の位置及び姿勢の相対関係を表わす運動を、前記移動体の運動の推定候補として各々算出する候補算出手段、
前記候補算出手段によって算出された前記運動の推定候補の各々について、該運動の推定候補がどれだけ信頼できるかを示す信頼度を算出する信頼度算出手段、
前記候補算出手段によって算出された前記運動の推定候補の各々に基づいて、前記運動の推定候補のばらつき度合いを算出し、前記運動の推定候補のばらつき度合いに基づいて、前記運動の推定候補の使用可否を判定する使用可否判定手段、及び
前記使用可否判定手段によって前記運動の推定候補が使用可能であると判定された場合に、前記信頼度算出手段によって算出された前記運動の推定候補の各々の信頼度に基づいて、前記運動の推定候補のうちの何れか１つを、前記移動体の運動の推定結果として選択する運動推定手段
として機能させるためのプログラム。