JP3638323B2 - 車両の障害物検出装置 - Google Patents

Description

【０００１】
【産業上の利用分野】
この発明は車両の障害物検出装置に関し、より具体的には走行時に障害となる前走車などの障害物の検出装置であって、走行環境におけるその障害物情報を精度良く検出できるようにしたものに関する。
【０００２】
【従来の技術】
車両や道路外の物体など複数の障害物が存在する場合、それらが道路（走行路）に対してどの位置にあるのかが分かれば、適切な警報動作や自車の制御が可能となる。しかしながら、カーブ路を通過するときなど、前走車など自車の進路上に存在する障害物と樹木など道路外に存在する障害物とが、明確に識別しがたい。そこで、特開平４−１９３６４１号公報記載の技術は、撮像信号の中から自車線または道路形状を示す信号を抽出して自車の進路を判別すると共に、撮像された範囲に存在する物体を測定し、測定された物体の中で自車の進路上に存在する物体を障害物と判断する技術を提案している。
【０００３】
【発明が解決しようとする課題】
このように、従来技術においては、進路の中か外かで障害物になるか否か判別している。これにより、樹木などの静止障害物は障害物ではないと判別することができるが、車両などは時々刻々と移動することから、この従来技術では自車にとって障害物となるか否か必ずしも明確に判別することができない。
【０００４】
即ち、レーダデータをそのまま使用しているので、必ずしも実際の障害物と一対一には対応しない。何故ならば、１つの障害物の異なる部位から別々にレーダ波の反射があり、複数の検出データが出力されることがしばしばあるからである。このため、上記した従来技術によるときは、実際の障害物の正確な位置を知ることができない。また、検出データは点の情報であるので、実際の障害物の大きさを掴むことができない。
【０００５】
従って、この発明の目的は従来技術の上記した欠点を解消することにあり、自車の走行にとって真に障害となる障害物を精度良く検出し、それによって警告、回避など必要な動作を的確に行うことができるようにした障害物の検出装置を提供することにある。
【０００６】
【課題を解決するための手段】
上記の目的を達成するために、この発明は請求項１項において、カーブ路を含む車両の走行路の境界位置を検出する走行路境界位置検出手段、送信した波動の反射波を通じて少なくとも物体の位置を検出し、検出点データとして出力する物体検出手段、前記検出点データの示す物体位置を前記走行路の境界位置と比較し、前記物体が走行路内に位置するか否かを判定する判定手段、前記検出点データが所定範囲内にあるか否かを判断し、前記所定範囲内にあると判断されるとき、前記検出点データを１つのクラスタにクラスタリングすると共に、前記判定手段の判定結果に基づき、前記検出点データが走行路内に位置する物体の検出点データと走行路外に位置する物体の検出点データとからなると判定されるとき、前記１つのクラスタをさらに別のクラスタとするクラスタリング手段、および前記走行路内に位置する物体の検出点データからなる前記クラスタを走行の障害となる障害物として検知する検知手段、からなる如く構成した。
【０００８】
上記の目的を達成するために、請求項２項の装置にあっては、前記所定範囲が、車両の投影面積に基づいて設定される値である如く構成した。
【０００９】
【作用】
請求項１項においては、物体が走行路内に位置するか否かを判定する判定手段、検出点データが所定範囲内にあるか否かを判断し、前記所定範囲内にあると判断されるとき、前記検出点データを１つのクラスタにクラスタリングすると共に、前記判定手段の判定結果に基づき、前記検出点データが走行路内に位置する物体の検出点データと走行路外に位置する物体の検出点データとからなると判定されるとき、前記１つのクラスタをさらに別のクラスタとするクラスタリング手段、および前記走行路内に位置する物体の検出点データからなる前記クラスタを走行の障害となる障害物として検知する検知手段、からなる如く構成したので、自車の走行にとって真に障害となる走行路内の障害物のみを精度良く検出することができる。それによって、警告、回避など必要な動作を的確に行うことができる。尚、ここで、走行路境界位置検出手段は具体的には、走行路画像のエッジ画像をハフ変換して直線成分を抽出して行う。また、物体検出手段は具体的にはマルチビームＦＭ−ＣＷレーダを意味する。さらに、前記した物体検出手段が例えばレーダであって送波ビームの照射広がり角、障害物までの距離によって同一障害物に対して複数の障害物があるかのように検出するときも、検出点データ群が所定範囲に対応した大きさの１つの障害物であると検知することができる。
【００１１】
請求項２項の装置にあっては、前記所定範囲が、車両の投影面積に基づいて設定される値である如く構成したので、請求項１項で述べた効果に加えて、複数の検出点データ群が同一の車両であると検知することができる。
【００１２】
【実施例】
以下、添付図面に即してこの発明の実施例を説明する。
【００１３】
図１はこの発明に係る車両の障害物検出装置を備えた車両を全体的に示す説明透視図である。図において、車両はＣＣＤカメラ（モノクロＴＶカメラ）１０を１基備える。ＣＣＤカメラ１０は運転席上方のルームミラー取り付け位置に固定され、車両進行方向を単眼視する。符号１２はミリ波レーダからなるレーダユニットを示し、車両前方に取り付けられた２基の前方レーダからなり、反射波を通じて他車などの移動体を含む立体障害物の存在を検出する。
【００１４】
レーダユニット１２は、より具体的にはＦＭ−ＣＷ型のレーダ装置からなり、図２に示す如く、複数本の電磁波ビームを送波し、その反射波を受信して立体障害物の存在を検出する。検出された障害物の位置座標（相対距離）は、ビームごとに発生する。尚、このレーダユニットの詳細は、本出願人が先に提案した特願平５−２７６３９６号に詳細に述べられているので、これ以上の説明は省略する。
【００１５】
図１に戻ると、車両室内の中央部付近にはヨーレートセンサ１４が設けられ、車両の重心を通る鉛直軸（ｚ軸）回りの車両回転角（ヨー角）の変化速度（ヨー角速度）を検出する。更に、車両のドライブシャフト（図示せず）の付近にはリードスイッチからなる車速センサ１６が設けられ、車両の進行速度を検出すると共に、舵角センサ１８が車両のステアリングシャフト２０の付近に設けられてステアリング舵角を検出する。ステアリングシャフト２０には舵角制御モータ２２が取り付けられると共に、スロットル弁（図示せず）にはパルスモータからなるスロットルアクチュエータ２４が取り付けられ、更にブレーキ（図示せず）にはブレーキ圧力アクチュエータ２６（図１で図示省略）が取り付けられる。この構成において、車両は算出された舵角制御量に応じて舵角制御されると共に、スロットル弁が開閉されて車速が調節され、また必要に応じてブレーキが作動させられて走行する。
【００１６】
図３は上記の構成をより詳細に示すブロック図で、図４は図３の構成をより機能的に示す説明図である。
【００１７】
図３を中心に説明すると、ＣＣＤカメラ１０の出力は、画像処理ハードウェア３０に送られてエッジ検出とハフ（Ｈｏｕｇｈ）変換により直線成分の抽出が行われ、その結果はバス３２を介して通信用メモリ３４にストアされる。画像処理ＣＰＵ３６は、直線成分から走行路（道路）区分線候補を抽出して通信用メモリ３４にストアする。画像評価ＣＰＵ３８は、所定時刻ごとにストア値を読み出して走行路（道路）区分線を決定する。レーダユニット１２の出力は、レーダ処理回路４０およびバス３２を介して通信用メモリ３４にストアされる。レーダ評価ＣＰＵ４２は、所定時刻ごとにストア値を読み出して障害物の位置を座標上で検出する。
【００１８】
また車速センサ１６などの出力は、軌跡推定ＣＰＵ４４に送られて自車両の移動軌跡が推定される。行動計画意思決定ＣＰＵ５０は、前記ストア値から目標経路を作成する。その目標経路と推定された自車の移動軌跡は、軌跡追従制御ＣＰＵ４６に送られ、そこで軌跡（目標経路）追従制御量が決定される。更に、軌跡追従制御ＣＰＵ４６は、舵角制御量を算出して舵角制御ＣＰＵ５２に出力する。舵角制御ＣＰＵ５２は、ＰＷＭコントローラ５４およびドライバ５６を介して前記舵角制御モータ２２を駆動する。尚、モータ駆動量はエンコーダ５８を通じて検出され、フィードバック制御が行われる。
【００１９】
また行動計画意思決定ＣＰＵ５０は、その速度・追従制御部で車体の目標加速度を求め、車速制御ＣＰＵ６０に送出する。車速制御ＣＰＵ６０は、アクセルパルスモータコントローラ６２、ドライバ６４を介してスロットルアクチュエータ２４を駆動すると共に、ブレーキソレノイドコントローラ６６およびドライバ６８を介してブレーキ圧力アクチュエータ２６を駆動する。その駆動量は圧力センサ７０を介して検出され、第２のフィードバック制御が行われる。
【００２０】
上記において、レーダ評価ＣＰＵ４２が主として、実施例に係る車両の障害物検出装置に相当する。
【００２１】
図５は実施例に係る車両の障害物検出装置の動作を示すメイン・フロー・チャートである。また、図６はその動作を説明する機能ブロック図である。
【００２２】
図５フロー・チャートを参照する前に、実施例に係る車両の障害物検出装置の動作を概説する。
【００２３】
実施例に係る装置において、ＦＭ−ＣＷ型のレーダユニット１２により時々刻々と検知された、前走車などの移動体を含む障害物の位置座標に自車の現在位置（座標軸とヨー角）を付与し、障害物の絶対座標を保持しながら、過去の系列から、次の検知時刻における位置をカルマンフィルタ（線形予測フィルタ）により予測し、同一のものかどうか判断する。従来の車間距離を計るレーダでは、ある時刻に検知されて次の時刻に検知されたものは同一であると言う前提を置いているが、その障害物が車両であるのか、どちらの方向に向かって進んでいるのか分からないため、警報などで誤ることがある。実施例に係る手法では、移動する障害物のおよその方向、速度、加速度を検出し、よって危険の予知と共に、的確な警報、自車の制御などを行うことを可能とした。
【００２４】
移動する車両などは通常の走行状態では、その軌跡は滑らかであり、速度も過度に急変することはないので、その性質を用いて検知した障害物の特性を表現することができる。実施例では後述の如く、それを「移動体らしさ」と表現する。それによって、障害物の位置しか検出できないレーダユニット１２を用いても、その動きを一意に表せ、自車の行動制御に利用できる。例えば、手前で一時的に検知された障害物が更に接近したところで再度検知されたとき、後述の如く、同一のものと見做すことができ、それが停止している可能性が高い、前述した表現で言えば移動体らしさが低いならば、その側方を通過できる余地があるときは減速して通過する、余地がないときはその前で停止するなどの制御を行うことができる。
【００２５】
この実施例においては後述の如く、時系列に発生した複数障害物の情報を効率良く管理し、新たに入ってきた情報とそれまで保持していた情報とを比較し、時間空間的に関連するものをノードとそれらを結ぶリンクとして表現する、いわゆるネットワーク型のデータ構造を用いる。それにより、複数障害物の情報の内容を更新する手続きが容易になる。実施例では現在検知されている１つの障害物に関するデータの一連を「セグメント」と呼ぶ。後述の如く、移動体らしさを評価しつつセグメントを更新することにより、複数の障害物に対しても移動情報を簡単に求めることができる。前記のネットワークセグメントの情報から、次に移動する位置が推定でき、入力データが一時的になくなっても、関連性の高いデータが再度検知されれば、そのデータはノードとしてセグメントにリンクされて再捕捉することができ、移動情報が途中で失われることがない。
【００２６】
以上を前提として、図５フロー・チャートを参照して実施例に係る装置の動作を説明する。尚、図５フロー・チャートに示すプログラムは、レーダ検知サイクルの１００ｍｓごとに起動される。
【００２７】
先ずＳ１０でセグメントの個数をカウントするカウンタｉの値を１にインクリメントし、Ｓ１２に進んで最後のセグメントｉｍａｘに達していないことを確認してＳ１４に進み、そこでデータ取込み・クラスタ化を行う。
【００２８】
実施例で使用するＦＭ−ＣＷ型レーダの特性上、入力データから障害物の幅を検知することができず、送波ビームの照射広がり角、障害物までの距離によっては同一障害物に対して複数の障害物があるかのように検知される。そこで、ある基準でまとめてクラスタとして扱う。その基準は、クラスタの幅と縦の長さをしきい値として、図７に示すように、普通乗用車のサイズ（ｚ軸上から見た投影面積）を用いる。クラスタの幅を障害物の幅とし、重心を中心とし、クラスタを後述するネットワークセグメントのノードとして扱う。その操作により、後の処理でのデータ量を抑えることができる。更に、どのビームに検知されているかを検知の度に意識する必要がないと言う利点もある。
【００２９】
車両や道路外の物体など複数の障害物が存在する場合、それらが道路に対してどの位置にあるのかが分かれば、適切な警報動作や自車の制御が可能となる。しかしながら、図８に示すようにカーブ路（曲線路）を通過するときなど、前走車など自車の進路上に存在する障害物と樹木など道路外に存在する障害物とが、明確に識別し難い。この場合、従来技術によるときは、実際の障害物の正確な位置を知ることができず、実際の障害物の大きさを掴むことができないことは前述の通りである。
【００３０】
そこで、この出願に係る実施例においては、画像処理によって検出された走行路（道路）区分線の情報とレーダによって検知された障害物の位置とを比較し、ある基準を用いて障害物としてデータを１つに扱う（クラスタ化する）ようにした。その場合に問題となるのが、走行路の外の物体（例えば、道路標識、ガードレール）に起因する検出データである。検出したい障害物（他の車両など）が、このような走行路外の物体の傍らに位置していると、障害物に起因するデータと一緒に走行路外の物体に起因する検出データまでもが同じクラスタに併合される。
【００３１】
実施例ではクラスタは検出データを距離の近いもの同士をまとめて大きさと中心位置を持つものと認識することとしたが、走行路外の物体まで同じクラスタに併合されると、障害物の大きさや中心位置を正しく求めることができない。そこで、図９に示す如く、画像処理で得た走行路区分線で分断されるか否か判断し、分断されるならば複数のデータを別々のものとして扱い、道路内と外に障害物があると判断した。それにより、図９左側に示すような１つの障害物と判断する不都合が生じない。
【００３２】
図１０はその作業を示すサブルーチン・フロー・チャートである。以下説明すると、Ｓ１００でカウンタｍ（クラスタ内の検知データ数カウンタ），ｎ（検知データ数カウンタ）を０に、ｊ（クラスタ数カウンタ）を１にリセットする。またフラグｆｌｇを０にリセットする。
【００３３】
続いてＳ１０２に進み、検知データ数カウンタｎをインクリメントし、Ｓ１０４に進んで当該ｎの検知データＤｎをその時点で注目している検知データＤとする。即ち、いま図１１の状態にあるとし、検知データＤ１，Ｄ２，Ｄ３，Ｄ４のうち、Ｄ１に注目する。続いてＳ１０６に進んで前記フラグｆｌｇが０であるか否か判断する。このフラグは新たにクラスタを新設するとき０にリセットされるものであり、先にＳ１００で０にリセットされていることから、ここでの判断は肯定され、Ｓ１０８に進んでＤが最終検知データか否か判断する。
【００３４】
図１１の状態にあると仮定すると、Ｓ１０８での判断は否定され、Ｓ１１０に進んで変数Ｌ，Ｐ，Ｇを検出データＤに書き替える（図１２）。ここで、Ｌはクラスタ内で最もｙ座標値の大きい検知データ、Ｐはその時点で最後にクラスタに併合した検知データ、Ｇはクラスタ内の検知データの座標値の総和を意味するが、ここではＤ１をとりあえず、ｙ座標値が最も大きく、最後にクラスタに併合したものであり、またそれを総和としておく。同時に、フラグｆｌｇの値を１にセットし、クラスタ内検知データ数カウンタｍを１に書き替える（即ち、最初のクラスタ内に検知データが１つあるとする）。
【００３５】
続いてＳ１０２に戻り、検知データ数カウンタ値ｎをインクリメントし、Ｓ１０４に進んでその時点で注目している検知データをＤ２とし、Ｓ１０６に進んでフラグの値が０か否か判断する。Ｓ１１０を経てきたことから今度は否定されてＳ１１２に進み、図１３に示すように、先の検知データ（Ｄ１）と現在の検知データ（Ｄ２）のｘ，ｙ方向の差（絶対値）Ｌｘ−Ｄｘ，Ｌｙ−Ｄｙが所定値（ここでは「Ｗｔｈ」（例えば５ｍ）と「Ｄｔｈ」（例えば２ｍ）とする）より共に小さいか否か判断する。所定値Ｗｔｈ，Ｄｔｈは、実際の乗用車の大きさ、即ち、ｚ軸から見た乗用車の投影面積にほぼ相当する。
【００３６】
Ｓ１１２で肯定されたとするとＳ１１４に進み、線分ＰＤ、即ち、Ｄ１（Ｐ）とＤ２（Ｄ）の間に引かれた線分と走行路区分線（画像処理を経て実線で示される）が交わるか否か判断する（図１４）。先に述べた如く、カーブ路を走行するときなど、道路内と外の物体が同時に検知され、それらが自車の走行に際して障害物になるか否か、そのままでは判断することはできない。そこで実施例においては、検知データが複数個あるとき、クラスタ化すると共に、検知データ間に走行路区分線が存在するときは、そこで別のクラスタとする。かく構成することによって、自車の走行にとって真に障害物となる物体を正確に判別することができる。
【００３７】
図１４に示す例では交わらないことから、ここでの判断は否定され、Ｓ１１６に進んで検知データＤ２をＰに書き替えてその時点で最後にクラスタ１（Ｓ１００で特定）に併合した検知データとし、Ｄ（Ｄ２）の座標値を加算して総和Ｇを更新し、Ｄ（Ｄ２）をクラスタ１内で最もｙ座標値の小さい検知データとする。同時にクラスタ１内の検知データ数カウンタ値ｍをインクリメントし、クラスタ１内に検知データが２個あるとする。
【００３８】
続いてＳ１１８に進んでＤ（Ｄ２）が最終検知データか否か判断する。ここで、最終か否かはｙ軸に関して判断されるが、Ｓ１１８の判断は図１４の例では否定され、Ｓ１０２に戻り、Ｓ１０４に進んで今度は検知データＤ３に注目し、Ｓ１１２を経てＳ１１４に進み、新たな検知データＤ３と最前の検知データＤ２の間に引かれた線分ＰＤと走行路区分線が交わるか否か判断する。図１５に示すように、この場合は肯定されてＳ１２０に進む。
【００３９】
尚、Ｓ１１２で否定されるときは、Ｓ１１４で線分と走行路区分線が交わるか否かを判断するまでもなく、Ｓ１２０に進む。Ｓ１２０以降はクラスタを別にするための処理であるが、検知データ間の距離が車両の大きさを超えるときは、別のクラスタとする。即ち、実施例で使用するレーダの特性から同一の車両に対して複数の検知データが生じる可能性があるため、Ｓ１１４を介して確認するものであるが、検知データ間の距離が車両の大きさを超えるときは、別の物体と判断するのが妥当と考えられるからである。
【００４０】
Ｓ１２０ではクラスタ内検知データ数カウンタ値ｍが１か否か判断する。即ち、新たなクラスタの検知データが１個であるか否か判断する。図示例の場合には先にＳ１１６で２にインクリメントされていることから否定され、Ｓ１２２に進んでクラスタ１内の検知データの座標値の総和Ｇをクラスタ内検知データ数カウンタ値ｍ（２）で除算してクラスタ１の重心位置Ｇｊ（ｊ＝１）を求める。同時に、Ｌ（Ｄ１），Ｒ（Ｄ２）間の距離の絶対値を求めてクラスタ１の幅Ｗｊとする。
【００４１】
また、ここまでで検知データＤ１，Ｄ２をクラスタ１とし、Ｄ３から別のクラスタとするためフラグの値を０にリセットし、クラスタ内検知データ数カウンタ値ｍをインクリメントする。続いてＳ１２４に進んでクラスタ数カウンタ値ｊをインクリメントする。尚、Ｓ１２０でクラスタ内の検知データが１と判断されるときはＳ１２６に進んでその検知データの位置Ｌを重心位置Ｇｊとすると共に、検知データが１個でクラスタとしての幅がないことから値Ｗｊは求めず、Ｓ１２８に進んでフラグの値を０にリセットする。
【００４２】
続いてＳ１０８に進み、最終検知データか否か判断する。図示ではＤ３であったことから否定され、Ｓ１１０に進んで図示の処理を行い、Ｓ１０２以降を同様に進み、Ｓ１１８でＤが最終検知データと判断されるまで以上のステップを繰り返す。そして、Ｓ１１８で最終検知データと判断されるときはＳ１３０に進んで検知データ数ｍが１か否か判断し、否定されるときはＳ１３２に、肯定されるときはＳ１３４に進んで先にＳ１２２，１２６で述べたと同様の処理を行って終わる。また、Ｓ１０８で肯定されるときは検知データが１個であったことから、Ｓ１３６に進んでその検知データを重心位置に書き替えて終わる。
【００４３】
図５フロー・チャートに戻ると、続いてＳ１６に進んで検知データがあるか否か判断する。肯定されるときはＳ１８に進んでネットワークが既に存在するか否か判断し、否定されるときはＳ２０に進んでネットワーク発生処理を行う。ここで、先に触れたネットワークについて詳説する。
【００４４】
レーダによって時々刻々検知されるデータは、それをつなげていけば、位置に関する時系列データとなる。実際の道路（走行路）上では、他の走行車両や停止している車両、ガードレールや標識など複数の物体が存在するために、同時に別のものが検知される。よって、これを切りわけ、各々のおよその移動速度、移動方向を求める必要がある。そこで、実施例の場合、同時に複数の障害物が扱えて、かつレーダユニットの検知誤差を考慮したデータ構造とし、それらの更新を新しい入力で過去までの累積情報にどれだけ依存しているか、即ち、関連性を持つかを移動予測による位置推定に基づいて行うこととした。
【００４５】
Ｓ２０の作業は、このネットワーク型のデータ構造を新設する作業を意味する。具体的には、図１６に示すように、時刻ｉにおける障害物情報をノードとする、移動体全体に関する時系列情報を格納したネットワークセグメントを作成する作業である。ノードには図１７に示す情報を持たせる。セグメントには図１８に示す情報を持たせる。以上のようなデータ構造によって、検知されたデータを内部に保持する。移動する物体が車両の場合、一般的な状況では車線変更を開始してから終了するまでに約２秒を要する。よって、車両の動きをとらえるのに十分なように約２秒分のデータが保持できるノードの数を設定する。また、およその速度と方向は、ネットワークを利用して、以下の計算により簡単に求められる。
速度＝（最新ノード位置−最古ノード位置）／（最新データ検知時刻−最古データ検知時刻）
方向＝最新ノード位置と最古ノード位置を通る直線の傾き
【００４６】
通常のレーダ、例えばドップラーレーダでは、対象物の相対速度が検知可能であるが、実施例で用いている、位置のみを検知するＦＭ−ＣＷ型のレーダでは、時間微分により速度を求めることになる。しかし、外乱の多い環境では、単純に位置座標を微分することはできない。そこで、実施例の場合、最古と最新のノード間の移動量と時刻からおおよその速度を求めた。これにより、ばらつきの影響はセグメントの始点と終点にのみ現れ、比較的安定した結果が得られる。また、方位についてもセグメント全体の向きを扱うので、ばらつきの影響は少ない。尚、ここで求めたおおよその速度は後述の如く、カルマンフィルタによる予測の初期値として用いる。
【００４７】
一般的な走行では０．２Ｇ程度の減速を考慮すれば良く、０．２Ｇで２秒間減速した場合、実施例では推定速度と実際の速度の差は、理論上７ｋｍ／ｈとなって、カルマンフィルタによる予測の初期値として適当である。図１９に推定速度、推定方位を示す。追従中の前走車の速度は自車の速度と比較して比較的安定している。但し、停止障害物について、レーダのばらつきの影響を受けている。これは、速度の計算が先のネットワークセグメントの始終点間距離に基づいているためである。
【００４８】
以上のネットワークセグメントを検知している障害物の数だけ作成し、コンピュータ内部に保持し、新たに検知されたデータに基づいてネットワークを更新する。
【００４９】
図５フロー・チャートの説明に戻ると、Ｓ１８でネットワークが既に存在すると判断されるときはＳ２２に進み、ネットワーク更新処理を行う。
【００５０】
これについて述べると、ネットワークの更新を単純更新と複数障害物存在時の更新に分類し、分類結果により処理を違えるようにした。
１）単純更新
新たに検知された障害物のデータがどのセグメントに属すかを推定位置との距離に基づいて判断する。即ち、その距離がレーダシステムの検知ばらつき（ｘ変位とｙ変位）より求めた範囲にあれば、セグメントに属し、なければ、新たな別の障害物として扱う。即ち、新しいセグメントを発生させる。更新の条件、処理方法については図２０に示す。
２）複数障害物存在時の更新
一般の道路において例えば、前走車、追い抜き車両、路肩に停止した車両、標識など様々な障害物が存在し、レーダにも検知される。実際の場面では、前走車の側方に他の車両が走行していたり、同一車線の前方に複数の車両が走行していて、そのうちの１台が隣の車線に移ったり、追い越した車両が追従している前走車の前あるいは後ろへ進入するなど種々の場合が考えられる。このような複雑な状況におけるネットワークの更新は、以下の３形態で行う。
ａ）新規発生
新たに別の障害物が検知されたとき、新しいセグメントを発生させる。
ｂ）分岐継承
同じような動きをしていた複数の障害物が別の方向へ向きを変えたとき、過去の経緯の情報（ノード情報、セグメント情報）を新しいセグメントに継承させる。
ｃ）合流消滅
障害物同士が近づき、どちらか一方しか検知されなくなったとき、移動体らしさと言う評価基準に基づいて一方を削除する。
それぞれの更新条件、処理方法を図２１に示す。
【００５１】
そして、複数障害物が存在するときの合流消滅時の更新において、一方を削除するために前記した移動体らしさと言う評価基準を設ける。これは、移動する過程、検知データの特性を考慮した評価基準である。その要素として、移動推移、検知時間、センサ検知範囲、障害物位置を図２２のように考え、移動体らしは図２３に示すように算出する。この表現によれば、停止物体（障害物）についても移動体と全く同じに扱うことができる。
【００５２】
図２４に自車が走行中に検知した障害物の移動体らしさの推移を示した。時刻４０（×５０ｍｓ）において検知されたデータは、前走車（実線）と停止障害物（破線）が同一に扱われたが、移動している前走車の移動体らしさが相対的に大きいため、停止障害物のセグメントは消滅した。
【００５３】
以上の手続きにより、ネットワークセグメントの情報を更新する。即ち、図１８の内容を計算する。ノードが所定の数（約２秒分）を超えたら、各値から最古のノードに関する移動量や曲率、これらの自乗和などを減じ、新たなノードの情報を加える。これにより、セグメントは、新しい入力の度に更新され、セグメントノードの一括計算を要しないので、複数の障害物があっても十分に短い処理時間で扱うことができる。
【００５４】
図２５はそのネットワークの更新作業を示すサブルーチン・フロー・チャートである。以下説明すると、Ｓ２００でノード接続テーブルＣｔｘ〔ｊ〕を初期化する。即ち、ノード接続テーブルＣｔｘ〔ｊ〕＝０とする（ｊ＝１〜検知データクラスタ数）。次いでＳ２０２に進んで接続判定用テーブルの配列を初期化、即ち、Ｍｔｘ〔ｉ〕〔ｊ〕＝０とする（ｉ＝１〜最大セグメント数）。
【００５５】
次いでＳ２０４に進んでセグメント数カウンタ値ｉをインクリメントし、Ｓ２０６に進んでセグメント〔ｉ〕のノード数ＮＮがしきい値Ｎｔｈ（例えば５（５００ｍｓ））を超えるか否か判断する。否定されるとき、即ち、データが５００ｍｓ分溜まっていないと判断されるときはＳ２０８に進んでフラグｆｌｇｎの値を０にリセットすると共に、５００ｍｓ分溜まっていると判断されるときはＳ２１０に進んでフラグｆｌｇｎの値を１にセットし、Ｓ２１２に進んで障害物位置予測を行う。尚、ここで５００ｍｓ分溜まっていないと位置予測を行わないのは、時系列データがこの程度蓄積されないと、予測データの信頼性が低く、当初の目的にそぐわないからである。また５００ｍｓは、レーダの検知サイクルで言えば、５回分に相当する。
【００５６】
図２６はその作業を示すサブルーチン・フロー・チャートであるが、同図を参照して説明する前に、その背景について説明する。
【００５７】
高速道路上の車両を想定すると、車両は急旋回せず、車線変更は二状態予測制御に基づくと言う前提条件に立てば、移動はほぼ直線的として扱うことができる。よってセグメントのノード位置を回帰直線で近似することが可能となる。即ち、車両は２秒の間この直線上を進んだと仮定する。更に、移動量は後述のようにカルマンフィルタにより予測し、次の検知時刻にその車両が移動している位置を先の直線上に求める。
【００５８】
実際は、セグメントの最新ノードを通り、セグメントの近似直線と直交する点から現在時刻（新たに検知された時刻）までに進む距離Ｌだけセグメントの近似直線上を進めた点Ｐを求める（図２７）。この位置を推定位置、この作業を移動予測と呼ぶ。この手法によると、現実に、運動モデルに直線移動、速度可変のものを適用したことと同等である。
【００５９】
実際に移動する車両と停止障害物における推定位置（実線）と入力位置（鎖線）をプロットしたものを図２８，２９に示す。移動する車両についてはほぼ一致し、停止障害物では約２ｍのずれ程度であり、移動障害物と停止障害物が近くにあると、区別できない場合が生じる。これは、後述の複数障害物存在時の処理で対処する。また、レーダは、障害物の形状や向きによっては、一時的に検知不能時間が発生する。即ち、短時間見失うことがある。この移動予測によれば、新たな時刻に基づく予測位置を出すので、短い時間ならば、障害物の再捕捉が可能である。
【００６０】
ところで、自動運転車の制御系では、画像処理、本処理、行動計画などがそれぞれ非同期に処理を行う場合、データの受渡しタイミングが異なる。即ち、データを受ける側の処理時間に依存してデータの遅れ時間が発生する。実施例では、障害物の検知時刻における速度、加速度、方位から運動モデル（物理モデル）に従い、カルマンフィルタ（前記した線形予測フィルタ）を用いて任意時間先の位置を計算し、データを受ける側の処理の時刻を基準とした障害物の情報が得られるようにした。これにより、処理時間の異なるシステム間で通信遅れ時間を意識しないで障害物の情報を扱うことができる。
【００６１】
但し、カルマンフィルタの処理結果により推定された速度、加速度はゆらぎがあり、その影響を受けるため、任意時間の設定は余り大きくできない。実施例では、レーダ検知時刻から１秒間を１００ｍｓごとに障害物の位置を算出している。図３０は前走車が定速から減速して停止する際の実際の前走車の速度（破線で示す）、本処理における推定速度、１秒間の予測位置における速度をプロットしたものである。減速時には、途中から推定速度が実測値を下回っているが、処理時間、通信遅れ時間、および制御時間の遅れを考慮して、この予測値を使うことにより、システム全体の遅れを減少させることが可能である。尚、予測時間と計算時間刻みを任意に変更しても良い。
【００６２】
処理結果は、図３に示す行動計画意思決定ＣＰＵ５０、画像処理ＣＰＵ３６、車速制御ＣＰＵ６０の各サブシステムに通信用メモリ３４経由で渡される。各サブシステムは、処理時間が異なるため、基本的に非同期である。このことは、レーダが障害物の位置を検知した時刻から、実施例の処理を行った後、実際に各サブシステムへデータが渡るまでに時間の遅れがあることを意味する。実施例では、障害物の現在の速度、加速度、移動方位に基づいて、任意の先の時刻を求めるが、その予測情報（後に「障害物移動予測テーブル」として示す）を渡すことにした。その結果、各サブシステムは、軌跡推定からデータを受けた時刻に基づいて、障害物移動予測テーブルの参照位置を決定し、データを使用することができる。これにより、通信時間の遅れを意識しないで、障害物の情報を扱うことができる。
【００６３】
その予測をカルマンフィルタを用いて行うので、次にカルマンフィルタについて説明する。
【００６４】
カルマンフィルタは、時系列なある信号ｘ（ｔ）が白色信号によってドライブされた線形ダイナミカルシステム（確率差分方程式）の出力として、信号の相関性がダイナミックスの構造に由来すると見做し、線形予測フィルタを構成する。これにより、信号や雑音の時系列を実時間で扱うことができると言う特徴がある。
【００６５】
宇宙開発におけ衛星の軌跡推定では、ベイズの重みつき最小２乗フィルタ（バッチフィルタ）が古くから使用されており、現在も使用されている。バッチフィルタは、データを一括処理することから、ある時点からある時点までの予測を一度で行う。一方、車載する障害物検知システムにおける障害物の移動推定は、検知してからの予測、認識、判断に要する時間が、短かくなければならない。通常のバッチフィルタでは遅れがあり、衛星のような遠距離にある移動物体の推定には十分使えるが、衝突の回避などを行うには、その遅れは致命的なものとなる。また、カルマンフィルタは、常に最新のデータを用いてリアルタイムに計算でき、専用ハードウェア化しやすいと言う利点がある。
【００６６】
実施例では、カルマンフィルタの構成は以下の通りとした。
【００６７】
離散時間系のプロセスは、数１のように表すことができる。
【００６８】
【数１】

【００６９】
ここで、ｘ（ｋ）は時間ｋにおける状態ベクトル、ｕ（ｋ）は入力、Ｇ（ｋ）は既知の行列、ｖ（ｋ）は平均値０、共分散行列Ｑ（ｋ）のガウシアンノイズである。
【００７０】
また、計測過程は、数２のように表すことができ、それに基づいてカルマンフィルタの計算ステップは数３から数８のように示すことができる。ここで、ｗ（ｋ）は平均値０、共分散行列Ｒ（ｋ）のガウシアンノイズである。初期値ｘ（０）は、平均値ｘ、共分散行列Ｐのガウシアンノイズとする。
【００７１】
【数２】

【００７２】
【数３】

【００７３】
【数４】

【００７４】
【数５】

【００７５】
【数６】

【００７６】
【数７】

【００７７】
【数８】

【００７８】
実施例では、図３１に示すように、状態ベクトルｘ（ｋ）の要素をそれぞれ、距離（計測原点（起点）からの移動距離）、速度、加速度とし、系を等加速度直線運動とする。従って、計測は障害物発見時からの距離とする。
【００７９】
ところで、カルマンフィルタの計算には初期値を与えなければならない。その初期値は、実際の値に近いほど、より真値に近づく。図３２は、真値５０ｋｍ／ｈに対して初期値に自車の速度１００ｋｍ／ｈを与えた場合である。収束までおよそ１．７秒を要している。実施例では先に述べた如く、およその速度を時系列データのネットワークから求め、カルマンフィルタの初期値として与えるようにした。その結果、図３３に示す如く、収束性を向上させることができた。
【００８０】
レーダが障害物を検知した時刻から、初めて速度推定値を出力するまでの時間を、どれぐらいに設定すべきかと言う問題がある。これは、実際のテストから約０．６秒（レーダのサンプリング周期で６回）とした。レーダの揺らぎによってこの時間は変動する。即ち、レーダが十分に精度が良ければ少ない時間で良く、逆に精度が悪い場合は、時間を長くとる必要がある。また、ネットワークによる推定錯誤を考慮すると、０．６秒間のネットワークによる速度の誤差は障害物の加速度に応じて１．８ｋｍ／ｈ（０．１Ｇ）から９．０ｋｍ／ｈ（０．５Ｇ）であり、カルマンフィルタへの初期値としては問題ない。
【００８１】
実施例では、カルマンフィルタの系のモデルに、等加速度直線運動の物理モデルを用いている。障害物を最初に発見してから（計測原点から）の移動距離に基づいて、レーダユニットの出力時間ごとにカルマンフィルタの計算を行っている。ところが、実際の車は、カーブなどを通過するため、直線運動はしない（図３４）。これは、車が長時間にわたってカーブを通過したり、トラックコースのようなスタート地点に戻るような場合（一般道路ではそのようなことはほとんどないが）、カルマンフィルタの計算が時間が経つにつれて、誤差が増えることになる。その誤差を増やさないために、計算スパンを設定し、ある時間ある個数のデータを処理したら、計測原点を最新検知位置付近に更新するようにした。尚、計算スパンは、誤差分散が収束する時間以上を設定すれば良いことが実験の結果判明した。その結果から、実施例では２秒とした。
【００８２】
図３５に原点更新しない場合とした場合の、カーブ通過中の前走車（９０ｋｍ／ｈの定速走行）の推定結果を示す。原点更新しない場合は時間の経過と共に、推定速度が低下している。原点更新した場合は、推定速度の低下が抑えられている。原点を更新せずにこのような問題に対応するには、非線形なモデルを用いたカルマンフィルタを用いれば良い。しかし、非線形なモデルを使用すると複数の障害物についての処理時間が長くなり、リアルタイムに予測するには適当ではない。原点を更新すれば、線形モデルを使用しても誤差の蓄積を防ぐことができる。
【００８３】
以上を前提として、障害物位置予測について、図２６フロー・チャートを参照して説明すると、Ｓ３００で上記したカルマンフィルタを用いて図示の如く予測値を計算する。ここで、ｘハット、ｙハットが、図６ブロック図のＰｏｓハットｎｅｘｔに相当するが、それについては最後に述べる。
【００８４】
図２５フロー・チャートに戻ると、続いてＳ２１４に進み、クラスタごとの接続ノード数テーブル値Ｓｔｘ〔ｉ〕を０にリセットし、Ｓ２１６に進んで検知クラスタカウンタ値ｊをインクリメントし、Ｓ２１８に進んでフラグｆｌｇｎの値が０であるか否か判断する。肯定されるとき、即ち、データが５００ｍｓ分溜まっていないと判断されるときはＳ２２０に進み、最短距離に基づいてノードリンクの判定処理を行う。
【００８５】
これは具体的には、セグメント終端と障害物との距離を求めて判定結果ｍを得る作業である。図３６の上部にその作業を示すが、ノードリンクの判定については図示の如く約束する。即ち、セグメント終端ノードと検知データとが同一位置にあるときを「停止」、５０ｃｍ以内のときを「単独」、５０ｃｍを超えてもレーダ検出誤差Ｘｅｒ内にあるときを「接続」、それ以外を「非接続」とする。
【００８６】
またＳ２１８で否定されるときは、ノード数が５００ｍｓを超えていることからＳ２２２に進み、障害物予測位置に基づいてセグメント終端との距離を同様に求めて行う。これは、予測位置と検知データの距離によって判定結果ｍを得る作業である。図３６の下部にその作業を示す。
【００８７】
ここで、距離ｄ＝０ならば「停止」、距離ｄ≦５０ｃｍならば「単独」とする。また距離ｄを予測位置との距離ｅｄと比較するとき、

とする。
【００８８】
次いでＳ２２４に進んで求めた判定結果ｍが" 非接続" か否か判断する。これは図３６に示すように、セグメント終端と検知データとが接続されるか否かを判断することである。
【００８９】
否定されるときはＳ２２６に進んで各テーブル値のクラスタ数Ｃｔｘ〔ｊ〕とセグメント数Ｓｔｘ〔ｉ〕とをインクリメントし、Ｓ２２８でクラスタｊが最大クラスタｊｍａｘに達したと判断され、Ｓ２３０でセグメントｉが最大セグメントｉｍａｘに達したと判断されるまで、上記の処理を繰り返す。
【００９０】
次いで図３７のＳ２３２に進んでクラスタ数カウンタ値ｊをインクリメントし、Ｓ２３４に進んでクラスタテーブル値Ｃｔｘ〔ｊ〕が１を超えるか、即ち、クラスタｊが複数のセグメントに接近するか否か判断する。肯定されるときはＳ２３６に進んでセグメント数カウンタ値ｉをインクリメントしてＳ２３８に進み、そこで接続判定用テーブル値Ｍｔｘ〔ｉ〕〔ｊ〕が接続、単独または停止か否か判断し、肯定、即ち、非接続状態にあればＳ２４０，Ｓ２４２に進んでクラスタ−セグメント間の距離が最小となる組み合わせを探す。
【００９１】
即ち、あるクラスタｊが複数のセグメントと接続可能な場合は、最短距離のものと接続すると見做す。尚、Ｓ２３４で否定されるときはＳ２４４に進み、そこでノード接続テーブル値Ｃｔｘ〔ｊ〕の値が０か否か判断し、肯定されるときはＳ２４６に進んでネットワークを新たに作成する。上記をＳ２４８で最大クラスタｊｍａｘに達したと判断されるまで繰り返す。次いでＳ２５０からＳ２６０の処理を行い、セグメントに接続しないときはセグメント終端から最も近いクラスタを選択し、そのセグメントに接続させる。
【００９２】
次いで図３８のＳ２６２からＳ２６８を進み、それまでに選択されなかったクラスタについて新たにネットワークを発生させる。即ち、図３９に示すような処理を行う。これは、先に図２１で示した「分岐継承」に相当する処理であり、セグメントの過去情報をコピーした上で新たなセグメントとして登録する。次いでＳ２７０からＳ２８０を経てＳ２８１に進み、接続ないし単独と判断されるときはＳ２８２に進んでネットワークリンク処理を行う。
【００９３】
図４０はその作業を示すサブルーチン・フロー・チャートである。以下説明すると、Ｓ４００でセグメント終端ｘ座標と自車位置のｘ座標との差と、検知クラスタｘ座標と自車位置ｘ座標との差とを求めて比較する。これは、検知クラスタが自車位置に対して対向しているか否か判断するためであり、肯定されるときは自車に接近していると判断してＳ４０２に進んでｍｏｖｅ（セグメント終端と検知クラスタとの距離を表す）の符号を逆転する。これは、図示しないルーチンで移動距離の変動を見るためである。
【００９４】
次いでＳ４０４に進んでセグメント終端と検知クラスタとの関係が単独であるか否か判断し、肯定されるとき、即ち、検知クラスタがほとんど動いていないとみられるときはＳ４０６に進んでセグメント〔ＳＮＯ〕マージ回数カウンタ値をインクリメントし、Ｓ４０８に進んでカウンタ値が１を超えたか否か判断し、肯定されるときはＳ４１０に進んで移動量総和よりセグメント終端位置を減じると共に、移動量２乗和よりセグメント終端位置の２乗値を減じる。これは、図４１に示すように、セグメント終端ノードを検知クラスタ側に付け替えるために、それまでのデータを更新する作業を意味する。
【００９５】
次いでＳ４１２に進んで同様にノード数カウンタ値が２を超えたか否か判断し、肯定されたときは曲率（図４１に示す）を計算し、Ｓ４１６に進んで曲率総和よりセグメント終端曲率を減じると共に、曲率２乗和よりノード終端曲率２乗値を減じる。これは、Ｓ４１０で説明した図４１に示す付け替え作業である。
【００９６】
次いでＳ４０４で否定、即ち、接続または停止とされた場合に備え、Ｓ４１８に進んでセグメントノード数カウンタ値をインクリメントし、Ｓ４２０，Ｓ４２２に進んで同様の処理を行う。次いで図４２フロー・チャートのＳ４２４に進んで障害物情報ノードメモリを確保する。次いでＳ４２６に進んでその確保ができなかったか否か判断し、肯定されるときはＳ４２８，Ｓ４３０，Ｓ４３２を進んで移動体らしさが最低のセグメントを削除してネットワークを圧縮する。また、Ｓ４２６で否定されるときはＳ４３４に進んで障害物クラスタ情報をノードに移送する。これは先に図１７に示した情報である。
【００９７】
次いでＳ４３６に進み、ノード数が所定保持時間分（例えば２秒分）あるか否か判断し、肯定されるときはＳ４３８に進んでセグメント先端の移動量と曲率を各総和より減じると共に、各２乗和も同様に２乗値を減じ、Ｓ４４０に進んでセグメント〔ＳＮＯ〕先端へのポインタ値を、セグメント先端の次のノードのポインタ値に書き替える。
【００９８】
次いでＳ４４４に進んで移動体らしさを計算し、Ｓ４４６に進んでセグメントノード長に基づく障害物平均速度計算を行い、Ｓ４４８に進んで障害物移動方位計算を行う。これは、先に述べたネットワークを利用しての最新、最古のノード位置などから速度、方向（方位）を計算することであり、うち速度値、より正確には平均速度値がカルマンフィルタに初期値として与えられる。最後にＳ４５０に進んでセグメント〔ＳＮＯ〕非接続回数カウンタ値を０にリセットする。
【００９９】
図３８フロー・チャートにおいては、次いでＳ２８３に進んで障害物の位置、速度、加速度を推定する。尚、Ｓ２８１で否定されたときはＳ２８４に進み、そこで停止と判断されたときはＳ２８５に進み、同一であることからセグメント併合処理を行い、Ｓ２８６に進んで同様に障害物位置、速度、加速度推定を行う。Ｓ２８７，Ｓ２８８で検知データクラスタ数、セグメント数がそれぞれ最大値に達したと判断されるまで、上記を繰り返す。
【０１００】
図４３ないし４４は、障害物位置、速度、加速度推定作業を示すサブルーチン・フロー・チャートである。
【０１０１】
以下説明すると、Ｓ５００で原点カウンタｒｃｎｔをインクリメントし、Ｓ５０２に進んでカウンタ値が所定値（例えば２０（２０００ｍｓ分））を超えるか否か判断し、肯定されたときＳ５０４に進んでカウンタ値ｒｃｎｔとフラグｐｒｃを０にリセットする。即ち、２秒経過するごとに、障害物移動距離の計測原点を移動（更新）する。
【０１０２】
続いてＳ５０６に進んでフラグｐｒｃが０であるか否か判断する。この場合は当然肯定されてＳ５０８に進み、フラグｐｒｃに１をセットし、Ｓ５１０に進んでセグメントノード数が１であるか否か判断する。初めてこの処理を行うときは肯定されてＳ５１２に進み、図示の如くセグメント終端ノードの障害物位置のｘ，ｙ座標を用いて原点（Ｏｘ，Ｏｙ）の移動（更新）を行う。またＳ５１０で否定されたときはＳ５１４に進み、セグメント終端より１つ前のノードの障害物位置情報を移動距離計測原点にする。
【０１０３】
続いて、Ｓ５１６に進み、前記したカルマンフィルタの値を設定し、Ｓ５１８に進んで前記距離ｄを移動距離ｘ〔０〕の初期値とする。続いてＳ５２０に進んで原点カウンタ値ｒｃｎｔが１か否か、即ち、原点更新後最初のカルマンフィルタ計算であるか否か判断し、肯定されるときはＳ５２２に進んでセグメント平均速度を速度ｘ〔１〕の初期値とする。セグメント平均速度とは、図４２フロー・チャートのＳ４４６で求めた平均速度を意味する。これは、前記した説明から明らかな如く、所定期間における平均速度である。
【０１０４】
即ち、実施例に係るカルマンフィルタにおいては、そのモデルに等加速度直線運動モデルを使用し、レーダユニット１２を通じて障害物を最初に発見した計測原点からの移動距離を求め、ネットワークから求めた速度を初期値として与えて計測時間ごと（１００ｍｓごと）にカルマンフィルタの計算を行って１００ｍｓ後、即ち、次のレーダ検知時刻での障害物の移動距離、速度、加速度を推定している。
【０１０５】
即ち、現在時刻の障害物の位置を推定し、初期値を適切に与えることにより、カルマンフィルタの収束性を向上させ、障害物情報をリアルタイムに得て図３の構成で他のサブシステムに提供することができる。更に、計測原点を２秒ごとに更新して誤差の蓄積を回避している。
【０１０６】
続いてＳ５２４に進んでＱを零クリアし、Ｓ５２６，Ｓ５２８に進んで共分散行列Ｑ，Ｒに図示の如き値をセットし、図４４フロー・チャートのＳ５３０に進んで運動方程式Ｆの転置行列を求めて値Ｆｔとし、Ｓ５３２に進んでカルマンフィルタにより、障害物の距離（位置）、速度（初速度）、加速度の推定を行う。ここで、Ｘが推定値である。
【０１０７】
尚、Ｓ５０６でフラグｐｒｃが０ではない、即ち、原点更新が行われないと判断されるときはＳ５３４に進んで距離ｄを図示の如く求める。これは、速度と加速度の初期値として前回推定した値をそのまま用いるので、改めて初期値を設定する必要がないからである。
【０１０８】
次いで図３８から図４５フロー・チャートに移り、そのＳ２８９からＳ２９１に進み、セグメント削除判定処理を行う。これは、Ｓｔｘ〔ｉ〕＝０、即ち、接続可能な障害物クラスタが１つもなかったセグメントｉに関して前記した移動体らしさを所定値と比較し、所定値未満のセグメントを削除する処理である。次いでＳ２９２からＳ２９４を進んでノード終端位置に基づいてセグメント併合処理を行う。これは、乗用車の平面と同一サイズ内に複数のセグメントが終端を持つとき、それらを併合して１つのセグメントにする処理である。次いでＳ２９５を経てＳ２９６に進んでネットワーク圧縮処理を行って不要なメモリ領域を開放する。
【０１０９】
図５フロー・チャートに戻ると、次いでＳ２４に進み、障害物情報出力処理を行う。図４６はその作業を示すサブルーチン・フロー・チャートである。これは、障害物移動予測テーブルの作成および他システムへの出力作業である。
【０１１０】
以下説明すると、Ｓ６００において自車位置を入力として道路（走行路）幅Ｗを算出する。走行路区分線は自車の前後２００ｍ分を自車が１０ｍ進むごとにリングバッファに取込み、保持しておくが、それから自車の左側（ないときは右側）の走行路区分線とそれに隣接する区分線を読み出し、道路幅Ｗを算出する。
【０１１１】
これについて説明すると、追従走行や障害物回避において、障害物がレーダに検知されていても、それが道路（走行路）に対してどの位置にあるのか分からなければ、自己の適切な制御は望めない。実施例において、画像処理によって走行路区分線が得られ、画像データ評価部によって不良データの削除、不足データの補填、および道路モデルの識別子が付与される。そのデータを受け、道路のモデルを基準に障害物がどの位置にあるかを統一的に表す。実際は、自己位置の後ろ２００ｍと最新の区分線の地図とモデルのデータを受け、区分線から道路幅を計算する。区分線の最遠の地点よりある程度遠くに障害物があるときは、区分線を２次式で近似する。区分線から障害物までの距離を求め、道路幅と車線位置から障害物の位置を計算する。
【０１１２】
この表現により、追従や回避の判断に必要な自車との位置関係が、単純な演算で可能である。即ち、位置が負のときは１を減算し、その後で少数点以下を切捨て、相手位置−自車位置により自車に対して何車線目にいるかが分かる。図４７にそれを示す。更に、走行路区分線に対する障害物の位置を計算する。これにより、自車に対する位置を簡単な計算で表すことができ、行動の計画や経路計画に利用することができる。ターゲットの位置を区分線に対する位置として表現する。
【０１１３】
続いてＳ６０２に進んでセグメント数カウンタ値ｉをインクリメントし、Ｓ６０４に進み、セグメントノード数ＮＮとノードマージ数ＮＭの和が、しきい値Ｔｈ（例えば５（５００ｍｓ））を超えるか否か、即ち、５００ｍｓ分のデータが集積したか否か判断する。肯定されときはＳ６０６に進み、障害物予測位置、即ち、その移動方位（セグメント傾き）、推定速度、推定加速度に基づいて、所定時刻（例えば１００ｍｓ）ごとの位置を計算し、障害物移動予測テーブルに書き込む。図４８に障害物移動予測テーブルを示す。
【０１１４】
続いてＳ６０８に進んでセグメントｉの速度Ｖが所定値Ｖｔｈ（例えば５ｋｍ／ｈ）未満か否か、即ち、障害物が移動しているか否か判断し、否定、即ち、移動していると判断されるときはＳ６１０に進み、図４９に示す如く、障害物移動ベクトルがｘ軸となるように、走行路区分線ＧＮＯデータを回転する。これにより、ｙ座標を参照するだけで対道路（走行路）位置を簡便に求めることができる。
【０１１５】
続いてＳ６１２に進み、障害物位置と予測位置の道路（走行路）に対する位置を求め、障害物移動予測テーブルに書き込む。図５０に対道路位置計算を、図５１に、障害物位置に対応する区分線データがないときの対道路位置計算を示す。次いでＳ６１４に進み、セグメント数カウンタ値ｉが最終値に達したことが確認されるまで、上記の処理を繰り返し、確認されるとＳ６１６に進み、予測テーブルを外部メモリ（前記した通信用メモリ３４）に書き込む。
【０１１６】
尚、Ｓ６０８で障害物が停止と見做されたときはＳ６１８に進んで現在障害物位置に最も近い走行路区分線点列を求め、Ｓ６２０に進んで図示の如く距離ｄと対走行路位置を求める。
【０１１７】
図５フロー・チャートに戻ると、Ｓ１６で検知データがないと判断されるときはＳ２６に進んでネットワークセグメント数カウンタｊをインクリメントし、Ｓ２８に進んで障害物位置予測を行い（先に図４３，４４で説明）、Ｓ３０に進んで障害物情報更新を行う。
【０１１８】
図５２はその作業を示すサブルーチン・フロー・チャートである。以下説明すると、Ｓ７００でセグメントの非接続回数が所定値（例えば２秒分）を超えたか否か判断し、肯定されるときは、その間見失っていたこととなってデータとして意味をなさないので、Ｓ７０２に進んでセグメントを削除する。これは該当するセグメント情報をクリアしてメモリ領域を空ける作業を意味する。即ち、作業メモリ領域を節約するために、例えばセグメント１，２，３があった後、セグメント２が消滅し、その後に新たなセグメントが発生したとき、セグメント４とせずに、セグメント２とできるようにしておく作業である。
【０１１９】
Ｓ７００で否定されるときはＳ７０４に進み、先に図４０，４２で説明したネットワークリンク処理を行って移動体らしさを計算し、判断する。続いてＳ７０６に進み、障害物の位置、速度、加速度の推定（先に図４３，４４で説明）を行い、Ｓ７０８に進んで障害物情報出力処理（先に図４６で説明）を行う。
【０１２０】
図５フロー・チャートにおいては次いでＳ３２に進んでネットワークセグメント数カウンタｊが最大値に達したことを確認してＳ３４に進み、ネットワーク圧縮処理を行う。これは先にも触れた通り、不要セグメントを削除してメモリ領域を活用する作業である。
【０１２１】
上記を図６ブロック図を参照して再説すると、移動状態量予測手段（カルマンフィルタ）は、次の検知時刻での障害物の予測位置Ｐｏｓハットｎｅｘｔを求める。また、道路領域境界検出手段（図３の画像評価ＣＰＵ３８）とレーダユニットとからの出力に基づき、検知位置データ処理手段は、図１０フロー・チャートに関して述べたクラスタリングを行って走行路内にある位置するものを障害物と検知すると共に、よって得たクラスタごとの障害物位置をＰｏｓ（絶対座標上の位置、即ち、原点を基準とした位置）として求める。
【０１２２】
また、障害物時系列情報更新手段は、検出データなどの情報を経時的に更新する。移動状態量予測手段（カルマンフィルタ）は、それに基づいて計測原点を更新すると共に、計測原点からの移動距離ｘ，時系列情報から求めたおよその障害物速度ｖないしその加速度ａに基づいて、次の検知時刻での予測位置Ｐｏｓハットｎｅｘｔを求める。障害物時系列情報記憶手段（ネットワーク型データ構造）では、それら予測値などから障害物の現在位置Ｐｏｓ，予測位置Ｐｏｓハットｎｅｘｔ，方位Ｄｉｒ，速度ｖなどの情報を障害物移動予測テーブル作成手段に送り、そこでテーブル化されて画像処理ＣＰＵ３６などの利用に供される。
【０１２３】
この実施例は上記の如く構成したので、自車の走行にとって真に障害となる前走車などの障害物や道路外の静止障害物などを精度良く検出することができる。また前走車などの情報をリアルタイムに検出し、その運動パラメータを予測するので、衝突の警告ないしは回避動作などをより的確に行うことができると共に、必要に応じて他のサブシステムにその情報を提供することができる。更に、カーブ路（曲線路）などを通行するときも、道路内外の障害物を的確に判別して真に自車の走行の妨げとなるもののみを検出することができる。
【０１２４】
更に、演算量の少ない直線運動モデルを使用しながら、所定個数のデータを処理したら最新検知位置付近に計測原点を更新するようにしたので、曲線路を含む走行環境においても移動距離の誤差が経時的に蓄積することがなく、よって予測しようとする位置、速度、加速度などの運動パラメータの誤差として表れることがない。
【０１２５】
尚、実施例は上記の如く構成したが、画像処理とレーダの検知範囲とは異なるために、障害物の位置によって判定条件を変え、レーダの検知データから求めた移動体らしさに反映させても良い。即ち、検知範囲と検知された位置との関係で、移動体らしさの評価を変えても良い。これにより、移動する障害物と停止している障害物の区別を道路（走行路）の形状を考慮した形で扱うことができる。
【０１２６】
また、レーダと画像処理の検知範囲が異なる場合、特にレーダの検知範囲が画像処理より遠い場合、画像処理の範囲外に検知された障害物は、道路に対する位置が分からないことになる。高速道路のような道路形状が長い距離にわたって余り変化しない場合は、過去の（通過してきた）道路構造が変わらない、例えば２車線から３車線への変化がない、ならば、数１００ｍ分の自車より後方と前方の最新の走行路区分線データから、最小２乗法により画像処理の範囲外の走行路区分線を補足して障害物の道路（走行路）に対する位置を計算しても良い。
【０１２７】
更に、毎回の画像処理による道路（走行路）の形状も同様なデータが入力されるので、前回結果と道路構造が変わず、自車の移動量も少ないならば、最小２乗法による画像処理の範囲外の走行路区分線の補足をキャンセルし、前回パラメータを用いて処理を高速化しても良い。
【０１２８】
更に、カルマンフィルタへの初期値として与える速度を単純平均値としたが、加重平均値などでも良い。
【０１２９】
また、適応的に動作する例としてカルマンフィルタを使用したが、それに限られるものではない。
【０１３０】
また、上記において、視覚センサを単眼視としたが、両眼視による距離測定を用いて前方レーダなどを省略しても良い。またレーダユニット１２を車両前方のみ設けたが、後方にも設けても良い。また、上記において、「走行路」「道路」「車線」などの名称を混用したが、同一のものを意味することは言うまでもない。
【０１３１】
【発明の効果】
請求項１項においては、自車の走行にとって真に障害となる走行路内の障害物のみを精度良く検出することができる。それによって、警告、回避など必要な動作を的確に行うことができる。また、前記した物体検出手段が例えばレーダであって送波ビームの照射広がり角、障害物までの距離によって同一障害物に対して複数の障害物があるかのように検出するときも、検出点データ群が所定範囲に対応した大きさの１つの障害物であると検知することができる。
【０１３３】
請求項２項の装置にあっては、前記所定範囲が、車両の投影面積に基づいて設定される値である如く構成したので、請求項１項で述べた効果に加えて、複数の検出点データ群が同一の車両であると検知することができる。
【図面の簡単な説明】
【図１】この発明に係る車両の障害物検出装置を備えた車両を全体的に示す説明透視図である。
【図２】図１のレーダの詳細を示す説明図である。
【図３】図１に示すセンサおよびその処理などを詳細に示すブロック図である。
【図４】図３ブロック図の構成をより機能的に示す図２と同様の説明図である。
【図５】この発明に係る車両の障害物検出装置の動作を示すメインフロー・チャートである。
【図６】図５フロー・チャートと同様にこの発明に係る車両の障害物検出装置の動作を説明する機能ブロック図である。
【図７】図５フロー・チャートの（障害物の）クラスタ化を説明する説明図である。
【図８】図１の車両がカーブ路を通過するときのレーダビームと障害物との関係を説明する説明図である。
【図９】実施例に係る走行路区分線によるクラスタの切りわけを説明する説明図である。
【図１０】図５フロー・チャートの検知データのクラスタ化作業を示すサブルーチン・フロー・チャートである。
【図１１】図１０フロー・チャートで予定する走行環境の説明図である。
【図１２】図１０フロー・チャートの作業を説明する説明図である。
【図１３】同様に図１０フロー・チャートの作業を説明する説明図である。
【図１４】同様に図１０フロー・チャートの作業を説明する説明図である。
【図１５】同様に図１０フロー・チャートの作業を説明する説明図である。
【図１６】この実施例で使用する移動情報のネットワークセグメントを示す説明図である。
【図１７】図１６でのノードの情報を示す説明図である。
【図１８】図１６でのネットワークセグメントの情報を示す説明図である。
【図１９】この実施例で用いた推定速度と推定方位を示すシミュレーションデータ図である。
【図２０】図１６のネットワークセグメントの単純更新を説明する説明図である。
【図２１】同様に図１６のネットワークセグメントの複数障害物が存在するときの更新を説明する説明図である。
【図２２】この実施例で用いる移動体らしさなる概念を表現する要素の説明図である。
【図２３】その移動体らしさの算出手法を示す説明図である。
【図２４】その移動体らしさの推移、前走車、停止障害物を示すシミュレーションデータ図である。
【図２５】図５フロー・チャートのネットワーク更新処理作業を示すサブルーチン・フロー・チャートである。
【図２６】図２５フロー・チャートの障害物位置予測作業を示すサブルーチン・フロー・チャート部分である。
【図２７】この実施例での移動予測による位置推定を説明する説明図である。
【図２８】図２７の移動予測手法による、実際の移動する車両における推定位置と入力位置とを示すシミュレーションデータ図である。
【図２９】同様に図２７の移動予測手法による、停止障害物における推定位置と入力位置とを示すシミュレーションデータ図である。
【図３０】前走車が定速から減速して停止する際の実際の前走車の速度、この実施例における推定速度、１秒間の予測位置における位置を示すシミュレーションデータ図である。
【図３１】この実施例におけるカルマンフィルタの移動体モデルを示す説明図である。
【図３２】この実施例におけるカルマンフィルタの初期値として、真値５０ｋｍ／ｈに対して初期値１００ｋｍ／ｈを与えた場合の収束状況を示すシミュレーションデータ図である。
【図３３】同様に、この実施例におけるカルマンフィルタの初期値として、セグメントの最新、最古のノードから求めた値を与えた場合の収束状況を示すシミュレーションデータ図である。
【図３４】図１に示す車両がカーブ路を走行するときの計測距離を説明する説明図である。
【図３５】カルマンフィルタの計算での原点更新をした場合としない場合の推定速度の優劣を示すシミュレーションデータ図である。
【図３６】この実施例での「停止」「単独」「接続」「非接続」の概念を説明する説明図
【図３７】図２５フロー・チャートに続くフロー・チャート部分である。
【図３８】図３７フロー・チャートに続くフロー・チャート部分である。
【図３９】図３８フロー・チャートのネットワーク発生処理を示す説明図である。
【図４０】図３８フロー・チャートのネットワークリンク処理作業を示すサブルーチン・フロー・チャートの前半部である。
【図４１】図４０の作業を説明する説明図である。
【図４２】図４０フロー・チャートの後半部である。
【図４３】図３８フロー・チャートの障害物位置、速度、加速度推定作業を示すサブルーチン・フロー・チャートの前半部である。
【図４４】図４３フロー・チャートの後半部である。
【図４５】図３８フロー・チャートに続く最終フロー・チャート部分である。
【図４６】図５フロー・チャートの障害物情報出力処理作業を示すサブルーチン・フロー・チャートである。
【図４７】図４６フロー・チャートの道路に対する障害物の位置表現を示す説明図である。
【図４８】図４６フロー・チャートの障害物移動予測テーブルを示す説明図である。
【図４９】図４６フロー・チャートの走行路区分線データの回転作業を説明する説明図である。
【図５０】図４６フロー・チャートの対道路位置計算を示す説明図である。
【図５１】図４６フロー・チャートの障害物移動予測テーブルへの書込み処理作業を示す説明図である。
【図５２】図５フロー・チャートの障害物情報更新作業を示すサブルーチン・フロー・チャートである。
【符号の説明】
１０ ＣＣＤカメラ（モノクロＴＶカメラ）
１２ レーダユニット
１４ ヨーレートセンサ
１６ 車速センサ
１８ 舵角センサ
３０ 画像処理ハードウェア
３６ 画像処理ＣＰＵ
３８ 画像評価ＣＰＵ
４０ レーダ処理回路
４２ レーダ評価ＣＰＵ
４４ 軌跡推定ＣＰＵ
４６ 軌跡追従制御ＣＰＵ
５０ 行動計画意思決定ＣＰＵ
５２ 舵角制御ＣＰＵ
６０ 車速制御（加速度制御）ＣＰＵ

Claims (2)

1. ａ．カーブ路を含む車両の走行路の境界位置を検出する走行路境界位置検出手段、
   ｂ．送信した波動の反射波を通じて少なくとも物体の位置を検出し、検出点データとして出力する物体検出手段、
   ｃ．前記検出点データの示す物体位置を前記走行路の境界位置と比較し、前記物体が走行路内に位置するか否かを判定する判定手段、
   ｄ．前記検出点データが所定範囲内にあるか否かを判断し、前記所定範囲内にあると判断されるとき、前記検出点データを１つのクラスタにクラスタリングすると共に、前記判定手段の判定結果に基づき、前記検出点データが走行路内に位置する物体の検出点データと走行路外に位置する物体の検出点データとからなると判定されるとき、前記１つのクラスタをさらに別のクラスタとするクラスタリング手段、
   および
   ｅ．前記走行路内に位置する物体の検出点データからなる前記クラスタを走行の障害となる障害物として検知する検知手段、からなることを特徴とする車両の障害物検出装置。
2. 前記所定範囲が、車両の投影面積に基づいて設定される値であることを特徴とする請求項１項記載の車両の障害物検出装置。

Images