JP4946739B2 - 移動体進路取得方法、および、移動体進路取得装置 - Google Patents

Description

本発明は、移動体進路取得方法、および、移動体進路取得装置に関し、特に、車両に関する移動体進路取得方法、および、移動体進路取得装置に関する。

従来、四輪自動車等の車両の安全性を高める技術の一つとして、車両が障害物に衝突する危険性を高精度で判定することによって衝突を防止する技術が知られている。例えば、下記特許文献１では、自車のヨーレートを検出するヨーレートセンサと、自車の速度を検出する速度検出装置と、周囲の障害物の位置や速度を検出するレーダ装置とを備えた衝突防止装置に関する技術が開示されている。

この従来技術では、ヨーレートセンサや速度センサによって自車の予測走行軌跡を求め、その予測走行軌跡の両側の所定領域を自車の予測走行エリアとして求める一方、レーダ装置によって検出された障害物の位置と速度から障害物の予測走行軌跡および予測走行エリアを求める。その後、自車および障害物の各予測走行エリアから両者の衝突点または近接点を算出する。また、目標減速加速度および目標減速度を算出することによって衝突危険性を判別し、この判別の結果、衝突の危険が生じたとき、目標減速加速度および目標減速度に応じた自車の速度制御を行っている。

特許第２７９９３７５号公報

しかしながら、上述した従来技術では、自車の周囲に複数の障害物が存在するような複雑な交通環境下で適用することができないという問題点がある。更に、乗り心地と安全性など複数の条件を両立させることが考慮されておらず、計算量も多いという問題点を有している。

特に、従来の手法によっては、操作量（可減速や操舵など）により発生する加速度を、進路を生成する際に物理限界値で制限するのみでランダムに進路を生成するので、発生する加速度が比較的低い経路が生成される割合が小さく、生成された候補から低い加速度条件の経路が選択される確率が少ないという問題があり、安全性と乗り心地の両立の必要性という課題が残されていた。

本発明は、前記に鑑みてなされたもので、計算量が膨大となることを防ぎながら、安全性と乗り心地を両立することができる、移動体進路取得方法、および、移動体進路取得装置を提供することを目的とする。また、自車の周囲に複数の障害物が存在するような複雑な交通環境下であっても障害物の進路を適切に予測して、移動体の進路を取得することができる、移動体進路取得方法、および、移動体進路取得装置を提供することを目的とする。

上述した課題を解決し、目的を達成するために、本発明の一態様は、移動体および前記移動体から所定の範囲に存在する障害物の、位置および内部状態を少なくとも記憶する記憶手段と、制御手段と、を備えた移動体進路取得装置において実行される移動体進路取得方法であって、前記制御手段において実行される、前記記憶手段から読み出した前記障害物の位置および内部情報に基づいて、前記障害物がとりうる進路の確率的な予測を行う障害物進路予測ステップと、前記記憶手段から読み出した前記移動体の位置および内部状態に基づいて、前記移動体がとりうる進路を所定の本数生成する第１進路生成ステップと、生成された前記移動体の前記進路が、予測された前記障害物の前記進路との関係で、所定の安全条件を満たすか否か、を評価する評価ステップと、前記評価ステップにおいて評価された前記進路に前記所定の安全条件を満たすものがない場合に、前記所定の本数より多くの本数の前記進路を生成し、当該進路が前記評価ステップにて評価されるよう制御する第２進路生成ステップと、を含むことを特徴とする。

また、本発明の別な態様は、移動体および前記移動体から所定の範囲に存在する障害物の、位置および内部状態を少なくとも記憶する記憶手段と、制御手段と、を備えた移動体進路取得装置において実行される移動体進路取得方法であって、前記制御手段において実行される、前記記憶手段から読み出した前記障害物の位置および内部情報に基づいて、前記障害物がとりうる進路の確率的な予測を行う障害物進路予測ステップと、前記記憶手段から読み出した前記移動体の位置および内部状態に基づいて、所定の物理条件において前記移動体がとりうる進路を生成する第１進路生成ステップと、生成された前記移動体の前記進路が、予測された前記障害物の前記進路との関係で、所定の安全条件を満たすか否か、を評価する評価ステップと、前記評価ステップにおいて評価された前記進路に前記所定の安全条件を満たすものがない場合に、前記所定の物理条件より緩やかな物理条件において前記移動体のとりうる前記進路を生成し、当該進路が前記評価ステップにて評価されるよう制御する第２進路生成ステップと、を含むことを特徴とする。

また、上記発明において、前記評価ステップは、生成された前記移動体の前記進路と、予測された前記障害物の前記進路と、に基づいて衝突確率を計算し、前記衝突確率が所定の閾値以内である場合に前記所定の安全条件を満たすと評価すること、を特徴とする。

また、上記発明において、前記物理条件は、一の前記進路をとった場合に前記移動体に発生する加速度についての条件であること、を特徴とする。

また、上記発明において、前記制御部において実行される、前記第２進路生成ステップにおいて生成され前記評価ステップにおいて評価された前記進路に前記所定の安全条件を満たすものがない場合に、目的値関数に基づいて目的地までの距離を計算し、前記目的地に近い前記進路を選択する距離関数計算ステップ、を更に含むことを特徴とする。

また、上記発明において、前記障害物進路予測ステップは、前記障害物が複数存在する場合に、予測した複数の前記障害物がとりうる前記進路の中で異なる前記障害物同士が互いに干渉し合う進路を求め、当該干渉し合う進路のうち前記確率的な予測を行った前記進路の予測確率を低下させる進路干渉評価ステップと、前記進路干渉評価ステップで前記予測確率が低下した前記進路を含む前記複数の進路の各々が実現される確率を算出する確率算出ステップと、を含むことを特徴とする。

また、本発明の別な態様は、移動体および前記移動体から所定の範囲に存在する障害物の、位置および内部状態を少なくとも記憶する記憶手段と、制御手段と、を備えた移動体進路取得装置において、前記制御手段は、前記記憶手段から読み出した前記障害物の位置および内部情報に基づいて、前記障害物がとりうる進路の確率的な予測を行う障害物進路予測手段と、前記記憶手段から読み出した前記移動体の位置および内部状態に基づいて、前記移動体がとりうる進路を所定の本数生成する第１進路生成手段と、生成された前記移動体の前記進路が、予測された前記障害物の前記進路との関係で、所定の安全条件を満たすか否か、を評価する評価手段と、前記評価手段よって評価された前記進路に前記所定の安全条件を満たすものがない場合に、前記所定の本数より多くの本数の前記進路を生成し、当該進路が前記評価手段によって評価されるよう制御する第２進路生成手段と、を備えたことを特徴とする。

また、本発明の別な態様は、移動体および前記移動体から所定の範囲に存在する障害物の、位置および内部状態を少なくとも記憶する記憶手段と、制御手段と、を備えた移動体進路取得装置において、前記制御手段は、前記記憶手段から読み出した前記障害物の位置および内部情報に基づいて、前記障害物がとりうる進路の確率的な予測を行う障害物進路予測手段と、前記記憶手段から読み出した前記移動体の位置および内部状態に基づいて、所定の物理条件において前記移動体がとりうる進路を生成する第１進路生成手段と、生成された前記移動体の前記進路が、予測された前記障害物の前記進路との関係で、所定の安全条件を満たすか否か、を評価する評価手段と、前記評価手段によって評価された前記進路に前記所定の安全条件を満たすものがない場合に、前記所定の物理条件より緩やかな物理条件において前記移動体のとりうる前記進路を生成し、当該進路が前記評価手段によって評価されるよう制御する第２進路生成手段と、を備えたことを特徴とする。

この発明によれば、障害物の位置および内部情報に基づいて障害物がとりうる進路の確率的な予測を行い、移動体の位置および内部状態に基づいて移動体がとりうる進路を所定の本数生成し、生成された移動体の進路が予測された障害物の進路との関係で所定の安全条件を満たすか否か、を評価し、評価された進路に所定の安全条件を満たすものがない場合に、所定の本数より多くの本数、または、所定の物理条件より緩やかな物理条件で、進路を生成し、当該進路が評価されるよう制御するので、計算量が膨大となることを防ぎながら、安全性と乗り心地を両立することができる。

以下、添付図面を参照して本発明を実施するための最良の形態（以下、「実施の形態」と称する。）を説明する。

［実施の形態１］
図１は、本発明の実施の形態１に係る移動体進路取得装置の機能構成を示すブロック図である。同図に示す移動体進路取得装置１は、移動体である四輪自動車等の車両に搭載され、移動体である自車の周囲に存在する障害物（他車や歩行者など）がとりうる進路を予測して、安全性と乗り心地を両立する進路の候補を取得する装置である。

（実施の形態１の構成）
図１に示すように、移動体進路取得装置１は、所定の範囲に存在する物体の位置や内部状態を検知する車外センサ２と、車外センサ２が検知した結果に基づいて所定の範囲に含まれる障害物を抽出する障害物抽出部３と、障害物抽出部３で抽出した障害物がとりうる進路の確率的な予測を行う障害物進路予測部４と、自車の位置や内部状態を検知する自車センサ５と、自車センサ５が検知した結果に基づいて、所定の物理条件における自車の現在位置からのとりうる進路を所定の本数生成する第１自車進路生成部６と、自車と障害物との衝突確率を計算する衝突確率計算部７と、衝突確率計算部７により計算された各進路の衝突確率に基づいて自車危険度を判定し、自車危険度が所定値以下か否かを判断する危険度判断部８と、危険度判断部８により判断された進路に危険度が所定値以下のものがない場合に、所定の本数より多くの本数の進路、または、所定の物理条件より緩やかな物理条件における自車のとりうる進路を生成する第２自車進路生成部９と、第２自車進路生成部９により生成された進路に危険度が所定値以下のものがない場合に、目的値関数に基づいて目的地までの距離を計算し、目的地に近い進路を選択する距離関数計算部１０と、危険度判定部８により判定された進路または距離関数計算部１０により計算された目的地に近い進路を出力する出力部１１と、車外センサ２で検知した物体の位置および内部状態や、各種演算結果を含む情報を記憶する記憶部１２と、を備える。

ここで、図１の破線で囲った部分は、本実施の形態における特徴部分を示す。すなわち、障害物進路予測部４が、車外センサ２によって測定され記憶部１２に記憶された障害物の位置および内部状態に基づいて、障害物がとりうる進路の確率的な予測を行うことにより障害物進路予測手段として機能する。そして、第１自車進路生成部６が、自車センサ５によって測定され記憶部１２に記憶された移動体（自車）の位置および内部状態に基づいて、所定の物理条件において移動体がとりうる進路を所定の本数生成することにより第１進路生成手段として機能する。そして、衝突確率計算部７が、生成された移動体の進路と、予測された障害物の進路と、に基づいて衝突確率を計算し、危険度判定部８が、所定の安全条件（例えば、衝突確率が５％以下）を満たすか否か、を評価することにより評価手段として機能する。そして、危険度判定部８により評価された進路に所定の安全条件を満たすものがない場合に、第２自車進路生成部９が、所定の本数より多くの本数の進路、または、所定の物理条件より緩やかな物理条件下でとりうる進路を生成し、衝突確率計算部７により計算されるよう制御することにより第２進路生成手段として機能する。また、第２自車進路生成部９により生成され危険度判定部８により評価された進路に所定の安全条件を満たすものがない場合に、距離関数計算部１０が、目的地関数に基づいて目的地までの距離を計算し、目的地に近い進路を出力部１１に出力することにより距離関数計算手段として機能する。

かかる構成において、車外センサ２または自車センサ５は、ミリ波レーダ、レーザレーダ、画像センサ等を用いることによって実現されてもよく、速度センサ、ヨーレートセンサ、加速度センサ、舵角センサ等の各種センサを備えてもよい。なお、車外センサ２または自車センサ５が検知する物体の「内部状態」とは、物体の進路予測に用いることができるような有益な状態のことであり、好ましくは物体の速度（速さと向きを有する）やヨーレート（大きさと向きを有する）等の物理量であり、それらの物理量の値が０の場合（物体が停止している状態）も含まれる。

また、出力部１１は、距離計算関数計算部１０の処理結果に対応した情報に基づいて画像を生成する画像生成部と、画像生成部で生成された画像を含む情報を表示出力するために液晶、プラズマ、または有機ＥＬ（Ｅｌｅｃｔｒｏｌｕｍｉｎｅｓｃｅｎｃｅ）等のディスプレイを用いて実現される表示部とを有して構成されてもよい。また、表示部として、運転席の後方上部にプロジェクタが設置されており、このプロジェクタによって自動車のフロントガラスへの重畳表示を行うことができるよう構成されてもよい。ここで、出力部１１は、衝突確率計算部７で算出された衝突確率に基づいて画像を生成する画像生成部と、画像生成部で生成された画像を含む情報を表示出力する表示部と、第１，第２自車進路生成部６，９で生成された自車進路と障害物と衝突確率が所定の閾値を超える場合に警告音（音声を含む）を発生する警告音発生部とを備えて構成されてもよい。

また、記憶部１２は、車外センサ２または自車センサ５での検知結果に加えて、障害物進路予測部４による予測結果や第１，第２自車進路生成部６，９による生成結果などを記憶する。記憶部１２は、所定のＯＳ（Ｏｐｅｒａｔｉｏｎ Ｓｙｓｔｅｍ）を起動するプログラムや移動体進路取得プログラム等が予め記憶されたＲＯＭ、および各処理の演算パラメータやデータ等を記憶するＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）を用いて実現される。また、記憶部１２は、移動体進路取得装置１に対してコンピュータ読み取り可能な記録媒体を搭載可能なインタフェースを設け、このインタフェースに対応する記録媒体を搭載することによって実現することもできる。

以上の機能構成を有する移動体進路取得装置１は、演算および制御機能を有するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を備えた電子的な装置（コンピュータ等）である。移動体進路取得装置１が備えるＣＰＵは、記憶部１２が記憶する情報および上述した移動体進路取得プログラムを含む各種プログラムを記憶部１２から読み出すことによって本実施の形態１に係る移動体進路取得方法に関する演算処理を実行する制御手段である。

なお、本実施の形態１に係る移動体進路取得プログラムは、ハードディスク、フレキシブルディスク、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、フラッシュメモリ、ＭＯディスク等のコンピュータ読み取り可能な記録媒体に記録して広く流通させることも可能である。

（実施の形態１に係る移動体進路取得方法）
次に、本実施の形態１に係る移動体進路取得方法について説明する。図２は、本実施の形態１に係る移動体進路取得方法の処理の概要を示すフローチャートである。

図２に示すように、まず、車外センサ２は、所定の範囲にある物体の自車に対する位置および内部状態を検知し記憶部１２に格納し、自車センサ５は、自車の位置および内部状態を検知し記憶部１２に格納する（ステップＳＡ−１）。ここで、本実施の形態においては、物体の位置は物体の中心の位置によるものとし、物体の内部状態は速度（速さｖ，向きθ）によって特定される。

次に、障害物抽出部３は、車外センサ２で検知された結果に基づいて所定の範囲内にある障害物を抽出し、障害物進路予測部４は、障害物抽出部３により抽出された障害物の位置及び内部情報を記憶部１２から読み出し、障害物がとりうる進路の確率的な予測を行う（ステップＳＡ−２）。

すなわち、障害部抽出部３は、車外センサ２で検知された物体のうち、自車の走行の妨げとなる物体を障害物として抽出し、それ以外の物体を除外する。ここで、障害物抽出部３は、複数の障害物を抽出した場合に、複数の障害物の中から一つの障害物（以下、「特定障害物」と呼ぶ。）を選択してもよい。例えば、障害部抽出部３は、自車が現状のままで走行したときに障害物と衝突するまでの時間ＴＴＣ（Ｔｉｍｅ Ｔｏ Ｃｏｌｌｉｓｉｏｎ）が最短の障害物を、特定障害物として選択するようにしてもよい。ここで、図３は、速さｖ_０で直進する自車Ｃ_０に対して２台の車両Ｃ_１およびＣ_２が障害物として抽出された場合の自車Ｃ_０周辺の状況を示す図である。図３においては、３つの走行レーンを有する道路Ｒの左レーンＬ_１を自車Ｃ_０が走行しており、その右側に隣接する中央レーンＬ_２に２台の先行車両Ｃ_１およびＣ_２が走行している場合を表している。また、一番右側の右レーンＬ_３には、少なくとも車外センサ２が検知可能な領域内に物体が存在していない。図３では、３台の車両は全て直進しており、中央レーンＬ_２では、後方の車両Ｃ_１の速さｖ_１が前方の車両Ｃ_２の速さｖ_２よりも大きい。障害物抽出部３は、抽出した障害物が複数存在する場合、その中から特定障害物を選択する場合、自車Ｃ_０からの距離が一番近い物体、自車Ｃ_０の周囲で速さが一番速い物体、または、速さが一番遅い物体のうちいずれかの条件で選択してもよい。例えば、図３においては、自車Ｃ_０から一番近い障害物Ｃ_１を特定障害物としてもよい。

また、障害物進路予測部４が障害物の進路を確率的に予測する方法は、従来知られているさまざまな手法を適用することができる。例えば、障害物進路予測部４は、障害物が自動車である場合に、運転者の操作が一様な確率（例えば、等確率）でなされるとして進路を予測してもよい。また、障害物進路予測部４は、現況に応じて障害物のとりうる複数の進路に所定の確率分布を付与することによって進路予測を行ってもよい。また、障害物進路予測部４は、予め、障害物の種類に応じたモデルが記憶された記憶部１２から当該種類に応じたモデルを読み出し、この読み出したモデルを用いて確率的な進路予測を行ってもよい。図４は、障害物の進路に対して付与する確率分布の一例を示す図である。具体的には、障害物に対して直進方向を最大とする確率分布曲線ρ_１が付与された場合を図示している。この意味で、図４におけるｘ座標は、道路Ｒの幅方向の座標であり、その原点は障害物の現在位置を表している。なお、障害物に対して付与する確率分布は、例えば正規分布などに代表されるような単峰性を有していれば好ましいが、その分布関数まで限定されるものではない。また、特定障害物の確率分布曲線ρ_１よりも分散が小さく、直進方向かその近傍への進路を選択する可能性が極めて高いような分布を示す、確率分布曲線ρ_Ｇを用いる場合、次式（１）で定義される確率密度関数ρ_３（ｘ）を適用してもよい。

ここで、Ｃは規格化定数、θ（ｘ）は、θ（ｘ）＝０（ｘ＜０），１（ｘ≧０）を満たす関数、εは正の定数である。

再び図２に戻り、第１自車進路生成部６は、自車センサ５によって検知された自車の位置及び内部状態に基づいて、所定の物理条件において自車がとりうる進路を網羅的に所定の本数生成する（ステップＳＡ−３）。本実施の形態においては、その進路を採用した場合に操作量（加減速や操舵など）により自車に発生する加速度について、物理条件（例えば、０．３Ｇ）が設定される。この物理条件は、物理限界値である約０．９Ｇよりも低い条件である。また、自車センサ５が白線等の路面環境を検知することが可能であれば、走行可能なレーンの数に応じた進路（軌跡）を生成するようにしてもよい。なお、このステップＳＡ−３は、ステップＳＡ−２の処理と並行して行われる。

そして、衝突確率計算部７は、第１自車進路生成部６により生成された自車の進路と、障害物進路予測部４により予測された障害物の進路に基づいて、衝突確率を計算し、危険度判定部８は、衝突確率に基づく危険度が低い（所定値以下）という所定の安全条件を満たすか否か判定する（ステップＳＡ−４）。本実施の形態においては、衝突確率計算部７は、生成された各自車のとりうる進路に対して、予測された障害物の全てのとりうる進路との衝突確率を計算する。ここで、衝突確率計算部７は、自車の進路と障害物の進路とに基づいて、同時間における位置が所定の距離よりも近くなった場合に衝突したとみなして、衝突確率の計算を行う。この際に衝突したとみなされる距離は、障害物の種類に応じて定められる。また、危険度判定部８は、ある時刻ｔにおいて、その時刻からｓ秒間、最善（衝突確率最小）の進路を走行した場合の衝突確率（危険度）を、自車危険度として判定し、当該自車危険度が所定値以下（例えば、５％以下）であるか判定する。

そして、危険度判定部８により自車危険度が所定値以下の進路がないと判定された場合に（ステップＳＡ−４、無し）、第２自車進路生成部９は、操作量による所定の物理条件をより緩やかに切り替えた物理条件（例えば、物理限界値０．９Ｇ）において自車がとりうる進路を所定の本数生成する（ステップＳＡ−５）。ここで、第２自車進路生成部９により生成する所定の本数は、第１自車進路生成部６により生成する本数より多く設定してもよい。

そして、再び、衝突確率計算部７は、第１自車進路生成部６により生成された自車の進路と、障害物進路予測部４により予測された障害物の進路に基づいて、衝突確率を計算し、危険度判定部８は、衝突確率に基づく危険度が低い（所定値以下）という所定の安全条件を満たすか否か判定する（ステップＳＡ−６）。

そして、第２自車進路生成部９により生成された進路のうち危険度判定部８によって所定値以下の進路がないと判定された場合（ステップＳＡ−６、無し）、距離関数計算部１０は、目的地関数に基づいて目的地までの距離を計算し、目的地に近い進路を選択する（ステップＳＡ−７）。

そして、距離関数計算部１０により選択された進路は、出力部１１により出力される（ステップＳＡ−８）。また、危険度判定部８により危険度が所定値以下であると判定された場合（ステップＳＡ−４，６、有り）、出力部１１は、その評価結果を出力する（ステップＳＡ−８）。ここで、進路の評価結果は、距離関数計算部１０により計算された目的地までの距離が近いものを優先的に出力するよう構成してもよい。ここで、出力部１１による出力される出力例について説明する。

出力部１１は、上述した移動体進路取得処理の結果に基づいて所定の情報を出力する。例えば、衝突確率が所定の閾値を超えた場合に表示部によって表示を行うとともに、衝突確率が所定の閾値を超えた場合に警告音発生部が警告音を発生する。逆に、衝突確率が最も低い進路（またはレーン）を推奨進路（または推奨レーン）として表示したり音声で報知したりしてもよい。また、障害物が所定値を超える確率で進路をとる可能性がある領域を表示するのは好ましい。図５は、表示部における予測結果の表示出力例を示す図であり、図３に示す道路環境における障害物としての車両Ｃ_１に対する進路予測結果の表示出力例を模式的に示す図である。図５では、車両Ｃ_１の予測進路のうち確率が所定値以上である進路を示す領域Ｄを自車Ｃ_０のフロントガラスＦに半透明に重畳表示した場合を図示している。

上述した重畳表示は、画像生成部で生成された画像を、自車Ｃ_０の運転席の後方上部に設置されたプロジェクタ（図示せず）からフロントガラスＦへ投影することによって実現される。これにより、自車Ｃ_０の運転者は、運転しながら近い将来に危険が生じる可能性のある領域を即座に認識することができる。したがって、その認識結果を運転に反映させることによって適確に危険を回避することが可能となる。反対に、出力部は、重畳表示により、危険がない安全な進路を表示してもよい（図示せず）。また、例えば自車Ｃ_０に搭載されたカーナビゲーションシステムの表示画面ＣＮ（図５を参照）に表示部の機能を具備させることによって障害物の進路予測結果を表示するようにしてもよい。以上が、移動体進路取得方法による処理である。なお、本実施の形態１に係る移動体進路取得方法は、所定の時間間隔で繰り返し行われ、常に最新の道路環境に即した情報が出力される。このため、本実施の形態１に係る移動体進路取得方法によれば、自車の運転者が刻々と変化する道路環境に対応して適確な操作を行うのを支援することが可能となる。

以上説明した本発明の実施の形態１によれば、障害物の位置および内部情報に基づいて障害物がとりうる進路の確率的な予測を行い、移動体の位置および内部状態に基づいて移動体がとりうる進路を所定の本数生成し、生成された移動体の進路が予測された障害物の進路との関係で所定の安全条件を満たすか否か、を評価し、評価された進路に所定の安全条件を満たすものがない場合に、所定の本数より多くの本数、または、所定の物理条件より緩やかな物理条件で、進路を生成し、当該進路が評価されるよう制御するので、計算量が膨大となることを防ぎながら、利用者に安全性と乗り心地を両立する進路を提供することができる。すなわち、情報の提示を受けた自車の運転者は、運転中の近い将来に起こりうる危険を迅速かつ適確に回避して安全な進路をとりながら、乗り心地のより快適な運転をすることが可能となる。また、処理に必要な計算量を抑え、装置への負荷が少ない実用的な障害物の進路予測処理を実現することができる。

以上が、実施の形態１の説明である。ここで、以上の説明では、自車と同じ方向を向いて走行している先行車を障害物として説明してきたが、例えば自車と同じ方向を向いて走行している後続車を障害物とすることもできる。また、自車と反対方向を向いて走行している対向車を障害物とすることもできる。さらには、静止している物体を障害物とすることもできる。また、以上の説明においては、予測対象の物体は全て２次元平面上を移動するものと仮定して説明したが、これに限られず、３次元空間を移動する物体に対しても適用可能である。また、一つの物体が複数の自由度を有する場合（例えば６自由度を有するロボットアームのような物体）にも適用することができる。

［実施の形態２］
本発明の実施の形態２は、上記実施の形態１と同様に、第１の自車進路生成処理と第２の自車進路生成処理を行うが、危険度が所定値以下の進路がないと判定した後に第２の自車進路生成を行うのではなく、第１の自車進路生成処理と並行して第２の自車進路生成処理を行うことを特徴とする。

図６は、本実施の形態２に係る移動体進路取得装置２１の機能的構成を示すブロック図である。同図に示す移動体進路取得装置２１において、危険度判定部２８と第２自車進路生成部２９以外の構成は上記実施の形態１と同様である。このため、障害物進路予測装置１と同じ機能を有する部位についてはそれぞれ同じ記号を付してある。

危険度判定部２８は、第１自車進路生成部６により生成された進路に危険度が所定値以下のものがなかった場合に、第２自車進路生成部２９により並行して生成された進路の中から危険度が所定値以下のものがあるか否か判定する。

第２自車進路生成部２９は、自車センサ５が検知した自車の位置や内部状態に基づいて、第１の自車進路生成処理の所定の物理条件よりも緩やかな物理条件において自車の現在位置からのとりうる進路を、所定の本数生成する。ここで、第２自車進路生成部２９は、第１自車進路生成処理における所定の本数より多くの本数の進路を生成してもよい。

以上の構成において本実施の形態２による移動体進路取得方法を説明する。本実施の形態２における移動体進路生成方法は、図２を用いて説明した実施の形態１の移動体進路取得方法におけるステップＳＡ−１〜ＳＡ−３の処理と同様であるので説明を省略する。

つぎに、第２自車進路生成部２９は、第１自車進路生成処理（ステップＳＡ−３）と並列的に第２自車進路生成処理を行う。すなわち、操作量による所定の物理条件をより緩やかに切り替えた物理条件（例えば、物理限界値０．９Ｇ）において自車がとりうる進路を所定の本数生成する。ここで、第２自車進路生成部２９により生成する所定の本数は、第１自車進路生成部６により生成する本数より多く設定してもよい。

そして、衝突確率計算部７による衝突確率の計算の後、危険度判定部２８は、まず、第１自車進路生成部６により生成された進路に危険度が所定値以下のものがあるか否か判定し、なかった場合に、次に第２自車進路生成部２９により生成された進路の中から危険度が所定値以下のものがあるか否かを判定する。

そして、これ以降の本実施の形態２における移動体進路生成方法は、上述した実施の形態１の移動体進路取得方法におけるステップＳＡ−６〜ＳＡ−８の処理と同様であるので説明を省略する。

以上説明した本発明の実施の形態２によれば、障害物の位置および内部情報に基づいて障害物がとりうる進路の確率的な予測を行い、移動体の位置および内部状態に基づいて移動体がとりうる進路を所定の本数生成し、生成された移動体の進路が予測された障害物の進路との関係で所定の安全条件を満たすか否か、を評価し、評価された進路に所定の安全条件を満たすものがない場合に、所定の本数より多くの本数、または、所定の物理条件より緩やかな物理条件で、進路を生成し、当該進路が評価されるよう制御するので、計算量が膨大となることを防ぎながら、利用者に安全性と乗り心地を両立する進路を提供することができる。さらに、第１の進路生成処理と第２の進路生成処理を並行して行うので、処理速度が向上し、利用者に迅速に処理結果を出力することができる。

［軌跡生成処理］
上述した実施の形態１または２における、障害物進路予測部４、第１自車進路生成部６、または、第２自車進路生成部９，２９による処理の一例について以下に説明する。障害物進路予測部４、第１自車進路生成部６、または、第２自車進路生成部９，２９は、障害物または移動体が時間の経過とともにとりうる位置の変化である進路を、時間および空間から構成される時空間上での軌跡として生成してもよい（以下、「軌跡生成処理」と呼ぶ。）。

すなわち、障害物進路予測部４、第１自車進路生成部６、または、第２自車進路生成部９，２９は、所定の時間が経過するまでに障害物または移動体がとりうる軌跡を予測生成するものであり、障害物をシミュレーション上で仮想的に動作させるための操作を複数の操作から選択し、選択した操作を所定の時間行い、操作した後の障害物の位置および内部状態が所定の物理条件を満たしているか否かを判定する。図７は、軌跡生成処理の一例を示すフローチャートである。なお以下、代表して障害物進路予測部４において実行される軌跡生成処理について説明するが、同様の軌跡生成処理は、第１自車進路生成部６、または、第２自車進路生成部９，２９により行われるよう構成してもよい。

図７においては、車外センサ２で検知した障害物Ｏ_Ｓに対して軌跡を生成する演算をＮ回行うものとする（Ｎは自然数）。また、軌跡を生成する時間（軌跡生成時間）をＴ（＞０）とする。この軌跡生成時間Ｔ（および後述する操作時間Δｔ）を適切に定めることにより、実用的な計算時間で一連の障害物進路予測処理を行うことが可能となる。

障害物進路予測部４は、最初に障害物Ｏ_Ｓに対する軌跡生成回数を示すカウンタｎの値を１とする初期化を行う（ステップＳＢ−１）。

次に、障害物進路予測部４は、車外センサ２で検知した結果を記憶部１２から読み出し、この読み出した検知結果を初期状態とする（ステップＳＢ−２）。具体的には、時間ｔを０とし、障害物Ｏ_Ｓの初期位置（ｘ（０），ｙ（０））および初期内部状態（ｖ（０），θ（０））を、それぞれ車外センサ２からの入力情報（ｘ_０，ｙ_０）および（ｖ_０，θ_０）とする。

続いて、障害物進路予測部４は、その後の時間Δｔの間に行う操作ｕ（ｔ）を、選択可能な複数の操作の中から、各操作に予め付与された操作選択確率にしたがって一つの操作を選択する（ステップＳＢ−３）。操作ｕ_ｃを選択する操作選択確率ｐ（ｕ_ｃ）は、例えばｕ（ｔ）として選択可能な操作の集合｛ｕ_ｃ｝の要素と所定の乱数とを対応付けることによって定義される。この意味で、操作ｕ_ｃごとに異なる操作選択確率ｐ（ｕ_ｃ）を付与してもよいし、操作集合｛ｕ_ｃ｝の全要素に対して等しい確率を付与してもよい。後者の場合には、ｐ（ｕ_ｃ）＝１／（選択可能な全操作数）となる。なお、操作選択確率ｐ（ｕ_ｃ）を、障害物Ｏ_Ｓの位置、内部状態、周囲の道路環境に依存する関数として定義することも可能である。

一般に、操作ｕ_ｃは複数の要素から構成され、障害物Ｏ_Ｓの種類によって選択可能な操作の内容が異なる。例えば、障害物Ｏ_Ｓが四輪自動車の場合、その四輪自動車の加速度や角速度はステアリングの切り具合やアクセルの踏み具合等によって決まる。この点に鑑みて、四輪自動車である障害物Ｏ_Ｓに対して施される操作ｕ_ｃは、加速度や角速度を含む要素によって決定される。これに対して、障害物Ｏ_Ｓが人である場合には、速度によって操作ｕ_ｃを指定することができる。

より具体的な操作ｕ_ｃの設定例を挙げる。障害物Ｏ_Ｓが自動車の場合には、加速度を−１０〜＋３０（ｋｍ／ｈ／ｓｅｃ）、操舵角を−７〜＋７（ｄｅｇ／ｓｅｃ）の範囲で取り（いずれも符号で向きを指定）、障害物Ｏ_Ｓが人の場合には、速さを０〜３６（ｋｍ／ｈ）、向きを０〜３６０（ｄｅｇ）の範囲で取る。なお、ここで記載した量は全て連続量である。このような場合には、適当な離散化を施すことによって各操作の要素数を有限とし、操作の集合｛ｕ_ｃ｝を構成すればよい。

この後、障害物進路予測部４は、ステップＳＢ−３で選択した操作ｕ_ｃを時間Δｔの間動作させる（ステップＳＢ−４）。この時間Δｔは、精度の上では小さい方がより好ましいが、実用上は０．１〜０．５（ｓｅｃ）程度の値とすればよい。なお、以下の説明において、軌跡生成時間ＴはΔｔの整数倍であるとするが、Ｔの値は障害物Ｏ_Ｓの速度に応じて可変としてもよく、Δｔの整数倍でなくてもよい。

続いて、障害物進路予測部４は、ステップＳＢ−４で操作ｕ_ｃを動作させた後の障害物Ｏ_Ｓの内部状態が所定の制御条件を満たしているか否かを判定する（ステップＳＢ−５）。このステップＳＢ−５で判定する制御条件は、障害物Ｏ_Ｓの種類に応じて定められ、例えば障害物Ｏ_Ｓが四輪自動車である場合には、ステップＳＢ−４の動作後の速度の範囲や、ステップＳＢ−４の動作後の加速度の最高車両Ｇ等によって定められる。

ステップＳＢ−５の判定の結果、障害物Ｏ_Ｓの内部状態が所定の制御条件を満たしている場合（ステップＳＢ−５でＹｅｓ）、障害物進路予測部４は、操作ｕ_ｃを動作させた後の障害物Ｏ_Ｓの位置が移動可能領域内にあるか否かを判定する（ステップＳＢ−６）。このステップＳＢ−６で判定する移動可能領域とは、道路（車道、歩道を含む）等の領域を指す。以後、物体が移動可能領域に位置する場合を、「移動条件を満たす」と表現する。

ステップＳＢ−６の判定の結果、障害物Ｏ_Ｓが移動可能領域内に位置している場合（ステップＳＢ−６でＹｅｓ）、障害物進路予測部４は、時間をΔｔだけ進め（ｔ←ｔ＋Δｔ）、ステップＳＢ−４の動作後の位置を（ｘ（ｔ），ｙ（ｔ））、内部状態を（ｖ（ｔ），θ（ｔ））とする（ステップＳＢ−７）。

なお、ステップＳＢ−５およびＳＢ−６において一つでも満足しない条件がある場合（ステップＳＢ−５でＮｏまたはステップＳＢ−６でＮｏ）には、ステップＳＢ−２に戻る。

以上説明したステップＳＢ−２〜ＳＢ−７の処理は、軌跡生成時間Ｔに達するまで繰り返し行われる。すなわち、ステップＳＢ−７で新たに定義された時間ｔがＴに達していない場合（ステップＳＢ−８でＮｏ）、ステップＳＢ−３に戻って処理を繰り返す。他方、ステップＳＢ−７で新たに定義された時間ｔがＴに達した場合（ステップＳＢ−８でＹｅｓ）、障害物Ｏ_Ｓに対する軌跡を出力し、記憶部１２に格納する（ステップＳＢ−９）。

図８は、時間ｔ＝０，Δｔ，２Δｔ，・・・，ＴでステップＳＢ−３からステップＳＢ−７に至る一連の処理を繰り返すことによって生成された障害物Ｏ_Ｓの軌跡を模式的に示す図である。同図に示す軌跡Ｐ_Ｓ（ｍ）（１≦ｍ≦Ｎ， ｍは自然数）は、空間２次元（ｘ，ｙ）、時間１次元（ｔ）の３次元時空間（ｘ，ｙ，ｔ）を通過する。この軌跡Ｐ_Ｓ（ｍ）をｘ−ｙ平面上に射影すれば、２次元空間（ｘ，ｙ）における障害物Ｏ_Ｓの予測進路を得ることができる。

ステップＳＢ−９の後、カウンタｎの値がＮに達していなければ（ステップＳＢ−１０でＮｏ）、障害物進路予測部４はカウンタｎの値を１増やし（ステップＳ４Ｂ−１１）、ステップＳＢ−２に戻って上述したステップＳＢ−２〜ＳＢ−８の処理を軌跡生成時間Ｔに達するまで繰り返し行う。

ステップＳＢ−１０でカウンタｎがＮに達した場合（ステップＳＢ−１０でＹｅｓ）、障害物Ｏ_Ｓに対する全ての軌跡の生成が完了する。図９は、障害物Ｏ_Ｓに対して生成されたＮ個の軌跡Ｐ_Ｓ（１），Ｐ_Ｓ（２），・・・，Ｐ_Ｓ（Ｎ）からなる軌跡集合｛Ｐ_Ｓ（ｎ）｝を３次元時空間上で模式的に示す図である。軌跡集合｛Ｐ_Ｓ（ｎ）｝の要素をなす各軌跡の始点すなわち初期位置（ｘ_０，ｙ_０，０）は同じである（ステップＳＢ−２を参照）。なお、図９はあくまでも模式図であり、Ｎの値としては、例えば数百〜数万程度の値をとることが可能である。

ステップＳＢ−１０でカウンタｎがＮに達した場合、軌跡生成が完了したことになるので、軌跡生成処理を終了する。

図９において、時空間の各領域における軌跡集合｛Ｐ_Ｓ（ｎ）｝の単位体積当たりの密度は、その時空間の各領域における障害物Ｏ_Ｓの存在確率の密度（以後、「時空間確率密度」と呼ぶ。）を与えている。したがって、軌跡生成処理によって構成された軌跡集合｛Ｐ_Ｓ（ｎ）｝を用いることにより、障害物Ｏ_Ｓが３次元時空間上の所定の領域を通過する確率を求めることが可能となる。なお、時空間確率密度は、あくまでも時空間上における確率概念であるため、一つの物体に対して時空間上でその値の総和を取ったときに１になるとは限らない。

ところで、軌跡生成時間Ｔの具体的な値は、予め固定値として設定する場合には、その値Ｔを超えたところまで軌跡を生成すると時空間上の確率密度分布が一様になってしまい、計算しても意味がないような値とするのが好ましい。例えば、物体が四輪自動車であり、その四輪自動車が通常の走行を行っている場合には、たかだかＴ＝５（ｓｅｃ）程度とすればよい。この場合、ステップＳＢ−４における操作時間Δｔを０．１〜０．５（ｓｅｃ）程度とすると、１本の軌跡Ｐ_Ｓ（ｍ）を生成するために、ステップＳＢ−３からステップＳＢ−７に至る一連の処理を１０〜５０回繰り返すことになる。

なお、高速道路、一般道、２車線道路などの道路の種類ごとに異なる軌跡生成時間Ｔを設定し、位置データを用いて現在走行中の道路の種類を地図データから読み取る方法や、画像認識等を応用した道路認識装置によって道路の種類を読み取る方法などによって切替を行うことは好ましい。

また、軌跡生成時間Ｔまで算出した軌跡を用いることによって時空間上の確率密度分布を統計的に評価し、分布が一定となっている場合には軌跡生成時間Ｔを減らし、分布が一定となっていない場合には生成時間を増やすような適応制御を行うことも好ましい。以上が、軌跡生成処理の一例の詳細な説明である。

［障害物進路予測処理］
障害物進路予測部４における障害物進路予測処理の一例について以下に説明する。以上、説明した障害物に対する軌跡生成処理の後、障害物進路予測部４においては、障害物がとりうる進路の確率的な予測を行う。以下では、障害物進路予測部４における具体的な予測演算処理として、障害物Ｏ_Ｓに対して生成された軌跡集合｛Ｐ_Ｓ（ｎ）｝の中での軌跡Ｐ_Ｓ（ｍ）が選ばれる確率を求める場合について説明するが、この予測演算が一例に過ぎないことは勿論である。

障害物Ｏ_Ｓの軌跡がＮ本生成されたとき、そのうちの１本の軌跡Ｐ_Ｓ（ｍ）が実際の軌跡となる確率ｐ（Ｐ_Ｓ（ｍ））は、次のように算出される。まず、障害物の軌跡Ｐ_Ｓ（ｍ）を実現するための操作列｛ｕ_ｍ（ｔ）｝が｛ｕ_ｍ（０），ｕ_ｍ（Δｔ），ｕ_ｍ（２Δｔ），・・・，ｕ_ｍ（Ｔ）｝であったとすると、時間ｔにおいて操作ｕ_ｍ（ｔ）が選択される確率はｐ（ｕ_ｍ（ｔ））なので、ｔ＝０〜Ｔで操作列｛ｕ_ｍ（ｔ）｝が実行される確率は、

と求められる。したがって、障害物Ｏ_ＳにＮ本の軌跡集合｛Ｐ_Ｓ（ｎ）｝が与えられたとき、障害物Ｏ_Ｓがとりうる一つの軌跡Ｐ_Ｓ（ｍ）が選ばれる確率ｐ（Ｐ_Ｓ（ｍ））は、

となる。

ここで、全ての操作ｕ_ｍ（ｔ）が等確率ｐ_０（ただし、０＜ｐ_０＜１）で選択される場合、式（２）は、

となる。ここで、自然数ｈはｔ＝０からＴまでの操作時間Δｔの総数すなわち操作回数である。したがって、障害物Ｏ_ＳがとりうるＮ本の軌跡に含まれる軌跡Ｐ_Ｓ（ｍ）の確率の総和はＮｐ_０ ^ｈとなり、そのうちの１本の軌跡Ｐ_Ｓ（ｍ）が選ばれる確率ｐ（Ｐ_Ｓ（ｍ））は、式（４）を式（３）に代入することによって、

となる。すなわち、確率ｐ（Ｐ_Ｓ（ｍ））は軌跡Ｐ_Ｓ（ｍ）に依存しない。

この後、障害物進路予測部４は、算出した確率ｐ（Ｐ_Ｓ（ｍ））に基づいて、３次元時空間の各領域における単位体積当たりの障害物Ｏ_Ｓの存在確率を求める。この存在確率は、軌跡集合｛Ｐ_Ｓ（ｎ）｝の３次元時空間上の時空間確率密度に対応しており、通過している軌跡の密度が高い領域は、存在確率が概ね大きい。

障害物が複数ある場合、当該処理の対象である特定障害物Ｏ_Ｓの軌跡集合｛Ｐ_Ｓ（ｎ）｝と一般障害物の予測進路とを加えることによって形成された時空間環境Ｅｎｖ（Ｐ_Ｓ，Ｐ_Ｇ）は、特定障害物Ｏ_Ｓの軌跡集合｛Ｐ_Ｓ（ｎ）｝および一般障害物Ｏ_Ｇの１本の軌跡Ｐ_Ｇからなる。

より具体的には、時空間環境Ｅｎｖ（Ｐ_Ｓ，Ｐ_Ｇ）は、特定障害物Ｏ_Ｓに加えて一般障害物（特定障害物以外の障害物）Ｏ_Ｇが、高速道路のような平坦かつ直線状の道路Ｒを＋ｙ軸方向に向かって移動している場合の時空間環境を示すものである（移動体進路取得装置１１が搭載されている自車Ｃ_０は時空間環境には含まれない）。ここでは、障害物同士の相関は考慮せずに個々の障害物に対して独立に軌跡生成を行っているため、異なる物体の軌跡同士が時空間上で交差することもありうる。

ここで、特定障害物Ｏ_Ｓと一般障害物Ｏ_Ｇとの干渉評価処理として、特定障害物Ｏ_Ｓの軌跡集合｛Ｐ_Ｓ（ｎ）｝から一般障害物Ｏ_Ｇの軌跡Ｐ_Ｇと交差する軌跡すなわち特定障害物Ｏ_Ｓと一般障害物Ｏ_Ｇが衝突する軌跡を除去してもよい。この際の衝突は、２つの軌跡が単に交点を有する場合に加えて、障害物の種類に応じた所定の距離よりも小さい距離まで接近した場合も含まれる。

その後、障害物進路予測部４は、除去されずに残った軌跡集合（非干渉軌跡集合）｛Ｐ_Ｓ’（ｎ）｝の各要素がとる確率の総和が１となるように規格化して確率を算出する。これにより、自車の周囲に複数の障害物が存在するような複雑な交通環境下であっても、実施の形態にかかる移動体の進路取得に適用することができる。

以上が障害物進路予測処理の概要である。ここで、障害物の時空間上での軌跡生成処理を行う際には、選択可能な全ての操作を動作させることによって軌跡を生成してもよい。このような軌跡生成処理を実現するアルゴリズムは、例えば縦型探索または横型探索による再帰呼出を適用することによって実現することが可能である。この場合、障害物Ｏ_Ｓの軌跡集合｛Ｐ_Ｓ（ｎ）｝の要素数すなわち軌跡の本数は、障害物Ｏ_Ｓに対する軌跡生成処理が終了するまで分からない。したがって、実行可能な操作を全探索することによって各物体がとりうる軌跡を生成する場合には、操作時間Δｔにおける操作ｕ_ｃ（ｔ）の要素の数（操作ｕ_ｃ（ｔ）が連続量の場合には離散化の度合い）に応じて最適な計算量を有する探索方法を選択すればよい。また、本障害物進路予測処理は、高低差のある道路を走行中の自動車に適用する場合のように、４次元時空間（空間３次元、時間１次元）においても適用可能である。

ここで、障害物進路予測部４は、複数の障害物がある場合に、それぞれの干渉を考慮して進路を予測してもよい。すなわち、障害物進路予測部４は、確率的に予測された障害物（特定障害物）の複数の進路と、予測されたその他の障害物（一般障害物）の進路との干渉を評価する。より具体的には、障害物の複数の進路のうち、一般障害物の進路と衝突するような進路をとる確率（予測確率）を０として除去する。この際の衝突は、障害物の種類に応じて定義される量であり、例えば障害物と一般障害物がともに自動車の場合には、同時間においてその両者が所定の距離（例えば車両の標準的な幅や長さ）よりも小さくなったときに衝突すると判定する。

［衝突確率計算処理］
衝突確率計算部７による衝突確率計算処理の一例について以下に詳細に説明する。以下の説明においては、衝突確率算出処理について詳細に説明する。

図１０は、衝突確率算出処理の一例を示すフローチャートである。同図に示す衝突確率算出処理は、２つのループ処理から構成されており、生成された自車Ｃ_０の軌跡Ｐ_０と予測された障害物Ｏ_ｓの進路（ここでは、除去されずに残った非干渉軌跡集合｛Ｐ_Ｓ’（ｎ）｝）との間の衝突確率を算出する。この際、衝突確率計算部７は、自車Ｃ_０の軌跡Ｐ_０、障害物Ｏ_Ｓの非干渉軌跡集合｛Ｐ_Ｓ’（ｎ）｝、および自車Ｃ_０と障害物Ｏ_Ｓの衝突確率を評価する評価関数を用いて衝突確率の計算を行う。なお、ここでは、衝突確率計算部７が評価関数を内蔵しているものとして説明を行うが、移動体進路取得装置１に入力部を設けることにより、その評価関数を外部から入力する構成としてもよい。また、評価関数を道路の種類や自車Ｃ_０の速度によって適応的に変化させる構成としてもよい。

まず、衝突確率計算部７は、障害物Ｏ_Ｓに対し、非干渉軌跡集合｛Ｐ_Ｓ’（ｎ）｝の全要素Ｐ_Ｓ’（ｎ_Ｓ）（ｎ_Ｓ＝１，２，・・・，Ｎ_Ｓ）に対する繰り返し処理（Ｌｏｏｐ１）を順次行う（ステップＳＣ−１）。この繰り返し処理においては、自車Ｃ_０と障害物Ｏ_Ｓとが干渉する度合いを定量的に与える量として干渉度ｒ_Ｓを導入し、この干渉度ｒ_Ｓの初期値を０とおく（ステップＳＣ−２）。

続いて、衝突確率計算部７は、自車Ｃ_０の軌跡Ｐ_０と障害物Ｏ_Ｓの一つの非干渉軌跡Ｐ_Ｓ’（ｎ_Ｓ）との干渉を評価する繰り返し処理（Ｌｏｏｐ２）を開始する（ステップＳＣ−３）。このＬｏｏｐ２では、二つの軌跡Ｐ_０と軌跡Ｐ_Ｓ’（ｎ_Ｓ）との同時間における距離を、時間ｔ＝０，Δｔ，・・・，Ｔにおいて順次求める。同時間における二つの軌跡の空間的な距離が所定値（例えば車両の標準的な幅や長さ）よりも小さくなった場合、自車Ｃ_０と障害物Ｏ_Ｓは衝突したとみなし、二つの車両が衝突したとみなすことができる距離の最大値（互いに干渉しあう空間的な距離）を干渉距離と呼ぶ。

図１１は、自車Ｃ_０の軌跡Ｐ_０と障害物Ｏ_Ｓの非干渉軌跡Ｐ_Ｓ’（ｎ_Ｓ）との時空間上での関係を模式的に示す図である。同図に示す場合、軌跡Ｐ_０と非干渉軌跡Ｐ_Ｓ’（ｎ_Ｓ）とは、２点ａ_１およびａ_２で交差している。したがって、この２点ａ_１およびａ_２の近傍には、二つの軌跡間の同時間における距離が干渉距離よりも小さい領域Ａ_１およびＡ_２が存在する。すなわち、二つの軌跡Ｐ_０および軌跡Ｐ_Ｓ’（ｎ_Ｓ）が領域Ａ_１およびＡ_２内にそれぞれ含まれる時間では、自車Ｃ_０と障害物Ｏ_Ｓとが衝突したという判定がなされる。換言すれば、時間ｔ＝０，Δｔ，・・・，Ｔのうちで、領域Ａ_１およびＡ_２内を通過する数が自車Ｃ_０と障害物Ｏ_Ｓとの衝突回数である。

図１１からも明らかなように、形成される時空間環境は、二つの軌跡が一度衝突してもその後の軌跡が生成される。これは、物体ごとの軌跡を独立に生成しているからである。

この後、衝突確率計算部７は、自車Ｃ_０と障害物Ｏ_Ｓの距離を求めた結果、上述した意味において自車Ｃ_０と障害物Ｏ_Ｓが衝突したと判定した場合（ステップＳＣ−４でＹｅｓ）、干渉度ｒ_Ｓの値を、

とする（ステップＳＣ−５）。ここで、第２項目ｐ（Ｐ_Ｓ’（ｎ_Ｓ））は、軌跡Ｐ_Ｓ’（ｎ_Ｓ）が選ばれる確率である（ここでは、既に軌跡ごとに付与された確率分布関数が規格化されている）。なお、ステップＳＣ−４で自車Ｃ_０と障害物Ｏ_Ｓが衝突しない場合には、後述するステップＳＣ−６に直接進む。

衝突確率計算部７は、ステップＳＣ−５の後、時間ｔがＴに達していない場合には繰り返しを終了せず（ステップＳＣ−６でＮｏ）、ｔの値をΔｔ増加させ（ステップＳＣ−７）、ステップＳＣ−３に戻ってＬｏｏｐ２を繰り返す。他方、衝突確率計算部７は、ステップＳＣ−５の後、時間ｔがＴに達している場合には、Ｌｏｏｐ２を終了する（ステップＳＣ−６でＹｅｓ）。

以上説明したＬｏｏｐ２の繰り返し処理により、干渉度ｒ_Ｓの値は、衝突回数が多いほど大きい値となる。このＬｏｏｐ２が終了した後、衝突確率計算部７は、Ｌｏｏｐ１を繰り返すか否かの判断処理を行う。すなわち、障害物Ｏ_Ｓに対して生成した軌跡のうち自車Ｃ_０の軌跡Ｐ_０との干渉評価が行われていないものがあれば、Ｌｏｏｐ１を終了せず（ステップＳＣ−８でＮｏ）、ｎ_Ｓをｎ_Ｓ＋１とし（ステップＳＣ−９）、ステップＳＣ−１に戻ってＬｏｏｐ１を繰り返す。

これに対して、障害物Ｏ_Ｓに対して生成した軌跡Ｐ_Ｓ’（ｎ_Ｓ）のうち自車Ｃ_０の軌跡Ｐ_０との干渉評価が全て行われた場合（ステップＳＣ−８でＹｅｓ）、衝突確率計算部７は、自車Ｃ_０の軌跡Ｐ_０と障害物Ｏ_Ｓの全ての非干渉軌跡集合｛Ｐ_Ｓ’（ｎ_Ｓ）｝との間の干渉を評価する最終的な干渉度ｒ_Ｓを付与し（ステップＳＣ−１０）、この付与した干渉度ｒ_Ｓを出力して記憶部１２へ格納する（ステップＳＣ−１１）。

ここで、自車Ｃ_０の軌跡Ｐ_０と障害物Ｏ_Ｓの軌跡Ｐ_Ｓ’（ｎ_Ｓ）との衝突回数をＭ（ｎ_Ｓ）とすると、干渉度ｒ_Ｓの値は、軌跡Ｐ_Ｓ’（ｎ_Ｓ）ごとの確率ｐ（Ｐ_Ｓ’（ｎ_Ｓ））をＭ（ｎ_Ｓ）倍した値を全ての軌跡集合｛Ｐ_Ｓ’（ｎ_Ｓ）｝の要素について和を取ったものである。

式（７）の右辺の和は、自車Ｃ_０の軌跡Ｐ_０が障害物Ｏ_Ｓがとりうる軌跡と衝突する衝突確率に他ならない。すなわち、式（７）によって自車Ｃ_０と障害物Ｏ_Ｓとが衝突する衝突確率が求められる。以上が、衝突確率計算部７による衝突確率計算処理の一例の説明である。

［他の実施の形態］
ここまで、本発明を実施するための最良の形態として、実施の形態を詳述してきたが、本発明はそれら実施の形態によってのみ限定されるべきものではなく、特願２００６−３０５７４４号明細書等において開示された種々の装置や方法、手段、処理、データ等を用いることができる。

また、本発明を自動運転システムに適用することも可能である。この場合には、移動体進路取得装置の出力（進路予測結果または自車との衝突確率）に対応して自車を操作する操作信号を生成し、この操作信号を自車に設けられた所定のアクチュエータ装置へ送信するようにすればよい。

また、センサで検知した実在の障害物に加えて、架空の障害物を配置し、この架空の障害物に対する進路予測を行ってもよい。より具体的には、自車にとって好ましくない挙動を示すような架空のモデルを構成し、このモデルを所定の位置に配置して進路予測を行ってもよい。このような架空のモデルは、例えば遮蔽物等が存在して見通しが悪い交差点付近を走行する自車から検知できない位置に配置することによって、交差点から飛び出してくる可能性のある障害物との衝突等の危険を予測することが可能となる。なお、架空のモデルの情報は予め記憶部で記憶しておき、別に設ける入力部からの条件設定に応じて所望の位置に配置することができるようにしてもよい。

ところで、本発明に係る移動体進路取得装置を、車両のみの走行が前提となる高速道路などの領域で適用する場合には、各車両に車車間通信用の通信手段をあわせて具備させることにより、互いに近くを走行している車両同士が、互いの走行状況を車車間通信によって交換し合うようにしてもよい。この場合には、各車両が操作履歴を各自の記憶部で記憶しておき、その操作履歴に基づいて操作ごとの操作選択確率を付与し、この操作選択確率に関する情報もあわせて他の車両に送信するようにしてもよい。これにより、進路予測の精度が高くなり、走行中の危険を一段と確実に回避することが可能となる。

加えて、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を位置検出手段として援用することも可能である。この場合には、ＧＰＳが記憶する３次元地図情報を参照することによってセンサで検知した物体の位置情報や移動情報の補正を行うことができる。さらには、ＧＰＳの出力を相互に通信することによってセンサとして機能させることも可能である。いずれの場合にも、ＧＰＳを援用することによって高精度の進路予測を実現することができ、予測結果の信頼性をさらに向上させることができる。

なお、本発明に係る移動体進路取得装置は、四輪自動車以外の車両、人、ロボット等の移動体に搭載することも可能である。

また、本発明に係る移動体進路取得装置は、移動体に搭載されている必要はない。例えば、自車が車車間通信や路車間通信を利用可能な場合、本発明に係る移動体進路取得装置を、自車と自車の周囲の他車とインフラとを含む進路干渉評価システムから構成することができる。この場合、障害物の進路予測計算をインフラ側で行うようにし、自車については、予測計算結果をインフラ側に要求して受信し、この受信した予測計算結果に基づいた処理を行う予測計算要求車両としてすることも可能である。

以上の説明からも明らかなように、本発明は、ここでは記載していないさまざまな実施の形態等を含みうるものであり、特許請求の範囲によりされる技術的思想を逸脱しない範囲内において種々の設計変更等を施すことが可能である。

本実施の形態１に係る移動体進路取得装置の機能構成を示すブロック図である。 本実施の形態１に係る移動体進路取得方法の処理の概要を示すフローチャートである。 速さｖ_０で直進する自車Ｃ_０に対して２台の車両Ｃ_１およびＣ_２が障害物として抽出された場合の自車Ｃ_０周辺の状況を示す図である。 障害物の進路に対して付与する確率分布の一例を示す図である。 表示部における予測結果の表示出力例を示す図であり、図３に示す道路環境における障害物としての車両Ｃ_１に対する進路予測結果の表示出力例を模式的に示す図である。 本実施の形態２に係る移動体進路取得装置２１の機能的構成を示すブロック図である。 軌跡生成処理の一例を示すフローチャートである。 時間ｔ＝０，Δｔ，２Δｔ，・・・，ＴでステップＳＢ−３からステップＳＢ−７に至る一連の処理を繰り返すことによって生成された障害物Ｏ_Ｓの軌跡を模式的に示す図である。 障害物Ｏ_Ｓに対して生成されたＮ個の軌跡Ｐ_Ｓ（１），Ｐ_Ｓ（２），・・・，Ｐ_Ｓ（Ｎ）からなる軌跡集合｛Ｐ_Ｓ（ｎ）｝を３次元時空間上で模式的に示す図である。 衝突確率算出処理の一例を示すフローチャートである。 自車Ｃ_０の軌跡Ｐ_０と障害物Ｏ_Ｓの非干渉軌跡Ｐ_Ｓ’（ｎ_Ｓ）との時空間上での関係を模式的に示す図である。

符号の説明

１，２１ 移動体進路取得装置
２ 車外センサ
３ 障害物抽出部
４ 障害物進路予測部
５ 自車センサ
６ 第１自車進路生成部
７ 衝突確率計算部
８，２８ 危険度判定部
９，２９ 第２自車進路生成部
１０ 距離関数計算部
１１ 出力部
１２ 記憶部

Claims (7)

1. 移動体および前記移動体から所定の範囲に存在する障害物の、位置および内部状態を少なくとも記憶する記憶手段と、制御手段と、を備えた移動体進路取得装置において実行される移動体進路取得方法であって、
   前記制御手段において実行される、
   前記記憶手段から読み出した前記障害物の位置および内部情報に基づいて、前記障害物がとりうる進路の確率的な予測を行う障害物進路予測ステップと、
   前記記憶手段から読み出した前記移動体の位置および内部状態に基づいて、前記移動体がとりうる進路を所定の本数生成する第１進路生成ステップと、
   生成された前記移動体の前記進路が、予測された前記障害物の前記進路との関係で、所定の安全条件を満たすか否か、を評価する評価ステップと、
   前記評価ステップにおいて評価された前記進路に前記所定の安全条件を満たすものがない場合に、前記所定の本数より多くの本数の前記進路を生成し、当該進路が前記評価ステップにて評価されるよう制御する第２進路生成ステップと、
   を含み、
   前記評価ステップは、
   生成された前記移動体の前記進路と、予測された前記障害物の前記進路と、に基づいて衝突確率を計算し、前記衝突確率が所定の閾値以内である場合に前記所定の安全条件を満たすと評価すること、
   を特徴とする移動体進路取得方法。
2. 移動体および前記移動体から所定の範囲に存在する障害物の、位置および内部状態を少なくとも記憶する記憶手段と、制御手段と、を備えた移動体進路取得装置において実行される移動体進路取得方法であって、
   前記制御手段において実行される、
   前記記憶手段から読み出した前記障害物の位置および内部情報に基づいて、前記障害物がとりうる進路の確率的な予測を行う障害物進路予測ステップと、
   前記記憶手段から読み出した前記移動体の位置および内部状態に基づいて、所定の物理条件において前記移動体がとりうる進路を生成する第１進路生成ステップと、
   生成された前記移動体の前記進路が、予測された前記障害物の前記進路との関係で、所定の安全条件を満たすか否か、を評価する評価ステップと、
   前記評価ステップにおいて評価された前記進路に前記所定の安全条件を満たすものがない場合に、物理限界値に近づけることにより前記所定の物理条件より緩やかな物理条件において前記移動体のとりうる前記進路を生成し、当該進路が前記評価ステップにて評価されるよう制御する第２進路生成ステップと、
   を含み、
   前記評価ステップは、
   生成された前記移動体の前記進路と、予測された前記障害物の前記進路と、に基づいて衝突確率を計算し、前記衝突確率が所定の閾値以内である場合に前記所定の安全条件を満たすと評価すること、
   を特徴とする移動体進路取得方法。
3. 請求項２に記載の移動体進路取得方法において、
   前記物理条件は、一の前記進路をとった場合に前記移動体に発生する加速度についての条件であること、
   を特徴とする移動体進路取得方法。
4. 請求項１乃至３のいずれか一つに記載の移動体進路取得方法において、
   前記制御部において実行される、
   前記第２進路生成ステップにおいて生成され前記評価ステップにおいて評価された前記進路に前記所定の安全条件を満たすものがない場合に、目的値関数に基づいて目的地までの距離を計算し、前記目的地に近い前記進路を選択する距離関数計算ステップ、
   を更に含むことを特徴とする移動体進路取得方法。
5. 請求項１乃至４のいずれか一つに記載の移動体進路取得方法において、
   前記障害物進路予測ステップは、
   前記障害物が複数存在する場合に、予測した複数の前記障害物がとりうる前記進路の中で異なる前記障害物同士が互いに干渉し合う進路を求め、当該干渉し合う進路のうち前記確率的な予測を行った前記進路の予測確率を低下させる進路干渉評価ステップと、
   前記進路干渉評価ステップで前記予測確率が低下した前記進路を含む前記複数の進路の各々が実現される確率を算出する確率算出ステップと、
   を含むことを特徴とする移動体進路取得方法。
6. 移動体および前記移動体から所定の範囲に存在する障害物の、位置および内部状態を少なくとも記憶する記憶手段と、制御手段と、を備えた移動体進路取得装置において、
   前記制御手段は、
   前記記憶手段から読み出した前記障害物の位置および内部情報に基づいて、前記障害物がとりうる進路の確率的な予測を行う障害物進路予測手段と、
   前記記憶手段から読み出した前記移動体の位置および内部状態に基づいて、前記移動体がとりうる進路を所定の本数生成する第１進路生成手段と、
   生成された前記移動体の前記進路が、予測された前記障害物の前記進路との関係で、所定の安全条件を満たすか否か、を評価する評価手段と、
   前記評価手段よって評価された前記進路に前記所定の安全条件を満たすものがない場合に、前記所定の本数より多くの本数の前記進路を生成し、当該進路が前記評価手段によって評価されるよう制御する第２進路生成手段と、
   を備え、
   前記評価手段は、
   生成された前記移動体の前記進路と、予測された前記障害物の前記進路と、に基づいて衝突確率を計算し、前記衝突確率が所定の閾値以内である場合に前記所定の安全条件を満たすと評価すること、
   を特徴とする移動体進路取得装置。
7. 移動体および前記移動体から所定の範囲に存在する障害物の、位置および内部状態を少なくとも記憶する記憶手段と、制御手段と、を備えた移動体進路取得装置において、
   前記制御手段は、
   前記記憶手段から読み出した前記障害物の位置および内部情報に基づいて、前記障害物がとりうる進路の確率的な予測を行う障害物進路予測手段と、
   前記記憶手段から読み出した前記移動体の位置および内部状態に基づいて、所定の物理条件において前記移動体がとりうる進路を生成する第１進路生成手段と、
   生成された前記移動体の前記進路が、予測された前記障害物の前記進路との関係で、所定の安全条件を満たすか否か、を評価する評価手段と、
   前記評価手段によって評価された前記進路に前記所定の安全条件を満たすものがない場合に、物理限界値に近づけることにより前記所定の物理条件より緩やかな物理条件において前記移動体のとりうる前記進路を生成し、当該進路が前記評価手段によって評価されるよう制御する第２進路生成手段と、
   を備え、
   前記評価手段は、
   生成された前記移動体の前記進路と、予測された前記障害物の前記進路と、に基づいて衝突確率を計算し、前記衝突確率が所定の閾値以内である場合に前記所定の安全条件を満たすと評価すること、
   を特徴とする移動体進路取得装置。

Images