対象物リスク予測装置
【課題】それぞれの他者(障害物)にインタラクション(相互干渉)の影響があることを考慮して、正確な移動変化予測に伴うリスク予測を行うことができる対象物リスク予測装置を提供することを課題とする。
【解決手段】本実施形態では、複数の他者(障害物)同士の将来予測位置(第1予測進路)が干渉(接近)する場合(複数の他者同士の将来予測位置が干渉するエリアを検出した場合)には、当該干渉の影響で当該将来予測位置から変更され得る複数の他者のそれぞれの将来予測位置(回避経路、第2予測進路)を算出し、当該将来予測位置に応じたリスク値を設定する。
【解決手段】本実施形態では、複数の他者(障害物)同士の将来予測位置(第1予測進路)が干渉(接近)する場合(複数の他者同士の将来予測位置が干渉するエリアを検出した場合)には、当該干渉の影響で当該将来予測位置から変更され得る複数の他者のそれぞれの将来予測位置(回避経路、第2予測進路)を算出し、当該将来予測位置に応じたリスク値を設定する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は対象物リスク予測装置に関する。
【背景技術】
【0002】
特許文献1および特許文献2には、異なる障害物同士が互いに干渉し合う進路を求め、この干渉し合う進路のうち確率的な予測を行った進路の予測確率を低下させ、この予測確率が低下した進路を含む複数の進路の各々が実現される確率を算出する障害物進路予測方法等および移動体進路取得方法等に関する技術が開示されている。これにより、個々の障害物単位で見た場合には高い確率で取り得る進路であっても、他の障害物と干渉することがあればその影響を考慮した進路予測を行うことができる。故に、複雑な交通環境下でも障害物の進路を適切に予測することが可能となる。
【0003】
なお、特許文献3には、時間短縮や滑らかさを優先した軌道と安全性を優先した軌道の2つの最適軌道を算出し続け、安全性基準を満たさない軌道を前者の軌道がとる場合には後者の軌道を選択する走行計画生成装置に関する技術が開示されている。また、特許文献4には、障害物の通常回避モードと、通常回避モードでは回避できないと判断した場合の緊急回避モードとを持ち、最適軌道を算出して車両制御を実施する車両用支援制御装置に関する技術が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開2008−123217号公報
【特許文献2】特開2009−064088号公報
【特許文献3】特開2009−051356号公報
【特許文献4】特開2007−253770号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
しかしながら、従来技術では、他者(障害物)同士の移動範囲の干渉により運動(移動)変化が起きることは予測しているものの、相互の他者同士の移動がどのよう変化するのかについては正確に予測していないので、移動変化後の双方の他者に応じて設定されるリスクマップ(リスク範囲)が作成(設定)できない可能性がある、という問題点があった。
【0006】
本発明は、上記の事情に鑑みてなされたものであって、それぞれの他者にインタラクション(相互干渉)の影響があることを考慮して、正確な移動変化予測に伴うリスク予測を行うことができる対象物リスク予測装置を提供することを目的とする。
【課題を解決するための手段】
【0007】
本発明にかかる対象物リスク予測装置は、車両周囲の対象物に関する情報を取得し、取得した情報から対象物の第1将来進路を予測し、予測した第1将来進路に応じたリスクを設定する対象物リスク予測装置において、予測した複数の対象物の第1将来進路同士が干渉する場合には、当該干渉の影響で当該第1将来進路から変更され得る当該複数の対象物それぞれの第2将来進路を予測し、予測した第2将来進路に応じたリスクを設定すること、を特徴とする。
【0008】
また、本発明にかかる対象物リスク予測装置は、複数の対象物の現在位置を結ぶ線分を長軸又は短軸に含み第1将来進路同士が干渉するエリアを含む楕円を作成し、作成した楕円をリスク範囲として設定すること、が好ましい。
【発明の効果】
【0009】
本発明によれば、予測した複数の対象物の第1将来進路同士が干渉する場合には、当該干渉の影響で当該第1将来進路から変更され得る当該複数の対象物それぞれの第2将来進路を予測し、予測した第2将来進路に応じたリスクを設定する。これにより、それぞれの他者(障害物)にインタラクション(相互干渉)の影響があることを考慮して、正確な移動変化予測に伴うリスク予測を行うことができるという効果を奏する。
【図面の簡単な説明】
【0010】
【図1】図1は、本実施形態の概要を示す図である。
【図2】図2は、本実施形態の対象物リスク予測装置等を含む車両の構成を示すブロック図である。
【図3】図3は、本実施形態の支援実施処理の一例を示すフローチャートである。
【図4】図4は、本実施形態の他者対将来位置予測処理の一例を示すフローチャートである。
【図5】図5は、他者間干渉度算定方法の一例を説明する図である。
【図6】図6は、他者間干渉領域算定方法の一例を説明する図である。
【図7】図7は、本実施形態の他者間将来位置予測処理の一例を示すフローチャートである。
【図8】図8は、干渉判定処理の一例を示すフローチャートである。
【図9】図9は、現時点での障害物の位置および速度の状態の一例を示す図である。
【図10】図10は、回避方向設定の一例を示す図である。
【図11】図11は、回避開始時間設定の一例を示す図である。
【図12】図12は、障害物が移動している方向にとった座標系の一例を示す図である。
【図13】図13は、障害物が移動している方向にとった座標系における当該障害物の回避経路の一例を示す図である。
【発明を実施するための形態】
【0011】
以下に、本発明にかかる対象物リスク予測装置の実施形態を図面に基づいて説明する。なお、本発明は本実施形態により限定されるものではない。
【0012】
[1.概要]
ここでは、本実施形態の概要について図1を参照して説明する。図1は、本実施形態の概要を示す図である。
【0013】
例えば、特開2007−253700号公報に開示されている従来技術では、自車走行状態と移動障害物の関係から、緊急回避すべき状態かどうかを判定し、周囲道路構造も含めた状態評価関数(例えば、リスクのポテンシャル)に対する最適制御軌道を算出して、車両制御を行う。また、特開2009−051356号公報に開示されている従来技術では、上位計画(旅行時間、滑らかさ等)を満たす下位計画(走行軌跡、速度プロファイル)と自車が安全に停止することを前提とした下位計画の2つを最適軌道として生成し、前者の下位計画が安全基準を満たさなくなった場合には後者の下位計画を選択する。
【0014】
しかし、従来技術では、図1に示すように、他者(移動障害物(車両、歩行者など))間同士の相互運動変化を考慮した他者の将来位置予測を行っていない。従って、他者間同士の相互運動変化が生じた場合には、適切な将来位置予測を行うことができず、それ故に、自車に対する危険度の演算や安全な回避経路の算出も適切に行うことができない。また、他者間の衝突可能性を考慮した将来位置予測では想定すべき挙動の組合せが多くなるため、衝突計算を行いながら移動可能領域を算定する従来技術では、計算量の甚大な増加により、実時間処理が長くなる等の問題がある。
【0015】
そこで、本実施形態では、自車周辺の2つ以上の他者間のインタラクション(相互干渉)に因りこれら他者の挙動軌跡(将来時間毎の存在位置)が変化すること(相互の行動変化)が予測される場合において、図1に示すように、他者間同士のインタラクションに因る相互の行動変化を考慮して各他者の将来位置を決定(予測)する。これにより、それぞれの他者にインタラクションの影響があると考慮して、正確な移動予測変化に伴うリスク予測を行うことができる。そして、本実施形態では、当該予測した各他者の将来位置に基づいて当該将来位置を考慮した危険度(衝突危険度)を演算し、演算した危険度に基づいて自車の安全な回避経路を生成し、当該回避経路に基づいて支援制御を実施する。
【0016】
ここで、他者間での干渉(衝突)が想定される場合には当該他者間で回避操作が行われることが想定されるが、自車にとっては衝突可能性のある他者間個別の将来位置を予測する必要があるわけでなく、単に、両他者を一対にした他者対の将来位置を予測すれば十分である。また、他者の将来位置の予測精度は、将来時間が長くなればなるほど高く維持できなくなる。そのため、緻密な計算手法を採用して当該予測の1サイクルの計算時間を長くするよりも、妥当な仮説に基づく計算手法を採用して当該予測の1サイクルの計算時間を短く効率よくしたほうが、最後に実施される支援制御における支援判断の精度向上に繋がることは明らかであると考える。
【0017】
そこで、本実施形態では、各他者の現在の挙動からの線形予測により他者間で挙動軌跡の重なりがある場合には、単に、両他者の現時刻での存在位置を結ぶ線分上での存在確率の広がりを大きくすることで、当該両他者の簡易的な将来位置を算定する。具体的には、本実施形態では、現時刻で衝突可能性のある他者対の干渉度合いをそれぞれの位置および速度を用いて算定し、干渉度合いが高いと判断された場合には、例えば、現時刻での両他者のそれぞれの位置を結ぶ線分を長軸または短軸とし当該両他者の最接近位置を包含する楕円の範囲を、他者対の将来位置として算定する。これにより、リスクを設定する際には他者間個別の回避進路を計算する必要がなくなり、その結果、計算量の減少に因り予測時間の刻みが小さなり、将来位置予測の計算の高速化が達成可能となる。また、干渉の影響に因る変更進路に応じたリスク計算を単純化することで、システム負荷を軽減することができる。
【0018】
[2.構成]
ここでは、本実施形態の対象物リスク予測装置等を含む車両の構成について図2を参照しながら説明する。図2は、本実施形態の対象物リスク予測装置等を含む車両の構成を示すブロック図である。
【0019】
図1において、符号1は、自車両の情報および当該自車両周辺の他者の情報を取得するための、当該自車両に搭載されるセンシング手段であり、符号2は、自車両のECU(電子制御ユニット)に組み込まれる状態検出手段であり、符号3は、自車両のECUに組み込まれる、本実施形態の対象物リスク予測装置を含む運動量算出手段であり、符号4は、自車両に搭載される制御手段である。
【0020】
符号1aは運動量センサであり、符号1bは可視光センサであり、符号1cは距離センサであり、符号1dは位置センサである。符号2aは自車状態検出部であり、符号2bは道路構造検出部であり、符号2cは可動物検出部である。符号3aは、本実施形態の対象物リスク予測装置に含まれる他者将来予測部であり、符号3bは、本実施形態の対象物リスク予測装置に含まれるリスク値算出部であり、符号3cは修正運動量算出部であり、符号3dは最適運動量算出部である。符号4aは操舵装置であり、符号4bは加減速装置であり、符号4cは自車両と当該自車両周辺の可動物とが干渉する場合に警告音などの音声を発生する音声発生装置であり、符号4dは操作反力発生装置である。
【0021】
自車状態検出部2aは、センシング手段1の各センサで検知した情報から自車状態(例えば自車両の位置ベクトルや速度ベクトルなど)を検出する。道路構造検出部2bは、センシング手段1の各センサで検知した情報から自車両が走行する道路構造を検出する。可動物検出部2cは、センシング手段1の各センサで検知した情報から自車両周辺の可動物(車両や歩行者など)に関する情報(例えば可動物の位置ベクトルや速度ベクトルなど)を検出する。
【0022】
他者将来予測部3aは、本実施形態の特長部分であり、道路構造検出部1bで検出した道路構造に関する情報および可動物検出部1cで検出した可動物に関する情報から、他者である可動物の将来位置を予測する。リスク値算出部3bは、他者将来予測部3aで予測した可動物の将来位置からリスク値を算出する。修正運動量算出部3cは、自車状態検出部2aで検出した自車状態に関する情報およびリスク値算出部3bで算出したリスク値から自車両の修正運動量を算出し、算出した修正運動量を制御手段4の各装置へ出力する。最適運動量算出部3dは、自車状態検出部2aで検出した自車状態に関する情報および修正運動量算出部3cで算出した修正運動量から自車両の最適運動量を算出し、算出した最適運動量を操舵装置4aおよび加減速装置4bへ出力する。
【0023】
[3.処理]
ここでは、上述した構成の車両(主にECU)にて行われる各種処理の一例について図3から図13を参照して説明する。
【0024】
[3−1.支援実施処理]
最初に、本実施形態の支援実施処理の一例について図3を参照して説明する。図3は、本実施形態の支援実施処理の一例を示すフローチャートである。
【0025】
まず、自車状態検出部2aは、自車両の位置ベクトルおよび速度ベクトルなどを含む自車状態情報を検出する(ステップSA1)。道路構造検出部2bは、自車両が走行する道路構造に関する道路構造情報を検出する(ステップSA1)。可動物検出部2cは、他者である可動物(車両や歩行者など)の幅、長さ、位置ベクトルおよび速度ベクトルなどを含む可動物情報を検出する(ステップSA1)。
【0026】
つぎに、他者将来位置予測部3aは、ステップSA1で検出した道路構造情報および可動物情報に基づいて他者の可動物の将来位置を予測し、この予測結果を他者将来位置情報としてリスク値算出部3bへ出力する(ステップSA2:他者将来位置予測処理)。なお、ステップSA2で行われる処理の具体的内容については、後述する[3−2.他者対将来位置予測処理]および[3−3.他者間将来位置予測処理]にて詳細に説明する。
【0027】
つぎに、リスク値算出部3bは、ステップSA2で予測した他者将来位置情報に基づいて、可動物の将来位置を考慮したリスク値(危険度)を算出する(ステップSA3)。
【0028】
つぎに、修正運動量算出部3cは、ステップSA1で検出した自車状態情報およびステップSA3で検出したリスク値に基づいて自車両の修正運動量を算出し、算出した修正運動量を制御手段4の各装置へ出力する(ステップSA4)。また、最適運動量算出部3dは、当該自車状態情報および当該算出した修正運動量に基づいて自車両の最適運動量を算出し、算出した最適運動量を操舵装置4aおよび加減速装置4bへ出力する(ステップSA4)。
【0029】
そして、操舵装置4aおよび加減速装置4bはそれぞれ、ステップSA4で出力された修正運動量および最適運動量に基づいてそれぞれのアクチュエータを駆動し、音声発生装置4cはステップSA4で出力された修正運動量から、必要に応じて干渉の警告のための音(音声)を発生し、操作反力発生装置4dはステップSA4で出力された修正運動量に基づいて、操作反力を発生させるためにアクチュエータを駆動する(ステップSA5)。
【0030】
これにて、支援実施処理の説明を終了する。
【0031】
[3−2.他者対将来位置予測処理]
次に、本実施形態の特長である他者対将来位置予測処理の一例について図4から図6を参照して詳細に説明する。図4は、本実施形態の他者対将来位置予測処理の一例を示すフローチャートである。
【0032】
まず、他者将来位置予測部3aは、自車両周辺にn個(nは2以上の整数)の他者(可動物、障害物)が検出された場合における2つの他者(他者対)の全ての組み合わせ(組み合わせ総数は、nC2(=n(n−1)/2))について、後述する他者間干渉度の算定が終了していない場合(ステップSB1:No)には、当該算定が終了していない他者対についての他者間干渉度を、各他者の現時点での位置ベクトルおよび速度ベクトルを用いて算定する(ステップSB2)。ここで、他者間干渉度の算定方法について図5を参照して説明する。図5は、他者間干渉度算定方法の一例を説明する図である。
【0033】
まず、幅W1で長さL1の他者O1の位置ベクトル(x1,y1)と速度ベクトル(vx1,vy1)および幅W2で長さL2の他者O2の位置ベクトル(x2,y2)と速度ベクトル(vx2,vy2)を用いて、現時点から時間t〔秒〕経過した時点での他者O1と他者O2間の距離D12(t)は以下の式(1)で表される。
D12(t)2
={(x1−x2)+t(vx1−vx2)}2+{(y1−y2)+t(vy1−vy2)}2
・・・(1)
【0034】
つぎに、式(1)を、係数A、BおよびCを用いて以下の式(2)に変形する。
D12(t)2
=At2+Bt+C
=A{t+(B/2A)}2+C−(B2/4A) ・・・(2)
【0035】
式(2)より、D12(t)の最小値Dminは、以下のように表される。
B/2A≧0の場合:Dmin=C1/2
B/2A<0の場合:Dmin={C−(B2/4A)}1/2
【0036】
そこで、最小値Dminを他者間干渉度と定義し、当該最小値Dminが閾値εlimit(=max(W1,L1)+max(W2,L2)+δ)以下の場合には、他者間干渉度が高い(大きい)と判定(判断)する。
【0037】
図4に戻り、他者将来位置予測部3aは、ステップSB2で他者間干渉度が大きいと判定された場合(ステップSB3:Yes)には、当該判定された他者対についての他者間干渉領域を、各他者の現時点での位置ベクトルおよび速度ベクトルを用いて算定する(ステップSB4)。ここで、他者間干渉領域の算定方法について図6を参照して説明する。図6は、他者間干渉領域算定方法の一例を説明する図である。
【0038】
まず、現時点での他者O1と他者O2とを通過する直線L1は、以下の式(3)で表され、また、現時点での他者O1と他者O2との中点((x1+x2)/2,(y1+y2)/2)を通過し直線L1に直交する直線L2は、以下の式(4)で表される。
L1:(x1−x2)y−(y1−y2)x+x1(y1−y2)−y1(x1−x2)=0
・・・(3)
L2:(y1−y2)y+(x1−x2)x−{(x1+x2)/2}(x1−x2)−{(y1+y2)/2}(y1−y2)=0 ・・・(4)
【0039】
つぎに、直線L1および直線L2を座標軸とし前記中点を原点とするL1−L2直交座標系を設定すると、当該座標系における楕円の方程式(標準形)は、以下の式(5)で表される。
L12/Q2+L22/S2=1 ・・・(5)
【0040】
ここで、係数Qは、以下の式(6)で決定する。
Q=D12(0)/2+max(max(W1,L1),max(W2,L2))
・・・(6)
【0041】
また、係数Sは、以下の方法で決定する。まず、他者O1と他者O2が現時点からそれぞれの速度ベクトルの方向に沿った線上を進んで、時間tZ(=−B/2A)経過した時点での他者O1と他者O2の存在位置Z1、Z2(最接近時の存在位置)をそれぞれ、図6に示すように、Z1(x1+tZvxl,y1+tZvy1)、Z2(x2+tZvx2,y2+tZvy2)とする。つぎに、他者O1と他者O2がそれぞれの存在位置Z1、Z2からそれぞれの進行方向に沿ってL1/2、L2/2の長さ分移動し、さらに当該進行方向に直交する方向であって前記中点から遠ざかる方向にW1/2、W2/2の長さ分移動した時点での他者O1と他者O2の存在位置Z1’、Z2’を算定する。そして、各Z1’、Z2’に対して係数Sを算定し、算定したうちの大きな方を係数Sとして選択する。
【0042】
そして、ステップSB4では、係数Q、Sが決定した楕円で囲まれる範囲を、他者間干渉領域として算定する。
【0043】
図4に戻り、他者将来位置予測部3aは、ステップSB4で算定した他者間干渉領域内に自車両が存在しない場合(ステップSB5:No)には、当該他者間干渉領域内に前記他者対が時間に関係なく一様に存在すると仮定して、当該両他者に対して存在確率を設定する(ステップSB6)。
【0044】
つぎに、他者将来位置予測部3aは、自車両周辺にn個の他者が検出された場合における他者対の全ての組み合わせについて上述したステップSB2からステップSB6までの処理を繰り返し、他者対の全ての組み合わせについて上述した他者間干渉度の算定が終了した場合(ステップSB1:Yes)には、存在確率が設定されていない他者に対して、後述する[3−3.他者間将来位置予測処理]にて詳細に説明する追加処理(他者間将来位置予測処理)を行って他者の将来位置(将来時間毎の存在位置)を予測し(ステップSB7)、当該予測結果を他者将来位置情報として、他者間干渉領域と共にリスク値算出部3bへ出力する(ステップSB8)。
【0045】
これにて、他者対将来位置予測処理の説明を終了する。
【0046】
[3−3.他者間将来位置予測処理]
次に、本実施形態の特長である他者間将来位置予測処理の一例について図7から図13を参照して詳細に説明する。図7は、本実施形態の他者間将来位置予測処理の一例を示すフローチャートである。なお、ここでは、他者を障害物として説明する。
【0047】
まず、他者将来位置予測部3aは、自車両周辺の障害物(上述した「存在確率が定義されていない他者」)を認識し、認識した障害物の総数Nを設定する(ステップSC1)。
【0048】
そして、他者将来位置予測部3aは、1からNまでの整数値を取る変数iと、i+1からNまでの整数値を取る変数jとを設定し、変数iを固定して変数jを変化させながら障害物iと障害物jとの全ての組み合わせ(組み合わせ総数は、N(N−1)/2)に対して、以下のステップSC2からステップSC15までの処理を繰り返す。
【0049】
まず、他者将来位置予測部3aは、障害物iが移動障害物である場合(ステップSC2:Yes)には、現時点での障害物iと障害物jの位置ベクトルおよび速度ベクトルを用いて、障害物iと障害物jとが将来干渉するか否かを判定する(ステップSC3:干渉判定処理)。ここで、干渉判定処理の一例について図8および図9を参照して説明する。図8は、本実施形態の干渉判定処理の一例を示すフローチャートである。図9は、現時刻での障害物の位置および速度の状態の一例を示す図である。
【0050】
本干渉判定処理では、現時点での障害物iと障害物jの位置ベクトルおよび速度ベクトルを用いて、障害物iと障害物jとが、現時点から所定の予測時間T〔秒〕(例えば5〔秒〕など)先の時点までの間に干渉するか否かを判定する。具体的には、他者将来位置予測部3aは、0からTまでの値(例えば整数値や小数値など)を取る変数tを設定し、設定した変数tの値を変化させながら、以下のステップSD1からステップSD5までの処理を、以下で説明する干渉フラグに「干渉あり」を表す「1」がセットされるまで繰り返し実行する。
【0051】
まず、他者将来位置予測部3aは、現時点での障害物iの位置ベクトル(xi_0,yi_0)と速度ベクトル(vxi_0,vyi_0)から(図9参照)、変数tの時点での障害物iの将来位置ベクトル(xi(t),yi(t))(=(xi_0+vxi_0×t,yi_0+vyi_0×t))を算出し、また、現時点での障害物jの位置ベクトル(xj_0,yj_0)と速度ベクトル(vxj_0,vyj_0)から(図9参照)、変数tの時点での障害物jの将来位置ベクトル(xj(t),yj(t))(=(xj_0+vxj_0×t,yj_0+vyj_0×t))を算出し、算出した2つの将来位置ベクトルを用いて、変数tの時点での障害物iと障害物jとの間の将来距離Lij(t)(={(xi(t)−xj(t))2+(yi(t)−yj(t))2}1/2)を算出する(ステップSD1)。
【0052】
つぎに、他者将来位置予測部3aは、障害物iと障害物jとの干渉の状態を管理するための干渉フラグ(例えば、0:干渉なし、1:干渉あり)に「干渉なし」を表す「0」をセットして、当該干渉フラグを初期化する(ステップSD2)。
【0053】
つぎに、他者将来位置予測部3aは、ステップSD1で算出した将来距離Lij(t)が障害物iの大きさriと障害物jの大きさrjとの和(=ri+rj)以下である場合(ステップSD3:Yes)には、変数tの時点に障害物iと障害物jとが干渉すると予測し、干渉フラグに「干渉あり」を表す「1」をセットして(ステップSD4)、本処理を終了する(ステップSD5)。
【0054】
これにて、干渉判定処理の説明を終了する。
【0055】
図7に戻り、他者将来位置予測部3aは、干渉フラグが「干渉あり」にセットされていた場合(ステップSC4:Yes)には、上述した干渉判定処理において「干渉あり」と判定されたときの変数tの値を、障害物iと障害物jの干渉時間Tg〔秒〕として設定する(ステップSC5)。
【0056】
つぎに、他者将来位置予測部3aは、障害物iと障害物jのそれぞれに、所定の回避方向(例えば右方向、左方向など)を設定する(ステップSD6)。ここで、回避方向を以下の方法1.または2.で予測し、当該予測した回避方向を設定してもよい。これにより、より精度の高い回避方向の判定が実施可能となる。
【0057】
1.干渉発生時における干渉される方向を算出し、干渉されない方向を回避方向として設定する。例えば図10に示すように、障害物iにおいて、干渉点での干渉される相手の障害物jの進入方向(図10の例では右側方向)を算出し、進入方向でない方向(図10の例では左側方向)を回避方向として設定する。
2.干渉する2つの障害物の位置、速度および周囲道路との位置関係から回避方向を予測し、当該予測した回避方向を設定する。例えば、障害物の移動可能路(走路)より、干渉発生時における移動可能方向が多い方向(例えば歩道が広い側)を回避方向として設定する。
【0058】
図7に戻り、他者将来位置予測部3aは、ステップSC5で設定した干渉時間Tgに起こると想定した干渉(衝突)を回避するために障害物iおよび障害物jが回避し始める回避開始時間Ts〔秒〕として、所定の値(例えば1.0〔秒〕など)を設定する(ステップSC7)。ここで、回避開始時間Tsを、例えば、干渉発生時点における干渉障害物(障害物iおよび障害物j)の干渉角度(図11参照)および/または干渉障害物の属性(例えば車両、歩行者など)に応じて可変としてもよい。
【0059】
図7に戻り、他者将来位置予測部3aは、干渉時に障害物iを横方向に回避させる回避距離Ai〔m〕として、所定の値(例えば歩行者の場合は0.5〔m〕など)を設定する(ステップSC8)。ここで、回避距離Aiを、例えば干渉障害物の属性(例えば車両、歩行者など)の幅や、干渉時Tgにおける干渉障害物間の距離Lij(Tg)(={(xi(Tg)−xj(Tg))2+(yi(Tg)−yj(Tg))2}1/2)に応じて可変としてもよい。
【0060】
つぎに、他者将来位置予測部3aは、ステップSC5からステップSC8までで設定した各情報に基づいて、障害物iが移動している方向にとった直交座標系(xi−yi座標系)での障害物iの回避経路(xi_new_pre(t),yi_new_pre(t))(図13に示す太線の経路)を算出する(ステップSC9:図12、図13参照)。ここで、xi_new_pre(t)およびyi_new_pre(t)は、以下の様に表される。
【0061】
xi_new_pre(t)
=xi_0+vi_0×t (0≦t<Tg−Ts)
={xi_0+vi_0×(Tg−Ts)}+(vi_0×cosφi)×{t−(Tg−Ts)}
(Tg−Ts≦t<Tg)
=〔xi_0+vi_0×{Tg−(1−cosφi)Ts}〕+vi_0×(t−Tg)
(Tg≦t)
yi_new_pre(t)
=0 (0≦t<Tg−Ts)
=(vi_0×sinφi)×{t−(Tg−Ts) (Tg−Ts≦t<Tg)
=Ai (Tg≦t)
※φi=sin−1{Ai/vi_0×Ts}
【0062】
つぎに、他者将来位置予測部3aは、ステップSC9で算出した回避経路(xi_new_pre(t),yi_new_pre(t))を以下の行列演算に従って座標変換することで、自車両からみた直交座標系(x−y座標系)での障害物iの回避経路(xi_new(t),yi_new(t))(予測位置平均ベクトル)を算出する(ステップSC10)。ここで、xi_new(t)およびyi_new(t)は、以下の様に表される。
【0063】
[xi_new(t);yi_new(t);1]
=[cosθi_0 −sinθi_0 xi_0;sinθi_0 cosθi_0 yi_0;0 0 1]×[xi_new_pre(t);yi_new_pre(t);1]
※[]は行列を表す記号である。;は行列の各行を区切る記号である。記号;で区切られた間に記述された文字(数字)または文字列(数字列)は、行列内の1つの行を構成する列要素である。
【0064】
つぎに、他者将来位置予測部3aは、ステップSC10で算出した障害物iの回避経路を用いて、障害物iの回避経路に関する共分散行列(X軸方向:Σi_x(t)、Y軸方向:Σi_y(t))(予測位置共分散行列)を、以下の行列演算に従って算出する(ステップSC11)。
【0065】
Σi_x(t)
=[σi_x(t)2 σi_xu(t);σi_ux(t) σi_u(t)2]
Σi_y(t)
=[σi_y(t)2 σi_yv(t);σi_vy(t) σi_v(t)2]
Fi(t)
=[1 Δt 0 0;0 1 0 0;0 0 1 Δt;0 0 0 1]
Σi_x(t+Δt)=Fi(t)×Σi_x(t)×inv(Fi(t))
Σi_y(t+Δt)=Fi(t)×Σi_y(t)×inv(Fi(t))
※分散σi_x(0)2およびσi_u(0)2は、ある一定定数である。共分散σi_xu(0)およびσi_ux(0)は、0である。分散σi_y(0)2およびσi_v(0)2は、ある一定定数である。共分散σi_yv(0)およびσi_vy(0)は、0である。
【0066】
ここで、ステップSC11において、共分散行列(分散)の値を、「干渉あり」と「干渉なし」とで可変としてもよい。具体的には、干渉あり時において、通常時(干渉なし時)と比較して分散(ばらつき)を大きくしてもよい。より具体的には、前記Fi(t)を、分散の広がりを決定する係数aが設定された以下のものに変更し、「干渉あり」と判定された場合と「干渉なし」と判定された場合とで係数aの値を切り替えてもよい。例えば、「干渉あり」と判定された場合には、係数aの値を「干渉なし」と判定された場合の値より大きくし、「干渉なし」と判定された場合には、係数aの値を「干渉あり」と判定された場合の値より小さくしてもよい。これにより、予測と実際とで回避タイミングや回避方向が異なってしまった場合でも、ある程度の支援制御を実施することが可能となる。
Fi(t)
=[1 a×Δt 0 0;0 1 0 0;0 0 1 a×Δt;0 0 0 1]
【0067】
なお、他者将来位置予測部3aは、干渉フラグが「干渉あり」にセットされていなかった場合(ステップSC4:No)には、現時点での障害物iと障害物jの位置ベクトルおよび速度ベクトルを用いて、障害物iの回避経路(xi_new(t),yi_new(t))(予測位置平均ベクトル)、および上述同様の演算に基づく障害物iの共分散行列(予測位置共分散行列)を算出する(ステップSC12、ステップSC13)。ここで、「干渉なし」の場合、障害物同士は回避行動を取らずに移動するので、xi_new(t)およびyi_new(t)は、以下の様に算定される。
【0068】
[xi_new(t);yi_new(t);1]
=[cosθi_0 −sinθi_0 xi_0;sinθi_0 cosθi_0 yi_0;0 0 1]×[xi_0+vi_0×t;0;1]
【0069】
また、他者将来位置予測部3aは、障害物iが移動障害物でない場合(ステップSC2:No)には、現時点での障害物iと障害物jの位置ベクトルを用いて、上述同様に、障害物iの回避経路(予測位置平均ベクトル)および障害物iの回避経路に関する共分散行列(予測位置共分散行列)を算出する(ステップSC14、ステップSC15)。
【0070】
これにて、他者間将来位置予測処理の説明を終了する。
【0071】
[4.本実施形態のまとめ]
以上説明したように、本実施形態では、複数の他者(障害物)同士の将来予測位置(第1予測進路)が干渉(接近)する場合(複数の他者同士の将来予測位置が干渉するエリアを検出した場合)には、当該干渉の影響で当該将来予測位置から変更され得る複数の他者のそれぞれの将来予測位置(回避経路、第2予測進路)を算出し、当該将来予測位置に応じたリスク値を設定する。これにより、それぞれの他者にインタラクションの影響があると考慮して、正確な移動予測変化に伴うリスク予測を行うことができる。
【0072】
また、本実施形態では、複数の他者同士の第1予測進路の干渉エリア(最接近位置)を算出し、複数の他者の現在位置を結ぶ線分を長軸または短軸とし当該干渉エリアを含む楕円を作成し、作成した楕円をリスク範囲として設定する。これにより、リスクを設定する際には個別の回避進路を計算する必要がなくなるので、干渉の影響による進路変更リスクを単純な計算で行なうことで、システム負荷を軽減させることができる。なお、干渉度合いの大きさを算出し、算出した干渉度合いの大きさに応じて楕円の大きさを変更してもよい。
【0073】
また、本実施形態では、第1予測進路から第2予測進路に回避(変更)し始めるタイミング(時間)を、他者が持つ情報(例えば干渉角度や属性など)に応じて設定してもよい。通常、回避開始時間は状況に応じて異なる。しかし、一定の回避開始時間を設定して回避経路を予測した場合、当該回避開始時間と他者の実際の回避開始時間との乖離が大きいと、回避経路の予測精度が低下して最適な支援制御が実施できない(例えば警報やブレーキなどが最適な場面で作動できない等)可能性がある。そこで、本実施形態では、回避開始時間を他者が持つ情報に応じて設定するので、実際の回避開始時間との乖離が大きくならず、結果として、回避経路の予測精度を低下させずに最適な支援制御を実施することができる。
【0074】
また、本実施形態では、第1予測進路から第2予測進路への回避距離(第1予測進路方向に対する横方向の移動距離)を、他者が持つ情報(例えば属性や干渉時における干渉距離など)に応じて設定してもよい。通常、回避距離は状況に応じて異なる。しかし、一定の回避距離を設定して回避経路を予測した場合、当該回避距離と他者の実際の回避距離との乖離が大きいと、回避経路の予測精度が低下して最適な支援制御が実施できない(例えば警報やブレーキなどが最適な場面で作動できない等)可能性がある。そこで、本実施形態では、回避距離を他者が持つ情報に応じて設定するので、実際の回避距離との乖離が大きくならず、結果として、回避経路の予測精度を低下させずに最適な支援制御を実施することができる。
【0075】
また、本実施形態では、他者の回避方向を、他者同士の干渉点への進入方向および/または他者同士の干渉点通過時の交通環境に応じて設定してもよい。通常、回避方向は状況に応じて異なる。しかし、一定の回避方向を常に仮定して回避経路を予測した場合、当該回避方向と他者の実際の回避方向が逆になると、回避経路の予測精度が低下して最適な支援制御が実施できない(例えば警報やブレーキなどが最適な場面で作動できない等)可能性がある。そこで、本実施形態では、回避方向を上述した進入方向や交通環境に応じて設定するので、実際の回避方向と逆になることなく、結果として、回避経路の予測精度を低下させずに最適な支援制御を実施することができる。
【0076】
また、本実施形態では、上述した共分散行列の値を「干渉あり」時と「干渉なし」時で切り換えてもよい。他者同士の干渉が発生する場面において回避開始タイミングや回避方向を想定した将来位置予測を実施する場合、実際の回避開始タイミングや回避方向を正確に予測することは難しい。そのため、当該想定が実際とは大きく異なると、回避経路の予測精度が低下して最適な支援制御が実施できない(例えば警報やブレーキなどが最適な場面で作動できない等)可能性がある。そこで、本実施形態では、干渉時において、通常時(干渉が発生しない場合)と比較して分散(ばらつき)を大きくすることで、想定と実際とで回避開始タイミングや回避方向が異なってしまった場合でも、ある程度適切な支援制御を実施することが可能となる。
【産業上の利用可能性】
【0077】
以上のように、本発明にかかる対象物リスク予測装置は、自動車製造産業において有用であり、特に、対象物の予測進路に応じたリスク設定を行うための利用に適している。
【符号の説明】
【0078】
2 状態検出手段
2b 道路構造検出部
2c 可動物検出部
3 運動量算出手段
3a 他者将来位置予測部
3b リスク値算出部
【技術分野】
【0001】
本発明は対象物リスク予測装置に関する。
【背景技術】
【0002】
特許文献1および特許文献2には、異なる障害物同士が互いに干渉し合う進路を求め、この干渉し合う進路のうち確率的な予測を行った進路の予測確率を低下させ、この予測確率が低下した進路を含む複数の進路の各々が実現される確率を算出する障害物進路予測方法等および移動体進路取得方法等に関する技術が開示されている。これにより、個々の障害物単位で見た場合には高い確率で取り得る進路であっても、他の障害物と干渉することがあればその影響を考慮した進路予測を行うことができる。故に、複雑な交通環境下でも障害物の進路を適切に予測することが可能となる。
【0003】
なお、特許文献3には、時間短縮や滑らかさを優先した軌道と安全性を優先した軌道の2つの最適軌道を算出し続け、安全性基準を満たさない軌道を前者の軌道がとる場合には後者の軌道を選択する走行計画生成装置に関する技術が開示されている。また、特許文献4には、障害物の通常回避モードと、通常回避モードでは回避できないと判断した場合の緊急回避モードとを持ち、最適軌道を算出して車両制御を実施する車両用支援制御装置に関する技術が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開2008−123217号公報
【特許文献2】特開2009−064088号公報
【特許文献3】特開2009−051356号公報
【特許文献4】特開2007−253770号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
しかしながら、従来技術では、他者(障害物)同士の移動範囲の干渉により運動(移動)変化が起きることは予測しているものの、相互の他者同士の移動がどのよう変化するのかについては正確に予測していないので、移動変化後の双方の他者に応じて設定されるリスクマップ(リスク範囲)が作成(設定)できない可能性がある、という問題点があった。
【0006】
本発明は、上記の事情に鑑みてなされたものであって、それぞれの他者にインタラクション(相互干渉)の影響があることを考慮して、正確な移動変化予測に伴うリスク予測を行うことができる対象物リスク予測装置を提供することを目的とする。
【課題を解決するための手段】
【0007】
本発明にかかる対象物リスク予測装置は、車両周囲の対象物に関する情報を取得し、取得した情報から対象物の第1将来進路を予測し、予測した第1将来進路に応じたリスクを設定する対象物リスク予測装置において、予測した複数の対象物の第1将来進路同士が干渉する場合には、当該干渉の影響で当該第1将来進路から変更され得る当該複数の対象物それぞれの第2将来進路を予測し、予測した第2将来進路に応じたリスクを設定すること、を特徴とする。
【0008】
また、本発明にかかる対象物リスク予測装置は、複数の対象物の現在位置を結ぶ線分を長軸又は短軸に含み第1将来進路同士が干渉するエリアを含む楕円を作成し、作成した楕円をリスク範囲として設定すること、が好ましい。
【発明の効果】
【0009】
本発明によれば、予測した複数の対象物の第1将来進路同士が干渉する場合には、当該干渉の影響で当該第1将来進路から変更され得る当該複数の対象物それぞれの第2将来進路を予測し、予測した第2将来進路に応じたリスクを設定する。これにより、それぞれの他者(障害物)にインタラクション(相互干渉)の影響があることを考慮して、正確な移動変化予測に伴うリスク予測を行うことができるという効果を奏する。
【図面の簡単な説明】
【0010】
【図1】図1は、本実施形態の概要を示す図である。
【図2】図2は、本実施形態の対象物リスク予測装置等を含む車両の構成を示すブロック図である。
【図3】図3は、本実施形態の支援実施処理の一例を示すフローチャートである。
【図4】図4は、本実施形態の他者対将来位置予測処理の一例を示すフローチャートである。
【図5】図5は、他者間干渉度算定方法の一例を説明する図である。
【図6】図6は、他者間干渉領域算定方法の一例を説明する図である。
【図7】図7は、本実施形態の他者間将来位置予測処理の一例を示すフローチャートである。
【図8】図8は、干渉判定処理の一例を示すフローチャートである。
【図9】図9は、現時点での障害物の位置および速度の状態の一例を示す図である。
【図10】図10は、回避方向設定の一例を示す図である。
【図11】図11は、回避開始時間設定の一例を示す図である。
【図12】図12は、障害物が移動している方向にとった座標系の一例を示す図である。
【図13】図13は、障害物が移動している方向にとった座標系における当該障害物の回避経路の一例を示す図である。
【発明を実施するための形態】
【0011】
以下に、本発明にかかる対象物リスク予測装置の実施形態を図面に基づいて説明する。なお、本発明は本実施形態により限定されるものではない。
【0012】
[1.概要]
ここでは、本実施形態の概要について図1を参照して説明する。図1は、本実施形態の概要を示す図である。
【0013】
例えば、特開2007−253700号公報に開示されている従来技術では、自車走行状態と移動障害物の関係から、緊急回避すべき状態かどうかを判定し、周囲道路構造も含めた状態評価関数(例えば、リスクのポテンシャル)に対する最適制御軌道を算出して、車両制御を行う。また、特開2009−051356号公報に開示されている従来技術では、上位計画(旅行時間、滑らかさ等)を満たす下位計画(走行軌跡、速度プロファイル)と自車が安全に停止することを前提とした下位計画の2つを最適軌道として生成し、前者の下位計画が安全基準を満たさなくなった場合には後者の下位計画を選択する。
【0014】
しかし、従来技術では、図1に示すように、他者(移動障害物(車両、歩行者など))間同士の相互運動変化を考慮した他者の将来位置予測を行っていない。従って、他者間同士の相互運動変化が生じた場合には、適切な将来位置予測を行うことができず、それ故に、自車に対する危険度の演算や安全な回避経路の算出も適切に行うことができない。また、他者間の衝突可能性を考慮した将来位置予測では想定すべき挙動の組合せが多くなるため、衝突計算を行いながら移動可能領域を算定する従来技術では、計算量の甚大な増加により、実時間処理が長くなる等の問題がある。
【0015】
そこで、本実施形態では、自車周辺の2つ以上の他者間のインタラクション(相互干渉)に因りこれら他者の挙動軌跡(将来時間毎の存在位置)が変化すること(相互の行動変化)が予測される場合において、図1に示すように、他者間同士のインタラクションに因る相互の行動変化を考慮して各他者の将来位置を決定(予測)する。これにより、それぞれの他者にインタラクションの影響があると考慮して、正確な移動予測変化に伴うリスク予測を行うことができる。そして、本実施形態では、当該予測した各他者の将来位置に基づいて当該将来位置を考慮した危険度(衝突危険度)を演算し、演算した危険度に基づいて自車の安全な回避経路を生成し、当該回避経路に基づいて支援制御を実施する。
【0016】
ここで、他者間での干渉(衝突)が想定される場合には当該他者間で回避操作が行われることが想定されるが、自車にとっては衝突可能性のある他者間個別の将来位置を予測する必要があるわけでなく、単に、両他者を一対にした他者対の将来位置を予測すれば十分である。また、他者の将来位置の予測精度は、将来時間が長くなればなるほど高く維持できなくなる。そのため、緻密な計算手法を採用して当該予測の1サイクルの計算時間を長くするよりも、妥当な仮説に基づく計算手法を採用して当該予測の1サイクルの計算時間を短く効率よくしたほうが、最後に実施される支援制御における支援判断の精度向上に繋がることは明らかであると考える。
【0017】
そこで、本実施形態では、各他者の現在の挙動からの線形予測により他者間で挙動軌跡の重なりがある場合には、単に、両他者の現時刻での存在位置を結ぶ線分上での存在確率の広がりを大きくすることで、当該両他者の簡易的な将来位置を算定する。具体的には、本実施形態では、現時刻で衝突可能性のある他者対の干渉度合いをそれぞれの位置および速度を用いて算定し、干渉度合いが高いと判断された場合には、例えば、現時刻での両他者のそれぞれの位置を結ぶ線分を長軸または短軸とし当該両他者の最接近位置を包含する楕円の範囲を、他者対の将来位置として算定する。これにより、リスクを設定する際には他者間個別の回避進路を計算する必要がなくなり、その結果、計算量の減少に因り予測時間の刻みが小さなり、将来位置予測の計算の高速化が達成可能となる。また、干渉の影響に因る変更進路に応じたリスク計算を単純化することで、システム負荷を軽減することができる。
【0018】
[2.構成]
ここでは、本実施形態の対象物リスク予測装置等を含む車両の構成について図2を参照しながら説明する。図2は、本実施形態の対象物リスク予測装置等を含む車両の構成を示すブロック図である。
【0019】
図1において、符号1は、自車両の情報および当該自車両周辺の他者の情報を取得するための、当該自車両に搭載されるセンシング手段であり、符号2は、自車両のECU(電子制御ユニット)に組み込まれる状態検出手段であり、符号3は、自車両のECUに組み込まれる、本実施形態の対象物リスク予測装置を含む運動量算出手段であり、符号4は、自車両に搭載される制御手段である。
【0020】
符号1aは運動量センサであり、符号1bは可視光センサであり、符号1cは距離センサであり、符号1dは位置センサである。符号2aは自車状態検出部であり、符号2bは道路構造検出部であり、符号2cは可動物検出部である。符号3aは、本実施形態の対象物リスク予測装置に含まれる他者将来予測部であり、符号3bは、本実施形態の対象物リスク予測装置に含まれるリスク値算出部であり、符号3cは修正運動量算出部であり、符号3dは最適運動量算出部である。符号4aは操舵装置であり、符号4bは加減速装置であり、符号4cは自車両と当該自車両周辺の可動物とが干渉する場合に警告音などの音声を発生する音声発生装置であり、符号4dは操作反力発生装置である。
【0021】
自車状態検出部2aは、センシング手段1の各センサで検知した情報から自車状態(例えば自車両の位置ベクトルや速度ベクトルなど)を検出する。道路構造検出部2bは、センシング手段1の各センサで検知した情報から自車両が走行する道路構造を検出する。可動物検出部2cは、センシング手段1の各センサで検知した情報から自車両周辺の可動物(車両や歩行者など)に関する情報(例えば可動物の位置ベクトルや速度ベクトルなど)を検出する。
【0022】
他者将来予測部3aは、本実施形態の特長部分であり、道路構造検出部1bで検出した道路構造に関する情報および可動物検出部1cで検出した可動物に関する情報から、他者である可動物の将来位置を予測する。リスク値算出部3bは、他者将来予測部3aで予測した可動物の将来位置からリスク値を算出する。修正運動量算出部3cは、自車状態検出部2aで検出した自車状態に関する情報およびリスク値算出部3bで算出したリスク値から自車両の修正運動量を算出し、算出した修正運動量を制御手段4の各装置へ出力する。最適運動量算出部3dは、自車状態検出部2aで検出した自車状態に関する情報および修正運動量算出部3cで算出した修正運動量から自車両の最適運動量を算出し、算出した最適運動量を操舵装置4aおよび加減速装置4bへ出力する。
【0023】
[3.処理]
ここでは、上述した構成の車両(主にECU)にて行われる各種処理の一例について図3から図13を参照して説明する。
【0024】
[3−1.支援実施処理]
最初に、本実施形態の支援実施処理の一例について図3を参照して説明する。図3は、本実施形態の支援実施処理の一例を示すフローチャートである。
【0025】
まず、自車状態検出部2aは、自車両の位置ベクトルおよび速度ベクトルなどを含む自車状態情報を検出する(ステップSA1)。道路構造検出部2bは、自車両が走行する道路構造に関する道路構造情報を検出する(ステップSA1)。可動物検出部2cは、他者である可動物(車両や歩行者など)の幅、長さ、位置ベクトルおよび速度ベクトルなどを含む可動物情報を検出する(ステップSA1)。
【0026】
つぎに、他者将来位置予測部3aは、ステップSA1で検出した道路構造情報および可動物情報に基づいて他者の可動物の将来位置を予測し、この予測結果を他者将来位置情報としてリスク値算出部3bへ出力する(ステップSA2:他者将来位置予測処理)。なお、ステップSA2で行われる処理の具体的内容については、後述する[3−2.他者対将来位置予測処理]および[3−3.他者間将来位置予測処理]にて詳細に説明する。
【0027】
つぎに、リスク値算出部3bは、ステップSA2で予測した他者将来位置情報に基づいて、可動物の将来位置を考慮したリスク値(危険度)を算出する(ステップSA3)。
【0028】
つぎに、修正運動量算出部3cは、ステップSA1で検出した自車状態情報およびステップSA3で検出したリスク値に基づいて自車両の修正運動量を算出し、算出した修正運動量を制御手段4の各装置へ出力する(ステップSA4)。また、最適運動量算出部3dは、当該自車状態情報および当該算出した修正運動量に基づいて自車両の最適運動量を算出し、算出した最適運動量を操舵装置4aおよび加減速装置4bへ出力する(ステップSA4)。
【0029】
そして、操舵装置4aおよび加減速装置4bはそれぞれ、ステップSA4で出力された修正運動量および最適運動量に基づいてそれぞれのアクチュエータを駆動し、音声発生装置4cはステップSA4で出力された修正運動量から、必要に応じて干渉の警告のための音(音声)を発生し、操作反力発生装置4dはステップSA4で出力された修正運動量に基づいて、操作反力を発生させるためにアクチュエータを駆動する(ステップSA5)。
【0030】
これにて、支援実施処理の説明を終了する。
【0031】
[3−2.他者対将来位置予測処理]
次に、本実施形態の特長である他者対将来位置予測処理の一例について図4から図6を参照して詳細に説明する。図4は、本実施形態の他者対将来位置予測処理の一例を示すフローチャートである。
【0032】
まず、他者将来位置予測部3aは、自車両周辺にn個(nは2以上の整数)の他者(可動物、障害物)が検出された場合における2つの他者(他者対)の全ての組み合わせ(組み合わせ総数は、nC2(=n(n−1)/2))について、後述する他者間干渉度の算定が終了していない場合(ステップSB1:No)には、当該算定が終了していない他者対についての他者間干渉度を、各他者の現時点での位置ベクトルおよび速度ベクトルを用いて算定する(ステップSB2)。ここで、他者間干渉度の算定方法について図5を参照して説明する。図5は、他者間干渉度算定方法の一例を説明する図である。
【0033】
まず、幅W1で長さL1の他者O1の位置ベクトル(x1,y1)と速度ベクトル(vx1,vy1)および幅W2で長さL2の他者O2の位置ベクトル(x2,y2)と速度ベクトル(vx2,vy2)を用いて、現時点から時間t〔秒〕経過した時点での他者O1と他者O2間の距離D12(t)は以下の式(1)で表される。
D12(t)2
={(x1−x2)+t(vx1−vx2)}2+{(y1−y2)+t(vy1−vy2)}2
・・・(1)
【0034】
つぎに、式(1)を、係数A、BおよびCを用いて以下の式(2)に変形する。
D12(t)2
=At2+Bt+C
=A{t+(B/2A)}2+C−(B2/4A) ・・・(2)
【0035】
式(2)より、D12(t)の最小値Dminは、以下のように表される。
B/2A≧0の場合:Dmin=C1/2
B/2A<0の場合:Dmin={C−(B2/4A)}1/2
【0036】
そこで、最小値Dminを他者間干渉度と定義し、当該最小値Dminが閾値εlimit(=max(W1,L1)+max(W2,L2)+δ)以下の場合には、他者間干渉度が高い(大きい)と判定(判断)する。
【0037】
図4に戻り、他者将来位置予測部3aは、ステップSB2で他者間干渉度が大きいと判定された場合(ステップSB3:Yes)には、当該判定された他者対についての他者間干渉領域を、各他者の現時点での位置ベクトルおよび速度ベクトルを用いて算定する(ステップSB4)。ここで、他者間干渉領域の算定方法について図6を参照して説明する。図6は、他者間干渉領域算定方法の一例を説明する図である。
【0038】
まず、現時点での他者O1と他者O2とを通過する直線L1は、以下の式(3)で表され、また、現時点での他者O1と他者O2との中点((x1+x2)/2,(y1+y2)/2)を通過し直線L1に直交する直線L2は、以下の式(4)で表される。
L1:(x1−x2)y−(y1−y2)x+x1(y1−y2)−y1(x1−x2)=0
・・・(3)
L2:(y1−y2)y+(x1−x2)x−{(x1+x2)/2}(x1−x2)−{(y1+y2)/2}(y1−y2)=0 ・・・(4)
【0039】
つぎに、直線L1および直線L2を座標軸とし前記中点を原点とするL1−L2直交座標系を設定すると、当該座標系における楕円の方程式(標準形)は、以下の式(5)で表される。
L12/Q2+L22/S2=1 ・・・(5)
【0040】
ここで、係数Qは、以下の式(6)で決定する。
Q=D12(0)/2+max(max(W1,L1),max(W2,L2))
・・・(6)
【0041】
また、係数Sは、以下の方法で決定する。まず、他者O1と他者O2が現時点からそれぞれの速度ベクトルの方向に沿った線上を進んで、時間tZ(=−B/2A)経過した時点での他者O1と他者O2の存在位置Z1、Z2(最接近時の存在位置)をそれぞれ、図6に示すように、Z1(x1+tZvxl,y1+tZvy1)、Z2(x2+tZvx2,y2+tZvy2)とする。つぎに、他者O1と他者O2がそれぞれの存在位置Z1、Z2からそれぞれの進行方向に沿ってL1/2、L2/2の長さ分移動し、さらに当該進行方向に直交する方向であって前記中点から遠ざかる方向にW1/2、W2/2の長さ分移動した時点での他者O1と他者O2の存在位置Z1’、Z2’を算定する。そして、各Z1’、Z2’に対して係数Sを算定し、算定したうちの大きな方を係数Sとして選択する。
【0042】
そして、ステップSB4では、係数Q、Sが決定した楕円で囲まれる範囲を、他者間干渉領域として算定する。
【0043】
図4に戻り、他者将来位置予測部3aは、ステップSB4で算定した他者間干渉領域内に自車両が存在しない場合(ステップSB5:No)には、当該他者間干渉領域内に前記他者対が時間に関係なく一様に存在すると仮定して、当該両他者に対して存在確率を設定する(ステップSB6)。
【0044】
つぎに、他者将来位置予測部3aは、自車両周辺にn個の他者が検出された場合における他者対の全ての組み合わせについて上述したステップSB2からステップSB6までの処理を繰り返し、他者対の全ての組み合わせについて上述した他者間干渉度の算定が終了した場合(ステップSB1:Yes)には、存在確率が設定されていない他者に対して、後述する[3−3.他者間将来位置予測処理]にて詳細に説明する追加処理(他者間将来位置予測処理)を行って他者の将来位置(将来時間毎の存在位置)を予測し(ステップSB7)、当該予測結果を他者将来位置情報として、他者間干渉領域と共にリスク値算出部3bへ出力する(ステップSB8)。
【0045】
これにて、他者対将来位置予測処理の説明を終了する。
【0046】
[3−3.他者間将来位置予測処理]
次に、本実施形態の特長である他者間将来位置予測処理の一例について図7から図13を参照して詳細に説明する。図7は、本実施形態の他者間将来位置予測処理の一例を示すフローチャートである。なお、ここでは、他者を障害物として説明する。
【0047】
まず、他者将来位置予測部3aは、自車両周辺の障害物(上述した「存在確率が定義されていない他者」)を認識し、認識した障害物の総数Nを設定する(ステップSC1)。
【0048】
そして、他者将来位置予測部3aは、1からNまでの整数値を取る変数iと、i+1からNまでの整数値を取る変数jとを設定し、変数iを固定して変数jを変化させながら障害物iと障害物jとの全ての組み合わせ(組み合わせ総数は、N(N−1)/2)に対して、以下のステップSC2からステップSC15までの処理を繰り返す。
【0049】
まず、他者将来位置予測部3aは、障害物iが移動障害物である場合(ステップSC2:Yes)には、現時点での障害物iと障害物jの位置ベクトルおよび速度ベクトルを用いて、障害物iと障害物jとが将来干渉するか否かを判定する(ステップSC3:干渉判定処理)。ここで、干渉判定処理の一例について図8および図9を参照して説明する。図8は、本実施形態の干渉判定処理の一例を示すフローチャートである。図9は、現時刻での障害物の位置および速度の状態の一例を示す図である。
【0050】
本干渉判定処理では、現時点での障害物iと障害物jの位置ベクトルおよび速度ベクトルを用いて、障害物iと障害物jとが、現時点から所定の予測時間T〔秒〕(例えば5〔秒〕など)先の時点までの間に干渉するか否かを判定する。具体的には、他者将来位置予測部3aは、0からTまでの値(例えば整数値や小数値など)を取る変数tを設定し、設定した変数tの値を変化させながら、以下のステップSD1からステップSD5までの処理を、以下で説明する干渉フラグに「干渉あり」を表す「1」がセットされるまで繰り返し実行する。
【0051】
まず、他者将来位置予測部3aは、現時点での障害物iの位置ベクトル(xi_0,yi_0)と速度ベクトル(vxi_0,vyi_0)から(図9参照)、変数tの時点での障害物iの将来位置ベクトル(xi(t),yi(t))(=(xi_0+vxi_0×t,yi_0+vyi_0×t))を算出し、また、現時点での障害物jの位置ベクトル(xj_0,yj_0)と速度ベクトル(vxj_0,vyj_0)から(図9参照)、変数tの時点での障害物jの将来位置ベクトル(xj(t),yj(t))(=(xj_0+vxj_0×t,yj_0+vyj_0×t))を算出し、算出した2つの将来位置ベクトルを用いて、変数tの時点での障害物iと障害物jとの間の将来距離Lij(t)(={(xi(t)−xj(t))2+(yi(t)−yj(t))2}1/2)を算出する(ステップSD1)。
【0052】
つぎに、他者将来位置予測部3aは、障害物iと障害物jとの干渉の状態を管理するための干渉フラグ(例えば、0:干渉なし、1:干渉あり)に「干渉なし」を表す「0」をセットして、当該干渉フラグを初期化する(ステップSD2)。
【0053】
つぎに、他者将来位置予測部3aは、ステップSD1で算出した将来距離Lij(t)が障害物iの大きさriと障害物jの大きさrjとの和(=ri+rj)以下である場合(ステップSD3:Yes)には、変数tの時点に障害物iと障害物jとが干渉すると予測し、干渉フラグに「干渉あり」を表す「1」をセットして(ステップSD4)、本処理を終了する(ステップSD5)。
【0054】
これにて、干渉判定処理の説明を終了する。
【0055】
図7に戻り、他者将来位置予測部3aは、干渉フラグが「干渉あり」にセットされていた場合(ステップSC4:Yes)には、上述した干渉判定処理において「干渉あり」と判定されたときの変数tの値を、障害物iと障害物jの干渉時間Tg〔秒〕として設定する(ステップSC5)。
【0056】
つぎに、他者将来位置予測部3aは、障害物iと障害物jのそれぞれに、所定の回避方向(例えば右方向、左方向など)を設定する(ステップSD6)。ここで、回避方向を以下の方法1.または2.で予測し、当該予測した回避方向を設定してもよい。これにより、より精度の高い回避方向の判定が実施可能となる。
【0057】
1.干渉発生時における干渉される方向を算出し、干渉されない方向を回避方向として設定する。例えば図10に示すように、障害物iにおいて、干渉点での干渉される相手の障害物jの進入方向(図10の例では右側方向)を算出し、進入方向でない方向(図10の例では左側方向)を回避方向として設定する。
2.干渉する2つの障害物の位置、速度および周囲道路との位置関係から回避方向を予測し、当該予測した回避方向を設定する。例えば、障害物の移動可能路(走路)より、干渉発生時における移動可能方向が多い方向(例えば歩道が広い側)を回避方向として設定する。
【0058】
図7に戻り、他者将来位置予測部3aは、ステップSC5で設定した干渉時間Tgに起こると想定した干渉(衝突)を回避するために障害物iおよび障害物jが回避し始める回避開始時間Ts〔秒〕として、所定の値(例えば1.0〔秒〕など)を設定する(ステップSC7)。ここで、回避開始時間Tsを、例えば、干渉発生時点における干渉障害物(障害物iおよび障害物j)の干渉角度(図11参照)および/または干渉障害物の属性(例えば車両、歩行者など)に応じて可変としてもよい。
【0059】
図7に戻り、他者将来位置予測部3aは、干渉時に障害物iを横方向に回避させる回避距離Ai〔m〕として、所定の値(例えば歩行者の場合は0.5〔m〕など)を設定する(ステップSC8)。ここで、回避距離Aiを、例えば干渉障害物の属性(例えば車両、歩行者など)の幅や、干渉時Tgにおける干渉障害物間の距離Lij(Tg)(={(xi(Tg)−xj(Tg))2+(yi(Tg)−yj(Tg))2}1/2)に応じて可変としてもよい。
【0060】
つぎに、他者将来位置予測部3aは、ステップSC5からステップSC8までで設定した各情報に基づいて、障害物iが移動している方向にとった直交座標系(xi−yi座標系)での障害物iの回避経路(xi_new_pre(t),yi_new_pre(t))(図13に示す太線の経路)を算出する(ステップSC9:図12、図13参照)。ここで、xi_new_pre(t)およびyi_new_pre(t)は、以下の様に表される。
【0061】
xi_new_pre(t)
=xi_0+vi_0×t (0≦t<Tg−Ts)
={xi_0+vi_0×(Tg−Ts)}+(vi_0×cosφi)×{t−(Tg−Ts)}
(Tg−Ts≦t<Tg)
=〔xi_0+vi_0×{Tg−(1−cosφi)Ts}〕+vi_0×(t−Tg)
(Tg≦t)
yi_new_pre(t)
=0 (0≦t<Tg−Ts)
=(vi_0×sinφi)×{t−(Tg−Ts) (Tg−Ts≦t<Tg)
=Ai (Tg≦t)
※φi=sin−1{Ai/vi_0×Ts}
【0062】
つぎに、他者将来位置予測部3aは、ステップSC9で算出した回避経路(xi_new_pre(t),yi_new_pre(t))を以下の行列演算に従って座標変換することで、自車両からみた直交座標系(x−y座標系)での障害物iの回避経路(xi_new(t),yi_new(t))(予測位置平均ベクトル)を算出する(ステップSC10)。ここで、xi_new(t)およびyi_new(t)は、以下の様に表される。
【0063】
[xi_new(t);yi_new(t);1]
=[cosθi_0 −sinθi_0 xi_0;sinθi_0 cosθi_0 yi_0;0 0 1]×[xi_new_pre(t);yi_new_pre(t);1]
※[]は行列を表す記号である。;は行列の各行を区切る記号である。記号;で区切られた間に記述された文字(数字)または文字列(数字列)は、行列内の1つの行を構成する列要素である。
【0064】
つぎに、他者将来位置予測部3aは、ステップSC10で算出した障害物iの回避経路を用いて、障害物iの回避経路に関する共分散行列(X軸方向:Σi_x(t)、Y軸方向:Σi_y(t))(予測位置共分散行列)を、以下の行列演算に従って算出する(ステップSC11)。
【0065】
Σi_x(t)
=[σi_x(t)2 σi_xu(t);σi_ux(t) σi_u(t)2]
Σi_y(t)
=[σi_y(t)2 σi_yv(t);σi_vy(t) σi_v(t)2]
Fi(t)
=[1 Δt 0 0;0 1 0 0;0 0 1 Δt;0 0 0 1]
Σi_x(t+Δt)=Fi(t)×Σi_x(t)×inv(Fi(t))
Σi_y(t+Δt)=Fi(t)×Σi_y(t)×inv(Fi(t))
※分散σi_x(0)2およびσi_u(0)2は、ある一定定数である。共分散σi_xu(0)およびσi_ux(0)は、0である。分散σi_y(0)2およびσi_v(0)2は、ある一定定数である。共分散σi_yv(0)およびσi_vy(0)は、0である。
【0066】
ここで、ステップSC11において、共分散行列(分散)の値を、「干渉あり」と「干渉なし」とで可変としてもよい。具体的には、干渉あり時において、通常時(干渉なし時)と比較して分散(ばらつき)を大きくしてもよい。より具体的には、前記Fi(t)を、分散の広がりを決定する係数aが設定された以下のものに変更し、「干渉あり」と判定された場合と「干渉なし」と判定された場合とで係数aの値を切り替えてもよい。例えば、「干渉あり」と判定された場合には、係数aの値を「干渉なし」と判定された場合の値より大きくし、「干渉なし」と判定された場合には、係数aの値を「干渉あり」と判定された場合の値より小さくしてもよい。これにより、予測と実際とで回避タイミングや回避方向が異なってしまった場合でも、ある程度の支援制御を実施することが可能となる。
Fi(t)
=[1 a×Δt 0 0;0 1 0 0;0 0 1 a×Δt;0 0 0 1]
【0067】
なお、他者将来位置予測部3aは、干渉フラグが「干渉あり」にセットされていなかった場合(ステップSC4:No)には、現時点での障害物iと障害物jの位置ベクトルおよび速度ベクトルを用いて、障害物iの回避経路(xi_new(t),yi_new(t))(予測位置平均ベクトル)、および上述同様の演算に基づく障害物iの共分散行列(予測位置共分散行列)を算出する(ステップSC12、ステップSC13)。ここで、「干渉なし」の場合、障害物同士は回避行動を取らずに移動するので、xi_new(t)およびyi_new(t)は、以下の様に算定される。
【0068】
[xi_new(t);yi_new(t);1]
=[cosθi_0 −sinθi_0 xi_0;sinθi_0 cosθi_0 yi_0;0 0 1]×[xi_0+vi_0×t;0;1]
【0069】
また、他者将来位置予測部3aは、障害物iが移動障害物でない場合(ステップSC2:No)には、現時点での障害物iと障害物jの位置ベクトルを用いて、上述同様に、障害物iの回避経路(予測位置平均ベクトル)および障害物iの回避経路に関する共分散行列(予測位置共分散行列)を算出する(ステップSC14、ステップSC15)。
【0070】
これにて、他者間将来位置予測処理の説明を終了する。
【0071】
[4.本実施形態のまとめ]
以上説明したように、本実施形態では、複数の他者(障害物)同士の将来予測位置(第1予測進路)が干渉(接近)する場合(複数の他者同士の将来予測位置が干渉するエリアを検出した場合)には、当該干渉の影響で当該将来予測位置から変更され得る複数の他者のそれぞれの将来予測位置(回避経路、第2予測進路)を算出し、当該将来予測位置に応じたリスク値を設定する。これにより、それぞれの他者にインタラクションの影響があると考慮して、正確な移動予測変化に伴うリスク予測を行うことができる。
【0072】
また、本実施形態では、複数の他者同士の第1予測進路の干渉エリア(最接近位置)を算出し、複数の他者の現在位置を結ぶ線分を長軸または短軸とし当該干渉エリアを含む楕円を作成し、作成した楕円をリスク範囲として設定する。これにより、リスクを設定する際には個別の回避進路を計算する必要がなくなるので、干渉の影響による進路変更リスクを単純な計算で行なうことで、システム負荷を軽減させることができる。なお、干渉度合いの大きさを算出し、算出した干渉度合いの大きさに応じて楕円の大きさを変更してもよい。
【0073】
また、本実施形態では、第1予測進路から第2予測進路に回避(変更)し始めるタイミング(時間)を、他者が持つ情報(例えば干渉角度や属性など)に応じて設定してもよい。通常、回避開始時間は状況に応じて異なる。しかし、一定の回避開始時間を設定して回避経路を予測した場合、当該回避開始時間と他者の実際の回避開始時間との乖離が大きいと、回避経路の予測精度が低下して最適な支援制御が実施できない(例えば警報やブレーキなどが最適な場面で作動できない等)可能性がある。そこで、本実施形態では、回避開始時間を他者が持つ情報に応じて設定するので、実際の回避開始時間との乖離が大きくならず、結果として、回避経路の予測精度を低下させずに最適な支援制御を実施することができる。
【0074】
また、本実施形態では、第1予測進路から第2予測進路への回避距離(第1予測進路方向に対する横方向の移動距離)を、他者が持つ情報(例えば属性や干渉時における干渉距離など)に応じて設定してもよい。通常、回避距離は状況に応じて異なる。しかし、一定の回避距離を設定して回避経路を予測した場合、当該回避距離と他者の実際の回避距離との乖離が大きいと、回避経路の予測精度が低下して最適な支援制御が実施できない(例えば警報やブレーキなどが最適な場面で作動できない等)可能性がある。そこで、本実施形態では、回避距離を他者が持つ情報に応じて設定するので、実際の回避距離との乖離が大きくならず、結果として、回避経路の予測精度を低下させずに最適な支援制御を実施することができる。
【0075】
また、本実施形態では、他者の回避方向を、他者同士の干渉点への進入方向および/または他者同士の干渉点通過時の交通環境に応じて設定してもよい。通常、回避方向は状況に応じて異なる。しかし、一定の回避方向を常に仮定して回避経路を予測した場合、当該回避方向と他者の実際の回避方向が逆になると、回避経路の予測精度が低下して最適な支援制御が実施できない(例えば警報やブレーキなどが最適な場面で作動できない等)可能性がある。そこで、本実施形態では、回避方向を上述した進入方向や交通環境に応じて設定するので、実際の回避方向と逆になることなく、結果として、回避経路の予測精度を低下させずに最適な支援制御を実施することができる。
【0076】
また、本実施形態では、上述した共分散行列の値を「干渉あり」時と「干渉なし」時で切り換えてもよい。他者同士の干渉が発生する場面において回避開始タイミングや回避方向を想定した将来位置予測を実施する場合、実際の回避開始タイミングや回避方向を正確に予測することは難しい。そのため、当該想定が実際とは大きく異なると、回避経路の予測精度が低下して最適な支援制御が実施できない(例えば警報やブレーキなどが最適な場面で作動できない等)可能性がある。そこで、本実施形態では、干渉時において、通常時(干渉が発生しない場合)と比較して分散(ばらつき)を大きくすることで、想定と実際とで回避開始タイミングや回避方向が異なってしまった場合でも、ある程度適切な支援制御を実施することが可能となる。
【産業上の利用可能性】
【0077】
以上のように、本発明にかかる対象物リスク予測装置は、自動車製造産業において有用であり、特に、対象物の予測進路に応じたリスク設定を行うための利用に適している。
【符号の説明】
【0078】
2 状態検出手段
2b 道路構造検出部
2c 可動物検出部
3 運動量算出手段
3a 他者将来位置予測部
3b リスク値算出部
【特許請求の範囲】
【請求項1】
車両周囲の対象物に関する情報を取得し、取得した情報から対象物の第1将来進路を予測し、予測した第1将来進路に応じたリスクを設定する対象物リスク予測装置において、
予測した複数の対象物の第1将来進路同士が干渉する場合には、当該干渉の影響で当該第1将来進路から変更され得る当該複数の対象物それぞれの第2将来進路を予測し、予測した第2将来進路に応じたリスクを設定すること、
を特徴とする対象物リスク予測装置。
【請求項2】
複数の対象物の現在位置を結ぶ線分を長軸又は短軸に含み第1将来進路同士が干渉するエリアを含む楕円を作成し、作成した楕円をリスク範囲として設定すること、
を特徴とする請求項1に記載の対象物リスク予測装置。
【請求項1】
車両周囲の対象物に関する情報を取得し、取得した情報から対象物の第1将来進路を予測し、予測した第1将来進路に応じたリスクを設定する対象物リスク予測装置において、
予測した複数の対象物の第1将来進路同士が干渉する場合には、当該干渉の影響で当該第1将来進路から変更され得る当該複数の対象物それぞれの第2将来進路を予測し、予測した第2将来進路に応じたリスクを設定すること、
を特徴とする対象物リスク予測装置。
【請求項2】
複数の対象物の現在位置を結ぶ線分を長軸又は短軸に含み第1将来進路同士が干渉するエリアを含む楕円を作成し、作成した楕円をリスク範囲として設定すること、
を特徴とする請求項1に記載の対象物リスク予測装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【公開番号】特開2011−221667(P2011−221667A)
【公開日】平成23年11月4日(2011.11.4)
【国際特許分類】
【出願番号】特願2010−88093(P2010−88093)
【出願日】平成22年4月6日(2010.4.6)
【出願人】(000003207)トヨタ自動車株式会社 (59,920)
【出願人】(000003609)株式会社豊田中央研究所 (4,200)
【Fターム(参考)】
【公開日】平成23年11月4日(2011.11.4)
【国際特許分類】
【出願日】平成22年4月6日(2010.4.6)
【出願人】(000003207)トヨタ自動車株式会社 (59,920)
【出願人】(000003609)株式会社豊田中央研究所 (4,200)
【Fターム(参考)】
[ Back to top ]