JP6203381B2

JP6203381B2 - 経路予測装置

Info

Publication number: JP6203381B2
Application number: JP2016512545A
Authority: JP
Inventors: 佑樹高林; 洋志亀田
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2014-04-10
Filing date: 2014-04-10
Publication date: 2017-09-27
Anticipated expiration: 2034-04-10
Also published as: WO2015155874A1; JPWO2015155874A1; US10102761B2; DE112014006561T5; CN106164998B; CN106164998A; US20170039865A1

Description

本発明は、レーダ、ＧＰＳなどのセンサからなる観測装置を用いて、航空機、船舶、車両などの移動する対象物体の位置を観測し、その観測値に基づき、対象物体とその周辺に存在する複数の物体との衝突を防止するための経路を予測する経路予測装置に関するものである。

近年、移動体同士の衝突を回避する安全な経路を予測する技術は車両の運転支援システムや航空管制など様々な分野で要求されている。
例えば車両の運転支援システムでは、自車両周辺に存在する車両や停止物等の障害物の位置を自車両に搭載したミリ波レーダやレーザレーダ等のセンサによって取得し、自車両と障害物の相対距離や相対速度より衝突危険性を判断した上で自車両を制御し、衝突を防止する技術が開発されている。また、より高度な技術として、前記のセンサによって周辺環境を認識し、運転者の操作無く、自動でハンドル操作やブレーキなどの操作を行い、目的地まで到達する自動運転技術も開発が進められている。

このような経路予測に関する従来の技術として、例えば特許文献１に示す装置では、車両の予測軌道を予め複数生成し、生成した予測軌道を元に時空間における予測経路の存在確率を算出していた。また、例えば特許文献２に示すような運転支援装置では、他車両に対する自車両のリスクポテンシャルマップを算出し、リスクに基づくアクセル，ブレーキなどの制御を介入させるようにしていた。

一方、航空管制では、従来の３次元位置による航法に対して３次元位置と時間を含む４次元軌道（４ＤＴ：４ＤｉｍｅｎｓｉｏｎａｌＴｒａｊｅｃｔｏｒｙ）を航法に取り入れる検討がされている。この４ＤＴは予測経路に相当し、４ＤＴを管理することで航空交通量や空域容量の予測が可能となることから運航に対する安全性の向上が期待されている。このような経路予測の技術として、例えば特許文献３では、将来位置を目標の現在の速度と機首方向とを基に等速直進と仮定し算出していた。

また、例えば、特許文献４に示されたようなシステムでは、将来位置を予測する方法として、Ａ＊アルゴリズムによる最適経路探索方法が用いられている。このアルゴリズムでは進入禁止域（障害物）を含む、経路候補をメッシュ上に細分した移動空間上における出発からゴール（もしくは経由点）までのノードを決定するものである。

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

しかしながら、上記特許文献１に記載されたような従来の装置では、存在確率を算出するためには予測軌道を多数生成する必要があり、演算負荷が大きくなる問題があった。また、特許文献２に示されたような装置では、リスクの算出方法が明確でなく、パラメータに依存した算出方法であるため、正確にリスクを評価することができないという問題があった。さらに、特許文献３に記載されたような従来技術では、目標が雷雲などの障害物を回避するために進路変更をした場合等に将来位置の推定精度が悪化するという問題があった。また、特許文献４に記載されたようなＡ＊アルゴリズムを用いるシステムでは、経路が格子点で決まるため、移動体の運動を考慮していないという問題がある。自然な経路を得るためには格子点の間隔を細かくする必要があり、処理時間が犠牲になるといった問題があった。

この発明は、かかる問題を解決するためになされたもので、衝突危険度の低い予測経路を算出する際の演算負荷を軽減することのできる経路予測装置を提供することを目的とする。

この発明に係る経路予測装置は、対象物体と対象物体の周辺に位置する周辺物体の位置に関する入力信号に基づいて追尾処理を行い、対象物体と周辺物体の推定位置と推定速度を算出する追尾処理部と、推定位置と推定速度から、対象物体と衝突する可能性のある周辺物体をターゲット物体として検知する衝突物体検知部と、衝突回避モデルに基づき、ターゲット物体に対する対象物体の経路を予測する経路予測部と、対象物体とターゲット物体との衝突危険度を衝突回避モデルに対応して算出する衝突危険度評価部と、衝突危険度より衝突の有無を判定し、衝突と判定した場合は経路予測部へ衝突回避モデル修正値をフィードバックする衝突判定部と、衝突判定部で衝突無しと判定された複数の衝突回避モデルから、いずれかの衝突回避モデルを選択し、衝突回避モデルの経路を、物体同士の衝突を回避する経路として選択する回避経路選択部とを備え、経路予測部は、衝突回避モデル修正値を用いて、新たな経路予測を行い、追尾処理部は、推定位置の推定誤差を算出し、衝突危険度評価部は、推定位置を推定誤差で正規化した値に基づいて、衝突危険度を求めるようにしたものである。

この発明に係る経路予測装置は、衝突回避モデルに基づき、ターゲット物体に対する対象物体の経路を予測し、対象物体とターゲット物体との衝突危険度を衝突回避モデルに対応して算出すると共に、この衝突危険度より衝突の有無を判定し、衝突無しと判定された複数の衝突回避モデルから選択したいずれかの衝突回避モデルの経路を、物体同士の衝突を回避する経路として選択するようにしたものである。これにより、衝突危険度の低い予測経路を算出する際の演算負荷を軽減することができる。

この発明の実施の形態１の経路予測装置を示す構成図である。この発明の実施の形態１の経路予測装置における衝突危険度の説明図である。この発明の実施の形態１の経路予測装置における衝突危険度が高い場合の説明図である。この発明の実施の形態１の経路予測装置における衝突危険度が低い場合の説明図である。この発明の実施の形態１の経路予測装置における操舵回避時の衝突危険度算出対象を示す説明図である。この発明の実施の形態１の経路予測装置における経路予測部から衝突判定部までの処理部の動作を示すフローチャートである。

以下、この発明をより詳細に説明するために、この発明を実施するための形態について、添付の図面に従って説明する。
実施の形態１．
図１は、本実施の形態による経路予測装置を示す構成図である。
本実施の形態による経路予測装置は、図示のように、センサ部１、追尾処理部２、衝突物体検知部３、経路予測部４、衝突危険度評価部５、衝突判定部６、衝突回避経路選択部７を備えている。

センサ部１は、対象物体と、この対象物体の周辺に位置する周辺物体との相対位置を観測する処理部であり、ミリ波レーダ、レーザレーダ、光学カメラ、赤外カメラ等のセンサ、または、周辺車両や歩行者のＧＰＳ位置を受信する通信装置等を用いて構成されている。追尾処理部２は、センサ部１で観測された相対位置より追尾処理を行い、対象物体と周辺物体の推定位置、推定速度、推定位置の推定誤差、推定速度の推定誤差を算出する処理部である。衝突物体検知部３は、推定位置と前記推定速度から、対象物体と衝突する可能性のある周辺物体をターゲット物体として検知する処理部である。経路予測部４は、Ｍ個の衝突回避モデルそれぞれにおいて、ターゲット物体に対する対象物体のＮステップ先までの予測位置を算出する処理部である（ここで、ＭおよびＮは任意の整数である）。衝突危険度評価部５は、追尾処理部２で算出された推定位置および推定誤差より衝突危険度を衝突回避モデルごとに算出する処理部である。衝突判定部６は、衝突危険度評価部５で算出された衝突危険度から衝突の有無を判定し、衝突と判定した場合は経路予測部４へ衝突回避モデル修正値をフィードバックし、衝突無しと判定した場合は、その衝突回避モデルを衝突回避経路選択部７に出力する処理部である。衝突回避経路選択部７は、衝突判定部６から出力された衝突回避モデルに対して、所定の判定基準に基づいていずれかの衝突回避モデルを選択し、衝突回避用の予測経路を決定する処理部である。

なお、経路予測装置はコンピュータを用いて構成され、追尾処理部２〜衝突回避経路選択部７は、それぞれの処理部の機能に対応したソフトウェアをＣＰＵで実行することにより実現されている。あるいは、上記センサ部１〜衝突回避経路選択部７の少なくともいずれかを専用のハードウェアで構成してもよい。

次に、実施の形態１の経路予測装置の動作について説明する。
センサ部１は、周辺車両や歩行者の位置と速度を測定する。追尾処理部２は、位置と速度に基づき、追尾処理を介して位置推定値、速度推定値、位置及び速度の推定誤差共分散行列を算出する。
衝突物体検知部３では、自車両と衝突する可能性のある周辺車両を検知する。例えば、ＴＴＣ（ＴｉｍｅＴｏＣｏｌｌｉｓｉｏｎ）の考え方に基づいて検知しても良いこととする。ＴＴＣは式（１）で定義され、ＴＴＣが閾値以下であれば衝突する可能性がある車両として検知する。さらに、検知された周辺車両ｉをターゲット車両として定義する。

また、衝突物体検知部３における別の方法として、自車周辺に所定領域を設定しておき、この所定領域に１〜Ｎステップ後の予測位置が入る車両を検知し、ターゲット車両とみなしても良い。ここで、式（２）のようにＮステップ先までのＮ個の予測位置を計算する。

経路予測部４では、衝突物体検知部３で検知されたターゲット車両ｔｇｔｉに対して、Ｍ個の衝突回避モデルそれぞれにおいてＮステップ先までの予測位置を算出する。
ここで、衝突回避モデルとして、例えば、制動回避モデル、左操舵回避モデル、右操舵回避モデルを定義しても良い。制動回避モデルは、車線維持したまま制動で衝突を回避するモデル、左／右操舵回避モデルは操舵量を入力することで左／右へ車線変更し、衝突を回避するモデルとする。また、これらのモデルについて、制動量もしくは操舵量は所定の限界値を超えないように設定することとする。特に、後述する衝突判定部６において、衝突回避不可能と判定された場合、制動量もしくは操舵量の修正値が経路予測部４へフィードバックされるが、所定の限界値を超えないような動作を実施する。
また、経路予測部４では、衝突回避モデルの制動量もしくは操舵量の初期値を設定する必要がある。初期値として、制動および操舵回避の際に入力される値を経験的に設定しても良い。または、学習アルゴリズムを利用して、運転者ごとに不快に感じない程度の制動量、操舵量を設定しても良い。

さらに、経路予測部４では、前記のモデルに限らず、それ以外にも様々なシーンに対応した衝突回避モデルを追加しても良い。また、地図データやＧＰＳ位置などによって、車線数および自車が走行する車線が既知である場合は、不要な衝突回避モデルを棄却して衝突回避モデル数を削減しても良い。例えば、車線数が２かつ自車が左車線を走行している場合は、左操舵回避は不可能であるため、左操舵回避モデルは棄却し、残りの衝突回避モデルについて計算することとする。他にも、例えば、途中で２車線から３車線へ車線数が増加する地点では、追加車線へ車線変更するための衝突回避モデルを追加する等、地図データによる衝突回避モデルの追加や削減は容易に可能となる。地図データ以外にもレーザレーダ、カメラ等を利用すれば、外部環境を認識することは可能であり、地図の代わりに用いても良い。

衝突回避モデルに基づく予測位置算出方法について説明する。制動回避モデルの制動用加速度ａ_ｂに基づいて式（６）のように予測経路（Ｎステップ先までの予測位置）を算出する。

左／右操舵回避モデルについても同様に算出できる。ここで、操舵に対する車両の予測位置は、車両重量、車体重心位置、ヨー慣性モーメントなどの車両パラメータによって異なるため、車両パラメータが既知である場合は、予め設定しておき、予測位置を算出する。また、車両パラメータが未知である場合は、公知の学習アルゴリズムなどによって推定されたパラメータを用いても良い。

衝突危険度評価部５では、追尾処理部２で出力される位置の推定誤差共分散行列および位置及び速度推定値に基づいて衝突危険度を算出する。
式（９）のようにサンプリング時刻ｋにおける自車両のｎステップ後の予測位置とターゲット車両ｔｇｔｉのｎ（ｎ＝１，…，Ｎ）ステップ後の予測位置の差分を推定誤差共分散行列で正規化した値、つまりマハラノビス距離の２乗値ε_ｋ＋ｎを算出する。

ここで、横位置ｘ、縦位置ｙの２変数が正規分布に従う場合、マハラノビス距離の２乗値ε_ｋ＋ｎの確率分布は自由度２のカイ二乗分布となることが知られている。この性質を利用し、衝突危険度評価部５では、図２のように衝突危険度をカイ二乗分布の上側確率（図２の斜線部分１００）として定義する。
衝突危険度を直感的に理解するために、自車両（目標２）とターゲット車両（目標１）の相対位置と衝突危険度の対応関係について説明する。例えば、図３のように目標１と目標２が衝突するシーン（目標１と目標２の位置が同じ）では、図３の斜線部分１０１が１に近づく。つまり、衝突危険度は１（もしくは１００％）として算出される。一方、図４のように目標１と目標２との距離が限りなく遠いシーンでは、図４の斜線部分は０に近づく。つまり、衝突危険度は０（０％）として算出される。そのため、カイ二乗分布の上側確率は、直感的にも衝突危険度に相当する値であることがわかる。さらに、マハラノビス距離の２乗値ε_ｋ＋ｎとカイ二乗分布の上側確率の対応表は予め計算可能であるため、対応表を保持しておけば、マハラノビス距離の２乗値に応じた衝突危険度を計算無く読み出すことが可能となる。

また、これまでに自車両と周辺車両との相対位置で衝突危険度を算出する方法を述べたが、加えて、目標１と目標２の絶対位置を用いる場合の衝突危険度算出方法を説明する。例えば、車両の運転支援システム向けには、車車間通信などで自車と周辺車両のＧＰＳ位置といった絶対値を取得する場合が考えられる。また、航空管制の分野では複数の航空機について、レーダによる観測位置やＧＰＳ位置が取得され、航空機管制に用いられる場合が考えられる。その場合、各目標位置には位置誤差が含まれることになるため、下記式（１２），（１３）を用いて衝突危険度の評価値を計算し、評価値に対応した衝突危険度を読み出す。

ここで、目標の誤差分布の重なりから衝突危険度を算出するためには、誤差分布に基づく煩雑な数値計算が必須となるが、本発明では煩雑な数値計算が必要なく、衝突危険度を算出することが可能となる。
また、マハラノビス距離の２乗値ε_ｋ＋ｎの確率分布を他の確率分布（例えば正規分布）で近似しても良い。

衝突判定部６では、衝突危険度評価部５で算出した衝突危険度より衝突を判定し、衝突する場合は予測経路修正値を経路予測部４へ出力し、予測経路を再修正する。衝突しない場合は予測経路と衝突危険度を衝突回避経路選択部７へ出力する。
衝突判定には確率変数ε_ｋ＋ｎ（ｎ＝１，…，Ｎ）の最小値に対して閾値ε_ｔｈ以下である場合は衝突することとみなす。前記の閾値ε_ｔｈは自由度ｍのカイ二乗分布表を用いることとし、衝突危険度評価部５でも述べたように、予め衝突危険度に対応する衝突閾値ε_ｔｈを設定しておけば、容易に衝突するか否かを判定できる。

また、図５のように操舵回避の場合は、自車両２００が操舵回避で車線変更した先に他の周辺車両が走行しているケースが考えられるため、変更先の車線について、最も近い前方車両２０１および最も近い後方車両２０２との衝突危険度を計算する。さらに、ターゲット車両２０３、最も近い前方車両２０１および最も近い後方車両２０２の衝突危険度の中から最大値を選択し、衝突判定を実施する。なお、図中の破線で囲まれる領域は予測誤差を表している。

さらに、衝突判定部６は、予測経路の修正値を経路予測部４にフィードバックし、これにより、経路予測部４および衝突危険度評価部５は、予測経路および衝突危険度を再計算する。これらの手順は閾値ε_ｔｈを超えるまで繰り返す。
これら経路予測部４〜衝突判定部６の処理フローを図６に示す。すなわち、ターゲット車両ごとに、全てのモデルに対してＮステップの経路予測（ステップＳＴ１）と衝突危険度の評価（ステップＳＴ２）および衝突判定（ステップＳＴ３，ＳＴ４）を行う。また、ステップＳＴ４で衝突閾値以下である場合は、衝突閾値を超えるまでモデルループを行う。なお、モデルループが予め定めた所定回数に達した場合は、その衝突回避モデルの計算を打ち切るようにしてもよい。

衝突回避経路選択部７では、経路予測部４〜衝突判定部６で計算済みの各衝突回避モデルに基づく予測経路より、衝突回避用の予測経路を決定する。
各衝突回避モデルに基づくＮ個の予測位置について、衝突危険度の最大値を比較し、最も小さい値を持つ衝突回避モデルを最も安全な回避経路とみなし、衝突回避用予測経路として出力する。なお、最小値を含む設定値以下の衝突回避モデルを選択するようにしてもよい。
また、前記Ｎ個の予測位置に付与されるＮ個の衝突危険度の合計を比較して最小となる経路を選択しても良い。なお、ここでも最小値を含む設定値以下の経路を選択するようにしてもよい。
また、制動量もしくは操舵量が所定の限界値を超える場合は、棄却しても良い。
また、運転者のニーズに合わせて、制動量の合計値が最小となる経路、もしくは操舵回避量の合計値が最小となる経路を選択しても良い。

このように実施の形態１では、現実的に想定される衝突回避モデルに限定することで、従来のような無数の経路を計算する必要がなくなり、演算負荷を軽減することが可能となる。

以上説明したように、実施の形態１の経路予測装置によれば、対象物体と対象物体の周辺に位置する周辺物体との位置を観測するセンサ部と、対象物体と周辺物体の位置に基づいて追尾処理を行い、対象物体と周辺物体の推定位置と推定速度を算出する追尾処理部と、推定位置と推定速度から、対象物体と衝突する可能性のある周辺物体をターゲット物体として検知する衝突物体検知部と、衝突回避モデルに基づき、ターゲット物体に対する対象物体の経路を予測する経路予測部と、対象物体とターゲット物体との衝突危険度を衝突回避モデルに対応して算出する衝突危険度評価部と、衝突危険度より衝突の有無を判定し、衝突と判定した場合は経路予測部へ衝突回避モデル修正値をフィードバックする衝突判定部と、衝突判定部で衝突無しと判定された複数の衝突回避モデルから、いずれかの衝突回避モデルを選択し、衝突回避モデルの経路を、物体同士の衝突を回避する経路として選択する回避経路選択部とを備え、経路予測部は、衝突回避モデル修正値を用いて、新たな経路予測を行うようにしたので、衝突危険度の低い予測経路を算出する際の演算負荷を軽減することができる。

また、実施の形態１の経路予測装置によれば、追尾処理部は、推定位置の推定誤差と推定速度の推定誤差とを算出し、衝突危険度評価部は、推定位置を推定誤差で正規化した値より衝突危険度を算出するようにしたので、煩雑な数値計算を必要とせずに、衝突危険度を算出することができる。

また、実施の形態１の経路予測装置によれば、衝突危険度評価部は、推定位置を推定誤差で正規化した値と衝突危険度との対応を示す対応表より衝突危険度を取得するようにしたので、数値計算を必要とせず、容易に衝突危険度を得ることができる。

また、実施の形態１の経路予測装置によれば、回避経路選択部は、衝突回避モデルの衝突危険度の時間方向累積値について、累積値が設定値以下となる衝突回避モデルを選択するようにしたので、無数の経路を計算する必要がなく、演算負荷を軽減することができる。

また、実施の形態１の経路予測装置によれば、回避経路選択部は、衝突回避モデルの衝突危険度の時間方向に対する最大値を代表値として、代表値が設定値以下となる衝突回避モデルを選択するようにしたので、無数の経路を計算する必要がなく、演算負荷を軽減することができる。

また、実施の形態１の経路予測装置によれば、衝突判定部は、衝突危険度を設定された閾値と比較することにより衝突判定を行うようにしたので、容易に衝突するか否かを判定することができる。

なお、本願発明はその発明の範囲内において、実施の形態の任意の構成要素の変形、もしくは実施の形態の任意の構成要素の省略が可能である。

以上のように、この発明に係る経路予測装置は、レーダ、ＧＰＳなどのセンサからなる観測装置を用いて、航空機、船舶、車両などの移動体の位置を観測し、その観測値に基づき、移動体とその周辺に存在する複数の移動体との衝突を防止するための経路を予測する経路予測装置に関するものであり、車両の運転支援システムや航空管制などに用いるのに適している。

１センサ部、２追尾処理部、３衝突物体検知部、４経路予測部、５衝突危険度評価部、６衝突判定部、７衝突回避経路選択部。

Claims

対象物体と前記対象物体の周辺に位置する周辺物体の位置に関する入力信号に基づいて追尾処理を行い、前記対象物体と前記周辺物体の推定位置と推定速度を算出する追尾処理部と、
前記推定位置と前記推定速度から、前記対象物体と衝突する可能性のある周辺物体をターゲット物体として検知する衝突物体検知部と、
衝突回避モデルに基づき、前記ターゲット物体に対する前記対象物体の経路を予測する経路予測部と、
前記対象物体と前記ターゲット物体との衝突危険度を前記衝突回避モデルに対応して算出する衝突危険度評価部と、
前記衝突危険度より衝突の有無を判定し、衝突と判定した場合は前記経路予測部へ衝突回避モデル修正値をフィードバックする衝突判定部と、
前記衝突判定部で衝突無しと判定された複数の衝突回避モデルから、いずれかの衝突回避モデルを選択し、当該衝突回避モデルの経路を、物体同士の衝突を回避する経路として選択する回避経路選択部とを備え、
前記経路予測部は、前記衝突回避モデル修正値を用いて、新たな経路予測を行い、
前記追尾処理部は、前記推定位置の推定誤差を算出し、
前記衝突危険度評価部は、前記推定位置を前記推定誤差で正規化した値に基づいて、前記衝突危険度を求めることを特徴とする経路予測装置。
前記衝突危険度評価部は、前記推定位置を前記推定誤差で正規化した値より前記衝突危険度を算出することを特徴とする請求項１記載の経路予測装置。
前記衝突危険度評価部は、前記推定位置を前記推定誤差で正規化した値と衝突危険度との対応を示す対応表より前記衝突危険度を取得することを特徴とする請求項１記載の経路予測装置。
対象物体と前記対象物体の周辺に位置する周辺物体の位置に関する入力信号に基づいて追尾処理を行い、前記対象物体と前記周辺物体の推定位置と推定速度を算出する追尾処理部と、
前記推定位置と前記推定速度から、前記対象物体と衝突する可能性のある周辺物体をターゲット物体として検知する衝突物体検知部と、
衝突回避モデルに基づき、前記ターゲット物体に対する前記対象物体の経路を予測する経路予測部と、
前記対象物体と前記ターゲット物体との衝突危険度を前記衝突回避モデルに対応して算出する衝突危険度評価部と、
前記衝突危険度より衝突の有無を判定し、衝突と判定した場合は前記経路予測部へ衝突回避モデル修正値をフィードバックする衝突判定部と、
前記衝突判定部で衝突無しと判定された複数の衝突回避モデルから、いずれかの衝突回避モデルを選択し、当該衝突回避モデルの経路を、物体同士の衝突を回避する経路として選択する回避経路選択部とを備え、
前記経路予測部は、前記衝突回避モデル修正値を用いて、新たな経路予測を行い、
前記回避経路選択部は、前記衝突回避モデルの衝突危険度を時間方向に処理した結果に基づいて、前記衝突回避モデルを選択することを特徴とする経路予測装置。
前記回避経路選択部は、前記衝突回避モデルの衝突危険度の時間方向累積値について、当該累積値が設定値以下となる衝突回避モデルを選択することを特徴とする請求項４記載の経路予測装置。
前記回避経路選択部は、前記衝突回避モデルの衝突危険度の時間方向に対する最大値を代表値として、当該代表値が設定値以下となる衝突回避モデルを選択することを特徴とする請求項４記載の経路予測装置。
前記衝突判定部は、前記衝突危険度を設定された閾値と比較することにより衝突判定を行うことを特徴とする請求項１記載の経路予測装置。
前記衝突判定部は、前記衝突危険度を設定された閾値と比較することにより衝突判定を行うことを特徴とする請求項４記載の経路予測装置。
前記対象物体と前記周辺物体との位置を観測するセンサ部を備えることを特徴とする請求項１記載の経路予測装置。
前記対象物体と前記周辺物体との位置を観測するセンサ部を備えることを特徴とする請求項４記載の経路予測装置。