JPH08122432A

JPH08122432A - 移動体の検出装置

Info

Publication number: JPH08122432A
Application number: JP6281217A
Authority: JP
Inventors: Yoshiaki Sakagami; 義秋坂上
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 1994-10-20
Filing date: 1994-10-20
Publication date: 1996-05-17
Anticipated expiration: 2018-04-28
Also published as: JP3400875B2

Abstract

(57)【要約】【構成】前走車など自車の走行にとって障害となる移
動体の位置観測データを入力として該移動体の速度を直
線運動モデルに基づいて推定するカルマンフィルタを用
いると共に、その初期値として該移動体の平均速度を与
える。また所定時間ごとに計算原点を更新する。【効果】カルマンフィルタの収束性が向上すると共
に、演算量が少ない直線運動モデルを用いながら誤差の
少ないリアルタイムな移動体情報を得られる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】この発明は移動体の検出装置に関
し、より具体的には走行時に障害となる前走車などの移
動体の検出装置であって、走行環境におけるその移動体
情報を精度良く検出できるようにしたものに関する。

【０００２】

【従来の技術】前走車の検出装置としては、例えば特開
昭６１−２７８７７５号公報記載の技術が知られてい
る。その従来技術においては、移動する前走車について
検出時刻の異なるデータ同士を関連づけて同定している
が、前走車の横方向への移動は、単に検出されるレーダ
ビームの変化として捉えているに過ぎないため、前走車
の位置や移動方向を精度良く検出することができなかっ
た。

【０００３】

【発明が解決しようとする課題】従って、この発明の目
的は上記した従来技術の欠点を解消することにあり、前
走車など走行環境において障害となる移動体の位置や移
動方向を正確に検出し、走行環境における移動体情報を
精度良く検出できるようにした移動体の検出装置を提供
することにある。

【０００４】更に、前走車などの移動体の位置や移動方
向を精度良く検出するには、その位置データから移動体
の運動パラメータを予測する必要がある。位置データは
様々なノイズを含んでおり、それから移動体の運動パラ
メータを予測するには、カルマンフィルタなどの予測フ
ィルタを用いることが有効である。しかし、そのとき障
害物の運動として曲線運動モデルを用いると演算量が増
え、処理の高速化と装置の小型低コスト化との両立が困
難となる。そこで、演算量の少ない直線運動モデルを用
いると、予測フィルタへの入力として例えば移動体の位
置データから求めた移動距離を使用することになるが、
移動体が曲線路を走行する場合、実際には曲線運動であ
るため移動距離の誤差が経時的に蓄積し、予測しようと
する位置、速度、加速度などの運動パラメータの誤差と
して表れる。

【０００５】従って、この発明の第２の目的は、演算量
の少ない直線運動モデルを用いつつ前走車などの移動体
の運動パラメータを精度良く予測すると共に、走行環境
が曲線路を含むときも、運動パラメータの誤差が蓄積し
ないようにした移動体の検出装置を提供することにあ
る。

【０００６】

【課題を解決するための手段】第１の目的を達成するた
めに、この発明は請求項１項において、自己に対して相
対的に移動する移動体の位置を検出して出力する検出手
段、前記検出手段の出力に基づき、前記移動体の所定の
時点からの移動距離を算出する移動距離算出手段、およ
び、前記算出された移動距離を入力として、前記移動体
の速度を少なくとも推定する線形予測フィルタ、からな
る移動体の検出装置であって、前記検出手段の出力に基
づき、前記移動体の所定期間における平均速度を算出す
る平均速度算出手段、を備え、前記線形予測フィルタ
は、前記算出された平均速度を初期値として前記速度を
少なくとも推定する如く構成した。

【０００７】第１の目的を達成するために、この発明は
請求項２項において、前記線形予測フィルタは、前記算
出された平均速度を初期値として前記移動体の初速度を
推定する如く構成した。

【０００８】第２の目的を達成するために、この発明は
請求項３項において、自己に対して相対的に移動する移
動体の位置を検出して出力する検出手段、前記検出手段
の出力に基づき、前記移動体と基準位置との間の距離を
算出する距離算出手段、前記算出された距離を入力と
し、直線運動モデルに基づいて前記移動体の運動パラメ
ータを推定する手段、からなる移動体の検出装置におい
て、所定時間ごとに前記基準位置を前記移動体の現在位
置付近に更新する如く構成した。

【０００９】

【作用】請求項１項においては、検出手段の出力に基づ
き、前記移動体の所定期間における平均速度を算出する
平均速度算出手段、を備え、線形予測フィルタは、前記
算出された平均速度を初期値として速度を推定する如く
構成したので、自車の走行にとって障害となる前走車な
どの障害物の位置や移動方向を精度良く検出することが
できる。それによって、警告、回避など必要な動作を的
確に行うことができる。尚、ここで「線形予測フィル
タ」は周囲の状況に適応して動作するフィルタを意味
し、実施例ではカルマンフィルタを用いた。また、平均
速度としたが、そうでなくとも良いことは言うまでもな
い。

【００１０】請求項２項においては、前記線形予測フィ
ルタは、前記算出された平均速度を初期値として前記移
動体の初速度を推定する如く構成したので、線形予測フ
ィルタの収束性が向上して推定精度を一層上げることが
できる。

【００１１】請求項３項においては、所定時間ごとに基
準位置を前記移動体の現在位置付近に更新する如く構成
したので、演算量の少ない直線運動モデルを用いなが
ら、走行環境が曲線路を含むときも、自車の走行にとっ
て障害となる前走車などの移動体の運動パラメータを誤
差なく予測することができる。

【００１２】

【実施例】以下、添付図面に即してこの発明の実施例を
説明する。

【００１３】図１はこの発明に係る移動体の検出装置を
備えた車両を全体的に示す説明透視図である。図におい
て、車両はＣＣＤカメラ（モノクロＴＶカメラ）１０を
１基備える。ＣＣＤカメラ１０は運転席上方のルームミ
ラー取り付け位置に固定され、車両進行方向を単眼視す
る。符号１２はミリ波レーダからなるレーダユニットを
示し、車両前方に取り付けられた２基の前方レーダから
なり、反射波を通じて他車などの移動体を含む立体障害
物の存在を検出する。

【００１４】レーダユニット１２は、より具体的にはＦ
Ｍ−ＣＷ型のレーダ装置からなり、図２に示す如く、複
数本の電磁波ビームを送波し、その反射波を受信して立
体障害物の存在を検出する。検出された障害物の位置座
標（相対距離）は、ビームごとに発生する。尚、このレ
ーダユニットの詳細は、本出願人が先に提案した特願平
５−２７６３９６号に詳細に述べられているので、これ
以上の説明は省略する。

【００１５】図１に戻ると、車両室内の中央部付近には
ヨーレートセンサ１４が設けられ、車両の重心を通る鉛
直軸（ｚ軸）回りの車両回転角（ヨー角）の変化速度
（ヨー角速度）を検出する。更に、車両のドライブシャ
フト（図示せず）の付近にはリードスイッチからなる車
速センサ１６が設けられ、車両の進行速度を検出すると
共に、舵角センサ１８が車両のステアリングシャフト２
０の付近に設けられてステアリング舵角を検出する。ス
テアリングシャフト２０には舵角制御モータ２２が取り
付けられると共に、スロットル弁（図示せず）にはパル
スモータからなるスロットルアクチュエータ２４が取り
付けられ、更にブレーキ（図示せず）にはブレーキ圧力
アクチュエータ２６（図１で図示省略）が取り付けられ
る。この構成において、車両は算出された舵角制御量に
応じて舵角制御されると共に、スロットル弁が開閉され
て車速が調節され、また必要に応じてブレーキが作動さ
せられて走行する。

【００１６】図３は上記の構成をより詳細に示すブロッ
ク図で、図４は図３の構成をより機能的に示す説明図で
ある。

【００１７】図３を中心に説明すると、ＣＣＤカメラ１
０の出力は、画像処理ハードウェア３０に送られてエッ
ジ検出とハフ（Ｈｏｕｇｈ）変換により直線成分の抽出
が行われ、その結果はバス３２を介して通信用メモリ３
４にストアされる。画像処理ＣＰＵ３６は、直線成分か
ら走行路（道路）区分線候補を抽出して通信用メモリ３
４にストアする。画像評価ＣＰＵ３８は、所定時刻ごと
にストア値を読み出して走行路（道路）区分線を決定す
る。レーダユニット１２の出力は、レーダ処理回路４０
およびバス３２を介して通信用メモリ３４にストアされ
る。レーダ評価ＣＰＵ４２は、所定時刻ごとにストア値
を読み出して障害物の位置を座標上で検出する。

【００１８】また車速センサ１６などの出力は、軌跡推
定ＣＰＵ４４に送られて自車両の移動軌跡が推定され
る。行動計画意思決定ＣＰＵ５０は、前記ストア値から
目標経路を作成する。その目標経路と推定された自車の
移動軌跡は、軌跡追従制御ＣＰＵ４６に送られ、そこで
軌跡（目標経路）追従制御量が決定される。更に、軌跡
追従制御ＣＰＵ４６は、舵角制御量を算出して舵角制御
ＣＰＵ５２に出力する。舵角制御ＣＰＵ５２は、ＰＷＭ
コントローラ５４およびドライバ５６を介して前記舵角
制御モータ２２を駆動する。尚、モータ駆動量はエンコ
ーダ５８を通じて検出され、フィードバック制御が行わ
れる。

【００１９】また行動計画意思決定ＣＰＵ５０は、その
速度・追従制御部で車体の目標加速度を求め、車速制御
ＣＰＵ６０に送出する。車速制御ＣＰＵ６０は、アクセ
ルパルスモータコントローラ６２、ドライバ６４を介し
てスロットルアクチュエータ２４を駆動すると共に、ブ
レーキソレノイドコントローラ６６およびドライバ６８
を介してブレーキ圧力アクチュエータ２６を駆動する。
その駆動量は圧力センサ７０を介して検出され、第２の
フィードバック制御が行われる。

【００２０】上記において、レーダ評価ＣＰＵ４２が主
として、実施例に係る移動体の検出装置に相当する。

【００２１】図５は実施例に係る移動体の検出装置の動
作を示すメイン・フロー・チャートである。また、図６
はその動作を説明する機能ブロック図である。

【００２２】図５フロー・チャートを参照する前に、実
施例に係る移動体の検出装置の動作を概説する。

【００２３】実施例に係る装置において、ＦＭ−ＣＷ型
のレーダユニット１２により時々刻々と検知された、前
走車などの移動体を含む障害物の位置座標に自車の現在
位置（座標軸とヨー角）を付与し、障害物の絶対座標を
保持しながら、過去の系列から、次の検知時刻における
位置をカルマンフィルタ（線形予測フィルタ）により予
測し、同一のものかどうか判断する。従来の車間距離を
計るレーダでは、ある時刻に検知されて次の時刻に検知
されたものは同一であると言う前提を置いているが、そ
の障害物が車両であるのか、どちらの方向に向かって進
んでいるのか分からないため、警報などで誤ることがあ
る。実施例に係る手法では、移動する障害物のおよその
方向、速度、加速度を検出し、よって危険の予知と共
に、的確な警報、自車の制御などを行うことを可能とし
た。

【００２４】移動する車両などは通常の走行状態では、
その軌跡は滑らかであり、速度も過度に急変することは
ないので、その性質を用いて検知した障害物の特性を表
現することができる。実施例では後述の如く、それを
「移動体らしさ」と表現する。それによって、障害物の
位置しか検出できないレーダユニット１２を用いても、
その動きを一意に表せ、自車の行動制御に利用できる。
例えば、手前で一時的に検知された障害物が更に接近し
たところで再度検知されたとき、後述の如く、同一のも
のと見做すことができ、それが停止している可能性が高
い、前述した表現で言えば移動体らしさが低いならば、
その側方を通過できる余地があるときは減速して通過す
る、余地がないときはその前で停止するなどの制御を行
うことができる。

【００２５】この実施例においては後述の如く、時系列
に発生した複数障害物の情報を効率良く管理し、新たに
入ってきた情報とそれまで保持していた情報とを比較
し、時間空間的に関連するものをノードとそれらを結ぶ
リンクとして表現する、いわゆるネットワーク型のデー
タ構造を用いる。それにより、複数障害物の情報の内容
を更新する手続きが容易になる。実施例では現在検知さ
れている１つの障害物に関するデータの一連を「セグメ
ント」と呼ぶ。後述の如く、移動体らしさを評価しつつ
セグメントを更新することにより、複数の障害物に対し
ても移動情報を簡単に求めることができる。前記のネッ
トワークセグメントの情報から、次に移動する位置が推
定でき、入力データが一時的になくなっても、関連性の
高いデータが再度検知されれば、そのデータはノードと
してセグメントにリンクされて再捕捉することができ、
移動情報が途中で失われることがない。

【００２６】以上を前提として、図５フロー・チャート
を参照して実施例に係る装置の動作を説明する。尚、図
５フロー・チャートに示すプログラムは、レーダ検知サ
イクルの１００ｍｓごとに起動される。

【００２７】先ずＳ１０でセグメントの個数をカウント
するカウンタｉの値を１にインクリメントし、Ｓ１２に
進んで最後のセグメントｉｍａｘに達していないことを
確認してＳ１４に進み、そこでデータ取込み・クラスタ
化を行う。

【００２８】実施例で使用するＦＭ−ＣＷ型レーダの特
性上、入力データから障害物の幅を検知することができ
ず、送波ビームの照射広がり角、障害物までの距離によ
っては同一障害物に対して複数の障害物があるかのよう
に検知される。そこで、ある基準でまとめてクラスタと
して扱う。その基準は、クラスタの幅と縦の長さをしき
い値として、図７に示すように、普通乗用車のサイズを
用いる。クラスタの幅を障害物の幅とし、重心を中心と
し、クラスタを後述するネットワークセグメントのノー
ドとして扱う。その操作により、後の処理でのデータ量
を抑えることができる。更に、どのビームに検知されて
いるかを検知の度に意識する必要がないと言う利点もあ
る。

【００２９】車両や道路外の物体など複数の障害物が存
在する場合、それらが道路に対してどの位置にあるのか
が分かれば、適切な警報動作や自車の制御が可能とな
る。しかしながら、図８に示すようにカーブ路（曲線
路）を通過するときなど、前走車など自車の進路上に存
在する障害物と樹木など道路外に存在する障害物とが、
明確に識別し難い。この場合、従来技術は例えば特開平
４−１９３６４１号公報に示されるように、撮像信号の
中から自車線または道路形状を示す信号を抽出して自車
の進路を判別すると共に、撮像された範囲に存在する物
体の位置を測定し、測定された物体の中で自車の進路上
に存在する物体を障害物と判断している。

【００３０】このように、特開平４−１９３６４１号公
報記載の技術は、進路の中か外かで障害物になるか否か
判別している。これにより、樹木などの静止障害物は障
害物ではないと判別することができるが、車両などは時
々刻々と移動することから、この従来技術ではそれが自
車にとって障害物になるか否かを必ずしも的確に判別す
ることができない。

【００３１】即ち、レーダデータをそのまま使用してい
るので、必ずしも実際の障害物と一対一には対応しな
い。何故ならば、１つの障害物の異なる部位から別々に
レーダ波の反射があり、複数の検出データが出力される
ことがしばしばあるからである。このため、上記した従
来技術によるときは、実際の障害物の正確な位置を知る
ことができない。また、検出データは点の情報であるの
で、実際の障害物の大きさを掴むことができない。

【００３２】そこで、この出願に係る実施例において
は、画像処理によって検出された走行路（道路）区分線
の情報とレーダによって検知された障害物の位置とを比
較し、ある基準を用いて障害物としてデータを１つに扱
う（クラスタ化する）ようにした。その場合に問題とな
るのが、走行路の外の物体（例えば、道路標識、ガード
レール）に起因する検出データである。検出したい障害
物（他の車両など）が、このような走行路外の物体の傍
らに位置していると、障害物に起因するデータと一緒に
走行路外の物体に起因する検出データまでもが同じクラ
スタに併合される。

【００３３】実施例ではクラスタは検出データを距離の
近いもの同士をまとめて大きさと中心位置を持つものと
認識することとしたが、走行路外の物体まで同じクラス
タに併合されると、障害物の大きさや中心位置を正しく
求めることができない。そこで、図９に示す如く、画像
処理で得た走行路区分線で分断されるか否か判断し、分
断されるならば複数のデータを別々のものとして扱い、
道路内と外に障害物があると判断した。それにより、図
９左側に示すような１つの障害物と判断する不都合が生
じない。

【００３４】図１０はその作業を示すサブルーチン・フ
ロー・チャートである。以下説明すると、Ｓ１００でカ
ウンタｍ（クラスタ内の検知データ数カウンタ），ｎ
（検知データ数カウンタ）を０に、ｊ（クラスタ数カウ
ンタ）を１にリセットする。またフラグｆｌｇを０にリ
セットする。

【００３５】続いてＳ１０２に進み、検知データ数カウ
ンタｎをインクリメントし、Ｓ１０４に進んで当該ｎの
検知データＤｎをその時点で注目している検知データＤ
とする。即ち、いま図１１の状態にあるとし、検知デー
タＤ１，Ｄ２，Ｄ３，Ｄ４のうち、Ｄ１に注目する。続
いてＳ１０６に進んで前記フラグｆｌｇが０であるか否
か判断する。このフラグは新たにクラスタを新設すると
き０にリセットされるものであり、先にＳ１００で０に
リセットされていることから、ここでの判断は肯定さ
れ、Ｓ１０８に進んでＤが最終検知データか否か判断す
る。

【００３６】図１１の状態にあると仮定すると、Ｓ１０
８での判断は否定され、Ｓ１１０に進んで変数Ｌ，Ｐ，
Ｇを検出データＤに書き替える（図１２）。ここで、Ｌ
はクラスタ内で最もｙ座標値の大きい検知データ、Ｐは
その時点で最後にクラスタに併合した検知データ、Ｇは
クラスタ内の検知データの座標値の総和を意味するが、
ここではＤ１をとりあえず、ｙ座標値が最も大きく、最
後にクラスタに併合したものであり、またそれを総和と
しておく。同時に、フラグｆｌｇの値を１にセットし、
クラスタ内検知データ数カウンタｍを１に書き替える
（即ち、最初のクラスタ内に検知データが１つあるとす
る）。

【００３７】続いてＳ１０２に戻り、検知データ数カウ
ンタ値ｎをインクリメントし、Ｓ１０４に進んでその時
点で注目している検知データをＤ２とし、Ｓ１０６に進
んでフラグの値が０か否か判断する。Ｓ１１０を経てき
たことから今度は否定されてＳ１１２に進み、図１３に
示すように、先の検知データ（Ｄ１）と現在の検知デー
タ（Ｄ２）のｘ，ｙ方向の差（絶対値）Ｌｘ−Ｄｘ，Ｌ
ｙ−Ｄｙが所定値（ここでは「Ｗｔｈ」（例えば５ｍ）
と「Ｄｔｈ」（例えば２ｍ）とする）より共に小さいか
否か判断する。所定値Ｗｔｈ，Ｄｔｈは、実際の乗用車
の大きさ、即ち、ｚ軸から見た投影面積にほぼ相当す
る。

【００３８】Ｓ１１２で肯定されたとするとＳ１１４に
進み、線分ＰＤ、即ち、Ｄ１（Ｐ）とＤ２（Ｄ）の間に
引かれた線分と走行路区分線（画像処理を経て実線で示
される）が交わるか否か判断する（図１４）。先に述べ
た如く、カーブ路を走行するときなど、道路内と外の物
体が同時に検知され、それらが自車の走行に際して障害
物になるか否か、そのままでは判断することはできな
い。そこで実施例においては、検知データが複数個ある
とき、クラスタ化すると共に、検知データ間に走行路区
分線が存在するときは、そこで別のクラスタとする。か
く構成することによって、自車の走行にとって真に障害
物となる物体を正確に判別することができる。

【００３９】図１４に示す例では交わらないことから、
ここでの判断は否定され、Ｓ１１６に進んで検知データ
Ｄ２をＰに書き替えてその時点で最後にクラスタ１（Ｓ
１００で特定）に併合した検知データとし、Ｄ（Ｄ２）
の座標値を加算して総和Ｇを更新し、Ｄ（Ｄ２）をクラ
スタ１内で最もｙ座標値の小さい検知データとする。同
時にクラスタ１内の検知データ数カウンタ値ｍをインク
リメントし、クラスタ１内に検知データが２個あるとす
る。

【００４０】続いてＳ１１８に進んでＤ（Ｄ２）が最終
検知データか否か判断する。ここで、最終か否かはｙ軸
に関して判断されるが、Ｓ１１８の判断は図１４の例で
は否定され、Ｓ１０２に戻り、Ｓ１０４に進んで今度は
検知データＤ３に注目し、Ｓ１１２を経てＳ１１４に進
み、新たな検知データＤ３と最前の検知データＤ２の間
に引かれた線分ＰＤと走行路区分線が交わるか否か判断
する。図１５に示すように、この場合は肯定されてＳ１
２０に進む。

【００４１】尚、Ｓ１１２で否定されるときは、Ｓ１１
４で線分と走行路区分線が交わるか否かを判断するまで
もなく、Ｓ１２０に進む。Ｓ１２０以降はクラスタを別
にするための処理であるが、検知データ間の距離が車両
の大きさを超えるときは、別のクラスタとする。即ち、
実施例で使用するレーダの特性から同一の車両に対して
複数の検知データが生じる可能性があるため、Ｓ１１４
を介して確認するものであるが、検知データ間の距離が
車両の大きさを超えるときは、別の物体と判断するのが
妥当と考えられるからである。

【００４２】Ｓ１２０ではクラスタ内検知データ数カウ
ンタ値ｍが１か否か判断する。即ち、新たなクラスタの
検知データが１個であるか否か判断する。図示例の場合
には先にＳ１１６で２にインクリメントされていること
から否定され、Ｓ１２２に進んでクラスタ１内の検知デ
ータの座標値の総和Ｇをクラスタ内検知データ数カウン
タ値ｍ（２）で除算してクラスタ１の重心位置Ｇｊ（ｊ
＝１）を求める。同時に、Ｌ（Ｄ１），Ｒ（Ｄ２）間の
距離の絶対値を求めてクラスタ１の幅Ｗｊとする。

【００４３】また、ここまでで検知データＤ１，Ｄ２を
クラスタ１とし、Ｄ３から別のクラスタとするためフラ
グの値を０にリセットし、クラスタ内検知データ数カウ
ンタ値ｍをインクリメントする。続いてＳ１２４に進ん
でクラスタ数カウンタ値ｊをインクリメントする。尚、
Ｓ１２０でクラスタ内の検知データが１と判断されると
きはＳ１２６に進んでその検知データの位置Ｌを重心位
置Ｇｊとすると共に、検知データが１個でクラスタとし
ての幅がないことから値Ｗｊは求めず、Ｓ１２８に進ん
でフラグの値を０にリセットする。

【００４４】続いてＳ１０８に進み、最終検知データか
否か判断する。図示ではＤ３であったことから否定さ
れ、Ｓ１１０に進んで図示の処理を行い、Ｓ１０２以降
を同様に進み、Ｓ１１８でＤが最終検知データと判断さ
れるまで以上のステップを繰り返す。そして、Ｓ１１８
で最終検知データと判断されるときはＳ１３０に進んで
検知データ数ｍが１か否か判断し、否定されるときはＳ
１３２に、肯定されるときはＳ１３４に進んで先にＳ１
２２，１２６で述べたと同様の処理を行って終わる。ま
た、Ｓ１０８で肯定されるときは検知データが１個であ
ったことから、Ｓ１３６に進んでその検知データを重心
位置に書き替えて終わる。

【００４５】図５フロー・チャートに戻ると、続いてＳ
１６に進んで検知データがあるか否か判断する。肯定さ
れるときはＳ１８に進んでネットワークが既に存在する
か否か判断し、否定されるときはＳ２０に進んでネット
ワーク発生処理を行う。ここで、先に触れたネットワー
クについて詳説する。

【００４６】レーダによって時々刻々検知されるデータ
は、それをつなげていけば、位置に関する時系列データ
となる。実際の道路（走行路）上では、他の走行車両や
停止している車両、ガードレールや標識など複数の物体
が存在するために、同時に別のものが検知される。よっ
て、これを切りわけ、各々のおよその移動速度、移動方
向を求める必要がある。そこで、実施例の場合、同時に
複数の障害物が扱えて、かつレーダユニットの検知誤差
を考慮したデータ構造とし、それらの更新を新しい入力
で過去までの累積情報にどれだけ依存しているか、即
ち、関連性を持つかを移動予測による位置推定に基づい
て行うこととした。

【００４７】Ｓ２０の作業は、このネットワーク型のデ
ータ構造を新設する作業を意味する。具体的には、図１
６に示すように、時刻ｉにおける障害物情報をノードと
する、移動体全体に関する時系列情報を格納したネット
ワークセグメントを作成する作業である。ノードには図
１７に示す情報を持たせる。セグメントには図１８に示
す情報を持たせる。以上のようなデータ構造によって、
検知されたデータを内部に保持する。移動する物体が車
両の場合、一般的な状況では車線変更を開始してから終
了するまでに約２秒を要する。よって、車両の動きをと
らえるのに十分なように約２秒分のデータが保持できる
ノードの数を設定する。また、およその速度と方向は、
ネットワークを利用して、以下の計算により簡単に求め
られる。速度＝（最新ノード位置−最古ノード位置）／（最新デ
ータ検知時刻−最古データ検知時刻）方向＝最新ノード位置と最古ノード位置を通る直線の傾
き

【００４８】通常のレーダ、例えばドップラーレーダで
は、対象物の相対速度が検知可能であるが、実施例で用
いている、位置のみを検知するＦＭ−ＣＷ型のレーダで
は、時間微分により速度を求めることになる。しかし、
外乱の多い環境では、単純に位置座標を微分することは
できない。そこで、実施例の場合、最古と最新のノード
間の移動量と時刻からおおよその速度を求めた。これに
より、ばらつきの影響はセグメントの始点と終点にのみ
現れ、比較的安定した結果が得られる。また、方位につ
いてもセグメント全体の向きを扱うので、ばらつきの影
響は少ない。尚、ここで求めたおおよその速度は後述の
如く、カルマンフィルタによる予測の初期値として用い
る。

【００４９】一般的な走行では０．２Ｇ程度の減速を考
慮すれば良く、０．２Ｇで２秒間減速した場合、実施例
では推定速度と実際の速度の差は、理論上７ｋｍ／ｈと
なって、カルマンフィルタによる予測の初期値として適
当である。図１９に推定速度、推定方位を示す。追従中
の前走車の速度は自車の速度と比較して比較的安定して
いる。但し、停止障害物について、レーダのばらつきの
影響を受けている。これは、速度の計算が先のネットワ
ークセグメントの始終点間距離に基づいているためであ
る。

【００５０】以上のネットワークセグメントを検知して
いる障害物の数だけ作成し、コンピュータ内部に保持
し、新たに検知されたデータに基づいてネットワークを
更新する。

【００５１】図５フロー・チャートの説明に戻ると、Ｓ
１８でネットワークが既に存在すると判断されるときは
Ｓ２２に進み、ネットワーク更新処理を行う。

【００５２】これについて述べると、ネットワークの更
新を単純更新と複数障害物存在時の更新に分類し、分類
結果により処理を違えるようにした。１）単純更新新たに検知された障害物のデータがどのセグメントに属
すかを推定位置との距離に基づいて判断する。即ち、そ
の距離がレーダシステムの検知ばらつき（ｘ変位とｙ変
位）より求めた範囲にあれば、セグメントに属し、なけ
れば、新たな別の障害物として扱う。即ち、新しいセグ
メントを発生させる。更新の条件、処理方法については
図２０に示す。２）複数障害物存在時の更新一般の道路において例えば、前走車、追い抜き車両、路
肩に停止した車両、標識など様々な障害物が存在し、レ
ーダにも検知される。実際の場面では、前走車の側方に
他の車両が走行していたり、同一車線の前方に複数の車
両が走行していて、そのうちの１台が隣の車線に移った
り、追い越した車両が追従している前走車の前あるいは
後ろへ進入するなど種々の場合が考えられる。このよう
な複雑な状況におけるネットワークの更新は、以下の３
形態で行う。ａ）新規発生新たに別の障害物が検知されたとき、新しいセグメント
を発生させる。ｂ）分岐継承同じような動きをしていた複数の障害物が別の方向へ向
きを変えたとき、過去の経緯の情報（ノード情報、セグ
メント情報）を新しいセグメントに継承させる。ｃ）合流消滅障害物同士が近づき、どちらか一方しか検知されなくな
ったとき、移動体らしさと言う評価基準に基づいて一方
を削除する。それぞれの更新条件、処理方法を図２１に示す。

【００５３】そして、複数障害物が存在するときの合流
消滅時の更新において、一方を削除するために前記した
移動体らしさと言う評価基準を設ける。これは、移動す
る過程、検知データの特性を考慮した評価基準である。
その要素として、移動推移、検知時間、センサ検知範
囲、障害物位置を図２２のように考え、移動体らしは図
２３に示すように算出する。この表現によれば、停止物
体（障害物）についても移動体と全く同じに扱うことが
できる。

【００５４】図２４に自車が走行中に検知した障害物の
移動体らしさの推移を示した。時刻４０（×５０ｍｓ）
において検知されたデータは、前走車（実線）と停止障
害物（破線）が同一に扱われたが、移動している前走車
の移動体らしさが相対的に大きいため、停止障害物のセ
グメントは消滅した。

【００５５】以上の手続きにより、ネットワークセグメ
ントの情報を更新する。即ち、図１８の内容を計算す
る。ノードが所定の数（約２秒分）を超えたら、各値か
ら最古のノードに関する移動量や曲率、これらの自乗和
などを減じ、新たなノードの情報を加える。これによ
り、セグメントは、新しい入力の度に更新され、セグメ
ントノードの一括計算を要しないので、複数の障害物が
あっても十分に短い処理時間で扱うことができる。

【００５６】図２５はそのネットワークの更新作業を示
すサブルーチン・フロー・チャートである。以下説明す
ると、Ｓ２００でノード接続テーブルＣｔｘ〔ｊ〕を初
期化する。即ち、ノード接続テーブルＣｔｘ〔ｊ〕＝０
とする（ｊ＝１〜検知データクラスタ数）。次いでＳ２
０２に進んで接続判定用テーブルの配列を初期化、即
ち、Ｍｔｘ〔ｉ〕〔ｊ〕＝０とする（ｉ＝１〜最大セグ
メント数）。

【００５７】次いでＳ２０４に進んでセグメント数カウ
ンタ値ｉをインクリメントし、Ｓ２０６に進んでセグメ
ント〔ｉ〕のノード数ＮＮがしきい値Ｎｔｈ（例えば５
（５００ｍｓ））を超えるか否か判断する。否定される
とき、即ち、データが５００ｍｓ分溜まっていないと判
断されるときはＳ２０８に進んでフラグｆｌｇｎの値を
０にリセットすると共に、５００ｍｓ分溜まっていると
判断されるときはＳ２１０に進んでフラグｆｌｇｎの値
を１にセットし、Ｓ２１２に進んで障害物位置予測を行
う。尚、ここで５００ｍｓ分溜まっていないと位置予測
を行わないのは、時系列データがこの程度蓄積されない
と、予測データの信頼性が低く、当初の目的にそぐわな
いからである。また５００ｍｓは、レーダの検知サイク
ルで言えば、５回分に相当する。

【００５８】図２６はその作業を示すサブルーチン・フ
ロー・チャートであるが、同図を参照して説明する前
に、その背景について説明する。

【００５９】高速道路上の車両を想定すると、車両は急
旋回せず、車線変更は二状態予測制御に基づくと言う前
提条件に立てば、移動はほぼ直線的として扱うことがで
きる。よってセグメントのノード位置を回帰直線で近似
することが可能となる。即ち、車両は２秒の間この直線
上を進んだと仮定する。更に、移動量は後述のようにカ
ルマンフィルタにより予測し、次の検知時刻にその車両
が移動している位置を先の直線上に求める。

【００６０】実際は、セグメントの最新ノードを通り、
セグメントの近似直線と直交する点から現在時刻（新た
に検知された時刻）までに進む距離Ｌだけセグメントの
近似直線上を進めた点Ｐを求める（図２７）。この位置
を推定位置、この作業を移動予測と呼ぶ。この手法によ
ると、現実に、運動モデルに直線移動、速度可変のもの
を適用したことと同等である。

【００６１】実際に移動する車両と停止障害物における
推定位置（実線）と入力位置（鎖線）をプロットしたも
のを図２８，２９に示す。移動する車両についてはほぼ
一致し、停止障害物では約２ｍのずれ程度であり、移動
障害物と停止障害物が近くにあると、区別できない場合
が生じる。これは、後述の複数障害物存在時の処理で対
処する。また、レーダは、障害物の形状や向きによって
は、一時的に検知不能時間が発生する。即ち、短時間見
失うことがある。この移動予測によれば、新たな時刻に
基づく予測位置を出すので、短い時間ならば、障害物の
再捕捉が可能である。

【００６２】ところで、自動運転車の制御系では、画像
処理、本処理、行動計画などがそれぞれ非同期に処理を
行う場合、データの受渡しタイミングが異なる。即ち、
データを受ける側の処理時間に依存してデータの遅れ時
間が発生する。実施例では、障害物の検知時刻における
速度、加速度、方位から運動モデル（物理モデル）に従
い、カルマンフィルタ（前記した線形予測フィルタ）を
用いて任意時間先の位置を計算し、データを受ける側の
処理の時刻を基準とした障害物の情報が得られるように
した。これにより、処理時間の異なるシステム間で通信
遅れ時間を意識しないで障害物の情報を扱うことができ
る。

【００６３】但し、カルマンフィルタの処理結果により
推定された速度、加速度はゆらぎがあり、その影響を受
けるため、任意時間の設定は余り大きくできない。実施
例では、レーダ検知時刻から１秒間を１００ｍｓごとに
障害物の位置を算出している。図３０は前走車が定速か
ら減速して停止する際の実際の前走車の速度（破線で示
す）、本処理における推定速度、１秒間の予測位置にお
ける速度をプロットしたものである。減速時には、途中
から推定速度が実測値を下回っているが、処理時間、通
信遅れ時間、および制御時間の遅れを考慮して、この予
測値を使うことにより、システム全体の遅れを減少させ
ることが可能である。尚、予測時間と計算時間刻みを任
意に変更しても良い。

【００６４】処理結果は、図３に示す行動計画意思決定
ＣＰＵ５０、画像処理ＣＰＵ３６、車速制御ＣＰＵ６０
の各サブシステムに通信用メモリ３４経由で渡される。
各サブシステムは、処理時間が異なるため、基本的に非
同期である。このことは、レーダが障害物の位置を検知
した時刻から、実施例の処理を行った後、実際に各サブ
システムへデータが渡るまでに時間の遅れがあることを
意味する。実施例では、障害物の現在の速度、加速度、
移動方位に基づいて、任意の先の時刻を求めるが、その
予測情報（後に「障害物移動予測テーブル」として示
す）を渡すことにした。その結果、各サブシステムは、
軌跡推定からデータを受けた時刻に基づいて、障害物移
動予測テーブルの参照位置を決定し、データを使用する
ことができる。これにより、通信時間の遅れを意識しな
いで、障害物の情報を扱うことができる。

【００６５】その予測をカルマンフィルタを用いて行う
ので、次にカルマンフィルタについて説明する。

【００６６】カルマンフィルタは、時系列なある信号ｘ
（ｔ）が白色信号によってドライブされた線形ダイナミ
カルシステム（確率差分方程式）の出力として、信号の
相関性がダイナミックスの構造に由来すると見做し、線
形予測フィルタを構成する。これにより、信号や雑音の
時系列を実時間で扱うことができると言う特徴がある。

【００６７】宇宙開発におけ衛星の軌跡推定では、ベイ
ズの重みつき最小２乗フィルタ（バッチフィルタ）が古
くから使用されており、現在も使用されている。バッチ
フィルタは、データを一括処理することから、ある時点
からある時点までの予測を一度で行う。一方、車載する
障害物検知システムにおける障害物の移動推定は、検知
してからの予測、認識、判断に要する時間が、短かくな
ければならない。通常のバッチフィルタでは遅れがあ
り、衛星のような遠距離にある移動物体の推定には十分
使えるが、衝突の回避などを行うには、その遅れは致命
的なものとなる。また、カルマンフィルタは、常に最新
のデータを用いてリアルタイムに計算でき、専用ハード
ウェア化しやすいと言う利点がある。

【００６８】実施例では、カルマンフィルタの構成は以
下の通りとした。

【００６９】離散時間系のプロセスは、数１のように表
すことができる。

【００７０】

【数１】

【００７１】ここで、ｘ（ｋ）は時間ｋにおける状態ベ
クトル、ｕ（ｋ）は入力、Ｇ（ｋ）は既知の行列、ｖ
（ｋ）は平均値０、共分散行列Ｑ（ｋ）のガウシアンノ
イズである。

【００７２】また、計測過程は、数２のように表すこと
ができ、それに基づいてカルマンフィルタの計算ステッ
プは数３から数８のように示すことができる。ここで、
ｗ（ｋ）は平均値０、共分散行列Ｒ（ｋ）のガウシアン
ノイズである。初期値ｘ（０）は、平均値ｘ、共分散行
列Ｐのガウシアンノイズとする。

【００７３】

【数２】

【００７４】

【数３】

【００７５】

【数４】

【００７６】

【数５】

【００７７】

【数６】

【００７８】

【数７】

【００７９】

【数８】

【００８０】実施例では、図３１に示すように、状態ベ
クトルｘ（ｋ）の要素をそれぞれ、距離（計測原点（起
点）からの移動距離）、速度、加速度とし、系を等加速
度直線運動とする。従って、計測は障害物発見時からの
距離とする。

【００８１】ところで、カルマンフィルタの計算には初
期値を与えなければならない。その初期値は、実際の値
に近いほど、より真値に近づく。図３２は、真値５０ｋ
ｍ／ｈに対して初期値に自車の速度１００ｋｍ／ｈを与
えた場合である。収束までおよそ１．７秒を要してい
る。実施例では先に述べた如く、およその速度を時系列
データのネットワークから求め、カルマンフィルタの初
期値として与えるようにした。その結果、図３３に示す
如く、収束性を向上させることができた。

【００８２】レーダが障害物を検知した時刻から、初め
て速度推定値を出力するまでの時間を、どれぐらいに設
定すべきかと言う問題がある。これは、実際のテストか
ら約０．６秒（レーダのサンプリング周期で６回）とし
た。レーダの揺らぎによってこの時間は変動する。即
ち、レーダが十分に精度が良ければ少ない時間で良く、
逆に精度が悪い場合は、時間を長くとる必要がある。ま
た、ネットワークによる推定錯誤を考慮すると、０．６
秒間のネットワークによる速度の誤差は障害物の加速度
に応じて１．８ｋｍ／ｈ（０．１Ｇ）から９．０ｋｍ／
ｈ（０．５Ｇ）であり、カルマンフィルタへの初期値と
しては問題ない。

【００８３】実施例では、カルマンフィルタの系のモデ
ルに、等加速度直線運動の物理モデルを用いている。障
害物を最初に発見してから（計測原点から）の移動距離
に基づいて、レーダユニットの出力時間ごとにカルマン
フィルタの計算を行っている。ところが、実際の車は、
カーブなどを通過するため、直線運動はしない（図３
４）。これは、車が長時間にわたってカーブを通過した
り、トラックコースのようなスタート地点に戻るような
場合（一般道路ではそのようなことはほとんどない
が）、カルマンフィルタの計算が時間が経つにつれて、
誤差が増えることになる。その誤差を増やさないため
に、計算スパンを設定し、ある時間ある個数のデータを
処理したら、計測原点を最新検知位置付近に更新するよ
うにした。尚、計算スパンは、誤差分散が収束する時間
以上を設定すれば良いことが実験の結果判明した。その
結果から、実施例では２秒とした。

【００８４】図３５に原点更新しない場合とした場合
の、カーブ通過中の前走車（９０ｋｍ／ｈの定速走行）
の推定結果を示す。原点更新しない場合は時間の経過と
共に、推定速度が低下している。原点更新した場合は、
推定速度の低下が抑えられている。原点を更新せずにこ
のような問題に対応するには、非線形なモデルを用いた
カルマンフィルタを用いれば良い。しかし、非線形なモ
デルを使用すると複数の障害物についての処理時間が長
くなり、リアルタイムに予測するには適当ではない。原
点を更新すれば、線形モデルを使用しても誤差の蓄積を
防ぐことができる。

【００８５】以上を前提として、障害物位置予測につい
て、図２６フロー・チャートを参照して説明すると、Ｓ
３００で上記したカルマンフィルタを用いて図示の如く
予測値を計算する。ここで、ｘハット、ｙハットが、図
６ブロック図のＰｏｓハットｎｅｘｔに相当するが、そ
れについては最後に述べる。

【００８６】図２５フロー・チャートに戻ると、続いて
Ｓ２１４に進み、クラスタごとの接続ノード数テーブル
値Ｓｔｘ〔ｉ〕を０にリセットし、Ｓ２１６に進んで検
知クラスタカウンタ値ｊをインクリメントし、Ｓ２１８
に進んでフラグｆｌｇｎの値が０であるか否か判断す
る。肯定されるとき、即ち、データが５００ｍｓ分溜ま
っていないと判断されるときはＳ２２０に進み、最短距
離に基づいてノードリンクの判定処理を行う。

【００８７】これは具体的には、セグメント終端と障害
物との距離を求めて判定結果ｍを得る作業である。図３
６の上部にその作業を示すが、ノードリンクの判定につ
いては図示の如く約束する。即ち、セグメント終端ノー
ドと検知データとが同一位置にあるときを「停止」、５
０ｃｍ以内のときを「単独」、５０ｃｍを超えてもレー
ダ検出誤差Ｘｅｒ内にあるときを「接続」、それ以外を
「非接続」とする。

【００８８】またＳ２１８で否定されるときは、ノード
数が５００ｍｓを超えていることからＳ２２２に進み、
障害物予測位置に基づいてセグメント終端との距離を同
様に求めて行う。これは、予測位置と検知データの距離
によって判定結果ｍを得る作業である。図３６の下部に
その作業を示す。

【００８９】ここで、距離ｄ＝０ならば「停止」、距離
ｄ≦５０ｃｍならば「単独」とする。また距離ｄを予測
位置との距離ｅｄと比較するとき、ｄ＞ｅｄの場合、５０ｃｍ＜ｅｄ＜レーダ検出誤差Ｘｅ
ｒならば「接続」ｅｄ≧Ｘｅｒならば「非接続」ｄ≦ｅｄの場合、５０ｃｍ＜ｄ＜Ｘｅｒならば「接続」ｄ≧Ｘｅｒならば「非接続」とする。

【００９０】次いでＳ２２４に進んで求めた判定結果ｍ
が" 非接続" か否か判断する。これは図３６に示すよう
に、セグメント終端と検知データとが接続されるか否か
を判断することである。

【００９１】否定されるときはＳ２２６に進んで各テー
ブル値のクラスタ数Ｃｔｘ〔ｊ〕とセグメント数Ｓｔｘ
〔ｉ〕とをインクリメントし、Ｓ２２８でクラスタｊが
最大クラスタｊｍａｘに達したと判断され、Ｓ２３０で
セグメントｉが最大セグメントｉｍａｘに達したと判断
されるまで、上記の処理を繰り返す。

【００９２】次いで図３７のＳ２３２に進んでクラスタ
数カウンタ値ｊをインクリメントし、Ｓ２３４に進んで
クラスタテーブル値Ｃｔｘ〔ｊ〕が１を超えるか、即
ち、クラスタｊが複数のセグメントに接近するか否か判
断する。肯定されるときはＳ２３６に進んでセグメント
数カウンタ値ｉをインクリメントしてＳ２３８に進み、
そこで接続判定用テーブル値Ｍｔｘ〔ｉ〕〔ｊ〕が接
続、単独または停止か否か判断し、肯定、即ち、非接続
状態にあればＳ２４０，Ｓ２４２に進んでクラスタ−セ
グメント間の距離が最小となる組み合わせを探す。

【００９３】即ち、あるクラスタｊが複数のセグメント
と接続可能な場合は、最短距離のものと接続すると見做
す。尚、Ｓ２３４で否定されるときはＳ２４４に進み、
そこでノード接続テーブル値Ｃｔｘ〔ｊ〕の値が０か否
か判断し、肯定されるときはＳ２４６に進んでネットワ
ークを新たに作成する。上記をＳ２４８で最大クラスタ
ｊｍａｘに達したと判断されるまで繰り返す。次いでＳ
２５０からＳ２６０の処理を行い、セグメントに接続し
ないときはセグメント終端から最も近いクラスタを選択
し、そのセグメントに接続させる。

【００９４】次いで図３８のＳ２６２からＳ２６８を進
み、それまでに選択されなかったクラスタについて新た
にネットワークを発生させる。即ち、図３９に示すよう
な処理を行う。これは、先に図２１で示した「分岐継
承」に相当する処理であり、セグメントの過去情報をコ
ピーした上で新たなセグメントとして登録する。次いで
Ｓ２７０からＳ２８０を経てＳ２８１に進み、接続ない
し単独と判断されるときはＳ２８２に進んでネットワー
クリンク処理を行う。

【００９５】図４０はその作業を示すサブルーチン・フ
ロー・チャートである。以下説明すると、Ｓ４００でセ
グメント終端ｘ座標と自車位置のｘ座標との差と、検知
クラスタｘ座標と自車位置ｘ座標との差とを求めて比較
する。これは、検知クラスタが自車位置に対して対向し
ているか否か判断するためであり、肯定されるときは自
車に接近していると判断してＳ４０２に進んでｍｏｖｅ
（セグメント終端と検知クラスタとの距離を表す）の符
号を逆転する。これは、図示しないルーチンで移動距離
の変動を見るためである。

【００９６】次いでＳ４０４に進んでセグメント終端と
検知クラスタとの関係が単独であるか否か判断し、肯定
されるとき、即ち、検知クラスタがほとんど動いていな
いとみられるときはＳ４０６に進んでセグメント〔ＳＮ
Ｏ〕マージ回数カウンタ値をインクリメントし、Ｓ４０
８に進んでカウンタ値が１を超えたか否か判断し、肯定
されるときはＳ４１０に進んで移動量総和よりセグメン
ト終端位置を減じると共に、移動量２乗和よりセグメン
ト終端位置の２乗値を減じる。これは、図４１に示すよ
うに、セグメント終端ノードを検知クラスタ側に付け替
えるために、それまでのデータを更新する作業を意味す
る。

【００９７】次いでＳ４１２に進んで同様にノード数カ
ウンタ値が２を超えたか否か判断し、肯定されたときは
曲率（図４１に示す）を計算し、Ｓ４１６に進んで曲率
総和よりセグメント終端曲率を減じると共に、曲率２乗
和よりノード終端曲率２乗値を減じる。これは、Ｓ４１
０で説明した図４１に示す付け替え作業である。

【００９８】次いでＳ４０４で否定、即ち、接続または
停止とされた場合に備え、Ｓ４１８に進んでセグメント
ノード数カウンタ値をインクリメントし、Ｓ４２０，Ｓ
４２２に進んで同様の処理を行う。次いで図４２フロー
・チャートのＳ４２４に進んで障害物情報ノードメモリ
を確保する。次いでＳ４２６に進んでその確保ができな
かったか否か判断し、肯定されるときはＳ４２８，Ｓ４
３０，Ｓ４３２を進んで移動体らしさが最低のセグメン
トを削除してネットワークを圧縮する。また、Ｓ４２６
で否定されるときはＳ４３４に進んで障害物クラスタ情
報をノードに移送する。これは先に図１７に示した情報
である。

【００９９】次いでＳ４３６に進み、ノード数が所定保
持時間分（例えば２秒分）あるか否か判断し、肯定され
るときはＳ４３８に進んでセグメント先端の移動量と曲
率を各総和より減じると共に、各２乗和も同様に２乗値
を減じ、Ｓ４４０に進んでセグメント〔ＳＮＯ〕先端へ
のポインタ値を、セグメント先端の次のノードのポイン
タ値に書き替える。

【０１００】次いでＳ４４４に進んで移動体らしさを計
算し、Ｓ４４６に進んでセグメントノード長に基づく障
害物平均速度計算を行い、Ｓ４４８に進んで障害物移動
方位計算を行う。これは、先に述べたネットワークを利
用しての最新、最古のノード位置などから速度、方向
（方位）を計算することであり、うち速度値、より正確
には平均速度値がカルマンフィルタに初期値として与え
られる。最後にＳ４５０に進んでセグメント〔ＳＮＯ〕
非接続回数カウンタ値を０にリセットする。

【０１０１】図３８フロー・チャートにおいては、次い
でＳ２８３に進んで障害物の位置、速度、加速度を推定
する。尚、Ｓ２８１で否定されたときはＳ２８４に進
み、そこで停止と判断されたときはＳ２８５に進み、同
一であることからセグメント併合処理を行い、Ｓ２８６
に進んで同様に障害物位置、速度、加速度推定を行う。
Ｓ２８７，Ｓ２８８で検知データクラスタ数、セグメン
ト数がそれぞれ最大値に達したと判断されるまで、上記
を繰り返す。

【０１０２】図４３ないし４４は、障害物位置、速度、
加速度推定作業を示すサブルーチン・フロー・チャート
である。

【０１０３】以下説明すると、Ｓ５００で原点カウンタ
ｒｃｎｔをインクリメントし、Ｓ５０２に進んでカウン
タ値が所定値（例えば２０（２０００ｍｓ分））を超え
るか否か判断し、肯定されたときＳ５０４に進んでカウ
ンタ値ｒｃｎｔとフラグｐｒｃを０にリセットする。即
ち、２秒経過するごとに、障害物移動距離の計測原点を
移動（更新）する。

【０１０４】続いてＳ５０６に進んでフラグｐｒｃが０
であるか否か判断する。この場合は当然肯定されてＳ５
０８に進み、フラグｐｒｃに１をセットし、Ｓ５１０に
進んでセグメントノード数が１であるか否か判断する。
初めてこの処理を行うときは肯定されてＳ５１２に進
み、図示の如くセグメント終端ノードの障害物位置の
ｘ，ｙ座標を用いて原点（Ｏｘ，Ｏｙ）の移動（更新）
を行う。またＳ５１０で否定されたときはＳ５１４に進
み、セグメント終端より１つ前のノードの障害物位置情
報を移動距離計測原点にする。

【０１０５】続いて、Ｓ５１６に進み、前記したカルマ
ンフィルタの値を設定し、Ｓ５１８に進んで前記距離ｄ
を移動距離ｘ

〔０〕の初期値とする。続いてＳ５２０に
進んで原点カウンタ値ｒｃｎｔが１か否か、即ち、原点
更新後最初のカルマンフィルタ計算であるか否か判断
し、肯定されるときはＳ５２２に進んでセグメント平均
速度を速度ｘ〔１〕の初期値とする。セグメント平均速
度とは、図４２フロー・チャートのＳ４４６で求めた平
均速度を意味する。これは、前記した説明から明らかな
如く、所定期間における平均速度である。

【０１０６】即ち、実施例に係るカルマンフィルタにお
いては、そのモデルに等加速度直線運動モデルを使用
し、レーダユニット１２を通じて障害物を最初に発見し
た計測原点からの移動距離を求め、ネットワークから求
めた速度を初期値として与えて計測時間ごと（１００ｍ
ｓごと）にカルマンフィルタの計算を行って１００ｍｓ
後、即ち、次のレーダ検知時刻での障害物の移動距離、
速度、加速度を推定している。

【０１０７】即ち、現在時刻の障害物の位置を推定し、
初期値を適切に与えることにより、カルマンフィルタの
収束性を向上させ、障害物情報をリアルタイムに得て図
３の構成で他のサブシステムに提供することができる。
更に、計測原点を２秒ごとに更新して誤差の蓄積を回避
している。

【０１０８】続いてＳ５２４に進んでＱを零クリアし、
Ｓ５２６，Ｓ５２８に進んで共分散行列Ｑ，Ｒに図示の
如き値をセットし、図４４フロー・チャートのＳ５３０
に進んで運動方程式Ｆの転置行列を求めて値Ｆｔとし、
Ｓ５３２に進んでカルマンフィルタにより、障害物の距
離（位置）、速度（初速度）、加速度の推定を行う。こ
こで、Ｘが推定値である。

【０１０９】尚、Ｓ５０６でフラグｐｒｃが０ではな
い、即ち、原点更新が行われないと判断されるときはＳ
５３４に進んで距離ｄを図示の如く求める。これは、速
度と加速度の初期値として前回推定した値をそのまま用
いるので、改めて初期値を設定する必要がないからであ
る。

【０１１０】次いで図３８から図４５フロー・チャート
に移り、そのＳ２８９からＳ２９１に進み、セグメント
削除判定処理を行う。これは、Ｓｔｘ〔ｉ〕＝０、即
ち、接続可能な障害物クラスタが１つもなかったセグメ
ントｉに関して前記した移動体らしさを所定値と比較
し、所定値未満のセグメントを削除する処理である。次
いでＳ２９２からＳ２９４を進んでノード終端位置に基
づいてセグメント併合処理を行う。これは、乗用車の平
面と同一サイズ内に複数のセグメントが終端を持つと
き、それらを併合して１つのセグメントにする処理であ
る。次いでＳ２９５を経てＳ２９６に進んでネットワー
ク圧縮処理を行って不要なメモリ領域を開放する。

【０１１１】図５フロー・チャートに戻ると、次いでＳ
２４に進み、障害物情報出力処理を行う。図４６はその
作業を示すサブルーチン・フロー・チャートである。こ
れは、障害物移動予測テーブルの作成および他システム
への出力作業である。

【０１１２】以下説明すると、Ｓ６００において自車位
置を入力として道路（走行路）幅Ｗを算出する。走行路
区分線は自車の前後２００ｍ分を自車が１０ｍ進むごと
にリングバッファに取込み、保持しておくが、それから
自車の左側（ないときは右側）の走行路区分線とそれに
隣接する区分線を読み出し、道路幅Ｗを算出する。

【０１１３】これについて説明すると、追従走行や障害
物回避において、障害物がレーダに検知されていても、
それが道路（走行路）に対してどの位置にあるのか分か
らなければ、自己の適切な制御は望めない。実施例にお
いて、画像処理によって走行路区分線が得られ、画像デ
ータ評価部によって不良データの削除、不足データの補
填、および道路モデルの識別子が付与される。そのデー
タを受け、道路のモデルを基準に障害物がどの位置にあ
るかを統一的に表す。実際は、自己位置の後ろ２００ｍ
と最新の区分線の地図とモデルのデータを受け、区分線
から道路幅を計算する。区分線の最遠の地点よりある程
度遠くに障害物があるときは、区分線を２次式で近似す
る。区分線から障害物までの距離を求め、道路幅と車線
位置から障害物の位置を計算する。

【０１１４】この表現により、追従や回避の判断に必要
な自車との位置関係が、単純な演算で可能である。即
ち、位置が負のときは１を減算し、その後で少数点以下
を切捨て、相手位置−自車位置により自車に対して何車
線目にいるかが分かる。図４７にそれを示す。更に、走
行路区分線に対する障害物の位置を計算する。これによ
り、自車に対する位置を簡単な計算で表すことができ、
行動の計画や経路計画に利用することができる。ターゲ
ットの位置を区分線に対する位置として表現する。

【０１１５】続いてＳ６０２に進んでセグメント数カウ
ンタ値ｉをインクリメントし、Ｓ６０４に進み、セグメ
ントノード数ＮＮとノードマージ数ＮＭの和が、しきい
値Ｔｈ（例えば５（５００ｍｓ））を超えるか否か、即
ち、５００ｍｓ分のデータが集積したか否か判断する。
肯定されときはＳ６０６に進み、障害物予測位置、即
ち、その移動方位（セグメント傾き）、推定速度、推定
加速度に基づいて、所定時刻（例えば１００ｍｓ）ごと
の位置を計算し、障害物移動予測テーブルに書き込む。
図４８に障害物移動予測テーブルを示す。

【０１１６】続いてＳ６０８に進んでセグメントｉの速
度Ｖが所定値Ｖｔｈ（例えば５ｋｍ／ｈ）未満か否か、
即ち、障害物が移動しているか否か判断し、否定、即
ち、移動していると判断されるときはＳ６１０に進み、
図４９に示す如く、障害物移動ベクトルがｘ軸となるよ
うに、走行路区分線ＧＮＯデータを回転する。これによ
り、ｙ座標を参照するだけで対道路（走行路）位置を簡
便に求めることができる。

【０１１７】続いてＳ６１２に進み、障害物位置と予測
位置の道路（走行路）に対する位置を求め、障害物移動
予測テーブルに書き込む。図５０に対道路位置計算を、
図５１に、障害物位置に対応する区分線データがないと
きの対道路位置計算を示す。次いでＳ６１４に進み、セ
グメント数カウンタ値ｉが最終値に達したことが確認さ
れるまで、上記の処理を繰り返し、確認されるとＳ６１
６に進み、予測テーブルを外部メモリ（前記した通信用
メモリ３４）に書き込む。

【０１１８】尚、Ｓ６０８で障害物が停止と見做された
ときはＳ６１８に進んで現在障害物位置に最も近い走行
路区分線点列を求め、Ｓ６２０に進んで図示の如く距離
ｄと対走行路位置を求める。

【０１１９】図５フロー・チャートに戻ると、Ｓ１６で
検知データがないと判断されるときはＳ２６に進んでネ
ットワークセグメント数カウンタｊをインクリメント
し、Ｓ２８に進んで障害物位置予測を行い（先に図４
３，４４で説明）、Ｓ３０に進んで障害物情報更新を行
う。

【０１２０】図５２はその作業を示すサブルーチン・フ
ロー・チャートである。以下説明すると、Ｓ７００でセ
グメントの非接続回数が所定値（例えば２秒分）を超え
たか否か判断し、肯定されるときは、その間見失ってい
たこととなってデータとして意味をなさないので、Ｓ７
０２に進んでセグメントを削除する。これは該当するセ
グメント情報をクリアしてメモリ領域を空ける作業を意
味する。即ち、作業メモリ領域を節約するために、例え
ばセグメント１，２，３があった後、セグメント２が消
滅し、その後に新たなセグメントが発生したとき、セグ
メント４とせずに、セグメント２とできるようにしてお
く作業である。

【０１２１】Ｓ７００で否定されるときはＳ７０４に進
み、先に図４０，４２で説明したネットワークリンク処
理を行って移動体らしさを計算し、判断する。続いてＳ
７０６に進み、障害物の位置、速度、加速度の推定（先
に図４３，４４で説明）を行い、Ｓ７０８に進んで障害
物情報出力処理（先に図４６で説明）を行う。

【０１２２】図５フロー・チャートにおいては次いでＳ
３２に進んでネットワークセグメント数カウンタｊが最
大値に達したことを確認してＳ３４に進み、ネットワー
ク圧縮処理を行う。これは先にも触れた通り、不要セグ
メントを削除してメモリ領域を活用する作業である。

【０１２３】上記を図６ブロック図を参照して再説する
と、移動状態量予測手段（カルマンフィルタ）は、次の
検知時刻での障害物の予測位置Ｐｏｓハットｎｅｘｔを
求める。また、道路領域境界検出手段（図３の画像評価
ＣＰＵ３８）とレーダユニットとからの出力に基づき、
検知位置データ処理手段は、図１０フロー・チャートに
関して述べたクラスタリングを行い、よって得たクラス
タごとの障害物位置をＰｏｓ（絶対座標上の位置、即
ち、原点を基準とした位置）として求める。

【０１２４】また、障害物時系列情報更新手段は、検出
データなどの情報を経時的に更新する。移動状態量予測
手段（カルマンフィルタ）は、それに基づいて計測原点
を更新すると共に、計測原点からの移動距離ｘ，時系列
情報から求めたおよその障害物速度ｖないしその加速度
ａに基づいて、次の検知時刻での予測位置Ｐｏｓハット
ｎｅｘｔを求める。障害物時系列情報記憶手段（ネット
ワーク型データ構造）では、それら予測値などから障害
物の現在位置Ｐｏｓ，予測位置Ｐｏｓハットｎｅｘｔ，
方位Ｄｉｒ，速度ｖなどの情報を障害物移動予測テーブ
ル作成手段に送り、そこでテーブル化されて画像処理Ｃ
ＰＵ３６などの利用に供される。

【０１２５】この実施例は上記の如く構成したので、自
車の走行にとって障害となる前走車などの移動体や道路
外の静止障害物などを精度良く検出することができる。
また前走車などの移動体の情報をリアルタイムに検出
し、移動体の運動パラメータを予測するので、衝突の警
告ないしは回避動作などをより的確に行うことができる
と共に、必要に応じて他のサブシステムにその情報を提
供することができる。更に、カーブ路（曲線路）などを
通行するときも、道路内外の障害物を的確に判別して真
に自車の走行の妨げとなるもののみを検出することがで
きる。

【０１２６】更に、演算量の少ない直線運動モデルを使
用しながら、所定個数のデータを処理したら最新検知位
置付近に計測原点を更新するようにしたので、曲線路を
含む走行環境においても移動距離の誤差が経時的に蓄積
することがなく、よって予測しようとする位置、速度、
加速度などの運動パラメータの誤差として表れることが
ない。

【０１２７】尚、実施例は上記の如く構成したが、画像
処理とレーダの検知範囲とは異なるために、障害物の位
置によって判定条件を変え、レーダの検知データから求
めた移動体らしさに反映させても良い。即ち、検知範囲
と検知された位置との関係で、移動体らしさの評価を変
えても良い。これにより、移動する障害物と停止してい
る障害物の区別を道路（走行路）の形状を考慮した形で
扱うことができる。

【０１２８】また、レーダと画像処理の検知範囲が異な
る場合、特にレーダの検知範囲が画像処理より遠い場
合、画像処理の範囲外に検知された障害物は、道路に対
する位置が分からないことになる。高速道路のような道
路形状が長い距離にわたって余り変化しない場合は、過
去の（通過してきた）道路構造が変わらない、例えば２
車線から３車線への変化がない、ならば、数１００ｍ分
の自車より後方と前方の最新の走行路区分線データか
ら、最小２乗法により画像処理の範囲外の走行路区分線
を補足して障害物の道路（走行路）に対する位置を計算
しても良い。

【０１２９】更に、毎回の画像処理による道路（走行
路）の形状も同様なデータが入力されるので、前回結果
と道路構造が変わず、自車の移動量も少ないならば、最
小２乗法による画像処理の範囲外の走行路区分線の補足
をキャンセルし、前回パラメータを用いて処理を高速化
しても良い。

【０１３０】更に、カルマンフィルタへの初期値として
与える速度を単純平均値としたが、加重平均値などでも
良い。

【０１３１】また、適応的に動作する例としてカルマン
フィルタを使用したが、それに限られるものではない。

【０１３２】また、上記において、視覚センサを単眼視
としたが、両眼視による距離測定を用いて前方レーダな
どを省略しても良い。またレーダユニット１２を車両前
方のみ設けたが、後方にも設けても良い。また、上記に
おいて、「走行路」「道路」「車線」などの名称を混用
したが、同一のものを意味することは言うまでもない。

【０１３３】

【発明の効果】請求項１項においては、自車の走行にと
って障害となる前走車などの障害物の位置や移動方向を
精度良く検出することができる。それによって、警告、
回避など必要な動作を的確に行うことができる。

【０１３４】請求項２項においては、線形予測フィルタ
の収束性が向上して推定精度を一層上げることができ
る。

【０１３５】請求項３項においては、演算量の少ない直
線運動モデルを用いながら、走行環境が曲線路を含むと
きも、自車の走行にとって障害となる前走車などの移動
体の運動パラメータを誤差なく予測することができる。

【図面の簡単な説明】

【図１】この発明に係る移動体の検出装置を備えた車両
を全体的に示す説明透視図である。

【図２】図１のレーダの詳細を示す説明図である。

【図３】図１に示すセンサおよびその処理などを詳細に
示すブロック図である。

【図４】図３ブロック図の構成をより機能的に示す図２
と同様の説明図である。

【図５】この発明に係る移動体の検出装置の動作を示す
メインフロー・チャートである。

【図６】図５フロー・チャートと同様にこの発明に係る
移動体の検出装置の動作を説明する機能ブロック図であ
る。

【図７】図５フロー・チャートの（障害物の）クラスタ
化を説明する説明図である。

【図８】図１の車両がカーブ路を通過するときのレーダ
ビームと障害物との関係を説明する説明図である。

【図９】実施例に係る走行路区分線によるクラスタの切
りわけを説明する説明図である。

【図１０】図５フロー・チャートの検知データのクラス
タ化作業を示すサブルーチン・フロー・チャートであ
る。

【図１１】図１０フロー・チャートで予定する走行環境
の説明図である。

【図１２】図１０フロー・チャートの作業を説明する説
明図である。

【図１３】同様に図１０フロー・チャートの作業を説明
する説明図である。

【図１４】同様に図１０フロー・チャートの作業を説明
する説明図である。

【図１５】同様に図１０フロー・チャートの作業を説明
する説明図である。

【図１６】この実施例で使用する移動情報のネットワー
クセグメントを示す説明図である。

【図１７】図１６でのノードの情報を示す説明図であ
る。

【図１８】図１６でのネットワークセグメントの情報を
示す説明図である。

【図１９】この実施例で用いた推定速度と推定方位を示
すシミュレーションデータ図である。

【図２０】図１６のネットワークセグメントの単純更新
を説明する説明図である。

【図２１】同様に図１６のネットワークセグメントの複
数障害物が存在するときの更新を説明する説明図であ
る。

【図２２】この実施例で用いる移動体らしさなる概念を
表現する要素の説明図である。

【図２３】その移動体らしさの算出手法を示す説明図で
ある。

【図２４】その移動体らしさの推移、前走車、停止障害
物を示すシミュレーションデータ図である。

【図２５】図５フロー・チャートのネットワーク更新処
理作業を示すサブルーチン・フロー・チャートである。

【図２６】図２５フロー・チャートの障害物位置予測作
業を示すサブルーチン・フロー・チャート部分である。

【図２７】この実施例での移動予測による位置推定を説
明する説明図である。

【図２８】図２７の移動予測手法による、実際の移動す
る車両における推定位置と入力位置とを示すシミュレー
ションデータ図である。

【図２９】同様に図２７の移動予測手法による、停止障
害物における推定位置と入力位置とを示すシミュレーシ
ョンデータ図である。

【図３０】前走車が定速から減速して停止する際の実際
の前走車の速度、この実施例における推定速度、１秒間
の予測位置における位置を示すシミュレーションデータ
図である。

【図３１】この実施例におけるカルマンフィルタの移動
体モデルを示す説明図である。

【図３２】この実施例におけるカルマンフィルタの初期
値として、真値５０ｋｍ／ｈに対して初期値１００ｋｍ
／ｈを与えた場合の収束状況を示すシミュレーションデ
ータ図である。

【図３３】同様に、この実施例におけるカルマンフィル
タの初期値として、セグメントの最新、最古のノードか
ら求めた値を与えた場合の収束状況を示すシミュレーシ
ョンデータ図である。

【図３４】図１に示す車両がカーブ路を走行するときの
計測距離を説明する説明図である。

【図３５】カルマンフィルタの計算での原点更新をした
場合としない場合の推定速度の優劣を示すシミュレーシ
ョンデータ図である。

【図３６】この実施例での「停止」「単独」「接続」
「非接続」の概念を説明する説明図

【図３７】図２５フロー・チャートに続くフロー・チャ
ート部分である。

【図３８】図３７フロー・チャートに続くフロー・チャ
ート部分である。

【図３９】図３８フロー・チャートのネットワーク発生
処理を示す説明図である。

【図４０】図３８フロー・チャートのネットワークリン
ク処理作業を示すサブルーチン・フロー・チャートの前
半部である。

【図４１】図４０の作業を説明する説明図である。

【図４２】図４０フロー・チャートの後半部である。

【図４３】図３８フロー・チャートの障害物位置、速
度、加速度推定作業を示すサブルーチン・フロー・チャ
ートの前半部である。

【図４４】図４３フロー・チャートの後半部である。

【図４５】図３８フロー・チャートに続く最終フロー・
チャート部分である。

【図４６】図５フロー・チャートの障害物情報出力処理
作業を示すサブルーチン・フロー・チャートである。

【図４７】図４６フロー・チャートの道路に対する障害
物の位置表現を示す説明図である。

【図４８】図４６フロー・チャートの障害物移動予測テ
ーブルを示す説明図である。

【図４９】図４６フロー・チャートの走行路区分線デー
タの回転作業を説明する説明図である。

【図５０】図４６フロー・チャートの対道路位置計算を
示す説明図である。

【図５１】図４６フロー・チャートの障害物移動予測テ
ーブルへの書込み処理作業を示す説明図である。

【図５２】図５フロー・チャートの障害物情報更新作業
を示すサブルーチン・フロー・チャートである。

【符号の説明】

１０ＣＣＤカメラ（モノクロＴＶカメラ）１２レーダユニット１４ヨーレートセンサ１６車速センサ１８舵角センサ３０画像処理ハードウェア３６画像処理ＣＰＵ３８画像評価ＣＰＵ４０レーダ処理回路４２レーダ評価ＣＰＵ４４軌跡推定ＣＰＵ４６軌跡追従制御ＣＰＵ５０行動計画意思決定ＣＰＵ５２舵角制御ＣＰＵ６０車速制御（加速度制御）ＣＰＵ

フロントページの続き (51)Int.Cl.⁶ 識別記号庁内整理番号ＦＩ技術表示箇所Ｇ０８Ｇ 1/16 ＡＨ０４Ｎ 7/18 Ｊ

Claims

【特許請求の範囲】

【請求項１】ａ．自己に対して相対的に移動する移動体
の位置を検出して出力する検出手段、ｂ．前記検出手段の出力に基づき、前記移動体の所定の
時点からの移動距離を算出する移動距離算出手段、およびｃ．前記算出された移動距離を入力として、前記移動体
の速度を少なくとも推定する線形予測フィルタ、からな
る移動体の検出装置であって、ｄ．前記検出手段の出力に基づき、前記移動体の所定期
間における平均速度を算出する平均速度算出手段、を備
え、前記線形予測フィルタは、前記算出された平均速度
を初期値として前記速度を少なくとも推定することを特
徴とする移動体の検出装置。
【請求項２】前記線形予測フィルタは、前記算出され
た平均速度を初期値として前記移動体の初速度を推定す
ることを特徴とする請求項１項記載の移動体の検出装
置。
【請求項３】ａ．自己に対して相対的に移動する移動体
の位置を検出して出力する検出手段、ｂ．前記検出手段の出力に基づき、前記移動体と基準位
置との間の距離を算出する距離算出手段、ｃ．前記算出された距離を入力とし、直線運動モデルに
基づいて前記移動体の運動パラメータを推定する手段、
からなる移動体の検出装置において、所定時間ごとに前
記基準位置を前記移動体の現在位置付近に更新するよう
にしたことを特徴とする移動体の検出装置。