JP7496797B2 - 軌道生成装置及び軌道生成方法 - Google Patents

Description

本発明は、移動体の未来の軌道を生成する軌道生成装置などに関する。

従来、軌道生成装置として特許文献１に記載されたものが知られている。この軌道生成装置は、移動体の軌道を円弧としてモデリングしたモデル式を用い、移動体が干渉対象との干渉を回避できるように、移動体の軌道を生成するものである。この軌道生成装置では、モデル式に基づいて評価関数が算出され、極値探索コントローラにより、評価関数が極値になるように、円弧の曲率が決定される。そして、この曲率に基づいて、移動体の未来の軌道が生成される。

特開２０１９－２２００５４号公報

通常の交通環境下では、移動体が干渉対象との干渉を回避する際、移動体の軌道は左方向又は右方向にのみ曲がる軌道に限らず、例えば、左方向に曲がった後、右方向に反転して曲がるような軌道になることがある（後述する図２４参照）。上記特許文献１の軌道生成装置によれば、そのような軌道を生成しようとした場合、複数の極値探索コントローラを用いる必要がある。

このように複数の極値探索コントローラを用いた場合、各極値探索コントローラにおいて１つの評価関数を共用したときには、各極値探索コントローラにおける参照入力の周波数を互いに干渉しないような値に設定する必要がある。その場合には、各極値探索コントローラにおける移動平均フィルタのタップ数が増大してしまい、それに伴って、応答遅れが生じてしまう。この場合、移動体が干渉対象との干渉を回避するための時間的マージンが減少するおそれがある。

これを回避するために、複数の極値探索コントローラにおいて互いに異なる評価関数を用いた場合、以下に述べるような問題が発生する。すなわち、移動体の未来の軌道を各極値探索コントローラで生成した場合、移動体が干渉対象にかなり接近した位置で、干渉対象との干渉を回避するような軌道が生成されてしまう可能性がある（後述する図２２照）。その場合にも、移動体が干渉対象との干渉を回避するための空間的マージンが減少するおそれがある。特に、移動体が車両である場合には、実現可能な軌道の制約が一般的に大きい関係上、この問題が顕著になる。

本発明は、上記課題を解決するためになされたもので、干渉対象との干渉を回避できるように、移動体の未来の軌道を適切に生成することができる軌道生成装置などを提供することを目的とする。

上記目的を達成するために、請求項１に係る発明は、移動体（自車両３、歩行者Ｍ１）の未来の軌道を生成する軌道生成装置１，１００であって、移動体の周辺状況を認識する周辺状況認識部（ＥＣＵ２、情報検出装置４）と、周辺状況認識部による周辺状況の認識結果に基づき、移動体の軌道をモデル化した複数の軌道モデル（式（１）～（２０），（１２４）～（１２５），（１３０）～（１３５），（１４２）～（１４７）,（１４８）～（１５３））を用いて、移動体の未来の軌道を複数に分割したときの複数の未来の軌道の予測値である複数の予測軌道（第１～第３予測軌道）を生成する予測軌道生成部（ＥＣＵ２、軌道決定部３０，２００）と、複数の予測軌道を用いて、移動体の周辺における干渉対象（交通参加者ＴＰ１）の移動体との干渉度合いを表す複数の干渉度合いパラメータ（リスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖ_ｒ，Ｒ２_ｐ_ｍｉ_ｖｊ_ｖ_ｒ、右側及び左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ_ｐ_ｒｂｌ_ｍｉ_ｒ，Ｒ２_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ２_ｐ_ｒｂｌ_ｍｉ_ｒ）を算出する干渉度合いパラメータ算出部（ＥＣＵ２、第１～第３評価関数算出部２１～２３，１２１～１２３）と、複数の干渉度合いパラメータが表す干渉度合いが減少するように、複数の予測軌道をそれぞれ修正するための複数の修正量（移動平均値Ｐ_ｃ１～Ｐ_ｃ３，Ｐ２_ｃ１～Ｐ２_ｃ３）を決定する修正量決定部（ＥＣＵ２、軌道決定部３０，２００）と、を備え、予測軌道生成部は、周辺状況の認識結果に基づき、複数の軌道モデル及び複数の修正量を用いて、複数の予測軌道を生成するとともに、互いに隣り合う２つの予測軌道を修正するための２つの修正量が２つの予測軌道を同じ方向に修正するように決定されている場合には、２つの予測軌道のうちの、移動体の現在位置から近い方の予測軌道を、移動体の現在位置から遠い方の予測軌道を修正するための修正量を反映させながら生成することを特徴とする。

この軌道生成装置によれば、移動体の周辺における干渉対象の移動体との干渉度合いを表す複数の干渉度合いパラメータが算出され、複数の干渉度合いパラメータが表す干渉度合いが減少するように、複数の予測軌道をそれぞれ修正するための複数の修正量が決定される。そして、周辺状況の認識結果に基づき、複数の軌道モデル及び複数の修正量を用いて、複数の予測軌道が生成されるので、複数の予測軌道は、干渉対象の移動体との干渉度合いが減少するように生成されることになる。

その際、互いに隣り合う２つの予測軌道を修正するための２つの修正量が２つの予測軌道を同じ方向に修正するように決定されている場合には、２つの予測軌道のうちの、移動体の現在位置から近い方の予測軌道が、移動体の現在位置から遠い方の予測軌道を修正するための修正量を反映させながら生成される。

ここで、干渉対象が移動体の進行方向に存在する場合には、現在位置から遠い方の予測軌道の方が、移動体の現在位置から近い方の予測軌道と比べて、干渉対象により近い軌道となる。したがって、干渉対象に近い方の予測軌道を修正するための修正量を反映させながら、干渉対象から遠い方（すなわち移動体に近い方）の予測軌道を生成することによって、干渉対象から遠い方の予測軌道において、干渉対象との干渉を回避するための回避マージンをより増大させることができる。

その結果、移動体によって実現可能な軌道が制限される場合、例えば、移動体が車両である場合にも、干渉対象との干渉を回避できるように、移動体の未来の軌道を適切に生成することができる。なお、本明細書における「軌道」は、曲線状のものに限らず、線分を組み合わせたものも含む。また、本明細書における「未来の軌道を生成する」ことは、未来の軌道を演算すること、未来の軌道を決定すること、及び、未来の軌道を予測することを含む。さらに、本明細書における「周辺状況を認識する」ことは、周辺状況を認識することに加えて、周辺状況を取得すること及び周辺状況を検出することを含む。

請求項２に係る発明は、請求項１に記載の軌道生成装置１，１００において、予測軌道生成部は、２つの修正量が２つの予測軌道を互いに異なる方向に修正するように決定されている場合には、移動体の現在位置から近い方の予測軌道を、移動体の現在位置から遠い方の予測軌道を修正するための修正量とは無関係に生成することを特徴とする。

この場合、２つの修正量が２つの予測軌道を互いに異なる方向に修正するように決定されている場合には、２つの予測軌道が干渉対象を回避するために互いに異なる方向に修正されていることになる。したがって、そのような条件下において、移動体の現在位置から遠い方の予測軌道を修正するための修正量を、現在位置から近い方の予測軌道に反映させたときには、現在位置から近い方の予測軌道において、干渉対象に対する回避マージンを減少させてしまうことになる。

これに対して、この軌道生成装置によれば、２つの予測軌道が干渉対象を回避するために互いに異なる方向に修正されている際には、移動体の現在位置から近い方の予測軌道が、移動体の現在位置から遠い方の予測軌道を修正するための修正量とは無関係に生成される。それにより、現在位置から近い方の予測軌道において、干渉対象に対する回避マージンを適切に確保することができる。

請求項３に係る発明は、請求項１又は２に記載の軌道生成装置１，１００において、移動体の現在位置から近い方の予測軌道は、移動体の現在位置から遠い方の予測軌道よりも短い複数の予測軌道に分割された状態で生成されることを特徴とする。

移動体の現在位置から近い方の予測軌道の場合、この予測軌道の近傍に干渉対象が存在するときには、移動体との距離が短いことに起因して、干渉対象との干渉をより確実に回避できるように、予測軌道を生成する必要がある。これに対して、この軌道生成装置によれば、移動体の現在位置から近い方の予測軌道が、移動体の現在位置から遠い方の予測軌道よりも短い複数の予測軌道に分割された状態で生成されるので、これらの複数の予測軌道を、干渉対象との干渉をより確実に回避できるように生成することができる。

請求項４に係る発明は、請求項１ないし３のいずれかに記載の軌道生成装置１において、移動体は、周辺状況認識部、予測軌道生成部、干渉度合いパラメータ算出部及び修正量決定部を備えていることを特徴とする。

この軌道生成装置によれば、干渉対象が移動体の進行方向に存在する場合には、移動体の干渉対象から遠い方の予測軌道において、干渉対象に対する回避マージンを増大させることができる。それにより、実現可能な軌道の制約が一般的に大きい移動体（例えば、車両などの真横に移動できない移動体）においても、干渉対象との干渉を回避できるように、移動体の未来の軌道を適切に生成することができる。

請求項５に係る発明は、請求項１ないし３のいずれかに記載の軌道生成装置１００において、移動体は、周辺状況認識部、予測軌道生成部、干渉度合いパラメータ算出部及び修正量決定部を備えた第１移動体（自車両３）以外の第２移動体（歩行者Ｍ１）であり、予測軌道生成部は、第２移動体の過去の位置、複数の軌道モデル及び複数の修正量を用いて、複数の予測軌道を生成することを特徴とする。

この軌道生成装置によれば、第１移動体以外の第２移動体の複数の予測軌道を、第２移動体の過去の軌道を反映させながら適切に生成することができる。それにより、第１移動体の移動状態を制御する際、第２移動体との干渉を回避しながら移動するように、第１移動体を制御することが可能になる。

請求項６に係る発明は、請求項５に記載の軌道生成装置１において、予測軌道生成部は、第２移動体の過去の位置の時系列と、第２移動体の現在位置から近い方の予測軌道を予測軌道に沿って第２移動体の過去の位置側に延ばしたときの軌道上の複数の推定位置の時系列との差分が減少するように、第２移動体の現在位置から近い方の予測軌道を生成することを特徴とする。

この軌道生成装置によれば、第２移動体の現在位置から近い方の予測軌道が、第２移動体の過去の位置の時系列と、複数の推定位置の時系列との差分が減少するように生成される。すなわち、第２移動体の現在位置から近い方の予測軌道を、第２移動体の過去の位置に対する追従性を考慮しながら生成することができ、その生成精度を向上させることができる。

請求項７に係る発明は、移動体の未来の軌道を演算処理装置（ＥＣＵ２）によって生成する軌道生成方法であって、演算処理装置は、移動体の周辺状況を認識する周辺状況認識ステップと、周辺状況認識ステップによる周辺状況の認識結果に基づき、移動体の軌道をモデル化した複数の軌道モデルを用いて、移動体の未来の軌道を複数に分割したときの複数の未来の軌道の予測値である複数の予測軌道を生成する予測軌道生成ステップと、複数の予測軌道を用いて、移動体の周辺における干渉対象の移動体との干渉度合いを表す複数の干渉度合いパラメータを算出する干渉度合いパラメータ算出ステップと、複数の干渉度合いパラメータが表す干渉度合いが減少するように、複数の予測軌道をそれぞれ修正するための複数の修正量を決定する修正量決定ステップと、を実行し、予測軌道生成ステップでは、周辺状況の認識結果に基づき、複数の軌道モデル及び複数の修正量を用いて、複数の予測軌道が生成されるとともに、互いに隣り合う２つの予測軌道を修正するための２つの修正量が２つの予測軌道を同じ方向に修正するように決定されている場合には、２つの予測軌道のうちの、移動体の現在位置から近い方の予測軌道が、移動体の現在位置から遠い方の予測軌道を修正するための修正量を反映させながら生成されることを特徴とする。

本発明の第１実施形態に係る軌道生成装置を備えた車両の構成を模式的に示す図である。 第１実施形態の軌道生成装置の機能的な構成を示すブロック図である。 自車両の座標系と交通参加者の座標系を説明するための図である。 自車両が交通参加者を回避する際の軌道の一例を示す図である。 自車両が交通参加者を回避する際の軌道の他の一例を示す図である。 自車両の第１～第３予測軌道を説明するための図である。 ｘ方向リスクポテンシャルの算出に用いるマップの一例を示す図である。 ｙ方向リスクポテンシャルの算出に用いるマップの一例を示す図である。 自車両の走路の右側境界線及び左側境界線を定義した関数を説明するための図である。 右側走路境界リスクポテンシャルと右側走路境界関数の関係を示す図である。 左側走路境界リスクポテンシャルと左側走路境界関数の関係を示す図である。 軌道決定部の構成を示すブロック図である。 極値探索コントローラの構成を示すブロック図である。 印加探索入力と評価関数の関係を示す図である。 移動平均値と印加探索入力の関係を示す図である。 ゲインＫ_ｂｐ_２１の算出に用いるマップを示す図である。 ゲインＫ_ｂｐ_２１の算出に用いるマップを示す図である。 ゲインＫ_ｂｐ_３１の算出に用いるマップを示す図である。 ゲインＫ_ｂｐ_３１の算出に用いるマップを示す図である。 ゲインＫ_ｂｐ_３２の算出に用いるマップを示す図である。 ゲインＫ_ｂｐ_３２の算出に用いるマップを示す図である。 自車両の右前方に干渉対象が存在する場合の第１～第３予測軌道の決定結果の一例を示す図である。 自車両の第１～第３予測軌道の決定結果の一例を示す図である。 自車両の第１～第３予測軌道の決定結果の他の一例を示す図である。 移動平均値Ｐ_ｃ１～Ｐ_ｃ３と予測曲率ρ_ｍ１の演算値との関係を示す図である。 移動平均値Ｐ_ｃ２～Ｐ_ｃ３と予測曲率ρ_ｍ２の演算値との関係を示す図である。 第２実施形態の軌道生成装置の機能的な構成を示すブロック図である。 歩行者Ｍ１の座標系と他の交通参加者ＴＰ４の座標系を説明するための図である。 歩行者Ｍ１の第１～第３予測軌道を説明するための図である。 ｘ方向リスクポテンシャルの算出に用いるマップの一例を示す図である。 ｙ方向リスクポテンシャルの算出に用いるマップの一例を示す図である。 歩行者が移動する歩道の右側境界線及び左側境界線を定義した関数を説明するための図である。 右側走路境界リスクポテンシャルと右側走路境界関数の関係を示す図である。 左側走路境界リスクポテンシャルと左側走路境界関数の関係を示す図である。 横断意思パラメータの算出に用いるマップを示す図である。 横断意思パラメータの算出に用いるマップを示す図である。 横断意思パラメータを使用しない場合の歩行者Ｍ１の予測軌道を示す図である。 横断意思パラメータを使用した場合の歩行者Ｍ１の予測軌道を示す図である。 トレースポテンシャルの算出手法を説明するための図である。 第２実施形態の軌道決定部の構成を示すブロック図である。 ゲインＫ２_ｂｐ_２１の算出に用いるマップを示す図である。 ゲインＫ２_ｂｐ_２１の算出に用いるマップを示す図である。 ゲインＫ２_ｂｐ_３１の算出に用いるマップを示す図である。 ゲインＫ２_ｂｐ_３１の算出に用いるマップを示す図である。 ゲインＫ２_ｂｐ_３２の算出に用いるマップを示す図である。 ゲインＫ２_ｂｐ_３２の算出に用いるマップを示す図である。 歩行者Ｍ１の右前方に干渉対象が存在する場合の第１～第３予測軌道の決定結果の一例を示す図である。 歩行者Ｍ１の第１～第３予測軌道の決定結果の一例を示す図である。 歩行者Ｍ１の第１～第３予測軌道の決定結果の他の一例を示す図である。 移動平均値Ｐ２_ｃ１～Ｐ２_ｃ３と予測曲率ρ２_ｍ１の演算値との関係を説明するための図である。 移動平均値Ｐ２_ｃ２～Ｐ２_ｃ３と予測曲率ρ２_ｍ２の演算値との関係を説明するための図である。

以下、図面を参照しながら、本発明の第１実施形態に係る軌道生成装置について説明する。図１に示すように、この軌道生成装置１は、自動運転可能な四輪車両３（以下「自車両３」という）に適用されたものである。なお、本実施形態では、自車両３が移動体に相当する。この軌道生成装置１では、自車両３の自動運転制御を実行するために、後述するアルゴリズムによって、自車両３の未来の走行軌道が決定される。

この軌道生成装置１は、ＥＣＵ２を備えており、このＥＣＵ２には、情報検出装置４、原動機５及びアクチュエータ６が電気的に接続されている。この情報検出装置４は、カメラ、ミリ波レーダ、ＬＩＤＡＲ、レーザーレーダ、ソナー、ＧＰＳ、各種のセンサ、及び、ＩＴＳやＶＴＳと呼ばれるインフラからの情報受信装置などで構成されている。

情報検出装置４は、自車両３の位置及び自車両３の進行方向の周辺情報を検出してＥＣＵ２に出力する。この場合、周辺情報としては、車道、歩道、横断歩道、信号、車道と歩道の境界及び交通参加者（歩行者、他の車両、障害物）の情報などが含まれている。なお、本実施形態では、情報検出装置４が周辺状況認識部に相当する。

原動機５は、例えば、電気モータなどで構成されており、自車両３の未来の走行軌道が決定されたときに、自車両３がこの走行軌道で走行するように、ＥＣＵ２によって原動機５の出力が制御される。

また、アクチュエータ６は、制動用アクチュエータ及び操舵用アクチュエータなどで構成されており、自車両３の未来の走行軌道が決定されたときに、自車両３がこの走行軌道で走行するように、ＥＣＵ２によってアクチュエータ６の動作が制御される。

一方、ＥＣＵ２は、ＣＰＵ、ＲＡＭ、ＲＯＭ、Ｉ／Ｏインターフェース及び各種の電気回路（いずれも図示せず）などからなるマイクロコンピュータで構成されている。ＥＣＵ２は、後述するように、上述した情報検出装置４からの周辺情報に基づいて、自車両３以外の交通参加者（例えば、後述する交通参加者ＴＰ１）の行動を予測するとともに、交通参加者の行動の予測結果、自車両３の位置及び自車両３の周辺の交通環境などに基づいて、自車両３の未来の走行軌道を決定する。

なお、本実施形態では、ＥＣＵ２が、周辺状況認識部、予測軌道生成部、干渉度合いパラメータ算出部、修正量決定部及び演算処理装置に相当する。

次に、図２を参照しながら、本実施形態の軌道生成装置１の機能的な構成について説明する。この軌道生成装置１は、自車両３の自動運転制御を実行するために、以下に述べるアルゴリズムによって、自車両３の走行軌道を決定するものである。

また、以下の説明では、自車両３の走行に対して干渉する対象を「干渉対象」という。この場合、後述するように、自車両３以外の交通参加者、及び交通環境（道路環境や信号の状態など）が干渉対象に相当する。

図２に示すように、軌道生成装置１は、交通参加者検出部１０、行動予測部１１、第１～第３評価関数算出部２１～２３及び軌道決定部３０を備えている。これらの要素１０～１１，２１～２３，３０は、具体的には、ＥＣＵ２によって構成されており、これらの要素１０～１１，２１～２３，３０での演算処理は、所定の制御周期ΔＴ（例えば５ｍｓｅｃ）で実行される。

なお、本実施形態では、第１～第３評価関数算出部２１～２３が干渉度合いパラメータ算出部に相当し、軌道決定部３０が予測軌道生成部及び修正量決定部に相当する。

まず、交通参加者検出部１０では、情報検出装置４からの周辺情報に基づき、所定の機械学習アルゴリズム（例えば、ＤＮＮ及びＣＮＮなどのニューラルネットワークを適用したアルゴリズム）を用いて、交通参加者の現在検出位置Ｗ_ｖｊ（ｋ）が検出／算出される。

この場合、現在検出位置Ｗ_ｖｊ（ｋ）における記号「（ｋ）」は、制御時刻を表しており、以下の説明では、この記号「（ｋ）」などを適宜省略する。また、現在検出位置Ｗ_ｖｊ（ｋ）における添え字「ｊ」は、存在する交通参加者の番号／数を表している。すなわち、交通参加者が１個の場合には、ｊ＝１となり、交通参加者がｎ（ｎは複数）個の場合には、ｊ＝１～ｎとなる。

以下、図３に示すように、他の車両である交通参加者ＴＰ１のみが自車両３の前方に存在する場合（すなわちｊ＝１の場合）を例にとって説明する。ここで、自車両３の座標系は、後述するように、自車両３の現在位置の中心を原点とし、自車両３の進行方向及び左方向をＸ軸及びＹ軸とする直交２次元座標系として規定される。また、図中のＺ_ｍ１１，Ｚ_ｍ１２，Ｚ_ｍ１３は、自車両３の後述する予測位置である。

一方、交通参加者ＴＰ１の座標系は、交通参加者ＴＰ１の中心を原点とし、交通参加者ＴＰ１の進行方向及び横方向をＸ_ｖ軸及びＹ_ｖ軸とし、Ｘ_ｖ軸のＸ軸に対する傾きをθ_ｖ１とする直交２次元座標系として規定される。

この直交２次元座標系において、交通参加者ＴＰ１のＸ_ｖ座標値をｘ_ｖ１とし、Ｙ_ｖ座標値をｙ_ｖ１とした場合、交通参加者ＴＰ１の現在検出位置Ｗ_ｖ１（ｋ）は、３つの値［ｘ_ｖ１（ｋ），ｙ_ｖ１（ｋ），θ_ｖ１（ｋ）］を要素とする値として算出される（ただし、ｘ_ｖ１（ｋ）＝ｙ_ｖ１（ｋ）＝０）。また、図３中の交通参加者ＴＰ１の周囲における１点鎖線で示すラインは、後述するリスクポテンシャルを表している。

行動予測部１１では、交通参加者検出部１０による現在検出位置Ｗ_ｖｊ（ｋ）の検出結果に基づき、所定の機械学習アルゴリズム（例えば、ＤＮＮ及びＣＮＮなどのニューラルネットワークを適用したアルゴリズム）を用いて、交通参加者ＴＰ１の現在位置Ｗ_ｖ１（ｋ：ｋ）と、５個の予測位置Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１１），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１２），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１３），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ２），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ３）とが算出される。

この場合、予測位置Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１１）の括弧内における「：」の左側の値ｋは、現在の制御時刻を表しており、「：」の右側の値ｋ＋ｋｍ１１は、予測時刻を表している。また、現在の制御時刻ｋと予測時刻ｋ＋ｋｍ１３との間隔、予測時刻ｋ＋ｋｍ１３と予測時刻ｋ＋ｋｍ２の間隔、及び、予測時刻ｋ＋ｋｍ２と予測時刻ｋ＋ｋｍ３の間隔は、互いに同一に設定されている。

さらに、現在の制御時刻ｋと予測時刻ｋ＋ｋｍ１１との間隔、予測時刻ｋ＋ｋｍ１１と予測時刻ｋ＋ｋｍ１２の間隔、及び、予測時刻ｋ＋ｋｍ１２と予測時刻ｋ＋ｋｍ１３の間隔は、互いに同一に設定されている。すなわち、２つの予測時刻ｋ＋ｋｍ１１，ｋ＋ｋｍ１２は、現在の制御時刻ｋと予測時刻ｋ＋ｋｍ１３の間を３等分したときの、現在の制御時刻ｋと予測時刻ｋ＋ｋｍ１３の間における２つの時刻に相当する。また、現在位置Ｗ_ｖ１（ｋ：ｋ）は、現在検出位置Ｗ_ｖ１（ｋ）に設定される。

以上のように、行動予測部１１では、交通参加者が１個の場合、交通参加者の６個の位置（現在位置及び５個の予測位置）が算出され、交通参加者がｎ個の場合には、ｎ×６個の位置が算出される。この点は、前述した各要素１１，２１～２３，３０，４０の演算においても同様である。以下、交通参加者が１個の場合を例にとって説明する。

なお、行動予測部１１において、所定の機械学習アルゴリズに代えて、後述する第２実施形態の手法を用いて、５個の予測位置Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１１），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１２），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１３），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ２），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ３）を算出してもよい。また、行動予測部１１において、４個以下又は６個以上の予測位置を算出するように構成してもよい。

次に、本実施形態の軌道生成装置１における自車両３の未来の走行軌道（以下「自車軌道」という）の定義及びその決定原理について説明する。

例えば、図４に示すように、自車両３が左車線を走行中、歩行者である交通参加者ＴＰ２が左前方に存在し、この交通参加者ＴＰ２が左車線側に向かって歩行している場合、交通参加者ＴＰ２との干渉を回避するために、自車両３が右車線に移動するように、自車軌道を決定する必要がある。それと同時に、自車両３が右車線から右外側にはみ出さないように、自車軌道を決定する必要がある。

また、例えば、図５に示すように、自車両３が左車線を走行中、図４と同様の交通参加者ＴＰ２に加えて、歩行者である交通参加者ＴＰ３が右車線内に存在する場合、交通参加者ＴＰ２との干渉を回避するために、自車両３が右車線側に移動した後、交通参加者ＴＰ３との干渉を回避するために、自車両３が左車線側に移動するように、自車軌道を決定する必要がある。

以上の条件を満たすために、本実施形態では、図６に示すように、自車軌道が３つの円弧状の第１～第３予測軌道を組み合わせたものとして決定される。最初に、第１予測軌道について説明する。まず、自車両３の現在位置の中心を原点とし、自車両３の前方をＸ軸とし、自車両３の左方をＹ軸とするＸ－Ｙ座標系を定義する。

そして、このＸ－Ｙ座標系における原点から第１予測時間Ｔ_ｍ１後の位置を第１予測位置Ｚ_ｍ１とし、原点から第１予測位置Ｚ_ｍ１までの自車軌道を円弧状の第１予測軌道と定義する。ここで、自車両３の現在速度をＶ_ｅｇｏとした場合、第１予測軌道の距離Ｌ_ｍ１は、下式（１）のように定義される。

さらに、第１予測軌道の曲率半径をＲ_ｍ１とした場合、第１予測軌道の回転角度θ_ｍ１は、下式（２）のように定義される。この回転角度θ_ｍ１は、Ｙ軸と後述するＹ’軸の間の角度に相当する。

また、原点から第１予測位置Ｚ_ｍ１までの自車軌道を３等分したときの予測位置を、原点から第１予測位置Ｚ_ｍ１に向かって順に、３つの予測位置Ｚ_ｍ１１，Ｚ_ｍ１２，Ｚ_ｍ１３（＝Ｚ_ｍ１）とする。その場合、予測位置Ｚ_ｍ１１のＸ座標値ｘ_ｍ１１及びＹ座標値ｙ_ｍ１１は、下式（３），（４）のように定義される。

また、予測位置Ｚ_ｍ１２のＸ座標値ｘ_ｍ１２及びＹ座標値ｙ_ｍ１２は、下式（５），（６）のように定義される。

さらに、予測位置Ｚ_ｍ１３のＸ座標値ｘ_ｍ１３及びＹ座標値ｙ_ｍ１３は、下式（７），（８）のように定義される。

以上の式（１）～（８）が第１予測軌道のモデル式（モデル軌道）に相当する。

次に、第２予測軌道について説明する。まず、第１予測位置Ｚ_ｍ１（＝予測位置Ｚ_ｍ１３）を原点とし、第１予測軌道の第１予測位置Ｚ_ｍ１における接線方向をＸ’軸とし、この接線と直交する方向をＹ’軸とするＸ’－Ｙ’座標系を定義する。

さらに、このＸ’－Ｙ’座標系における原点すなわち第１予測位置Ｚ_ｍ１から第２予測時間Ｔ_ｍ２後の位置を第２予測位置Ｚ_ｍ２とし、第１予測位置Ｚ_ｍ１から第２予測位置Ｚ_ｍ２までの自車軌道を円弧状の第２予測軌道と定義する。この場合、第２予測軌道の距離Ｌ_ｍ２は、下式（９）のように定義される。

さらに、第２予測軌道の曲率半径をＲ_ｍ２とした場合、第２予測軌道の回転角度θ_ｍ２は、下式（１０）のように定義される。この回転角度θ_ｍ２は、Ｙ’軸と後述するＹ”軸の間の角度に相当する。

そして、第２予測位置Ｚ_ｍ２のＸ’座標値ｘ’_ｍ２及びＹ’座標値ｙ’_ｍ２は、下式（１１），（１２）のように定義される。

さらに、第２予測位置Ｚ_ｍ２のＸ座標値ｘ_ｍ２及びＹ座標値ｙ_ｍ２は、下式（１３），（１４）のように定義される。

以上の式（９）～（１４）が第２予測軌道のモデル式（モデル軌道）に相当する。

次に、第３予測軌道について説明する。まず、第２予測位置Ｚ_ｍ２を原点とし、第２予測軌道の第２予測位置Ｚ_ｍ２における接線方向をＸ”軸とし、この接線と直交する方向をＹ”軸とするＸ”－Ｙ”座標系を定義する。

さらに、このＸ”－Ｙ”座標系における原点（すなわち第２予測位置Ｚ_ｍ２）から第３予測時間Ｔ_ｍ３後の位置を第３予測位置Ｚ_ｍ３とし、第２予測位置Ｚ_ｍ２から第３予測位置Ｚ_ｍ３までの自車軌道を円弧状の第３予測軌道と定義する。この場合、第３予測軌道の距離Ｌ_ｍ３は、下式（１５）のように定義される。

さらに、第３予測軌道の曲率半径をＲ_ｍ３とした場合、第３予測軌道の回転角度θ_ｍ３は、下式（１６）のように定義される。

そして、第３予測位置Ｚ_ｍ３のＸ”座標値ｘ”_ｍ３及びＹ”座標値ｙ”_ｍ３は、下式（１７），（１８）のように定義される。

さらに、第３予測位置Ｚ_ｍ３のＸ座標値ｘ_ｍ３及びＹ座標値ｙ_ｍ３は、下式（１９），（２０）のように定義される。

以上の式（１５）～（２０）が第３予測軌道のモデル式（モデル軌道）に相当する。

以上のように、第１予測軌道は、第２予測軌道及び第３予測軌道よりも短い３つの予測軌道を組み合わせたものとして決定される。具体的には、現在位置から予測位置Ｚ_ｍ１１までの軌道と、予測位置Ｚ_ｍ１１から予測位置Ｚ_ｍ１２までの軌道と、予測位置Ｚ_ｍ１２から予測位置Ｚ_ｍ１３までの軌道を組み合わせたものとして、第１予測軌道が決定される。

これは、第１予測軌道は、第２予測軌道及び第３予測軌道と比べて、自車両３との距離が近いことに起因して、干渉対象との干渉をより確実に回避できるように、予測軌道を生成する必要があるので、それを実現するためである。なお、第１予測軌道を２つ又は４つ以上の予測軌道を組み合わせたものとして決定してもよい。

次に、前述した第１～第３評価関数算出部２１～２３について説明する。これらの第１～第３評価関数算出部２１～２３では、以上のように決定された５つの予測位置Ｚ_ｍ１１，Ｚ_ｍ１２，Ｚ_ｍ１３，Ｚ_ｍ２，Ｚ_ｍ３を用いて、自車両３と干渉対象との干渉度合いを表すものとして、第１評価関数Ｊ_１、第２評価関数Ｊ_２及び第３評価関数Ｊ_３がそれぞれ算出される。

ここで、第１評価関数Ｊ_１、第２評価関数Ｊ_２及び第３評価関数Ｊ_３は、後述するように、５つの予測位置Ｚ_ｍ１１，Ｚ_ｍ１２，Ｚ_ｍ１３，Ｚ_ｍ２，Ｚ_ｍ３の算出に用いられるので、第１評価関数Ｊ_１、第２評価関数Ｊ_２及び第３評価関数Ｊ_３の今回値は、５つの予測位置Ｚ_ｍ１１，Ｚ_ｍ１２，Ｚ_ｍ１３，Ｚ_ｍ２，Ｚ_ｍ３の前回値に基づいて算出されることになる。

なお、以下の説明では、５つの予測位置Ｚ_ｍ１１，Ｚ_ｍ１２，Ｚ_ｍ１３，Ｚ_ｍ２，Ｚ_ｍ３をまとめて「予測位置Ｚ_ｍｉ」と表記する。すなわち、「予測位置Ｚ_ｍｉ」の添え字ｉは、ｉ＝１１～１３，２，３を表すものである。

以下、第１～第３評価関数算出部２１～２３において、第１～第３評価関数Ｊ_１～Ｊ_３の算出に用いる手法及びその原理について説明する。

まず、リスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖの算出手法について説明する。このリスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖは、自車両３が予測位置Ｚ_ｍｉにあり、かつ交通参加者などの干渉対象が予測位置Ｗ_ｖｊにあると想定した場合の、自車両３と干渉対象との干渉度合いを表すものである。

まず、ｘ座標偏差ｄｘ_ｍｉ_ｖｊ及びｙ座標偏差ｄｙ_ｍｉ_ｖｊが、下式（２１），（２２）により算出される。

これらの偏差ｄｘ_ｍｉ_ｖｊ，ｄｙ_ｍｉ_ｖｊは、自車両３の予測位置Ｚ_ｍｉと干渉対象の予測位置Ｗ_ｖｊとの偏差を表している。この場合、例えば、自車両３の予測位置Ｚ_ｍｉ＝Ｚ_ｍ１１である場合、これと干渉対象の予測位置Ｗ_ｖｊ（ｋ：ｋ＋ｋｍ１１）との偏差が算出される。

次いで、変換ｘ座標偏差ｄｘ_ｍｉ_ｖｊ_ｖ及び変換ｙ座標偏差ｄｙ_ｍｉ_ｖｊ_ｖが、下式（２３），（２４）により算出される。これらの偏差ｄｘ_ｍｉ_ｖｊ_ｖ，ｄｙ_ｍｉ_ｖｊ_ｖは、上記２つの偏差ｄｘ_ｍｉ_ｖｊ，ｄｙ_ｍｉ_ｖｊを、干渉対象の座標系に変換したものである。

次に、変換ｘ座標偏差ｄｘ_ｍｉ_ｖｊ_ｖに応じて、図７に示すマップを検索することにより、ｘ方向リスクポテンシャルＲ_ｐｘ_ｍｉ_ｖｊ_ｖが算出される。このｘ方向リスクポテンシャルＲ_ｐｘ_ｍｉ_ｖｊ_ｖのマップ値の横幅及び形状などは、干渉対象の種類（例えば、歩行者、自転車、４輪車両、オートバイなど）に応じて設定される。

さらに、変換ｙ座標偏差ｄｙ_ｍｉ_ｖｊ_ｖに応じて、図８に示すマップを検索することにより、ｙ方向リスクポテンシャルＲ_ｐｙ_ｍｉ_ｖｊ_ｖが算出される。このｙ方向リスクポテンシャルＲ_ｐｙ_ｍｉ_ｖｊ_ｖのマップ値の横幅及び形状なども、ｘ方向リスクポテンシャルＲ_ｐｘ_ｍｉ_ｖｊ_ｖのマップ値と同様に、干渉対象の種類（例えば、歩行者、自転車、４輪車両、オートバイなど）に応じて設定される。

そして、下式（２５）に示すように、ｘ方向リスクポテンシャルＲ_ｐｘ_ｍｉ_ｖｊ_ｖとｙ方向リスクポテンシャルＲ_ｐｙ_ｍｉ_ｖｊ_ｖの積として、リスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖが算出される。

以上の手法により、リスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖは、自車両３と干渉対象との干渉度合が大きくなるほど、より大きい値になるように算出される。

次に、右側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍｉ及び左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｌ_ｍｉの算出手法について説明する。これらの２つのリスクポテンシャルＲ_ｐ_ｒｂｒ_ｍｉ，Ｒ_ｐ_ｒｂｌ_ｍｉは、自車両３が予測位置Ｚ_ｍｉにあると想定した場合の、自車両３が走路逸脱を生じる可能性を表す値である。

まず、自車両３の走路の右側境界線ＬＲ及び左側境界線ＬＬが、例えば図９に示すようになっている場合、これらの境界線ＬＲ，ＬＬは、下式（２６），（２７）に示す２つの１次関数として定義することができる。

上式（２６）におけるａ_ｒｂｒ，ｂ_ｒｂｒは、右側境界線ＬＲを規定するパラメータであり、上式（２７）におけるａ_ｒｂｌ，ｂ_ｒｂｌは、左側境界線ＬＬを規定するパラメータである。これらの４つのパラメータａ_ｒｂｒ，ｂ_ｒｂｒ，ａ_ｒｂｌ，ｂ_ｒｂｌは、前述した情報検出装置４の検出結果に基づき、リアルタイムで算出される。

次に、右側走路境界関数σ_ｒ及び左側走路境界関数σ_ｌを、下式（２８），（２９）に示すように定義する。

これらの式（２８），（２９）は、上式（２６），（２７）の左辺を右辺に移項したものに相当する。したがって、右側走路境界関数σ_ｒが正値である場合、自車両３の中心が右側境界線ＬＲよりも左側の走路内の領域に存在し、右側走路境界関数σ_ｒが負値である場合、自車両３の中心が右側境界線ＬＲよりも右側の走路外の領域に存在することになる。

これと同様に、左側走路境界関数σ_ｌが正値である場合、自車両３の中心が左側境界線ＬＬよりも左側の走路外の領域に存在し、左側走路境界関数σ_ｌが負値である場合、自車両３の中心が左側境界線ＬＬよりも右側の走路内の領域に存在することになる。

以上の原理に基づき、右側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒは、下式（３０），（３１）により算出され、左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｌは、下式（３２），（３３）により算出される。

以上の式（３０）～（３３）におけるσ_ｍｒｇは、自車両３が走路逸脱を確実に回避できるようにするためのマージン（正の所定値）である。ここで、右側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒと右側走路境界関数σ_ｒの関係、及び、左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｌと左側走路境界関数σ_ｌの関係を図式化した場合、図１０，１１に示すものとなる。

次に、以上と同じ原理により、自車両３の予測位置Ｚ_ｍｉにおける右側走路境界関数σ_ｒ_ｍｉ及び左側走路境界関数σ_ｌ_ｍｉを、下式（３４），（３５）に示すように定義する。

さらに、前述した原理により、自車両３の予測位置Ｚ_ｍｉにおける右側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍｉは、下式（３６），（３７）により算出され、自車両３の予測位置Ｚ_ｍｉにおける左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｌ_ｍｉは、下式（３８），（３９）により算出される。

なお、自車両３が中央分離帯又はセンターラインが存在する道路を走行中の場合には、以上と同じ手法により、中央分離帯又はセンターラインに対する走路境界リスクポテンシャルを算出することが可能である。

次に、前述した第１評価関数算出部２１における第１評価関数Ｊ_１の算出手法について説明する。この第１評価関数算出部２１の算出に用いる各種パラメータの添え字「ｉ」は、ｉ＝１１～１３を表すものである。

第１評価関数算出部２１では、前述したリスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖ、右側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍｉ及び左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｌ_ｍｉの算出手法及び算出原理に基づき、以下に述べるように、軌道決定部３０からの３つの印加予測位置Ｚ_ｍｉ_ｒを用いて、第１評価関数Ｊ_１が算出される。これらの印加予測位置Ｚ_ｍｉ_ｒは、後述するように、前述した３つの予測位置Ｚ_ｍｉに後述する参照入力ｒを印加したものである。

第１評価関数算出部２１では、予測位置Ｚ_ｍｉに参照入力ｒを印加した印加予測位置Ｚ_ｍｉ_ｒが用いられる関係上、以下の説明では、印加予測位置Ｚ_ｍｉ_ｒと同様に、前述した各種パラメータの最後に「_ｒ」を付加したものを用いる。この点は、第２評価関数算出部２２及び第３評価関数算出部２３においても同様である。

第１評価関数算出部２１では、まず、下式（４０），（４１）により、現在の制御時刻でのｘ座標偏差ｄｘ_ｍｉ_ｖｊ_ｒ（ｋ：ｋ）及びｙ座標偏差ｄｙ_ｍｉ_ｖｊ_ｒ（ｋ：ｋ）が算出される。

上式（４０）のｘ_ｍｉ_ｒは、印加予測位置Ｚ_ｍｉ_ｒのＸ座標値であり、ｘ_ｖｊ（ｋ：ｋ）は、交通参加者などの干渉対象の現在位置Ｗ_ｖｊ（ｋ：ｋ）のＸ座標値である。また、上式（４１）のｙ_ｍｉ_ｒは、印加予測位置Ｚ_ｍｉ_ｒのＹ座標値であり、ｙ_ｖｊ（ｋ：ｋ）は、干渉対象の現在位置Ｗ_ｖｊ（ｋ：ｋ）のＹ座標値である。

次いで、下式（４２），（４３）により、予測時刻ｋ＋ｋｍｉでのｘ座標偏差ｄｘ_ｍｉ_ｖｊ_ｒ（ｋ：ｋ＋ｋｍｉ）及びｙ座標偏差ｄｙ_ｍｉ_ｖｊ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。

上式（４２）のｘ_ｖｊ（ｋ：ｋ＋ｋｍｉ）は、干渉対象の予測位置Ｗ_ｖｊ（ｋ：ｋ＋ｋｍｉ）のＸ座標値であり、上式（４３）のｙ_ｖｊ（ｋ：ｋ＋ｋｍｉ）は、干渉対象の予測位置Ｗ_ｖｊ（ｋ：ｋ＋ｋｍｉ）のＹ座標値である。

さらに、下式（４４），（４５）により、現在の制御時刻での変換ｘ座標偏差ｄｘ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）及び変換ｙ座標偏差ｄｙ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）が算出される。

これらの偏差ｄｘ_ｍｉ_ｖｊ_ｖ_ｒ，ｄｙ_ｍｉ_ｖｊ_ｖ_ｒは、上記の自車両３の座標系における偏差ｄｘ_ｍｉ_ｖｊ_ｒ，ｄｙ_ｍｉ_ｖｊ_ｒを、干渉対象の座標系に変換した値に相当する。

次に、下式（４６），（４７）により、予測時刻ｋ＋ｋｍｉでの変換ｘ座標偏差ｄｘ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）及び変換ｙ座標偏差ｄｙ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。

これらの偏差ｄｘ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ），ｄｙ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）は、上記の自車両３の座標系における偏差ｄｘ_ｍｉ_ｖｊ_ｒ（ｋ：ｋ＋ｋｍｉ），ｄｙ_ｍｉ_ｖｊ_ｒ（ｋ：ｋ＋ｋｍｉ）を、干渉対象の座標系に変換した値に相当する。

さらに、前述した変換ｘ座標偏差ｄｘ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）に応じて、図示しないマップを検索することにより、ｘ方向リスクポテンシャルＲ_ｐｘ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）が算出される。この場合、マップとしては、前述した図７と同様のものが用いられる。

また、変換ｙ座標偏差ｄｙ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）に応じて、図示しないマップを検索することにより、ｙ方向リスクポテンシャルＲ_ｐｙ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）が算出される。この場合、マップとしては、前述した図８と同様のものが用いられる。

次いで、下式（４８）により、現在の制御時刻ｋでのリスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）が算出される。

さらに、前述した変換ｘ座標偏差ｄｘ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）に応じて、ｘ方向リスクポテンシャルＲ_ｐｘ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）の算出に用いたマップを検索することにより、ｘ方向リスクポテンシャルＲ_ｐｘ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。

また、前述した変換ｙ座標偏差ｄｙ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）に応じて、ｙ方向リスクポテンシャルＲ_ｐｙ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）の算出に用いたマップを検索することにより、ｙ方向リスクポテンシャルＲ_ｐｙ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。

次いで、下式（４９）により、予測時刻ｋ＋ｋｍｉでのリスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。なお、本実施形態では、リスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ），Ｒ_ｐ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が干渉度合いパラメータに相当する。

また、自車両３の予測位置Ｚ_ｍｉにおける右側走路境界関数σ_ｒ_ｍｉ_ｒ及び左側走路境界関数σ_ｌ_ｍｉ_ｒは、下式（５０），（５１）に示すように定義される。

さらに、右側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍｉ_ｒは、下式（５２），（５３）によって算出され、左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｌ_ｍｉ_ｒは、下式（５４），（５５）によって算出される。なお、本実施形態では、右側及び左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ_ｐ_ｒｂｌ_ｍｉ_ｒが干渉度合いパラメータに相当する。

そして、最終的に、第１評価関数Ｊ_１が、下式（５６）によって算出される。

上式（５６）において、ｎは、自車両３の前方に位置する交通参加者などの干渉対象の数を表している。この式（５６）に示すように、第１評価関数Ｊ_１は、現在の制御時刻ｋ及び予測時刻ｋ＋ｋｍｉ（ｉ＝１１～１３）における、３つの予測位置Ｚ_ｍｉ（ｉ＝１１～１３）のリスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖ_ｒの総和と、右側及び左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ_ｐ_ｒｂｌ_ｍｉ_ｒとの和として算出される。

次に、前述した第２評価関数算出部２２における第２評価関数Ｊ_２の算出手法について説明する。この第２評価関数算出部２２では、以下に述べるように、軌道決定部３０からの印加第２予測位置Ｚ_ｍ２_ｒを用いて、第１評価関数算出部２１と同様の手法により、第２評価関数Ｊ_２が算出される。この印加予測位置Ｚ_ｍ２_ｒは、第２予測位置Ｚ_ｍ２に参照入力ｒを印加したものである。

第２評価関数算出部２２では、まず、下式（５７），（５８）により、予測時刻ｋ＋ｋｍ２でのｘ座標偏差ｄｘ_ｍ２_ｖｊ_ｒ（ｋ：ｋ＋ｋｍ２）及びｙ座標偏差ｄｙ_ｍ２_ｖｊ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。

上式（５７）のｘ_ｖｊ（ｋ：ｋ＋ｋｍ２）は、干渉対象の予測位置Ｗ_ｖｊ（ｋ：ｋ＋ｋｍ２）のＸ座標値であり、上式（５８）のｙ_ｖｊ（ｋ：ｋ＋ｋｍ２）は、干渉対象の予測位置Ｗ_ｖｊ（ｋ：ｋ＋ｋｍ２）のＹ座標値である。

さらに、下式（５９），（６０）により、予測時刻ｋ＋ｋｍ２での変換ｘ座標偏差ｄｘ_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）及び変換ｙ座標偏差ｄｙ_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。

次に、前述した変換ｘ座標偏差ｄｘ_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）に応じて、図示しないマップを検索することにより、ｘ方向リスクポテンシャルＲ_ｐｘ_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。この場合、マップとしては、前述した図７と同様のものが用いられる。

また、前述した変換ｙ座標偏差ｄｙ_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）に応じて、図示しないマップを検索することにより、ｙ方向リスクポテンシャルＲ_ｐｙ_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。この場合、マップとしては、前述した図８と同様のものが用いられる。

次いで、下式（６１）により、予測時刻ｋ＋ｋｍ２でのリスクポテンシャルＲ_ｐ_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。

また、自車両３の第２予測位置Ｚ_ｍ２における右側走路境界関数σ_ｒ_ｍ２_ｒ及び左側走路境界関数σ_ｌ_ｍ２_ｒは、下式（６２），（６３）に示すように定義される。

さらに、右側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍ２_ｒは、下式（６４），（６５）によって算出され、左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｌ_ｍ２_ｒは、下式（６６），（６７）によって算出される。

そして、最終的に、第２評価関数Ｊ_２が、下式（６８）によって算出される。

この式（６８）に示すように、第２評価関数Ｊ_２は、第２予測位置Ｚ_ｍ２におけるリスクポテンシャルＲ_ｐ_ｍ２_ｖｊ_ｖ_ｒの総和と、右側及び左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍ２_ｒ，Ｒ_ｐ_ｒｂｌ_ｍ２_ｒとの和として算出される。

次に、前述した第３評価関数算出部２３における第３評価関数Ｊ_３の算出手法について説明する。この第３評価関数算出部２３では、以下に述べるように、軌道決定部３０からの印加第３予測位置Ｚ_ｍ３_ｒを用いて、第１及び第２評価関数算出部２１，２２と同様の手法により、第３評価関数Ｊ_３が算出される。この印加予測位置Ｚ_ｍ３_ｒは、第３予測位置Ｚ_ｍ３に参照入力ｒを印加したものである。

第３評価関数算出部２３では、まず、下式（６９），（７０）により、予測時刻ｋ＋ｋｍ３でのｘ座標偏差ｄｘ_ｍ３_ｖｊ_ｒ（ｋ：ｋ＋ｋｍ３）及びｙ座標偏差ｄｙ_ｍ３_ｖｊ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。

上式（６９）のｘ_ｖｊ（ｋ：ｋ＋ｋｍ３）は、干渉対象の予測位置Ｗ_ｖｊ（ｋ：ｋ＋ｋｍ３）のＸ座標値であり、上式（７０）のｙ_ｖｊ（ｋ：ｋ＋ｋｍ３）は、干渉対象の予測位置Ｗ_ｖｊ（ｋ：ｋ＋ｋｍ３）のＹ座標値である。

さらに、下式（７１），（７２）により、予測時刻ｋ＋ｋｍ３での変換ｘ座標偏差ｄｘ_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）及び変換ｙ座標偏差ｄｙ_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。

次に、前述した変換ｘ座標偏差ｄｘ_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）に応じて、図示しないマップを検索することにより、ｘ方向リスクポテンシャルＲ_ｐｘ_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。この場合、マップとしては、前述した図７と同様のものが用いられる。

また、前述した変換ｙ座標偏差ｄｙ_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）に応じて、図示しないマップを検索することにより、ｙ方向リスクポテンシャルＲ_ｐｙ_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。この場合、マップとしては、前述した図８と同様のものが用いられる。

次いで、下式（７３）により、予測時刻ｋ＋ｋｍ３でのリスクポテンシャルＲ_ｐ_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。

また、自車両３の第３予測位置Ｚ_ｍ３における右側走路境界関数σ_ｒ_ｍ３_ｒ及び左側走路境界関数σ_ｌ_ｍ３_ｒは、下式（７４），（７５）に示すように定義される。

さらに、右側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍ３_ｒは、下式（７６），（７７）によって算出され、左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｌ_ｍ３_ｒは、下式（７８），（７９）によって算出される。

そして、最終的に、第３評価関数Ｊ_３が、下式（８０）によって算出される。

この式（８０）に示すように、第３評価関数Ｊ_３は、第３予測位置Ｚ_ｍ３におけるリスクポテンシャルＲ_ｐ_ｍ３_ｖｊ_ｖ_ｒの総和と、右側及び左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍ３_ｒ，Ｒ_ｐ_ｒｂｌ_ｍ３_ｒとの和として算出される。

次に、前述した軌道決定部３０について説明する。図１２に示すように、軌道決定部３０は、第１～第３極値探索コントローラ３１～３３及び第１～第３予測軌道算出部３４～３６を備えている。これらの第１～第３極値探索コントローラ３１～３３では、後述する手法により、第１～第３予測軌道の曲率である予測曲率ρ_ｍ１～ρ_ｍ３が算出される。これらの予測曲率ρ_ｍ１～ρ_ｍ３は、前述した図６における曲率半径Ｒ_ｍ１～Ｒ_ｍ３の逆数に相当する。

以下、第１～第３極値探索コントローラ３１～３３の算出手法及びその原理について、図１３に示す極値探索コントローラ５０を例にとって説明する。同図に示すように、この極値探索コントローラ５０は、印加探索入力ｕ_ｓｋ_ｒが入力された際に評価関数Ｊを出力する系が探索対象７０である場合において、評価関数Ｊを用いて、探索入力ｕ_ｓｋ及び印加探索入力ｕ_ｓｋ_ｒを算出するものである。

この極値探索コントローラ５０は、ハイパスフィルタ５１、参照入力発生器５２、２つの遅延要素５４，５８、乗算器５３、移動平均フィルタ５５、増幅要素５６、及び２つの加算要素５７，５９を備えている。

このハイパスフィルタ５１では、下式（８１）により、フィルタ値ｈが算出される。

上式（８１）に示すように、フィルタ値ｈは、評価関数Ｊの今回値Ｊ（ｋ）と前回値Ｊ（ｋ－１）の差分として算出される。また、ハイパスフィルタ５１は、評価関数値Ｊに含まれている、後述する参照入力ｒに起因する周波数成分を通過させるためのものである。この場合、上式（８１）に代えて、後述する参照入力ｒの周波数成分を通過させるフィルタアルゴリズム（バタワースハイパスフィルタアルゴリズム又はバンドパスフィルタアルゴリズム）を用いて、フィルタ値ｈを算出するように構成してもよい。

また、参照入力発生器５２は、参照入力ｒを発生する。なお、参照入力ｒの波形としては、例えば、正弦波、余弦波、三角波、台形波及び矩形波などが用いられる。

さらに、この参照入力ｒは、遅延要素５４を介して乗算器５３に入力され、乗算器５３では、下式（８２）により、中間値Ｐ_ｈｒが算出される。

また、移動平均フィルタ５５では、下式（８３）により、移動平均値Ｐ_ｃが算出される。

この式（８３）において、移動平均値Ｐ_ｃのタップ数ｎ_ｒ＋１は、移動平均値Ｐ_ｃから参照入力ｒの周波数成分を除去するために、タップ数ｎ_ｒ＋１と制御周期ΔＴの積ΔＴ・（ｎ_ｒ＋１）が参照入力ｒの周期Ｔｒと等しい値又は周期Ｔｒの整数倍になるように設定されている。

次いで、加算要素５７には、移動平均値Ｐ_ｃが増幅要素５６によって所定ゲインＫ_ｓｋ分、増幅された状態で入力されるとともに、遅延要素５８を介して、探索入力ｕ_ｓｋが入力される。

そして、加算要素５７では、下式（８４）に示す積分制御アルゴリズムより、探索入力ｕ_ｓｋが算出される。

さらに、加算要素５９では、下式（８５）により、印加探索入力ｕ_ｓｋ_ｒが算出される。

次に、以上の算出アルゴリズムを用いて、探索入力ｕ_ｓｋ及び印加探索入力ｕ_ｓｋ_ｒを算出した理由及びその原理について説明する。本実施形態の場合、前述したように、評価関数Ｊが各種のリスクポテンシャルの総和を含むように算出される関係上、評価関数Ｊが極小値になるように算出することによって、リスクポテンシャルの総和を減少させることができることになる。

この理由により、本実施形態では、評価関数Ｊが極小値となるように探索入力ｕ_ｓｋを算出するために、以下の原理が用いられる。まず、評価関数Ｊは、印加探索入力ｕ_ｓｋ_ｒを用いて算出される関係上、印加探索入力ｕ_ｓｋ_ｒに含まれる参照入力ｒの特性（周期関数）に起因して、所定振幅の振動的な挙動を示すことになる。

ここで、印加探索入力ｕ_ｓｋ_ｒと評価関数Ｊの関係が例えば図１４に示す曲線として表されると仮定した場合、参照入力ｒに起因する評価関数Ｊの振動的な挙動は、図中の矢印に示すように、ある傾きを持った状態となる。なお、図１４のｕ_ｓｋ_ｏｐは、印加探索入力ｕ_ｓｋ_ｒの所定値である。

一方、前述した移動平均値Ｐ_ｃは、評価関数Ｊのフィルタ値ｈと参照入力ｒの乗算値の移動平均値であるので、評価関数Ｊと参照入力ｒの相関関数に相当する疑似相関関数と見なすことができる。

そのため、疑似相関関数と見なせる移動平均値Ｐ_ｃが正値であれば、評価関数Ｊの傾きが正値を示し、移動平均値Ｐ_ｃが負値であれば、評価関数Ｊの傾きは負値を示すことになる。これに加えて、移動平均値Ｐ_ｃは、前述した式（８３）で算出されることにより、参照入力ｒの周波数成分が除去された状態で算出される。以上の理由により、移動平均値Ｐ_ｃと印加探索入力ｕ_ｓｋ_ｒの関係は、例えば、図１５に示すような単調増加の関数として表すことができる。すなわち、移動平均値Ｐ_ｃは、印加探索入力ｕ_ｓｋ_ｒを変更したときに、評価関数Ｊが変化する方向を表すことになる。

したがって、評価関数Ｊが極小値になるように、印加探索入力ｕ_ｓｋ_ｒを算出するには、図１５に示す関数の傾きが値０になるように、移動平均値Ｐ_ｃを算出すればよいことになる。すなわち、移動平均値Ｐ_ｃが値０に収束するように、フィードバック制御アルゴリズムを用いて、印加探索入力ｕ_ｓｋ_ｒ言い換えれば探索入力ｕ_ｓｋを算出すればよいことになる。

以上の原理により、極値探索コントローラ５０では、式（８４）の積分制御アルゴリズムを用いて、評価関数Ｊが極小値になるような解として、探索入力ｕ_ｓｋが算出される。

本実施形態の第１～第３極値探索コントローラ３１～３３の場合、以上の極値探索コントローラ５０と同様の原理に基づき、探索入力ｕ_ｓｋとしての予測曲率ρ_ｍ１～ρ_ｍ３が算出される。

まず、第１極値探索コントローラ３１について説明する。第１極値探索コントローラ３１は、以下に述べる手法によって、予測曲率ρ_ｍ１を算出するものであり、この予測曲率ρ_ｍ１は、前述した第１予測軌道の曲率半径Ｒ_ｍ１の逆数に相当する。

図１３に示すように、第１極値探索コントローラ３１は、ハイパスフィルタ３１ａ、参照入力発生器３１ｂ、２つの遅延要素３１ｄ，３１ｈ、乗算器３１ｃ、移動平均フィルタ３１ｅ、３つの増幅要素３１ｆ，３１ｊ，３１ｋ、及び２つの加算要素３１ｇ，３１ｉを備えている。

このハイパスフィルタ３１ａでは、下式（８６）により、フィルタ値ｈ_１が算出される。

また、参照入力発生器３１ｂは、前述した参照入力ｒを発生する。この参照入力ｒは、遅延要素３１ｄを介して乗算器３１ｃに入力され、乗算器３１ｃでは、下式（８７）により、中間値Ｐ_ｈｒ１が算出される。

また、移動平均フィルタ３１ｅでは、下式（８８）により、移動平均値Ｐ_ｃ１が算出される。なお、本実施形態では、移動平均値Ｐ_ｃ１が修正量に相当する。

次いで、加算要素３１ｇには、移動平均値Ｐ_ｃ１が増幅要素３１ｆによって所定ゲインＫ_ｓｋ１分、増幅された状態で入力され、後述する移動平均値Ｐ_ｃ２が増幅要素３１ｋによってゲインＫ_ｓｋ１・Ｋ_ｂｐ_２１分、増幅された状態で入力されるとともに、後述する移動平均値Ｐ_ｃ３が増幅要素３１ｊによってゲインＫ_ｓｋ１・Ｋ_ｂｐ_３２分、増幅された状態で入力される。

ここで、ゲインＫ_ｂｐ_２１は、Ｐ_ｃ２≧０のときには、移動平均値Ｐ_ｃ１に応じて、図１６に示すマップを検索することにより算出される。図１６のＰｒｅｆ１は、正の所定値である。このマップでは、ゲインＫ_ｂｐ_２１は、０＜Ｐ_ｃ１の範囲では値１に、Ｐ_ｃ１＜－Ｐｒｅｆ１の範囲では値０にそれぞれ設定されているとともに、－Ｐｒｅｆ１≦Ｐ_ｃ１≦０の範囲では、移動平均値Ｐ_ｃ１が大きいほど、より値１に近づくように設定されている。

また、ゲインＫ_ｂｐ_２１は、Ｐ_ｃ２＜０のときには、移動平均値Ｐ_ｃ１に応じて、図１７に示すマップを検索することにより算出される。図１７のＰｒｅｆ２は、正の所定値であり、Ｐｒｅｆ１とＰｒｅｆ２は同一の値でもよく、互いに異なる値でもよい。このマップでは、ゲインＫ_ｂｐ_２１は、Ｐ_ｃ１＜０の範囲では値１に、Ｐｒｅｆ２＜Ｐ_ｃ１の範囲では値０にそれぞれ設定されているとともに、０≦Ｐ_ｃ１≦Ｐｒｅｆ２の範囲では、移動平均値Ｐ_ｃ１が大きいほど、より値０に近づくように設定されている。

さらに、ゲインＫ_ｂｐ_３１は、Ｐ_ｃ２≧０＆Ｐ_ｃ３≧０のときには、移動平均値Ｐ_ｃ１に応じて、図１８に示すマップを検索することにより算出される。このマップでは、ゲインＫ_ｂｐ_３１は、０＜Ｐ_ｃ１の範囲では値１に、Ｐ_ｃ１＜－Ｐｒｅｆ１の範囲では値０にそれぞれ設定されているとともに、－Ｐｒｅｆ１≦Ｐ_ｃ１≦０の範囲では、移動平均値Ｐ_ｃ１が大きいほど、より値１に近づくように設定されている。

また、ゲインＫ_ｂｐ_３１は、Ｐ_ｃ２＜０＆Ｐ_ｃ３＜０のときには、移動平均値Ｐ_ｃ１に応じて、図１９に示すマップを検索することにより算出される。このマップでは、ゲインＫ_ｂｐ_３１は、Ｐ_ｃ１＜０の範囲では値１に、Ｐｒｅｆ２＜Ｐ_ｃ１の範囲では値０にそれぞれ設定されているとともに、０≦Ｐ_ｃ１≦Ｐｒｅｆ２の範囲では、移動平均値Ｐ_ｃ１が大きいほど、より値０に近づくように設定されている。

さらに、ゲインＫ_ｂｐ_３１は、２つの移動平均値Ｐ_ｃ２，Ｐ_ｃ３の符号が異なるときには値０として算出される。

２つのゲインＫ_ｂｐ_２１，Ｋ_ｂｐ_３１が以上のように算出される理由と、値Ｐ_ｃ２・Ｋ_ｓｋ１・Ｋ_ｂｐ_２１が第２極値探索コントローラ３２から第１極値探索コントローラ３１に入力される理由と、値Ｐ_ｃ３・Ｋ_ｓｋ１・Ｋ_ｂｐ_３１が第３極値探索コントローラ３３から第１極値探索コントローラ３１に入力される理由については後述する。

さらに、加算要素３１ｇには、予測曲率ρ_ｍ１が遅延要素３１ｈを介して入力される。そして、加算要素３１ｇでは、下式（８９）により、予測曲率ρ_ｍ１が算出される。

上式（８９）に示すように、予測曲率ρ_ｍ１は、その前回値ρ_ｍ１（ｋ－１）に、値Ｐ_ｃ１・Ｋ_ｓｋ１と、値Ｐ_ｃ２・Ｋ_ｓｋ１・Ｋ_ｂｐ_２１と、値Ｐ_ｃ３・Ｋ_ｓｋ１・Ｋ_ｂｐ_３１とを加算することによって算出されるので、これらの移動平均値Ｐ_ｃ１～Ｐ_ｃ３によって修正（更新）されることになる。すなわち、３つの移動平均値Ｐ_ｃ１～Ｐ_ｃ３は、予測曲率ρ_ｍ１を修正する修正量として作用することになる。

さらに、加算要素３１ｉでは、下式（９０）により、印加予測曲率ρ_ｍ１_ｒが算出される。

そして、以上のように算出された予測曲率ρ_ｍ１及び印加予測曲率ρ_ｍ１_ｒは、第１極値探索コントローラ３１から第１予測軌道算出部３４に入力される。

次に、第２極値探索コントローラ３２について説明する。第２極値探索コントローラ３２は、以下に述べる手法によって、予測曲率ρ_ｍ２及び印加予測曲率ρ_ｍ２_ｒを算出するものであり、この予測曲率ρ_ｍ２は、前述した第２予測軌道の曲率半径Ｒ_ｍ２の逆数に相当する。

図１３に示すように、第２極値探索コントローラ３２は、ハイパスフィルタ３２ａ、参照入力発生器３２ｂ、２つの遅延要素３２ｄ，３２ｈ、乗算器３２ｃ、移動平均フィルタ３２ｅ、２つの増幅要素３２ｆ，３２ｊ及び２つの加算要素３２ｇ，３２ｉを備えている。

このハイパスフィルタ３２ａでは、下式（９１）により、フィルタ値ｈ_２が算出される。

また、参照入力発生器３２ｂは、前述した参照入力ｒを発生する。この参照入力ｒは、遅延要素３２ｄを介して乗算器３２ｃに入力され、乗算器３２ｃでは、下式（９２）により、中間値Ｐ_ｈｒ２が算出される。

また、移動平均フィルタ３２ｅでは、下式（９３）により、移動平均値Ｐ_ｃ２が算出される。なお、本実施形態では、移動平均値Ｐ_ｃ２が修正量に相当する。

次いで、加算要素３２ｇには、移動平均値Ｐ_ｃ２が増幅要素３２ｆによって所定ゲインＫ_ｓｋ２分、増幅された状態で入力され、後述する移動平均値Ｐ_ｃ３が増幅要素３２ｊによってゲインＫ_ｓｋ２・Ｋ_ｂｐ_３２分、増幅された状態で入力される。

ここで、ゲインＫ_ｂｐ_３２は、Ｐ_ｃ３≧０のときには、移動平均値Ｐ_ｃ２に応じて、図２０に示すマップを検索することにより算出される。図２０のＰｒｅｆ３は、正の所定値である。このマップでは、ゲインＫ_ｂｐ_３２は、０＜Ｐ_ｃ２の範囲では値１に、Ｐ_ｃ２＜－Ｐｒｅｆ３の範囲では値０にそれぞれ設定されているとともに、－Ｐｒｅｆ３≦Ｐ_ｃ２≦０の範囲では、移動平均値Ｐ_ｃ２が大きいほど、より値１に近づくように設定されている。

また、ゲインＫ_ｂｐ_３２は、Ｐ_ｃ３＜０のときには、移動平均値Ｐ_ｃ２に応じて、図２１に示すマップを検索することにより算出される。図２１のＰｒｅｆ４は、正の所定値である。ここで、Ｐｒｅｆ３とＰｒｅｆ４は、同一の値でもよく、互いに異なる値でもよい。

このマップでは、ゲインＫ_ｂｐ_３２は、Ｐ_ｃ２＜０の範囲では値１に、Ｐｒｅｆ４＜Ｐ_ｃ２の範囲では値０にそれぞれ設定されているとともに、０≦Ｐ_ｃ２≦Ｐｒｅｆ４の範囲では、移動平均値Ｐ_ｃ２が大きいほど、より値０に近づくように設定されている。

ゲインＫ_ｂｐ_３２が以上のように算出される理由と、値Ｐ_ｃ３・Ｋ_ｓｋ２・Ｋ_ｂｐ_３２が第３極値探索コントローラ３３から第２極値探索コントローラ３２に入力される理由については後述する。

さらに、加算要素３２ｇには、予測曲率ρ_ｍ２が遅延要素３２ｈを介して入力される。そして、加算要素３２ｇでは、下式（９４）により、予測曲率ρ_ｍ２が算出される。

上式（９４）に示すように、予測曲率ρ_ｍ２は、その前回値ρ_ｍ２（ｋ－１）に、値Ｐ_ｃ２・Ｋ_ｓｋ２と、値Ｐ_ｃ３・Ｋ_ｓｋ２・Ｋ_ｂｐ_３２とを加算することによって算出されるので、これらの移動平均値Ｐ_ｃ２，Ｐ_ｃ３によって修正（更新）されることになる。すなわち、２つの移動平均値Ｐ_ｃ２，Ｐ_ｃ３は、予測曲率ρ_ｍ２を修正する修正量として作用することになる。

さらに、加算要素３２ｉでは、下式（９５）により、印加予測曲率ρ_ｍ２_ｒが算出される。

そして、以上のように算出された予測曲率ρ_ｍ２及び印加予測曲率ρ_ｍ２_ｒは、第２極値探索コントローラ３２から第２予測軌道算出部３５に入力される。

次に、第３極値探索コントローラ３３について説明する。第３極値探索コントローラ３３は、以下に述べる手法によって、予測曲率ρ_ｍ３及び印加予測曲率ρ_ｍ３_ｒを算出するものであり、この予測曲率ρ_ｍ３は、前述した第３予測軌道の曲率半径Ｒ_ｍ３の逆数に相当する。

図１３に示すように、第３極値探索コントローラ３３は、ハイパスフィルタ３３ａ、参照入力発生器３３ｂ、２つの遅延要素３３ｄ，３３ｈ、乗算器３３ｃ、移動平均フィルタ３３ｅ、増幅要素３３ｆ及び２つの加算要素３３ｇ，３３ｉを備えている。

このハイパスフィルタ３３ａでは、下式（９６）により、フィルタ値ｈ_３が算出される。

また、参照入力発生器３３ｂは、前述した参照入力ｒを発生する。この参照入力ｒは、遅延要素３３ｄを介して乗算器３３ｃに入力され、乗算器３３ｃでは、下式（９７）により、中間値Ｐ_ｈｒ３が算出される。

また、移動平均フィルタ３３ｅでは、下式（９８）により、移動平均値Ｐ_ｃ３が算出される。なお、本実施形態では、移動平均値Ｐ_ｃ３が修正量に相当する。

次いで、加算要素３３ｇには、移動平均値Ｐ_ｃ３が増幅要素３３ｆによって所定ゲインＫ_ｓｋ３分、増幅された状態で入力され、予測曲率ρ_ｍ３が遅延要素３３ｈを介して入力される。

そして、加算要素３３ｇでは、下式（９９）により、予測曲率ρ_ｍ３が算出される。

さらに、加算要素３３ｉでは、下式（１００）により、印加予測曲率ρ_ｍ３_ｒが算出される。

そして、以上のように算出された予測曲率ρ_ｍ３及び印加予測曲率ρ_ｍ３_ｒは、第３極値探索コントローラ３３から第３予測軌道算出部３６に入力される。

次に、前述した第１予測軌道算出部３４について説明する。この第１予測軌道算出部３４では、第１極値探索コントローラ３１からの予測曲率ρ_ｍ１及び印加予測曲率ρ_ｍ１_ｒを用いて、前述した原理に基づき、３つの予測位置Ｚ_ｍ１１～Ｚ_ｍ１３と、３つの印加予測位置Ｚ_ｍ１１_ｒ～Ｚ_ｍ１３_ｒが算出される。

まず、前述した式（１）～（８）により、予測位置Ｚ_ｍ１１のＸ座標値ｘ_ｍ１１及びＹ座標値ｙ_ｍ１１と、予測位置Ｚ_ｍ１２のＸ座標値ｘ_ｍ１２及びＹ座標値ｙ_ｍ１２と、予測位置Ｚ_ｍ１３のＸ座標値ｘ_ｍ１３及びＹ座標値ｙ_ｍ１３とが算出される。

次いで、下式（１０１），（１０２）により、第１予測軌道の曲率半径Ｒ_ｍ１_ｒが算出される。

上式（１０１），（１０２）のρ_ｍｉｎは、所定の最小曲率（正値）である。また、式（１０２）のＳｉｇｎは、Ｇ≧０の場合にはＳｉｇｎ（Ｇ）＝１となり、Ｇ＜０の場合にはＳｉｇｎ（Ｇ）＝－１となる符号関数である。この点は、他の数式の符号関数Ｓｉｇｎにおいても同様である。

次に、下式（１０３）により、第１予測軌道の回転角度θ_ｍ１_ｒが算出される。

さらに、下式（１０４），（１０５）により、印加予測位置Ｚ_ｍ１１_ｒのＸ座標値ｘ_ｍ１１_ｒ及びＹ座標値ｙ_ｍ１１_ｒが算出される。

次いで、下式（１０６），（１０７）により、印加予測位置Ｚ_ｍ１２_ｒのＸ座標値ｘ_ｍ１２_ｒ及びＹ座標値ｙ_ｍ１２_ｒが算出される。

次いで、下式（１０８），（１０９）により、印加予測位置Ｚ_ｍ１３_ｒのＸ座標値ｘ_ｍ１３_ｒ及びＹ座標値ｙ_ｍ１３_ｒが算出される。

次に、前述した第２予測軌道算出部３５について説明する。この第２予測軌道算出部３５では、第２極値探索コントローラ３２からの予測曲率ρ_ｍ２及び印加予測曲率ρ_ｍ２_ｒを用いて、前述した原理に基づき、第２予測位置Ｚ_ｍ２及び印加第２予測位置Ｚ_ｍ２_ｒが算出される。

まず、前述した式（９）～（１４）により、第２予測位置Ｚ_ｍ２のＸ座標値ｘ_ｍ２及びＹ座標値ｙ_ｍ２が算出される。

次いで、下式（１１０），（１１１）により、第２予測軌道の曲率半径Ｒ_ｍ２_ｒが算出される。

次に、下式（１１２）により、第２予測軌道の回転角度θ_ｍ２_ｒが算出される。

さらに、下式（１１３），（１１４）により、前述したＸ’－Ｙ’座標系（図６参照）における印加第２予測位置Ｚ_ｍ２_ｒのＸ’座標値ｘ’_ｍ２_ｒ及びＹ’座標値ｙ’_ｍ２_ｒが算出される。

次いで、下式（１１５），（１１６）により、Ｘ－Ｙ座標系（図６参照）における印加第２予測位置Ｚ_ｍ２_ｒのＸ座標値ｘ_ｍ２_ｒ及びＹ座標値ｙ_ｍ２_ｒが算出される。

次に、前述した第３予測軌道算出部３６について説明する。この第３予測軌道算出部３６では、第３極値探索コントローラ３３からの予測曲率ρ_ｍ３及び印加予測曲率ρ_ｍ３_ｒを用いて、前述した原理に基づき、第３予測位置Ｚ_ｍ３及び印加第３予測位置Ｚ_ｍ３_ｒが算出される。

まず、前述した式（１５）～（２０）により、第３予測位置Ｚ_ｍ３のＸ座標値ｘ_ｍ３及びＹ座標値ｙ_ｍ３が算出される。

次いで、下式（１１７），（１１８）により、第３予測軌道の曲率半径Ｒ_ｍ３_ｒが算出される。

次に、下式（１１９）により、第３予測軌道の回転角度θ_ｍ３_ｒが算出される。

さらに、下式（１２０），（１２１）により、前述したＸ”－Ｙ”座標系（図６参照）における印加第３予測位置Ｚ_ｍ３_ｒのＸ”座標値ｘ”_ｍ３_ｒ及びＹ”座標値ｙ”_ｍ３_ｒが算出される。

次いで、下式（１２２），（１２３）により、Ｘ－Ｙ座標系（図６参照）における印加第３予測位置Ｚ_ｍ３_ｒのＸ座標値ｘ_ｍ３_ｒ及びＹ座標値ｙ_ｍ３_ｒが算出される。

以上のように、軌道決定部３０では、第１～第３極値探索コントローラ３１～３３によって、第１～第３予測軌道の予測曲率ρ_ｍ１～ρ_ｍ３が算出され、これらの予測曲率ρ_ｍ１～ρ_ｍ３に基づいて、前述した５つの予測位置Ｚ_ｍｉ（ｉ＝１１～１３，２，３）及び５つの印加予測位置Ｚ_ｍｉ_ｒが算出される。

次に、前述した値Ｐ_ｃ２・Ｋ_ｓｋ１・Ｋ_ｂｐ_２１及び値Ｐ_ｃ３・Ｋ_ｓｋ１・Ｋ_ｂｐ_３１が第１極値探索コントローラ３１に入力される理由と、値Ｐ_ｃ３・Ｋ_ｓｋ２・Ｋ_ｂｐ_３２が第２極値探索コントローラ３２に入力される理由と、３つのゲインＫ_ｂｐ_２１，Ｋ_ｂｐ_３１，Ｋ_ｂｐ_３２が前述したように設定されている理由について説明する。

例えば、第１～第３極値探索コントローラ３１～３３において、予測曲率ρ_ｍ１～ρ_ｍ３を互いに無関係に算出した場合、以下に述べるような状態が発生することがある。

すなわち、図２２に示すように、自車両３の右前方に干渉対象が存在する場合、第１予測軌道（現在位置～予測位置Ｚ_ｍ１３）と、第２予測軌道（予測位置Ｚ_ｍ１３～第２予測位置Ｚ_ｍ２）とが直線状に決定される一方、第２予測位置Ｚ_ｍ２から第３予測位置Ｚ_ｍ３までの第３予測軌道が円弧状に決定されることがある。

このように予測軌道が決定された場合、自車両３は、第２予測位置Ｚ_ｍ２に達した時点で急激に左方に旋回する状態となるため、横加速度が急増することで、不安定な走行状態となってしまう。これに加えて、自車両３の運動性能によっては、第３予測軌道に沿って旋回することができず、干渉対象との間での干渉を回避するためのマージンが減少する可能性がある。

以上の問題を解消するために、軌道決定部３０では、例えば、図２３に示すように、第１予測軌道、第２予測軌道及び第３予測軌道が同じ旋回方向に決定されている場合、すなわち、３つの移動平均値Ｐ_ｃ１～Ｐ_ｃ３の符号が互いに同一である場合には、予測曲率ρ_ｍ１を２つの移動平均値Ｐ_ｃ２，Ｐ_ｃ３を反映させながら算出するとともに、予測曲率ρ_ｍ２を移動平均値Ｐ_ｃ３を反映させながら算出する必要がある。

一方、例えば、図２４に示すように、第１予測軌道と第２予測軌道が逆の旋回方向に決定され、第２予測軌道と第３予測軌道が逆の旋回方向に決定されている場合、すなわち、移動平均値Ｐ_ｃ１，Ｐｃ_２の符号の正負が異なるとともに、移動平均値Ｐ_ｃ２，Ｐ_ｃ３の符号の正負が異なる場合において、予測曲率ρ_ｍ１を２つの移動平均値Ｐ_ｃ２，Ｐ_ｃ３を反映させながら算出したときには、干渉対象に対する回避マージンを減少させてしまうことになる。さらに、予測曲率ρ_ｍ２を移動平均値Ｐ_ｃ３を反映させながら算出した場合にも、同じ問題が発生することになる。したがって、予測曲率ρ_ｍ１を２つの移動平均値Ｐ_ｃ２，Ｐ_ｃ３を反映させないように算出するとともに、予測曲率ρ_ｍ２を移動平均値Ｐ_ｃ３を反映させないように算出する必要がある。

以上の理由により、２つのゲインＫ_ｂｐ_２１，Ｋ_ｂｐ_３１が前述したように設定されているとともに、前述した値Ｐ_ｃ２・Ｋ_ｓｋ１・Ｋ_ｂｐ_２１及び値Ｐ_ｃ３・Ｋ_ｓｋ１・Ｋ_ｂｐ_３１が第１極値探索コントローラ３１に入力されるように構成されている。

同じ理由により、ゲインＫ_ｂｐ_３２が前述したように設定されているとともに、値Ｐ_ｃ３・Ｋ_ｓｋ２・Ｋ_ｂｐ_３２が第２極値探索コントローラ３２に入力されるように構成されている。

以上の構成により、３つの移動平均値Ｐ_ｃ１～Ｐ_ｃ３の符号と、２つの値Ｐ_ｃ２・Ｋ_ｂｐ_２１，Ｐ_ｃ３・Ｋ_ｂｐ_３１と、予測曲率ρ_ｍ１に対する移動平均値Ｐ_ｃ２，Ｐ_ｃ３の反映状態との関係は、図２５に示す１２個のパターンＡ１～Ａ１２になる。この場合、前述した図２３の状態では、パターンＡ１（又はＡ２）のようになり、前述した図２４の状態では、パターンＡ９～Ａ１２のいずれかのようになる。

また、２つの移動平均値Ｐ_ｃ２，Ｐ_ｃ３の符号と、値Ｐ_ｃ３・Ｋ_ｂｐ_３２と、予測曲率ρ_ｍ２に対する移動平均値Ｐ_ｃ３の反映状態との関係は、図２６に示す６個のパターンＢ１～Ｂ６になる。

以上のように、本実施形態の軌道決定部３０では、５つの予測位置Ｚ_ｍ１１～Ｚ_ｍ１３，Ｚ_ｍ２，Ｚ_ｍ３が算出された後、これらの値が車両制御部４０に入力される。

車両制御部４０では、５つの予測位置Ｚ_ｍ１１～Ｚ_ｍ１３，Ｚ_ｍ２，Ｚ_ｍ３及び現在速度Ｖ_ｅｇｏに応じて、原動機５及びアクチュエータ６が制御される。それにより、自車両３は、５つの予測位置Ｚ_ｍ１１～Ｚ_ｍ１３，Ｚ_ｍ２，Ｚ_ｍ３によって決定される第１～第３予測軌道に沿って走行することになる。

以上のように、第１実施形態の軌道生成装置１によれば、第１～第３予測軌道の各々において、交通参加者などの干渉対象との干渉度合いを表す各種のリスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖ_ｒ，Ｒ_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ_ｐ_ｒｂｌ_ｍｉ_ｒが算出され、これらのリスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖ_ｒ，Ｒ_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ_ｐ_ｒｂｌ_ｍｉ_ｒの総和を含むように、第１～第３評価関数Ｊ_１～Ｊ_３が算出される。

そして、これらの第１～第３評価関数Ｊ_１～Ｊ_３が極小値になるように、予測曲率ρ_ｍ１～ρ_ｍ３が算出され、これらの予測曲率ρ_ｍ１～ρ_ｍ３に基づいて、第１～第３予測軌道が決定される。すなわち、５つの予測位置Ｚ_ｍｉ（ｉ＝１１～１３，２，３）は、第１～第３評価関数Ｊ_１～Ｊ_３が極小値になるように決定されることになる。

この場合、上述したように、第１～第３評価関数Ｊ_１～Ｊ_３は、干渉対象との干渉度合いを表すリスクポテンシャルＲ_ｐ_ｍｉ_ｖｊ_ｖ_ｒの総和を含むように算出されるので、そのような第１～第３評価関数Ｊ_１～Ｊ_３が極小値になるように決定されることによって、第１～第３予測軌道を、干渉対象との干渉渉度合いが減少するように決定することができる。

さらに、予測曲率ρ_ｍ１は、第１極値探索コントローラ３１における移動平均値Ｐ_ｃ１と第２極値探索コントローラ３２における移動平均値Ｐ_ｃ２とが同符号の場合には、移動平均値Ｐ_ｃ２を反映させながら決定される（図２３及び図２５参照）。

また、予測曲率ρ_ｍ２は、第２極値探索コントローラ３２における移動平均値Ｐ_ｃ２と第３極値探索コントローラ３３における移動平均値Ｐ_ｃ３とが同符号の場合には、移動平均値Ｐ_ｃ３を反映させながら決定される（図２３及び図２６参照）。

この場合、３つの移動平均値Ｐ_ｃ１～Ｐ_ｃ３は、３つの予測曲率ρ_ｍ１～ρ_ｍ３をそれぞれ修正するための修正量、すなわち第１～第３予測軌道を修正するための修正量として算出される。したがって、第１予測軌道及び第２予測軌道の修正量の符号が同一の場合、第１予測軌道は、それよりも自車両３の現在位置から遠い第２予測軌道の修正方向と同方向にさらに修正されることになる。

これと同様に、第２予測軌道及び第３予測軌道の修正量の符号が同一の場合、第２予測軌道は、それよりも自車両３の現在位置から遠い第３予測軌道の修正方向と同方向にさらに修正されることになる。

ここで、干渉対象が自車両３の進行方向に存在する場合には、現在位置から遠い方の予測軌道の方が、自車両３の現在位置から近い方の予測軌道と比べて、干渉対象により近い軌道となる。したがって、干渉対象に近い方の予測軌道を修正するための修正量を反映させながら、干渉対象から遠い方（すなわち自車両３に近い方）の予測軌道を生成することによって、自車両３に近い方の予測軌道において、干渉対象との干渉を回避するための回避マージンをより増大させることができる。その結果、実現可能な軌道が制限される自車両３においても、干渉対象との干渉を回避するためのマージンが十分に確保されるように、自車両３の未来の軌道を適切に生成することができる。

一方、隣り合う２つの予測軌道の修正量が異符号である場合、２つの予測軌道が干渉対象を回避するために互いに異なる方向に修正されていることになる。したがって、そのような条件下において、自車両３の現在位置から遠い方の予測軌道を修正するための修正量を、現在位置から近い方の予測軌道に反映させたときには、現在位置から近い方の予測軌道において、干渉対象に対する回避マージンを減少させてしまうことになる。

これに対して、本実施形態の場合、移動平均値Ｐ_ｃ１と移動平均値Ｐ_ｃ２が異符号である場合には、予測曲率ρ_ｍ１は、移動平均値Ｐ_ｃ２を反映させることなく決定される（図２４及び図２５参照）。これと同様に、移動平均値Ｐ_ｃ２と移動平均値Ｐ_ｃ３が異符号である場合には、予測曲率ρ_ｍ２は、移動平均値Ｐ_ｃ３を反映させることなく決定される（図２４及び図２６参照）。それにより、現在位置から近い方の予測軌道においても、干渉対象との干渉を回避するためのマージンを十分に確保することができる。

また、第１予測軌道は、第２予測軌道及び第３予測軌道よりも短い３つの予測軌道を組み合わせたものとして決定される。それにより、第２予測軌道及び第３予測軌道と比べて、自車両３との距離が近い第１予測軌道においても、干渉対象との干渉をより確実に回避することができるように、第１予測軌道を生成することができる。

なお、第１実施形態は、自車両３を移動体とした例であるが、これに代えて、２輪車両、ロボット、航空機又は船舶などを移動体としてもよい。

さらに、第１実施形態は、本発明の軌道生成装置１を自動運転可能な４輪車両３に搭載した例であるが、これに代えて、軌道生成装置を４輪車両以外の移動体（例えば、２輪車両、ロボット、航空機又は船舶など）に搭載してもよく、軌道生成装置を単独で使用してもよい。

また、第１実施形態は、交通参加者検出部１０において、所定の機械学習アルゴリズム（例えば、ＤＮＮ及びＣＮＮなどのニューラルネットワークを用いたアルゴリズム）を用いて、交通参加者の現在検出位置Ｗ_ｖｊ（ｋ）を算出した例であるが、これに代えて、画像認識アルゴリズムなどを用いて、交通参加者の現在検出位置Ｗ_ｖｊ（ｋ）を算出してもよい。

さらに、第１実施形態は、行動予測部１１において、５個の予測位置Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１１），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１２），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１３），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ２），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ３）を、所定の機械学習アルゴリズム（例えば、ＤＮＮ及びＣＮＮなどのニューラルネットワークを用いたアルゴリズム）を用いて、算出した例であるが、これに代えて、ニューラルネットワーク以外の機械学習アルゴリズムを用いて、５個の予測位置Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１１），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１２），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ１３），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ２），Ｗ_ｖ１（ｋ：ｋ＋ｋｍ３）を算出してもよい。

また、第１実施形態は、干渉度合いパラメータとして、各種のリスクポテンシャルを用いた例であるが、本発明の干渉度合いパラメータは、これに限らず、自車両３の周辺における干渉対象の自車両３との干渉度合いを表すものであればよい。

第１実施形態は、軌道決定部３０において、予測曲率ρ_ｍｉを算出し、これに基づいて予測位置Ｚ_ｍｉを算出した例であるが、予測曲率ρ_ｍｉに代えて、特許第６５８００８７号公報の図１８に記載したような線分と角度の組み合わせの予測値を算出し、これらに基づいて予測位置Ｚ_ｍｉを算出するように構成してもよい。

次に、第２実施形態に係る軌道生成装置１００（図２７参照）について説明する。本実施形態の軌道生成装置１００は、自車両３の自動運転制御を実行するために、以下に述べるアルゴリズムによって、自車両３の前方に存在する交通参加者の予測軌道を決定するものである。なお、本実施形態では、自車両３が第１移動体に相当し、交通参加者が第２移動体に相当する。

ここで、交通参加者としては、歩行者、自転車、４輪車及び２輪オートバイなどの移動可能なものが相当するが、以下、交通参加者が歩行者Ｍ１（図２９参照）である場合を例にとって説明するとともに、歩行者Ｍ１の移動に対して干渉する対象を「干渉対象」という。この場合、歩行者Ｍ１以外の交通参加者及び交通環境（道路環境や信号の状態など）が干渉対象に相当する。

図２７に示すように、軌道生成装置１００は、交通参加者検出部１１０、行動予測部１１１、第１～第３評価関数算出部１２１～１２３及び軌道決定部２００を備えている。これらの要素１００～１１１，１２１～１２３，２００は、具体的には、ＥＣＵ２によって構成されており、これらの要素１００～１１１，１２１～１２３，２００での演算処理は、前述した所定の制御周期ΔＴで実行される。

なお、本実施形態では、第１～第３評価関数算出部１２１～１２３が干渉度合いパラメータ算出部に相当し、軌道決定部２００が予測軌道生成部及び修正量決定部に相当する。

まず、交通参加者検出部１１０では、前述した第１実施形態の交通参加者検出部１０と同様の手法により、歩行者Ｍ１の現在検出位置と、歩行者Ｍ１以外の交通参加者の現在検出位置Ｗ_ｖｊ（ｋ）が検出／算出される。まず、１人の歩行者である交通参加者ＴＰ４のみが歩行者Ｍ１の前方に存在する場合（すなわちｊ＝１の場合）を例にとって説明する。

本実施形態の場合、歩行者Ｍ１の座標系は、図２８に示すように、歩行者Ｍ１の中心を原点とし、歩行者Ｍ１の進行方向及び横方向をＸ軸及びＹ軸として定義される。なお、図中のＺ２_ｍ１ｈ０は歩行者Ｍ１の現在位置であり、Ｚ２_ｍ１１，Ｚ２_ｍ１２，Ｚ２_ｍ１３は、歩行者Ｍ１の後述する予測位置である。

また、交通参加者ＴＰ４の座標系は、交通参加者ＴＰ４の進行方向及び横方向をＸ_ｖ軸及びＹ_ｖ軸とし、Ｘ_ｖ軸のＸ軸に対する傾きをθ２_ｖ１とする直交２次元座標系として定義される。

この直交２次元座標系において、Ｘ_ｖ座標値をｘ２_ｖ１とし、Ｙ_ｖ座標値をｙ２_ｖ１とした場合、交通参加者ＴＰ４の現在検出位置Ｗ２_ｖ１（ｋ）は、３つの値［ｘ２_ｖ１（ｋ），ｙ２_ｖ１（ｋ），θ２_ｖ１（ｋ）］を要素とする値として算出される。また、図２８中の交通参加者ＴＰ４の周囲における１点鎖線で示すラインは、後述するリスクポテンシャルを模擬的に表している。

交通参加者検出部１１１では、前述した第１実施形態の交通参加者検出部１０と同様の手法により、交通参加者ＴＰ４の現在位置Ｗ２_ｖ１（ｋ：ｋ）と、５個の予測位置Ｗ２_ｖ１（ｋ：ｋ＋ｋｍ１１），Ｗ２_ｖ１（ｋ：ｋ＋ｋｍ１２），Ｗ２_ｖ１（ｋ：ｋ＋ｋｍ１３），Ｗ２_ｖ１（ｋ：ｋ＋ｋｍ２），Ｗ２_ｖ１（ｋ：ｋ＋ｋｍ３）とが算出される。

以上のように、交通参加者検出部１１１では、交通参加者が１個（１人）の場合、交通参加者の６個の位置（現在位置及び５個の予測位置）が算出され、交通参加者がｎ個の場合には、ｎ×６個の位置が算出される。この点は、前述した各要素１２１～１２３，２００の演算においても同様である。以下、交通参加者が１個の場合を例にとって説明する。

次に、本実施形態の軌道生成装置１００における歩行者Ｍ１の未来の予測軌道の定義及びその決定原理について説明する。図２９に示すように、本実施形態の軌道生成装置１００では、第１実施形態の軌道生成装置１と同様に、歩行者Ｍ１の未来の予測軌道が、３つの円弧状の第１～第３予測軌道を組み合わせたものとして決定される。

最初に、第１予測軌道について説明する。まず、前述したように、歩行者Ｍ１の現在位置の中心を原点とし、歩行者Ｍ１の進行方向をＸ軸とし、歩行者Ｍ１の横方向をＹ軸とするＸ－Ｙ座標系を定義する。

そして、このＸ－Ｙ座標系における原点から第１予測時間Ｔ２_ｍ１後の位置を第１予測位置Ｚ２_ｍ１とし、原点から第１予測位置Ｚ２_ｍ１までの予測軌道を円弧状の第１予測軌道と定義する。ここで、歩行者Ｍ１の現在速度をＶ_ｔｒｇｔとした場合、第１予測軌道の距離Ｌ２_ｍ１は、下式（１２４）のように定義される。

さらに、第１予測軌道の曲率半径をＲ２_ｍ１とした場合、第１予測軌道の回転角度θ２_ｍ１は、下式（１２５）のように定義される。この回転角度θ２_ｍ１は、Ｙ軸と後述するＹ’軸の間の角度に相当する。

また、歩行者Ｍ１の所定時刻Ｔｈ１前（例えば、１制御周期前）の推定移動距離Ｌ２_ｍｈ１は、下式（１２６）のように定義される。この推定移動距離Ｌ２_ｍｈ１は、歩行者Ｍ１が所定時刻Ｔｈ１前の制御時刻から今回の制御時刻までの間、第１予測軌道を延長した円弧軌道に沿って移動したと推定したときの移動距離である。

さらに、歩行者Ｍ１の所定時刻Ｔｈ１前の推定位置Ｚ２_ｍ１ｈ１における回転角度θ２_ｍｈ１は、下式（１２７）のように定義される。この所定時刻Ｔｈ１前の推定位置Ｚ２_ｍ１ｈ１は、歩行者Ｍ１が所定時刻Ｔｈ１前の制御時刻から今回の制御時刻までの間、第１予測軌道を延長した円弧軌道に沿って移動したと推定したときの所定時刻Ｔｈ１前の制御時刻での位置である。

さらに、歩行者Ｍ１の所定時刻Ｔｈ２前（例えば、２制御周期前）の推定移動距離Ｌ２_ｍｈ２は、下式（１２８）のように定義される。この推定移動距離Ｌ２_ｍｈ２は、歩行者Ｍ１が所定時刻Ｔｈ２前の制御時刻から今回の制御時刻までの間、第１予測軌道を延長した円弧軌道に沿って移動したと推定したときの移動距離である。

さらに、歩行者Ｍ１の所定時刻Ｔｈ２前の推定位置Ｚ２_ｍ１ｈ２における回転角度θ２_ｍｈ２は、下式（１２９）のように定義される。所定時刻Ｔｈ２前の推定位置Ｚ２_ｍ１ｈ２は、歩行者Ｍ１が所定時刻Ｔｈ２前の制御時刻から今回の制御時刻までの間、第１予測軌道を延長した円弧軌道に沿って移動したと推定したときの所定時刻Ｔｈ２前の制御時刻での位置である。

一方、原点から第１予測位置Ｚ２_ｍ１までの予測軌道を３等分したときの予測位置を、原点から第１予測位置Ｚ２_ｍ１に向かって順に、３つの予測位置Ｚ２_ｍ１１，Ｚ２_ｍ１２，Ｚ２_ｍ１３（＝Ｚ２_ｍ１）とする。その場合、予測位置Ｚ２_ｍ１１のＸ座標値ｘ２_ｍ１１及びＹ座標値ｙ２_ｍ１１は、下式（１３０），（１３１）のように定義される。

また、予測位置Ｚ２_ｍ１２のＸ座標値ｘ２_ｍ１２及びＹ座標値ｙ２_ｍ１２は、下式（１３２），（１３３）のように定義される。

さらに、予測位置Ｚ２_ｍ１３のＸ座標値ｘ２_ｍ１３及びＹ座標値ｙ２_ｍ１３は、下式（１３４），（１３５）のように定義される。

一方、現在位置Ｚ２_ｍ１ｈ０のＸ座標値ｘ２_ｍ１ｈ０及びＹ座標値ｙ２_ｍ１ｈ０は、下式（１３６），（１３７）のように定義される。

また、歩行者Ｍ１の所定時刻Ｔｈ1前の推定位置Ｚ２_ｍ１ｈ１のＸ座標値ｘ２_ｍ１ｈ１及びＹ座標値ｙ２_ｍ１ｈ１は、下式（１３８），（１３９）のように定義される。

さらに、歩行者Ｍ１の所定時刻Ｔｈ２前の推定位置Ｚ２_ｍ１ｈ２のＸ座標値ｘ２_ｍ１ｈ２及びＹ座標値ｙ２_ｍ１ｈ２は、下式（１４０），（１４１）のように定義される。

以上の式（１２４）～（１２５），（１３０）～（１３５）が第１予測軌道のモデル式（モデル軌道）に相当する。

次に、第２予測軌道について説明する。まず、第１予測位置Ｚ２_ｍ１（＝予測位置Ｚ２_ｍ１３）を原点とし、第１予測軌道の第１予測位置Ｚ２_ｍ１における接線方向をＸ’軸とし、この接線と直交する方向をＹ’軸とするＸ’－Ｙ’座標系を定義する。

さらに、このＸ’－Ｙ’座標系における原点すなわち第１予測位置Ｚ２_ｍ１から第２予測時間Ｔ２_ｍ２後の位置を第２予測位置Ｚ２_ｍ２とし、第１予測位置Ｚ２_ｍ１から第２予測位置Ｚ２_ｍ２までの予測軌道を円弧状の第２予測軌道と定義する。この場合、第２予測軌道の距離Ｌ２_ｍ２は、下式（１４２）のように定義される。

さらに、第２予測軌道の曲率半径をＲ２_ｍ２とした場合、第２予測軌道の回転角度θ２_ｍ２は、下式（１４３）のように定義される。この回転角度θ２_ｍ２は、Ｙ’軸と後述するＹ”軸の間の角度に相当する。

そして、第２予測位置Ｚ２_ｍ２のＸ’座標値ｘ２’_ｍ２及びＹ’座標値ｙ２’_ｍ２は、下式（１４４），（１４５）のように定義される。

さらに、第２予測位置Ｚ２_ｍ２のＸ座標値ｘ２_ｍ２及びＹ座標値ｙ２_ｍ２は、下式（１４６），（１４７）のように定義される。

以上の式（１４２）～（１４７）が第２予測軌道のモデル式（モデル軌道）に相当する。

次に、第３予測軌道について説明する。まず、第２予測位置Ｚ２_ｍ２を原点とし、第２予測軌道の第２予測位置Ｚ２_ｍ２における接線方向をＸ”軸とし、この接線と直交する方向をＹ”軸とするＸ”－Ｙ”座標系を定義する。

さらに、このＸ”－Ｙ”座標系における原点すなわち第２予測位置Ｚ２_ｍ２から第３予測時間Ｔ２_ｍ３後の位置を第３予測位置Ｚ２_ｍ３とし、第２予測位置Ｚ２_ｍ２から第３予測位置Ｚ２_ｍ３までの予測軌道を円弧状の第３予測軌道と定義する。この場合、第３予測軌道の距離Ｌ２_ｍ３は、下式（１４８）のように定義される。

さらに、第３予測軌道の曲率半径をＲ２_ｍ３とした場合、第３予測軌道の回転角度θ２_ｍ３は、下式（１４９）のように定義される。

そして、第３予測位置Ｚ２_ｍ３のＸ”座標値ｘ２”_ｍ３及びＹ”座標値ｙ２”_ｍ３は、下式（１５０），（１５１）のように定義される。

さらに、第３予測位置Ｚ２_ｍ３のＸ座標値ｘ２_ｍ３及びＹ座標値ｙ２_ｍ３は、下式（１５２），（１５３）のように定義される。

以上の式（１４８）～（１５３）が第３予測軌道のモデル式（モデル軌道）に相当する。

次に、前述した第１～第３評価関数算出部１２１～１２３について説明する。これらの第１～第３評価関数算出部１２１～１２３では、以上の５つの予測位置Ｚ２_ｍ１１，Ｚ２_ｍ１２，Ｚ２_ｍ１３，Ｚ２_ｍ２，Ｚ２_ｍ３と、現在位置Ｚ２_ｍ１ｈ０と、２つの推定位置Ｚ２_ｍ１ｈ１，Ｚ２_ｍ１ｈ２とを用いて、歩行者Ｍ１と干渉対象との干渉度合いを表すものとして、第１評価関数Ｊ２_１、第２評価関数Ｊ２_２及び第３評価関数Ｊ２_３がそれぞれ算出される。

ここで、第１評価関数Ｊ２_１、第２評価関数Ｊ２_２及び第３評価関数Ｊ２_３は、後述するように、５つの予測位置Ｚ２_ｍ１１，Ｚ２_ｍ１２，Ｚ２_ｍ１３，Ｚ２_ｍ２，Ｚ２_ｍ３の算出に用いられるので、第１評価関数Ｊ２_１、第２評価関数Ｊ２_２及び第３評価関数Ｊ２_３の今回値は、５つの予測位置Ｚ２_ｍ１１，Ｚ２_ｍ１２，Ｚ２_ｍ１３，Ｚ２_ｍ２，Ｚ２_ｍ３の前回値に基づいて算出されることになる。

なお、以下の説明では、５つの予測位置Ｚ２_ｍ１１，Ｚ２_ｍ１２，Ｚ２_ｍ１３，Ｚ２_ｍ２，Ｚ２_ｍ３をまとめて「予測位置Ｚ２_ｍｉ」と表記する。すなわち、「予測位置Ｚ２_ｍｉ」の添え字ｉは、ｉ＝１１～１３，２，３を表すものである。

以下、第１～第３評価関数算出部１２１～１２３において、第１～第３評価関数Ｊ２_１～Ｊ２_３の算出に用いる手法及びその原理について説明する。

まず、リスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖの算出手法について説明する。このリスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖは、歩行者Ｍ１が予測位置Ｚ２_ｍｉにあり、かつ交通参加者が予測位置Ｗ２_ｖｊにあると想定した場合の、歩行者Ｍ１と交通参加者との干渉度合いを表すものである。

まず、ｘ座標偏差ｄｘ２_ｍｉ_ｖｊ及びｙ座標偏差ｄｙ２_ｍｉ_ｖｊが、下式（１５４），（１５５）により算出される。

これらの偏差ｄｘ２_ｍｉ_ｖｊ，ｄｙ２_ｍｉ_ｖｊは、歩行者Ｍ１の予測位置Ｚ２_ｍｉと交通参加者の予測位置Ｗ２_ｖｊとの偏差を表している。

次いで、変換ｘ座標偏差ｄｘ２_ｍｉ_ｖｊ_ｖ及び変換ｙ座標偏差ｄｙ２_ｍｉ_ｖｊ_ｖが、下式（１５６），（１５７）により算出される。これらの偏差ｄｘ２_ｍｉ_ｖｊ_ｖ，ｄｙ２_ｍｉ_ｖｊ_ｖは、上記２つの偏差ｄｘ２_ｍｉ_ｖｊ，ｄｙ２_ｍｉ_ｖｊを、交通参加者の座標系に変換したものである。

次に、変換ｘ座標偏差ｄｘ２_ｍｉ_ｖｊ_ｖに応じて、図３０に示すマップを検索することにより、ｘ方向リスクポテンシャルＲ２_ｐｘ２_ｍｉ_ｖｊ_ｖが算出される。このｘ方向リスクポテンシャルＲ２_ｐｘ２_ｍｉ_ｖｊ_ｖのマップ値の横幅及び形状などは、干渉対象の種類（例えば、歩行者、自転車、４輪車両、オートバイなど）に応じて設定される。

さらに、変換ｙ座標偏差ｄｙ２_ｍｉ_ｖｊ_ｖに応じて、図３１に示すマップを検索することにより、ｙ方向リスクポテンシャルＲ２_ｐｙ２_ｍｉ_ｖｊ_ｖが算出される。このｙ方向リスクポテンシャルＲ２_ｐｙ２_ｍｉ_ｖｊ_ｖのマップ値の横幅及び形状なども、ｘ方向リスクポテンシャルＲ２_ｐｘ２_ｍｉ_ｖｊ_ｖのマップ値と同様に、干渉対象の種類（例えば、歩行者、自転車、４輪車両、オートバイなど）に応じて設定される。

そして、下式（１５８）に示すように、ｘ方向リスクポテンシャルＲ２_ｐｘ２_ｍｉ_ｖｊ_ｖとｙ方向リスクポテンシャルＲ２_ｐｙ２_ｍｉ_ｖｊ_ｖの積として、リスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖが算出される。

以上の手法により、リスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖは、歩行者Ｍ１と交通参加者との干渉度合が大きくなるほど、より大きい値になるように算出される。

次に、右側走路境界リスクポテンシャルＲ_ｐ_ｒｂｒ_ｍｉ及び左側走路境界リスクポテンシャルＲ_ｐ_ｒｂｌ_ｍｉの算出手法について説明する。なお、本実施形態の説明では、歩行者Ｍ１が移動する歩道などの経路を「走路」という。これらの２つのリスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍｉ，Ｒ２_ｐ_ｒｂｌ_ｍｉは、歩行者Ｍ１が予測位置Ｚ２_ｍｉにあると想定した場合の、歩行者Ｍ１が走路逸脱を生じる可能性を表す値である。

まず、歩行者Ｍ１の走路の右側境界線ＬＲ２及び左側境界線ＬＬ２が、例えば図３２に示すようになっている場合、これらの境界線ＬＲ２，ＬＬ２は、下式（１５９），（１６０）に示す２つの１次関数として定義することができる。

上式（１５９）におけるａ２_ｒｂｒ，ｂ２_ｒｂｒは、右側境界線ＬＲ２を規定するパラメータであり、上式（１６０）におけるａ２_ｒｂｌ，ｂ２_ｒｂｌは、左側境界線ＬＬ２を規定するパラメータである。これらの４つのパラメータａ２_ｒｂｒ，ｂ２_ｒｂｒ，ａ２_ｒｂｌ，ｂ２_ｒｂｌは、前述した情報検出装置４の検出結果に基づき、リアルタイムで算出される。

次に、右側走路境界関数σ２_ｒ及び左側走路境界関数σ２_ｌを、下式（１６１），（１６２）に示すように定義する。

これらの式（１６１），（１６２）は、上式（１５９），（２７）の左辺を右辺に移項したものに相当する。したがって、右側走路境界関数σ２_ｒが正値である場合、歩行者Ｍ１の中心が右側境界線ＬＲよりも左側の走路内の領域に存在し、右側走路境界関数σ２_ｒが負値である場合、歩行者Ｍ１の中心が右側境界線ＬＲよりも右側の走路外の領域に存在することになる。

これと同様に、左側走路境界関数σ２_ｌが正値である場合、歩行者Ｍ１の中心が左側境界線ＬＬよりも左側の走路外の領域に存在し、左側走路境界関数σ２_ｌが負値である場合、歩行者Ｍ１の中心が左側境界線ＬＬよりも右側の走路内の領域に存在することになる。

以上の原理に基づき、右側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒは、下式（１６３），（１６４）により算出され、左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｌは、下式（１６５），（１６６）により算出される。

以上の式（１６３）～（１６６）におけるσ２_ｍｒｇは、歩行者Ｍ１が走路逸脱を確実に回避できるようにするためのマージン（正の所定値）である。ここで、右側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒと右側走路境界関数σ２_ｒの関係、及び、左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｌと左側走路境界関数σ２_ｌの関係を図式化した場合、図３３，３４に示すものとなる。

また、上式（１６３）のΩ_ｃｒｏｓｓ（ａ２_ｒｂｒ（ｋ））は、横断意思パラメータであり、前述した右側境界線ＬＲ２を規定するパラメータａ２_ｒｂｒに応じて、図３５に示すマップを検索することにより算出される。同図において、ａｘ１，ａｘ２は、０＜ａｘ１＜ａｘ２が成立するパラメータａ２_ｒｂｒの所定値であり、Ω_１は、横断意思パラメータΩ_ｃｒｏｓｓの正の所定値である。

図３５に示すように、このマップでは、横断意思パラメータΩ_ｃｒｏｓｓは、パラメータａ２_ｒｂｒが－ａｘ１≦ａ２_ｒｂｒ≦ａｘ１の範囲内にある場合には、所定値Ω_１に設定され、パラメータａ２_ｒｂｒがａ２_ｒｂｒ≦－ａｘ２，ａｘ２≦ａ２_ｒｂｒの範囲内にある場合には、値０に設定されている。さらに、横断意思パラメータΩ_ｃｒｏｓｓは、パラメータａ２_ｒｂｒが－ａｘ２＜ａ２_ｒｂｒ＜－ａｘ１の範囲内にある場合には、パラメータａ２_ｒｂｒが大きいほど、所定値Ω_１に向かって増大するように設定され、パラメータａ２_ｒｂｒがａｘ１＜ａ２_ｒｂｒ＜ａｘ２の範囲内にある場合には、パラメータａ２_ｒｂｒが大きいほど、値に０に向かって減少するように設定されている。

さらに、上式（１６５）のΩ_ｃｒｏｓｓ（ａ２_ｒｂｌ（ｋ））も、横断意思パラメータであり、前述した左側境界線ＬＬ２を規定するパラメータａ２_ｒｂｌに応じて、図３６に示すマップを検索することにより算出される。このマップでは、横断意思パラメータΩ_ｃｒｏｓｓは、パラメータａ２_ｒｂｌの値に対して、図３５と同様に設定されている。

上式（１６３），（１６５）においてこれらの２つの値Ω_ｃｒｏｓｓ（ｒｂｒ（ｋ）），Ω_ｃｒｏｓｓ（ｒｂｌ（ｋ））を用いるのは以下の理由による。例えば、図３７に示すように，歩行者Ｍ１が歩道から横断歩道に向かっている際において、上式（１６３），（１６５）の値Ω_ｃｒｏｓｓ（ｒｂｒ（ｋ）），Ω_ｃｒｏｓｓ（ｒｂｌ（ｋ））を省略した式を用いた場合、２つの走路境界関数σ２_ｒ，σ２_ｌの効果に起因して、予測軌道が歩道内に留まるように決定されてしまうことになる。

したがって、本実施形態の場合、図３７に示すような事象を回避し、図３８に示すように、歩行者Ｍ１が歩道から横断歩道に向かっている際、予測軌道が横断歩道側に向かう状態で決定されるように、２つの値Ω_ｃｒｏｓｓ（ｒｂｒ（ｋ）），Ω_ｃｒｏｓｓ（ｒｂｌ（ｋ））が用いられる。

次に、以上と同じ原理により、歩行者Ｍ１の予測位置Ｚ２_ｍｉにおける右側走路境界関数σ２_ｒ_ｍｉ及び左側走路境界関数σ２_ｌ_ｍｉを、下式（１６７），（１６８）に示すように定義する。

さらに、前述した原理により、歩行者Ｍ１の予測位置Ｚ２_ｍｉにおける右側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍｉは、下式（１６９），（１７０）により算出され、歩行者Ｍ１の予測位置Ｚ２_ｍｉにおける左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｌ_ｍｉは、下式（１７１），（１７２）により算出される。

次に、図３９を参照しながら、トレースポテンシャルＴｒの算出手法について説明する。同図において、Ｚ２_ｈ１は、所定時刻Ｔｈ1前の制御時刻における歩行者Ｍ１の実測位置であり、Ｚ２_ｈ２は、所定時刻Ｔｈ２前の制御時刻における歩行者Ｍ１の実測位置である。

また、前述したように、図中の所定時刻Ｔｈ1前の推定位置Ｚ２_ｍ１ｈ１は、歩行者Ｍ１が所定時刻Ｔｈ1前から今回の制御時刻までの間、第１予測軌道を延長した円弧軌道に沿って移動したと推定したときの所定時刻Ｔｈ1前の制御時刻での位置であり、所定時刻Ｔｈ２前の推定位置Ｚ２_ｍ１ｈ２は、歩行者Ｍ１が所定時刻Ｔｈ２前から今回の制御時刻までの間、第１予測軌道を延長した円弧軌道に沿って移動したと推定したときの所定時刻Ｔｈ２前の制御時刻での位置である。

本実施形態では、第１予測軌道の算出において、歩行者Ｍ１の２つの実測位置Ｚ２_ｈ１，Ｚ２_ｈ２に対する第１予測軌道の追従性を確保するために、トレースポテンシャルＴｒが以下に述べる手法により算出される。

まず、下式（１７３），（１７４）により、所定時刻Ｔｈ1前のｘ座標偏差δｘｈ１及び所定時刻Ｔｈ1前のｙ座標偏差δｙｈ１が算出される。

次いで、下式（１７５），（１７６）により、所定時刻Ｔｈ２前のｘ座標偏差δｘｈ２及び所定時刻Ｔｈ２前のｙ座標偏差δｙｈ２が算出される。

そして、下式（１７７）により、トレースポテンシャルＴｒが算出される。

上式（１７７）に示すように、トレースポテンシャルＴｒは、２つの推定位置Ｚ２_ｍ１ｈ１，Ｚ２_ｍ１ｈ２と、２つの実測位置Ｚ２_ｈ１，Ｚ２_ｈ２との二乗和誤差に相当する値として算出される。したがって、トレースポテンシャルＴｒが小さいほど、２つの推定位置Ｚ２_ｍ１ｈ１，Ｚ２_ｍ１ｈ２と、２つの実測位置Ｚ２_ｈ１，Ｚ２_ｈ２との差分が小さいことになる。

なお、トレースポテンシャルＴｒを以下に述べるように算出してもよい。例えば、所定時刻Ｔｈ1前及び所定時刻Ｔｈ２前の推定位置Ｚ２_ｍ１ｈ１，Ｚ２_ｍ１ｈ２に加えて、所定時刻Ｔｈ２よりも前の時刻である所定時刻Ｔｈ３前の推定値を算出し、所定時刻Ｔｈ1前及び所定時刻Ｔｈ２前の歩行者Ｍ１の実測位置Ｚ２_ｈ１，Ｚ２_ｈ２に加えて、所定時刻Ｔｈ３前の歩行者Ｍ１の実測位置を取得し、３つ以上の推定値の各々と３つ以上の実測位置の各々との偏差の二乗和を、トレースポテンシャルＴｒとして用いてもよい。

次に、前述した第１評価関数算出部１２１における第１評価関数Ｊ２_１の算出手法について説明する。この第１評価関数算出部１２１の算出に用いる各種パラメータの添え字「ｉ」は、ｉ＝１１～１３を表すものである。

第１評価関数算出部１２１では、前述したリスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖ、右側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍｉ及び左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｌ_ｍｉの算出手法及び算出原理に基づき、以下に述べるように、軌道決定部２００からの３つの印加予測位置Ｚ２_ｍｉ_ｒを用いて、第１評価関数Ｊ２_１が算出される。これらの印加予測位置Ｚ２_ｍｉ_ｒは、後述するように、前述した３つの予測位置Ｚ２_ｍｉに参照入力ｒを印加したものである。

第１評価関数算出部１２１では、予測位置Ｚ２_ｍｉに参照入力ｒを印加した印加予測位置Ｚ２_ｍｉ_ｒが用いられる関係上、以下の説明では、印加予測位置Ｚ２_ｍｉ_ｒと同様に、前述した各種パラメータの最後に「_ｒ」を付加したものを用いる。この点は、第２評価関数算出部１２２及び第３評価関数算出部１２３においても同様である。

第１評価関数算出部１２１では、まず、下式（１７８），（１７９）により、現在の制御時刻でのｘ座標偏差ｄｘ２_ｍｉ_ｖｊ_ｒ（ｋ：ｋ）及びｙ座標偏差ｄｙ２_ｍｉ_ｖｊ_ｒ（ｋ：ｋ）が算出される。

上式（１７８）のｘ２_ｍｉ_ｒは、印加予測位置Ｚ２_ｍｉ_ｒのＸ座標値であり、ｘ２_ｖｊ（ｋ：ｋ）は、交通参加者の現在位置Ｗ２_ｖｊ（ｋ：ｋ）のＸ座標値である。また、上式（１７９）のｙ２_ｍｉ_ｒは、印加予測位置Ｚ２_ｍｉ_ｒのＹ座標値であり、ｙ２_ｖｊ（ｋ：ｋ）は、交通参加者の現在位置Ｗ２_ｖｊ（ｋ：ｋ）のＹ座標値である。

次いで、下式（１８０），（１８１）により、予測時刻ｋ＋ｋｍｉでのｘ座標偏差ｄｘ２_ｍｉ_ｖｊ_ｒ（ｋ：ｋ＋ｋｍｉ）及びｙ座標偏差ｄｙ２_ｍｉ_ｖｊ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。

上式（１８０）のｘ２_ｖｊ（ｋ：ｋ＋ｋｍｉ）は、交通参加者の予測位置Ｗ２_ｖｊ（ｋ：ｋ＋ｋｍｉ）のＸ座標値であり、上式（１８１）のｙ２_ｖｊ（ｋ：ｋ＋ｋｍｉ）は、交通参加者の予測位置Ｗ２_ｖｊ（ｋ：ｋ＋ｋｍｉ）のＹ座標値である。

さらに、下式（１８２），（１８３）により、現在の制御時刻での変換ｘ座標偏差ｄｘ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）及び変換ｙ座標偏差ｄｙ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）が算出される。

これらの偏差ｄｘ２_ｍｉ_ｖｊ_ｖ_ｒ，ｄｙ２_ｍｉ_ｖｊ_ｖ_ｒは、上記の歩行者Ｍ１の座標系における偏差ｄｘ２_ｍｉ_ｖｊ_ｒ，ｄｙ２_ｍｉ_ｖｊ_ｒを、交通参加者の座標系に変換した値に相当する。

次に、下式（１８４），（１８５）により、予測時刻ｋ＋ｋｍｉでの変換ｘ座標偏差ｄｘ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）及び変換ｙ座標偏差ｄｙ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。

これらの偏差ｄｘ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ），ｄｙ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）は、上記の歩行者Ｍ１の座標系における偏差ｄｘ２_ｍｉ_ｖｊ_ｒ（ｋ：ｋ＋ｋｍｉ），ｄｙ２_ｍｉ_ｖｊ_ｒ（ｋ：ｋ＋ｋｍｉ）を、交通参加者の座標系に変換した値に相当する。

さらに、前述した変換ｘ座標偏差ｄｘ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）に応じて、図示しないマップを検索することにより、ｘ方向リスクポテンシャルＲ２_ｐｘ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）が算出される。この場合、マップとしては、前述した図３０と同様のものが用いられる。

また、変換ｙ座標偏差ｄｙ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）に応じて、図示しないマップを検索することにより、ｙ方向リスクポテンシャルＲ２_ｐｙ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）が算出される。この場合、マップとしては、前述した図３１と同様のものが用いられる。

次いで、下式（１８６）により、現在の制御時刻ｋでのリスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）が算出される。

さらに、前述した変換ｘ座標偏差ｄｘ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）に応じて、ｘ方向リスクポテンシャルＲ２_ｐｘ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）の算出に用いたマップを検索することにより、ｘ方向リスクポテンシャルＲ２_ｐｘ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。

また、前述した変換ｙ座標偏差ｄｙ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）に応じて、ｙ方向リスクポテンシャルＲ２_ｐｙ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ）の算出に用いたマップを検索することにより、ｙ方向リスクポテンシャルＲ２_ｐｙ２_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。

次いで、下式（１８７）により、予測時刻ｋ＋ｋｍｉでのリスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が算出される。なお、本実施形態では、リスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ），Ｒ２_ｐ_ｍｉ_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍｉ）が干渉度合いパラメータに相当する。

また、歩行者Ｍ１の予測位置Ｚ２_ｍｉにおける右側走路境界関数σ２_ｒ_ｍｉ_ｒ及び左側走路境界関数σ２_ｌ_ｍｉ_ｒは、下式（１８８），（１８９）に示すように定義される。

さらに、右側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍｉ_ｒは、下式（１９０），（１９１）によって算出され、左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｌ_ｍｉ_ｒは、下式（１９２），（１９３）によって算出される。なお、本実施形態では、右側及び左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ２_ｐ_ｒｂｌ_ｍｉ_ｒが干渉度合いパラメータに相当する。

次いで、下式（１９４），（１９５）により、所定時刻Ｔｈ１前のｘ座標偏差δｘｈ１_ｒ及び所定時刻Ｔｈ１前のｙ座標偏差δｙｈ１_ｒが算出される。

さらに、下式（１９６），（１９７）により、所定時刻Ｔｈ２前のｘ座標偏差δｘｈ２_ｒ及び所定時刻Ｔｈ２前のｙ座標偏差δｙｈ２_ｒが算出される。

また、下式（１９８）により、トレースポテンシャルＴｒ_ｒが算出される。

以上の式（１９４）～（１９８）に示すように、トレースポテンシャルＴｒ_ｒは、２つの推定位置Ｚ２_ｍ１ｈ１_ｒ，Ｚ２_ｍ１ｈ２_ｒと、２つの実測位置Ｚ２_ｈ１，Ｚ２_ｈ２との二乗和誤差に相当する値として算出される。したがって、トレースポテンシャルＴｒが小さいほど、２つの推定位置Ｚ２_ｍ１ｈ１_ｒ，Ｚ２_ｍ１ｈ２_ｒと、２つの実測位置Ｚ２_ｈ１，Ｚ２_ｈ２との差分が小さいことになる。

そして、最終的に、第１評価関数Ｊ２_１が、下式（１９９）によって算出される。

上式（１９９）において、ω_ｔｒは、所定の重み係数であり、ｎは、歩行者Ｍ１の前方に位置する干渉対象の数を表している。この式（１９９）に示すように、第１評価関数Ｊ２_１は、現在の制御時刻ｋ及び予測時刻ｋ＋ｋｍｉ（ｉ＝１１～１３）における、３つの予測位置Ｚ２_ｍｉ（ｉ＝１１～１３）のリスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖ_ｒの総和と、右側及び左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ２_ｐ_ｒｂｌ_ｍｉ_ｒと、重み係数とトレースポテンシャルの積ω_ｔｒ・Ｔｒ_ｒとの和として算出される。

次に、前述した第２評価関数算出部１２２における第２評価関数Ｊ２_２の算出手法について説明する。この第２評価関数算出部１２２では、以下に述べるように、軌道決定部２００からの印加第２予測位置Ｚ２_ｍ２_ｒを用いて、第１評価関数算出部１２１とほぼ同様の手法により、第２評価関数Ｊ２_２が算出される。この印加予測位置Ｚ２_ｍ２_ｒは、第２予測位置Ｚ２_ｍ２に参照入力ｒを印加したものである。

第２評価関数算出部１２２では、まず、下式（２００），（２０１）により、予測時刻ｋ＋ｋｍ２でのｘ座標偏差ｄｘ２_ｍ２_ｖｊ_ｒ（ｋ：ｋ＋ｋｍ２）及びｙ座標偏差ｄｙ２_ｍ２_ｖｊ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。

上式（２００）のｘ２_ｖｊ（ｋ：ｋ＋ｋｍ２）は、交通参加者の予測位置Ｗ２_ｖｊ（ｋ：ｋ＋ｋｍ２）のＸ座標値であり、上式（２０１）のｙ２_ｖｊ（ｋ：ｋ＋ｋｍ２）は、交通参加者の予測位置Ｗ２_ｖｊ（ｋ：ｋ＋ｋｍ２）のＹ座標値である。

さらに、下式（２０２），（２０３）により、予測時刻ｋ＋ｋｍ２での変換ｘ座標偏差ｄｘ２_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）及び変換ｙ座標偏差ｄｙ２_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。

次に、前述した変換ｘ座標偏差ｄｘ２_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）に応じて、図示しないマップを検索することにより、ｘ方向リスクポテンシャルＲ２_ｐｘ２_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。この場合、マップとしては、前述した図３０と同様のものが用いられる。

また、前述した変換ｙ座標偏差ｄｙ２_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）に応じて、図示しないマップを検索することにより、ｙ方向リスクポテンシャルＲ２_ｐｙ２_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。この場合、マップとしては、前述した図３１と同様のものが用いられる。

次いで、下式（２０４）により、予測時刻ｋ＋ｋｍ２でのリスクポテンシャルＲ２_ｐ_ｍ２_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ２）が算出される。

また、歩行者Ｍ１の第２予測位置Ｚ２_ｍ２における右側走路境界関数σ２_ｒ_ｍ２_ｒ及び左側走路境界関数σ２_ｌ_ｍ２_ｒは、下式（２０５），（２０６）に示すように定義される。

さらに、右側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍ２_ｒは、下式（２０７），（２０８）によって算出され、左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｌ_ｍ２_ｒは、下式（２０９），（２１０）によって算出される。

そして、最終的に、第２評価関数Ｊ２_２が、下式（２１１）によって算出される。

この式（２１１）に示すように、第２評価関数Ｊ２_２は、第２予測位置Ｚ２_ｍ２におけるリスクポテンシャルＲ２_ｐ_ｍ２_ｖｊ_ｖ_ｒの総和と、右側及び左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍ２_ｒ，Ｒ２_ｐ_ｒｂｌ_ｍ２_ｒとの和として算出される。

次に、前述した第３評価関数算出部１２３における第３評価関数Ｊ２_３の算出手法について説明する。この第３評価関数算出部１２３では、以下に述べるように、軌道決定部２００からの印加第３予測位置Ｚ２_ｍ３_ｒを用いて、第２評価関数算出部１２２と同様の手法により、第３評価関数Ｊ２_３が算出される。この印加予測位置Ｚ２_ｍ３_ｒは、第３予測位置Ｚ２_ｍ３に参照入力ｒを印加したものである。

第３評価関数算出部１２３では、まず、下式（２１２），（２１３）により、予測時刻ｋ＋ｋｍ３でのｘ座標偏差ｄｘ２_ｍ３_ｖｊ_ｒ（ｋ：ｋ＋ｋｍ３）及びｙ座標偏差ｄｙ２_ｍ３_ｖｊ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。

上式（２１２）のｘ２_ｖｊ（ｋ：ｋ＋ｋｍ３）は、交通参加者の予測位置Ｗ２_ｖｊ（ｋ：ｋ＋ｋｍ３）のＸ座標値であり、上式（２１３）のｙ２_ｖｊ（ｋ：ｋ＋ｋｍ３）は、交通参加者の予測位置Ｗ２_ｖｊ（ｋ：ｋ＋ｋｍ３）のＹ座標値である。

さらに、下式（２１４），（２１５）により、予測時刻ｋ＋ｋｍ３での変換ｘ座標偏差ｄｘ２_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）及び変換ｙ座標偏差ｄｙ２_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。

次に、前述した変換ｘ座標偏差ｄｘ２_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）に応じて、図示しないマップを検索することにより、ｘ方向リスクポテンシャルＲ２_ｐｘ２_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。この場合、マップとしては、前述した図３０と同様のものが用いられる。

また、前述した変換ｙ座標偏差ｄｙ２_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）に応じて、図示しないマップを検索することにより、ｙ方向リスクポテンシャルＲ２_ｐｙ２_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。この場合、マップとしては、前述した図３１と同様のものが用いられる。

次いで、下式（２１６）により、予測時刻ｋ＋ｋｍ３でのリスクポテンシャルＲ２_ｐ_ｍ３_ｖｊ_ｖ_ｒ（ｋ：ｋ＋ｋｍ３）が算出される。

また、歩行者Ｍ１の第３予測位置Ｚ２_ｍ３における右側走路境界関数σ２_ｒ_ｍ３_ｒ及び左側走路境界関数σ２_ｌ_ｍ３_ｒは、下式（２１７），（２１８）に示すように定義される。

さらに、右側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍ３_ｒは、下式（２１９），（２２０）によって算出され、左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｌ_ｍ３_ｒは、下式（２２１），（２２２）によって算出される。

そして、最終的に、第３評価関数Ｊ２_３が下式（２２３）によって算出される。

この式（２２３）に示すように、第３評価関数Ｊ２_３は、第３予測位置Ｚ２_ｍ３におけるリスクポテンシャルＲ_ｐ_ｍ３_ｖｊ_ｖ_ｒの総和と、右側及び左側走路境界リスクポテンシャルＲ２_ｐ_ｒｂｒ_ｍ３_ｒ，Ｒ２_ｐ_ｒｂｌ_ｍ３_ｒとの和として算出される。

次に、前述した軌道決定部２００について説明する。図４０に示すように、軌道決定部２００は、第１～第３極値探索コントローラ２１０～２３０及び第１～第３予測軌道算出部２４１～２４３を備えている。

まず、第１極値探索コントローラ２１０について説明する。この第１極値探索コントローラ２１０は、以下に述べるように、第１実施形態の第１極値探索コントローラ３１と同様の手法により、予測曲率ρ２_ｍ１及び印加予測曲率ρ２_ｍ１_ｒを算出するものである。この予測曲率ρ２_ｍ１は、前述した図２９における第１予測軌道の曲率半径Ｒ２_ｍ１の逆数に相当する。

図４０に示すように、第１極値探索コントローラ２１０は、ハイパスフィルタ２１１、参照入力発生器２１２、２つの遅延要素２１４，２１８、乗算器２１３、移動平均フィルタ２１５、３つの増幅要素２１６ａ，２１６ｂ，２１６ｃ、及び２つの加算要素２１７，２１９を備えている。

このハイパスフィルタ２１１では、下式（２２４）により、フィルタ値ｈ２_１が算出される。

また、参照入力発生器２１２は、前述した参照入力ｒを発生する。この参照入力ｒは、遅延要素２１４を介して乗算器２１３に入力され、乗算器２１３では、下式（２２５）により、中間値Ｐ２_ｈｒ１が算出される。

また、移動平均フィルタ２１５では、下式（２２６）により、移動平均値Ｐ２_ｃ１が算出される。なお、本実施形態では、移動平均値Ｐ２_ｃ１が修正量に相当する。

次いで、加算要素２１７には、移動平均値Ｐ２_ｃ１が増幅要素２１６ａによって所定ゲインＫ２_ｓｋ１分、増幅された状態で入力され、後述する移動平均値Ｐ２_ｃ２が増幅要素２１６ｃによってゲインＫ２_ｓｋ１・Ｋ２_ｂｐ_２１分、増幅された状態で入力されるとともに、後述する移動平均値Ｐ２_ｃ３が増幅要素２１６ｂによってゲインＫ２_ｓｋ１・Ｋ２_ｂｐ_３２分、増幅された状態で入力される。

ここで、ゲインＫ２_ｂｐ_２１は、Ｐ２_ｃ２≧０のときには、移動平均値Ｐ２_ｃ１に応じて、図４１に示すマップを検索することにより算出される。図４１のＰｒｅｆ５は、正の所定値である。このマップでは、ゲインＫ２_ｂｐ_２１は、０＜Ｐ２_ｃ１の範囲では値１に設定され、Ｐ２_ｃ１＜－Ｐｒｅｆ５の範囲では値０に設定されているとともに、－Ｐｒｅｆ５≦Ｐ２_ｃ１≦０の範囲では、移動平均値Ｐ２_ｃ１が大きいほど、より値１に近づくように設定されている。

また、ゲインＫ２_ｂｐ２_２１は、Ｐ２_ｃ２＜０のときには、移動平均値Ｐ２_ｃ１に応じて、図４２に示すマップを検索することにより算出される。図４２のＰｒｅｆ６は、正の所定値であり、Ｐｒｅｆ５とＰｒｅｆ６は同一の値でもよく、互いに異なる値でもよい。このマップでは、ゲインＫ２_ｂｐ２_２１は、Ｐ２_ｃ１＜０の範囲では値１に設定され、Ｐｒｅｆ６＜Ｐ２_ｃ１の範囲では値０に設定されているとともに、０≦Ｐ２_ｃ１≦Ｐｒｅｆ６の範囲では、移動平均値Ｐ２_ｃ１が大きいほど、より値０に近づくように設定されている。

さらに、ゲインＫ２_ｂｐ２_３１は、Ｐ２_ｃ２≧０＆Ｐ２_ｃ３≧０のときには、移動平均値Ｐ２_ｃ１に応じて、図４３に示すマップを検索することにより算出される。このマップでは、ゲインＫ２_ｂｐ２_３１は、０＜Ｐ２_ｃ１の範囲では値１に設定され、Ｐ２_ｃ１＜－Ｐｒｅｆ５の範囲では値０に設定されているとともに、－Ｐｒｅｆ５≦Ｐ２_ｃ１≦０の範囲では、移動平均値Ｐ２_ｃ１が大きいほど、より値１に近づくように設定されている。

また、ゲインＫ２_ｂｐ２_３１は、Ｐ２_ｃ２＜０＆Ｐ２_ｃ３＜０のときには、移動平均値Ｐ２_ｃ１に応じて、図４４に示すマップを検索することにより算出される。このマップでは、ゲインＫ２_ｂｐ２_３１は、Ｐ２_ｃ１＜０の範囲では値１に設定され、Ｐｒｅｆ６＜Ｐ２_ｃ１の範囲では値０に設定されているとともに、０≦Ｐ２_ｃ１≦Ｐｒｅｆ６の範囲では、移動平均値Ｐ２_ｃ１が大きいほど、より値０に近づくように設定されている。

さらに、ゲインＫ２_ｂｐ２_３１は、２つの移動平均値Ｐ２_ｃ２，Ｐ２_ｃ３の符号が異なるときには値０として算出される。

２つのゲインＫ２_ｂｐ_２１，Ｋ２_ｂｐ_３１が以上のように算出される理由と、値Ｐ２_ｃ２・Ｋ２_ｓｋ１・Ｋ２_ｂｐ_２１が第２極値探索コントローラ２２０から第１極値探索コントローラ２１０に入力される理由と、値Ｐ２_ｃ３・Ｋ２_ｓｋ１・Ｋ２_ｂｐ_３１が第３極値探索コントローラ２３０から第１極値探索コントローラ２１０に入力される理由については後述する。

さらに、加算要素２１７には、予測曲率ρ２_ｍ１が遅延要素２１８を介して入力される。そして、加算要素２１７では、下式（２２７）により、予測曲率ρ２_ｍ１が算出される。

さらに、加算要素２１９では、下式（２２８）により、印加予測曲率ρ２_ｍ１_ｒが算出される。

そして、以上のように算出された予測曲率ρ２_ｍ１及び印加予測曲率ρ２_ｍ１_ｒは、第１極値探索コントローラ２１０から第１予測軌道算出部２４１に入力される。

次に、第２極値探索コントローラ２２０について説明する。第２極値探索コントローラ２２０は、第１実施形態の前述した第２極値探索コントローラ３２と同様の手法によって、予測曲率ρ２_ｍ２及び印加予測曲率ρ２_ｍ２_ｒを算出するものである。この予測曲率ρ２_ｍ２は、前述した第２予測軌道の曲率半径Ｒ２_ｍ２の逆数に相当する。

図４０に示すように、第２極値探索コントローラ２２０は、ハイパスフィルタ２２１、参照入力発生器２２２、２つの遅延要素２２４，２２８、乗算器２２３、移動平均フィルタ２２５、２つの増幅要素２２６ａ，２２６ｂ及び２つの加算要素２２７，２２９を備えている。

このハイパスフィルタ２２１では、下式（２２９）により、フィルタ値ｈ２_２が算出される。

また、参照入力発生器２２２は、参照入力ｒを発生する。この参照入力ｒは、遅延要素２２４を介して乗算器２２３に入力され、乗算器２２３では、下式（２３０）により、中間値Ｐ２_ｈｒ２が算出される。

また、移動平均フィルタ２２５では、下式（２３１）により、移動平均値Ｐ２_ｃ２が算出される。なお、本実施形態では、移動平均値Ｐ２_ｃ２が修正量に相当する。

次いで、加算要素２２７には、移動平均値Ｐ２_ｃ２が増幅要素２２６ａによって所定ゲインＫ２_ｓｋ２分、増幅された状態で入力され、後述する移動平均値Ｐ２_ｃ３が増幅要素２２６ｂによってゲインＫ２_ｓｋ２・Ｋ２_ｂｐ_３２分、増幅された状態で入力される。

ここで、ゲインＫ２_ｂｐ_３２は、Ｐ２_ｃ３≧０のときには、移動平均値Ｐ２_ｃ２に応じて、図４５に示すマップを検索することにより算出される。図４５のＰｒｅｆ７は、正の所定値である。このマップでは、ゲインＫ２_ｂｐ_３２は、０＜Ｐ２_ｃ２の範囲では値１に、Ｐ２_ｃ２＜－Ｐｒｅｆ７の範囲では値０にそれぞれ設定されているとともに、－Ｐｒｅｆ７≦Ｐ２_ｃ２≦０の範囲では、移動平均値Ｐ２_ｃ２が大きいほど、より値１に近づくように設定されている。

また、ゲインＫ２_ｂｐ_３２は、Ｐ２_ｃ３＜０のときには、移動平均値Ｐ２_ｃ２に応じて、図４６に示すマップを検索することにより算出される。図４６のＰｒｅｆ８は、正の所定値である。ここで、Ｐｒｅｆ７とＰｒｅｆ８は、同一の値でもよく、互いに異なる値でもよい。

このマップでは、ゲインＫ２_ｂｐ_３２は、Ｐ２_ｃ２＜０の範囲では値１に、Ｐｒｅｆ８＜Ｐ２_ｃ２の範囲では値０にそれぞれ設定されているとともに、０≦Ｐ２_ｃ２≦Ｐｒｅｆ８の範囲では、移動平均値Ｐ２_ｃ２が大きいほど、より値０に近づくように設定されている。

ゲインＫ２_ｂｐ_３２が以上のように算出される理由と、値Ｐ２_ｃ３・Ｋ２_ｓｋ２・Ｋ２_ｂｐ_３２が第３極値探索コントローラ２３０から第２極値探索コントローラ２２０に入力される理由については後述する。

さらに、加算要素２２７には、予測曲率ρ２_ｍ２が遅延要素２２８を介して入力される。そして、加算要素２２７では、下式（２３２）により、予測曲率ρ２_ｍ２が算出される。

上式（２３２）のλ_ｆｇｔは、忘却係数であり、０＜λ_ｆｇｔ＜１が成立するように設定される。この忘却係数λ_ｆｇｔは、リスクポテンシャルがゼロのときに第２予測軌道を直線状に戻すため（すなわち、予測曲率ρ２_ｍ２を値０に近づけるため）の値である。

さらに、加算要素２２９では、下式（２３３）により、印加予測曲率ρ２_ｍ２_ｒが算出される。

そして、以上のように算出された予測曲率ρ２_ｍ２及び印加予測曲率ρ２_ｍ２_ｒは、第２極値探索コントローラ２２０から第２予測軌道算出部２４２に入力される。

次に、第３極値探索コントローラ２３０について説明する。第３極値探索コントローラ２３０は、第１実施形態の前述した第３極値探索コントローラ３３と同様の手法によって、予測曲率ρ２_ｍ３及び印加予測曲率ρ２_ｍ３_ｒを算出するものである。この予測曲率ρ２_ｍ３は、前述した第３予測軌道の曲率半径Ｒ２_ｍ３の逆数に相当する。

図４０に示すように、第３極値探索コントローラ２３０は、ハイパスフィルタ２３１、参照入力発生器２３２、２つの遅延要素２３４，２３８、乗算器２３３、移動平均フィルタ２３５、増幅要素２３６及び２つの加算要素２３７，２３９を備えている。

このハイパスフィルタ２３１では、下式（２３４）により、フィルタ値ｈ２_３が算出される。

また、参照入力発生器２３２は、参照入力ｒを発生する。この参照入力ｒは、遅延要素２３４を介して乗算器２３３に入力され、乗算器２３３では、下式（２３５）により、中間値Ｐ２_ｈｒ３が算出される。

また、移動平均フィルタ２３５では、下式（２３６）により、移動平均値Ｐ２_ｃ３が算出される。なお、本実施形態では、移動平均値Ｐ２_ｃ３が修正量に相当する。

次いで、加算要素２３７には、移動平均値Ｐ２_ｃ３が増幅要素２３６によって所定ゲインＫ２_ｓｋ３分、増幅された状態で入力され、予測曲率ρ２_ｍ３が遅延要素２３８を介して入力される。

そして、加算要素２３７では、下式（２３７）により、予測曲率ρ２_ｍ３が算出される。

上式（２３７）の忘却係数λ_ｆｇｔは、リスクポテンシャルがゼロのときに第３予測軌道を直線状に戻すため（すなわち予測曲率ρ２_ｍ３を値０に近づけるため）の値である。

さらに、加算要素２３９では、下式（２３８）により、印加予測曲率ρ２_ｍ３_ｒが算出される。

そして、以上のように算出された予測曲率ρ２_ｍ３及び印加予測曲率ρ２_ｍ３_ｒは、第３極値探索コントローラ２３０から第３予測軌道算出部２４３に入力される。

次に、前述した第１予測軌道算出部２４１について説明する。この第１予測軌道算出部２４１では、第１極値探索コントローラ２１０からの予測曲率ρ２_ｍ１及び印加予測曲率ρ２_ｍ１_ｒを用いて、前述した原理に基づき、３つの予測位置Ｚ２_ｍ１１～Ｚ２_ｍ１３と、３つの印加予測位置Ｚ２_ｍ１１_ｒ～Ｚ２_ｍ１３_ｒが算出される。

まず、前述した式（１２４）～（１２５）,（１３０）～（１３５）により、予測位置Ｚ２_ｍ１１のＸ座標値ｘ２_ｍ１１及びＹ座標値ｙ２_ｍ１１と、予測位置Ｚ２_ｍ１２のＸ座標値ｘ２_ｍ１２及びＹ座標値ｙ２_ｍ１２と、予測位置Ｚ２_ｍ１３のＸ座標値ｘ２_ｍ１３及びＹ座標値ｙ２_ｍ１３とが算出される。

次いで、下式（２３９），（２４０）により、第１予測軌道の曲率半径Ｒ２_ｍ１_ｒが算出される。

上式（２３９），（２４０）のρ２_ｍｉｎは、所定の最小曲率（正値）である。

次に、下式（２４１）により、第１予測軌道の回転角度θ２_ｍ１_ｒが算出される。

さらに、下式（２４２），（２４３）により、印加予測位置Ｚ２_ｍ１１_ｒのＸ座標値ｘ２_ｍ１１_ｒ及びＹ座標値ｙ２_ｍ１１_ｒが算出される。

次いで、下式（２４４），（２４５）により、印加予測位置Ｚ２_ｍ１２_ｒのＸ座標値ｘ２_ｍ１２_ｒ及びＹ座標値ｙ２_ｍ１２_ｒが算出される。

また、下式（２４６），（２４７）により、印加予測位置Ｚ２_ｍ１３_ｒのＸ座標値ｘ２_ｍ１３_ｒ及びＹ座標値ｙ２_ｍ１３_ｒが算出される。

さらに、下式（２４８），（２４９）により、所定時刻Ｔｈ１前の推定位置Ｚ２_ｍ１ｈ１_ｒのＸ座標値ｘ２_ｍ１ｈ１_ｒ及びＹ座標値ｙ２_ｍ１ｈ１_ｒが算出される。

次に、下式（２５０），（２５１）により、所定時刻Ｔｈ２前の推定位置Ｚ２_ｍ１ｈ２_ｒのＸ座標値ｘ２_ｍ１ｈ２_ｒ及びＹ座標値ｙ２_ｍ１ｈ２_ｒが算出される。

次に、前述した第２予測軌道算出部２４２について説明する。この第２予測軌道算出部２４２では、第２極値探索コントローラ２２０からの予測曲率ρ２_ｍ２及び印加予測曲率ρ２_ｍ２_ｒを用いて、前述した原理に基づき、第２予測位置Ｚ２_ｍ２及び印加第２予測位置Ｚ２_ｍ２_ｒが算出される。

まず、前述した式（１４２）～（１４７）により、第２予測位置Ｚ２_ｍ２のＸ座標値ｘ２_ｍ２及びＹ座標値ｙ２_ｍ２が算出される。

次いで、下式（２５２），（２５３）により、第２予測軌道の曲率半径Ｒ２_ｍ２_ｒが算出される。

次に、下式（２５４）により、第２予測軌道の回転角度θ２_ｍ２_ｒが算出される。

さらに、下式（２５５），（２５６）により、前述したＸ’－Ｙ’座標系（図２９参照）における印加第２予測位置Ｚ２_ｍ２_ｒのＸ’座標値ｘ’_ｍ２_ｒ及びＹ’座標値ｙ’_ｍ２_ｒが算出される。

次いで、下式（２５７），（２５８）により、Ｘ－Ｙ座標系（図２９参照）における印加第２予測位置Ｚ２_ｍ２_ｒのＸ座標値ｘ２_ｍ２_ｒ及びＹ座標値ｙ２_ｍ２_ｒが算出される。

次に、前述した第３予測軌道算出部２４３について説明する。この第３予測軌道算出部２４３では、第３極値探索コントローラ２３０からの予測曲率ρ２_ｍ３及び印加予測曲率ρ２_ｍ３_ｒを用いて、前述した原理に基づき、第３予測位置Ｚ２_ｍ３及び印加第３予測位置Ｚ２_ｍ３_ｒが算出される。

まず、前述した式（１４８）～（１５３）により、第３予測位置Ｚ２_ｍ３のＸ座標値ｘ２_ｍ３及びＹ座標値ｙ２_ｍ３が算出される。

次いで、下式（２５９），（２６０）により、第３予測軌道の曲率半径Ｒ２_ｍ３_ｒが算出される。

次に、下式（２６１）により、第３予測軌道の回転角度θ２_ｍ３_ｒが算出される。

さらに、下式（２６２），（２６３）により、前述したＸ”－Ｙ”座標系（図２９参照）における印加第３予測位置Ｚ２_ｍ３_ｒのＸ”座標値ｘ”_ｍ３_ｒ及びＹ”座標値ｙ”_ｍ３_ｒが算出される。

次いで、下式（２６４），（２６５）により、Ｘ－Ｙ座標系（図２９参照）における印加第３予測位置Ｚ２_ｍ３_ｒのＸ座標値ｘ２_ｍ３_ｒ及びＹ座標値ｙ２_ｍ３_ｒが算出される。

以上のように、軌道決定部２００では、第１～第３極値探索コントローラ２１０～２３０によって、第１～第３予測軌道の予測曲率ρ２_ｍ１～ρ２_ｍ３が算出され、これらの予測曲率ρ２_ｍ１～ρ２_ｍ３に基づいて、前述した５つの予測位置Ｚ２_ｍｉ（ｉ＝１１～１３，２，３）と、５つの印加予測位置Ｚ２_ｍｉ_ｒ（ｉ＝１１～１３，２，３）と、所定時刻Ｔｈ１前及び所定時刻Ｔｈ２前の推定位置Ｚ２_ｍ１ｈ１，Ｚ２_ｍ１ｈ２とが算出される。

次に、前述した値Ｐ２_ｃ２・Ｋ２_ｓｋ１・Ｋ２_ｂｐ_２１及び値Ｐ２_ｃ３・Ｋ２_ｓｋ１・Ｋ２_ｂｐ_３１が第１極値探索コントローラ２１０に入力される理由と、値Ｐ２_ｃ３・Ｋ２_ｓｋ２・Ｋ２_ｂｐ_３２が第２極値探索コントローラ２２０に入力される理由と、３つのゲインＫ２_ｂｐ_２１，Ｋ２_ｂｐ_３１，Ｋ２_ｂｐ_３２が前述したように設定されている理由について説明する。

例えば、第１～第３極値探索コントローラ２１０～２３０において、予測曲率ρ２_ｍ１～ρ２_ｍ３を互いに無関係に算出した場合、以下に述べるような状態が発生することがある。

すなわち、図４７に示すように、歩行者Ｍ１の右前方に干渉対象が存在する場合、第１予測軌道（現在位置～予測位置Ｚ２_ｍ１３）と、第２予測軌道（予測位置Ｚ２_ｍ１３～第２予測位置Ｚ２_ｍ２）とが直線状に決定される一方、第２予測位置Ｚ２_ｍ２から第３予測位置Ｚ２_ｍ３までの第３予測軌道が円弧状に決定されることがある。

これに対して、通常の場合、歩行者Ｍ１は、図４７に示すような軌道で移動することはなく、第２予測位置Ｚ２_ｍ２に達する前に干渉対象との干渉を回避できるように予め旋回を開始しながら移動するのが一般的である。

したがって、軌道決定部２００では、そのような歩行者Ｍ１の移動軌道を予測して決定する必要がある。例えば、図４８に示すように、第１予測軌道、第２予測軌道及び第３予測軌道が同じ旋回方向に決定されている場合、すなわち、３つの移動平均値Ｐ２_ｃ１～Ｐ２_ｃ３の符号が互いに同一である場合には、予測曲率ρ２_ｍ１を２つの移動平均値Ｐ２_ｃ２，Ｐ２_ｃ３を反映させながら算出するとともに、予測曲率ρ２_ｍ２を移動平均値Ｐ２_ｃ３を反映させながら算出する必要がある。

一方、例えば、図４９に示すように、第１予測軌道と第２予測軌道が逆の旋回方向に決定され、第２予測軌道と第３予測軌道が逆の旋回方向に決定されている場合、すなわち、移動平均値Ｐ２_ｃ１，Ｐｃ_２の符号の正負が異なるとともに、移動平均値Ｐ２_ｃ２，Ｐ２_ｃ３の符号の正負が異なる場合において、予測曲率ρ２_ｍ１を２つの移動平均値Ｐ２_ｃ２，Ｐ２_ｃ３を反映させながら算出したときには、干渉対象に対する回避マージンを減少させてしまうことになる。さらに、予測曲率ρ２_ｍ２を移動平均値Ｐ２_ｃ３を反映させながら算出した場合にも、同じ問題が発生することになる。したがって、予測曲率ρ２_ｍ１を２つの移動平均値Ｐ２_ｃ２，Ｐ２_ｃ３を反映させないように算出するとともに、予測曲率ρ２_ｍ２を移動平均値Ｐ２_ｃ３を反映させないように算出する必要がある。

以上の理由により、２つのゲインＫ２_ｂｐ_２１，Ｋ２_ｂｐ_３１が前述したように設定されているとともに、前述した値Ｐ２_ｃ２・Ｋ２_ｓｋ１・Ｋ２_ｂｐ_２１及び値Ｐ２_ｃ３・Ｋ２_ｓｋ１・Ｋ２_ｂｐ_３１が第１極値探索コントローラ２１０に入力されるように構成されている。

同じ理由により、ゲインＫ２_ｂｐ_３２が前述しように設定されているとともに、値Ｐ２_ｃ３・Ｋ２_ｓｋ２・Ｋ２_ｂｐ_３２が第２極値探索コントローラ２２０に入力されるように構成されている。

以上の構成により、３つの移動平均値Ｐ２_ｃ１～Ｐ２_ｃ３の符号と、２つの値Ｐ２_ｃ２・Ｋ２_ｂｐ_２１，Ｐ２_ｃ３・Ｋ２_ｂｐ_３１と、予測曲率ρ２_ｍ１に対する移動平均値Ｐ２_ｃ２，Ｐ２_ｃ３の反映状態との関係は、図５０に示す１２個のパターンＣ１～Ｃ１２になる。この場合、前述した図４８の状態では、パターンＣ１（又はＣ２）のようになり、前述した図４９の状態では、パターンＣ９～Ｃ１２のいずれかのようになる。

また、２つの移動平均値Ｐ２_ｃ２，Ｐ２_ｃ３の符号と、値Ｐ２_ｃ３・Ｋ２_ｂｐ_３２と、予測曲率ρ２_ｍ２に対する移動平均値Ｐ２_ｃ３の反映状態との関係は、図５１に示す６個のパターンＤ１～Ｄ６になる。

以上のように、本実施形態の軌道決定部２００では、５つの予測位置Ｚ２_ｍ１１，Ｚ２_ｍ１２，Ｚ２_ｍ１３，Ｚ２_ｍ２，Ｚ２_ｍ３が算出された後、これらの値が車両制御部４０に入力される。

車両制御部４０では、５つの予測位置Ｚ２_ｍ１１，Ｚ２_ｍ１２，Ｚ２_ｍ１３，Ｚ２_ｍ２，Ｚ２_ｍ３に応じて、原動機５及びアクチュエータ６が制御される。それにより、自車両３は、歩行者Ｍ１の第１～第３予測軌道に基づき、歩行者Ｍ１との干渉を回避しながら走行することが可能になる。

以上のように、第２実施形態の軌道生成装置１００によれば、第１～第３予測軌道の各々において、干渉対象との干渉度合いを表すリスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖ_ｒ，Ｒ２_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ２_ｐ_ｒｂｌ_ｍｉ_ｒが算出され、これらのリスクポテンシャルＲ２_ｐ_ｍｉ_ｖｊ_ｖ_ｒ，Ｒ２_ｐ_ｒｂｒ_ｍｉ_ｒ，Ｒ２_ｐ_ｒｂｌ_ｍｉ_ｒの総和を含むように、第１～第３評価関数Ｊ２_１～Ｊ２_３が算出される。

そして、これらの第１～第３評価関数Ｊ２_１～Ｊ２_３が極小値になるように、予測曲率ρ２_ｍ１～ρ２_ｍ３が算出され、これらの予測曲率ρ２_ｍ１～ρ２_ｍ３に基づいて、第１～第３予測軌道が決定される。すなわち、５つの予測位置Ｚ２_ｍｉ（ｉ＝１１，１２，１３，２，３）は、第１～第３評価関数Ｊ２_１～Ｊ２_３が極小値になるように決定されることになる。

この場合、上述したように、第１評価関数Ｊ２_１は、歩行者Ｍ１と干渉対象との干渉度合いを表すリスクポテンシャルの総和と、重み係数とトレースポテンシャルの積ω_ｔｒ・Ｔｒ_ｒとの総和として算出され、第２及び第３評価関数Ｊ２_２，Ｊ２_３は、歩行者Ｍ１と干渉対象との干渉度合いを表すリスクポテンシャルの総和として算出される。そのため、そのような第１～第３評価関数Ｊ２_１～Ｊ２_３が極小値になるように決定されることによって、第１～第３予測軌道は、干渉対象との干渉渉度合いが減少するように決定されることになる。

さらに、予測曲率ρ２_ｍ１は、第１極値探索コントローラ３１における移動平均値Ｐ２_ｃ１と第２極値探索コントローラ３２における移動平均値Ｐ２_ｃ２とが同符号の場合には、移動平均値Ｐ２_ｃ２を反映させながら決定される（図４８及び図５０参照）。

また、予測曲率ρ２_ｍ２は、第２極値探索コントローラ３２における移動平均値Ｐ２_ｃ２と第３極値探索コントローラ３３における移動平均値Ｐ２_ｃ３とが同符号の場合には、移動平均値Ｐ２_ｃ３を反映させながら決定される（図４８及び図５１参照）。

この場合、３つの移動平均値Ｐ２_ｃ１～Ｐ２_ｃ３は、３つの予測曲率ρ２_ｍ１～ρ２_ｍ３をそれぞれ修正するための修正量、すなわち第１～第３予測軌道を修正するための修正量として算出される。したがって、第１予測軌道及び第２予測軌道の修正量の符号が同一の場合、第１予測軌道は、それよりも歩行者Ｍ１の現在位置から遠い第２予測軌道の修正方向と同方向にさらに修正されることになる。

これと同様に、第２予測軌道及び第３予測軌道の修正量の符号が同一の場合、第２予測軌道は、これよりも歩行者Ｍ１の現在位置から遠い第３予測軌道の修正方向と同方向にさらに修正されることになる。

ここで、干渉対象が歩行者Ｍ１の進行方向に存在する場合には、現在位置から遠い方の予測軌道の方が、歩行者Ｍ１の現在位置から近い方の予測軌道と比べて、干渉対象により近い軌道となる。したがって、干渉対象に近い方の予測軌道を修正するための修正量を反映させながら、干渉対象から遠い方（すなわち歩行者Ｍ１に近い方）の予測軌道を生成することによって、干渉対象から遠い方の予測軌道において、干渉対象との干渉を回避するための回避マージンをより増大させることができる。このように、干渉対象に対する歩行者Ｍ１の回避行動をより早く予測することができることで、歩行者Ｍ１の予測行動に基づいて自車両３の走行軌道を決定する際、実現可能な走行軌道が制限される自車両３においても、歩行者Ｍ１との干渉を回避できるように、車両３の未来の走行軌道を適切に決定することができる。

一方、隣り合う２つの予測軌道の修正量が異符号である場合、２つの予測軌道が干渉対象を回避するために互いに異なる方向に修正されていることになる。したがって、そのような条件下において、歩行者Ｍ１の現在位置から遠い方の予測軌道を修正するための修正量を、現在位置から近い方の予測軌道に反映させたときには、現在位置から近い方の予測軌道において、干渉対象に対する回避マージンを減少させてしまうことになる。その結果、干渉対象に対する歩行者Ｍ１の回避行動の変化を小さく予測してしまうことになり，歩行者Ｍ１の予測行動に基づいて自車両３の走行軌道を決定する際、その実現可能な走行軌道が制限される自車両３においては、歩行者M1との干渉を回避できる十分なマージンの確保が難しくなり、自車両３の未来の走行軌道を適切に決定できなくなる。

これに対して、本実施形態の場合、移動平均値Ｐ２_ｃ１と移動平均値Ｐ２_ｃ２が異符号である場合には、予測曲率ρ２_ｍ１は、移動平均値Ｐ２_ｃ２を反映させることなく決定される（図４９及び図５０参照）。これと同様に、移動平均値Ｐ２_ｃ２と移動平均値Ｐ２_ｃ３が異符号である場合には、予測曲率ρ２_ｍ２は、移動平均値Ｐ２_ｃ３を反映させることなく決定される（図４９及び図５１参照）。それにより、現在位置から近い方の予測軌道において、干渉対象に対する回避マージンを適切に確保することができる。

また、第１予測軌道は、第２予測軌道及び第３予測軌道よりも短い３つの予測軌道を組み合わせたものとして決定される。それにより、第２予測軌道及び第３予測軌道と比べて、歩行者Ｍ１との距離が近い第１予測軌道においても、干渉対象との干渉を回避するための回避マージンを適切に確保することができる。

さらに、第１評価関数Ｊ２_１は、前述した式（１９９）に示すように、リスクポテンシャルの総和に加えて、重み係数とトレースポテンシャルの積ω_ｔｒ・Ｔｒ_ｒを含むものとして算出される。このトレースポテンシャルＴｒ_ｒは、２つの推定位置Ｚ２_ｍ１ｈ１_ｒ，Ｚ２_ｍ１ｈ２_ｒと、２つの実測位置Ｚ２_ｈ１，Ｚ２_ｈ２との二乗和誤差に相当する値として算出される。

したがって、第１評価関数Ｊ２_１が極小値になるように、第１予測軌道を決定することによって、２つの推定位置Ｚ２_ｍ１ｈ１_ｒ，Ｚ２_ｍ１ｈ２_ｒと、２つの実測位置Ｚ２_ｈ１，Ｚ２_ｈ２との差分が小さくなるように、第１予測軌道を決定することができる。それにより、歩行者Ｍ１の過去の軌道に対する追従性を考慮しながら、第１予測軌道を決定することができる。

なお、第２実施形態は、自車両３を第１移動体とした例であるが、これに代えて、２輪車両、ロボット、航空機又は船舶などを第１移動体としてもよい。

また、第２実施形態は、本発明の軌道生成装置１００を自動運転可能な４輪車両３に搭載した例であるが、これに代えて、軌道生成装置を４輪車両以外の移動体（例えば、２輪車両、ロボット、航空機又は船舶など）に搭載してもよく、軌道生成装置を単独で使用してもよい。

また、第２実施形態は、歩行者Ｍ１を第２移動体とした例であるが、これに代えて、２輪車両、ロボット、航空機又は船舶などを第２移動体としてもよい。

また、第２実施形態は、交通参加者を干渉対象とした例であるが、これに代えて、２輪車両、ロボット、航空機又は船舶などを干渉対象としてもよい。

１ 軌道生成装置
２ ＥＣＵ２（周辺状況認識部、予測軌道生成部、干渉度合いパラメータ算出部、修 正量決定部、演算処理装置）
３ 自車両（移動体、第１移動体）
４ 情報検出装置（周辺状況認識部）
２１ 第１評価関数算出部（干渉度合いパラメータ算出部）
２２ 第２評価関数算出部（干渉度合いパラメータ算出部）
２３ 第３評価関数算出部（干渉度合いパラメータ算出部）
３０ 軌道決定部（予測軌道生成部、修正量決定部）
ＴＰ１ 交通参加者（干渉対象）
R_p_mi_vj_v_r リスクポテンシャル（干渉度合いパラメータ）
R_p_rbr_mi_r 右側走路境界リスクポテンシャル（干渉度合いパラメータ）
R_p_rbl_mi_r 左側走路境界リスクポテンシャル（干渉度合いパラメータ）
P_c1 移動平均値（修正量）
P_c2 移動平均値（修正量）
P_c3 移動平均値（修正量）
１００ 軌道生成装置
１２１ 第１評価関数算出部（干渉度合いパラメータ算出部）
１２２ 第２評価関数算出部（干渉度合いパラメータ算出部）
１２３ 第３評価関数算出部（干渉度合いパラメータ算出部）
２００ 軌道決定部（予測軌道生成部、修正量決定部）
R2_p_mi_vj_v_r リスクポテンシャル（干渉度合いパラメータ）
R2_p_rbr_mi_r 右側走路境界リスクポテンシャル（干渉度合いパラメータ）
R2_p_rbl_mi_r 左側走路境界リスクポテンシャル（干渉度合いパラメータ）
P2_c1 移動平均値（修正量）
P2_c2 移動平均値（修正量）
P2_c3 移動平均値（修正量）
Ｍ１ 歩行者（移動体、第２移動体）
ＴＰ４ 交通参加者（干渉対象）

Claims (7)

1. 移動体の未来の軌道を生成する軌道生成装置であって、
   前記移動体の周辺状況を認識する周辺状況認識部と、
   当該周辺状況認識部による前記周辺状況の認識結果に基づき、前記移動体の軌道をモデル化した複数の軌道モデルを用いて、前記移動体の前記未来の軌道を複数に分割したときの複数の未来の軌道の予測値である複数の予測軌道を生成する予測軌道生成部と、
   当該複数の予測軌道を用いて、前記移動体の周辺における干渉対象の当該移動体との干渉度合いを表す複数の干渉度合いパラメータを算出する干渉度合いパラメータ算出部と、
   当該複数の干渉度合いパラメータが表す干渉度合いが減少するように、前記複数の予測軌道をそれぞれ修正するための複数の修正量を決定する修正量決定部と、
   を備え、
   前記予測軌道生成部は、前記周辺状況の認識結果に基づき、前記複数の軌道モデル及び前記複数の修正量を用いて、前記複数の予測軌道を生成するとともに、互いに隣り合う２つの前記予測軌道を修正するための２つの前記修正量が当該２つの予測軌道を同じ方向に修正するように決定されている場合には、当該２つの予測軌道のうちの、前記移動体の現在位置から近い方の前記予測軌道を、前記移動体の当該現在位置から遠い方の前記予測軌道を修正するための前記修正量を反映させながら生成することを特徴とする軌道生成装置。
2. 請求項１に記載の軌道生成装置において、
   前記予測軌道生成部は、前記２つの前記修正量が前記２つの予測軌道を互いに異なる方向に修正するように決定されている場合には、前記移動体の前記現在位置から近い方の前記予測軌道を、前記移動体の前記現在位置から遠い方の前記予測軌道を修正するための前記修正量とは無関係に生成することを特徴とする軌道生成装置。
3. 請求項１又は２に記載の軌道生成装置において、
   前記移動体の前記現在位置から近い方の前記予測軌道は、前記移動体の前記現在位置から遠い方の前記予測軌道よりも短い複数の予測軌道に分割された状態で生成されることを特徴とする軌道生成装置。
4. 請求項１ないし３のいずれかに記載の軌道生成装置において、
   前記移動体は、前記周辺状況認識部、前記予測軌道生成部、前記干渉度合いパラメータ算出部及び前記修正量決定部を備えていることを特徴とする軌道生成装置。
5. 請求項１ないし３のいずれかに記載の軌道生成装置において、
   前記移動体は、前記周辺状況認識部、前記予測軌道生成部、前記干渉度合いパラメータ算出部及び前記修正量決定部を備えた第１移動体以外の第２移動体であり、
   前記予測軌道生成部は、前記第２移動体の過去の位置、前記複数の軌道モデル及び前記複数の修正量を用いて、前記複数の予測軌道を生成することを特徴とする軌道生成装置。
6. 請求項５に記載の軌道生成装置において、
   前記予測軌道生成部は、前記移動体の過去の位置の時系列と、前記移動体の前記現在位置から近い方の前記予測軌道を当該予測軌道に沿って当該移動体の過去の位置側に延ばしたときの軌道上の複数の推定位置の時系列との差分が減少するように、前記移動体の前記現在位置から近い方の前記予測軌道を生成することを特徴とする軌道生成装置。
7. 移動体の未来の軌道を演算処理装置によって生成する軌道生成方法であって、
   前記演算処理装置は、
   前記移動体の周辺状況を認識する周辺状況認識ステップと、
   当該周辺状況認識ステップによる前記周辺状況の認識結果に基づき、前記移動体の軌道をモデル化した複数の軌道モデルを用いて、前記移動体の前記未来の軌道を複数に分割したときの複数の未来の軌道の予測値である複数の予測軌道を生成する予測軌道生成ステップと、
   当該複数の予測軌道を用いて、前記移動体の周辺における干渉対象の当該移動体との干渉度合いを表す複数の干渉度合いパラメータを算出する干渉度合いパラメータ算出ステップと、
   当該複数の干渉度合いパラメータが表す干渉度合いが減少するように、前記複数の予測軌道をそれぞれ修正するための前記複数の修正量を決定する修正量決定ステップと、
   を実行し、
   前記予測軌道生成ステップでは、前記周辺状況の認識結果に基づき、前記複数の軌道モデル及び前記複数の修正量を用いて、前記複数の予測軌道が生成されるとともに、互いに隣り合う２つの前記予測軌道を修正するための２つの前記修正量が当該２つの予測軌道を同じ方向に修正するように決定されている場合には、当該２つの予測軌道のうちの、前記移動体の現在位置から近い方の前記予測軌道が、前記移動体の当該現在位置から遠い方の前記予測軌道を修正するための前記修正量を反映させながら生成されることを特徴とする軌道生成方法。

Images