JP4062353B1 - 障害物進路予測方法、装置、およびプログラム - Google Patents

Abstract

【課題】複雑な交通環境下でも障害物の進路を適切に予測することができる障害物進路予測方法、装置、およびプログラムを提供する。
【解決手段】障害物の位置および内部状態に基づいてその障害物がとりうる進路を予測し、この予測の際に少なくとも一つの障害物に対して複数の進路の確率的な予測を行い、障害物が複数存在する場合には、予測した複数の障害物がとりうる進路の中で異なる障害物同士が互いに干渉し合う進路を求め、この干渉し合う進路のうち確率的な予測を行った進路の予測確率を低下させ、この予測確率が低下した進路を含む複数の進路の各々が実現される確率を算出する。
【選択図】 図１

Description

本発明は、所定の範囲に存在する障害物がとりうる進路を予測する障害物進路予測方法、装置、およびプログラムに関する。

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

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

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

しかしながら、上述した従来技術では、自車に対して一つの障害物の進路のみを予測しているため、自車の周囲に複数の障害物が存在するような複雑な交通環境下で適用することができなかった。

本発明は、上記に鑑みてなされたものであって、複雑な交通環境下でも障害物の進路を適切に予測することができる障害物進路予測方法、装置、およびプログラムを提供することを目的とする。

上述した課題を解決し、目的を達成するために、本発明の一態様は、移動体から所定の範囲に存在する障害物の位置および内部状態を少なくとも記憶する記憶手段を備えたコンピュータが、前記障害物の進路を予測する障害物進路予測方法であって、前記記憶手段から読み出した前記障害物の位置および内部状態に基づいて前記障害物がとりうる進路を予測し、この予測を行う際に少なくとも一つの前記障害物に対して該障害物がとりうる複数の進路の確率的な予測を行う進路予測ステップと、前記障害物が複数存在する場合、前記進路予測ステップで予測した複数の前記障害物がとりうる進路の中で異なる前記障害物同士が互いに干渉し合う進路を求め、この干渉し合う進路のうち前記確率的な予測を行った進路の予測確率を低下させる進路干渉評価ステップと、前記進路干渉評価ステップで前記予測確率が低下した進路を含む前記複数の進路の各々が実現される確率を算出する確率算出ステップと、を有することを特徴とする。

また、上記発明において、所定の条件を満たす一つの前記障害物を特定障害物として選択する特定障害物選択ステップをさらに有し、前記進路予測ステップは、前記特定障害物がとりうる複数の進路の確率的な予測を行う特定障害物進路予測ステップと、前記特定障害物以外の前記障害物がとりうる進路の予測を行う一般障害物進路予測ステップと、を含み、前記進路干渉評価ステップは、前記障害物が複数存在する場合、前記特定障害物がとりうる複数の進路のうち、前記一般障害物がとりうる進路との同時間における距離が所定値よりも小さい進路の予測確率を低下させ、前記確率算出ステップは、前記進路干渉評価ステップで前記予測確率が低下した進路を含む前記特定障害物の複数の進路の各々が実現される確率を算出することを特徴とする。

また、上記発明において、前記特定障害物進路予測ステップは、前記障害物の位置および内部状態に基づいて、前記特定障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成する軌跡生成ステップと、前記軌跡生成ステップで生成した軌跡を用いることによって前記特定障害物がとりうる進路の確率的な予測演算を行う予測演算ステップと、を含むことを特徴とする。

また、上記発明において、前記一般障害物進路予測ステップは、前記一般障害物の内部状態が保持されるものとして前記一般障害物の進路を予測することを特徴とする。

また、上記発明において、前記移動体の位置および内部状態に基づいて前記移動体の進路を生成する進路生成ステップと、前記進路生成ステップで生成した前記移動体の進路と、前記確率算出ステップで進路ごとに確率が算出された前記特定障害物がとりうる複数の進路の各々との衝突確率を算出する衝突確率算出ステップと、をさらに有することを特徴とする。

また、上記発明において、前記進路予測ステップは、前記障害物がとりうる複数の進路の確率的な予測を行い、前記進路干渉評価ステップは、前記障害物が複数存在する場合、複数の前記障害物がとりうる進路のうち、異なる前記障害物の進路との同時間における距離が所定値よりも小さい進路をとる確率を低下させ、前記確率算出ステップは、前記進路干渉評価ステップで前記予測確率が低下した進路を含む複数の前記障害物の全ての進路の各々が実現される確率を算出することを特徴とする。

また、上記発明において、前記進路予測ステップは、前記障害物の位置および内部状態に基づいて、前記障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成する軌跡生成ステップと、前記軌跡生成ステップで生成した軌跡を用いることによって前記障害物の進路の確率的な予測演算を行う予測演算ステップと、を含むことを特徴とする。

また、上記発明において、前記移動体の位置および内部状態に基づいて前記移動体の進路を生成する進路生成ステップと、前記進路生成ステップで生成した前記移動体の進路と、前記確率算出ステップで進路ごとに確率が算出された前記障害物がとりうる全ての進路の各々との衝突確率を算出する衝突確率算出ステップと、をさらに有することを特徴とする。

本発明の別な態様は、移動体の周囲に存在する障害物の進路を予測する障害物進路予測装置であって、前記移動体から所定の範囲に存在する障害物の位置および内部状態を少なくとも記憶する記憶手段と、前記記憶手段から読み出した前記障害物の位置および内部状態に基づいて前記障害物がとりうる進路を予測し、この予測を行う際に少なくとも一つの前記障害物に対して該障害物がとりうる複数の進路の確率的な予測を行う進路予測手段と、前記障害物が複数存在する場合、前記進路予測手段で予測した複数の前記障害物がとりうる進路の中で異なる前記障害物同士が互いに干渉し合う進路を求め、この干渉し合う進路のうち前記確率的な予測を行った進路の予測確率を低下させる進路干渉評価手段と、前記進路干渉評価手段によって前記予測確率が低下した進路を含む前記複数の進路の各々が実現される確率を算出する確率算出手段と、を備えたことを特徴とする。

また、上記発明において、所定の条件を満たす一つの前記障害物を特定障害物として選択する特定障害物選択手段をさらに備え、前記進路予測手段は、前記特定障害物がとりうる複数の進路の確率的な予測を行う特定障害物進路予測手段と、前記特定障害物以外の前記障害物がとりうる進路の予測を行う一般障害物進路予測手段と、を有し、前記進路干渉評価手段は、前記障害物が複数存在する場合、前記特定障害物がとりうる複数の進路のうち、前記一般障害物がとりうる進路との同時間における距離が所定値よりも小さい進路の予測確率を低下させ、前記確率算出手段は、前記進路干渉評価手段によって前記予測確率が低下した進路を含む前記特定障害物の複数の進路の各々が実現される確率を算出することを特徴とする。

また、上記発明において、前記特定障害物進路予測手段は、前記障害物の位置および内部状態に基づいて、前記特定障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成する軌跡生成手段と、前記軌跡生成手段で生成した軌跡を用いることによって前記特定障害物がとりうる進路の確率的な予測演算を行う予測演算手段と、を有することを特徴とする。

また、上記発明において、前記一般障害物進路予測手段は、前記一般障害物の内部状態が保持されるものとして前記一般障害物の進路を予測することを特徴とする。

また、上記発明において、前記移動体の位置および内部状態に基づいて前記移動体の進路を生成する進路生成手段と、前記進路生成手段で生成した前記移動体の進路と、前記確率算出手段で進路ごとに確率が算出された前記特定障害物がとりうる複数の進路の各々との衝突確率を算出する衝突確率算出手段と、をさらに備えたことを特徴とする。

また、上記発明において、前記進路予測手段は、前記障害物がとりうる複数の進路の確率的な予測を行い、前記進路干渉評価手段は、前記障害物が複数存在する場合、複数の前記障害物がとりうる進路のうち、異なる前記障害物の進路との同時間における距離が所定値よりも小さい進路をとる確率を低下させ、前記確率算出手段は、前記進路干渉評価手段によって前記予測確率が低下した進路を含む複数の前記障害物の全ての進路の各々が実現される確率を算出することを特徴とする。

また、上記発明において、前記進路予測手段は、前記障害物の位置および内部状態に基づいて、前記障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成する軌跡生成手段と、前記軌跡生成手段で生成した軌跡を用いることによって前記障害物の進路の確率的な予測演算を行う予測演算手段と、を有することを特徴とする。

また、上記発明において、前記移動体の位置および内部状態に基づいて前記移動体の進路を生成する進路生成手段と、前記進路生成手段で生成した前記移動体の進路と、前記確率算出手段で進路ごとに確率が算出された複数の前記障害物がとりうる全ての進路の各々との衝突確率を算出する衝突確率算出手段と、をさらに有することを特徴とする。

本発明の別な態様に係る障害物進路予測プログラムは、上記発明に係る障害物進路予測方法を前記コンピュータに実行させることを特徴とする。

本発明によれば、障害物の位置および内部状態に基づいてその障害物がとりうる進路を予測し、この予測の際に少なくとも一つの障害物に対して複数の進路の確率的な予測を行い、障害物が複数存在する場合には、予測した複数の障害物がとりうる進路の中で異なる障害物同士が互いに干渉し合う進路を求め、この干渉し合う進路のうち確率的な予測を行った進路の予測確率を低下させ、この予測確率が低下した進路を含む複数の進路の各々が実現される確率を算出することにより、個々の障害物単位で見た場合には高い確率でとりうる進路であっても他の障害物と干渉することがあればその影響を考慮した予測を行うことができる。したがって、複雑な交通環境下でも障害物の進路を適切に予測することが可能となる。

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

（実施の形態１）
図１は、本発明の実施の形態１に係る障害物進路予測装置の機能構成を示すブロック図である。同図に示す障害物進路予測装置１は、移動体である四輪自動車等の車両に搭載され、自車の周囲に存在する障害物がとりうる進路を予測する装置である。

障害物進路予測装置１は、所定の範囲に存在する物体の位置や内部状態を検知するセンサ部２と、センサ部２が検知した結果に基づいて所定の範囲に含まれる障害物を抽出する障害物抽出部３と、障害物抽出部３で抽出した障害物の中から所定の条件を満たす一つの障害物（特定障害物）を選択する特定障害物選択部４と、特定障害物選択部４で選択した特定障害物の進路の確率的な予測を行う特定障害物進路予測部５と、特定障害物以外の一般障害物の進路を予測する一般障害物進路予測部６と、特定障害物の予測進路と一般障害物の予測進路との干渉を評価する進路干渉評価部７と、進路干渉評価部７での評価結果を用いて特定障害物がとりうる進路が実現される確率を算出する確率算出手段である規格化部８と、規格化部８で算出された特定障害物の進路に関する情報を出力する出力部９と、センサ部２で検知した物体の位置および内部状態や、各種演算結果を含む情報を記憶する記憶部１０と、を備える。

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

出力部９は、進路干渉評価部７で行われた処理結果に対応した情報に基づいて画像を生成する画像生成部９１と、画像生成部９１で生成された画像を含む情報を表示出力するために液晶、プラズマ、または有機ＥＬ（Electroluminescence）等のディスプレイを用いて実現される表示部９２とを有する。また、表示部９２として、運転席の後方上部にプロジェクタが設置されており、このプロジェクタによって自動車のフロントガラスへの重畳表示を行うことができる。

記憶部１０は、センサ部２での検知結果に加えて、特定障害物進路予測部５および一般障害物進路予測部６における予測結果、進路干渉評価部７における干渉評価結果などを記憶する。記憶部１０は、所定のＯＳ（Operation System）を起動するプログラムや本実施の形態１に係る障害物進路予測プログラム等が予め記憶されたＲＯＭ、および各処理の演算パラメータやデータ等を記憶するＲＡＭ（Random Access Memory）を用いて実現される。また、記憶部１０は、障害物進路予測装置１に対してコンピュータ読み取り可能な記録媒体を搭載可能なインタフェースを設け、このインタフェースに対応する記録媒体を搭載することによって実現することもできる。

以上の機能構成を有する障害物進路予測装置１は、演算および制御機能を有するＣＰＵ（Central Processing Unit）を備えた電子的な装置（コンピュータ）である。障害物進路予測装置１が備えるＣＰＵは、記憶部１０が記憶、格納する情報および上述した障害物進路予測プログラムを含む各種プログラムを記憶部１０から読み出すことによって本実施の形態１に係る障害物進路予測方法に関する演算処理を実行する。

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

次に、本実施の形態１に係る障害物進路予測方法について説明する。図２は、本実施の形態１に係る障害物進路予測方法の処理の概要を示すフローチャートである。以下の説明においては、予測対象の物体は全て２次元平面上を移動するものと仮定して説明を行うが、本実施の形態１に係る障害物進路予測方法は、３次元空間を移動する物体に対しても適用可能である。また、一つの物体が複数の自由度を有する場合（例えば６自由度を有するロボットアームのような物体）にも適用することができる。

まず、センサ部２が、所定の範囲にある物体の自車に対する位置および内部状態を検知し、検知した情報を記憶部１０に格納する（ステップＳ１）。以後、物体の位置は物体の中心の値であるとし、物体の内部状態は速度（速さｖ，向きθ）によって特定されるものとする。

次に、障害物抽出部３が、センサ部２で検知した結果に基づいて所定の範囲内にある障害物を抽出する（ステップＳ２）。このステップＳ２では、ステップＳ１で検知された物体のうち、自車の走行の妨げとなる障害物と見なすことができる物体が抽出され、それ以外の物体は除外される。図３は、速さｖ₀で直進する自車Ｃ₀に対して２台の車両Ｃ₁およびＣ₂が障害物として抽出された場合の自車Ｃ₀周辺の状況を示す図である。同図においては、３つの走行レーンを有する道路Ｒの左レーンＬ₁を自車Ｃ₀が走行しており、その右側に隣接する中央レーンＬ₂に２台の先行車両Ｃ₁およびＣ₂が走行している場合を表している。また、一番右側の右レーンＬ₃には、少なくともセンサ部２が検知可能な領域内に物体が存在していない。図３では、３台の車両は全て直進しており、中央レーンＬ₂では、後方の車両Ｃ₁の速さｖ₁が前方の車両Ｃ₂の速さｖ₂よりも大きいとする。

続いて、特定障害物選択部４は、障害物抽出部３で抽出した障害物が複数存在する場合、その中から一つの特定障害物を選択する（ステップＳ３）。特定障害物を選択する際の選択基準は予め設定されており、自車Ｃ₀からの距離が一番近い物体、自車Ｃ₀の周囲で速さが一番速い物体または速さが一番遅い物体のうちいずれかの条件を選択基準とすることができる。例えば、図３において自車Ｃ₀から一番近い障害物を特定障害物とする場合、車両Ｃ₁が特定障害物となる。

この後、特定障害物進路予測部５は、ステップＳ３で選択した特定障害物がとりうる複数の進路を確率的に予測する（ステップＳ４）。このステップＳ４では、従来知られているさまざまな手法を適用することができる。例えば、現況に応じて特定障害物のとりうる複数の進路に所定の確率分布を付与することによって進路予測を行ってもよい。また、特定障害物の種類に応じたモデルを予め記憶部１０に記憶させておいてもよい。モデルを使用する場合には、該当するモデルを記憶部１０から読み出し、この読み出したモデルを用いて確率的な進路予測を行う。

図４は、ステップＳ４における特定障害物の進路に対して付与する確率分布の例を示す図である。具体的には、特定障害物に対して直進方向を最大とする確率分布曲線ρ₁が付与された場合を図示している。この意味で、図４におけるｘ座標は、道路Ｒの幅方向の座標であり、その原点は特定障害物の現在位置を表している。なお、特定障害物に対して付与する確率分布は、例えば正規分布などに代表されるような単峰性を有していれば好ましいが、その分布関数まで限定されるものではない。

これに対して、一般障害物進路予測部６は、一般障害物の現在位置からの進路を予測する（ステップＳ５）。この際、一般障害物は、センサ部２において検知し、記憶部１０で記憶している内部状態を保持したまま移動するものとし、１つの一般障害物に対して１本の進路を予測する。このステップＳ５は、上述したステップＳ４の特定障害物進路予測処理と並列に行われる。

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

図５は、図３に示す道路環境下において、特定障害物である車両Ｃ₁の進路確率分布が図４に示す確率分布曲線ρ₁で与えられている場合、一般障害物である車両Ｃ₂に対して車両Ｃ₁が所定時間内に衝突する線を除去することによって得られる曲線を示す図である。同図に示す曲線ρ₁'は、確率の総和が１になっていないため、このままでは正確な意味での確率分布を与えているわけではない。そこで、規格化部８は、ステップＳ６で進路干渉評価部７によって除去されずに残った車両Ｃ₁の進路予測確率を求める（ステップＳ７）。すなわち、規格化部８は、ステップＳ６において除去されずに残った特定障害物の進路の確率の総和が１となるように規格化を行い、全ての予測進路に対して正しく定義された確率を付与する。

図６は、ステップＳ７において分布ρ₁'を規格化することによって得られた確率分布曲線ρ₂を示す図である。この図６からも明らかなように、図３に示す道路環境下において、車両Ｃ₁は先行する車両Ｃ₂が存在するために、直進の追い上げが危険と判定され、直進する可能性はないと判定される。これに対して、上記特許文献１に記載の従来技術では、図３に示す道路環境下であっても複数の障害物に対する予測を行うことはできない。この従来技術において、仮に車両Ｃ₁のみの進路を予測したとすると、直進が最も確率的に高いと判断してしまう。しかしながらこの進路は、車両Ｃ₂と衝突する確率が極めて高いため、非現実的な進路であることはいうまでもない。この例からも明らかなように、本実施の形態１に係る障害物進路予測方法によれば、複雑な交通環境下における進路予測を従来技術よりも適切に行うことができる。

次に、出力部９は、ステップＳ７で求めた特定障害物の予測進路確率に基づいて所定の情報を出力する（ステップＳ８）。例えば、特定障害物が所定値を超える確率で進路を取る可能性がある領域を表示するのは好ましい。図７は、表示部９２における予測結果の表示出力例を示す図であり、図３に示す道路環境における特定障害物としての車両Ｃ₁に対する進路予測結果の表示出力例を模式的に示す図である。図７では、車両Ｃ₁の予測進路のうち確率が所定値以上である進路を示す領域Ｄを自車Ｃ₀のフロントガラスＦに半透明に重畳表示した場合を図示している。

上述した重畳表示は、画像生成部９１で生成された画像を、自車Ｃ₀の運転席の後方上部に設置されたプロジェクタ（図示せず）からフロントガラスＦへ投影することによって実現される。これにより、自車Ｃ₀の運転者は、運転しながら近い将来に危険が生じる可能性のある領域を即座に認識することができる。したがって、その認識結果を運転に反映させることによって適確に危険を回避することが可能となる。

なお、障害物抽出部３で抽出された障害物が一つしかない場合には、この障害物を特定障害物とみなし、特定障害物進路予測部５における予測結果を出力部９で出力してもよい。

本実施の形態１に係る障害物進路予測方法は、所定の時間間隔で繰り返し行われ、常に最新の道路環境に即した情報が出力される。このため、本実施の形態１に係る障害物進路予測方法によれば、自車の運転者が刻々と変化する道路環境に対応して適確な操作を行うのを支援することが可能となる。

以上説明した本発明の実施の形態１によれば、障害物の位置および内部状態に基づいてその障害物がとりうる進路を予測し、この予測の際に所定の条件によって選択された特定障害物の複数の進路の確率的な予測を行うとともに、それ以外の一般障害物の進路の予測を行い、障害物が複数存在する場合には、特定障害物がとりうる複数の進路のうち、一般障害物がとりうる進路との同時間における距離が所定値よりも小さい進路の予測確率を低下させ、この予測確率が低下した進路を含む特定障害物の複数の進路の各々が実現される確率を算出することにより、個々の障害物単位で見た場合には高い確率でとりうる進路であっても他の障害物と干渉することがあればその影響を考慮した予測を行うことができる。したがって、複雑な交通環境下でも障害物の進路を適切に予測することが可能となる。

また、本実施の形態１によれば、車両にとって最も障害となりやすい障害物を特定障害物として選択し、この特定障害物の複数の進路について確率的な予測を行う一方、他の一般障害物の進路については現状を保持した進路をとるとしているため、処理に必要な計算量を抑え、装置への負荷が少ない実用的な障害物の進路予測処理を実現することができる。

さらに、本実施の形態１によれば、特定障害物に対して予測した結果を出力することによって危険度を含む情報を提示することができる。このため、情報の提示を受けた自車の運転者は、運転中の近い将来に起こりうる危険を迅速かつ適確に回避しながら運転することが可能となる。

本実施の形態１では、一般障害物は現状の状態を保持しているとし、一般障害物進路予測部６は１本の進路を生成していたが、この一般障害物も所定の確率で複数の進路をとりうるとして進路予測を行ってもよい。その場合の確率としては、単峰性を有し、特定障害物よりも空間的な広がりが少ない確率分布を採用することが好ましい。図８は、特定障害物の確率分布を図４に示すρ₁とした場合に、一般障害物に対して付与する確率分布の例を示す図である。図８に示す確率分布曲線ρ_Gは、特定障害物の確率分布曲線ρ₁よりも分散が小さく、直進方向かその近傍への進路を選択する可能性が極めて高いような分布を示している。このような確率分布曲線ρ_Gを用いる場合、図６に示す確率分布曲線ρ₂に相当する確率分布として、次式（１）で定義される確率密度関数ρ₃（ｘ）を適用してもよい。

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

また、例えば自車Ｃ₀に搭載されたカーナビゲーションシステムの表示画面ＣＮ（図７を参照）に表示部９２の機能を具備させることによって特定障害物の進路予測結果を表示するようにしてもよい。図９は、この場合に特定障害物である車両Ｃ₁が所定値以上の確率で走行することが予測される領域Ｄを図示した画面である。

なお、以上の説明では、自車と同じ方向を向いて走行している先行車を障害物として説明してきたが、例えば自車と同じ方向を向いて走行している後続車を障害物とすることもできる。また、自車と反対方向を向いて走行している対向車を障害物とすることもできる。さらには、静止している物体を障害物とすることもできる。

（実施の形態２）
本発明の実施の形態２は、上記実施の形態１と同様に特定される特定障害物が時間の経過とともにとりうる位置の変化を、時間および空間から構成される時空間上の軌跡として生成し、この生成した軌跡を用いて進路予測を行うことを特徴とする。

図１０は、本実施の形態２に係る障害物進路予測装置の機能構成を示すブロック図である。同図に示す障害物進路予測装置１１において、特定障害物進路予測部１２以外の構成は上記実施の形態１に係る障害物進路予測装置１と同様である。このため、障害物進路予測装置１と同じ機能を有する部位についてはそれぞれ同じ符号を付してある。

特定障害物進路予測部１２は、特定障害物選択部４で選択された特定障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成する軌跡生成部１３と、軌跡生成部１３から出力される特定障害物の軌跡を用いることによって特定障害物の進路の確率的な予測演算を行う予測演算部１４と、を有する。

軌跡生成部１３は、所定の時間が経過するまでに特定障害物がとりうる軌跡を予測生成するものであり、特定障害物をシミュレーション上で仮想的に動作させるための操作を複数の操作から選択する操作選択部１３１と、操作選択部１３１で選択した操作を所定の時間行う物体操作部１３２と、物体操作部１３２で操作した後の特定障害物の位置および内部状態が所定の条件を満たしているか否かを判定する判定部１３３とを有する。

次に、本発明の実施の形態２に係る障害物進路予測方法について説明する。本実施の形態２に係る障害物進路予測方法は、特定障害物の進路の予測処理を除いて、上記実施の形態１に係る障害物進路予測方法と同じである（図２のフローチャートを参照）。そこで、以下の説明においては、特定障害物の進路予測処理（図２のステップＳ４に対応）について詳細に説明する。

図１１は、特定障害物の進路予測処理の概要を示すフローチャートである。まず、軌跡生成部１３が特定障害物の複数の軌跡を生成する（ステップＳ４１）。図１２は、軌跡生成部１３における軌跡生成処理の詳細を示すフローチャートである。図１２においては、センサ部２で検知した特定障害物Ｏ_Sに対して軌跡を生成する演算をＮ回行うものとする（Ｎは自然数）。また、軌跡を生成する時間（軌跡生成時間）をＴ（＞０）とする。この軌跡生成時間Ｔ（および後述する操作時間Δｔ）を適切に定めることにより、実用的な計算時間で一連の進路予測処理を行うことが可能となる。

軌跡生成部１３は、最初に特定障害物Ｏ_Sに対する軌跡生成回数を示すカウンタｎの値を１とする初期化を行う（ステップＳ４０１）。

次に、軌跡生成部１３は、センサ部２で検知した結果を記憶部１０から読み出し、この読み出した検知結果を初期状態とする（ステップＳ４０２）。具体的には、時間ｔを０とし、特定障害物Ｏ_Sの初期位置（ｘ（０）,ｙ（０））および初期内部状態（ｖ（０）,θ（０））を、それぞれセンサ部２からの入力情報（ｘ₀,ｙ₀）および（ｖ₀,θ₀）とする。

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

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

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

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

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

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

ステップＳ４０６の判定の結果、特定障害物Ｏ_Sが移動可能領域内に位置している場合（ステップＳ４０６でＹｅｓ）、軌跡生成部１３は時間をΔｔだけ進め（ｔ←ｔ＋Δｔ）、ステップＳ４０４の動作後の位置を（ｘ（ｔ）,ｙ（ｔ））、内部状態を（ｖ（ｔ）,θ（ｔ））とする（ステップＳ４０７）。

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

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

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

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

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

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

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

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

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

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

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

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

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

となる。

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

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

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

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

図１５は、特定障害物Ｏ_Sの軌跡集合｛Ｐ_S（ｎ）｝と一般障害物の予測進路とを加えることによって形成された時空間環境の構成例を模式的に示す図である。同図に示す時空間環境Ｅｎｖ（Ｐ_S,Ｐ_G）は、特定障害物Ｏ_Sの軌跡集合｛Ｐ_S（ｎ）｝（実線で表示）および一般障害物Ｏ_Gの１本の軌跡Ｐ_G（破線で表示）からなる。

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

この後の処理は、上記実施の形態１と同様に行われる。すなわち、特定障害物Ｏ_Sと一般障害物Ｏ_Gとの干渉評価処理として、特定障害物Ｏ_Sの軌跡集合｛Ｐ_S（ｎ）｝から一般障害物Ｏ_Gの軌跡Ｐ_Gと交差する軌跡すなわち特定障害物Ｏ_Sと一般障害物Ｏ_Gが衝突する軌跡を除去する（ステップＳ６）。この際の衝突は、上記実施の形態１と同様に定められ、２つの軌跡が単に交点を有する場合に加えて、障害物の種類に応じた所定の距離よりも小さい距離まで接近した場合も含まれる。

その後、規格化部８は、除去されずに残った軌跡集合（非干渉軌跡集合）｛Ｐ_S'（ｎ）｝の各要素がとる確率の総和が１となるように規格化して確率を算出する（ステップＳ７）。続いて、出力部９は、ステップＳ７で得られた確率分布に基づいた情報の出力を行う（ステップＳ８）。

以上説明した本発明の実施の形態２によれば、障害物の位置および内部状態に基づいてその障害物がとりうる進路を予測し、この予測の際に所定の条件によって選択された特定障害物の複数の進路の確率的な予測を行うとともに、それ以外の一般障害物の進路の予測を行い、障害物が複数存在する場合には、特定障害物がとりうる複数の進路のうち、一般障害物がとりうる進路との同時間における距離が所定値よりも小さい進路の予測確率を低下させ、この予測確率が低下した進路を含む特定障害物の複数の進路の各々が実現される確率を算出することにより、個々の障害物単位で見た場合には高い確率でとりうる進路であっても他の障害物と干渉することがあればその影響を考慮した予測を行うことができる。したがって、上記実施の形態１と同様に、複雑な交通環境下でも障害物の進路を適切に予測することが可能となる。

また、本実施の形態２によれば、特定障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成し、この生成した軌跡を用いることによって特定障害物の進路の確率的な予測を行うことにより、動的物体の進路予測を精度よく行うことができるようになる。

なお、本実施の形態２において、特定障害物の時空間上での軌跡生成処理を行う際には、選択可能な全ての操作を動作させることによって軌跡を生成してもよい。このような軌跡生成処理を実現するアルゴリズムは、例えば縦型探索または横型探索による再帰呼出を適用することによって実現することが可能である。この場合、特定障害物Ｏ_Sの軌跡集合｛Ｐ_S（ｎ）｝の要素数すなわち軌跡の本数は、特定障害物Ｏ_Sに対する軌跡生成処理が終了するまで分からない。したがって、実行可能な操作を全探索することによって各物体がとりうる軌跡を生成する場合には、操作時間Δｔにおける操作ｕ_c（ｔ）の要素の数（操作ｕ_c（ｔ）が連続量の場合には離散化の度合い）に応じて最適な計算量を有する探索方法を選択すればよい。

また、本実施の形態２は、高低差のある道路を走行中の自動車に適用する場合のように、４次元時空間（空間３次元、時間１次元）においても適用可能である。

（実施の形態３）
本発明の実施の形態３は、上述した２つの実施の形態とは異なり、全ての障害物を対等に扱い、各障害物が時間の経過とともにとりうる位置の変化を時空間上の軌跡として生成し、この生成した軌跡を用いて障害物の進路予測を行うことを特徴とする。

図１６は、本発明の実施の形態３に係る障害物進路予測装置の機能構成を示すブロック図である。同図に示す障害物進路予測装置２１は、センサ部２と、障害物抽出部３と、障害物抽出部３で抽出された障害物の進路の確率予測を行う障害物進路予測部２２と、障害物進路予測部２２で予測された障害物間の予測進路の干渉を評価する進路干渉評価部７と、規格化部８と、出力部９と、記憶部１０と、を備える。このうち出力部９は、画像生成部９１および表示部９２を有する。

障害物進路予測部２２は、障害物抽出部３で抽出された障害物が時間の経過とともにとりうる位置の変化を時空間上での軌跡として生成する軌跡生成部２３と、軌跡生成部２３から出力される複数の障害物の軌跡を用いることによって各障害物の進路の確率的な予測演算を行う予測演算部２４と、を有する。このうち軌跡生成部２３は、所定の時間が経過するまでに物体がとりうる軌跡を予測生成するものであり、上記実施の形態２で説明した軌跡生成部１３と同様に、操作選択部２３１と、物体操作部２３２と、判定部２３３とを有する。

障害物進路予測部２２は、複数の障害物を対等に扱って障害物全体の進路予測を行う。また、進路干渉評価部７では、予測した進路の中で干渉する進路、すなわち同時間で所定の距離よりも小さい距離に近づく軌跡同士を全て除去することによって干渉評価を行う。

次に、図１７に示すフローチャートを参照して、本実施の形態３に係る障害物進路予測方法を説明する。まず、センサ部２において、所定の範囲にある物体の自車に対する位置および内部状態を検知し、検知した情報を記憶部１０に格納する（ステップＳ１１）。本実施の形態３においても、物体の位置は物体の中心の値であるとし、物体の内部状態は速度（速さｖ，向きθ）によって特定されるものとする。

この後、障害物抽出部３が、センサ部２で検知した結果に基づいて所定の範囲内にある障害物を抽出する（ステップＳ１２）。

続いて、障害物進路予測部２２は、障害物抽出部３で抽出した複数の障害物がとりうる進路を確率的に予測する（ステップＳ１３）。このステップＳ１３で個々の障害物に対して行われる具体的な進路予測処理は、上記実施の形態２における特定障害物の進路予測処理（図１２を参照）と同様である。以後の説明では、センサ部２で検知した障害物の総数をＩとし、一つの障害物Ｏ_i（１≦ｉ≦Ｉ,ｉは自然数）に対して軌跡を生成する演算をＮ_i回行うものとする（ＩおよびＮ_iはともに自然数）。

軌跡生成部２３は、まずセンサ部２で検知した結果を記憶部１０から読み出し、この読み出した検知結果を初期状態とする。その後、操作選択部２３１は、初期状態に対してその後の時間Δｔの間に行う操作ｕ_i（ｔ）を選択する。この際には操作選択部２３１が、選択可能な複数の操作の中から、各操作に予め付与された操作選択確率にしたがって一つの操作を選択する。なお、具体的な操作ｕ_icは、上記実施の形態２における操作ｕ_cと同様に設定される。また、操作ｕ_icを選択する操作選択確率ｐ（ｕ_ic）も、上述した操作選択確率ｐ（ｕ_c）と同様に定められる。

この後、物体操作部２３２は、操作選択部２３１が選択した操作ｕ_icを時間Δｔの間動作させ、判定部２３３では、操作ｕ_icを動作させた後の物体Ｏ_iの内部状態が所定の制御条件を満たしているか否かを判定するとともに、操作ｕ_icを動作させた後の物体Ｏ_iの位置が移動可能領域内にあるか否かを判定する。軌跡生成部２３は、判定部２３３における判定の結果、操作ｕ_ic終了後の物体Ｏ_iの位置および内部状態が全ての条件を満足している場合のみ、時間をΔｔだけ進め（ｔ←ｔ＋Δｔ）、動作後の位置を（ｘ_i（ｔ）,ｙ_i（ｔ））、内部状態を（ｖ_i（ｔ）,θ_i（ｔ））とする。この処理をＮ_i回繰り返すことによって一つの障害物Ｏ_iに対する処理を終了する。その後、他の障害物Ｏ_iに対しても同様の処理を行う。

軌跡生成部２３が全ての障害物に対して軌跡生成処理を行うことにより、３次元時空間の所定の範囲内に存在する複数の物体がとりうる軌跡の集合からなる時空間環境が形成される。図１８は、時空間環境の構成例を模式的に示す図である。同図に示す時空間環境Ｅｎｖ（Ｐ₁,Ｐ₂）は、障害物Ｏ₁の軌跡集合｛Ｐ₁（ｎ）｝（図１８では実線で表示）および障害物Ｏ₂の軌跡集合｛Ｐ₂（ｎ）｝（図１８では破線で表示）からなる。より具体的には、時空間環境Ｅｎｖ（Ｐ₁,Ｐ₂）は、二つの障害物Ｏ₁およびＯ₂が、高速道路のような平坦かつ直線状の道路Ｒを＋ｙ軸方向に向かって移動している場合の時空間環境を示すものである。本実施の形態３においては、物体同士の相関は考慮せず物体ごとに軌跡生成を行っているため、異なる物体の軌跡同士が時空間上で交差することもある。

図１８において、時空間の各領域における軌跡集合｛Ｐ_i（ｎ）｝（ｉ＝１，２）の単位体積当たりの密度は、その時空間の各領域における障害物Ｏ_iの時空間確率密度を与えている。したがって、本実施の形態２においては、障害物Ｏ_iが３次元時空間上の所定の領域を通過する確率を求めることが可能となる。

次に、予測演算部２４は、各障害物がとりうる進路の確率的な予測を行う。この際の個々の障害物に対する予測演算は、上記実施の形態２におけるステップＳ４２と実質的に同じである。したがって、軌跡集合｛Ｐ_i（ｎ）｝が与えられたとき、この集合に属する一つの軌跡Ｐ_i（ｍ）が選ばれる確率ｐ（Ｐ_i（ｍ））は、

となる。ここで、全ての操作ｕ_im（ｔ）が等確率ｐ₀（ただし、０＜ｐ₀＜１）で選択される場合、１本の軌跡Ｐ_i（ｍ）が選ばれる確率ｐ（Ｐ_i（ｍ））は、

となり、確率ｐ（Ｐ_i（ｍ））は軌跡Ｐ_i（ｍ）に依存しない。なお、式（７）において、全ての物体に対して生成する軌跡の数が同じ（Ｎ本）であるとすると、Ｎ₁＝Ｎ₂＝・・・＝Ｎ_I＝Ｎ（定数）なので、ｐ（Ｐ_i（ｍ））＝１／Ｎとなり、障害物Ｏ_iによらず一定となる。したがってこの場合には、確率ｐ（Ｐ_i（ｍ））の値を１と規格化することによって予測演算部２４における予測演算を簡素化し、より迅速に所定の予測演算を実行することが可能となる。

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

以上説明したステップＳ１３の障害物進路予測処理の後、進路干渉評価部７が障害物同士の干渉評価を行う（ステップＳ１４）。この進路干渉評価を行う際には、全ての障害物に対する軌跡集合｛Ｐ₁（ｎ）｝,｛Ｐ₂（ｎ）｝,・・・,｛Ｐ_I（ｎ）｝の中で互いに交差する軌跡を除去していく。なお、本実施の形態３においても、軌跡同士の交差については、上記実施の形態２と同じ定義を採用する。

この後、除去されずに残った全ての障害物の進路を時空間環境下での全障害物の進路予測確率として算出する（ステップＳ１５）。この際、規格化部８では、除去されずに残った全ての進路の確率の総和が１となるような規格化を行う。

最後に、出力部９は、ステップＳ１５で算出した全障害物の予測進路確率に基づいて所定の情報を出力する（ステップＳ１６）。例えば、所定値を超えた確率で進路が取られる領域を表示するのは好ましい。この場合の表示部９２における表示方法は、上記実施の形態１で説明したフロントガラスへの重畳表示や、カーナビゲーションの表示画面ＣＮへの表示などでもよい。

なお、本実施の形態３に係る障害物進路予測方法は、上記実施の形態２と同じく、４次元時空間（空間３次元、時間１次元）にも適用可能である。

以上説明した本発明の実施の形態３によれば、障害物の位置および内部状態に基づいてその障害物がとりうる進路を予測し、この予測の際に障害物に対して複数の進路の確率的な予測を行い、障害物が複数存在する場合には、複数の障害物がとりうる進路のうち、異なる障害物の進路との同時間における距離が所定値よりも小さい進路をとる確率を低下させ、この予測確率が低下した進路を含む複数の障害物の全ての進路の各々が実現される確率を算出することにより、個々の障害物単位で見た場合には高い確率でとりうる進路であっても他の障害物と干渉することがあればその影響を考慮した予測を行うことができる。したがって、複雑な交通環境下でも障害物の進路を適切に予測することが可能となる。

また、本実施の形態３によれば、障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での複数の軌跡として生成し、この生成した複数の軌跡を用いることによって障害物全体でとりうる進路の確率的な予測を行うことにより、動的物体の進路予測を精度よく行うことができるようになる。

なお、本実施の形態３において障害物の進路を予測する際には、従来知られている確率的な手法のいずれかを適用することも可能である。

（実施の形態４）
本発明の実施の形態４は、特定障害物の進路を上記実施の形態１と同様に予測することに加えて自車の進路を生成し、特定障害物と自車との衝突確率を算出することを特徴とする。

図１９は、本実施の形態４に係る障害物進路予測装置の機能構成を示すブロック図である。同図に示す障害物進路予測装置３１は、所定の範囲に存在する物体の位置や内部状態を検知するとともに、自車の位置や内部状態を検知するセンサ部３２と、自車の現在位置からの進路を生成する自車進路生成部３３と、自車と特定障害物との衝突確率を計算する衝突確率計算部３４と、衝突確率計算部３４で算出された自車と特定障害物との衝突確率に関する情報を出力する出力部３５と、を備える。出力部３５は、衝突確率計算部３４で算出された衝突確率に基づいて画像を生成する画像生成部３５１と、画像生成部３５１で生成された画像を含む情報を表示出力する表示部３５２と、自車進路生成部３３で生成された自車進路と特定障害物と衝突確率が所定の閾値を超える場合に警告音（音声を含む）を発生する警告音発生部３５３とを有する。ここで説明した以外の障害物進路予測装置３１の構成は、上記実施の形態１に係る障害物進路予測装置１の構成と同じである（図１を参照）。

図２０は、本実施の形態４に係る障害物進路予測方法の処理の概要を示すフローチャートである。まず、センサ部３２において、所定の範囲にある物体の自車Ｃ₀に対する位置および内部状態と自車の位置および内部状態を検知し、検知した情報を記憶部１０に格納する（ステップＳ２１）。

次に、障害物抽出部３が、センサ部３２で検知した結果に基づいて所定の範囲内にある障害物を抽出する（ステップＳ２２）。

続いて、特定障害物選択部４は、障害物抽出部３で抽出した障害物の中から一つの特定障害物を選択する（ステップＳ２３）。本実施の形態４では、センサ部３２が自車Ｃ₀の内部状態も検知可能であるため、特定障害物として、自車Ｃ₀が現状のままで走行したときに障害物と衝突するまでの時間ＴＴＣ（Time To Collision）が最短の障害物を選択するようにしてもよい。

この後、自車Ｃ₀以外の特定障害物Ｏ_Sや一般障害物Ｏ_Gに対して行うステップＳ２４〜Ｓ２７の処理は、上記実施の形態１で説明したステップＳ４〜Ｓ７の処理と同じである。

一方、自車進路生成部３３は、センサ部２における検知した自車情報を用いて自車の進路を生成する（ステップＳ２８）。具体的には、自車進路生成部３３は、自車が現状のまま走行した場合の軌跡を生成する。なお、センサ部３２が白線等の路面環境を検知することが可能であれば、走行可能なレーンの数に応じた軌跡を生成するようにしてもよい。このステップＳ２８は、ステップＳ２２〜Ｓ２７の処理と並行して行われる。

この後、衝突確率計算部３４は、自車進路生成部３３で生成した自車の進路と進路干渉評価部７によって得られた特定障害物の進路とが衝突する確率を計算する（ステップＳ２９）。このステップＳ２９では、自車の進路と特定障害物の進路とが同時間で所定の距離よりも近くなった場合に衝突したとみなし、衝突確率の計算を行う。この際に衝突したとみなされる距離は、特定障害物の種類に応じて定められる。

出力部３５は、ステップＳ２９で求められた衝突確率に基づいて所定の情報を出力する（ステップＳ３０）。例えば、衝突確率が所定の閾値を超えた場合に表示部３５２によって表示を行うとともに、衝突確率が所定の閾値を超えた場合に警告音発生部３５３が警告音を発生する。なお、自車進路生成部３３が複数の進路を生成する場合には、衝突確率が最も低い進路（またはレーン）を推奨進路（または推奨レーン）として表示したり音声で報知したりしてもよい。

以上説明した本発明の実施の形態４によれば、障害物の位置および内部状態に基づいてその障害物がとりうる進路を予測し、この予測の際に所定の条件によって選択された特定障害物の複数の進路の確率的な予測を行うとともに、それ以外の一般障害物の進路の予測を行い、障害物が複数存在する場合には、特定障害物がとりうる複数の進路のうち、一般障害物がとりうる進路との同時間における距離が所定値よりも小さい進路の予測確率を低下させ、この予測確率が低下した進路を含む特定障害物の複数の進路の各々が実現される確率を算出することにより、個々の障害物単位で見た場合には高い確率でとりうる進路であっても他の障害物と干渉することがあればその影響を考慮した予測を行うことができる。したがって、複雑な交通環境下でも障害物の進路を適切に予測することが可能となる。

また、本実施の形態４によれば、自車進路と特定障害物の予測進路との衝突確率を算出することにより、複雑な交通環境下における自車進路の安全性を実用的な時間の内で適確に判定することが可能となる。

なお、本実施の形態４の変形例として、上記実施の形態３に係る障害物進路予測装置に対して自車進路生成部および衝突確率計算部をさらに具備させることも可能である。この場合には、全ての障害物の進路に対する確率が算出され、自車進路との衝突確率が計算される。

（実施の形態５）
本発明の実施の形態５は、特定障害物の進路を、上記実施の形態２と同様に３次元時空間上で生成した軌跡を用いて行うことに加えて自車の進路を生成し、特定障害物と自車との衝突確率を求めることを特徴とする。

図２１は、本発明の実施の形態５に係る障害物進路予測装置の機能構成を示すブロック図である。同図に示す障害物進路予測装置４１は、所定の範囲に存在する物体の位置や内部状態を検知するとともに、自車の位置や内部状態を検知するセンサ部３２と、自車の現在位置からの進路を生成する自車進路生成部３３と、自車と特定障害物との衝突確率を計算する衝突確率計算部３４と、衝突確率計算部３４で算出された自車と特定障害物との衝突確率に関する情報を出力する出力部３５と、を備える。出力部３５は、衝突確率計算部３４で算出された衝突確率に基づいて画像を生成する画像生成部３５１と、画像生成部３５１で生成された画像を含む情報を表示出力する表示部３５２と、自車進路生成部３３で生成された自車進路と特定障害物と衝突確率が所定の閾値を超える場合に警告音（音声を含む）を発生する警告音発生部３５３とを有する。ここで説明した以外の障害物進路予測装置４１の構成は、上記実施の形態２に係る障害物進路予測装置１１の構成と同じである（図１０を参照）。

本実施の形態５に係る障害物進路予測方法は、特定障害物の進路予測処理および衝突確率算出処理の詳細を除いて、上記実施の形態４に係る障害物進路予測方法と同じである（図２０のフローチャートを参照）。また、特定障害物の進路予測処理は、上記実施の形態２に係る障害物進路予測方法と同じである（図１１および図１２を参照）。そこで、以下の説明においては、衝突確率算出処理（図２０のステップＳ２９に対応）について詳細に説明する。なお、以下の説明において、上記実施の形態４に係る障害物進路予測方法と同じ処理については、同じステップ番号を使用する。

図２２は、衝突確率算出処理の詳細を示すフローチャートである。同図に示す衝突確率算出処理は、２つのループ処理から構成されており、ステップＳ２８で生成した自車Ｃ₀の軌跡Ｐ₀とステップＳ２６で除去されずに残った非干渉軌跡集合｛Ｐ_S'（ｎ）｝との間の衝突確率を算出する。この際、衝突確率計算部３４は、自車Ｃ₀の軌跡Ｐ₀、特定障害物Ｏ_Sの非干渉軌跡集合｛Ｐ_S'（ｎ）｝、および自車Ｃ₀と特定障害物Ｏ_Sの衝突確率を評価する評価関数を用いて衝突確率の計算を行う。なお、本実施の形態５では、衝突確率計算部３４が評価関数を内蔵しているものとして説明を行うが、障害物進路予測装置４１に入力部を設けることにより、その評価関数を外部から入力する構成としてもよい。また、評価関数を道路の種類や自車Ｃ₀の速度によって適応的に変化させる構成としてもよい。

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

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

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

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

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

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

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

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

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

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

式（９）の右辺の和は、自車Ｃ₀の軌跡Ｐ₀が特定障害物Ｏ_Sがとりうる軌跡と衝突する衝突確率に他ならない。すなわち、式（９）によって自車Ｃ₀と特定障害物Ｏ_Sとが衝突する衝突確率が求められる。

この後、出力部３５は、自車Ｃ₀と特定障害物Ｏ_Sとの衝突確率である干渉度ｒ_Sが所定の閾値を超える場合に表示部３５２から情報を表示したり、警告音発生部３５３から警告音を発生したりする（ステップＳ３０）。

以上説明した本発明の実施の形態５によれば、障害物の位置および内部状態に基づいてその障害物がとりうる進路を予測し、この予測の際に所定の条件によって選択された特定障害物の複数の進路の確率的な予測を行うとともに、それ以外の一般障害物の進路の予測を行い、障害物が複数存在する場合には、特定障害物がとりうる複数の進路のうち、一般障害物がとりうる進路との同時間における距離が所定値よりも小さい進路の予測確率を低下させ、この予測確率が低下した進路を含む特定障害物の複数の進路の各々が実現される確率を算出することにより、個々の障害物単位で見た場合には高い確率でとりうる進路であっても他の障害物と干渉することがあればその影響を考慮した予測を行うことができる。したがって、複雑な交通環境下でも障害物の進路を適切に予測することが可能となる。

また、本実施の形態５によれば、特定障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成し、この生成した軌跡を用いることによって特定障害物の進路の確率的な予測を行うことにより、動的物体の進路予測を精度よく行うことができるようになる。

さらに、本実施の形態５によれば、時空間上で特定障害物がとりうる軌跡との自車がとりうる軌跡との干渉の程度を定量的に示す干渉度を算出し、この算出した干渉度を衝突確率として求めることにより、現実として起こりうる状況下における自車進路の安全性を、実用的な時間の内で適確に判定することが可能となる。

なお、本実施の形態５の変形例として、上記実施の形態３に係る障害物進路予測装置に対して自車進路生成部および衝突確率計算部をさらに具備させることも可能である。この場合には、上述したステップＳ５０１からステップＳ５１１の処理を、さらに複数の障害物に対して繰り返し行うことによって、時空間環境下における自車Ｃ₀と全障害物との衝突確率を算出すればよい。

（その他の実施の形態）
ここまで、本発明を実施するための最良の形態として、実施の形態１〜５を詳述してきたが、本発明はそれら５つの実施の形態によってのみ限定されるべきものではない。例えば、互いに干渉し合う進路をとる予測確率を０として除去する代わりに、互いに干渉し合う進路のとる予測確率を低下させるような構成としてもよい。

また、上記実施の形態１または２において説明した障害物進路予測方法を用いることにより、全ての障害物を対等に扱うことも可能である。この場合、特定障害物選択部では、障害物抽出部で抽出された障害物を任意の規則（例えば自車から近い順など）に基づいて順次選択していき、各特定障害物に対する進路予測処理をループで繰り返し行うようにすればよい。

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

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

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

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

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

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

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

本発明の実施の形態１に係る障害物進路予測装置の機能構成を示すブロック図である。 本発明の実施の形態１に係る障害物進路予測方法の処理の概要を示すフローチャートである。 障害物抽出後の自車の周辺の状況を示す図である。 特定障害物の進路に対して付与する確率分布の例を示す図である。 特定障害物が一般障害物に対して所定時間内に衝突する軌跡を除去することによって得られる曲線を示す図である。 図５の曲線を規格化した後の確率分布曲線を示す図である。 表示部における特定障害物の進路予測結果の表示出力例を示す図である。 一般障害物に対して付与する確率分布の例を示す図である。 表示部における特定障害物の進路予測結果の別な表示出力例を示す図である。 本発明の実施の形態２に係る障害物進路予測装置の機能構成を示すブロック図である。 本発明の実施の形態２に係る障害物進路予測方法における特定障害物の進路予測処理の概要を示すフローチャートである。 特定障害物の軌跡生成処理の詳細を示すフローチャートである。 特定障害物の軌跡を模式的に示す図である。 特定障害物に対して生成された軌跡集合を３次元時空間上で模式的に示す図である。 特定障害物の軌跡集合に一般障害物の予測進路を加えることによって形成された時空間環境の構成例を模式的に示す図である。 本発明の実施の形態３に係る障害物進路予測装置の機能構成を示すブロック図である。 本発明の実施の形態３に係る障害物進路予測方法の処理の概要を示すフローチャートである。 本発明の実施の形態３に係る障害物進路予測方法における軌跡生成処理によって得られた時空間環境の構成例を模式的に示す図である。 本発明の実施の形態４に係る障害物進路予測装置の機能構成を示すブロック図である。 本発明の実施の形態４に係る障害物進路予測方法の処理の概要を示すフローチャートである。 本発明の実施の形態５に係る障害物進路予測装置の機能構成を示すブロック図である。 本発明の実施の形態５に係る障害物進路予測方法における衝突確率算出処理の詳細を示すフローチャートである。 自車の軌跡と特定障害物の非干渉軌跡との３次元時空間上での関係を模式的に示す図である。

符号の説明

１，１１，２１，３１，４１ 障害物進路予測装置
２、３２ センサ部
３ 障害物抽出部
４ 特定障害物選択部
５、１２ 特定障害物進路予測部
６ 一般障害物進路予測部
７ 進路干渉評価部
８ 規格化部
９ 出力部
１０ 記憶部
１３、２３ 軌跡生成部
１４、２４ 予測演算部
２２ 障害物進路予測部
３３ 自車進路生成部
３４ 衝突確率計算部
３５ 出力部
９１、３５１ 画像生成部
９２、３５２ 表示部
１３１、２３１ 操作選択部
１３２、２３２ 物体操作部
１３３、２３３ 判定部
３５３ 警告音発生部

Claims (17)

1. 移動体から所定の範囲に存在する障害物の位置および内部状態を少なくとも記憶する記憶手段を備えたコンピュータが、前記障害物の進路を予測する障害物進路予測方法であって、
   前記記憶手段から読み出した前記障害物の位置および内部状態に基づいて前記障害物がとりうる進路を予測し、この予測を行う際に少なくとも一つの前記障害物に対して該障害物がとりうる複数の進路の確率的な予測を行う進路予測ステップと、
   前記障害物が複数存在する場合、前記進路予測ステップで予測した複数の前記障害物がとりうる進路の中で異なる前記障害物同士が互いに干渉し合う進路を求め、この干渉し合う進路のうち前記確率的な予測を行った進路の予測確率を低下させる進路干渉評価ステップと、
   前記進路干渉評価ステップで前記予測確率が低下した進路を含む前記複数の進路の各々が実現される確率を算出する確率算出ステップと、
   を有することを特徴とする障害物進路予測方法。
2. 所定の条件を満たす一つの前記障害物を特定障害物として選択する特定障害物選択ステップをさらに有し、
   前記進路予測ステップは、前記特定障害物がとりうる複数の進路の確率的な予測を行う特定障害物進路予測ステップと、前記特定障害物以外の前記障害物がとりうる進路の予測を行う一般障害物進路予測ステップと、を含み、
   前記進路干渉評価ステップは、前記障害物が複数存在する場合、前記特定障害物がとりうる複数の進路のうち、前記一般障害物がとりうる進路との同時間における距離が所定値よりも小さい進路の予測確率を低下させ、
   前記確率算出ステップは、前記進路干渉評価ステップで前記予測確率が低下した進路を含む前記特定障害物の複数の進路の各々が実現される確率を算出することを特徴とする請求項１記載の障害物進路予測方法。
3. 前記特定障害物進路予測ステップは、
   前記障害物の位置および内部状態に基づいて、前記特定障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成する軌跡生成ステップと、
   前記軌跡生成ステップで生成した軌跡を用いることによって前記特定障害物がとりうる進路の確率的な予測演算を行う予測演算ステップと、
   を含むことを特徴とする請求項２記載の障害物進路予測方法。
4. 前記一般障害物進路予測ステップは、
   前記一般障害物の内部状態が保持されるものとして前記一般障害物の進路を予測することを特徴とする請求項２または３記載の障害物進路予測方法。
5. 前記移動体の位置および内部状態に基づいて前記移動体の進路を生成する進路生成ステップと、
   前記進路生成ステップで生成した前記移動体の進路と、前記確率算出ステップで進路ごとに確率が算出された前記特定障害物がとりうる複数の進路の各々との衝突確率を算出する衝突確率算出ステップと、
   をさらに有することを特徴とする請求項２〜４のいずれか一項記載の障害物進路予測方法。
6. 前記進路予測ステップは、前記障害物がとりうる複数の進路の確率的な予測を行い、
   前記進路干渉評価ステップは、前記障害物が複数存在する場合、複数の前記障害物がとりうる進路のうち、異なる前記障害物の進路との同時間における距離が所定値よりも小さい進路をとる確率を低下させ、
   前記確率算出ステップは、前記進路干渉評価ステップで前記予測確率が低下した進路を含む複数の前記障害物の全ての進路の各々が実現される確率を算出することを特徴とする請求項１記載の障害物進路予測方法。
7. 前記進路予測ステップは、
   前記障害物の位置および内部状態に基づいて、前記障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成する軌跡生成ステップと、
   前記軌跡生成ステップで生成した軌跡を用いることによって前記障害物の進路の確率的な予測演算を行う予測演算ステップと、
   を含むことを特徴とする請求項６記載の障害物進路予測方法。
8. 前記移動体の位置および内部状態に基づいて前記移動体の進路を生成する進路生成ステップと、
   前記進路生成ステップで生成した前記移動体の進路と、前記確率算出ステップで進路ごとに確率が算出された前記障害物がとりうる全ての進路の各々との衝突確率を算出する衝突確率算出ステップと、
   をさらに有することを特徴とする請求項６または７記載の障害物進路予測方法。
9. 移動体の周囲に存在する障害物の進路を予測する障害物進路予測装置であって、
   前記移動体から所定の範囲に存在する障害物の位置および内部状態を少なくとも記憶する記憶手段と、
   前記記憶手段から読み出した前記障害物の位置および内部状態に基づいて前記障害物がとりうる進路を予測し、この予測を行う際に少なくとも一つの前記障害物に対して該障害物がとりうる複数の進路の確率的な予測を行う進路予測手段と、
   前記障害物が複数存在する場合、前記進路予測手段で予測した複数の前記障害物がとりうる進路の中で異なる前記障害物同士が互いに干渉し合う進路を求め、この干渉し合う進路のうち前記確率的な予測を行った進路の予測確率を低下させる進路干渉評価手段と、
   前記進路干渉評価手段によって前記予測確率が低下した進路を含む前記複数の進路の各々が実現される確率を算出する確率算出手段と、
   を備えたことを特徴とする障害物進路予測装置。
10. 所定の条件を満たす一つの前記障害物を特定障害物として選択する特定障害物選択手段をさらに備え、
    前記進路予測手段は、前記特定障害物がとりうる複数の進路の確率的な予測を行う特定障害物進路予測手段と、前記特定障害物以外の前記障害物がとりうる進路の予測を行う一般障害物進路予測手段と、を有し、
    前記進路干渉評価手段は、前記障害物が複数存在する場合、前記特定障害物がとりうる複数の進路のうち、前記一般障害物がとりうる進路との同時間における距離が所定値よりも小さい進路の予測確率を低下させ、
    前記確率算出手段は、前記進路干渉評価手段によって前記予測確率が低下した進路を含む前記特定障害物の複数の進路の各々が実現される確率を算出することを特徴とする請求項９記載の障害物進路予測装置。
11. 前記特定障害物進路予測手段は、
    前記障害物の位置および内部状態に基づいて、前記特定障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成する軌跡生成手段と、
    前記軌跡生成手段で生成した軌跡を用いることによって前記特定障害物がとりうる進路の確率的な予測演算を行う予測演算手段と、
    を有することを特徴とする請求項１０記載の障害物進路予測装置。
12. 前記一般障害物進路予測手段は、
    前記一般障害物の内部状態が保持されるものとして前記一般障害物の進路を予測することを特徴とする請求項１０または１１記載の障害物進路予測装置。
13. 前記移動体の位置および内部状態に基づいて前記移動体の進路を生成する進路生成手段と、
    前記進路生成手段で生成した前記移動体の進路と、前記確率算出手段で進路ごとに確率が算出された前記特定障害物がとりうる複数の進路の各々との衝突確率を算出する衝突確率算出手段と、
    をさらに備えたことを特徴とする請求項１０〜１２のいずれか一項記載の障害物進路予測装置。
14. 前記進路予測手段は、前記障害物がとりうる複数の進路の確率的な予測を行い、
    前記進路干渉評価手段は、前記障害物が複数存在する場合、複数の前記障害物がとりうる進路のうち、異なる前記障害物の進路との同時間における距離が所定値よりも小さい進路をとる確率を低下させ、
    前記確率算出手段は、前記進路干渉評価手段によって前記予測確率が低下した進路を含む複数の前記障害物の全ての進路の各々が実現される確率を算出することを特徴とする請求項９記載の障害物進路予測装置。
15. 前記進路予測手段は、
    前記障害物の位置および内部状態に基づいて、前記障害物が時間の経過とともにとりうる位置の変化を時間および空間から構成される時空間上での軌跡として生成する軌跡生成手段と、
    前記軌跡生成手段で生成した軌跡を用いることによって前記障害物の進路の確率的な予測演算を行う予測演算手段と、
    を有することを特徴とする請求項１４記載の障害物進路予測装置。
16. 前記移動体の位置および内部状態に基づいて前記移動体の進路を生成する進路生成手段と、
    前記進路生成手段で生成した前記移動体の進路と、前記確率算出手段で進路ごとに確率が算出された複数の前記障害物がとりうる全ての進路の各々との衝突確率を算出する衝突確率算出手段と、
    をさらに有することを特徴とする請求項１４または１５記載の障害物進路予測装置。
17. 請求項１〜８のいずれか一項記載の障害物進路予測方法を前記コンピュータに実行させることを特徴とする障害物進路予測プログラム。

Images