JP7005978B2 - 軌跡推定方法及び軌跡推定装置 - Google Patents
軌跡推定方法及び軌跡推定装置 Download PDFInfo
- Publication number
- JP7005978B2 JP7005978B2 JP2017139888A JP2017139888A JP7005978B2 JP 7005978 B2 JP7005978 B2 JP 7005978B2 JP 2017139888 A JP2017139888 A JP 2017139888A JP 2017139888 A JP2017139888 A JP 2017139888A JP 7005978 B2 JP7005978 B2 JP 7005978B2
- Authority
- JP
- Japan
- Prior art keywords
- vehicle
- dimensional
- time
- locus
- image
- 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.)
- Active
Links
Images
Landscapes
- Traffic Control Systems (AREA)
- Image Analysis (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Description
本発明は、軌跡推定方法及び軌跡推定装置に関する。
先行車の走行軌跡を算出する技術として、例えば特許文献１に記載の先行車走行軌跡算出装置が知られている。先行車走行軌跡算出装置は、自車両に対する先行車の位置を、レーダ又はレーザーセンサを用いて逐次的に検出して保存し、保存された先行車の位置を用いて先行車の走行軌跡を算出する。先行車走行軌跡算出装置は、車速及び横滑り角等を用いて、或いは、ＧＰＳ装置から得られる自車両の位置情報を用いて自車両の移動距離を算出し、ヨーレートを用いてヨー角変化を算出する。先行車走行軌跡算出装置は、自車両の移動距離とヨー角変化を用いて、先行車の過去の位置の座標値を自車両の移動後の座標系における座標値へ変換する。
しかしながら、自車両の移動距離とヨーレートのみを用いて自車両の運動を先行車の走行軌跡に反映させると、２次元平面上の運動しか推定することができない。このため対象物の３次元走行軌跡を得ることができない。
本発明は、対象物の上下動を含んだ軌跡を推定することを目的とする。
本発明は、対象物の上下動を含んだ軌跡を推定することを目的とする。
本発明の一態様に係る軌跡推定方法は、自車両に搭載された画像センサにより第１時刻及び第１時刻より後の第２時刻において自車両周囲の画像を取得し、第１時刻の画像中の特徴点と、第１時刻の特徴点に対応する第２時刻の画像中の特徴点とを抽出し、第１時刻の画像中の特徴点が表す３次元空間中の３次元座標と、第２時刻の画像中の特徴点が表す３次元空間中の３次元座標とを算出し、画像センサの３軸並進運動及び３軸回転運動を検出し、検出した３軸並進運動及び３軸回転運動に基づき、第１時刻の画像中の特徴点が表す３次元座標と、第１時刻の特徴点に対応する第２時刻の画像中の特徴点が表す３次元座標と、を接続する３次元軌跡を算出し、複数の前記３次元軌跡を接続する。
本発明の一形態によれば、対象物の３次元軌跡を推定できる。
以下、本発明の実施形態について、図面を参照しつつ説明する。
（第１実施形態）
（構成）
（第１実施形態）
（構成）
以下において、図面を参照して本発明の実施形態を説明する。なお、以下に示す本発明の実施形態は、本発明の技術的思想を具体化するための装置や方法を例示するものであって、本発明の技術的思想は、構成部品の構成、配置等を下記のものに特定するものではない。本発明の技術的思想は、特許請求の範囲に記載された請求項が規定する技術的範囲内において、種々の変更を加えることができる。
（構成）
図１を参照する。運転支援装置１は、運転支援装置１を搭載する車両（以下、「自車両」と表記する）の周囲の走行環境に基づいて、自車両を自動的に操舵したり停止させる走行支援制御と、運転者が関与せずに自車両を自動で運転する自動運転制御を行う。
運転支援装置１は、軌跡推定装置１０と、車両挙動センサ群２０と、車両走行コントローラ３０と、車両制御アクチュエータ群３１を備える。
図１を参照する。運転支援装置１は、運転支援装置１を搭載する車両（以下、「自車両」と表記する）の周囲の走行環境に基づいて、自車両を自動的に操舵したり停止させる走行支援制御と、運転者が関与せずに自車両を自動で運転する自動運転制御を行う。
運転支援装置１は、軌跡推定装置１０と、車両挙動センサ群２０と、車両走行コントローラ３０と、車両制御アクチュエータ群３１を備える。
軌跡推定装置１０は、自車両周囲の画像から移動物体を検出する。軌跡推定装置１０は、移動物体毎に移動物体の複数の点の３次元座標の移動軌跡を推定する。例えば、軌跡推定装置１０は、自車両前方の先行車や対向車、歩行者などの複数の点の３次元座標の移動軌跡を推定してよい。以下の説明において、３次元座標の移動軌跡を「３次元軌跡」と表記する。尚、本実施形態における３次元座標の原点は、任意に設定されているものであればよく、自車両を原点としてもよく、しなくともよい。自車両を原点とする場合は、自車両の重心位置や、画像センサを取り付けた位置を原点としてもよい。
軌跡推定装置１０は、周囲環境センサ群１１と、コントローラ１２と、ユーザインタフェース装置１３を備える。なお、図１、図３及び図１２においてユーザインタフェース装置を「ユーザＩ／Ｆ装置」と表記する。
周囲環境センサ群１１は、自車両の周囲環境、例えば自車両の周囲の物体を検出するセンサ群である。
周囲環境センサ群１１は、自車両の周囲環境、例えば自車両の周囲の物体を検出するセンサ群である。
例えば周囲環境センサ群１１は、ステレオカメラ５０と、レーダ５１（例えば、レーザレーダ）を備える。
ステレオカメラ５０は、自車両周囲のステレオ画像を自車両の周囲環境の情報として生成する。ステレオカメラ５０は、ステレオカメラ５０の撮影方向と直交する方向に沿って互いの視野が重なるように配置された第１画像センサ５２と、第２画像センサ５３を備える。例えば、第１画像センサ５２はステレオカメラ５０の撮影方向を向いたときに左側に配置され、第２画像センサ５３は右側に配置されてよい。このステレオカメラ５０によって、自車両周囲に存在する物体の有無、自車両と物体との相対位置及び相対距離を、自車両の周囲環境として検出する。また、このステレオカメラ５０では、物体の相対位置や相対距離を連続的に検出し、加えて、自車両の挙動（移動距離、移動速度等）を用いることで、物体が移動物体であるか否かを検出することができる。
ステレオカメラ５０は、自車両周囲のステレオ画像を自車両の周囲環境の情報として生成する。ステレオカメラ５０は、ステレオカメラ５０の撮影方向と直交する方向に沿って互いの視野が重なるように配置された第１画像センサ５２と、第２画像センサ５３を備える。例えば、第１画像センサ５２はステレオカメラ５０の撮影方向を向いたときに左側に配置され、第２画像センサ５３は右側に配置されてよい。このステレオカメラ５０によって、自車両周囲に存在する物体の有無、自車両と物体との相対位置及び相対距離を、自車両の周囲環境として検出する。また、このステレオカメラ５０では、物体の相対位置や相対距離を連続的に検出し、加えて、自車両の挙動（移動距離、移動速度等）を用いることで、物体が移動物体であるか否かを検出することができる。
レーダ５１は、自車両周囲に存在する物体の有無、自車両と物体との相対位置及び相対距離を、自車両の周囲環境として検出する。
周囲環境センサ群１１は、検出した周囲環境の情報である周囲環境情報をコントローラ１２と車両走行コントローラ３０へ出力する。
周囲環境センサ群１１は、検出した周囲環境の情報である周囲環境情報をコントローラ１２と車両走行コントローラ３０へ出力する。
コントローラ１２は、ステレオカメラ５０が生成したステレオ画像に基づいて移動物体の３次元軌跡を推定する処理を実行する電子制御ユニットである。コントローラ１２は、プロセッサ５５と、記憶装置５６等の周辺部品を含む。プロセッサ５５は、例えばＣＰＵ（Central Processing Unit）、やＭＰＵ（Micro-Processing Unit）であってよい。
なお、汎用の半導体集積回路中に設定される機能的な論理回路でコントローラ１２を実現してもよい。例えば、コントローラ１２はフィールド・プログラマブル・ゲート・アレイ（ＦＰＧＡ：Field-Programmable Gate Array）等のプログラマブル・ロジック・デバイス（ＰＬＤ：Programmable Logic Device）等を有していてもよい。
なお、汎用の半導体集積回路中に設定される機能的な論理回路でコントローラ１２を実現してもよい。例えば、コントローラ１２はフィールド・プログラマブル・ゲート・アレイ（ＦＰＧＡ：Field-Programmable Gate Array）等のプログラマブル・ロジック・デバイス（ＰＬＤ：Programmable Logic Device）等を有していてもよい。
記憶装置５６は、半導体記憶装置、磁気記憶装置及び光学記憶装置のいずれかを備えてよい。記憶装置５６は、レジスタ、キャッシュメモリ、主記憶装置として使用されるＲＯＭ（Read Only Memory）及びＲＡＭ（Random Access Memory）等のメモリを含んでよい。
記憶装置５６には、プロセッサ５５上で実行されて、移動物体の３次元軌跡を推定する処理をコントローラ１２に実行させるためのコンピュータプログラムが記憶される。
記憶装置５６には、プロセッサ５５上で実行されて、移動物体の３次元軌跡を推定する処理をコントローラ１２に実行させるためのコンピュータプログラムが記憶される。
コントローラ１２は、ステレオカメラ５０が生成したステレオ画像から抽出された特徴点の３次元軌跡を推定し、３次元軌跡が類似する特徴点をグループ化して移動物体を検出する。
図２を参照する。コントローラ１２は、現時点ｔ０のステレオ画像から自車両２の周囲の物体３の特徴点４ａ０、４ｂ０、４ｃ０及び４ｄ０を抽出する。
また、点４ａ１、４ｂ１、４ｃ１及び４ｄ１は、現時点ｔ０より１サイクル前の時刻ｔ－１に撮像されたステレオ画像から抽出した物体３の特徴点である。１サイクルは、所定の長さの期間であり、例えばコントローラ１２による移動物体の軌跡推定処理の処理周期であってよい。
図２を参照する。コントローラ１２は、現時点ｔ０のステレオ画像から自車両２の周囲の物体３の特徴点４ａ０、４ｂ０、４ｃ０及び４ｄ０を抽出する。
また、点４ａ１、４ｂ１、４ｃ１及び４ｄ１は、現時点ｔ０より１サイクル前の時刻ｔ－１に撮像されたステレオ画像から抽出した物体３の特徴点である。１サイクルは、所定の長さの期間であり、例えばコントローラ１２による移動物体の軌跡推定処理の処理周期であってよい。
点４ａ２、４ｂ２、４ｃ２及び４ｄ２は、現時点ｔ０より２サイクル前の時刻ｔ－２に撮像されたステレオ画像から抽出した物体３の特徴点である。
点４ａ３、４ｂ３、４ｃ３及び４ｄ３は、現時点ｔ０より３サイクル前の時刻ｔ－３に撮像されたステレオ画像から抽出した物体３の特徴点である。
点４ａ４、４ｂ４、４ｃ４及び４ｄ４は、現時点ｔ０より４サイクル前の時刻ｔ－４に撮像されたステレオ画像から抽出した物体３の特徴点である。
点４ａ３、４ｂ３、４ｃ３及び４ｄ３は、現時点ｔ０より３サイクル前の時刻ｔ－３に撮像されたステレオ画像から抽出した物体３の特徴点である。
点４ａ４、４ｂ４、４ｃ４及び４ｄ４は、現時点ｔ０より４サイクル前の時刻ｔ－４に撮像されたステレオ画像から抽出した物体３の特徴点である。
点４ａ５、４ｂ５、４ｃ５及び４ｄ５は、現時点ｔ０より５サイクル前の時刻ｔ－５に撮像されたステレオ画像から抽出した物体３の特徴点である。
点４ａ６、４ｂ６、４ｃ６及び４ｄ６は、現時点ｔ０より６サイクル前の時刻ｔ－６に撮像されたステレオ画像から抽出した物体３の特徴点である。
例えばコントローラ１２は、点４ａ０～点４ａ６を接続して特徴点４ａ０の３次元軌跡５ａを推定する。
点４ａ６、４ｂ６、４ｃ６及び４ｄ６は、現時点ｔ０より６サイクル前の時刻ｔ－６に撮像されたステレオ画像から抽出した物体３の特徴点である。
例えばコントローラ１２は、点４ａ０～点４ａ６を接続して特徴点４ａ０の３次元軌跡５ａを推定する。
また、コントローラ１２は、点４ｂ０～点４ｂ６を接続して特徴点４ｂ０の３次元軌跡５ｂを推定する。
また、コントローラ１２は、点４ｃ０～点４ｃ６を接続して特徴点４ｃ０の３次元軌跡５ｃを推定する。
また、コントローラ１２は、点４ｄ０～点４ｄ６を接続して特徴点４ｄ０の３次元軌跡５ｄを推定する。
また、コントローラ１２は、点４ｃ０～点４ｃ６を接続して特徴点４ｃ０の３次元軌跡５ｃを推定する。
また、コントローラ１２は、点４ｄ０～点４ｄ６を接続して特徴点４ｄ０の３次元軌跡５ｄを推定する。
コントローラ１２は、移動物体３の複数の点に特徴点４ａ０～４ｄ０を当てはめることにより移動物体３の複数の点の３次元軌跡を推定する。コントローラ１２は、これらの点の３次元軌跡に基づいて移動物体の３次元運動を推定する。例えばコントローラ１２は、自車両周囲の画像の先行車の位置の特徴点を抽出し、この先行車の運動として３次元軌跡を算出してよい。特に、コントローラ１２は上下方向の変位を含んだ３次元運動を推定することができる。
例えば、コントローラ１２は、自車両周囲の移動物体の３次元運動として移動物体の加減速度やヨーレート、ピッチ軸回転角速度を推定してよい。
コントローラ１２は、先行車や対向車などの自車両周囲の移動物体のヨー軸回転を検出することにより、移動物体の旋回などの３次元運動を推定してよい。
また例えば、コントローラ１２は、先行車などの自車両周囲の移動物体のピッチ軸回転を検出することにより、移動物体の登坂又は降坂などの３次元運動として推定してもよい。
コントローラ１２は、先行車や対向車などの自車両周囲の移動物体のヨー軸回転を検出することにより、移動物体の旋回などの３次元運動を推定してよい。
また例えば、コントローラ１２は、先行車などの自車両周囲の移動物体のピッチ軸回転を検出することにより、移動物体の登坂又は降坂などの３次元運動として推定してもよい。
またコントローラ１２は、ブレーキングによる先行車などの自車両周囲の移動物体のピッチ軸回転を検出することにより、移動物体の制動動作などの３次元運動を推定してもよい。
またコントローラ１２は、先行車などの自車両周囲の移動物体の運動（例えば移動物体の上下動）を検出することにより、路面凹凸やスピードバンプなどの段差に起因する先行車の上下動などの３次元運動を推定してもよい。
またコントローラ１２は、先行車などの自車両周囲の移動物体の運動（例えば移動物体の上下動）を検出することにより、路面凹凸やスピードバンプなどの段差に起因する先行車の上下動などの３次元運動を推定してもよい。
さらに、コントローラ１２は、自車両の周囲の移動物体の３次元運動に基づいて自車両の周囲状況を推定する。
例えば、コントローラ１２は、先行車や対向車などの自車両周囲の移動物体の旋回運動に基づいて自車両前方にカーブ路などの道路形状を推定してよい。
例えば、コントローラ１２は、先行車や対向車などの自車両周囲の移動物体の旋回運動に基づいて自車両前方にカーブ路などの道路形状を推定してよい。
また例えば、コントローラ１２は、先行車などの自車両周囲の移動物体の登坂又は降坂に基づいて自車両前方の走行路の勾配変化を推定してよい。
また例えば、コントローラ１２は、先行車などの自車両周囲の移動物体の運動（例えば移動物体の上下動）に基づいて自車両前方の路面凹凸やスピードバンプなどの路面状況を推定してよい。
また例えば、コントローラ１２は、先行車などの自車両周囲の移動物体の制動動作に基づいて自車両前方の交通状況を推定してもよい。
また例えば、コントローラ１２は、先行車などの自車両周囲の移動物体の運動（例えば移動物体の上下動）に基づいて自車両前方の路面凹凸やスピードバンプなどの路面状況を推定してよい。
また例えば、コントローラ１２は、先行車などの自車両周囲の移動物体の制動動作に基づいて自車両前方の交通状況を推定してもよい。
このように、物体の複数の３次元座標の軌跡を推定することで、物体の向きや角速度を精度良く求めることができる。このため、レーダ等で物体の１点のみを追跡した場合には、対象の微妙な動きや向きの変化を捉えることができなかったり、動きの検出に時間を要していたのに比べて、本発明では、物体の姿勢や微小な運動を精度よくかつ迅速に検出することが可能となる。
図１を参照する。コントローラ１２は、推定した移動物体の３次元軌跡の情報（以下、「３次元軌跡情報」と表記することがある）、移動物体の運動の情報（以下、「移動物体運動情報」と表記することがある）、及び自車両の周囲状況の情報（以下、「周囲状況情報」と表記することがある）を、ユーザインタフェース装置１３へ出力する。
ユーザインタフェース装置１３は、コントローラ１２から受信した３次元軌跡情報、移動物体運動情報、周囲状況情報を視覚的に運転者に呈示する視覚信号出力装置や、聴覚的に呈示する発音装置を備えてよい。
ユーザインタフェース装置１３は、コントローラ１２から受信した３次元軌跡情報、移動物体運動情報、周囲状況情報を視覚的に運転者に呈示する視覚信号出力装置や、聴覚的に呈示する発音装置を備えてよい。
例えば、ユーザインタフェース装置１３は、移動物体の３次元軌跡を第１画像センサ５２の撮像画像に重畳して、ナビ画面等のディスプレイ装置やヘッドアップディスプレイ等に表示してもよい。
また、例えばユーザインタフェース装置１３は、周囲状況情報に基づいて「前方にカーブがあります」、「前方に路面凹凸があります」などの周囲状況を知らせる音声信号や視覚的なメッセージを運転者に呈示してもよい。
また、例えばユーザインタフェース装置１３は、運転席のシートを振動させるバイブレータを備えていてもよい。ユーザインタフェース装置１３は、周囲状況情報に基づいて、運転席のシートを振動させて周囲状況を運転者に知らせ、運転者の注意を促してもよい。例えば、例えば前方に路面凹凸やスピードバンプがある場合に運転席のシートを振動させてよい。
また、例えばユーザインタフェース装置１３は、周囲状況情報に基づいて「前方にカーブがあります」、「前方に路面凹凸があります」などの周囲状況を知らせる音声信号や視覚的なメッセージを運転者に呈示してもよい。
また、例えばユーザインタフェース装置１３は、運転席のシートを振動させるバイブレータを備えていてもよい。ユーザインタフェース装置１３は、周囲状況情報に基づいて、運転席のシートを振動させて周囲状況を運転者に知らせ、運転者の注意を促してもよい。例えば、例えば前方に路面凹凸やスピードバンプがある場合に運転席のシートを振動させてよい。
車両挙動センサ群２０は、自車両の車両挙動を検出するセンサ群であり、車速センサ２１と、加速度センサ２２と、ジャイロセンサ２３と、操舵角センサ２４を備える。
車速センサ２１は、自車両の車輪速を検出し、車輪速に基づいて自車両の速度を算出する。加速度センサ２２は、自車両の前後方向の加速度、車幅方向の加速度、上下方向の加速度を検出する。すなわち加速度センサ２２は、自車両の３軸並進運動方向（すなわちステレオカメラ５０の３軸並進運動方向）の加速度を検出する。
車速センサ２１は、自車両の車輪速を検出し、車輪速に基づいて自車両の速度を算出する。加速度センサ２２は、自車両の前後方向の加速度、車幅方向の加速度、上下方向の加速度を検出する。すなわち加速度センサ２２は、自車両の３軸並進運動方向（すなわちステレオカメラ５０の３軸並進運動方向）の加速度を検出する。
ジャイロセンサ２３は、ロール軸、ピッチ軸及びヨー軸を含む３軸回りの自車両の回転角度の角速度を検出する。すなわち、ジャイロセンサ２３は、自車両の３軸回転運動（すなわちステレオカメラ５０の３軸回転運動）の角速度を検出する。
操舵角センサ２４は、操舵操作子であるステアリングホイールの現在の回転角度（操舵操作量）である現在操舵角を検出する。
これら車速、加速度、回転角速度、及び操舵角の情報を、総称して「車両挙動情報」と表記することがある。車両挙動センサ群２０は、車両挙動情報を車両走行コントローラ３０へ出力する。
操舵角センサ２４は、操舵操作子であるステアリングホイールの現在の回転角度（操舵操作量）である現在操舵角を検出する。
これら車速、加速度、回転角速度、及び操舵角の情報を、総称して「車両挙動情報」と表記することがある。車両挙動センサ群２０は、車両挙動情報を車両走行コントローラ３０へ出力する。
車両走行コントローラ３０は、自車両の自動運転制御又は運転支援制御を行う電子制御ユニットである。車両走行コントローラ３０は、プロセッサと、記憶装置等の周辺部品とを含む。車両走行コントローラ３０のプロセッサは例えばＣＰＵやＭＰＵであってよい。
なお、汎用の半導体集積回路中に設定される機能的な論理回路で車両走行コントローラ３０を実現してもよい。例えば、車両走行コントローラ３０は、フィールド・プログラマブル・ゲート・アレイ等のプログラマブル・ロジック・デバイス等を有していてもよい。
車両走行コントローラ３０の記憶装置は、記憶媒体として半導体記憶装置、磁気記憶装置及び光学記憶装置のいずれかを備えてよい。車両走行コントローラ３０の記憶装置は、レジスタ、キャッシュメモリ、主記憶装置として使用されるＲＯＭ及びＲＡＭ等のメモリを含んでよい。
なお、汎用の半導体集積回路中に設定される機能的な論理回路で車両走行コントローラ３０を実現してもよい。例えば、車両走行コントローラ３０は、フィールド・プログラマブル・ゲート・アレイ等のプログラマブル・ロジック・デバイス等を有していてもよい。
車両走行コントローラ３０の記憶装置は、記憶媒体として半導体記憶装置、磁気記憶装置及び光学記憶装置のいずれかを備えてよい。車両走行コントローラ３０の記憶装置は、レジスタ、キャッシュメモリ、主記憶装置として使用されるＲＯＭ及びＲＡＭ等のメモリを含んでよい。
車両走行コントローラ３０は、周囲環境センサ群１１から出力された周囲環境情報と、車両挙動センサ群２０から出力された車両挙動情報とに基づいて車両制御アクチュエータ群３１を駆動することにより、自車両の走行支援制御又は自動運転制御を実行する。
尚、本実施形態においては、ステアリングアクチュエータ４１、アクセル開度アクチュエータ４２は、ブレーキ制御アクチュエータ４３、のうち少なくとも一つが自動で制御されている状態を自動運転とし、これらすべてのアクチュエータが手動で制御されている状態を手動運転とする。また、本実施形態においては、自動運転及び手動運転の両方にも適用することが可能である。
車両制御アクチュエータ群３１は、車両走行コントローラ３０からの制御信号に応じて、自車両のステアリングホイール、アクセル開度、ブレーキ装置、トランスミッションのギアを操作して、自車両の走行を制御する。車両制御アクチュエータ群３１は、ステアリングアクチュエータ３２と、アクセル開度アクチュエータ３３と、ブレーキ制御アクチュエータ３４と、シフトアクチュエータ３５を備える。
尚、本実施形態においては、ステアリングアクチュエータ４１、アクセル開度アクチュエータ４２は、ブレーキ制御アクチュエータ４３、のうち少なくとも一つが自動で制御されている状態を自動運転とし、これらすべてのアクチュエータが手動で制御されている状態を手動運転とする。また、本実施形態においては、自動運転及び手動運転の両方にも適用することが可能である。
車両制御アクチュエータ群３１は、車両走行コントローラ３０からの制御信号に応じて、自車両のステアリングホイール、アクセル開度、ブレーキ装置、トランスミッションのギアを操作して、自車両の走行を制御する。車両制御アクチュエータ群３１は、ステアリングアクチュエータ３２と、アクセル開度アクチュエータ３３と、ブレーキ制御アクチュエータ３４と、シフトアクチュエータ３５を備える。
ステアリングアクチュエータ３２は、自車両のステアリングの操舵方向及び操舵量を制御する。アクセル開度アクチュエータ３３は、自車両のアクセル開度を制御する。ブレーキ制御アクチュエータ３４は、自車両のブレーキ装置の制動動作を制御する。シフトアクチュエータ３５は、自車両のトランスミッションのギアポジションを制御する。
コントローラ１２は、３次元軌跡情報、移動物体運動情報、及び周囲状況情報を車両走行コントローラ３０へ出力する。
車両走行コントローラ３０は、３次元軌跡情報、移動物体運動情報、及び周囲状況情報に基づいて、自車両の走行支援制御と自動運転制御を行う。
例えば、車両走行コントローラ３０は、移動物体運動情報に含まれる先行車の加減速やヨーレートに基づいて、自車の目標加減速度及び目標操舵角を決定し、車両制御アクチュエータ群３１を制御して、先行車の軌跡を追従する追従走行を行ってもよい。
車両走行コントローラ３０は、３次元軌跡情報、移動物体運動情報、及び周囲状況情報に基づいて、自車両の走行支援制御と自動運転制御を行う。
例えば、車両走行コントローラ３０は、移動物体運動情報に含まれる先行車の加減速やヨーレートに基づいて、自車の目標加減速度及び目標操舵角を決定し、車両制御アクチュエータ群３１を制御して、先行車の軌跡を追従する追従走行を行ってもよい。
例えば、車両走行コントローラ３０は、先行車が制動動作を行ったと推定されたとき、車両制御アクチュエータ群３１を制御して自動ブレーキをかけてもよい。
また車両走行コントローラ３０は、３次元軌跡情報に基づいて対向車が自車両の方向に向かう兆候が推定されたとき、対向車を回避する自動操舵や対向車との衝突を回避するために自動ブレーキをかけてもよい。
また車両走行コントローラ３０は、３次元軌跡情報に基づいて対向車が自車両の方向に向かう兆候が推定されたとき、対向車を回避する自動操舵や対向車との衝突を回避するために自動ブレーキをかけてもよい。
また車両走行コントローラ３０は、周囲状況情報に基づいて自車両前方の勾配変化が推定されたときに、車両制御アクチュエータ群３１を制御してアクセル開度制御、ギア制御、ブレーキ制御を行い、勾配変化に起因する自車両の加減速度変化を抑制してもよい。
例えば、上り勾配が増加すると推定された場合、車両走行コントローラ３０は、アクセル開度を増加させギアのシフトポジションを低速側に変更させてよい。上り勾配が減少すると推定された場合、車両走行コントローラ３０は、アクセル開度を減少させギアのシフトポジションを高速側に変更させてよい
例えば、上り勾配が増加すると推定された場合、車両走行コントローラ３０は、アクセル開度を増加させギアのシフトポジションを低速側に変更させてよい。上り勾配が減少すると推定された場合、車両走行コントローラ３０は、アクセル開度を減少させギアのシフトポジションを高速側に変更させてよい
下り勾配が増加すると推定された場合、車両走行コントローラ３０は、アクセル開度を減少させ、ブレーキ量を増加させ、ギアのシフトポジションを高速側に変更させてよい。下り勾配が減少すると推定された場合、車両走行コントローラ３０は、ブレーキ量を減少させてよい。
また車両走行コントローラ３０は、周囲状況情報に基づいて自車両前方の路面凹凸が推定されたときに、車両制御アクチュエータ群３１を制御してアクセル開度制御、ブレーキ制御を行うことにより、自車両の速度を下げ凹凸による乗員の上下動を抑制してもよい。
また車両走行コントローラ３０は、周囲状況情報に基づいて自車両前方の路面凹凸が推定されたときに、車両制御アクチュエータ群３１を制御してアクセル開度制御、ブレーキ制御を行うことにより、自車両の速度を下げ凹凸による乗員の上下動を抑制してもよい。
さらに、運転支援装置１は、シートコントローラ４０と、シートアクチュエータ４１と、反力コントローラ４２と、反力アクチュエータ４３と、サスペンションコントローラ４４と、アブソーバアクチュエータ４５を備える。
シートコントローラ４０は、乗員席シートの位置又は姿勢を変更する電子制御ユニットである。シートコントローラ４０は、シートアクチュエータ４１を駆動することにより乗員席シートの位置又は姿勢を変更する。シートコントローラ４０は、例えば乗員によるシートスイッチの操作に応じて乗員席シートの位置又は姿勢を変更する。
シートコントローラ４０は、乗員席シートの位置又は姿勢を変更する電子制御ユニットである。シートコントローラ４０は、シートアクチュエータ４１を駆動することにより乗員席シートの位置又は姿勢を変更する。シートコントローラ４０は、例えば乗員によるシートスイッチの操作に応じて乗員席シートの位置又は姿勢を変更する。
コントローラ１２は、自車両の周囲状況に基づいて運転席シートの位置又は姿勢を変更させる第１指示信号をシートコントローラ４０に出力してもよい。シートコントローラ４０は、コントローラ１２からの第１指示信号にもとづいて運転席シートの位置又は姿勢を変更する。
例えばコントローラ１２は、自動運転制御実行時に前方に路面凹凸やスピードバンプがあると推定された場合に、手動運転への切り替えに備えて運転席シートの背もたれの後方へのリクライニング量を減らしたり、座席位置の後退量を低減させてよい。
例えばコントローラ１２は、自動運転制御実行時に前方に路面凹凸やスピードバンプがあると推定された場合に、手動運転への切り替えに備えて運転席シートの背もたれの後方へのリクライニング量を減らしたり、座席位置の後退量を低減させてよい。
反力コントローラ４２は、ステアリングホイールへ付与する操舵反力を制御する電子制御ユニットである。反力コントローラ４２は、反力アクチュエータ４３を駆動することにより操舵反力を制御する。反力コントローラ４２は、例えばステアリングホイールの操舵角と車速などに基づいて操舵反力を制御する。
コントローラ１２は、自車両の周囲の路面状況に基づいて操舵反力を変更させる第２指示信号を反力コントローラ４２に出力する。反力コントローラ４２はコントローラ１２からの第２指示信号に基づいてステアリングホイールへ付与する操舵反力を制御する。
例えばコントローラ１２は、自車両前方の路面状況、例えば路面凹凸やスピードバンプの存在、路面凹凸の大きさ又は多さに基づいて操舵反力を制御してよい。例えば、路面凹凸やスピードバンプがあったり、路面凹凸が大きかったり、多い場合に、これらの凹凸やバンプによりハンドルが取られないように操舵反力を大きくしてもよい。
例えばコントローラ１２は、自車両前方の路面状況、例えば路面凹凸やスピードバンプの存在、路面凹凸の大きさ又は多さに基づいて操舵反力を制御してよい。例えば、路面凹凸やスピードバンプがあったり、路面凹凸が大きかったり、多い場合に、これらの凹凸やバンプによりハンドルが取られないように操舵反力を大きくしてもよい。
サスペンションコントローラ４４は、自車両のサスペンションのショックアブソーバの減衰力を制御する電子制御ユニットである。サスペンションコントローラ４４は、アブソーバアクチュエータ４５を駆動することにより、ショックアブソーバの減衰力を変更する。例えばサスペンションコントローラ４４は、車両挙動センサ群２０から入力される車両挙動情報に基づいてショックアブソーバの減衰力を変更する。
コントローラ１２は、自車両の周囲状況に基づいてショックアブソーバの減衰力を変更させる第３指示信号をサスペンションコントローラ４４に出力する。サスペンションコントローラ４４は、コントローラ１２からの第３指示信号に基づいてショックアブソーバの減衰力を変更する。
例えばコントローラ１２は、周囲状況情報に基づいて自車両前方の路面凹凸が推定されたときに、ショックアブソーバの減衰力を低減させることにより凹凸による乗員の上下動を抑制してもよい。
例えばコントローラ１２は、周囲状況情報に基づいて自車両前方の路面凹凸が推定されたときに、ショックアブソーバの減衰力を低減させることにより凹凸による乗員の上下動を抑制してもよい。
次にコントローラ１２の機能構成を説明する。図３を参照する。コントローラ１２は、画像補正部６０及び６１と、特徴点抽出部６２及び６３と、３次元軌跡記憶部６４と、３次元座標算出部６５と、オプティカルフロー算出部６６と、センサ運動検出部６７と、３次元軌跡算出部６８と、軌跡接続部７０と、運動推定部７１を備える。３次元軌跡算出部６８は、軌跡変換部６９を備える。
画像補正部６０及び６１と、特徴点抽出部６２及び６３と、３次元座標算出部６５と、オプティカルフロー算出部６６と、センサ運動検出部６７と、３次元軌跡算出部６８と、軌跡変換部６９と、軌跡接続部７０と、運動推定部７１の機能は、コントローラ１２のプロセッサ５５が、記憶装置５６に格納されたコンピュータプログラムを実行することによって実現されてよい。
画像補正部６０及び６１と、特徴点抽出部６２及び６３と、３次元座標算出部６５と、オプティカルフロー算出部６６と、センサ運動検出部６７と、３次元軌跡算出部６８と、軌跡変換部６９と、軌跡接続部７０と、運動推定部７１の機能は、コントローラ１２のプロセッサ５５が、記憶装置５６に格納されたコンピュータプログラムを実行することによって実現されてよい。
ステレオカメラ５０を構成する第１画像センサ５２と第２画像センサ５３は、自車両の周囲のステレオ画像を所定周期で連続して撮影する。以下、第１画像センサ５２の撮像画像を「第１画像」と表記し、第２画像センサ５３の撮像画像を「第２画像」と表記することがある。
画像補正部６０には第１画像が連続して入力され、画像補正部６１には第２画像が連続して入力される。
画像補正部６０には第１画像が連続して入力され、画像補正部６１には第２画像が連続して入力される。
画像補正部６０及び６１は、それぞれ第１画像及び第２画像の補正処理を行う。例えば画像補正部６０及び６１は、第１画像及び第２画像のレンズ歪みを補正してよい。画像補正部６０及び６１は、第１画像センサ５２及び第２画像センサ５３の視野にある空間の同一点の像が、第１画像及び第２画像の同一走査線上に位置するように変換する平行化処理（例えば、第１画像及び第２画像のエピポーラ線の平行化処理）を行ってもよい。
特徴点抽出部６２及び６３は、第１画像及び第２画像それぞれにおいて、他の画素と区別可能な特徴を持つ画素である特徴点を抽出する。特徴点の抽出には、例えば非特許文献「Jianbo Shi and Carlo Tomasi, "Good Features to Track," 1994 IEEE Conference on Computer Vision and Pattern Recognition (CVPR'94), 1994, pp. 593 - 600.」に記載の手法を用いてよい。
３次元軌跡記憶部６４には、特徴点の軌跡である３次元軌跡が記憶される。図４は、現時点ｔ０より１サイクル前の時刻ｔ－１の後から現時点ｔ０より前の期間において３次元軌跡記憶部６４に記憶されている３次元軌跡の例を示す。
３次元軌跡５ａ及び５ｂが３次元軌跡記憶部６４に記憶されている場合を想定する。
３次元軌跡５ａは、時刻ｔ－１以前の複数の時刻ｔ－１～時刻ｔ－６のステレオ画像からそれぞれ抽出された特徴点４ａ１～４ａ６の軌跡であり、３次元軌跡５ｂは、時刻ｔ－１～時刻ｔ－６のステレオ画像からそれぞれ抽出された特徴点４ｂ１～４ｂ６の軌跡である。
３次元軌跡５ａ及び５ｂが３次元軌跡記憶部６４に記憶されている場合を想定する。
３次元軌跡５ａは、時刻ｔ－１以前の複数の時刻ｔ－１～時刻ｔ－６のステレオ画像からそれぞれ抽出された特徴点４ａ１～４ａ６の軌跡であり、３次元軌跡５ｂは、時刻ｔ－１～時刻ｔ－６のステレオ画像からそれぞれ抽出された特徴点４ｂ１～４ｂ６の軌跡である。
現時点ｔ０は第２時刻の一例であり、１サイクル前の時刻ｔ－１は第１時刻の一例であり、時刻ｔ－１～時刻ｔ－６は第１時刻以前の時刻の一例である。
なお、一点鎖線２は、３次元軌跡５ａ及び５ｂに対する自車両の相対位置を示す。時刻ｔ－１の後から現時点ｔ０より前の期間に３次元軌跡記憶部６４に記憶されている３次元軌跡５ａ及び５ｂの位置は、時刻ｔ－１における自車両２の位置に対する相対位置である。
なお、一点鎖線２は、３次元軌跡５ａ及び５ｂに対する自車両の相対位置を示す。時刻ｔ－１の後から現時点ｔ０より前の期間に３次元軌跡記憶部６４に記憶されている３次元軌跡５ａ及び５ｂの位置は、時刻ｔ－１における自車両２の位置に対する相対位置である。
図３を参照する。３次元座標算出部６５は、互いに対応する第１画像の特徴点と第２画像の特徴点を探索するステレオ視差算出処理を行う。第１画像及び第２画像には、画像補正部６０及び６１による平行化処理を施されているため、空間中の同一点の像が第１画像及び第２画像の同一走査線上に位置する。したがって、１次元の探索処理により対応する特徴点を探索できる。
３次元座標算出部６５は、対応する特徴点間の視差を算出し、算出した視差と、第１画像センサ５２と第２画像センサ５３の画角、取り付け位置、光軸方向等のパラメータとに基づいて、特徴点が表す空間中の点の３次元座標を算出する。
図５を参照する。現時点ｔ０において特徴点４ａ０及び４ｂ０の３次元座標が算出された場合を想定する。以下、特徴点が表す空間中の点を単に「特徴点」と表記することがある。
図５を参照する。現時点ｔ０において特徴点４ａ０及び４ｂ０の３次元座標が算出された場合を想定する。以下、特徴点が表す空間中の点を単に「特徴点」と表記することがある。
図３を参照する。オプティカルフロー算出部６６は、現時点の特徴点と３次元軌跡記憶部６４に記憶されている過去の特徴点とを対応付け、互いに対応する特徴点の組み合せをオプティカルフローとして抽出する。オプティカルフローによって、現時点の特徴点と３次元軌跡記憶部６４に記憶されている３次元軌跡とが対応付けられる。
図６を参照する。例えばオプティカルフロー算出部６６は、互いに対応する現時点ｔ０の特徴点４ａ０と時刻ｔ－１の特徴点４ａ１とのオプティカルフロー８０ａを抽出することにより特徴点４ａ０と３次元軌跡５ａとを対応付ける。
図６を参照する。例えばオプティカルフロー算出部６６は、互いに対応する現時点ｔ０の特徴点４ａ０と時刻ｔ－１の特徴点４ａ１とのオプティカルフロー８０ａを抽出することにより特徴点４ａ０と３次元軌跡５ａとを対応付ける。
また、オプティカルフロー算出部６６は、互いに対応する現時点ｔ０の特徴点４ｂ０と時刻ｔ－１の特徴点４ｂ１とのオプティカルフロー８０ｂを抽出することにより特徴点４ｂ０と３次元軌跡５ｂとを対応付ける。
オプティカルフロー算出部６６は、過去の特徴点（例えば時刻ｔ－１の特徴点４ａ１）と現時点ｔ０の特徴点（例えば特徴点４ａ０）とが空間中の同一の点を表す場合（すなわち過去の特徴点と現時点ｔ０の特徴点とが空間中の同一の点の像である場合）に、これらの特徴点の組み合わせを対応する特徴点の組み合わせとして検出する。
オプティカルフローの抽出には、例えば非特許文献「Bruce D. Lucas and Takeo Kanade. An Iterative Image Registration Technique with an Application to Stereo Vision. International Joint Conference on Artificial Intelligence, pages 674-679, 1981」に記載の手法を用いてよい。
オプティカルフロー算出部６６は、過去の特徴点（例えば時刻ｔ－１の特徴点４ａ１）と現時点ｔ０の特徴点（例えば特徴点４ａ０）とが空間中の同一の点を表す場合（すなわち過去の特徴点と現時点ｔ０の特徴点とが空間中の同一の点の像である場合）に、これらの特徴点の組み合わせを対応する特徴点の組み合わせとして検出する。
オプティカルフローの抽出には、例えば非特許文献「Bruce D. Lucas and Takeo Kanade. An Iterative Image Registration Technique with an Application to Stereo Vision. International Joint Conference on Artificial Intelligence, pages 674-679, 1981」に記載の手法を用いてよい。
センサ運動検出部６７は、自車両の３軸並進運動及び３軸回転運動（すなわちステレオカメラ５０の３軸並進運動及び３軸回転運動）を検出する。３軸並進運動は、自車両の前後方向、車幅方向、上下方向の運動である。３軸回転運動は、ロール軸、ピッチ軸及びヨー軸を含む３軸回りの回転運動である。
例えばセンサ運動検出部６７は、第１画像及び第２画像に基づいて自車両の３軸並進運動及び３軸回転運動を検出してよい。
図７を参照する。ここでは、１サイクル前の時刻ｔ－１の位置８１から現時点ｔ０の位置８２へ移動したときの自車両２の運動を検出する。
例えばセンサ運動検出部６７は、第１画像及び第２画像に基づいて自車両の３軸並進運動及び３軸回転運動を検出してよい。
図７を参照する。ここでは、１サイクル前の時刻ｔ－１の位置８１から現時点ｔ０の位置８２へ移動したときの自車両２の運動を検出する。
例えば、センサ運動検出部６７は、複数の現時点ｔ０の第１画像中の特徴点と、これらの特徴点にそれぞれ対応する過去の特徴点（例えば１サイクル前の時刻ｔ－１の特徴点）の３次元座標と、に基づいて３軸並進運動及び３軸回転運動を検出してよい。
具体的には、センサ運動検出部６７は、オプティカルフローにより１サイクル前の時刻ｔ－１の複数の特徴点にそれぞれ対応付けられた現時点ｔ０の第１画像中の特徴点を特定することにより、１サイクル前の時刻ｔ－１の複数の特徴点の３次元座標にそれぞれ対応する現時点ｔ０の画像上の位置を求める。
具体的には、センサ運動検出部６７は、オプティカルフローにより１サイクル前の時刻ｔ－１の複数の特徴点にそれぞれ対応付けられた現時点ｔ０の第１画像中の特徴点を特定することにより、１サイクル前の時刻ｔ－１の複数の特徴点の３次元座標にそれぞれ対応する現時点ｔ０の画像上の位置を求める。
センサ運動検出部６７は、１サイクル前の時刻ｔ－１の複数の特徴点の３次元座標のそれぞれが、１サイクル経過後に現時点ｔ０の画像上のそれぞれの位置に投影された場合に、画像上の位置誤差が最小になる自車両の３軸並進運動及び３軸回転運動を推定する。このような推定方法としては、例えば非特許文献「Geiger, Andreas, Julius Ziegler, and Christoph Stiller. "Stereoscan: Dense 3d reconstruction in real-time." Intelligent Vehicles Symposium (IV), 2011 IEEE. Ieee, 2011.」に記載の方法を用いてよい。
このようにステレオ画像からステレオカメラ５０の運動を推定することで、２次元平面上で動く車両だけでなく、３次元的な運動を行うプラットフォームに搭載した場合でも、本発明を適用することができる。
また、車速やヨーレートなどの車両センサから得られる情報だけでなく、ロール、ピッチ、横滑りを含む６軸運動の情報が得られる。
また、車速やヨーレートなどの車両センサから得られる情報だけでなく、ロール、ピッチ、横滑りを含む６軸運動の情報が得られる。
また、外部の車両センサを用いずに自車両の運動を検出できるので、画像センサと外部の車両センサとの間の同期が不要になる。
また、遠方の移動物体の移動を検出しやすくなる。自車両が回転運動すると遠方の特徴点が大きく動く。このため、従来、遠方の移動物体自体の移動と自車両の回転運動に伴う遠方の特徴点の移動との区別が難しかった。画像からセンサ運動を求めると、自車両の回転時に遠方の特徴点が大きく動くので、自車両の回転運動の検出精度が高くなる。このため、自車両の回転運動に伴う遠方の特徴点の移動を精度良く取り除くことができるので、遠方の移動物体の移動を検出しやすくなる。例えば右折時や左折時において検出しやすくなる。
また、遠方の移動物体の移動を検出しやすくなる。自車両が回転運動すると遠方の特徴点が大きく動く。このため、従来、遠方の移動物体自体の移動と自車両の回転運動に伴う遠方の特徴点の移動との区別が難しかった。画像からセンサ運動を求めると、自車両の回転時に遠方の特徴点が大きく動くので、自車両の回転運動の検出精度が高くなる。このため、自車両の回転運動に伴う遠方の特徴点の移動を精度良く取り除くことができるので、遠方の移動物体の移動を検出しやすくなる。例えば右折時や左折時において検出しやすくなる。
図３を参照する。３次元軌跡算出部６８は、センサ運動検出部６７が検出した３軸並進運動及び３軸回転運動に基づき、１サイクル前の時刻ｔ－１の画像中の特徴点と、この特徴点に対応する現時点ｔ０の画像中の特徴点と、の間の３次元軌跡を算出する。
具体的には、軌跡変換部６９は、センサ運動検出部６７が検出した自車両の運動（ステレオカメラ５０の運動）に基づいて、３次元軌跡記憶部６４に記憶されている３次元軌跡５ａ及び５ｂの３次元座標を、現時点ｔ０の第１画像センサ５２の位置を基準とするセンサ座標系上の座標へ変換する。すなわち、３次元軌跡記憶部６４に記憶されている３次元軌跡５ａ及び５ｂを、現時点ｔ０のセンサ座標系上の３次元軌跡へ変換する。
具体的には、軌跡変換部６９は、センサ運動検出部６７が検出した自車両の運動（ステレオカメラ５０の運動）に基づいて、３次元軌跡記憶部６４に記憶されている３次元軌跡５ａ及び５ｂの３次元座標を、現時点ｔ０の第１画像センサ５２の位置を基準とするセンサ座標系上の座標へ変換する。すなわち、３次元軌跡記憶部６４に記憶されている３次元軌跡５ａ及び５ｂを、現時点ｔ０のセンサ座標系上の３次元軌跡へ変換する。
図８を参照する。軌跡変換部６９は、１サイクル前の時刻ｔ－１の位置８１から現時点ｔ０の位置８２へ移動したときの自車両２の運動と反対に３次元軌跡５ａ及び５ｂを移動させることにより、３次元軌跡５ａ及び５ｂを、現時点ｔ０のセンサ座標系上の３次元軌跡へ変換する。
これにより、１サイクル前の時刻ｔ－１の特徴点４ａ１と特徴点４ｂ１が、現時点ｔ０のセンサ座標系上のどの座標にあるのかが求まるので、１サイクル前の時刻ｔ－１の特徴点４ａ１と現時点ｔ０の特徴点４ａ０との間の３次元軌跡５ｘ、１サイクル前の時刻ｔ－１の特徴点４ｂ１と現時点ｔ０の特徴点４ｂ０との間の３次元軌跡５ｙを算出できる。
これにより、１サイクル前の時刻ｔ－１の特徴点４ａ１と特徴点４ｂ１が、現時点ｔ０のセンサ座標系上のどの座標にあるのかが求まるので、１サイクル前の時刻ｔ－１の特徴点４ａ１と現時点ｔ０の特徴点４ａ０との間の３次元軌跡５ｘ、１サイクル前の時刻ｔ－１の特徴点４ｂ１と現時点ｔ０の特徴点４ｂ０との間の３次元軌跡５ｙを算出できる。
図３を参照する。軌跡接続部７０は、軌跡変換部６９により現時点ｔ０のセンサ座標系上の軌跡に変換された３次元軌跡へ、オプティカルフローによってこの軌跡に対応付けられる現時点ｔ０の特徴点の３次元座標を接続することにより、３次元軌跡記憶部６４に記憶されている３次元軌跡を更新する。軌跡接続部７０は、更新した３次元軌跡を３次元軌跡記憶部６４に記憶する。
図９を参照する。軌跡接続部７０は、軌跡変換部６９によりセンサ座標系上の軌跡に変換された３次元軌跡５ａへ、オプティカルフローにより３次元軌跡５ａに対応付けられている現時点ｔ０の特徴点４ａ０の３次元座標を接続する。具体的には、３次元軌跡５ａの１サイクル前の時刻ｔ－１の特徴点４ａ１に特徴点４ａ０を接続する。同様に、軌跡接続部７０は、センサ座標系上の軌跡に変換された３次元軌跡５ｂへ、オプティカルフローにより３次元軌跡５ｂに対応付けられている現時点ｔ０の特徴点４ｂ０の３次元座標を接続する。
図９を参照する。軌跡接続部７０は、軌跡変換部６９によりセンサ座標系上の軌跡に変換された３次元軌跡５ａへ、オプティカルフローにより３次元軌跡５ａに対応付けられている現時点ｔ０の特徴点４ａ０の３次元座標を接続する。具体的には、３次元軌跡５ａの１サイクル前の時刻ｔ－１の特徴点４ａ１に特徴点４ａ０を接続する。同様に、軌跡接続部７０は、センサ座標系上の軌跡に変換された３次元軌跡５ｂへ、オプティカルフローにより３次元軌跡５ｂに対応付けられている現時点ｔ０の特徴点４ｂ０の３次元座標を接続する。
ここで、前回のサイクルでは、現時点ｔ０より２サイクル前の時刻ｔ－２の特徴点４ａ２、４ｂ２と、１サイクル前の時刻ｔ－１の特徴点４ａ１、４ｂ１との３次元軌跡が算出されている。今回のサイクルで１サイクル前の時刻ｔ－１の特徴点４ａ１に特徴点４ａ０を接続することにより、特徴点４ａ２と特徴点４ａ１との間の３次元軌跡と、特徴点４ａ１と特徴点４ａ０との間の３次元軌跡が接続される。また、特徴点４ｂ１に特徴点４ｂ０を接続することにより、特徴点４ｂ２と特徴点４ｂ１との間の３次元軌跡と、特徴点４ｂ１と特徴点４ｂ０との間の３次元軌跡が接続される。
同様に、前々回のサイクルでは、現時点ｔ０より３サイクル前の時刻ｔ－３の特徴点４ａ３、４ｂ３と、２サイクル前の時刻ｔ－２の特徴点４ａ２、４ｂ２との３次元軌跡が算出されている。前回のサイクルで２サイクル前の時刻ｔ－２の特徴点４ａ２に１サイクル前の時刻ｔ－１の特徴点４ａ１を接続することにより、特徴点４ａ３と特徴点４ａ２との間の３次元軌跡と、特徴点４ａ２と特徴点４ａ１との間の３次元軌跡が接続される。また、特徴点４ｂ２に特徴点４ｂ１を接続することにより、特徴点４ｂ３と特徴点４ｂ２との間の３次元軌跡と、特徴点４ｂ２と特徴点４ｂ１との間の３次元軌跡が接続される。
このように軌跡接続部７０は、異なるサイクルで算出された複数の３次元軌跡を接続する。
このように軌跡接続部７０は、異なるサイクルで算出された複数の３次元軌跡を接続する。
図３を参照する。運動推定部７１は、３次元軌跡が類似する特徴点をグループ化することで移動物体を検出する。すなわち、運動推定部７１は、同様の軌跡を持つ３次元軌跡をまとめるクラスタリング処理を行うことで、同一の移動物体上の点の軌跡を移動物体候補として抽出する。
図１０を参照する。運動推定部７１は、互いに類似する３次元軌跡５ａ～５ｄの特徴点をグループ化し、移動物体３について予め想定した３次元モデル８３上の点に３次元軌跡５ａ～５ｄの特徴点４ａ０～４ｄ０を当てはめる。運動推定部７１は、グループ化された特徴点の３次元軌跡５ａ～５ｄに基づいて、移動物体３の３次元運動を推定する。運動推定部７１は、３次元運動として、移動物体３の３軸並進運動及び３軸回転運動を検出してよい。
このように同一の移動物体３に属する複数の３次元軌跡５ａ～５ｄをグループ化することで、この移動物体３の姿勢や微小な運動変化を精度よく且つ迅速に検知することが可能となる。
このように同一の移動物体３に属する複数の３次元軌跡５ａ～５ｄをグループ化することで、この移動物体３の姿勢や微小な運動変化を精度よく且つ迅速に検知することが可能となる。
図３を参照する。運動推定部７１は、移動物体の３次元運動に基づいて自車両の周囲状況を推定する。移動物体の３次元運動に基づいて自車両の周囲状況を推定する。
運動推定部７１は、移動物体の３次元軌跡情報、移動物体運動情報及び周囲状況情報を、ユーザインタフェース装置１３及び車両走行コントローラ３０へ出力する。
運動推定部７１は、移動物体の３次元軌跡情報、移動物体運動情報及び周囲状況情報を、ユーザインタフェース装置１３及び車両走行コントローラ３０へ出力する。
また、運動推定部７１は、自車両の周囲状況に基づいて運転席シートの位置又は姿勢を変更させる第１指示信号、自車両の周囲状況に基づいて操舵反力を変更させる第２指示信号、自車両の周囲状況に基づいてショックアブソーバの減衰力を変更させる第３指示信号を生成する。運動推定部７１は、第１指示信号、第２指示信号、及び第３指示信号を、それぞれシートコントローラ４０、反力コントローラ４２、及びサスペンションコントローラ４４へ出力する。
また、コントローラ１２は、図１０に示すような移動物体３の３次元軌跡５ａ～５ｄを、第１画像センサ５２の撮像画像に重畳して、ユーザインタフェース装置１３のディスプレイ装置やヘッドアップディスプレイ等に表示する。
このとき、例えばコントローラ１２は、異なるサイクルで算出された特徴点間の複数の３次元軌跡が連続的に接続されることにより生成された３次元軌跡５ａ～５ｄのそれぞれの座標を、第１画像センサ５２の画角、取り付け位置、光軸方向等のパラメータに基づいて第１画像センサ５２の撮像画像上の座標へ変換することにより、これら３次元軌跡５ａ～５ｄを第１画像センサ５２の撮像画像上の２次元座標系に投影してよい。コントローラ１２は、２次元座標系に投影した３次元軌跡５ａ～５ｄを第１画像センサ５２の撮像画像に重畳する。
このとき、例えばコントローラ１２は、異なるサイクルで算出された特徴点間の複数の３次元軌跡が連続的に接続されることにより生成された３次元軌跡５ａ～５ｄのそれぞれの座標を、第１画像センサ５２の画角、取り付け位置、光軸方向等のパラメータに基づいて第１画像センサ５２の撮像画像上の座標へ変換することにより、これら３次元軌跡５ａ～５ｄを第１画像センサ５２の撮像画像上の２次元座標系に投影してよい。コントローラ１２は、２次元座標系に投影した３次元軌跡５ａ～５ｄを第１画像センサ５２の撮像画像に重畳する。
（動作）
次に、軌跡推定装置１０の動作を説明する。図１１は、軌跡推定処理の１処理周期（すなわち１サイクル）における処理のフローを示す。軌跡推定装置１０は、図１１のフローを所定の処理周期で反復する。
ステップＳ１においてステレオカメラ５０は、自車両周囲のステレオ画像を所定の周期で取得する。
ステップＳ２において画像補正部６０及び６１は、ステレオ画像に含まれる第１画像及び第２画像の補正処理をそれぞれ行う。
次に、軌跡推定装置１０の動作を説明する。図１１は、軌跡推定処理の１処理周期（すなわち１サイクル）における処理のフローを示す。軌跡推定装置１０は、図１１のフローを所定の処理周期で反復する。
ステップＳ１においてステレオカメラ５０は、自車両周囲のステレオ画像を所定の周期で取得する。
ステップＳ２において画像補正部６０及び６１は、ステレオ画像に含まれる第１画像及び第２画像の補正処理をそれぞれ行う。
ステップＳ３において特徴点抽出部６２及び６３は、第１画像及び第２画像それぞれにおいて特徴点を抽出する。
ステップＳ４において３次元座標算出部６５は、互いに対応する第１画像の特徴点と第２画像の特徴点を探索するステレオ視差算出処理により特徴点の３次元座標を算出する。
ステップＳ４において３次元座標算出部６５は、互いに対応する第１画像の特徴点と第２画像の特徴点を探索するステレオ視差算出処理により特徴点の３次元座標を算出する。
ステップＳ５においてオプティカルフロー算出部６６は、互いに対応する現時点の特徴点と過去の特徴点との組み合せをオプティカルフローとして抽出する。オプティカルフローにより現時点の特徴点と３次元軌跡記憶部６４に記憶されている３次元軌跡とが対応付けられる。
なお、ステップＳ４の３次元座標の算出より先にステップＳ５のオプティカルフローの算出を行ってもよく、ステップＳ４の３次元座標の算出と並列に（同時に）ステップＳ５のオプティカルフローの算出を行ってもよい。
なお、ステップＳ４の３次元座標の算出より先にステップＳ５のオプティカルフローの算出を行ってもよく、ステップＳ４の３次元座標の算出と並列に（同時に）ステップＳ５のオプティカルフローの算出を行ってもよい。
ステップＳ６においてセンサ運動検出部６７は、自車両の３軸並進運動及び３軸回転運動（すなわちステレオカメラ５０の３軸並進運動及び３軸回転運動）を検出する。
ステップＳ７において軌跡変換部６９は、３次元軌跡記憶部６４に記憶されている３次元軌跡のそれぞれを、現時点ｔ０の第１画像センサ５２の位置を基準とするセンサ座標系上の軌跡へ変換する。
ステップＳ７において軌跡変換部６９は、３次元軌跡記憶部６４に記憶されている３次元軌跡のそれぞれを、現時点ｔ０の第１画像センサ５２の位置を基準とするセンサ座標系上の軌跡へ変換する。
ステップＳ８において軌跡接続部７０は、センサ座標系上の軌跡へ変換された３次元軌跡のそれぞれについて、オプティカルフロー算出部６６が算出したオプティカルフローにより対応付けできる現時点ｔ０の特徴点があるか否かを判定する。３次元軌跡に対応付けできる現時点ｔ０の特徴点がある場合（ステップＳ８：Ｙ）に処理はステップＳ９に進む。３次元軌跡に対応付けできる現時点ｔ０の特徴点がない場合（ステップＳ８：Ｎ）に処理はステップＳ１０に進む。
ステップＳ９において軌跡接続部７０は、センサ座標系上の軌跡へ変換された３次元軌跡へ、この軌跡に対応付けられた現時点ｔ０の特徴点を接続することにより、３次元軌跡を更新する。軌跡接続部７０は、更新した３次元軌跡を３次元軌跡記憶部６４に記憶する。その後に処理は終了する。
ステップＳ１０において軌跡接続部７０は、オプティカルフローにより現時点ｔ０の特徴点に対応付けできない３次元軌跡について、所定サイクル以上にわたって現時点ｔ０の特徴点に対応付けられないか否かを判定する。
ステップＳ１０において軌跡接続部７０は、オプティカルフローにより現時点ｔ０の特徴点に対応付けできない３次元軌跡について、所定サイクル以上にわたって現時点ｔ０の特徴点に対応付けられないか否かを判定する。
所定サイクル以上、現時点ｔ０の特徴点に対応付けられない場合（ステップＳ１０：Ｙ）に処理はステップＳ１１に進む。所定サイクル内に再び現時点ｔ０の特徴点に対応付けられた場合（ステップＳ１０：Ｎ）に処理は終了する。
ステップＳ１１において軌跡接続部７０は、所定サイクル以上にわたって現時点ｔ０の特徴点に対応付けられない３次元軌跡を３次元軌跡記憶部６４から削除する。その後に処理は終了する。
ステップＳ１１において軌跡接続部７０は、所定サイクル以上にわたって現時点ｔ０の特徴点に対応付けられない３次元軌跡を３次元軌跡記憶部６４から削除する。その後に処理は終了する。
（第１実施形態の効果）
（１）自車両に搭載されたステレオカメラ５０は、自車両周囲の第１画像と第２画像を取得する。特徴点抽出部６２及び６３は、第１画像中の特徴点と第２画像中の特徴点とを抽出する。
３次元座標算出部６５は、特徴点の３次元座標を取得する。オプティカルフロー算出部６６は、第１時刻の画像から抽出した特徴点と、第２時刻より前の第１時刻以前の画像から抽出した特徴点の３次元軌跡とを対応付ける。
センサ運動検出部６７は、ステレオカメラ５０の３軸並進運動及び３軸回転運動を検出する。軌跡変換部６９は、３軸並進運動及び前記３軸回転運動に基づき、３次元軌跡を、第１時刻の第１画像センサ５２の位置を基準とするセンサ座標系上の軌跡へ変換する。
軌跡接続部７０は、センサ座標系上の軌跡に変換された３次元軌跡へ、この軌跡に対応付けられた第２時刻の画像から抽出した特徴点の３次元座標を接続することにより、３次元軌跡を更新する。
これにより、移動物体の３次元軌跡を生成することができるようになる。すなわち移動動物体の上下動を含んだ軌跡を推定できる。このため、移動物体の挙動を正確に検出できるようになる。移動物体の詳細な挙動を検出し、移動物体の詳細な挙動に基づいて路面状況を検出できるようになるため、それに合わせて正確な運転支援を実行できる。
（１）自車両に搭載されたステレオカメラ５０は、自車両周囲の第１画像と第２画像を取得する。特徴点抽出部６２及び６３は、第１画像中の特徴点と第２画像中の特徴点とを抽出する。
３次元座標算出部６５は、特徴点の３次元座標を取得する。オプティカルフロー算出部６６は、第１時刻の画像から抽出した特徴点と、第２時刻より前の第１時刻以前の画像から抽出した特徴点の３次元軌跡とを対応付ける。
センサ運動検出部６７は、ステレオカメラ５０の３軸並進運動及び３軸回転運動を検出する。軌跡変換部６９は、３軸並進運動及び前記３軸回転運動に基づき、３次元軌跡を、第１時刻の第１画像センサ５２の位置を基準とするセンサ座標系上の軌跡へ変換する。
軌跡接続部７０は、センサ座標系上の軌跡に変換された３次元軌跡へ、この軌跡に対応付けられた第２時刻の画像から抽出した特徴点の３次元座標を接続することにより、３次元軌跡を更新する。
これにより、移動物体の３次元軌跡を生成することができるようになる。すなわち移動動物体の上下動を含んだ軌跡を推定できる。このため、移動物体の挙動を正確に検出できるようになる。移動物体の詳細な挙動を検出し、移動物体の詳細な挙動に基づいて路面状況を検出できるようになるため、それに合わせて正確な運転支援を実行できる。
（２）運動推定部７１は、３次元軌跡が類似する特徴点をグループ化することで移動物体を検出し、移動物体の３次元モデルに特徴点を当てはめ、グループ化された特徴点の３次元軌跡に基づいて移動物体の運動を推定する。
これにより、類似した軌跡を持つ特徴点を統合することで、これらの特徴点が属する車両などの移動物体を抽出することができる。対象の大きさなどにより対象の種別を判断することで、種別に応じた３次元運動モデルを仮定して複数の特徴点を追跡することで、移動物体の運動をより正確に推定することができる。
これにより、類似した軌跡を持つ特徴点を統合することで、これらの特徴点が属する車両などの移動物体を抽出することができる。対象の大きさなどにより対象の種別を判断することで、種別に応じた３次元運動モデルを仮定して複数の特徴点を追跡することで、移動物体の運動をより正確に推定することができる。
（３）特徴点抽出部６２及び６３は、自車両周囲の画像の先行車の位置より特徴点を抽出し、三次元軌跡算出部６８は、先行車の運動として、３次元軌跡を算出し、車両走行コントローラ３０は、３次元軌跡に基づいてアクチュエータを制御して自車両の自動運転又は走行支援を行う。
このとき、運動推定部７１は、３次元軌跡に基づいて先行車の運動を推定してもよい。車両走行コントローラ３０は、先行車の運動に基づいて車両制御アクチュエータ群３１を制御して自車両の自動運転又は走行支援を行う。
これにより、先行車の詳細な３次元軌跡を利用して自動運転又は走行支援を実行できるので、乗員の違和感を抑制する走行支援を実行できる。
このとき、運動推定部７１は、３次元軌跡に基づいて先行車の運動を推定してもよい。車両走行コントローラ３０は、先行車の運動に基づいて車両制御アクチュエータ群３１を制御して自車両の自動運転又は走行支援を行う。
これにより、先行車の詳細な３次元軌跡を利用して自動運転又は走行支援を実行できるので、乗員の違和感を抑制する走行支援を実行できる。
（４）センサ運動検出部６７は、複数の第１時刻の画像から抽出した特徴点の３次元座標と、複数の第１時刻の画像から抽出した特徴点にそれぞれ対応する第２時刻の画像から抽出した特徴点の座標に基づいて、ステレオカメラ５０の３軸並進運動及び３軸回転運動を算出する。
これにより、２次元平面上で動く車両だけでなく、３次元的な運動を行うプラットフォームに搭載した場合でも、本発明を適用することができる。
車速やヨーレートなどの車両センサから得られる情報だけでなく、ロール、ピッチ、横滑りを含む６軸運動の情報が得られる。
また、外部の車両センサを用いずに自車両の運動を検出できるので、画像センサと外部の車両センサとの間の同期が不要になる。
さらに、従来、遠方の移動物体自体の移動と、自車両の回転運動に伴う遠方の特徴点の移動との区別が難しかったところ、画像からセンサ運動を求めると自車両の回転時に遠方の特徴点が大きく動くので、自車両の回転運動の検出精度がよい。このため、自車両の回転運動に伴う遠方の特徴点の移動を精度良く取り除くことができるので、遠方の移動物体の移動を検出しやすくなる。例えば右折時や左折時において移動物体の移動を検出しやすくなる。
これにより、２次元平面上で動く車両だけでなく、３次元的な運動を行うプラットフォームに搭載した場合でも、本発明を適用することができる。
車速やヨーレートなどの車両センサから得られる情報だけでなく、ロール、ピッチ、横滑りを含む６軸運動の情報が得られる。
また、外部の車両センサを用いずに自車両の運動を検出できるので、画像センサと外部の車両センサとの間の同期が不要になる。
さらに、従来、遠方の移動物体自体の移動と、自車両の回転運動に伴う遠方の特徴点の移動との区別が難しかったところ、画像からセンサ運動を求めると自車両の回転時に遠方の特徴点が大きく動くので、自車両の回転運動の検出精度がよい。このため、自車両の回転運動に伴う遠方の特徴点の移動を精度良く取り除くことができるので、遠方の移動物体の移動を検出しやすくなる。例えば右折時や左折時において移動物体の移動を検出しやすくなる。
（５）ユーザインタフェース装置１３は、接続された複数の３次元軌跡をカメラで撮影した画像の前記特徴点の位置に重畳して表示する。例えばユーザインタフェース装置１３は、３次元軌跡として得られる先行車の走行軌跡を、第１画像センサ５２で撮影した画像に重畳して表示する。これにより悪視界時における運転者の付加を軽減させることができる。
（６）運動推定部７１は、３次元軌跡から自車両の周囲の移動物体の運動を推定し、移動物体の運動に基づいて自車両の周囲状況を推定する。ユーザインタフェース装置１３は、周囲状況を乗員に報知する。
これにより、自車両周囲を視認するだけでは知覚が難しい自車両の周囲状況の変化を乗員に知らせることが可能となる。
（６）運動推定部７１は、３次元軌跡から自車両の周囲の移動物体の運動を推定し、移動物体の運動に基づいて自車両の周囲状況を推定する。ユーザインタフェース装置１３は、周囲状況を乗員に報知する。
これにより、自車両周囲を視認するだけでは知覚が難しい自車両の周囲状況の変化を乗員に知らせることが可能となる。
（７）運動推定部７１は、３次元軌跡から自車両の周囲の移動物体の運動を推定し、移動物体の運動に基づいて自車両の周囲状況を推定し、周囲状況に応じて運転席シートの位置又は姿勢を変化させる。これにより、自動運転制御実行時に周囲状況に応じて手動運転への切り替えに備えて転席シートの位置又は姿勢を変化させることができる。
（８）運動推定部７１は、３次元軌跡として得られる自車両の周囲の移動物体の運動より自車両の前方の走行路の勾配変化を推定する。
例えば、３次元軌跡として得られる先行車の走行軌跡の上下動から自車両の前方の走行路の勾配変化を推定する。車両走行コントローラ３０は、推定した勾配変化に基づいて車両制御アクチュエータ群３１を制御して自車両の加減速度変化を抑制する。これにより、周囲の移動物体の詳細な３次元軌跡を利用して自車両の加減速度変化を抑制できるので、乗員の違和感を抑制する運転支援を実行できる。
例えば、３次元軌跡として得られる先行車の走行軌跡の上下動から自車両の前方の走行路の勾配変化を推定する。車両走行コントローラ３０は、推定した勾配変化に基づいて車両制御アクチュエータ群３１を制御して自車両の加減速度変化を抑制する。これにより、周囲の移動物体の詳細な３次元軌跡を利用して自車両の加減速度変化を抑制できるので、乗員の違和感を抑制する運転支援を実行できる。
（９）運動推定部７１は、３次元軌跡として得られる自車両の周囲の移動物体の運動より自車両の前方の走行路の凹凸を推定する。例えば３次元軌跡として得られる先行車の走行軌跡の上下動から自車両の前方の走行路の凹凸を検出する。車両走行コントローラ３０は検出した凹凸に基づいて車両制御アクチュエータ群３１を制御して凹凸による乗員の上下動を抑制する。運動推定部７１及びサスペンションコントローラ４４は、検出した凹凸に基づいてアブソーバアクチュエータ４５を制御して凹凸による乗員の上下動を抑制する。これにより、乗員に伝達される振動を抑制して快適な乗り心地を提供できる。
（１０）運動推定部７１は、３次元軌跡として得られる自車両の周囲の移動物体の運動より、自車両の周囲の路面状況を検出する。例えば、３次元軌跡として得られる先行車の走行軌跡の上下動から自車両の周囲の路面状況を検出する。運動推定部７１及び反力コントローラ４２は、検出した路面状況に基づいて反力アクチュエータ４３を制御することにより、ステアリングホイールに付与する操舵反力を変更する。これにより、路面状況に応じた操舵反力を付与することが可能になり、快適な操舵フィーリングを提供できる。
（第１実施形態の変形例）
（１）自車両の車速信号と、自車両に搭載された加速度センサ２２及びジャイロセンサ２３の出力信号とに基づいて、ステレオカメラ５０の３軸並進運動及び３軸回転運動を算出してもよい。
これにより、特徴点が検出しにくい場面でも自車両の運動（すなわちステレオカメラ５０の運動）を検出できるため、軌跡推定装置１０の利用シーンを拡大できる。
（２）また、特徴点までの距離をレーダ５１で検出して特徴点の３次元座標を取得してもよい。この場合、ステレオカメラ５０に代えて単眼カメラで自車両周囲の画像を取得してもよい。
（１）自車両の車速信号と、自車両に搭載された加速度センサ２２及びジャイロセンサ２３の出力信号とに基づいて、ステレオカメラ５０の３軸並進運動及び３軸回転運動を算出してもよい。
これにより、特徴点が検出しにくい場面でも自車両の運動（すなわちステレオカメラ５０の運動）を検出できるため、軌跡推定装置１０の利用シーンを拡大できる。
（２）また、特徴点までの距離をレーダ５１で検出して特徴点の３次元座標を取得してもよい。この場合、ステレオカメラ５０に代えて単眼カメラで自車両周囲の画像を取得してもよい。
（第２実施形態）
次に第２実施形態について説明する。第２実施形態では、特徴点の位置と自車両の速度を状態量とする時系列フィルタを用いて３次元軌跡を生成する。
図１２を参照する。図３に示す構成要素と同一の構成要素には同じ参照符号を付して、説明を省略する。
次に第２実施形態について説明する。第２実施形態では、特徴点の位置と自車両の速度を状態量とする時系列フィルタを用いて３次元軌跡を生成する。
図１２を参照する。図３に示す構成要素と同一の構成要素には同じ参照符号を付して、説明を省略する。
第２実施形態の軌跡推定装置１０は、３次元軌跡の生成に用いる時系列フィルタとして拡張カルマンフィルタ９０を備える。
拡張カルマンフィルタは時系列フィルタの一例であり、本発明はこれに限定されるものではない。軌跡推定装置１０は、拡張カルマンフィルタの代わりにカルマンフィルタやパーティクルフィルタなどの他の時系列フィルタを用いて３次元軌跡を推定してもよい。
拡張カルマンフィルタは時系列フィルタの一例であり、本発明はこれに限定されるものではない。軌跡推定装置１０は、拡張カルマンフィルタの代わりにカルマンフィルタやパーティクルフィルタなどの他の時系列フィルタを用いて３次元軌跡を推定してもよい。
また軌跡推定装置１０は、拡張カルマンフィルタ９０を生成するフィルタ生成部９１を備える。フィルタ生成部９１は、３次元軌跡ごとに１つずつ拡張カルマンフィルタ９０を生成する。
拡張カルマンフィルタ９０は、予測部９２と、計測部９３とを備える。
予測部９２は、センサ運動検出部６７が推定したステレオカメラ５０の運動に基づいて、１サイクル前の時刻ｔ－１までの３次元軌跡を現時点ｔ０のセンサ座標系上の３次元軌跡へ変換する。
拡張カルマンフィルタ９０は、予測部９２と、計測部９３とを備える。
予測部９２は、センサ運動検出部６７が推定したステレオカメラ５０の運動に基づいて、１サイクル前の時刻ｔ－１までの３次元軌跡を現時点ｔ０のセンサ座標系上の３次元軌跡へ変換する。
また予測部９２は、１サイクル前の時刻ｔ－１の特徴点の位置及び自車両の速度に基づいて現時点ｔ０の特徴点の３次元座標を推定する。その際に予測部９２は、推定した特徴点の座標の平均値だけでなくその座標の誤差共分散行列も推定する。
計測部９３は、推定した現時点ｔ０の特徴点の３次元座標と、誤差共分散行列と、３次元座標算出部６５が算出した現時点ｔ０の特徴点の３次元座標の観測量とに基づき、現時点ｔ０の特徴点の３次元座標を算出する。
計測部９３は、推定した現時点ｔ０の特徴点の３次元座標と、誤差共分散行列と、３次元座標算出部６５が算出した現時点ｔ０の特徴点の３次元座標の観測量とに基づき、現時点ｔ０の特徴点の３次元座標を算出する。
計測部９３は、予測部９２によりセンサ座標系上の軌跡に変換された３次元軌跡へ、オプティカルフローによってこの軌跡に対応付けられる現時点ｔ０の特徴点の３次元座標を接続することにより、３次元軌跡を更新する。
フィルタ生成部９１は、以前に生成済の３次元軌跡に対応付けられない現時点ｔ０の特徴点を検出すると、その現時点ｔ０を含んだ新規の３次元軌跡を記録し、新規の３次元軌跡のための新たな拡張カルマンフィルタ９０を生成する。
フィルタ生成部９１は、以前に生成済の３次元軌跡に対応付けられない現時点ｔ０の特徴点を検出すると、その現時点ｔ０を含んだ新規の３次元軌跡を記録し、新規の３次元軌跡のための新たな拡張カルマンフィルタ９０を生成する。
（第２実施形態の効果）
軌跡推定装置１０は、時系列フィルタを用いて３次元軌跡を生成する。これにより、特徴点の位置や自車両の速度の誤差を推定することができるので、観測された運動が誤差によるものか移動物体の運動によるものかを統計的に判断できる。
軌跡推定装置１０は、時系列フィルタを用いて３次元軌跡を生成する。これにより、特徴点の位置や自車両の速度の誤差を推定することができるので、観測された運動が誤差によるものか移動物体の運動によるものかを統計的に判断できる。
１…運転支援装置、２…自車両、３…移動物体、１０…軌跡推定装置、１１…周囲環境センサ群、１２…コントローラ、１３…ユーザインタフェース装置、２０…車両挙動センサ群、２１…車速センサ、２２…加速度センサ、２３…ジャイロセンサ、２４…操舵角センサ、３０…車両走行コントローラ、３１…車両制御アクチュエータ群、３２…ステアリングアクチュエータ、３３…アクセル開度アクチュエータ、３４…ブレーキ制御アクチュエータ、３５…シフトアクチュエータ、４０…シートコントローラ、４１…シートアクチュエータ、４２…反力コントローラ、４３…反力アクチュエータ、４４…サスペンションコントローラ、４５…アブソーバアクチュエータ、５０…ステレオカメラ、５１…レーダ、５２…第１画像センサ、５３…第２画像センサ、５５…プロセッサ、５６…記憶装置、６０、６１…画像補正部、６２、６３…特徴点抽出部、６４…次元軌跡記憶部、６５…次元座標算出部、６６…オプティカルフロー算出部、６７…センサ運動検出部、６８…３次元軌跡算出部、６９…軌跡変換部、７０…軌跡接続部、７１…運動推定部、９０…拡張カルマンフィルタ、９１…フィルタ生成部、９２…予測部、９３…計測部
Claims (12)
- 自車両に搭載された画像センサにより第１時刻及び前記第１時刻より後の第２時刻において自車両周囲の画像を取得し、
前記第１時刻の画像中の特徴点と、前記第１時刻の特徴点に対応する前記第２時刻の画像中の特徴点とを抽出し、
前記第１時刻の画像中の特徴点が表す３次元空間中の３次元座標と、前記第２時刻の画像中の特徴点が表す３次元空間中の３次元座標とを算出し、
前記画像センサの３軸並進運動及び３軸回転運動を検出し、
検出した前記３軸並進運動及び前記３軸回転運動に基づき、前記第１時刻の画像中の特徴点が表す３次元座標と、前記第１時刻の特徴点に対応する前記第２時刻の画像中の特徴点が表す３次元座標と、を接続する３次元軌跡を算出し、
複数の前記３次元軌跡を接続する、ことを特徴とする軌跡推定方法。 - 時系列フィルタを用いて前記３次元軌跡を生成することを特徴とする請求項１に記載の軌跡推定方法。
- 前記３次元軌跡が類似する特徴点をグループ化することで移動物体を検出し、
前記移動物体の３次元モデルに前記特徴点を当てはめ、グループ化された前記特徴点の３次元軌跡に基づいて前記移動物体の運動を推定する、
ことを特徴とする請求項１又は２に記載の軌跡推定方法。 - 前記自車両周囲の画像の先行車の位置より、前記特徴点を抽出し、
前記先行車の運動として、前記３次元軌跡を算出し、
前記３次元軌跡に基づいてアクチュエータを制御して前記自車両の自動運転又は走行支援を行う、
ことを特徴とする請求項１～３のいずれか一項に記載の軌跡推定方法。 - 前記第１時刻の画像から抽出した特徴点の３次元座標と、前記第１時刻の画像から抽出した特徴点に対応する前記第２時刻の画像から抽出した特徴点の座標と、に基づいて、前記３軸並進運動及び前記３軸回転運動を算出することを特徴とする請求項１～４のいずれか一項に記載の軌跡推定方法。
- 接続した前記複数の３次元軌跡をカメラで撮影した画像の前記特徴点の位置に重畳して表示することを特徴とする請求項１～５のいずれか一項に記載の軌跡推定方法。
- 前記３次元軌跡から前記自車両の周囲の移動物体の運動を推定し、
前記移動物体の運動に基づいて前記自車両の周囲状況を推定し、前記周囲状況を乗員に報知することを特徴とする請求項１～６のいずれか一項に記載の軌跡推定方法。 - 前記３次元軌跡から前記自車両の周囲の移動物体の運動を推定し、
前記移動物体の運動に基づいて前記自車両の周囲状況を推定し、前記周囲状況に応じて運転席シートの位置又は姿勢を変化させることを特徴とする請求項１～７のいずれか一項に記載の軌跡推定方法。 - 前記３次元軌跡として得られる前記自車両の周囲の移動物体の運動より前記自車両の前方の走行路の勾配変化を推定し、
推定した前記勾配変化に基づいてアクチュエータを制御して前記自車両の加減速度変化を抑制することを特徴とする請求項１～８のいずれか一項に記載の軌跡推定方法。 - 自車両に搭載された画像センサにより第１時刻及び前記第１時刻より後の第２時刻において自車両周囲の画像を取得し、
前記第１時刻の画像中の特徴点と、前記第１時刻の特徴点に対応する前記第２時刻の画像中の特徴点とを抽出し、
前記画像センサの３軸並進運動及び３軸回転運動を検出し、
検出した前記３軸並進運動及び前記３軸回転運動に基づき、前記第１時刻の画像中の特徴点と、前記第１時刻の特徴点に対応する前記第２時刻の画像中の特徴点と、の３次元軌跡を算出し、
複数の前記３次元軌跡を接続し、
前記３次元軌跡として得られる前記自車両の周囲の移動物体の運動より前記自車両の前方の走行路の凹凸を推定し、
検出した前記凹凸に基づいてアクチュエータを制御して前記凹凸による乗員の上下動を抑制することを特徴とする軌跡推定方法。 - 前記３次元軌跡として得られる前記自車両の周囲の移動物体の運動より、前記自車両の周囲の路面状況を検出し、検出した前記路面状況に基づいてステアリングホイールに付与する操舵反力を変更することを特徴とする請求項１～１０のいずれか一項に記載の軌跡推定方法。
- 第１時刻及び前記第１時刻より後の第２時刻において自車両周囲の画像を取得する画像センサと、
前記第１時刻の画像中の特徴点と、前記第１時刻の特徴点に対応する前記第２時刻の画像中の特徴点とを抽出する特徴点抽出部と、
前記第１時刻の画像中の特徴点が表す３次元空間中の３次元座標と、前記第２時刻の画像中の特徴点が表す３次元空間中の３次元座標とを算出する３次元座標算出部と、
前記画像センサの３軸並進運動及び３軸回転運動を検出するセンサ運動検出部と、
検出した前記３軸並進運動及び前記３軸回転運動に基づき、前記第１時刻の画像中の特徴点が表す３次元座標と、前記第１時刻の特徴点に対応する前記第２時刻の画像中の特徴点が表す３次元座標と、を接続する３次元軌跡を算出する３次元軌跡算出部と、
複数の前記３次元軌跡を接続する軌跡接続部と、
を備えることを特徴とする軌跡推定装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017139888A JP7005978B2 (ja) | 2017-07-19 | 2017-07-19 | 軌跡推定方法及び軌跡推定装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2017139888A JP7005978B2 (ja) | 2017-07-19 | 2017-07-19 | 軌跡推定方法及び軌跡推定装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2019018743A JP2019018743A (ja) | 2019-02-07 |
| JP7005978B2 true JP7005978B2 (ja) | 2022-02-10 |
Family
ID=65353612
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2017139888A Active JP7005978B2 (ja) | 2017-07-19 | 2017-07-19 | 軌跡推定方法及び軌跡推定装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7005978B2 (ja) |
Families Citing this family (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11468681B1 (en) | 2018-06-28 | 2022-10-11 | Amazon Technologies, Inc. | Associating events with actors using digital imagery and machine learning |
| US11468698B1 (en) | 2018-06-28 | 2022-10-11 | Amazon Technologies, Inc. | Associating events with actors using digital imagery and machine learning |
| US11482045B1 (en) | 2018-06-28 | 2022-10-25 | Amazon Technologies, Inc. | Associating events with actors using digital imagery and machine learning |
| KR102175947B1 (ko) * | 2019-04-19 | 2020-11-11 | 주식회사 아이유플러스 | 레이더 및 영상을 결합하여 차량용 3차원 장애물을 표시하는 방법 및 장치 |
| CN113874683A (zh) | 2019-05-28 | 2021-12-31 | 御眼视觉技术有限公司 | 用于车辆导航的***和方法 |
| KR102178585B1 (ko) * | 2019-07-02 | 2020-11-13 | 한국교통대학교산학협력단 | 주변 차량 자세 추정 시스템 |
| CN111402293B (zh) * | 2020-03-10 | 2023-11-14 | 北京邮电大学 | 面向智能交通的一种车辆跟踪方法及装置 |
| US11398094B1 (en) | 2020-04-06 | 2022-07-26 | Amazon Technologies, Inc. | Locally and globally locating actors by digital cameras and machine learning |
| US11443516B1 (en) * | 2020-04-06 | 2022-09-13 | Amazon Technologies, Inc. | Locally and globally locating actors by digital cameras and machine learning |
| CN118140158A (zh) * | 2021-10-12 | 2024-06-04 | 日产自动车株式会社 | 物体识别方法及物体识别装置 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001266160A (ja) | 2000-03-22 | 2001-09-28 | Toyota Motor Corp | 周辺認識方法および周辺認識装置 |
| JP2005061376A (ja) | 2003-08-20 | 2005-03-10 | Keihin Corp | 二輪自動車の出力制御装置 |
| JP2011070593A (ja) | 2009-09-28 | 2011-04-07 | Mitsubishi Motors Corp | 車両周辺監視装置 |
| JP2013093865A (ja) | 2012-12-07 | 2013-05-16 | Mitsubishi Motors Corp | 車両周辺監視装置 |
| JP2014026555A (ja) | 2012-07-30 | 2014-02-06 | Denso It Laboratory Inc | 移動体認識システム、移動体認識プログラム、及び移動体認識方法 |
| JP2017034623A (ja) | 2015-08-06 | 2017-02-09 | 株式会社日本自動車部品総合研究所 | ピッチ角推定装置 |
| JP2017052411A (ja) | 2015-09-09 | 2017-03-16 | 株式会社デンソー | 運転支援装置、及び運転支援方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS6192938A (ja) * | 1984-10-11 | 1986-05-10 | Mazda Motor Corp | 自動車の定速走行装置 |
-
2017
- 2017-07-19 JP JP2017139888A patent/JP7005978B2/ja active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2001266160A (ja) | 2000-03-22 | 2001-09-28 | Toyota Motor Corp | 周辺認識方法および周辺認識装置 |
| JP2005061376A (ja) | 2003-08-20 | 2005-03-10 | Keihin Corp | 二輪自動車の出力制御装置 |
| JP2011070593A (ja) | 2009-09-28 | 2011-04-07 | Mitsubishi Motors Corp | 車両周辺監視装置 |
| JP2014026555A (ja) | 2012-07-30 | 2014-02-06 | Denso It Laboratory Inc | 移動体認識システム、移動体認識プログラム、及び移動体認識方法 |
| JP2013093865A (ja) | 2012-12-07 | 2013-05-16 | Mitsubishi Motors Corp | 車両周辺監視装置 |
| JP2017034623A (ja) | 2015-08-06 | 2017-02-09 | 株式会社日本自動車部品総合研究所 | ピッチ角推定装置 |
| JP2017052411A (ja) | 2015-09-09 | 2017-03-16 | 株式会社デンソー | 運転支援装置、及び運転支援方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2019018743A (ja) | 2019-02-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP7005978B2 (ja) | 軌跡推定方法及び軌跡推定装置 | |
| KR102275310B1 (ko) | 자동차 주변의 장애물 검출 방법 | |
| CN108496178B (zh) | 用于估计未来路径的***和方法 | |
| JP5016022B2 (ja) | 多重３次元ワーピングに基づく物体運動検出システムおよび方法とこのようなシステムを備えた車両 | |
| JP6397934B2 (ja) | 走行制御装置 | |
| JP5023186B2 (ja) | ３ｄワーピング手法と固有対象物運動（ｐｏｍ）の検出の組み合わせに基づく対象物の動き検出システム | |
| US9734719B2 (en) | Method and apparatus for guiding a vehicle in the surroundings of an object | |
| CN110920609A (zh) | 用于模仿前车的***和方法 | |
| CN111164652B (zh) | 移动体图像生成记录显示装置和程序产品 | |
| JPWO2018131090A1 (ja) | 車両制御装置、車両制御方法、および車両制御プログラム | |
| JP2007300181A (ja) | 周辺認識装置、周辺認識方法、プログラム | |
| JP6984624B2 (ja) | 表示制御装置および表示制御プログラム | |
| JP7075833B2 (ja) | 物体検出方法及び物体検出装置 | |
| JP6939198B2 (ja) | 物体検出方法及び物体検出装置 | |
| JP7375576B2 (ja) | 車両制御装置 | |
| JP6595647B2 (ja) | 走行制御装置、車両および走行制御方法 | |
| JP2019064301A (ja) | 車両の制御装置及び車両の制御方法 | |
| JP7320184B2 (ja) | 電動車両 | |
| WO2020090320A1 (ja) | 情報処理装置、情報処理方法及び情報処理プログラム | |
| WO2020079927A1 (ja) | 情報処理装置、プログラム及び情報処理方法 | |
| JP2020064603A (ja) | 情報処理装置、プログラム及び情報処理方法 | |
| JP7357284B2 (ja) | 描画システム、表示システム、移動体、描画方法及びプログラム | |
| JP2024038527A (ja) | 信号処理装置、信号処理方法及び信号処理システム | |
| JP7379253B2 (ja) | 挙動推定システム及び挙動推定方法 | |
| JP2021047609A (ja) | 周辺認識装置、周辺認識方法、およびプログラム |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20190401 |
|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20200512 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20210428 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20210511 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20210623 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20211207 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20211220 |
|
| R151 | Written notification of patent or utility model registration |
Ref document number: 7005978 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |