JP6003673B2 - ３次元位置推定装置、車両制御装置、および３次元位置推定方法 - Google Patents

Description

本発明は、撮像画像中の特徴点の実空間での３次元位置を推定する３次元位置推定装置、車両制御装置、および３次元位置推定方法に関する。

上記３次元位置推定装置として、加速度センサ等の挙動情報を利用してカメラの移動量を求め、この移動量と、カメラで連続的に得られた撮像画像中の特徴点の位置とに基づいて撮像画像中の特徴点の実空間での３次元位置を推定するものが知られている（例えば、特許文献１参照）。

特開平１１−０６３９４９号公報

しかしながら、上記３次元位置推定装置では、移動量を検出する精度が挙動情報を検出するセンサの精度に依存するため、特徴点の実空間での３次元位置を精度よく求めようとすると、挙動情報を精度よく求めるための高性能（高価）な構成が必要になるという問題点があった。

そこで、このような問題点を鑑み、撮像画像中の特徴点の実空間での３次元位置を推定する３次元位置推定装置、車両制御装置、および３次元位置推定方法において、挙動情報を得るための高性能な構成を利用することなく、特徴点の実空間での３次元位置の推定精度を向上させることができるようにすることを本発明の目的とする。

かかる目的を達成するために成された本発明の３次元位置推定装置において、特徴点対応付手段は、複数の撮像画像中において１または複数の特徴点をそれぞれ抽出し、該特徴点について複数の撮像画像間での対応付けを行う。そして、挙動情報取得手段は、移動体の挙動に関わる挙動情報を取得し、移動量算出手段は、挙動情報に基づいて、複数の撮像画像を撮像する間での移動体の移動量を表す移動体移動量を算出する。さらに、実位置推定手段は、複数の撮像画像間での特徴点の移動量と移動体移動量とに基づいて実位置を推定する。

また、移動量補正手段は、複数の撮像画像のうちのある撮像画像に実位置を再投影した際の撮像画像上の位置を表す再投影位置とこの撮像画像において抽出された特徴点の位置を表す抽出位置との差異を検出し、該差異が小さくなるように前記移動体移動量を補正する。そして、補正後実位置推定手段は、複数の撮像画像間での特徴点の移動量と補正後の移動体移動量とに基づいて実位置を推定する。

このような３次元位置推定装置によれば、再投影位置と抽出位置とに基づいて移動体移動量を補正し、補正後の移動体移動量を利用して特徴点の実空間での３次元位置（実位置）を推定するので、挙動情報を得るための高性能な構成を利用することなく、実位置の推定精度を向上させることができる。

なお、請求項２に記載の構成と請求項５に記載の構成とを組み合わせる場合には、運動量成分毎の変化比率（例えば進行方向への移動量は大きいが横方向への移動量は少ない）を考慮して請求項２に記載の基準量を設定するとよい。

また、上記目的を達成するためには、３次元位置推定装置を含む車両制御装置や３次元位置推定方法としてもよい。或いは、コンピュータを、距離推定装置を構成する各手段として実現するための距離推定プログラムとしてもよい。

また、各請求項の記載は、可能な限りにおいて任意に組み合わせることができる。この際、発明の目的を達成できる範囲内において一部構成を除外してもよい。

本発明が適用された車両制御システム１の概略構成を示すブロック図である。 演算部１０が実行する３次元位置推定処理を示すフローチャートである。 実空間での座標系を示す概念図である。 カメラ座標系（ａ）および画像座標系（ｂ）を示す概念図である。 特徴点の対応付け（対応点の抽出）の処理の概念図である。 光線ベクトルの算出をする処理の概念図である。 ３次元位置の推定を行う処理の概念図である。 ノイズ除去を行う処理の概念図である。 再投影誤差の概念図である。 ３次元位置推定処理のうちの最適化処理を示すフローチャートである。 ３次元位置推定処理の出力例を示す説明図である。 運転支援システム３１が実行する車両制御処理を示すフローチャートである。

以下に本発明にかかる実施の形態を図面と共に説明する。
［本実施形態の構成］
本発明が適用された車両制御システム１は、例えば乗用車等の車両（自車両）に搭載されている。そして、カメラから得られた撮像画像によって障害物を認識し、この障害物と自車両とが衝突することを回避しようとする機能を有する。特に、障害物を認識する際には、障害物の３次元位置や形状を高精度に認識できるよう設定されている。

詳細には、図１に示すように、車両制御システム１は、演算部１０と、広角カメラ２１と、移動検出部２２と、運転支援システム３１と、備えている。広角カメラ２１は、いわゆる魚眼レンズを備えており、等距離射影方式で撮像を行う。そして、広角カメラ２１は、一般的に車両において利用されるカメラよりも広範囲（左右方向の概ね１００度以上の範囲内）を撮像範囲内とする撮像画像を生成し、この撮像画像を演算部１０に送る。

移動検出部２２は、移動体（自車両）の挙動を検出するためのセンサを備えた構成とされており、本実施形態においては、車速センサ２３とヨーレートセンサ２４とを備えている。車速センサ２３は自車両の車輪速度（車速）を検出する周知の車速センサとして構成されている。ヨーレートセンサ２４は、自車両の旋回角速度を検出する周知のヨーレートセンサとして構成されている。これらのセンサは、検出結果を演算部１０に送る。

演算部１０は、ＣＰＵ１１やメモリ１２を備えたコンピュータとして構成されており、メモリ１２に記録されたプログラムに応じて後述する３次元位置推定処理等の各種処理を実施する。３次元位置推定処理では、周囲の障害物等の物体の３次元位置を検出し、この３次元位置の情報を運転支援システム３１に出力する。

運転支援システム３１は、演算部１０と同様に、ＣＰＵやメモリを備えたコンピュータとして構成されており、メモリに記録されたプログラムに応じて後述する車両制御処理等の各種処理を実施する。車両制御処理では、演算部１０から入力される物体の３次元位置の情報を利用して自車両の挙動を制御する。

［本実施形態の処理］
このような車両制御システム１においては、図２に示すような３次元位置推定処理を実施する。３次元位置推定処理は、例えば車両の電源が投入されると開始され、その後、所定周期（例えば５０ｍｓ毎）で繰り返し実施される。

３次元位置推定処理は、広角カメラ２１からの撮像画像や移動検出部２２によって検出された自車両の挙動を利用して、周囲の障害物等の物体の３次元位置（実空間での３次元の座標）を検出する処理である。詳細には、図２に示すように、まず、現在時刻（最新）の広角カメラ２１からの撮像画像を入力するとともに（Ｓ１１０）、この撮像画像の直前に撮像された撮像画像を取得する（Ｓ１２０）。なお、広角カメラ２１から得られた撮像画像は後述するＳ２５０の処理にてメモリ１２に記録され、Ｓ１２０の処理ではこの撮像画像を取得する。

ここで、本処理においては３つの異なる座標系を用いる。まず、実際の空間（実空間）における座標系を示す車両座標系では、図３に示すように、広角カメラ２１が設置された位置の真下の地面の位置を基準に、広角カメラ２１の光軸方向（車両の進行方向（図３の例では前方であるが、後方や側方であってもよい。））をＺｖ軸、Ｚｖ軸に直交し路面と平行な方向（図３の例では車両の幅方向）をＹｖ軸、路面に直交する方向（車両の上下方向）をＸｖ軸、それぞれの軸において時計回りに回転する方向を、それぞれθｚｖ、θｙｖ、θｘｖとする。

また、撮像画像内での３次元座標を示すカメラ座標系では、図４（ａ）に示すように、広角カメラ２１の光軸方向をＺｃ軸、Ｚｃ軸に直交し路面と平行な方向をＹｃ軸、Ｚｃ軸とＹｃ軸に直交する方向をＸｃ軸、それぞれの軸において時計回りに回転する方向を、それぞれθｚｃ、θｙｃ、θｘｃとする。さらに、撮像画像内の２次元座標を示す画像座標系では、図４（ｂ）に示すように、撮像画像の左上隅を基準に、横方向にＸＩ軸、縦方向にＹＩ軸とする。

このような座標系を用いて、まず、複数（本実施形態では２つ）の撮像画像中において複数の特徴点をそれぞれ抽出し、各特徴点について複数の撮像画像間での対応付けを行う（Ｓ１３０）。対応付けができると、図５（ａ）の矢印に示すように、２枚の画像間での移動量が検出できる。

また、特徴点とは、図５（ｂ）に示すように、コーナー点、縦エッジや横エッジの交点等を示す。また、特徴点は、特定の色の点やその境界等であってもよい。
なお、特徴点の対応付け（オプティカルフロー）には、一般的な技術であるKLT法（Kanade-Lucas-Tomasi Feature Tracker）を利用している。この手法を用いるのは、勾配法に基づくKLT法が、車載画像のような動画像に適しているためである。ただし、下記のような他の特徴点対応付け手法を用いてもよい。

例えば、ブロックマッチング、SIFT（Scale Invariance Feature Transform）、CARD（Compact And Real-time Descriptors）、BRIEF（Binary Robust Independent Elementary Features）、BRISK（Binary Robust Invariant Scalable Keypoints）、ORB（Oriented FAST and Robust BRIEF）等の手法である。

続いて、光線ベクトルｐの算出を行う（Ｓ１４０）。ここで、光線ベクトルｐとは、カメラのレンズ中心から特徴点の３次元空間上の点Ｐに向かう光線をベクトル表記したものである。図６に示すように、カメラ座標系において天頂角θと方位角φを定義する。

なお、天頂角θは、光軸に対する光線の角度であり、像高ｄ（画像上における光軸と特徴点間の距離）とレンズの歪みパラメータ（像高と天頂角の関係を表すデータ）から求めることができる。また、方位角φは、軸ＸＩ或いは軸Ｙｃに対する光線の角度を示す。

これらの天頂角θおよび方位角φを用いて、光線ベクトルｐは、以下のように定義できる。

なお、上記式中のＴは、転置を示す。
続いて、車両情報を入力する（Ｓ１５０）。この処理では、移動検出部２２により検出された移動体の挙動に関わる挙動情報（車速およびヨーレート）を取得する。そして、並進ベクトルと回転行列とを生成する（Ｓ１６０）。この処理では、挙動情報に基づいて、複数の撮像画像を撮像する間での移動体の移動量（並進量や回転量等：移動体移動量）を算出する。

すなわち、車速とヨーレート（あるいは車輪速など）から前時刻（前回の撮像画像が得られた時刻）から現時刻（最新の撮像画像が得られた時刻）までの自車両の移動量を算出し、その値をカメラ搭載位置に基づいてカメラ座標系における値に変換してから、並進ベクトルと回転行列を生成する。

より詳細には、車両座標系での車両移動量、

を、カメラ座標系における値に変換する。

なお、車両座標系からカメラ座標系への変換は、予め実験を介して準備された所定の変換式を用いることで行うことができる。
そして、カメラ座標系における自車両の移動量を利用して、以下のように並進ベクトルｔと回転行列Ｒとを求めることができる。

ここで求められた並進ベクトルｔおよび回転行列Ｒは、後述する並進ベクトルの初期値ｔ０および回転行列の初期値Ｒ０として利用される。
なお、この並進ベクトルｔや回転行列Ｒの精度は、車両センサの精度に依存しており、特に低速域では真値との誤差が大きくなる傾向にある。また、センサの特性上Ｘｖ軸の移動量や、Ｙｖ軸・Ｚｖ軸の回転量は推定できない。

しかしながら、これらのセンサデータを用いることで、従来は多数の画像だけから移動量を推定していたのに比べ、低計算コストで推定ができる。さらに、スケールの不定性に関する問題も生じない。なお、スケールの不定性とは、方向については特定できるがその大きさが特定できない状態を示す。

次に、３次元位置の推定を行う（Ｓ１７０）。この処理は、複数の撮像画像間での特徴点の移動量と移動体移動量とに基づいて実位置を推定する処理である。
すなわち、並進ベクトルｔと回転行列Ｒとが得られれば、２画像間での対応付けを利用して、特徴点の３次元座標

を推定することができる。

上記数式では、図７に示すように、移動前と移動後のカメラのレンズ中心から特徴点に向かう光線ベクトルｐｉ（前時刻での光線ベクトル）、ｐ’ｉ（現時刻での光線ベクトル）が、３次元空間上で交わる点を特徴点の３次元位置Ｘｉとする。ただし、実際には、ノイズ等の影響で２つの光線ベクトルは３次元空間上で交わらない。そこで、Ｘｉから２直線（光線ベクトルｐｉ、ｐ’ｉ）までのユークリッド距離の２乗和Δが最小となるように３次元位置を求める。

次に、推定結果の信頼性評価を行い（Ｓ２１０）、信頼性が低い特徴点を除去する（Ｓ２２０）。すなわち、一般的に３次元位置の推定精度は、カメラの移動方向に近いほど、また、カメラから遠いものほど低下する傾向がある。これは、カメラが移動しても推定に必要な十分な視差が得られないためである。そこで、本処理では、カメラの移動方向、および距離に基づいて、推定結果の信頼性の低い特徴点を抽出し、除去する。

詳細には、例えば図８に示すように、車両座標系においてカメラから特徴点Ｘｉまでの距離ｄｃａｍ、予測進路からの距離ｄｐａｔｈを求め、以下のように評価値Ｖを演算する。

なお、上記式中において、Ｄｃａｍはｄｃａｍの最大値を示し、Ｄｐａｔｈはｄｐａｔｈの最大値を示す。これらの最大値を予め設定しておくことで、カメラや予測進路から過度に離れた特徴点の影響を軽減することが期待できる。

上記式で得られた評価値Ｖが予め設定された閾値未満である場合に、推定結果の信頼性が低いものとして除去する。
次に、並進ベクトルｔや回転行列Ｒを最適化する最適化処理を行う（Ｓ２３０）。最適化処理では、再投影誤差を最小化したときの値に基づき、並進ベクトルｔや回転行列Ｒを再設定（補正）する。ここで、再投影誤差とは、図９に示すように、推定された特徴点の３次元位置Ｘｉに対応する車両座標系の座標を、カメラ座標系、画像座標系の順に変換し、画像座標系において再投影したときの座標（再投影点）と、元の画像座標（特徴点ｐの座標）との差を示す。

挙動情報はセンサの精度・分解能などにより誤差が生じる虞があり、この誤差が生じると、並進ベクトルｔ、および回転行列Ｒにも誤差が生じることになる。並進ベクトルｔ、および回転行列Ｒの誤差が大きくなると、再投影誤差も大きくなる。

また、車両のピッチングの影響は車速センサやヨーレートセンサには現れない。そこで本処理では、特徴点の再投影誤差に基づいて、並進ベクトルｔと回転行列Ｒを最適化し、３次元位置の推定精度を高めるようにしている。

ここで、再投影誤差は次式で定義される。

なお、本実施形態においては、広角カメラ２１が車載カメラという前提であるため、ｔＸｖ方向の移動やθｚｖ方向への回転は、基本的に発生しない。そこで、ｔＹｖ、ｔＺｖ、θＸｖ、θＹｖ方向だけを考慮する。

ただし、ｔＸｖ、ｔＹｖ、ｔＺｖとは、自車両の移動量をＸｖ軸、Ｙｖ軸、Ｚｖ軸に分解した方向を示す。つまり、本処理では、移動体移動量を運動量成分に分解し、車両運動では変化しにくい運動量成分を除いた運動量成分について補正を行うことで移動体移動量を補正する。

そして、これらの移動量および回転量に微小変位Δ（成分毎の基準量）を加える。

さらに、それぞれの移動量および回転量に基づいて、それぞれ再投影誤差Ｅ１〜Ｅ８を
算出する。
この際に、再投影誤差が最小となったＥ１〜Ｅ８の何れか１つを採用する。

このような処理を繰り返すことで、反復法による最小二乗アルゴリズムを実現し、並進ベクトルｔおよび回転行列Ｒの最適化を行う。
なお、本処理ではこのような処理にて最適化処理を実施することで、車載センサの分解能よりも細かな微小変位を組み込むことが可能となり、最適化処理をセンサの分解能以上に高精度に行うことが可能となる。また、最適化するパラメータを限定したり、移動体の運動特性に基づいて基準量を変えることで、ノイズ等が最適化処理に与える影響を軽減することもできる。ただし、本処理にて提案する最適化処理に限らず、一般的に使用されている非線形最適化（Levenberg-Marquardt法など）を利用してもよい。

次に、最適化処理のより詳細な手順について図１０を用いて説明する。最適化処理では、まず、並進ベクトルｔの初期値ｔ０と、回転行列Ｒの初期値Ｒ０を取得する（Ｓ３１０）。

そして、再投影誤差の初期値Ｅ０を求める（Ｓ３２０）。なお、再投影誤差の初期値とは、並進ベクトルｔの初期値ｔ０、回転行列Ｒの初期値Ｒ０に対応する再投影誤差を示す。

続いて、処理回数ｊをリセット（ｊ＝０に設定）し（Ｓ３３０）、処理回数ｊと予め設定された処理回数の上限値Ｍ（例えば、Ｍ＝２０）とを比較する（Ｓ３４０）。処理回数ｊが上限値Ｍ未満であれば（Ｓ３４０：ＹＥＳ）、Ｅ１〜Ｅ８について再投影誤差を算出し（Ｓ３５０）、再投影誤差が最小となるＥｋをＥｍｉｎに設定するとともに、Ｅｋに対応する並進ベクトルｔや回転行列ＲをｔｍｉｎやＲｍｉｎに設定する（Ｓ３６０）。

そして、再投影誤差の初期値Ｅ０とＥｍｉｎとを比較する（Ｓ３７０）。ＥｍｉｎがＥ０以上であれば（Ｓ３７０：ＮＯ）、再投影誤差が小さくなっていないため最適化処理を終了する。また、ＥｍｉｎがＥ０未満であれば（Ｓ３７０：ＹＥＳ）、ＥｍｉｎをＥ０に、ｔｍｉｎをｔ０に、ＲｍｉｎをＲ０に設定し（Ｓ３８０）、処理回数ｊをインクリメントする（Ｓ３９０）。

続いて、Ｅ０と所定の閾値（Ｔｈｒｅｓ）とを比較する（Ｓ４００）。Ｅ０が閾値以上であれば（Ｓ４００：ＮＯ）、Ｓ３４０の処理に戻る。また、Ｅ０が閾値未満であれば（Ｓ４００：ＹＥＳ）、最適化処理を終了する。

なお、最適化処理によって得られた最適化後の並進ベクトルと回転行列については、運転支援システムでの利用を容易にするために、最適化後の並進ベクトルと回転行列を並進量と回転量とに分解しておくとよい。

並進量については並進ベクトルの要素をそのまま取り出せばよい。一方、回転行列は、各軸の回転量の合成となっているため、下記のような分解処理が必要となる。

このような処理が終了すると、複数の撮像画像間での特徴点の移動量と補正後の移動体移動量とに基づいて下記式を用いて実位置を再度推定する（Ｓ２４０）。

そして、広角カメラ２１から得られた最新の撮像画像や３次元位置推定結果をメモリ１２に記録し（Ｓ２５０）、本処理で得られた並進量および回転量を運転支援しシステム３１に出力するとともに（Ｓ２６０）、３次元位置推定結果を運転支援しシステム３１に出力し（Ｓ２７０）、３次元位置推定処理を終了する。

このような処理を繰り返し実施すると、図１１（ａ）および図１１（ｂ）に示すような走行環境を自車両が走行している場合には、図１１（ｃ）に示すようなグリッドマップが得られる。グリッドマップとは、実空間をグリッド（格子）状に分割し、物体の位置や形状を表現する地図である。図１１（ｃ）では、走行環境を鳥瞰図のように車両上空から俯瞰した２次元のグリッドマップを表している。つまり、このグリッドマップでは、自車両の移動を考慮しつつ、道路の周囲の建物や車両等の障害物の位置が累積された点で可視化されているので、これらの点を避けるように自車両を制御すればよいことが分かる。

次に、３次元位置推定処理による処理結果を利用する処理について図１２を用いて説明する。図１２に示す車両制御処理では、自車両と障害物との衝突を回避する処理を示し、例えば、運転支援システム３１の電源が投入されると開始される。

車両制御処理では、まず、衝突を回避するシステムが起動しているか否かを判定する（Ｓ５１０）。システムが起動していれば（Ｓ５１０：ＹＥＳ）、走行路形状推定結果を取得する（Ｓ５２０）。この処理では、前述の３次元位置推定結果（の累積）を取得する。

続いて、３次元位置推定結果に基づいて、障害物を認識する（Ｓ５３０）。そして、自車両の進路上に障害物が存在するか否かを判定する（Ｓ５４０）。なお、自車両の進路は、前述の挙動情報や前述の処理にて得られた並進量および回転量から推定する。

自車両の進路上に障害物が存在していなければ（Ｓ５４０：ＮＯ）、Ｓ５１０の処理に戻る。また、自車両の進路上に障害物が存在していれば（Ｓ５４０：ＹＥＳ）、この障害物との衝突を回避するための車両制御を演算する（Ｓ５５０）。具体的には、操舵や制動で障害物との衝突を避けるための制御量を演算する。

続いて、障害物との衝突を回避可能であるか否かを判定する（Ｓ５６０）。衝突を回避可能であれば（Ｓ５６０：ＹＥＳ）、衝突を回避するための経路（制動を含む）を３次元位置推定結果に基づいて障害物が存在しない方向に設定する（Ｓ５７０）。

また、衝突を回避不可能であれば（Ｓ５６０：ＮＯ）、衝突を軽減するためのブレーキ圧を演算する（Ｓ５８０）。そして、演算されたブレーキ圧や衝突を回避するための経路への走行を実現するための車両制御量を演算し、この演算結果に基づく車両制御を実施する（Ｓ５９０）。

このような処理が終了すると、Ｓ５１０の処理に戻る。また、Ｓ５１０の処理にて、システムが起動していなければ（Ｓ５１０：ＮＯ）、車両制御処理を終了する。
［本実施形態による効果］
以上ように詳述した車両制御システム１において、演算部１０は、複数の撮像画像中において１または複数の特徴点をそれぞれ抽出し、該特徴点について複数の撮像画像間での対応付けを行う。そして、移動体の挙動に関わる挙動情報を取得し、挙動情報に基づいて、複数の撮像画像を撮像する間での移動体の移動量（並進量や回転量等）を表す移動体移動量を算出する。さらに、複数の撮像画像間での特徴点の移動量と移動体移動量とに基づいて実位置を推定する。

また、演算部１０は、複数の撮像画像のうちのある撮像画像に実位置を再投影した際の撮像画像上の位置を表す再投影位置とこの撮像画像において抽出された特徴点の位置を表す抽出位置との差異を検出し、該差異が小さくなるように移動体移動量を補正する。そして、演算部１０は、複数の撮像画像間での特徴点の移動量と補正後の移動体移動量とに基づいて実位置を推定する。

このような車両制御システム１によれば、再投影位置と抽出位置とに基づいて移動体移動量を補正し、補正後の移動体移動量を利用して特徴点の実空間での３次元位置（実位置）を推定するので、挙動情報を得るための高性能な構成を利用することなく、実位置の検出精度を向上させることができる。

また、車両制御システム１において、演算部１０は、移動体移動量を予め設定された基準量だけ仮に変更した仮移動量に対応する差異を演算し、差異が小さくなっていれば仮移動量を移動体移動量とする補正を行う。

このような車両制御システム１によれば、再投影位置と抽出位置との差異が小さくなる場合だけ移動体移動量を補正するので確実に実位置の検出精度を向上させることができる。

さらに、車両制御システム１において、演算部１０は、仮移動量に対応する差異を演算し、差異が小さくなっていれば仮移動量を移動体移動量とする補正を繰り返す。
このような車両制御システム１によれば、再投影位置と抽出位置との差異が小さくなるようにする処理を繰り返すので、再投影位置と抽出位置との差異を極力小さくすることができる。よって、より実位置の推定精度を向上させることができる。

また、車両制御システム１において、演算部１０は、挙動情報として得られる値の分解能よりも小さな値を、基準量として採用する。
このような車両制御システム１によれば、挙動情報として得られる値の分解能よりも高精度に移動体移動量を求めることができるので、実位置をより高精度に推定することができる。

さらに、車両制御システム１において、演算部１０は、移動体移動量を運動量成分に分解し、車両運動では変化しにくい運動量成分を除いた運動量成分について補正を行うことで移動体移動量を補正する。

このような車両制御システム１によれば、車両運動では変化しにくい運動量成分については補正せずに、この運動量成分を除いた運動量成分についてだけ補正することで、処理負荷を軽減しつつ、効果的に移動体移動量を補正することができる。

なお、車両運動では変化しにくい運動量成分とは、例えば、車両の真横方向への並進成分等が該当する。
また、車両制御システム１において、演算部１０は、等距離射影方式により撮像を行う撮像部から得られた撮像画像を利用する。

このような車両制御システム１によれば、魚眼レンズ等の広角レンズを利用してより広範囲が映った撮像画像を利用して処理を行うことができる。
さらに、車両制御システム１において、演算部１０は、障害物を検出し、運転支援システム３１は、障害物が検出されると該障害物と衝突しないよう自車両を制御する。

このような車両制御システム１によれば、障害物の位置を精度よく推定することができるので自車両の誤制御を抑制することができる。
［その他の実施形態］
本発明の実施の形態は、上記の実施形態に何ら限定されることはなく、本発明の技術的範囲に属する限り種々の形態を採りうる。

上記実施形態においては、記述した全ての構成が必須の構成ではなく、発明として成立する範囲内において、任意の構成を除外してもよい。また、記述した構成を任意の構成に置換することもできる。

例えば、上記最適化処理では、移動体移動量を運動量成分に分解し、車両運動では変化しにくい運動量成分を除いた運動量成分について補正を行ったが、変化しにくい運動量成分を除外することなく全方向の運動量成分を考慮して補正を行ってもよい。

また、上記実施形態では、フロントグリルなど車両進行方向を撮像する位置に設置したカメラに注目して、説明を行ったが、カメラの搭載位置はフロントに限定したものではなく、一般的な車両に多く普及しているリアカメラやドアミラー等に設置したカメラを用いてもよい。さらに、上記実施形態の広角カメラ２１は、等距離射影方式のものとして説明したが、等立体角射影方式等の他の射影方式であってもよい。

また、運転支援システム３１としては、障害物との衝突を抑制する構成を述べたが、この構成に限らず、例えば、静止物と移動物とを検出し、移動体（車両や歩行者など）に対して警告を行う構成や、走行可能領域の検知（静止立体物の存在しない領域の検出）と逸脱警報とを行う構成、或いは、駐車可能区画の検知（静止立体物の存在しない領域の検出）とこの領域への誘導を行う構成等としてもよい。

さらに、車両制御システム１は、車両に搭載されているものとして説明したが、車両に限らず、例えば、ロボットアーム等の移動体に備えられていてもよい。
［実施形態の構成と本発明の構成との対応］
上記実施形態における車両制御システム１は、本発明でいう車両制御装置に相当し、演算部１０は、本発明でいう３次元位置推定装置、および障害物検出手段に相当する。また、上記実施形態における運転支援システム３１制御手段に相当する。

さらに、上記実施形態におけるＳ１３０の処理は、本発明でいう特徴点対応付手段Ｓ１３０特徴点対応付工程に相当し、上記実施形態におけるＳ１５０の処理は、本発明でいう挙動情報取得手段、およびＳ１５０挙動情報取得工程に相当する。また、上記実施形態におけるＳ１６０の処理は、本発明でいう移動量算出手段、および移動量算出工程に相当し、上記実施形態におけるＳ１７０の処理は、本発明でいう実位置推定手段、および実位置推定工程に相当する。

さらに、上記実施形態におけるＳ２３０の処理は、本発明でいう移動量補正手段、および移動量補正工程に相当し、上記実施形態におけるＳ２４０の処理は、本発明でいう補正後実位置推定手段、および補正後実位置推定工程に相当する。

１…車両制御システム、１０…演算部、１１…ＣＰＵ、１２…メモリ、２１…広角カメラ、２２…移動検出部、２３…車速センサ、２４…ヨーレートセンサ、３１…運転支援システム。

Claims (8)

1. 移動体の移動に伴って移動する撮像部によって順次撮像された複数の撮像画像に基づいて撮像画像中に存在する特徴点の実空間での位置を表す実位置を推定する３次元位置推定装置（１０）であって、
   前記複数の撮像画像中において１または複数の特徴点をそれぞれ抽出し、該特徴点について前記複数の撮像画像間での対応付けを行う特徴点対応付手段（Ｓ１３０）と、
   前記移動体の挙動に関わる挙動情報を取得する挙動情報取得手段（Ｓ１５０）と、
   前記挙動情報に基づいて、前記複数の撮像画像を撮像する間での前記移動体の移動量を前記移動体の並進量および前記移動体の回転量で表す移動体移動量を算出する移動量算出手段（Ｓ１６０）と、
   前記複数の撮像画像間での前記撮像部が有するレンズの中心から前記特徴点に向かう光線ベクトルの変化量と前記移動体移動量とに基づいて前記実位置を推定する実位置推定手段（Ｓ１７０）と、
   前記複数の撮像画像のうちのある撮像画像に前記実位置を再投影した際の撮像画像上の位置を表す再投影位置とこの撮像画像において抽出された特徴点の位置を表す抽出位置との差異を検出し、該差異が小さくなるように前記移動体の並進量および前記移動体の回転量を補正することによって前記移動体移動量を補正する移動量補正手段（Ｓ２３０）と、
   前記複数の撮像画像間での前記撮像部が有するレンズの中心から前記特徴点に向かう光線ベクトルの変化量と補正後の移動体移動量とに基づいて前記実位置を推定する補正後実位置推定手段（Ｓ２４０）と、
   を備えたことを特徴とする３次元位置推定装置。
2. 請求項１に記載の３次元位置推定装置において、
   前記移動量補正手段は、前記移動体移動量を予め設定された基準量だけ仮に変更した仮移動量に対応する前記差異を演算し、前記差異が小さくなっていれば前記仮移動量を前記移動体移動量とする補正を行うこと
   を特徴とする３次元位置推定装置。
3. 請求項２に記載の３次元位置推定装置において、
   前記移動量補正手段は、前記仮移動量に対応する前記差異を演算し、前記差異が小さくなっていれば前記仮移動量を前記移動体移動量とする補正を繰り返すこと
   を特徴とする３次元位置推定装置。
4. 請求項２または請求項３に記載の３次元位置推定装置において、
   前記移動量補正手段は、前記挙動情報として得られる値の分解能よりも小さな値を、前記基準量として採用すること
   を特徴とする３次元位置推定装置。
5. 請求項１〜請求項４の何れか１項に記載の３次元位置推定装置において、
   当該３次元位置推定装置は車両に搭載されており、
   前記移動量補正手段は、前記移動体移動量を複数の運動量成分に分解し、該複数の運動量成分のうちの、車両の上下方向への並進量および前記撮像部の光軸回りの回転量を表す運動量成分を除いた運動量成分について補正を行うことで前記移動体移動量を補正すること
   を特徴とする３次元位置推定装置。
6. 請求項１〜請求項５の何れか１項に記載の３次元位置推定装置において、
   当該３次元位置推定装置は、等距離射影方式により撮像を行う撮像部から得られた撮像画像を利用すること
   を特徴とする３次元位置推定装置。
7. 車両（以下、「自車両」という。）に搭載され、自車両が周囲の障害物と衝突することを回避するための制御を行う車両制御装置（１）であって、
   前記障害物を検出する障害物検出手段（１０）と、
   前記障害物が検出されると該障害物と衝突しないよう自車両を制御する制御手段（３１）と、
   を備え、
   前記障害物検出手段として、請求項１〜請求項６の何れか１項に記載の３次元位置推定装置を備えたこと
   を特徴とする車両制御装置。
8. 移動体に搭載され、前記移動体の移動に伴って移動する撮像部によって順次撮像された複数の撮像画像に基づいて撮像画像中に存在する特徴点の実空間での位置を表す実位置を推定する３次元位置推定装置（１０）において前記実位置を推定する際に実施される３次元位置推定方法あって、
   前記複数の撮像画像中において１または複数の特徴点をそれぞれ抽出し、該特徴点について前記複数の撮像画像間での対応付けを行う特徴点対応付工程（Ｓ１３０）と、
   前記移動体の挙動に関わる挙動情報を取得する挙動情報取得工程（Ｓ１５０）と、
   前記挙動情報に基づいて、前記複数の撮像画像を撮像する間での前記移動体の移動量を前記移動体の並進量および前記移動体の回転量で表す移動体移動量を算出する移動量算出工程（Ｓ１６０）と、
   前記複数の撮像画像間での前記撮像部が有するレンズの中心から前記特徴点に向かう光線ベクトルの変化量と前記移動体移動量とに基づいて前記実位置を推定する実位置推定工程（Ｓ１７０）と、
   前記複数の撮像画像のうちのある撮像画像に前記実位置を再投影した際の撮像画像上の位置を表す再投影位置とこの撮像画像において抽出された特徴点の位置を表す抽出位置との差異を検出し、該差異が小さくなるように前記移動体の並進量および前記移動体の回転量を補正することによって前記移動体移動量を補正する移動量補正工程（Ｓ２３０）と、
   前記複数の撮像画像間での前記撮像部が有するレンズの中心から前記特徴点に向かう光線ベクトルの変化量と補正後の移動体移動量とに基づいて前記実位置を推定する補正後実位置推定工程（Ｓ２４０）と、
   を実施することを特徴とする３次元位置推定方法。