JP2002048860A - 移動体追尾装置及び方法 - Google Patents
移動体追尾装置及び方法Info
- Publication number
- JP2002048860A JP2002048860A JP2000230824A JP2000230824A JP2002048860A JP 2002048860 A JP2002048860 A JP 2002048860A JP 2000230824 A JP2000230824 A JP 2000230824A JP 2000230824 A JP2000230824 A JP 2000230824A JP 2002048860 A JP2002048860 A JP 2002048860A
- Authority
- JP
- Japan
- Prior art keywords
- azimuth
- observation
- distance
- dimensional
- tracking target
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Abandoned
Links
Abstract
(57)【要約】
【課題】 ２次元レーダと画像センサにより観測した移
動体の２次元情報を用いて移動体の３次元運動を追尾可
能とする移動体追尾装置及び方法を得る。 【解決手段】 追尾対象としての移動体の距離及び方位
角を観測する２次元レーダ装置１と、追尾対象の俯角を
観測する画像センサ装置２と、追尾対象の３次元座標の
距離及び方位角予測値と、２次元レーダ装置によって観
測された距離及び方位角とを用いて追尾対象の３次元座
標の平滑値を演算する距離・方位角による平滑器７と、
追尾対象の３次元座標の俯角予測値と、画像センサ装置
によって観測された俯角を用いて追尾対象の３次元座標
の平滑値を演算する俯角による平滑器１５とを備える。
動体の２次元情報を用いて移動体の３次元運動を追尾可
能とする移動体追尾装置及び方法を得る。 【解決手段】 追尾対象としての移動体の距離及び方位
角を観測する２次元レーダ装置１と、追尾対象の俯角を
観測する画像センサ装置２と、追尾対象の３次元座標の
距離及び方位角予測値と、２次元レーダ装置によって観
測された距離及び方位角とを用いて追尾対象の３次元座
標の平滑値を演算する距離・方位角による平滑器７と、
追尾対象の３次元座標の俯角予測値と、画像センサ装置
によって観測された俯角を用いて追尾対象の３次元座標
の平滑値を演算する俯角による平滑器１５とを備える。
Description
【０００１】
【発明の属する技術分野】この発明は、レーダ及び画像
センサにて観測した人、自転車、車両等の路上の移動体
の位置、速度等の運動諸元を推定する移動体追尾装置及
び方法に関するものである。
センサにて観測した人、自転車、車両等の路上の移動体
の位置、速度等の運動諸元を推定する移動体追尾装置及
び方法に関するものである。
【０００２】
【従来の技術】図１１は、例えば、"Multisensor Track
ing of a Maneuvering Target in Clutter"，IEEE Tran
s. on Aerospace and Electronic Systems， Vol. AES
−25，No.２，March 1989，pp176−189に示された従来
の複数センサを使用した移動体追尾装置を示す構成図で
ある。図１１において、４５は追尾対象の３次元位置を
観測する３次元レーダ装置、２は追尾対象の画像を観測
してそれらの俯角及び方位角を観測する画像センサ装置
である。
ing of a Maneuvering Target in Clutter"，IEEE Tran
s. on Aerospace and Electronic Systems， Vol. AES
−25，No.２，March 1989，pp176−189に示された従来
の複数センサを使用した移動体追尾装置を示す構成図で
ある。図１１において、４５は追尾対象の３次元位置を
観測する３次元レーダ装置、２は追尾対象の画像を観測
してそれらの俯角及び方位角を観測する画像センサ装置
である。
【０００３】４６は３次元レーダ装置４５により観測さ
れた観測ベクトルのうち、追尾対象が存在する可能性の
高い観測ベクトルを位置の尤度をもとに選択する位置相
関器、５は位置相関器４６で何れの追尾対象とも相関の
取れなかった連続する２点の位置から対象の位置と速度
の初期値と、平滑誤差共分散行列の初期値を算出する初
期値算出器、４７は後述する第２の遅延回路５１から出
力された予測誤差共分散行列（現時刻より１サンプリン
グ前に算出された予測誤差共分散行列）と予め設定され
た３次元レーダ装置４５の観測誤差共分散行列からゲイ
ン行列を算出する位置ゲイン行列算出器、４８は位置相
関器４６により選択された観測ベクトルと、後述する第
１の遅延回路２２から出力された予測位置（現時刻より
１サンプリング前に算出された予測位置）及び位置ゲイ
ン行列算出器４７により算出されたゲイン行列を使用し
て、追尾対象の位置及び速度の平滑値を算出する位置に
よる平滑器である。
れた観測ベクトルのうち、追尾対象が存在する可能性の
高い観測ベクトルを位置の尤度をもとに選択する位置相
関器、５は位置相関器４６で何れの追尾対象とも相関の
取れなかった連続する２点の位置から対象の位置と速度
の初期値と、平滑誤差共分散行列の初期値を算出する初
期値算出器、４７は後述する第２の遅延回路５１から出
力された予測誤差共分散行列（現時刻より１サンプリン
グ前に算出された予測誤差共分散行列）と予め設定され
た３次元レーダ装置４５の観測誤差共分散行列からゲイ
ン行列を算出する位置ゲイン行列算出器、４８は位置相
関器４６により選択された観測ベクトルと、後述する第
１の遅延回路２２から出力された予測位置（現時刻より
１サンプリング前に算出された予測位置）及び位置ゲイ
ン行列算出器４７により算出されたゲイン行列を使用し
て、追尾対象の位置及び速度の平滑値を算出する位置に
よる平滑器である。
【０００４】４９は第１の遅延回路２２から出力された
予測位置（現時刻より１サンプリング前に算出された予
測位置）、位置ゲイン行列算出器４７により算出された
ゲイン行列、及び後述する第２の遅延回路１１から出力
された予測誤差共分散行列（現時刻より１サンプリング
前に算出された予測誤差共分散行列）を用いて、追尾対
象の平滑誤差を評価する平滑誤差共分散行列を算出する
位置データによる平滑誤差評価器、１０は初期値算出器
５により算出された平滑誤差共分散行列または位置デー
タによる平滑誤差評価器４９により算出された平滑誤差
共分散行列から予測誤差を評価する予測誤差共分散行列
を算出する予測誤差評価器Ａである。
予測位置（現時刻より１サンプリング前に算出された予
測位置）、位置ゲイン行列算出器４７により算出された
ゲイン行列、及び後述する第２の遅延回路１１から出力
された予測誤差共分散行列（現時刻より１サンプリング
前に算出された予測誤差共分散行列）を用いて、追尾対
象の平滑誤差を評価する平滑誤差共分散行列を算出する
位置データによる平滑誤差評価器、１０は初期値算出器
５により算出された平滑誤差共分散行列または位置デー
タによる平滑誤差評価器４９により算出された平滑誤差
共分散行列から予測誤差を評価する予測誤差共分散行列
を算出する予測誤差評価器Ａである。
【０００５】１１は予測誤差評価器Ａ１０により算出さ
れた予測誤差共分散行列を１サンプリング時間だけ遅延
する第２の遅延回路、５０は第２の遅延回路１１の出力
する予測誤差共分散行列と、予め設定された３次元レー
ダ装置４５の観測誤差共分散行列を入力し、追尾対象の
存在位置の確率分布を算出する位置確率分布算出器であ
る。
れた予測誤差共分散行列を１サンプリング時間だけ遅延
する第２の遅延回路、５０は第２の遅延回路１１の出力
する予測誤差共分散行列と、予め設定された３次元レー
ダ装置４５の観測誤差共分散行列を入力し、追尾対象の
存在位置の確率分布を算出する位置確率分布算出器であ
る。
【０００６】３１は画像センサ装置２から画像センサ観
測座標系における追尾対象の俯角及び方位角を入力し、
これらの観測ベクトルのうち、追尾対象が存在する可能
性の高い観測ベクトルを、後述する俯角・方位角確率分
布算出器３７から入力した追尾対象の俯角及び方位角の
確率分布をもとに選択する俯角・方位角相関器、３２は
後述する第３の遅延回路１９から出力される現時刻より
１サンプリング前に算出された予測誤差共分散行列と、
後述する俯角・方位角観測行列算出器３４から出力され
る俯角及び方位角の観測行列と、画像センサ装置２の観
測誤共分散行列とから俯角及び方位角のゲイン行列を算
出する俯角・方位角ゲイン行列算出器である。
測座標系における追尾対象の俯角及び方位角を入力し、
これらの観測ベクトルのうち、追尾対象が存在する可能
性の高い観測ベクトルを、後述する俯角・方位角確率分
布算出器３７から入力した追尾対象の俯角及び方位角の
確率分布をもとに選択する俯角・方位角相関器、３２は
後述する第３の遅延回路１９から出力される現時刻より
１サンプリング前に算出された予測誤差共分散行列と、
後述する俯角・方位角観測行列算出器３４から出力され
る俯角及び方位角の観測行列と、画像センサ装置２の観
測誤共分散行列とから俯角及び方位角のゲイン行列を算
出する俯角・方位角ゲイン行列算出器である。
【０００７】３３は俯角・方位角相関器３１により選択
された観測ベクトル、後述する第１の座標変換器２４か
ら出力される俯角及び方位角の予測ベクトルと３次元予
測ベクトル、俯角・方位角ゲイン行列算出器３２により
算出された俯角及び方位角のゲイン行列を用いて追尾対
象の３次元位置及び速度の平滑ベクトルを算出する俯角
・方位角による平滑器である。
された観測ベクトル、後述する第１の座標変換器２４か
ら出力される俯角及び方位角の予測ベクトルと３次元予
測ベクトル、俯角・方位角ゲイン行列算出器３２により
算出された俯角及び方位角のゲイン行列を用いて追尾対
象の３次元位置及び速度の平滑ベクトルを算出する俯角
・方位角による平滑器である。
【０００８】３４は第１の座標変換器２４から出力され
る俯角及び方位角の予測ベクトルを入力し、俯角及び方
位角の観測行列を算出する俯角・方位角観測行列算出
器、３５は俯角・方位角データによる平滑誤差評価器で
あって、第１の座標変換器２４から出力される俯角及び
方位角の予測ベクトルと、俯角・方位角観測行列算出器
３４から出力される俯角及び方位角の観測行列と、俯角
・方位角ゲイン行列算出器３２から出力される俯角及び
方位角のゲイン行列と、後述する第３の遅延回路１９か
ら出力される現時刻より１サンプリング前に算出された
予測誤差共分散行列とを用いて追尾対象の３次元位置及
び速度の平滑誤差を評価する平滑誤差共分散行列を算出
する。
る俯角及び方位角の予測ベクトルを入力し、俯角及び方
位角の観測行列を算出する俯角・方位角観測行列算出
器、３５は俯角・方位角データによる平滑誤差評価器で
あって、第１の座標変換器２４から出力される俯角及び
方位角の予測ベクトルと、俯角・方位角観測行列算出器
３４から出力される俯角及び方位角の観測行列と、俯角
・方位角ゲイン行列算出器３２から出力される俯角及び
方位角のゲイン行列と、後述する第３の遅延回路１９か
ら出力される現時刻より１サンプリング前に算出された
予測誤差共分散行列とを用いて追尾対象の３次元位置及
び速度の平滑誤差を評価する平滑誤差共分散行列を算出
する。
【０００９】３６は俯角・方位角データによる平滑誤差
評価器３５により算出された平滑誤差共分散行列を入力
し、追尾対象の３次元位置及び速度の予測誤差を評価す
る予測誤差共分散行列を算出する予測誤差評価器Ｂであ
る。１９は予測誤差評価器Ｂ１８により算出された予測
誤差共分散行列を１サンプリング時間だけ遅延して出力
する第３の遅延回路、３７は第３の遅延回路１９の出力
する予測誤差共分散行列と、俯角・方位角観測行列算出
器３４で算出した俯角及び方位角の観測行列とを入力
し、予め設定された画像センサ装置２の観測誤差共分差
行列に基づいて追尾対象の俯角及び方位角の確率分布を
算出する俯角・方位角確率分布算出器である。
評価器３５により算出された平滑誤差共分散行列を入力
し、追尾対象の３次元位置及び速度の予測誤差を評価す
る予測誤差共分散行列を算出する予測誤差評価器Ｂであ
る。１９は予測誤差評価器Ｂ１８により算出された予測
誤差共分散行列を１サンプリング時間だけ遅延して出力
する第３の遅延回路、３７は第３の遅延回路１９の出力
する予測誤差共分散行列と、俯角・方位角観測行列算出
器３４で算出した俯角及び方位角の観測行列とを入力
し、予め設定された画像センサ装置２の観測誤差共分差
行列に基づいて追尾対象の俯角及び方位角の確率分布を
算出する俯角・方位角確率分布算出器である。
【００１０】２１は初期値算出器５、位置による平滑器
４８、俯角・方位角による平滑器３３のいずれかから出
力される追尾対象の３次元位置及び速度から成る３次元
平滑ベクトルと、予め設定された追尾対象の運動モデル
を用いて現時刻より１サンプリング後の追尾対象の３次
元予測位置及び予測速度から成る３次元予測ベクトルを
算出する位置・速度予測器、２２は位置・速度予測器２
１により算出された追尾対象の３次元予測ベクトルを１
サンプリング時間だけ遅延して出力する第１の遅延回
路、２４は第１の遅延回路２２の出力する現時刻より１
サンプリング前の追尾対象の３次元予測ベクトルを俯角
及び方位角の予測ベクトルに座標変換し、これと変換前
の３次元予測ベクトルを出力する第１の座標変換器であ
る。
４８、俯角・方位角による平滑器３３のいずれかから出
力される追尾対象の３次元位置及び速度から成る３次元
平滑ベクトルと、予め設定された追尾対象の運動モデル
を用いて現時刻より１サンプリング後の追尾対象の３次
元予測位置及び予測速度から成る３次元予測ベクトルを
算出する位置・速度予測器、２２は位置・速度予測器２
１により算出された追尾対象の３次元予測ベクトルを１
サンプリング時間だけ遅延して出力する第１の遅延回
路、２４は第１の遅延回路２２の出力する現時刻より１
サンプリング前の追尾対象の３次元予測ベクトルを俯角
及び方位角の予測ベクトルに座標変換し、これと変換前
の３次元予測ベクトルを出力する第１の座標変換器であ
る。
【００１１】次に動作について説明する。位置相関器４
６は、３次元レーダ装置４５から観測ベクトルを受け取
ると、その観測ベクトルが追尾対象に係る観測ベクトル
である可能性が高い場合にのみ有効なデータとして採用
するため、位置確率分布算出器５０により算出された存
在位置の確率分布を用いて、その観測ベクトルの有効性
を判定し、有効性が認められると、その観測ベクトルを
位置データによる平滑器４８に出力する。
６は、３次元レーダ装置４５から観測ベクトルを受け取
ると、その観測ベクトルが追尾対象に係る観測ベクトル
である可能性が高い場合にのみ有効なデータとして採用
するため、位置確率分布算出器５０により算出された存
在位置の確率分布を用いて、その観測ベクトルの有効性
を判定し、有効性が認められると、その観測ベクトルを
位置データによる平滑器４８に出力する。
【００１２】位置ゲイン行列算出器４７では、第２の遅
延回路１１から出力された予測誤差共分散行列（現時刻
より１サンプリング前に算出された予測誤差共分散行
列）と、予め設定された３次元レーダ装置４５の観測雑
音共分散行列とから位置のゲイン行列を算出する。
延回路１１から出力された予測誤差共分散行列（現時刻
より１サンプリング前に算出された予測誤差共分散行
列）と、予め設定された３次元レーダ装置４５の観測雑
音共分散行列とから位置のゲイン行列を算出する。
【００１３】位置による平滑器４８は、位置相関器４６
により選択された観測ベクトルと、第１の遅延回路２２
から出力された予測位置（現時刻より１サンプリング前
に算出された予測位置）と、位置ゲイン行列算出器４７
により算出されたゲイン行列とを使用して、追尾対象の
３次元位置及び速度の平滑値を算出する。
により選択された観測ベクトルと、第１の遅延回路２２
から出力された予測位置（現時刻より１サンプリング前
に算出された予測位置）と、位置ゲイン行列算出器４７
により算出されたゲイン行列とを使用して、追尾対象の
３次元位置及び速度の平滑値を算出する。
【００１４】位置データによる平滑誤差評価器４９は、
第１の遅延回路２２から出力された予測位置（現時刻よ
り１サンプリング前に算出された予測位置）と、位置ゲ
イン行列算出器４７により算出されたゲイン行列と、位
置相関器４６により選択された位置観測ベクトルと、第
２の遅延回路１１から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）とを用いて、追尾対象の位置及び速度の平滑
誤差を評価する平滑誤差共分散行列を算出する。
第１の遅延回路２２から出力された予測位置（現時刻よ
り１サンプリング前に算出された予測位置）と、位置ゲ
イン行列算出器４７により算出されたゲイン行列と、位
置相関器４６により選択された位置観測ベクトルと、第
２の遅延回路１１から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）とを用いて、追尾対象の位置及び速度の平滑
誤差を評価する平滑誤差共分散行列を算出する。
【００１５】そして、予測誤差評価器Ａ１０は、位置デ
ータによる平滑誤差評価器４９から出力された平滑誤差
共分散行列と、初期値算出器５から出力された追尾対象
の平滑誤差共分散行列から位置及び速度の予測誤差を評
価する予測誤差共分散行列を算出する。また、位置確率
分布算出器５０は、予め設定された追尾対象の運動モデ
ルを用いて、第１の遅延回路２２が出力する予測位置
（現時刻より１サンプリング前に算出された予測位置）
から追尾対象の位置を算出し、その位置を中心にして、
第２の遅延回路１１が出力する予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）から追尾対象の存在位置の確率分布を算出する。
ータによる平滑誤差評価器４９から出力された平滑誤差
共分散行列と、初期値算出器５から出力された追尾対象
の平滑誤差共分散行列から位置及び速度の予測誤差を評
価する予測誤差共分散行列を算出する。また、位置確率
分布算出器５０は、予め設定された追尾対象の運動モデ
ルを用いて、第１の遅延回路２２が出力する予測位置
（現時刻より１サンプリング前に算出された予測位置）
から追尾対象の位置を算出し、その位置を中心にして、
第２の遅延回路１１が出力する予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）から追尾対象の存在位置の確率分布を算出する。
【００１６】画像センサ装置２では、画像から画像セン
サ観測座標系で移動体の俯角及び方位角を検出し、その
結果（観測ベクトル）を俯角・方位角相関器３１に出力
する。俯角・方位角相関器３１では、画像センサ装置２
から観測ベクトルを受け取ると、その観測ベクトルから
追尾対象と相関のある観測ベクトルを選択するため、俯
角・方位角確率分布算出器３７により算出された俯角及
び方位角の確率分布を用いて、追尾対象と相関の可能性
のある観測ベクトルを選択し、俯角・方位角による平滑
器３３に出力する。
サ観測座標系で移動体の俯角及び方位角を検出し、その
結果（観測ベクトル）を俯角・方位角相関器３１に出力
する。俯角・方位角相関器３１では、画像センサ装置２
から観測ベクトルを受け取ると、その観測ベクトルから
追尾対象と相関のある観測ベクトルを選択するため、俯
角・方位角確率分布算出器３７により算出された俯角及
び方位角の確率分布を用いて、追尾対象と相関の可能性
のある観測ベクトルを選択し、俯角・方位角による平滑
器３３に出力する。
【００１７】俯角・方位角ゲイン行列算出器３２では、
第３の遅延回路１９から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）と、俯角・方位角観測行列算出器３４から出
力された画像センサ装置２の観測行列と予め設定された
画像センサ装置２の観測雑音共分散行列とから俯角及び
方位角のゲイン行列を算出する。
第３の遅延回路１９から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）と、俯角・方位角観測行列算出器３４から出
力された画像センサ装置２の観測行列と予め設定された
画像センサ装置２の観測雑音共分散行列とから俯角及び
方位角のゲイン行列を算出する。
【００１８】俯角・方位角による平滑器３３では、俯角
・方位角相関器３１から出力された追尾対象の観測ベク
トルと、第１の座標変換器２４から出力された俯角及び
方位角の予測値（現時刻より１サンプリング前に算出さ
れた予測値）と、俯角・方位角ゲイン行列算出器３２に
より算出されたゲイン行列とを用いて、追尾対象の位置
及び速度の平滑ベクトルを算出する。
・方位角相関器３１から出力された追尾対象の観測ベク
トルと、第１の座標変換器２４から出力された俯角及び
方位角の予測値（現時刻より１サンプリング前に算出さ
れた予測値）と、俯角・方位角ゲイン行列算出器３２に
より算出されたゲイン行列とを用いて、追尾対象の位置
及び速度の平滑ベクトルを算出する。
【００１９】俯角・方位角観測行列算出器３４では、第
１の座標変換器２４から出力された俯角及び方位角の予
測値（現時刻より１サンプリング前に算出された予測
値）を入力し、画像センサ装置２の観測行列を算出す
る。俯角・方位角データによる平滑誤差評価器３５で
は、第１の座標変換器２４から出力された俯角及び方位
角の予測値（現時刻より１サンプリング前に算出された
予測値）と、俯角・方位角観測行列算出器３４から出力
された画像センサ装置２の観測行列と、俯角・方位角ゲ
イン行列算出器３２から出力されたゲイン行列と、第３
の遅延回路１９から出力された予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）とから平滑誤差共分散行列を算出する。予測誤差
評価器Ｂ３６では、俯角・方位角データによる平滑誤差
評価器３５から出力された平滑誤差共分散行列を入力
し、予測誤差共分散行列を算出する。
１の座標変換器２４から出力された俯角及び方位角の予
測値（現時刻より１サンプリング前に算出された予測
値）を入力し、画像センサ装置２の観測行列を算出す
る。俯角・方位角データによる平滑誤差評価器３５で
は、第１の座標変換器２４から出力された俯角及び方位
角の予測値（現時刻より１サンプリング前に算出された
予測値）と、俯角・方位角観測行列算出器３４から出力
された画像センサ装置２の観測行列と、俯角・方位角ゲ
イン行列算出器３２から出力されたゲイン行列と、第３
の遅延回路１９から出力された予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）とから平滑誤差共分散行列を算出する。予測誤差
評価器Ｂ３６では、俯角・方位角データによる平滑誤差
評価器３５から出力された平滑誤差共分散行列を入力
し、予測誤差共分散行列を算出する。
【００２０】俯角・方位角確率分布算出器３７は、第３
の遅延回路１９から出力された予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）と、俯角・方位角観測行列算出器３４から出力さ
れた画像センサ装置２の観測行列とを入力し、予め設定
された画像センサ装置２の観測誤差共分散行列を用い
て、現時刻での存在が予測される追尾対象の俯角及び方
位角の確率分布を算出する。
の遅延回路１９から出力された予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）と、俯角・方位角観測行列算出器３４から出力さ
れた画像センサ装置２の観測行列とを入力し、予め設定
された画像センサ装置２の観測誤差共分散行列を用い
て、現時刻での存在が予測される追尾対象の俯角及び方
位角の確率分布を算出する。
【００２１】上記のようにして、位置による平滑器４８
または俯角・方位角による平滑器３３が追尾対象の位置
及び速度の平滑値を算出すると、位置・速度予測器２１
は、その平滑値と予め設定された追尾対象の運動モデル
を用いて、現時刻より１サンプリング後の追尾対象の予
測位置及び速度を算出する。
または俯角・方位角による平滑器３３が追尾対象の位置
及び速度の平滑値を算出すると、位置・速度予測器２１
は、その平滑値と予め設定された追尾対象の運動モデル
を用いて、現時刻より１サンプリング後の追尾対象の予
測位置及び速度を算出する。
【００２２】
【発明が解決しようとする課題】従来の移動体追尾装置
は以上のように構成されているので、３次元空間の移動
体を追尾するには、移動体の３次元位置を計測する３次
元レーダ装置４５を必要とし、３次元位置を計測できな
い２次元レーダと画像センサを用いて３次元運動を追尾
することができないという課題があった。
は以上のように構成されているので、３次元空間の移動
体を追尾するには、移動体の３次元位置を計測する３次
元レーダ装置４５を必要とし、３次元位置を計測できな
い２次元レーダと画像センサを用いて３次元運動を追尾
することができないという課題があった。
【００２３】この発明は上記のような課題を解決するた
めになされたもので、２次元レーダと画像センサにより
観測した移動体の２次元情報を用いて移動体の３次元運
動を追尾可能とする移動体追尾装置及び方法を得ること
を目的とする。
めになされたもので、２次元レーダと画像センサにより
観測した移動体の２次元情報を用いて移動体の３次元運
動を追尾可能とする移動体追尾装置及び方法を得ること
を目的とする。
【００２４】
【課題を解決するための手段】この発明に係る移動体追
尾装置は、追尾対象としての移動体の距離及び方位角を
観測する第１の観測手段と、追尾対象の俯角を観測する
第２の観測手段と、追尾対象の３次元座標の距離及び方
位角予測値と、前記第１の観測手段によって観測された
距離及び方位角とを用いて追尾対象の３次元座標の平滑
値を演算する第１の平滑手段と、追尾対象の３次元座標
の俯角予測値と、前記第２の観測手段によって観測され
た俯角を用いて追尾対象の３次元座標の平滑値を演算す
る第２の平滑手段とを備えたものである。
尾装置は、追尾対象としての移動体の距離及び方位角を
観測する第１の観測手段と、追尾対象の俯角を観測する
第２の観測手段と、追尾対象の３次元座標の距離及び方
位角予測値と、前記第１の観測手段によって観測された
距離及び方位角とを用いて追尾対象の３次元座標の平滑
値を演算する第１の平滑手段と、追尾対象の３次元座標
の俯角予測値と、前記第２の観測手段によって観測され
た俯角を用いて追尾対象の３次元座標の平滑値を演算す
る第２の平滑手段とを備えたものである。
【００２５】また、追尾対象としての移動体の距離及び
方位角を観測する第１の観測手段と、追尾対象の方位角
を観測する第２の観測手段と、追尾対象の３次元座標の
距離及び方位角予測値と、前記第１の観測手段によって
観測された距離及び方位角とを用いて追尾対象の３次元
座標の平滑値を演算する第１の平滑手段と、追尾対象の
３次元座標の方位角予測値と、前記第２の観測手段によ
って観測された方位角を用いて追尾対象の３次元座標の
平滑値を演算する第２の平滑手段とを備えたものであ
る。
方位角を観測する第１の観測手段と、追尾対象の方位角
を観測する第２の観測手段と、追尾対象の３次元座標の
距離及び方位角予測値と、前記第１の観測手段によって
観測された距離及び方位角とを用いて追尾対象の３次元
座標の平滑値を演算する第１の平滑手段と、追尾対象の
３次元座標の方位角予測値と、前記第２の観測手段によ
って観測された方位角を用いて追尾対象の３次元座標の
平滑値を演算する第２の平滑手段とを備えたものであ
る。
【００２６】また、追尾対象としての移動体の距離及び
方位角を観測する第１の観測手段と、追尾対象の俯角及
び方位角を観測する第２の観測手段と、追尾対象の３次
元座標の距離及び方位角予測値と、前記第１の観測手段
によって観測された距離及び方位角を用いて追尾対象の
３次元座標の平滑値を演算する第１の平滑手段と、追尾
対象の３次元座標の方位角予測値と、前記第２の観測手
段によって観測された俯角及び方位角を用いて追尾対象
の３次元座標の平滑値を演算する第２の平滑手段とを備
えたものである。
方位角を観測する第１の観測手段と、追尾対象の俯角及
び方位角を観測する第２の観測手段と、追尾対象の３次
元座標の距離及び方位角予測値と、前記第１の観測手段
によって観測された距離及び方位角を用いて追尾対象の
３次元座標の平滑値を演算する第１の平滑手段と、追尾
対象の３次元座標の方位角予測値と、前記第２の観測手
段によって観測された俯角及び方位角を用いて追尾対象
の３次元座標の平滑値を演算する第２の平滑手段とを備
えたものである。
【００２７】また、追尾対象としての移動体の距離及び
方位角を観測する第１の観測手段と、追尾対象の距離及
び方位角を観測する第２の観測手段と、追尾対象の３次
元座標の距離及び方位角予測値と、前記第１の観測手段
によって観測された距離及び方位角を用いて追尾対象の
３次元座標の平滑値を演算する第１の平滑手段と、追尾
対象の３次元座標の距離及び方位角予測値と、前記第２
の観測手段によって観測された距離及び方位角を用いて
追尾対象の３次元座標の平滑値を演算する第２の平滑手
段とを備えたものである。
方位角を観測する第１の観測手段と、追尾対象の距離及
び方位角を観測する第２の観測手段と、追尾対象の３次
元座標の距離及び方位角予測値と、前記第１の観測手段
によって観測された距離及び方位角を用いて追尾対象の
３次元座標の平滑値を演算する第１の平滑手段と、追尾
対象の３次元座標の距離及び方位角予測値と、前記第２
の観測手段によって観測された距離及び方位角を用いて
追尾対象の３次元座標の平滑値を演算する第２の平滑手
段とを備えたものである。
【００２８】また、この発明に係る移動体追尾方法は、
追尾対象としての移動体の距離及び方位角を観測する第
１の観測ステップと、追尾対象の俯角を観測する第２の
観測ステップと、追尾対象の３次元座標の距離及び方位
角予測値と、前記第１の観測ステップによって観測され
た距離及び方位角を用いて追尾対象の３次元座標の平滑
値を演算する第１の平滑ステップと、追尾対象の３次元
座標の俯角予測値と、第２の観測手段によって観測され
た俯角を用いて追尾対象の３次元座標の平滑値を演算す
る第２の平滑ステップとを備えたものである。
追尾対象としての移動体の距離及び方位角を観測する第
１の観測ステップと、追尾対象の俯角を観測する第２の
観測ステップと、追尾対象の３次元座標の距離及び方位
角予測値と、前記第１の観測ステップによって観測され
た距離及び方位角を用いて追尾対象の３次元座標の平滑
値を演算する第１の平滑ステップと、追尾対象の３次元
座標の俯角予測値と、第２の観測手段によって観測され
た俯角を用いて追尾対象の３次元座標の平滑値を演算す
る第２の平滑ステップとを備えたものである。
【００２９】また、追尾対象としての移動体の距離及び
方位角を観測する第１の観測ステップと、追尾対象の方
位角を観測する第２の観測ステップと、追尾対象の３次
元座標の距離及び方位角予測値と、前記第１の観測ステ
ップによって観測された距離及び方位角を用いて追尾対
象の３次元座標の平滑値を演算する第１の平滑ステップ
と、追尾対象の３次元座標の方位角予測値と、前記第２
の観測ステップによって観測された方位角を用いて追尾
対象の３次元座標の平滑値を演算する第２の平滑ステッ
プとを備えたものである。
方位角を観測する第１の観測ステップと、追尾対象の方
位角を観測する第２の観測ステップと、追尾対象の３次
元座標の距離及び方位角予測値と、前記第１の観測ステ
ップによって観測された距離及び方位角を用いて追尾対
象の３次元座標の平滑値を演算する第１の平滑ステップ
と、追尾対象の３次元座標の方位角予測値と、前記第２
の観測ステップによって観測された方位角を用いて追尾
対象の３次元座標の平滑値を演算する第２の平滑ステッ
プとを備えたものである。
【００３０】また、追尾対象としての移動体の距離及び
方位角を観測する第１の観測ステップと、追尾対象の俯
角及び方位角を観測する第２の観測ステップと、追尾対
象の３次元座標の距離及び方位角予測値と、前記第１の
観測ステップによって観測された距離及び方位角を用い
て追尾対象の３次元座標の平滑値を演算する第１の平滑
ステップと、追尾対象の３次元座標の俯角予測値と、前
記第２の観測ステップによって観測された俯角及び方位
角を用いて追尾対象の３次元座標の平滑値を演算する第
２の平滑ステップとを備えたものである。
方位角を観測する第１の観測ステップと、追尾対象の俯
角及び方位角を観測する第２の観測ステップと、追尾対
象の３次元座標の距離及び方位角予測値と、前記第１の
観測ステップによって観測された距離及び方位角を用い
て追尾対象の３次元座標の平滑値を演算する第１の平滑
ステップと、追尾対象の３次元座標の俯角予測値と、前
記第２の観測ステップによって観測された俯角及び方位
角を用いて追尾対象の３次元座標の平滑値を演算する第
２の平滑ステップとを備えたものである。
【００３１】さらに、追尾対象としての移動体の距離及
び方位角を観測する第１の観測ステップと、追尾対象の
距離及び方位角を観測する第２の観測ステップと、追尾
対象の３次元座標の距離及び方位角予測値と、前記第１
の観測ステップによって観測された距離及び方位角を用
いて追尾対象の３次元座標の平滑値を演算する第１の平
滑ステップと、追尾対象の３次元座標の距離及び方位角
予測値と、前記第２の観測ステップによって観測された
距離及び方位角を用いて追尾対象の３次元座標の平滑値
を演算する第２の平滑ステップとを備えたものである。
び方位角を観測する第１の観測ステップと、追尾対象の
距離及び方位角を観測する第２の観測ステップと、追尾
対象の３次元座標の距離及び方位角予測値と、前記第１
の観測ステップによって観測された距離及び方位角を用
いて追尾対象の３次元座標の平滑値を演算する第１の平
滑ステップと、追尾対象の３次元座標の距離及び方位角
予測値と、前記第２の観測ステップによって観測された
距離及び方位角を用いて追尾対象の３次元座標の平滑値
を演算する第２の平滑ステップとを備えたものである。
【００３２】
【発明の実施の形態】実施の形態１．図１は、この発明
の実施の形態１による移動体追尾装置及び方法を説明す
るための構成図である。図１において、１は後述するレ
ーダ観測座標系において追尾対象としての移動体を観測
してそれらの距離及び方位角を出力する第１の観測手段
としての２次元レーダ装置、２は後述する画像センサ観
測座標系において移動体の画像を観測し、その代表点の
俯角を出力する第２の観測手段としての画像センサ装置
である。
の実施の形態１による移動体追尾装置及び方法を説明す
るための構成図である。図１において、１は後述するレ
ーダ観測座標系において追尾対象としての移動体を観測
してそれらの距離及び方位角を出力する第１の観測手段
としての２次元レーダ装置、２は後述する画像センサ観
測座標系において移動体の画像を観測し、その代表点の
俯角を出力する第２の観測手段としての画像センサ装置
である。
【００３３】３は２次元レーダ装置１からレーダ観測座
標系における追尾対象の距離及び方位角を入力し、これ
らの観測ベクトルのうち、追尾対象が存在する可能性の
高い観測ベクトルを、後述する距離・方位角確率分布算
出器１２から入力した追尾対象の距離及び方位角の確率
分布をもとに選択する距離・方位角相関器、４は距離・
方位角相関器３から同一対象からの初めの２回の観測で
得られた観測ベクトルを入力し、予め設定された観測開
始エリアの道路高を用いて３次元直交座標の位置に変換
する第２の座標変換器である。
標系における追尾対象の距離及び方位角を入力し、これ
らの観測ベクトルのうち、追尾対象が存在する可能性の
高い観測ベクトルを、後述する距離・方位角確率分布算
出器１２から入力した追尾対象の距離及び方位角の確率
分布をもとに選択する距離・方位角相関器、４は距離・
方位角相関器３から同一対象からの初めの２回の観測で
得られた観測ベクトルを入力し、予め設定された観測開
始エリアの道路高を用いて３次元直交座標の位置に変換
する第２の座標変換器である。
【００３４】５は第２の座標変換器４により変換された
３次元直交座標における連続する２点の位置から対象の
位置と速度の初期値と、平滑誤差共分散行列の初期値を
算出する初期値算出器、６は第２の遅延回路１１から出
力される現時刻より１サンプリング前に算出された予測
誤差共分散行列と、後述する距離・方位角観測行列算出
器８から出力されるレーダ観測行列と、予め設定された
２次元レーダ装置１の観測誤差共分散行列とから距離と
方位角のゲイン行列を算出する距離・方位角ゲイン行列
算出器である。
３次元直交座標における連続する２点の位置から対象の
位置と速度の初期値と、平滑誤差共分散行列の初期値を
算出する初期値算出器、６は第２の遅延回路１１から出
力される現時刻より１サンプリング前に算出された予測
誤差共分散行列と、後述する距離・方位角観測行列算出
器８から出力されるレーダ観測行列と、予め設定された
２次元レーダ装置１の観測誤差共分散行列とから距離と
方位角のゲイン行列を算出する距離・方位角ゲイン行列
算出器である。
【００３５】７は距離・方位角相関器３により選択され
た観測ベクトル、後述する第３の座標変換器２３から出
力される距離及び方位角の予測ベクトルと３次元予測ベ
クトル、距離・方位角ゲイン行列算出器６により算出さ
れた距離と方位角のゲイン行列を用いて追尾対象の３次
元位置及び速度の平滑ベクトルを算出する第１の平滑手
段としての距離・方位角による平滑器、８は第３の座標
変換器２３から出力される距離及び方位角の予測ベクト
ルを入力し、距離・方位角の観測行列を算出する距離・
方位角観測行列算出器である。
た観測ベクトル、後述する第３の座標変換器２３から出
力される距離及び方位角の予測ベクトルと３次元予測ベ
クトル、距離・方位角ゲイン行列算出器６により算出さ
れた距離と方位角のゲイン行列を用いて追尾対象の３次
元位置及び速度の平滑ベクトルを算出する第１の平滑手
段としての距離・方位角による平滑器、８は第３の座標
変換器２３から出力される距離及び方位角の予測ベクト
ルを入力し、距離・方位角の観測行列を算出する距離・
方位角観測行列算出器である。
【００３６】９は距離・方位角データによる平滑誤差評
価器であって、第３の座標変換器２３から出力される距
離及び方位角の予測ベクトルと、距離・方位角観測行列
算出器８から出力される距離・方位角の観測行列と、距
離・方位角ゲイン行列算出器６から出力される距離と方
位角のゲイン行列と、第２の遅延回路１１から出力され
る現時刻より１サンプリング前に算出された予測誤差共
分散行列とを用いて追尾対象の３次元位置及び速度の平
滑誤差を評価する平滑誤差共分散行列を算出する。
価器であって、第３の座標変換器２３から出力される距
離及び方位角の予測ベクトルと、距離・方位角観測行列
算出器８から出力される距離・方位角の観測行列と、距
離・方位角ゲイン行列算出器６から出力される距離と方
位角のゲイン行列と、第２の遅延回路１１から出力され
る現時刻より１サンプリング前に算出された予測誤差共
分散行列とを用いて追尾対象の３次元位置及び速度の平
滑誤差を評価する平滑誤差共分散行列を算出する。
【００３７】１０は初期値算出器５により算出された平
滑誤差共分散行列または距離・方位角データによる平滑
誤差評価器９により算出された平滑誤差共分散行列を入
力し、追尾対象の３次元位置及び速度の予測誤差を評価
する予測誤差共分散行列を算出する予測誤差評価器Ａで
ある。１１は予測誤差評価器Ａ１０により算出された予
測誤差共分散行列を１サンプリング時間だけ遅延して出
力する第２の遅延回路、１２は第２の遅延回路１１の出
力する予測誤差共分散行列と、距離・方位角観測行列算
出器８で算出した距離・方位角の観測行列と、予め設定
された２次元レーダ装置１の観測誤差共分散行列を入力
し、追尾対象の距離及び方位角の確率分布を算出する距
離・方位角確率分布算出器である。
滑誤差共分散行列または距離・方位角データによる平滑
誤差評価器９により算出された平滑誤差共分散行列を入
力し、追尾対象の３次元位置及び速度の予測誤差を評価
する予測誤差共分散行列を算出する予測誤差評価器Ａで
ある。１１は予測誤差評価器Ａ１０により算出された予
測誤差共分散行列を１サンプリング時間だけ遅延して出
力する第２の遅延回路、１２は第２の遅延回路１１の出
力する予測誤差共分散行列と、距離・方位角観測行列算
出器８で算出した距離・方位角の観測行列と、予め設定
された２次元レーダ装置１の観測誤差共分散行列を入力
し、追尾対象の距離及び方位角の確率分布を算出する距
離・方位角確率分布算出器である。
【００３８】１３は画像センサ装置２から画像センサ観
測座標系における追尾対象の俯角を入力し、これらの観
測ベクトルのうち、追尾対象が存在する可能性の高い観
測ベクトルを、後述する俯角確率分布算出器２０から入
力した追尾対象の俯角の確率分布をもとに選択する俯角
相関器、１４は後述する第３の遅延回路１９から出力さ
れる現時刻より１サンプリング前に算出された予測誤差
共分散行列と、後述する俯角観測行列算出器１６から出
力される俯角の観測行列と、予め設定された画像センサ
装置２の観測誤差共分散行列とから俯角のゲイン行列を
算出する俯角ゲイン行列算出器である。
測座標系における追尾対象の俯角を入力し、これらの観
測ベクトルのうち、追尾対象が存在する可能性の高い観
測ベクトルを、後述する俯角確率分布算出器２０から入
力した追尾対象の俯角の確率分布をもとに選択する俯角
相関器、１４は後述する第３の遅延回路１９から出力さ
れる現時刻より１サンプリング前に算出された予測誤差
共分散行列と、後述する俯角観測行列算出器１６から出
力される俯角の観測行列と、予め設定された画像センサ
装置２の観測誤差共分散行列とから俯角のゲイン行列を
算出する俯角ゲイン行列算出器である。
【００３９】１５は俯角相関器１３により選択された観
測ベクトル、第１の座標変換器２４から出力される俯角
の予測ベクトルと３次元予測ベクトル、俯角ゲイン行列
算出器１４により算出された俯角のゲイン行列を用いて
追尾対象の３次元位置及び速度の平滑ベクトルを算出す
る第２の平滑手段としての俯角による平滑器、１６は第
１の座標変換器２４から出力される俯角の予測ベクトル
を入力し、俯角の観測行列を算出する俯角観測行列算出
器である。
測ベクトル、第１の座標変換器２４から出力される俯角
の予測ベクトルと３次元予測ベクトル、俯角ゲイン行列
算出器１４により算出された俯角のゲイン行列を用いて
追尾対象の３次元位置及び速度の平滑ベクトルを算出す
る第２の平滑手段としての俯角による平滑器、１６は第
１の座標変換器２４から出力される俯角の予測ベクトル
を入力し、俯角の観測行列を算出する俯角観測行列算出
器である。
【００４０】１７は俯角データによる平滑誤差評価器で
あって、第１の座標変換器２４から出力される俯角の予
測ベクトルと、俯角観測行列算出器１６から出力される
俯角の観測行列と、俯角ゲイン行列算出器１４から出力
される俯角のゲイン行列と、後述する第３の遅延回路１
９から出力される現時刻より１サンプリング前に算出さ
れた予測誤差共分散行列とを用いて追尾対象の３次元位
置及び速度の平滑誤差を評価する平滑誤差共分散行列を
算出する。
あって、第１の座標変換器２４から出力される俯角の予
測ベクトルと、俯角観測行列算出器１６から出力される
俯角の観測行列と、俯角ゲイン行列算出器１４から出力
される俯角のゲイン行列と、後述する第３の遅延回路１
９から出力される現時刻より１サンプリング前に算出さ
れた予測誤差共分散行列とを用いて追尾対象の３次元位
置及び速度の平滑誤差を評価する平滑誤差共分散行列を
算出する。
【００４１】１８は俯角データによる平滑誤差評価器１
７により算出された平滑誤差共分散行列を入力し、追尾
対象の３次元位置及び速度の予測誤差を評価する予測誤
差共分散行列を算出する予測誤差評価器Ｂである。１９
は予測誤差評価器Ｂ１８により算出された予測誤差共分
散行列を１サンプリング時間だけ遅延して出力する第３
の遅延回路、２０は第３の遅延回路１９の出力する予測
誤差共分散行列と、俯角観測行列算出器１６で算出した
俯角の観測行列と、予め設定された画像センサ装置２の
観測誤差共分散行列を入力し、追尾対象の俯角の確率分
布を算出する俯角確率分布算出器である。
７により算出された平滑誤差共分散行列を入力し、追尾
対象の３次元位置及び速度の予測誤差を評価する予測誤
差共分散行列を算出する予測誤差評価器Ｂである。１９
は予測誤差評価器Ｂ１８により算出された予測誤差共分
散行列を１サンプリング時間だけ遅延して出力する第３
の遅延回路、２０は第３の遅延回路１９の出力する予測
誤差共分散行列と、俯角観測行列算出器１６で算出した
俯角の観測行列と、予め設定された画像センサ装置２の
観測誤差共分散行列を入力し、追尾対象の俯角の確率分
布を算出する俯角確率分布算出器である。
【００４２】２１は初期値算出器５、距離・方位角によ
る平滑器７、俯角による平滑器１５のいずれかから出力
される追尾対象の３次元位置及び速度から成る３次元平
滑ベクトルと、予め設定された追尾対象の運動モデルを
用いて現時刻より１サンプリング後の追尾対象の３次元
予測位置及び予測速度から成る３次元予測ベクトルを算
出する位置・速度予測器、２２は位置・速度予測器２１
により算出された追尾対象の３次元予測ベクトルを１サ
ンプリング時間だけ遅延して出力する第１の遅延回路で
ある。
る平滑器７、俯角による平滑器１５のいずれかから出力
される追尾対象の３次元位置及び速度から成る３次元平
滑ベクトルと、予め設定された追尾対象の運動モデルを
用いて現時刻より１サンプリング後の追尾対象の３次元
予測位置及び予測速度から成る３次元予測ベクトルを算
出する位置・速度予測器、２２は位置・速度予測器２１
により算出された追尾対象の３次元予測ベクトルを１サ
ンプリング時間だけ遅延して出力する第１の遅延回路で
ある。
【００４３】２３は第１の遅延回路２２の出力する現時
刻より１サンプリング前の追尾対象の３次元予測ベクト
ルを距離及び方位角の予測ベクトルに座標変換し、これ
と変換前の３次元予測ベクトルを出力する第３の座標変
換器、２４は第１の遅延回路２２の出力する現時刻より
１サンプリング前の追尾対象の３次元予測ベクトルを俯
角の予測ベクトルに座標変換し、これと変換前の３次元
予測ベクトルを出力する第１の座標変換器である。
刻より１サンプリング前の追尾対象の３次元予測ベクト
ルを距離及び方位角の予測ベクトルに座標変換し、これ
と変換前の３次元予測ベクトルを出力する第３の座標変
換器、２４は第１の遅延回路２２の出力する現時刻より
１サンプリング前の追尾対象の３次元予測ベクトルを俯
角の予測ベクトルに座標変換し、これと変換前の３次元
予測ベクトルを出力する第１の座標変換器である。
【００４４】次に動作について説明する。最初に、この
実施の形態１による移動体追尾装置及び方法の動作原理
を説明する。図２は、２次元レーダ装置１及び画像セン
サ装置２による追尾対象の観測状態を示す概念図であ
る。２次元レーダ装置１と画像センサ装置２が道路の高
所に設置され、道路上の移動体を観測する場合を仮定す
る。道路脇を原点ｏとし、道路交差方向をｘ軸、道路に
沿った方向をｚ軸、鉛直上向きをｙ軸の正とするｏ−ｘ
ｙｚ座標系を基準座標系とする。基準座標系における２
次元レーダ装置１および画像センサ装置２の取り付け位
置は、それぞれ（ｘr，ｈr，ｚr）及び（ｘc，ｈc，
ｚc）とする。
実施の形態１による移動体追尾装置及び方法の動作原理
を説明する。図２は、２次元レーダ装置１及び画像セン
サ装置２による追尾対象の観測状態を示す概念図であ
る。２次元レーダ装置１と画像センサ装置２が道路の高
所に設置され、道路上の移動体を観測する場合を仮定す
る。道路脇を原点ｏとし、道路交差方向をｘ軸、道路に
沿った方向をｚ軸、鉛直上向きをｙ軸の正とするｏ−ｘ
ｙｚ座標系を基準座標系とする。基準座標系における２
次元レーダ装置１および画像センサ装置２の取り付け位
置は、それぞれ（ｘr，ｈr，ｚr）及び（ｘc，ｈc，
ｚc）とする。
【００４５】図３には、２次元レーダ装置１を原点とす
るレーダ座標系ｏ(r)−ｘ(r)ｙ(r)ｚ(r)が示されてい
る。このレーダ座標系では、２次元レーダ装置１はアン
テナ面がｙ軸回りにγ（ｚ軸正の方向より反時計回りを
正とする）だけ傾いているものとする。
るレーダ座標系ｏ(r)−ｘ(r)ｙ(r)ｚ(r)が示されてい
る。このレーダ座標系では、２次元レーダ装置１はアン
テナ面がｙ軸回りにγ（ｚ軸正の方向より反時計回りを
正とする）だけ傾いているものとする。
【００４６】図４に示すように、レーダ座標系を基準と
する極座標（Ｒ，Ａｚr）をレーダ観測座標系とする。
２次元レーダ装置１は、追尾対象の距離Ｒ及び方位角Ａ
ｚr（ｚ(r)軸正の方向より反時計回りを正とする）を観
測する。図３及び図４から、式（１）及び式（２）の関
係が導かれる。
する極座標（Ｒ，Ａｚr）をレーダ観測座標系とする。
２次元レーダ装置１は、追尾対象の距離Ｒ及び方位角Ａ
ｚr（ｚ(r)軸正の方向より反時計回りを正とする）を観
測する。図３及び図４から、式（１）及び式（２）の関
係が導かれる。
【００４７】
【数１】
【００４８】一方、図５及び図６に示すように、画像セ
ンサ装置２の焦点を原点ｏ(c)とし、画像センサ装置２
前方をｚ(c)軸負の方向、画像センサ装置２左手をｘ(c)
軸負の方向、画像センサ装置上方をｙ(c)軸正の方向と
する座標系ｏ(c)−ｘ(c)ｙ(c)ｚ(c)を画像センサ座標系
とする。画像センサ装置２は、ｙ軸回りにα（ｚ軸正の
方向より反時計回りを正とする）だけ傾き、更にｘ(c)
軸回りにβ（ｚ軸負の方向より下方を負とする）だけ傾
いているものとする。図７に示すように、画像センサ座
標系を基準とする極座標（Ｅ，Ａｚc）を画像センサ観
測座標系とする。図５及び図６から、式（３）及び式
（４）の関係が導かれる。
ンサ装置２の焦点を原点ｏ(c)とし、画像センサ装置２
前方をｚ(c)軸負の方向、画像センサ装置２左手をｘ(c)
軸負の方向、画像センサ装置上方をｙ(c)軸正の方向と
する座標系ｏ(c)−ｘ(c)ｙ(c)ｚ(c)を画像センサ座標系
とする。画像センサ装置２は、ｙ軸回りにα（ｚ軸正の
方向より反時計回りを正とする）だけ傾き、更にｘ(c)
軸回りにβ（ｚ軸負の方向より下方を負とする）だけ傾
いているものとする。図７に示すように、画像センサ座
標系を基準とする極座標（Ｅ，Ａｚc）を画像センサ観
測座標系とする。図５及び図６から、式（３）及び式
（４）の関係が導かれる。
【００４９】
【数２】
【００５０】次に、追尾対象の運動モデルを式（５）に
示す。ここで、アンダバーｘkはサンプリング時刻ｔkに
おける移動体運動諸元の真値を表す状態ベクトルであ
り、３次元直交座標（基準座標）における目標位置ベク
トルを式（６）とし、同速度ベクトルを式（７）とする
と、追尾対象の状態ベクトルは式（８）で表される。な
お、アンダバーＡTはベクトルアンダバーＡの転置ベク
トルを表す。
示す。ここで、アンダバーｘkはサンプリング時刻ｔkに
おける移動体運動諸元の真値を表す状態ベクトルであ
り、３次元直交座標（基準座標）における目標位置ベク
トルを式（６）とし、同速度ベクトルを式（７）とする
と、追尾対象の状態ベクトルは式（８）で表される。な
お、アンダバーＡTはベクトルアンダバーＡの転置ベク
トルを表す。
【００５１】
【数３】
【００５２】ここで、Φk-1はサンプリング時刻ｔk-1よ
りｔkへの状態ベクトルアンダバーｘkの推移行列である
が、追尾対象が等速直線運動を行うと仮定する場合、Φ
k-1は式（９）で表される。また、アンダバーｗkはサン
プリング時刻ｔkにおける駆動雑音ベクトルであり、Γ1
（ｋ）はサンプリング時刻ｔkにおける駆動雑音ベクト
ルの変換行列である。例えば、目標の運動モデルを等速
直線運動と仮定したことによる打ち切り誤差項をΓ
1（ｋ−１）アンダバーｗk-1とみれば、アンダバーｗk
は加速度ベクトル相当であり、Γ1（ｋ−１）は式（１
０）で表される。なお、Ｉは３行３列の単位行列であ
る。
りｔkへの状態ベクトルアンダバーｘkの推移行列である
が、追尾対象が等速直線運動を行うと仮定する場合、Φ
k-1は式（９）で表される。また、アンダバーｗkはサン
プリング時刻ｔkにおける駆動雑音ベクトルであり、Γ1
（ｋ）はサンプリング時刻ｔkにおける駆動雑音ベクト
ルの変換行列である。例えば、目標の運動モデルを等速
直線運動と仮定したことによる打ち切り誤差項をΓ
1（ｋ−１）アンダバーｗk-1とみれば、アンダバーｗk
は加速度ベクトル相当であり、Γ1（ｋ−１）は式（１
０）で表される。なお、Ｉは３行３列の単位行列であ
る。
【００５３】
【数４】
【００５４】また、平均を表す記号として、Ｅを用いる
と、アンダバーｗkは平均アンダバーＯの３次元正規分
布白色雑音であり、式（１１）及び（１２）を満たすも
のとする。ただし、アンダバーＯは零ベクトルであり、
Ｑkはサンプリング時刻ｔkにおける駆動雑音共分散行列
である。
と、アンダバーｗkは平均アンダバーＯの３次元正規分
布白色雑音であり、式（１１）及び（１２）を満たすも
のとする。ただし、アンダバーＯは零ベクトルであり、
Ｑkはサンプリング時刻ｔkにおける駆動雑音共分散行列
である。
【００５５】
【数５】
【００５６】次に、追尾対象の距離及び方位角がサンプ
リング時刻ｔkに２次元レーダ装置１より観測される場
合の２次元レーダ装置１の観測モデルを式（１３）とす
る。ただし、アンダバーｚ1 kはサンプリング時刻ｔkに
おけるレーダ観測装置１の観測ベクトルである。ｈ
1（アンダバーｘk）は２次元レーダ装置１の観測ベクト
ルの真値で、式（１）及び式（２）より式（１４）で表
される。アンダバーν1 kはサンプリング時刻ｔkにおけ
る２次元レーダ装置１の観測雑音ベクトルであり、平均
アンダバーＯの２次元白色正規分布で、式（１５）及び
（１６）で表される。なお、Ｒ1 kはサンプリング時刻ｔ
kにおける２次元レーダ装置１の観測雑音共分散行列で
ある。
リング時刻ｔkに２次元レーダ装置１より観測される場
合の２次元レーダ装置１の観測モデルを式（１３）とす
る。ただし、アンダバーｚ1 kはサンプリング時刻ｔkに
おけるレーダ観測装置１の観測ベクトルである。ｈ
1（アンダバーｘk）は２次元レーダ装置１の観測ベクト
ルの真値で、式（１）及び式（２）より式（１４）で表
される。アンダバーν1 kはサンプリング時刻ｔkにおけ
る２次元レーダ装置１の観測雑音ベクトルであり、平均
アンダバーＯの２次元白色正規分布で、式（１５）及び
（１６）で表される。なお、Ｒ1 kはサンプリング時刻ｔ
kにおける２次元レーダ装置１の観測雑音共分散行列で
ある。
【００５７】
【数６】
【００５８】２次元レーダ装置１の真値ｈ1（アンダバ
ーｘk）の線形近似は式（１７）より得られる。式（１
８）は２次元レーダ装置１の観測行列である。ここで、
Ｒp、Ｅp及びＡｚpは３次元予測ベクトルアンダバーハ
ットｘk（−）を式（１）、式（２）及び式（１９）の
関係式を用いてレーダ観測座標系における距離、俯角、
方位角に変換したものである。
ーｘk）の線形近似は式（１７）より得られる。式（１
８）は２次元レーダ装置１の観測行列である。ここで、
Ｒp、Ｅp及びＡｚpは３次元予測ベクトルアンダバーハ
ットｘk（−）を式（１）、式（２）及び式（１９）の
関係式を用いてレーダ観測座標系における距離、俯角、
方位角に変換したものである。
【００５９】
【数７】
【００６０】一方、追尾対象の俯角がサンプリング時刻
ｔkに画像センサ装置２より観測される場合の画像セン
サ装置２の観測モデルを式（２０）により表す。ただ
し、アンダバーｚ2 kはサンプリング時刻ｔkにおける画
像センサ装置２の観測ベクトルである。ｈ2（アンダバ
ーｘk）は画像センサ装置２の観測ベクトルの真値で、
式（３）及び式（４）より式（２１）で表される。アン
ダバーν2 kはサンプリング時刻ｔkにおける画像センサ
装置２の観測雑音ベクトルであり、平均アンダバーＯの
１次元白色正規分布で、式（２２）及び（２３）で表さ
れる。なお、Ｒ2 kはサンプリング時刻ｔkにおける画像
センサ装置２の観測誤差分散である。
ｔkに画像センサ装置２より観測される場合の画像セン
サ装置２の観測モデルを式（２０）により表す。ただ
し、アンダバーｚ2 kはサンプリング時刻ｔkにおける画
像センサ装置２の観測ベクトルである。ｈ2（アンダバ
ーｘk）は画像センサ装置２の観測ベクトルの真値で、
式（３）及び式（４）より式（２１）で表される。アン
ダバーν2 kはサンプリング時刻ｔkにおける画像センサ
装置２の観測雑音ベクトルであり、平均アンダバーＯの
１次元白色正規分布で、式（２２）及び（２３）で表さ
れる。なお、Ｒ2 kはサンプリング時刻ｔkにおける画像
センサ装置２の観測誤差分散である。
【００６１】
【数８】
【００６２】画像センサ装置２の真値ｈ2（アンダバー
ｘk）の線形近似は式（２４）より得られる。式（２
５）は画像センサ装置２の観測行列である。ここで、Ｒ
p、Ｅp及びＡｚpは３次元予測ベクトルアンダバーハッ
トｘk（−）を式（３）、式（４）及び式（２６）の関
係式を用いて画像センサ観測座標系における距離、俯
角、方位角に変換したものである。
ｘk）の線形近似は式（２４）より得られる。式（２
５）は画像センサ装置２の観測行列である。ここで、Ｒ
p、Ｅp及びＡｚpは３次元予測ベクトルアンダバーハッ
トｘk（−）を式（３）、式（４）及び式（２６）の関
係式を用いて画像センサ観測座標系における距離、俯
角、方位角に変換したものである。
【００６３】
【数９】
【００６４】サンプリング時刻ｔkにおける追尾目標と
相関をとるべき目標予測存在範囲内の２次元レーダ装置
１または画像センサ装置２からの観測ベクトルの総数を
ｍs k、観測ベクトルの全体をＺs kとし（式（２７）を参
照）、サンプリング時刻ｔ1からｔkまでの２次元レーダ
装置１または画像センサ装置２からの観測ベクトル全体
をＺk,sとする（式（２８）を参照）。また、Ｚk,sに対
する観測数の全体を式（２９）とし、さらにまた、式
（３０）とする。ここで、ｓ＝１は２次元レーダ装置１
を、ｓ＝２は画像センサ装置２をそれぞれ表す。
相関をとるべき目標予測存在範囲内の２次元レーダ装置
１または画像センサ装置２からの観測ベクトルの総数を
ｍs k、観測ベクトルの全体をＺs kとし（式（２７）を参
照）、サンプリング時刻ｔ1からｔkまでの２次元レーダ
装置１または画像センサ装置２からの観測ベクトル全体
をＺk,sとする（式（２８）を参照）。また、Ｚk,sに対
する観測数の全体を式（２９）とし、さらにまた、式
（３０）とする。ここで、ｓ＝１は２次元レーダ装置１
を、ｓ＝２は画像センサ装置２をそれぞれ表す。
【００６５】
【数１０】
【００６６】次に、サンプリング時刻ｔk-1までの２次
元レーダ装置１または画像センサ装置により観測された
観測ベクトルＺs kが得られているときの、サンプリング
時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダ
バーハットｘk（−）、予測誤差共分散行列をＰk（−）
とすると、それぞれ条件付平均ベクトル、条件付共分散
行列で定義され、式（３１）及び式（３２）で表され
る。
元レーダ装置１または画像センサ装置により観測された
観測ベクトルＺs kが得られているときの、サンプリング
時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダ
バーハットｘk（−）、予測誤差共分散行列をＰk（−）
とすると、それぞれ条件付平均ベクトル、条件付共分散
行列で定義され、式（３１）及び式（３２）で表され
る。
【００６７】
【数１１】
【００６８】ここで、アンダバーハットｘk（＋）及び
Ｐk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベ
クトル及び平滑誤差共分散行列であり、前サンプリング
時刻ｔk-1に２次元レーダ装置１からの観測ベクトルを
処理した場合には式（３３）及び式（３４）、画像セン
サ装置２からの観測ベクトルを処理した場合には式（３
５）及び式（３６）である。これらの算出方法について
は後で述べる。
Ｐk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベ
クトル及び平滑誤差共分散行列であり、前サンプリング
時刻ｔk-1に２次元レーダ装置１からの観測ベクトルを
処理した場合には式（３３）及び式（３４）、画像セン
サ装置２からの観測ベクトルを処理した場合には式（３
５）及び式（３６）である。これらの算出方法について
は後で述べる。
【００６９】
【数１２】
【００７０】次に、移動体の距離及び方位角（観測ベク
トル）がサンプリング時刻ｔkにおいて、２次元レーダ
装置１より観測される場合の相関処理について述べる。
追尾対象からの観測ベクトルは、式（３７）で与えられ
る予測位置ベクトルアンダバーｚ1 k（−）を平均とし、
式（３８）で与えられるＳ1 kを共分散行列とする２次元
正規分布に従うとする。そこで、式（３９）を満たす観
測ベクトルアンダバーｚ1 k,iのうち、値の最も小さい観
測ベクトルアンダバーｚ1 kを追尾対象と相関がある観測
ベクトルと判定する。ここで、ｄ1は追尾対象との相関
範囲を決めるパラメータであり、自由度２のχ自乗分布
により算出する。
トル）がサンプリング時刻ｔkにおいて、２次元レーダ
装置１より観測される場合の相関処理について述べる。
追尾対象からの観測ベクトルは、式（３７）で与えられ
る予測位置ベクトルアンダバーｚ1 k（−）を平均とし、
式（３８）で与えられるＳ1 kを共分散行列とする２次元
正規分布に従うとする。そこで、式（３９）を満たす観
測ベクトルアンダバーｚ1 k,iのうち、値の最も小さい観
測ベクトルアンダバーｚ1 kを追尾対象と相関がある観測
ベクトルと判定する。ここで、ｄ1は追尾対象との相関
範囲を決めるパラメータであり、自由度２のχ自乗分布
により算出する。
【００７１】
【数１３】
【００７２】次に、追尾対象の位置ベクトルがサンプリ
ング時刻ｔkにおいて、２次元レーダ装置１により観測
される場合の平滑処理について述べる。ゲイン行列、平
滑ベクトル及び平滑誤差共分散行列は拡張カルマンフィ
ルタの理論により、式（４０）〜（４２）で与えられ
る。
ング時刻ｔkにおいて、２次元レーダ装置１により観測
される場合の平滑処理について述べる。ゲイン行列、平
滑ベクトル及び平滑誤差共分散行列は拡張カルマンフィ
ルタの理論により、式（４０）〜（４２）で与えられ
る。
【００７３】
【数１４】
【００７４】次に、移動体の画像がサンプリング時刻ｔ
kにおいて画像センサ装置２より観測され、画像処理に
より画像の代表点の俯角が出力される場合の相関処理に
ついて述べる。追尾対象からの俯角（観測ベクトル）
は、式（４３）で与えられる予測位置ベクトルアンダバ
ーｚ2 k（−）を平均とし、式（４４）で与えられるＳ2 k
を共分散行列とする１次元正規分布に従うとする。そこ
で、式（４５）を満たす観測ベクトルアンダバーｚ2 k,i
のうち、値の最も小さい観測ベクトルアンダバーｚ2 kを
追尾対象と相関がある観測ベクトルと判定する。ここ
で、ｄ2は追尾対象との相関範囲を決めるパラメータで
あり、自由度１のχ自乗分布により算出する。
kにおいて画像センサ装置２より観測され、画像処理に
より画像の代表点の俯角が出力される場合の相関処理に
ついて述べる。追尾対象からの俯角（観測ベクトル）
は、式（４３）で与えられる予測位置ベクトルアンダバ
ーｚ2 k（−）を平均とし、式（４４）で与えられるＳ2 k
を共分散行列とする１次元正規分布に従うとする。そこ
で、式（４５）を満たす観測ベクトルアンダバーｚ2 k,i
のうち、値の最も小さい観測ベクトルアンダバーｚ2 kを
追尾対象と相関がある観測ベクトルと判定する。ここ
で、ｄ2は追尾対象との相関範囲を決めるパラメータで
あり、自由度１のχ自乗分布により算出する。
【００７５】
【数１５】
【００７６】次に、追尾対象の観測ベクトルがサンプリ
ング時刻ｔkにおいて、画像センサ装置２より観測され
る場合の平滑処理について述べる。ゲイン行列、平滑ベ
クトル及び平滑誤差共分散行列は拡張カルマンフィルタ
の理論により、式（４６）〜式（４８）で与えられる。
ング時刻ｔkにおいて、画像センサ装置２より観測され
る場合の平滑処理について述べる。ゲイン行列、平滑ベ
クトル及び平滑誤差共分散行列は拡張カルマンフィルタ
の理論により、式（４６）〜式（４８）で与えられる。
【００７７】
【数１６】
【００７８】次に、追尾領域に新たな追尾対象が進入し
た場合の追尾初期化方法について述べる。サンプリング
時刻ｔk-1及びサンプリング時刻ｔkにおいて、どの既追
尾対象とも相関のとれない新たな追尾対象の観測ベクト
ルが２次元レーダ装置１から連続して得られた場合、こ
れらの観測ベクトルをそれぞれアンダバーｚ1 k-1及びア
ンダバーｚ1 kとする。追尾対象の進入箇所の道路高の代
表値をｙ0とすると、式（１）及び式（２）の関係より
式（４９）及び式（５０）が得られるので、アンダバー
ｚ1 k-1及びアンダバーｚ1 kはこれらの式を用いて基準座
標に変換することができる。
た場合の追尾初期化方法について述べる。サンプリング
時刻ｔk-1及びサンプリング時刻ｔkにおいて、どの既追
尾対象とも相関のとれない新たな追尾対象の観測ベクト
ルが２次元レーダ装置１から連続して得られた場合、こ
れらの観測ベクトルをそれぞれアンダバーｚ1 k-1及びア
ンダバーｚ1 kとする。追尾対象の進入箇所の道路高の代
表値をｙ0とすると、式（１）及び式（２）の関係より
式（４９）及び式（５０）が得られるので、アンダバー
ｚ1 k-1及びアンダバーｚ1 kはこれらの式を用いて基準座
標に変換することができる。
【００７９】
【数１７】
【００８０】基準座標に変換後のアンダバーｚ1 k-1及び
アンダバーｚ1 kを、それぞれアンダバーｕ1 k-1及びアン
ダバーｕ1 kとすると、平滑ベクトル及び平滑誤差共分散
行列の初期値は式（５１）及び式（５２）を用いて算出
できる。ここで、Γ1 2（ｋ）は２次元レーダ装置１の観
測座標系より基準座標系のｘ−ｚ平面への観測雑音ベク
トルの変換行列である。
アンダバーｚ1 kを、それぞれアンダバーｕ1 k-1及びアン
ダバーｕ1 kとすると、平滑ベクトル及び平滑誤差共分散
行列の初期値は式（５１）及び式（５２）を用いて算出
できる。ここで、Γ1 2（ｋ）は２次元レーダ装置１の観
測座標系より基準座標系のｘ−ｚ平面への観測雑音ベク
トルの変換行列である。
【００８１】
【数１８】
【００８２】次に、この実施の形態１に係る移動体追尾
装置の動作及び方法を具体的に説明する。まず、２次元
レーダ装置１では、レーダ観測座標系で移動体の距離及
び方位角を観測し、その結果（観測ベクトル）を距離・
方位角相関器３に出力する。距離・方位角相関器３で
は、２次元レーダ装置１から観測ベクトルを受け取る
と、その観測ベクトルから追尾対象と相関のある観測ベ
クトルを採用するため、距離・方位角確率分布算出器１
２により算出された距離及び方位角の確率分布Ｓ1 k（式
（３８）を参照）を用いて、追尾対象と相関の可能性の
ある観測ベクトルを式（３９）を用いて選択する。そし
て、選択された観測ベクトルのうち式（３９）の値の最
も小さい観測ベクトルアンダバーｚ1 kを追尾対象の観測
ベクトルと判定し、距離・方位角による平滑器７に出力
する。一方、どの追尾対象の観測ベクトルとも判定され
ない観測ベクトルは、第２の座標変換器４に出力する
（ステップＳＴ１）。
装置の動作及び方法を具体的に説明する。まず、２次元
レーダ装置１では、レーダ観測座標系で移動体の距離及
び方位角を観測し、その結果（観測ベクトル）を距離・
方位角相関器３に出力する。距離・方位角相関器３で
は、２次元レーダ装置１から観測ベクトルを受け取る
と、その観測ベクトルから追尾対象と相関のある観測ベ
クトルを採用するため、距離・方位角確率分布算出器１
２により算出された距離及び方位角の確率分布Ｓ1 k（式
（３８）を参照）を用いて、追尾対象と相関の可能性の
ある観測ベクトルを式（３９）を用いて選択する。そし
て、選択された観測ベクトルのうち式（３９）の値の最
も小さい観測ベクトルアンダバーｚ1 kを追尾対象の観測
ベクトルと判定し、距離・方位角による平滑器７に出力
する。一方、どの追尾対象の観測ベクトルとも判定され
ない観測ベクトルは、第２の座標変換器４に出力する
（ステップＳＴ１）。
【００８３】第２の座標変換器４は、距離方位角相関器
３からどの追尾対象とも相関のない観測ベクトルアンダ
バーｚ1 kを受け取ると、式（４９）及び式（５０）を用
いて高度ｙ0の基準座標系の観測ベクトルアンダバーｕ1
kに変換し、初期値算出器５に出力する。初期値算出器
５は、第２の座標変換器４から基準座標系の観測ベクト
ルアンダバーｕ1 kを２サンプル連続して受け取ると、平
滑ベクトル及び平滑誤差共分散行列の初期値を式（５
１）及び式（５２）に従い算出する（ステップＳＴ
２）。
３からどの追尾対象とも相関のない観測ベクトルアンダ
バーｚ1 kを受け取ると、式（４９）及び式（５０）を用
いて高度ｙ0の基準座標系の観測ベクトルアンダバーｕ1
kに変換し、初期値算出器５に出力する。初期値算出器
５は、第２の座標変換器４から基準座標系の観測ベクト
ルアンダバーｕ1 kを２サンプル連続して受け取ると、平
滑ベクトル及び平滑誤差共分散行列の初期値を式（５
１）及び式（５２）に従い算出する（ステップＳＴ
２）。
【００８４】距離・方位角ゲイン行列算出器６では、第
２の遅延回路１１から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）と、距離・方位角観測行列算出器８から出力
された式（１８）の２次元レーダ装置１の観測行列と予
め設定された式（１６）の２次元レーダ装置１の観測雑
音共分散行列とから距離及び方位角のゲイン行列Ｋ1 kを
式（４０）に従い算出する（ステップＳＴ３）。
２の遅延回路１１から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）と、距離・方位角観測行列算出器８から出力
された式（１８）の２次元レーダ装置１の観測行列と予
め設定された式（１６）の２次元レーダ装置１の観測雑
音共分散行列とから距離及び方位角のゲイン行列Ｋ1 kを
式（４０）に従い算出する（ステップＳＴ３）。
【００８５】距離・方位角による平滑器７では、距離・
方位角相関器３から出力された追尾対象の観測ベクトル
アンダバーｚ1 kと、第３の座標変換器２３から出力され
た距離及び方位角の予測値（現時刻より１サンプリング
前に算出された予測値）と、距離・方位角ゲイン行列算
出器６により算出されたゲイン行列Ｋ1 kとを用いて、追
尾対象の位置及び速度の平滑ベクトルを式（４１）に従
い算出する（ステップＳＴ４）。
方位角相関器３から出力された追尾対象の観測ベクトル
アンダバーｚ1 kと、第３の座標変換器２３から出力され
た距離及び方位角の予測値（現時刻より１サンプリング
前に算出された予測値）と、距離・方位角ゲイン行列算
出器６により算出されたゲイン行列Ｋ1 kとを用いて、追
尾対象の位置及び速度の平滑ベクトルを式（４１）に従
い算出する（ステップＳＴ４）。
【００８６】距離・方位角観測行列算出器８では、第３
の座標変換器２３から出力された距離と方位角の予測値
（現時刻より１サンプリング前に算出された予測値）を
入力し、２次元レーダ装置１の観測行列を式（１８）に
従い算出する（ステップＳＴ５）。
の座標変換器２３から出力された距離と方位角の予測値
（現時刻より１サンプリング前に算出された予測値）を
入力し、２次元レーダ装置１の観測行列を式（１８）に
従い算出する（ステップＳＴ５）。
【００８７】距離・方位角データによる平滑誤差評価器
９では、第３の座標変換器２３から出力された距離及び
方位角の予測値（現時刻より１サンプリング前に算出さ
れた予測値）と、距離・方位角観測行列算出器８から出
力された２次元レーダ装置１の観測行列と、距離・方位
角ゲイン行列算出器６から出力されたゲイン行列Ｋ
1 kと、第２の遅延回路１１から出力された予測誤差共分
散行列（現時刻より１サンプリング前に算出された予測
誤差共分散行列）とから平滑誤差共分散行列を式（４
１）に従い算出する（ステップＳＴ６）。
９では、第３の座標変換器２３から出力された距離及び
方位角の予測値（現時刻より１サンプリング前に算出さ
れた予測値）と、距離・方位角観測行列算出器８から出
力された２次元レーダ装置１の観測行列と、距離・方位
角ゲイン行列算出器６から出力されたゲイン行列Ｋ
1 kと、第２の遅延回路１１から出力された予測誤差共分
散行列（現時刻より１サンプリング前に算出された予測
誤差共分散行列）とから平滑誤差共分散行列を式（４
１）に従い算出する（ステップＳＴ６）。
【００８８】予測誤差評価器Ａ１０では、距離・方位角
データによる平滑誤差評価器９から出力された平滑誤差
共分散行列と、初期値算出器５から出力された新規追尾
対象の平滑誤差共分散行列を入力し、予測誤差共分散行
列を式（３２）に従い算出する（ステップＳＴ７）。
データによる平滑誤差評価器９から出力された平滑誤差
共分散行列と、初期値算出器５から出力された新規追尾
対象の平滑誤差共分散行列を入力し、予測誤差共分散行
列を式（３２）に従い算出する（ステップＳＴ７）。
【００８９】距離・方位角確率分布算出器１２は、第２
の遅延回路１１から出力された予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）と、距離・方位角観測行列算出器８から出力され
た２次元レーダ装置１の観測行列と、予め設定された式
（１６）の２次元レーダ装置１の観測雑音共分散行列と
を入力し、現時刻での存在が予測される追尾対象の距離
及び方位角の確率分布Ｓ1 kを式（３８）に従い算出する
（ステップＳＴ８）。
の遅延回路１１から出力された予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）と、距離・方位角観測行列算出器８から出力され
た２次元レーダ装置１の観測行列と、予め設定された式
（１６）の２次元レーダ装置１の観測雑音共分散行列と
を入力し、現時刻での存在が予測される追尾対象の距離
及び方位角の確率分布Ｓ1 kを式（３８）に従い算出する
（ステップＳＴ８）。
【００９０】一方、画像センサ装置２では、画像から画
像センサ観測座標系で移動体の俯角を検出し、その結果
（観測ベクトル）を俯角相関器１３に出力する。俯角相
関器１３では、画像センサ装置２から観測ベクトルを受
け取ると、その観測ベクトルから追尾対象と相関のある
観測ベクトルを選択するため、俯角確率分布算出器２０
により算出された俯角の確率分布Ｓ2 k（式（４４）を参
照）を用いて、追尾対象と相関の可能性のある観測ベク
トルを式（４５）を用いて選択する。そして、選択され
た観測ベクトルのうち式（４５）の値の最も小さい観測
ベクトルアンダバーｚ2 kを追尾対象の観測ベクトルと判
定し、俯角による平滑器１５に出力する（ステップＳＴ
９）。
像センサ観測座標系で移動体の俯角を検出し、その結果
（観測ベクトル）を俯角相関器１３に出力する。俯角相
関器１３では、画像センサ装置２から観測ベクトルを受
け取ると、その観測ベクトルから追尾対象と相関のある
観測ベクトルを選択するため、俯角確率分布算出器２０
により算出された俯角の確率分布Ｓ2 k（式（４４）を参
照）を用いて、追尾対象と相関の可能性のある観測ベク
トルを式（４５）を用いて選択する。そして、選択され
た観測ベクトルのうち式（４５）の値の最も小さい観測
ベクトルアンダバーｚ2 kを追尾対象の観測ベクトルと判
定し、俯角による平滑器１５に出力する（ステップＳＴ
９）。
【００９１】俯角ゲイン行列算出器１４では、第３の遅
延回路１９から出力された予測誤差共分散行列（現時刻
より１サンプリング前に算出された予測誤差共分散行
列）と、俯角観測行列算出器１６から出力された式（２
５）の画像センサ装置２の観測行列と予め設定された式
（２３）の画像センサ装置２の観測雑音分散とから俯角
のゲイン行列Ｋ2 kを式（４６）に従い算出する（ステッ
プＳＴ１０）。
延回路１９から出力された予測誤差共分散行列（現時刻
より１サンプリング前に算出された予測誤差共分散行
列）と、俯角観測行列算出器１６から出力された式（２
５）の画像センサ装置２の観測行列と予め設定された式
（２３）の画像センサ装置２の観測雑音分散とから俯角
のゲイン行列Ｋ2 kを式（４６）に従い算出する（ステッ
プＳＴ１０）。
【００９２】俯角による平滑器１５では、俯角相関器１
３から出力された追尾対象の観測ベクトルアンダバーｚ
2 kと、第１の座標変換器２４から出力された俯角の予測
値（現時刻より１サンプリング前に算出された予測値）
と、俯角ゲイン行列算出器１４により算出されたゲイン
行列Ｋ2 kとを用いて、追尾対象の位置及び速度の平滑ベ
クトルを式（４７）に従い算出する（ステップＳＴ１
１）。
３から出力された追尾対象の観測ベクトルアンダバーｚ
2 kと、第１の座標変換器２４から出力された俯角の予測
値（現時刻より１サンプリング前に算出された予測値）
と、俯角ゲイン行列算出器１４により算出されたゲイン
行列Ｋ2 kとを用いて、追尾対象の位置及び速度の平滑ベ
クトルを式（４７）に従い算出する（ステップＳＴ１
１）。
【００９３】俯角観測行列算出器１６では、第１の座標
変換器２４から出力された俯角の予測値（現時刻より１
サンプリング前に算出された予測値）を入力し、画像セ
ンサ装置２の観測行列を式（２５）に従い算出する（ス
テップＳＴ１２）。
変換器２４から出力された俯角の予測値（現時刻より１
サンプリング前に算出された予測値）を入力し、画像セ
ンサ装置２の観測行列を式（２５）に従い算出する（ス
テップＳＴ１２）。
【００９４】俯角データによる平滑誤差評価器１７で
は、第１の座標変換器２４から出力された俯角の予測値
（現時刻より１サンプリング前に算出された予測値）
と、俯角観測行列算出器１６から出力された画像センサ
装置２の観測行列と、俯角ゲイン行列算出器１４から出
力されたゲイン行列Ｋ2 kと、第３の遅延回路１９から出
力された予測誤差共分散行列（現時刻より１サンプリン
グ前に算出された予測誤差共分散行列）とから平滑誤差
共分散行列を式（４８）に従い算出する（ステップＳＴ
１３）。
は、第１の座標変換器２４から出力された俯角の予測値
（現時刻より１サンプリング前に算出された予測値）
と、俯角観測行列算出器１６から出力された画像センサ
装置２の観測行列と、俯角ゲイン行列算出器１４から出
力されたゲイン行列Ｋ2 kと、第３の遅延回路１９から出
力された予測誤差共分散行列（現時刻より１サンプリン
グ前に算出された予測誤差共分散行列）とから平滑誤差
共分散行列を式（４８）に従い算出する（ステップＳＴ
１３）。
【００９５】予測誤差評価器Ｂ１８では、俯角データに
よる平滑誤差評価器１７から出力された平滑誤差共分散
行列を入力し、予測誤差共分散行列を式（３２）に従い
算出する（ステップＳＴ１４）。
よる平滑誤差評価器１７から出力された平滑誤差共分散
行列を入力し、予測誤差共分散行列を式（３２）に従い
算出する（ステップＳＴ１４）。
【００９６】俯角確率分布算出器２０は、第３の遅延回
路１９から出力された予測誤差共分散行列（現時刻より
１サンプリング前に算出された予測誤差共分散行列）
と、俯角観測行列算出器１６から出力された画像センサ
装置２の観測行列と、予め設定された式（２３）の画像
センサ装置２の観測雑音分散とを入力し、現時刻での存
在が予測される追尾対象の俯角の確率分布Ｓ2 kを式（４
４）に従い算出する（ステップＳＴ１５）。
路１９から出力された予測誤差共分散行列（現時刻より
１サンプリング前に算出された予測誤差共分散行列）
と、俯角観測行列算出器１６から出力された画像センサ
装置２の観測行列と、予め設定された式（２３）の画像
センサ装置２の観測雑音分散とを入力し、現時刻での存
在が予測される追尾対象の俯角の確率分布Ｓ2 kを式（４
４）に従い算出する（ステップＳＴ１５）。
【００９７】上記のようにして、距離・方位角による平
滑器７または俯角による平滑器１７または初期値算出器
５が追尾対象の位置及び速度の平滑値を算出すると、位
置・速度予測器２１は、これらのどちらかの平滑値と予
め設定された追尾対象の運動モデルを用いて、現時刻よ
り１サンプリング後の追尾対象の予測ベクトル（位置及
び速度）を式（３１）に従い算出し、第１の遅延回路２
２に出力する（ステップＳＴ１６）。
滑器７または俯角による平滑器１７または初期値算出器
５が追尾対象の位置及び速度の平滑値を算出すると、位
置・速度予測器２１は、これらのどちらかの平滑値と予
め設定された追尾対象の運動モデルを用いて、現時刻よ
り１サンプリング後の追尾対象の予測ベクトル（位置及
び速度）を式（３１）に従い算出し、第１の遅延回路２
２に出力する（ステップＳＴ１６）。
【００９８】そして、次に追尾対象を観測するセンサが
２次元レーダ装置１の場合は、第３の座標変換器２３が
第１の遅延回路２２から予測ベクトルを入力し、式
（１）、式（２）、式（１９）を使用して２次元レーダ
装置１を基準とする極座標に変換する。一方、次に追尾
対象を観測するセンサが画像センサ装置２の場合は、第
１の座標変換器２４が第１の遅延回路２２から予測ベク
トルを入力し、式（３）、式（４）、式（２６）を使用
して画像センサ装置２を基準とする極座標に変換する。
以上、追尾終了になるまでこの一連の処理をくり返す。
２次元レーダ装置１の場合は、第３の座標変換器２３が
第１の遅延回路２２から予測ベクトルを入力し、式
（１）、式（２）、式（１９）を使用して２次元レーダ
装置１を基準とする極座標に変換する。一方、次に追尾
対象を観測するセンサが画像センサ装置２の場合は、第
１の座標変換器２４が第１の遅延回路２２から予測ベク
トルを入力し、式（３）、式（４）、式（２６）を使用
して画像センサ装置２を基準とする極座標に変換する。
以上、追尾終了になるまでこの一連の処理をくり返す。
【００９９】以上で明らかなように、この実施の形態１
によれば、距離・方位角観測行列算出器８において、２
次元レーダ装置１の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、距離・方位角
による平滑器７で、この観測行列と、距離及び方位角の
観測ベクトルを用いて３次元位置及び速度の平滑ベクト
ルを算出し、また、俯角観測行列算出器１６において、
画像センサ装置２の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、俯角による平
滑器１５で、この観測行列と、俯角の観測ベクトルを用
いて３次元位置及び速度の平滑ベクトルを算出する構成
としている。
によれば、距離・方位角観測行列算出器８において、２
次元レーダ装置１の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、距離・方位角
による平滑器７で、この観測行列と、距離及び方位角の
観測ベクトルを用いて３次元位置及び速度の平滑ベクト
ルを算出し、また、俯角観測行列算出器１６において、
画像センサ装置２の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、俯角による平
滑器１５で、この観測行列と、俯角の観測ベクトルを用
いて３次元位置及び速度の平滑ベクトルを算出する構成
としている。
【０１００】このため、追尾対象の追尾開始領域の道路
高の情報を利用することによって初期値算出器５により
算出した３次元位置及び速度の初期値をもとに、２次元
レーダ装置１の観測する距離及び方位角と、画像センサ
装置２の観測する俯角とを用いて逐次的に追尾対象の３
次元位置及び速度を推定することができる。従って、２
次元レーダと俯角を観測する画像センサを併用すること
により、移動体の３次元運動を追尾できる効果を奏す
る。
高の情報を利用することによって初期値算出器５により
算出した３次元位置及び速度の初期値をもとに、２次元
レーダ装置１の観測する距離及び方位角と、画像センサ
装置２の観測する俯角とを用いて逐次的に追尾対象の３
次元位置及び速度を推定することができる。従って、２
次元レーダと俯角を観測する画像センサを併用すること
により、移動体の３次元運動を追尾できる効果を奏す
る。
【０１０１】なお、この実施の形態１は、２次元レーダ
装置１や画像センサ装置２の代わりにレーザ装置や音波
センサ装置を用いて追尾対象の距離及び方位角を観測し
た場合にも適用可能である。また、この実施の形態１で
は、３次元位置及び速度の初期位置の計算に２次元レー
ダ装置１の観測ベクトルを用いているが、画像センサ装
置２の観測ベクトルを用いて計算してもよい。
装置１や画像センサ装置２の代わりにレーザ装置や音波
センサ装置を用いて追尾対象の距離及び方位角を観測し
た場合にも適用可能である。また、この実施の形態１で
は、３次元位置及び速度の初期位置の計算に２次元レー
ダ装置１の観測ベクトルを用いているが、画像センサ装
置２の観測ベクトルを用いて計算してもよい。
【０１０２】実施の形態２．図８は、この実施の形態２
による移動体追尾装置及び方法を説明するための構成図
である。図８において、図１に示す実施の形態１と同一
部分は同一符号を付してその説明は省略する。すなわ
ち、１、３〜１２、２１〜２４は実施の形態１と同一部
分を示し、その説明は省略する。
による移動体追尾装置及び方法を説明するための構成図
である。図８において、図１に示す実施の形態１と同一
部分は同一符号を付してその説明は省略する。すなわ
ち、１、３〜１２、２１〜２４は実施の形態１と同一部
分を示し、その説明は省略する。
【０１０３】２は後述する画像センサ観測座標系におい
て画像を観測し、移動体の代表点の方位角を出力する本
実施の形態２に係る画像センサ装置、２５は画像センサ
装置２から画像センサ観測座標系における追尾対象の方
位角を入力し、これらの観測ベクトルのうち、追尾対象
が存在する可能性の高い観測ベクトルを、方位角確率分
布算出器３０から入力した追尾対象の方位角の確率分布
をもとに選択する方位角相関器、２６は第３の遅延回路
１９から出力される現時刻より１サンプリング前に算出
された予測誤差共分散行列と、後述する方位角観測行列
算出器２８から出力される方位角の観測行列と、画像セ
ンサ装置２の観測誤差分散とから方位角のゲイン行列を
算出する方位角ゲイン行列算出器である。
て画像を観測し、移動体の代表点の方位角を出力する本
実施の形態２に係る画像センサ装置、２５は画像センサ
装置２から画像センサ観測座標系における追尾対象の方
位角を入力し、これらの観測ベクトルのうち、追尾対象
が存在する可能性の高い観測ベクトルを、方位角確率分
布算出器３０から入力した追尾対象の方位角の確率分布
をもとに選択する方位角相関器、２６は第３の遅延回路
１９から出力される現時刻より１サンプリング前に算出
された予測誤差共分散行列と、後述する方位角観測行列
算出器２８から出力される方位角の観測行列と、画像セ
ンサ装置２の観測誤差分散とから方位角のゲイン行列を
算出する方位角ゲイン行列算出器である。
【０１０４】２７は方位角相関器２５により選択された
観測ベクトル、第１の座標変換器２４から出力される方
位角の予測ベクトルと３次元予測ベクトル、方位角ゲイ
ン行列算出器２６により算出された方位角のゲイン行列
を用いて追尾対象の３次元位置及び速度の平滑ベクトル
を算出する方位角による平滑器、２８は第１の座標変換
器２４から出力される方位角の予測ベクトルを入力し、
方位角の観測行列を算出する方位角観測行列算出器であ
る。
観測ベクトル、第１の座標変換器２４から出力される方
位角の予測ベクトルと３次元予測ベクトル、方位角ゲイ
ン行列算出器２６により算出された方位角のゲイン行列
を用いて追尾対象の３次元位置及び速度の平滑ベクトル
を算出する方位角による平滑器、２８は第１の座標変換
器２４から出力される方位角の予測ベクトルを入力し、
方位角の観測行列を算出する方位角観測行列算出器であ
る。
【０１０５】２９は方位角データによる平滑誤差評価器
であって、第１の座標変換器２４から出力される方位角
の予測ベクトルと、方位角観測行列算出器２８から出力
される方位角の観測行列と、方位角ゲイン行列算出器２
６から出力される方位角のゲイン行列と、第３の遅延回
路１９から出力される現時刻より１サンプリング前に算
出された予測誤差共分散行列とを用いて追尾対象の３次
元位置及び速度の平滑誤差を評価する平滑誤差共分散行
列を算出する。
であって、第１の座標変換器２４から出力される方位角
の予測ベクトルと、方位角観測行列算出器２８から出力
される方位角の観測行列と、方位角ゲイン行列算出器２
６から出力される方位角のゲイン行列と、第３の遅延回
路１９から出力される現時刻より１サンプリング前に算
出された予測誤差共分散行列とを用いて追尾対象の３次
元位置及び速度の平滑誤差を評価する平滑誤差共分散行
列を算出する。
【０１０６】１８は方位角データによる平滑誤差評価器
２９により算出された平滑誤差共分散行列を入力し、追
尾対象の３次元位置及び速度の予測誤差を評価する予測
誤差共分散行列を算出する予測誤差評価器Ｂである。１
９は予測誤差評価器Ｂ１８により算出された予測誤差共
分散行列を１サンプリング時間だけ遅延して出力する第
３の遅延回路、３０は第３の遅延回路１９の出力する予
測誤差共分散行列と、方位角観測行列算出器２８で算出
した方位角の観測行列と、予め設定された画像センサ装
置２の観測誤差分散を入力し、追尾対象の方位角の確率
分布を算出する方位角確率分布算出器である。
２９により算出された平滑誤差共分散行列を入力し、追
尾対象の３次元位置及び速度の予測誤差を評価する予測
誤差共分散行列を算出する予測誤差評価器Ｂである。１
９は予測誤差評価器Ｂ１８により算出された予測誤差共
分散行列を１サンプリング時間だけ遅延して出力する第
３の遅延回路、３０は第３の遅延回路１９の出力する予
測誤差共分散行列と、方位角観測行列算出器２８で算出
した方位角の観測行列と、予め設定された画像センサ装
置２の観測誤差分散を入力し、追尾対象の方位角の確率
分布を算出する方位角確率分布算出器である。
【０１０７】２１は初期値算出器５、距離・方位角によ
る平滑器７、方位角による平滑器２７のいずれかから出
力される追尾対象の３次元位置及び速度から成る３次元
平滑ベクトルと、予め設定された追尾対象の運動モデル
を用いて現時刻より１サンプリング後の追尾対象の３次
元予測位置及び予測速度から成る３次元予測ベクトルを
算出する位置・速度予測器、２２は位置・速度予測器２
１により算出された追尾対象の３次元予測ベクトルを１
サンプリング時間だけ遅延して出力する第１の遅延回路
である。
る平滑器７、方位角による平滑器２７のいずれかから出
力される追尾対象の３次元位置及び速度から成る３次元
平滑ベクトルと、予め設定された追尾対象の運動モデル
を用いて現時刻より１サンプリング後の追尾対象の３次
元予測位置及び予測速度から成る３次元予測ベクトルを
算出する位置・速度予測器、２２は位置・速度予測器２
１により算出された追尾対象の３次元予測ベクトルを１
サンプリング時間だけ遅延して出力する第１の遅延回路
である。
【０１０８】２３は第１の遅延回路２２の出力する現時
刻より１サンプリング前の追尾対象の３次元予測ベクト
ルを距離及び方位角の予測ベクトルに座標変換し、これ
と変換前の３次元予測ベクトルを出力する第２の座標変
換器、２４は第１の遅延回路２２の出力する現時刻より
１サンプリング前の追尾対象の３次元予測ベクトルを方
位角の予測ベクトルに座標変換し、これと変換前の３次
元予測ベクトルを出力する第３の座標変換器である。
刻より１サンプリング前の追尾対象の３次元予測ベクト
ルを距離及び方位角の予測ベクトルに座標変換し、これ
と変換前の３次元予測ベクトルを出力する第２の座標変
換器、２４は第１の遅延回路２２の出力する現時刻より
１サンプリング前の追尾対象の３次元予測ベクトルを方
位角の予測ベクトルに座標変換し、これと変換前の３次
元予測ベクトルを出力する第３の座標変換器である。
【０１０９】次に動作について説明する。最初に、この
実施の形態２による移動体追尾装置及び方法の動作原理
を説明する。ただし、２次元レーダ装置１と画像センサ
装置２の観測座標系の定義、観測座標系と基準座標系の
関係、移動体の運動モデル、２次元レーダ装置１の観測
モデルの定義に関して、すなわち、式（１）〜式（１
９）に表される関係と、２次元レーダ装置１の相関処理
及び平滑処理に関して、すなわち、式（３７）〜式（４
２）に表される関係と、追尾初期化方法に関して、すな
わち、式（４９）〜式（５２）で表される関係とについ
ては実施の形態１と同様である。
実施の形態２による移動体追尾装置及び方法の動作原理
を説明する。ただし、２次元レーダ装置１と画像センサ
装置２の観測座標系の定義、観測座標系と基準座標系の
関係、移動体の運動モデル、２次元レーダ装置１の観測
モデルの定義に関して、すなわち、式（１）〜式（１
９）に表される関係と、２次元レーダ装置１の相関処理
及び平滑処理に関して、すなわち、式（３７）〜式（４
２）に表される関係と、追尾初期化方法に関して、すな
わち、式（４９）〜式（５２）で表される関係とについ
ては実施の形態１と同様である。
【０１１０】追尾対象の方位角がサンプリング時刻ｔk
に画像センサ装置２より観測される場合の画像センサ装
置２の観測モデルを式（５３）により表す。ただし、ア
ンダバーｚ3 kはサンプリング時刻ｔkにおける画像セン
サ装置２の方位角の観測ベクトルである。ｈ3（アンダ
バーｘk）は画像センサ装置２の観測ベクトルの真値
で、式（３）及び式（４）より式（５４）で表される。
アンダバーν3 kはサンプリング時刻ｔkにおける画像セ
ンサ装置２の観測雑音ベクトルであり、平均アンダバー
Ｏの１次元白色正規分布で、式（５５）及び（５６）で
表される。なお、Ｒ 3 kはサンプリング時刻ｔkにおける
画像センサ装置２の観測雑音分散である。
に画像センサ装置２より観測される場合の画像センサ装
置２の観測モデルを式（５３）により表す。ただし、ア
ンダバーｚ3 kはサンプリング時刻ｔkにおける画像セン
サ装置２の方位角の観測ベクトルである。ｈ3（アンダ
バーｘk）は画像センサ装置２の観測ベクトルの真値
で、式（３）及び式（４）より式（５４）で表される。
アンダバーν3 kはサンプリング時刻ｔkにおける画像セ
ンサ装置２の観測雑音ベクトルであり、平均アンダバー
Ｏの１次元白色正規分布で、式（５５）及び（５６）で
表される。なお、Ｒ 3 kはサンプリング時刻ｔkにおける
画像センサ装置２の観測雑音分散である。
【０１１１】
【数１９】
【０１１２】画像センサ装置２の真値ｈ3（アンダバー
ｘk）の線形近似は式（５７）より得られる。式（５
８）は画像センサ装置２の観測行列である。ここで、Ｒ
p、Ｅp及びＡｚpは３次元予測ベクトルアンダバーハッ
トｘk（−）を式（３）、式（４）及び式（２６）の関
係式を用いて画像センサ観測座標系における距離、俯
角、方位角に変換したものである。
ｘk）の線形近似は式（５７）より得られる。式（５
８）は画像センサ装置２の観測行列である。ここで、Ｒ
p、Ｅp及びＡｚpは３次元予測ベクトルアンダバーハッ
トｘk（−）を式（３）、式（４）及び式（２６）の関
係式を用いて画像センサ観測座標系における距離、俯
角、方位角に変換したものである。
【０１１３】
【数２０】
【０１１４】サンプリング時刻ｔkにおける追尾目標と
相関をとるべき目標予測存在範囲内の２次元レーダ装置
１または画像センサ装置２からの観測ベクトルの総数を
ｍs k、観測ベクトルの全体をＺs kとし（式（５９）を参
照）、サンプリング時刻ｔ1からｔkまでの２次元レーダ
装置１または画像センサ装置２からの観測ベクトル全体
をＺk,sとする（式（６０）を参照）。また、Ｚk,sに対
する観測数の全体を式（６１）とし、さらにまた、式
（６２）とする。ここで、ｓ＝１は２次元レーダ装置１
を、ｓ＝３は画像センサ装置２をそれぞれ表す。
相関をとるべき目標予測存在範囲内の２次元レーダ装置
１または画像センサ装置２からの観測ベクトルの総数を
ｍs k、観測ベクトルの全体をＺs kとし（式（５９）を参
照）、サンプリング時刻ｔ1からｔkまでの２次元レーダ
装置１または画像センサ装置２からの観測ベクトル全体
をＺk,sとする（式（６０）を参照）。また、Ｚk,sに対
する観測数の全体を式（６１）とし、さらにまた、式
（６２）とする。ここで、ｓ＝１は２次元レーダ装置１
を、ｓ＝３は画像センサ装置２をそれぞれ表す。
【０１１５】
【数２１】
【０１１６】次に、サンプリング時刻ｔk-1までの２次
元レーダ装置１または画像センサ装置により観測された
観測ベクトルＺs kが得られているときの、サンプリング
時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダ
バーハットｘk（−）、予測誤差共分散行列をＰk（−）
とすると、それぞれ条件付平均ベクトル、条件付共分散
行列で定義され、式（６３）及び式（６４）で表され
る。
元レーダ装置１または画像センサ装置により観測された
観測ベクトルＺs kが得られているときの、サンプリング
時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダ
バーハットｘk（−）、予測誤差共分散行列をＰk（−）
とすると、それぞれ条件付平均ベクトル、条件付共分散
行列で定義され、式（６３）及び式（６４）で表され
る。
【数２２】
【０１１７】ここで、アンダバーハットｘk（＋）及び
Ｐk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベ
クトル及び平滑誤差共分散行列であり、前サンプリング
時刻に２次元レーダ装置１からの観測ベクトルを処理し
た場合には式（６５）及び式（６６）、画像センサ装置
２からの観測ベクトルを処理した場合には式（６７）及
び式（６８）である。これらの算出方法については後で
述べる。
Ｐk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベ
クトル及び平滑誤差共分散行列であり、前サンプリング
時刻に２次元レーダ装置１からの観測ベクトルを処理し
た場合には式（６５）及び式（６６）、画像センサ装置
２からの観測ベクトルを処理した場合には式（６７）及
び式（６８）である。これらの算出方法については後で
述べる。
【０１１８】
【数２３】
【０１１９】次に、移動体の画像がサンプリング時刻ｔ
kにおいて画像センサ装置２より観測され、画像処理に
より画像の代表点の方位角が出力される場合の相関処理
について述べる。追尾対象からの観測ベクトルは、式
（６３）で与えられる予測位置ベクトルアンダバーｚ3 k
（−）を平均とし、式（６４）で与えられるＳ3 kを共分
散行列とする１次元正規分布に従うとする。そこで、式
（６５）を満たす観測ベクトルアンダバーｚ3 k,iのう
ち、値の最も小さい観測ベクトルアンダバーｚ3 kを追尾
対象と相関がある観測ベクトルと判定する。ここで、ｄ
3は追尾対象との相関範囲を決めるパラメータであり、
自由度１のχ自乗分布により算出する。
kにおいて画像センサ装置２より観測され、画像処理に
より画像の代表点の方位角が出力される場合の相関処理
について述べる。追尾対象からの観測ベクトルは、式
（６３）で与えられる予測位置ベクトルアンダバーｚ3 k
（−）を平均とし、式（６４）で与えられるＳ3 kを共分
散行列とする１次元正規分布に従うとする。そこで、式
（６５）を満たす観測ベクトルアンダバーｚ3 k,iのう
ち、値の最も小さい観測ベクトルアンダバーｚ3 kを追尾
対象と相関がある観測ベクトルと判定する。ここで、ｄ
3は追尾対象との相関範囲を決めるパラメータであり、
自由度１のχ自乗分布により算出する。
【０１２０】
【数２４】
【０１２１】次に、追尾対象の観測ベクトルがサンプリ
ング時刻ｔkにおいて、画像センサ装置２より観測され
る場合の平滑処理について述べる。ゲイン行列、平滑ベ
クトル及び平滑誤差共分散行列は拡張カルマンフィルタ
の理論により、次式で与えられる。
ング時刻ｔkにおいて、画像センサ装置２より観測され
る場合の平滑処理について述べる。ゲイン行列、平滑ベ
クトル及び平滑誤差共分散行列は拡張カルマンフィルタ
の理論により、次式で与えられる。
【０１２２】
【数２５】
【０１２３】次に、この実施の形態２に係る移動体追尾
装置の動作及び方法を具体的に説明する。２次元レーダ
装置１で、移動体の距離及び方位角を観測し、距離・方
位角確率分布算出器１２で、現時刻での存在が予測され
る追尾対象の距離及び方位角の確率分布Ｓ1 kを算出する
までは実施の形態１と同一動作を行う。
装置の動作及び方法を具体的に説明する。２次元レーダ
装置１で、移動体の距離及び方位角を観測し、距離・方
位角確率分布算出器１２で、現時刻での存在が予測され
る追尾対象の距離及び方位角の確率分布Ｓ1 kを算出する
までは実施の形態１と同一動作を行う。
【０１２４】画像センサ装置２では、画像から画像セン
サ観測座標系で移動体の方位角を検出し、その結果（観
測ベクトル）を方位角相関器２５に出力する。方位角相
関器２５では、画像センサ装置２から観測ベクトルを受
け取ると、その観測ベクトルから追尾対象と相関のある
観測ベクトルを選択するため、方位角確率分布算出器３
０により算出された方位角の確率分布Ｓ3 k（式（７０）
を参照）を用いて、追尾対象と相関の可能性のある観測
ベクトルを式（７１）を用いて選択する。そして、選択
された観測ベクトルのうち式（７１）の値の最も小さい
観測ベクトルアンダバーｚ3 kを追尾対象の観測ベクトル
と判定し、方位角による平滑器３３に出力する（ステッ
プＳＴ９）。
サ観測座標系で移動体の方位角を検出し、その結果（観
測ベクトル）を方位角相関器２５に出力する。方位角相
関器２５では、画像センサ装置２から観測ベクトルを受
け取ると、その観測ベクトルから追尾対象と相関のある
観測ベクトルを選択するため、方位角確率分布算出器３
０により算出された方位角の確率分布Ｓ3 k（式（７０）
を参照）を用いて、追尾対象と相関の可能性のある観測
ベクトルを式（７１）を用いて選択する。そして、選択
された観測ベクトルのうち式（７１）の値の最も小さい
観測ベクトルアンダバーｚ3 kを追尾対象の観測ベクトル
と判定し、方位角による平滑器３３に出力する（ステッ
プＳＴ９）。
【０１２５】方位角ゲイン行列算出器３２では、第３の
遅延回路１９から出力された予測誤差共分散行列（現時
刻より１サンプリング前に算出された予測誤差共分散行
列）と、方位角観測行列算出器２８から出力された式
（５４）の画像センサ装置２の観測行列と予め設定され
た式（５６）の画像センサ装置２の観測雑音分散とから
方位角のゲイン行列Ｋ3 kを式（７２）に従い算出する
（ステップＳＴ１０）。
遅延回路１９から出力された予測誤差共分散行列（現時
刻より１サンプリング前に算出された予測誤差共分散行
列）と、方位角観測行列算出器２８から出力された式
（５４）の画像センサ装置２の観測行列と予め設定され
た式（５６）の画像センサ装置２の観測雑音分散とから
方位角のゲイン行列Ｋ3 kを式（７２）に従い算出する
（ステップＳＴ１０）。
【０１２６】方位角による平滑器２７では、方位角相関
器２５から出力された追尾対象の観測ベクトルアンダバ
ーｚ3 kと、第１の座標変換器２４から出力された方位角
の予測値（現時刻より１サンプリング前に算出された予
測値）と、方位角ゲイン行列算出器２６により算出され
たゲイン行列Ｋ3 kとを用いて、追尾対象の位置及び速度
の平滑ベクトルを式（７３）に従い算出する（ステップ
ＳＴ１１）。
器２５から出力された追尾対象の観測ベクトルアンダバ
ーｚ3 kと、第１の座標変換器２４から出力された方位角
の予測値（現時刻より１サンプリング前に算出された予
測値）と、方位角ゲイン行列算出器２６により算出され
たゲイン行列Ｋ3 kとを用いて、追尾対象の位置及び速度
の平滑ベクトルを式（７３）に従い算出する（ステップ
ＳＴ１１）。
【０１２７】方位角観測行列算出器２８では、第１の座
標変換器２４から出力された方位角の予測値（現時刻よ
り１サンプリング前に算出された予測値）を入力し、画
像センサ装置２の観測行列を式（５８）に従い算出する
（ステップＳＴ１２）。
標変換器２４から出力された方位角の予測値（現時刻よ
り１サンプリング前に算出された予測値）を入力し、画
像センサ装置２の観測行列を式（５８）に従い算出する
（ステップＳＴ１２）。
【０１２８】方位角データによる平滑誤差評価器２９で
は、第１の座標変換器２４から出力された方位角の予測
値（現時刻より１サンプリング前に算出された予測値）
と、方位角観測行列算出器２８から出力された画像セン
サ装置２の観測行列と、方位角ゲイン行列算出器２６か
ら出力されたゲイン行列Ｋ3 kと、第３の遅延回路１９か
ら出力された予測誤差共分散行列（現時刻より１サンプ
リング前に算出された予測誤差共分散行列）とから平滑
誤差共分散行列を式（７４）に従い算出する（ステップ
ＳＴ１３）。
は、第１の座標変換器２４から出力された方位角の予測
値（現時刻より１サンプリング前に算出された予測値）
と、方位角観測行列算出器２８から出力された画像セン
サ装置２の観測行列と、方位角ゲイン行列算出器２６か
ら出力されたゲイン行列Ｋ3 kと、第３の遅延回路１９か
ら出力された予測誤差共分散行列（現時刻より１サンプ
リング前に算出された予測誤差共分散行列）とから平滑
誤差共分散行列を式（７４）に従い算出する（ステップ
ＳＴ１３）。
【０１２９】予測誤差評価器Ｂ１８では、方位角データ
による平滑誤差評価器２９から出力された平滑誤差共分
散行列を入力し、予測誤差共分散行列を式（６４）に従
い算出する（ステップＳＴ１４）。
による平滑誤差評価器２９から出力された平滑誤差共分
散行列を入力し、予測誤差共分散行列を式（６４）に従
い算出する（ステップＳＴ１４）。
【０１３０】方位角確率分布算出器３０は、第３の遅延
回路１９から出力された予測誤差共分散行列（現時刻よ
り１サンプリング前に算出された予測誤差共分散行列）
と、方位角観測行列算出器２８から出力された画像セン
サ装置２の観測行列と、予め設定された式（５６）の画
像センサ装置２の観測雑音分散とを入力し、現時刻での
存在が予測される追尾対象の方位角の確率分布Ｓ3 kを式
（７０）に従い算出する（ステップＳＴ１５）。
回路１９から出力された予測誤差共分散行列（現時刻よ
り１サンプリング前に算出された予測誤差共分散行列）
と、方位角観測行列算出器２８から出力された画像セン
サ装置２の観測行列と、予め設定された式（５６）の画
像センサ装置２の観測雑音分散とを入力し、現時刻での
存在が予測される追尾対象の方位角の確率分布Ｓ3 kを式
（７０）に従い算出する（ステップＳＴ１５）。
【０１３１】上記のようにして、距離・方位角による平
滑器７または方位角による平滑器２７または初期値算出
器５が追尾対象の位置及び速度の平滑値を算出すると、
位置・速度予測器２１はこれらのどちらかの平滑値と予
め設定された追尾対象の運動モデルを用いて、現時刻よ
り１サンプリング後の追尾対象の予測ベクトル（位置及
び速度）を式（６３）に従い算出し、第１の遅延回路２
２に出力する（ステップＳＴ１６）。
滑器７または方位角による平滑器２７または初期値算出
器５が追尾対象の位置及び速度の平滑値を算出すると、
位置・速度予測器２１はこれらのどちらかの平滑値と予
め設定された追尾対象の運動モデルを用いて、現時刻よ
り１サンプリング後の追尾対象の予測ベクトル（位置及
び速度）を式（６３）に従い算出し、第１の遅延回路２
２に出力する（ステップＳＴ１６）。
【０１３２】そして、次に追尾対象を観測するセンサが
２次元レーダ装置１の場合は、第３の座標変換器２３が
第１の遅延回路２２から予測ベクトルを入力し、式
（１）、式（２）、式（１９）を使用して２次元レーダ
装置１を基準とする極座標に変換する。一方、次に追尾
対象を観測するセンサが画像センサ装置２の場合は、第
１の座標変換器２４が第１の遅延回路２２から予測ベク
トルを入力し、式（３）、式（４）、式（２６）を使用
して画像センサ装置を基準とする極座標に変換する。以
上、追尾終了になるまでこの一連の処理をくり返す。
２次元レーダ装置１の場合は、第３の座標変換器２３が
第１の遅延回路２２から予測ベクトルを入力し、式
（１）、式（２）、式（１９）を使用して２次元レーダ
装置１を基準とする極座標に変換する。一方、次に追尾
対象を観測するセンサが画像センサ装置２の場合は、第
１の座標変換器２４が第１の遅延回路２２から予測ベク
トルを入力し、式（３）、式（４）、式（２６）を使用
して画像センサ装置を基準とする極座標に変換する。以
上、追尾終了になるまでこの一連の処理をくり返す。
【０１３３】以上で明らかなように、この実施の形態２
によれば、距離・方位角観測行列算出器８において、２
次元レーダ装置１の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、距離・方位角
による平滑器７で、この観測行列と、距離及び方位角の
観測ベクトルを用いて３次元位置及び速度の平滑ベクト
ルを算出し、また、方位角観測行列算出器２８におい
て、画像センサ装置２の観測モデルを３次元予測ベクト
ルのまわりで線形近似した観測行列を算出し、方位角に
よる平滑器２７で、この観測行列と、方位角の観測ベク
トルを用いて３次元位置及び速度の平滑ベクトルを算出
する構成としている。
によれば、距離・方位角観測行列算出器８において、２
次元レーダ装置１の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、距離・方位角
による平滑器７で、この観測行列と、距離及び方位角の
観測ベクトルを用いて３次元位置及び速度の平滑ベクト
ルを算出し、また、方位角観測行列算出器２８におい
て、画像センサ装置２の観測モデルを３次元予測ベクト
ルのまわりで線形近似した観測行列を算出し、方位角に
よる平滑器２７で、この観測行列と、方位角の観測ベク
トルを用いて３次元位置及び速度の平滑ベクトルを算出
する構成としている。
【０１３４】このため、追尾対象の追尾開始領域の道路
高の情報を利用することによって初期値算出器５により
算出した３次元位置及び速度の初期値をもとに、２次元
レーダ装置１の観測する距離及び方位角と、画像センサ
装置２の観測する俯角とを用いて逐次的に追尾対象の３
次元位置及び速度を推定することができる。従って、２
次元レーダと方位角を観測する画像センサを併用するこ
とにより、移動体の３次元運動を追尾できる効果を奏す
る。
高の情報を利用することによって初期値算出器５により
算出した３次元位置及び速度の初期値をもとに、２次元
レーダ装置１の観測する距離及び方位角と、画像センサ
装置２の観測する俯角とを用いて逐次的に追尾対象の３
次元位置及び速度を推定することができる。従って、２
次元レーダと方位角を観測する画像センサを併用するこ
とにより、移動体の３次元運動を追尾できる効果を奏す
る。
【０１３５】なお、この実施の形態２は、２次元レーダ
装置の代わりにレーザ装置や音波センサ装置を用いて追
尾対象の距離及び方位角を観測した場合にも適用可能で
ある。また、この実施の形態２では、３次元位置及び速
度の初期位置の計算に２次元レーダ装置１の観測ベクト
ルを用いているが、画像センサ装置２の観測ベクトルを
用いて計算してもよい。
装置の代わりにレーザ装置や音波センサ装置を用いて追
尾対象の距離及び方位角を観測した場合にも適用可能で
ある。また、この実施の形態２では、３次元位置及び速
度の初期位置の計算に２次元レーダ装置１の観測ベクト
ルを用いているが、画像センサ装置２の観測ベクトルを
用いて計算してもよい。
【０１３６】実施の形態３．図９は、この実施の形態３
による移動体追尾装置及び方法を説明するための構成図
である。図９において、図１に示す実施の形態１と同一
部分は同一符号を付してその説明は省略する。すなわ
ち、１、３〜１２、２１〜２４は実施の形態１と同一部
分を示し、その説明は省略する。
による移動体追尾装置及び方法を説明するための構成図
である。図９において、図１に示す実施の形態１と同一
部分は同一符号を付してその説明は省略する。すなわ
ち、１、３〜１２、２１〜２４は実施の形態１と同一部
分を示し、その説明は省略する。
【０１３７】２は画像センサ観測座標系において画像を
観測し、移動体の代表点の方位角を出力する本実施の形
態３に係る画像センサ装置である。３１は画像センサ装
置２から画像センサ観測座標系における追尾対象の俯角
及び方位角を入力し、これらの観測ベクトルのうち、追
尾対象が存在する可能性の高い観測ベクトルを、俯角・
方位角確率分布算出器３７から入力した追尾対象の俯角
及び方位角の確率分布をもとに選択する俯角・方位角相
関器である。
観測し、移動体の代表点の方位角を出力する本実施の形
態３に係る画像センサ装置である。３１は画像センサ装
置２から画像センサ観測座標系における追尾対象の俯角
及び方位角を入力し、これらの観測ベクトルのうち、追
尾対象が存在する可能性の高い観測ベクトルを、俯角・
方位角確率分布算出器３７から入力した追尾対象の俯角
及び方位角の確率分布をもとに選択する俯角・方位角相
関器である。
【０１３８】３２は第３の遅延回路１９から出力される
現時刻より１サンプリング前に算出された予測誤差共分
散行列と、後述する俯角・方位角観測行列算出器２８か
ら出力される俯角及び方位角の観測行列と、画像センサ
装置２の観測誤共分散行列とから俯角及び方位角のゲイ
ン行列を算出する俯角・方位角ゲイン行列算出器であ
る。
現時刻より１サンプリング前に算出された予測誤差共分
散行列と、後述する俯角・方位角観測行列算出器２８か
ら出力される俯角及び方位角の観測行列と、画像センサ
装置２の観測誤共分散行列とから俯角及び方位角のゲイ
ン行列を算出する俯角・方位角ゲイン行列算出器であ
る。
【０１３９】３３は俯角・方位角相関器３１により選択
された観測ベクトル、第１の座標変換器２４から出力さ
れる俯角及び方位角の予測ベクトルと３次元予測ベクト
ル、俯角・方位角ゲイン行列算出器３２により算出され
た俯角及び方位角のゲイン行列を用いて追尾対象の３次
元位置及び速度の平滑ベクトルを算出する俯角・方位角
による平滑器である。
された観測ベクトル、第１の座標変換器２４から出力さ
れる俯角及び方位角の予測ベクトルと３次元予測ベクト
ル、俯角・方位角ゲイン行列算出器３２により算出され
た俯角及び方位角のゲイン行列を用いて追尾対象の３次
元位置及び速度の平滑ベクトルを算出する俯角・方位角
による平滑器である。
【０１４０】３４は第１の座標変換器２４から出力され
る俯角及び方位角の予測ベクトルを入力し、俯角及び方
位角の観測行列を算出する俯角・方位角観測行列算出
器、３５は俯角・方位角データによる平滑誤差評価器で
あって、第１の座標変換器２４から出力される俯角及び
方位角の予測ベクトルと、俯角・方位角観測行列算出器
３４から出力される俯角及び方位角の観測行列と、俯角
・方位角ゲイン行列算出器３２から出力される俯角及び
方位角のゲイン行列と、第３の遅延回路１９から出力さ
れる現時刻より１サンプリング前に算出された予測誤差
共分散行列とを用いて追尾対象の３次元位置及び速度の
平滑誤差を評価する平滑誤差共分散行列を算出する。
る俯角及び方位角の予測ベクトルを入力し、俯角及び方
位角の観測行列を算出する俯角・方位角観測行列算出
器、３５は俯角・方位角データによる平滑誤差評価器で
あって、第１の座標変換器２４から出力される俯角及び
方位角の予測ベクトルと、俯角・方位角観測行列算出器
３４から出力される俯角及び方位角の観測行列と、俯角
・方位角ゲイン行列算出器３２から出力される俯角及び
方位角のゲイン行列と、第３の遅延回路１９から出力さ
れる現時刻より１サンプリング前に算出された予測誤差
共分散行列とを用いて追尾対象の３次元位置及び速度の
平滑誤差を評価する平滑誤差共分散行列を算出する。
【０１４１】１８は俯角・方位角データによる平滑誤差
評価器３５により算出された平滑誤差共分散行列を入力
し、追尾対象の３次元位置及び速度の予測誤差を評価す
る予測誤差共分散行列を算出する予測誤差評価器Ｂであ
る。１９は予測誤差評価器Ｂ１８により算出された予測
誤差共分散行列を１サンプリング時間だけ遅延して出力
する第３の遅延回路、３７は第３の遅延回路１９の出力
する予測誤差共分散行列と、俯角・方位角観測行列算出
器３４で算出した俯角及び方位角の観測行列と、予め設
定された画像センサ装置２の観測誤差共分差行列を入力
し、追尾対象の俯角及び方位角の確率分布を算出する俯
角・方位角確率分布算出器である。
評価器３５により算出された平滑誤差共分散行列を入力
し、追尾対象の３次元位置及び速度の予測誤差を評価す
る予測誤差共分散行列を算出する予測誤差評価器Ｂであ
る。１９は予測誤差評価器Ｂ１８により算出された予測
誤差共分散行列を１サンプリング時間だけ遅延して出力
する第３の遅延回路、３７は第３の遅延回路１９の出力
する予測誤差共分散行列と、俯角・方位角観測行列算出
器３４で算出した俯角及び方位角の観測行列と、予め設
定された画像センサ装置２の観測誤差共分差行列を入力
し、追尾対象の俯角及び方位角の確率分布を算出する俯
角・方位角確率分布算出器である。
【０１４２】２１は初期値算出器５、距離・方位角によ
る平滑器７、俯角・方位角による平滑器３３のいずれか
から出力される追尾対象の３次元位置及び速度から成る
３次元平滑ベクトルと、予め設定された追尾対象の運動
モデルを用いて現時刻より１サンプリング後の追尾対象
の３次元予測位置及び予測速度から成る３次元予測ベク
トルを算出する位置・速度予測器である。
る平滑器７、俯角・方位角による平滑器３３のいずれか
から出力される追尾対象の３次元位置及び速度から成る
３次元平滑ベクトルと、予め設定された追尾対象の運動
モデルを用いて現時刻より１サンプリング後の追尾対象
の３次元予測位置及び予測速度から成る３次元予測ベク
トルを算出する位置・速度予測器である。
【０１４３】２２は位置・速度予測器２１により算出さ
れた追尾対象の３次元予測ベクトルを１サンプリング時
間だけ遅延して出力する第１の遅延回路、２３は第１の
遅延回路２２の出力する現時刻より１サンプリング前の
追尾対象の３次元予測ベクトルを距離及び方位角の予測
ベクトルに座標変換し、これと変換前の３次元予測ベク
トルを出力する第２の座標変換器、２４は第１の遅延回
路２２の出力する現時刻より１サンプリング前の追尾対
象の３次元予測ベクトルを俯角及び方位角の予測ベクト
ルに座標変換し、これと変換前の３次元予測ベクトルを
出力する第３の座標変換器である。
れた追尾対象の３次元予測ベクトルを１サンプリング時
間だけ遅延して出力する第１の遅延回路、２３は第１の
遅延回路２２の出力する現時刻より１サンプリング前の
追尾対象の３次元予測ベクトルを距離及び方位角の予測
ベクトルに座標変換し、これと変換前の３次元予測ベク
トルを出力する第２の座標変換器、２４は第１の遅延回
路２２の出力する現時刻より１サンプリング前の追尾対
象の３次元予測ベクトルを俯角及び方位角の予測ベクト
ルに座標変換し、これと変換前の３次元予測ベクトルを
出力する第３の座標変換器である。
【０１４４】次に動作について説明する。最初に、この
実施の形態３による移動体追尾装置及び方法の動作原理
を説明する。ただし、２次元レーダ装置１と画像センサ
装置２の観測座標系の定義、観測座標系と基準座標系の
関係、移動体の運動モデル、２次元レーダ装置１の観測
モデルの定義に関して、すなわち、式（１）〜式（１
９）に表される関係と、２次元レーダ装置１の相関処理
及び平滑処理に関して、すなわち、式（３７）〜式（４
２）に表される関係と、追尾初期化方法に関して、すな
わち、式（４９）〜式（５２）で表される関係とについ
ては実施の形態１と同様である。
実施の形態３による移動体追尾装置及び方法の動作原理
を説明する。ただし、２次元レーダ装置１と画像センサ
装置２の観測座標系の定義、観測座標系と基準座標系の
関係、移動体の運動モデル、２次元レーダ装置１の観測
モデルの定義に関して、すなわち、式（１）〜式（１
９）に表される関係と、２次元レーダ装置１の相関処理
及び平滑処理に関して、すなわち、式（３７）〜式（４
２）に表される関係と、追尾初期化方法に関して、すな
わち、式（４９）〜式（５２）で表される関係とについ
ては実施の形態１と同様である。
【０１４５】追尾対象の俯角及び方位角がサンプリング
時刻ｔkに画像センサ装置２より観測される場合の画像
センサ装置２の観測モデルを式（７５）により表す。た
だし、アンダバーｚ4 kはサンプリング時刻ｔkにおける
画像センサ装置２の俯角及び方位角の観測ベクトルであ
る。ｈ4（アンダバーｘk）は画像センサ装置２の観測ベ
クトルの真値で、式（３）及び式（４）より式（７６）
で表される。アンダバーν4 kはサンプリング時刻ｔkに
おける画像センサ装置２の観測雑音ベクトルであり、平
均アンダバーＯの２次元白色正規分布で、式（７７）及
び（７８）で表される。なお、Ｒ4 kはサンプリング時刻
ｔkにおける画像センサ装置２の観測雑音共分散行列で
ある。
時刻ｔkに画像センサ装置２より観測される場合の画像
センサ装置２の観測モデルを式（７５）により表す。た
だし、アンダバーｚ4 kはサンプリング時刻ｔkにおける
画像センサ装置２の俯角及び方位角の観測ベクトルであ
る。ｈ4（アンダバーｘk）は画像センサ装置２の観測ベ
クトルの真値で、式（３）及び式（４）より式（７６）
で表される。アンダバーν4 kはサンプリング時刻ｔkに
おける画像センサ装置２の観測雑音ベクトルであり、平
均アンダバーＯの２次元白色正規分布で、式（７７）及
び（７８）で表される。なお、Ｒ4 kはサンプリング時刻
ｔkにおける画像センサ装置２の観測雑音共分散行列で
ある。
【０１４６】
【数２６】
【０１４７】画像センサ装置２の真値ｈ4（アンダバー
ｘk）の線形近似は式（７８）より得られる。式（７
９）は画像センサ装置２の観測行列である。ここで、Ｒ
p、Ｅp及びＡｚpは３次元予測ベクトルアンダバーハッ
トｘk（−）を式（３）、式（４）及び式（２６）の関
係式を用いて画像センサ観測座標系における距離、俯
角、方位角に変換したものである。
ｘk）の線形近似は式（７８）より得られる。式（７
９）は画像センサ装置２の観測行列である。ここで、Ｒ
p、Ｅp及びＡｚpは３次元予測ベクトルアンダバーハッ
トｘk（−）を式（３）、式（４）及び式（２６）の関
係式を用いて画像センサ観測座標系における距離、俯
角、方位角に変換したものである。
【０１４８】
【数２７】
【０１４９】サンプリング時刻ｔkにおける追尾目標と
相関をとるべき目標予測存在範囲内の２次元レーダ装置
１または画像センサ装置２からの観測ベクトルの総数を
ｍs k、観測ベクトルの全体をＺs kとし（式（８１）を参
照）、サンプリング時刻ｔ1からｔkまでの２次元レーダ
装置１または画像センサ装置２からの観測ベクトル全体
をＺk,sとする（式（８２）を参照）。また、Ｚk,sに対
する観測数の全体を式（８３）とし、さらにまた、式
（８４）とする。ここで、ｓ＝１は２次元レーダ装置１
を、ｓ＝４は画像センサ装置２をそれぞれ表す。
相関をとるべき目標予測存在範囲内の２次元レーダ装置
１または画像センサ装置２からの観測ベクトルの総数を
ｍs k、観測ベクトルの全体をＺs kとし（式（８１）を参
照）、サンプリング時刻ｔ1からｔkまでの２次元レーダ
装置１または画像センサ装置２からの観測ベクトル全体
をＺk,sとする（式（８２）を参照）。また、Ｚk,sに対
する観測数の全体を式（８３）とし、さらにまた、式
（８４）とする。ここで、ｓ＝１は２次元レーダ装置１
を、ｓ＝４は画像センサ装置２をそれぞれ表す。
【０１５０】
【数２８】
【０１５１】次に、サンプリング時刻ｔk-1までの２次
元レーダ装置１または画像センサ装置により観測された
観測ベクトルＺs kが得られているときの、サンプリング
時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダ
バーハットｘk（−）、予測誤差共分散行列をＰk（−）
とすると、それぞれ条件付平均ベクトル、条件付共分散
行列で定義され、式（８５）及び式（８６）で表され
る。
元レーダ装置１または画像センサ装置により観測された
観測ベクトルＺs kが得られているときの、サンプリング
時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダ
バーハットｘk（−）、予測誤差共分散行列をＰk（−）
とすると、それぞれ条件付平均ベクトル、条件付共分散
行列で定義され、式（８５）及び式（８６）で表され
る。
【数２９】
【０１５２】ここで、アンダバーハットｘk（＋）及び
Ｐk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベ
クトル及び平滑誤差共分散行列であり、前サンプリング
時刻ｔk -1に２次元レーダ装置１からの観測ベクトルを
処理した場合には式（８７）及び式（８８）、画像セン
サ装置２からの観測ベクトルを処理した場合には式（８
９）及び式（９０）である。これらの算出方法について
は後で述べる。
Ｐk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベ
クトル及び平滑誤差共分散行列であり、前サンプリング
時刻ｔk -1に２次元レーダ装置１からの観測ベクトルを
処理した場合には式（８７）及び式（８８）、画像セン
サ装置２からの観測ベクトルを処理した場合には式（８
９）及び式（９０）である。これらの算出方法について
は後で述べる。
【０１５３】
【数３０】
【０１５４】次に、移動体の画像がサンプリング時刻ｔ
kにおいて画像センサ装置２より観測され、画像処理に
より画像の代表点の俯角及び方位角が出力される場合の
相関処理について述べる。追尾対象からの観測ベクトル
は、式（９１）で与えられる予測位置ベクトルアンダバ
ーｚ4 k（−）を平均とし、式（９２）で与えられるＳ 4 k
を共分散行列とする２次元正規分布に従うとする。そこ
で、式（９３）を満たす観測ベクトルアンダバーｚ4 k,i
のうち、値の最も小さい観測ベクトルアンダバーｚ4 kを
追尾対象と相関がある観測ベクトルと判定する。ここ
で、ｄ4は追尾対象との相関範囲を決めるパラメータで
あり、自由度２のχ自乗分布により算出する。
kにおいて画像センサ装置２より観測され、画像処理に
より画像の代表点の俯角及び方位角が出力される場合の
相関処理について述べる。追尾対象からの観測ベクトル
は、式（９１）で与えられる予測位置ベクトルアンダバ
ーｚ4 k（−）を平均とし、式（９２）で与えられるＳ 4 k
を共分散行列とする２次元正規分布に従うとする。そこ
で、式（９３）を満たす観測ベクトルアンダバーｚ4 k,i
のうち、値の最も小さい観測ベクトルアンダバーｚ4 kを
追尾対象と相関がある観測ベクトルと判定する。ここ
で、ｄ4は追尾対象との相関範囲を決めるパラメータで
あり、自由度２のχ自乗分布により算出する。
【０１５５】
【数３１】
【０１５６】次に、追尾対象の観測ベクトルがサンプリ
ング時刻ｔkにおいて、画像センサ装置２より観測され
る場合の平滑処理について述べる。ゲイン行列、平滑ベ
クトル及び平滑誤差共分散行列は拡張カルマンフィルタ
の理論により、次式で与えられる。
ング時刻ｔkにおいて、画像センサ装置２より観測され
る場合の平滑処理について述べる。ゲイン行列、平滑ベ
クトル及び平滑誤差共分散行列は拡張カルマンフィルタ
の理論により、次式で与えられる。
【０１５７】
【数３２】
【０１５８】次に、この発明の移動体追尾装置の動作及
び方法を具体的に説明する。２次元レーダ装置１で、移
動体の距離及び方位角を観測し、距離・方位角確率分布
算出器１２で、現時刻での存在が予測される追尾対象の
距離及び方位角の確率分布Ｓ1 kを算出するまでは実施の
形態１と同一動作を行う。
び方法を具体的に説明する。２次元レーダ装置１で、移
動体の距離及び方位角を観測し、距離・方位角確率分布
算出器１２で、現時刻での存在が予測される追尾対象の
距離及び方位角の確率分布Ｓ1 kを算出するまでは実施の
形態１と同一動作を行う。
【０１５９】画像センサ装置２では、画像から画像セン
サ観測座標系で移動体の俯角及び方位角を検出し、その
結果（観測ベクトル）を俯角・方位角相関器３１に出力
する。俯角・方位角相関器３１では、画像センサ装置２
から観測ベクトルを受け取ると、その観測ベクトルから
追尾対象と相関のある観測ベクトルを選択するため、俯
角・方位角確率分布算出器３７により算出された俯角及
び方位角の確率分布Ｓ 4 k（式（９２）を参照）を用い
て、追尾対象と相関の可能性のある観測ベクトルを式
（９３）を用いて選択する。そして、選択された観測ベ
クトルのうち式（９３）の値の最も小さい観測ベクトル
アンダバーｚ4 kを追尾対象の観測ベクトルと判定し、俯
角・方位角による平滑器３３に出力する（ステップＳＴ
９）。
サ観測座標系で移動体の俯角及び方位角を検出し、その
結果（観測ベクトル）を俯角・方位角相関器３１に出力
する。俯角・方位角相関器３１では、画像センサ装置２
から観測ベクトルを受け取ると、その観測ベクトルから
追尾対象と相関のある観測ベクトルを選択するため、俯
角・方位角確率分布算出器３７により算出された俯角及
び方位角の確率分布Ｓ 4 k（式（９２）を参照）を用い
て、追尾対象と相関の可能性のある観測ベクトルを式
（９３）を用いて選択する。そして、選択された観測ベ
クトルのうち式（９３）の値の最も小さい観測ベクトル
アンダバーｚ4 kを追尾対象の観測ベクトルと判定し、俯
角・方位角による平滑器３３に出力する（ステップＳＴ
９）。
【０１６０】俯角・方位角ゲイン行列算出器３２では、
第３の遅延回路１９から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）と、俯角・方位角観測行列算出器３４から出
力された式（８０）の画像センサ装置２の観測行列と予
め設定された式（７８）の画像センサ装置２の観測雑音
共分散行列とから俯角及び方位角のゲイン行列Ｋ4 kを式
（９４）に従い算出する（ステップＳＴ１０）。
第３の遅延回路１９から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）と、俯角・方位角観測行列算出器３４から出
力された式（８０）の画像センサ装置２の観測行列と予
め設定された式（７８）の画像センサ装置２の観測雑音
共分散行列とから俯角及び方位角のゲイン行列Ｋ4 kを式
（９４）に従い算出する（ステップＳＴ１０）。
【０１６１】俯角・方位角による平滑器３３では、俯角
・方位角相関器３１から出力された追尾対象の観測ベク
トル と、第１の座標変換器２４から出力された俯角及
び方位角の予測値（現時刻より１サンプリング前に算出
された予測値）と、俯角・方位角ゲイン行列算出器３２
により算出されたゲイン行列Ｋ4 kとを用いて、追尾対象
の位置及び速度の平滑ベクトルを式（９５）に従い算出
する（ステップＳＴ１１）。
・方位角相関器３１から出力された追尾対象の観測ベク
トル と、第１の座標変換器２４から出力された俯角及
び方位角の予測値（現時刻より１サンプリング前に算出
された予測値）と、俯角・方位角ゲイン行列算出器３２
により算出されたゲイン行列Ｋ4 kとを用いて、追尾対象
の位置及び速度の平滑ベクトルを式（９５）に従い算出
する（ステップＳＴ１１）。
【０１６２】俯角・方位角観測行列算出器３４では、第
１の座標変換器２４から出力された俯角及び方位角の予
測値（現時刻より１サンプリング前に算出された予測
値）を入力し、画像センサ装置２の観測行列を式（８
０）に従い算出する（ステップＳＴ１２）。
１の座標変換器２４から出力された俯角及び方位角の予
測値（現時刻より１サンプリング前に算出された予測
値）を入力し、画像センサ装置２の観測行列を式（８
０）に従い算出する（ステップＳＴ１２）。
【０１６３】俯角・方位角データによる平滑誤差評価器
３５では、第１の座標変換器２４から出力された俯角及
び方位角の予測値（現時刻より１サンプリング前に算出
された予測値）と、俯角・方位角観測行列算出器３４か
ら出力された画像センサ装置２の観測行列と、俯角・方
位角ゲイン行列算出器３２から出力されたゲイン行列
と、第３の遅延回路１９から出力された予測誤差共分散
行列（現時刻より１サンプリング前に算出された予測誤
差共分散行列）とから平滑誤差共分散行列を式（９６）
に従い算出する（ステップＳＴ１３）。
３５では、第１の座標変換器２４から出力された俯角及
び方位角の予測値（現時刻より１サンプリング前に算出
された予測値）と、俯角・方位角観測行列算出器３４か
ら出力された画像センサ装置２の観測行列と、俯角・方
位角ゲイン行列算出器３２から出力されたゲイン行列
と、第３の遅延回路１９から出力された予測誤差共分散
行列（現時刻より１サンプリング前に算出された予測誤
差共分散行列）とから平滑誤差共分散行列を式（９６）
に従い算出する（ステップＳＴ１３）。
【０１６４】予測誤差評価器Ｂ１８では、俯角・方位角
データによる平滑誤差評価器３５から出力された平滑誤
差共分散行列を入力し、予測誤差共分散行列を式（８
６）に従い算出する（ステップＳＴ１４）。
データによる平滑誤差評価器３５から出力された平滑誤
差共分散行列を入力し、予測誤差共分散行列を式（８
６）に従い算出する（ステップＳＴ１４）。
【０１６５】俯角・方位角確率分布算出器３７は、第３
の遅延回路１９から出力された予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）と、俯角・方位角観測行列算出器３４から出力さ
れた画像センサ装置２の観測行列と、予め設定された式
（７８）の画像センサ装置２の観測誤差共分散行列とを
入力し、現時刻での存在が予測される追尾対象の俯角及
び方位角の確率分布Ｓ 4 kを式（９２）に従い算出する
（ステップＳＴ１５）。
の遅延回路１９から出力された予測誤差共分散行列（現
時刻より１サンプリング前に算出された予測誤差共分散
行列）と、俯角・方位角観測行列算出器３４から出力さ
れた画像センサ装置２の観測行列と、予め設定された式
（７８）の画像センサ装置２の観測誤差共分散行列とを
入力し、現時刻での存在が予測される追尾対象の俯角及
び方位角の確率分布Ｓ 4 kを式（９２）に従い算出する
（ステップＳＴ１５）。
【０１６６】上記のようにして、距離・方位角による平
滑器７または俯角及び方位角による平滑器３３または初
期値算出器５が追尾対象の位置及び速度の平滑値を算出
すると、位置・速度予測器２１はこれらのどちらかの平
滑値と予め設定された追尾対象の運動モデルを用いて、
現時刻より１サンプリング後の追尾対象の予測ベクトル
（位置及び速度）を式（８５）に従い算出し、第１の遅
延回路２２に出力する（ステップＳＴ１６）。
滑器７または俯角及び方位角による平滑器３３または初
期値算出器５が追尾対象の位置及び速度の平滑値を算出
すると、位置・速度予測器２１はこれらのどちらかの平
滑値と予め設定された追尾対象の運動モデルを用いて、
現時刻より１サンプリング後の追尾対象の予測ベクトル
（位置及び速度）を式（８５）に従い算出し、第１の遅
延回路２２に出力する（ステップＳＴ１６）。
【０１６７】そして、次に追尾対象を観測するセンサが
２次元レーダ装置１の場合は、第３の座標変換器２３が
第１の遅延回路２２から予測ベクトルを入力し、式
（１）、式（２）、式（１９）を使用して２次元レーダ
装置１を基準とする極座標に変換する。一方、次に追尾
対象を観測するセンサが画像センサ装置２の場合は、第
１の座標変換器２４が第１の遅延回路２２から予測ベク
トルを入力し、式（３）、式（４）、式（２６）を使用
して画像センサ装置を基準とする極座標に変換する。以
上、追尾終了になるまでこの一連の処理をくり返す。
２次元レーダ装置１の場合は、第３の座標変換器２３が
第１の遅延回路２２から予測ベクトルを入力し、式
（１）、式（２）、式（１９）を使用して２次元レーダ
装置１を基準とする極座標に変換する。一方、次に追尾
対象を観測するセンサが画像センサ装置２の場合は、第
１の座標変換器２４が第１の遅延回路２２から予測ベク
トルを入力し、式（３）、式（４）、式（２６）を使用
して画像センサ装置を基準とする極座標に変換する。以
上、追尾終了になるまでこの一連の処理をくり返す。
【０１６８】以上で明らかなように、この実施の形態３
によれば、距離・方位角観測行列算出器８において、２
次元レーダ装置１の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、距離・方位角
による平滑器７で、この観測行列と、距離及び方位角の
観測ベクトルを用いて３次元位置及び速度の平滑ベクト
ルを算出し、また、俯角・方位角観測行列算出器３４に
おいて、画像センサ装置２の観測モデルを３次元予測ベ
クトルのまわりで線形近似した観測行列を算出し、俯角
・方位角による平滑器３３で、この観測行列と、俯角及
び方位角の観測ベクトルを用いて３次元位置及び速度の
平滑ベクトルを算出する構成としている。
によれば、距離・方位角観測行列算出器８において、２
次元レーダ装置１の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、距離・方位角
による平滑器７で、この観測行列と、距離及び方位角の
観測ベクトルを用いて３次元位置及び速度の平滑ベクト
ルを算出し、また、俯角・方位角観測行列算出器３４に
おいて、画像センサ装置２の観測モデルを３次元予測ベ
クトルのまわりで線形近似した観測行列を算出し、俯角
・方位角による平滑器３３で、この観測行列と、俯角及
び方位角の観測ベクトルを用いて３次元位置及び速度の
平滑ベクトルを算出する構成としている。
【０１６９】このため、追尾対象の追尾開始領域の道路
高の情報を利用することによって初期値算出器５により
算出した３次元位置及び速度の初期値をもとに、２次元
レーダ装置１の観測する距離及び方位角と、画像センサ
装置２の観測する俯角とを用いて逐次的に追尾対象の３
次元位置及び速度を推定することができる。従って、２
次元レーダと俯角及び方位角を観測する画像センサを併
用することにより、移動体の３次元運動を追尾できる効
果を奏する。また、画像センサ装置２の俯角及び方位角
の観測データを用いて平滑ベクトルを算出しているの
で、実施の形態１または実施の形態２に比べて追尾性能
が良い。
高の情報を利用することによって初期値算出器５により
算出した３次元位置及び速度の初期値をもとに、２次元
レーダ装置１の観測する距離及び方位角と、画像センサ
装置２の観測する俯角とを用いて逐次的に追尾対象の３
次元位置及び速度を推定することができる。従って、２
次元レーダと俯角及び方位角を観測する画像センサを併
用することにより、移動体の３次元運動を追尾できる効
果を奏する。また、画像センサ装置２の俯角及び方位角
の観測データを用いて平滑ベクトルを算出しているの
で、実施の形態１または実施の形態２に比べて追尾性能
が良い。
【０１７０】なお、この実施の形態は、２次元レーダ装
置の代わりにレーザ装置や音波センサ装置を用いて追尾
対象の距離及び方位角を観測した場合にも適用可能であ
る。また、この実施の形態では、３次元位置及び速度の
初期位置の計算に２次元レーダ装置１の観測ベクトルを
用いているが、画像センサ装置２の観測ベクトルを用い
て計算してもよい。
置の代わりにレーザ装置や音波センサ装置を用いて追尾
対象の距離及び方位角を観測した場合にも適用可能であ
る。また、この実施の形態では、３次元位置及び速度の
初期位置の計算に２次元レーダ装置１の観測ベクトルを
用いているが、画像センサ装置２の観測ベクトルを用い
て計算してもよい。
【０１７１】実施の形態４．図１０は、この実施の形態
４による移動体追尾装置及び方法を説明するための構成
図である。図１０において、図１に示す実施の形態１と
同一部分は同一符号を付してその説明は省略する。すな
わち、１、３〜１２、２１〜２４は実施の形態１と同一
部分を示し、その説明は省略する。
４による移動体追尾装置及び方法を説明するための構成
図である。図１０において、図１に示す実施の形態１と
同一部分は同一符号を付してその説明は省略する。すな
わち、１、３〜１２、２１〜２４は実施の形態１と同一
部分を示し、その説明は省略する。
【０１７２】３８は２次元レーダ装置１と同様にレーダ
観測座標系において移動体の距離及び方位角を出力する
２次元レーダ装置Ｂである。３９は２次元レーダ装置Ｂ
３８からレーダ観測座標系における追尾対象の距離及び
方位角を入力し、これらの観測ベクトルのうち、追尾対
象が存在する可能性の高い観測ベクトルを、距離・方位
角確率分布算出器Ｂ４４から入力した追尾対象の距離及
び方位角の確率分布をもとに選択する距離・方位角相関
器Ｂである。
観測座標系において移動体の距離及び方位角を出力する
２次元レーダ装置Ｂである。３９は２次元レーダ装置Ｂ
３８からレーダ観測座標系における追尾対象の距離及び
方位角を入力し、これらの観測ベクトルのうち、追尾対
象が存在する可能性の高い観測ベクトルを、距離・方位
角確率分布算出器Ｂ４４から入力した追尾対象の距離及
び方位角の確率分布をもとに選択する距離・方位角相関
器Ｂである。
【０１７３】４０は第３の遅延回路１９から出力される
現時刻より１サンプリング前に算出された予測誤差共分
散行列と、後述する距離・方位角観測行列算出器Ｂ４２
から出力されるレーダ観測行列と、２次元レーダ装置Ｂ
３８の観測誤差共分散行列とから距離と方位角のゲイン
行列を算出する距離・方位角ゲイン行列算出器Ｂであ
る。
現時刻より１サンプリング前に算出された予測誤差共分
散行列と、後述する距離・方位角観測行列算出器Ｂ４２
から出力されるレーダ観測行列と、２次元レーダ装置Ｂ
３８の観測誤差共分散行列とから距離と方位角のゲイン
行列を算出する距離・方位角ゲイン行列算出器Ｂであ
る。
【０１７４】４１は距離・方位角相関器Ｂ３９により選
択された観測ベクトル、第３の座標変換器１９から出力
される距離及び方位角の予測ベクトルと３次元予測ベク
トル、距離・方位角ゲイン行列算出器Ｂ４０により算出
された距離と方位角のゲイン行列を用いて追尾対象の３
次元位置及び速度の平滑ベクトルを算出する距離・方位
角による平滑器Ｂ、４２は第１の座標変換器２４から出
力される距離及び方位角の予測ベクトルを入力し、距離
・方位角の観測行列を算出する距離・方位角観測行列算
出器Ｂである。
択された観測ベクトル、第３の座標変換器１９から出力
される距離及び方位角の予測ベクトルと３次元予測ベク
トル、距離・方位角ゲイン行列算出器Ｂ４０により算出
された距離と方位角のゲイン行列を用いて追尾対象の３
次元位置及び速度の平滑ベクトルを算出する距離・方位
角による平滑器Ｂ、４２は第１の座標変換器２４から出
力される距離及び方位角の予測ベクトルを入力し、距離
・方位角の観測行列を算出する距離・方位角観測行列算
出器Ｂである。
【０１７５】４３は距離・方位角データによる平滑誤差
評価器Ｂであって、第１の座標変換器２４から出力され
る距離及び方位角の予測ベクトルと、距離・方位角観測
行列算出器Ｂ４２から出力される距離・方位角の観測行
列と、距離・方位角ゲイン行列算出器Ｂ４０から出力さ
れる距離と方位角のゲイン行列と、第３の遅延回路１９
から出力される現時刻より１サンプリング前に算出され
た予測誤差共分散行列とを用いて追尾対象の３次元位置
及び速度の平滑誤差を評価する平滑誤差共分散行列を算
出する。
評価器Ｂであって、第１の座標変換器２４から出力され
る距離及び方位角の予測ベクトルと、距離・方位角観測
行列算出器Ｂ４２から出力される距離・方位角の観測行
列と、距離・方位角ゲイン行列算出器Ｂ４０から出力さ
れる距離と方位角のゲイン行列と、第３の遅延回路１９
から出力される現時刻より１サンプリング前に算出され
た予測誤差共分散行列とを用いて追尾対象の３次元位置
及び速度の平滑誤差を評価する平滑誤差共分散行列を算
出する。
【０１７６】１８は距離・方位角データによる平滑誤差
評価器Ｂ４３により算出された平滑誤差共分散行列を入
力し、追尾対象の３次元位置及び速度の予測誤差を評価
する予測誤差共分散行列を算出する予測誤差評価器Ｂで
ある。１９は予測誤差評価器Ｂ１８により算出された予
測誤差共分散行列を１サンプリング時間だけ遅延して出
力する第３の遅延回路、４４は第３の遅延回路１９の出
力する予測誤差共分散行列と、距離・方位角観測行列算
出器Ｂ４２で算出した距離・方位角の観測行列と、予め
設定された２次元レーダ装置Ｂ３８の観測誤差共分散行
列を入力し、追尾対象の距離及び方位角の確率分布を算
出する距離・方位角確率分布算出器である。
評価器Ｂ４３により算出された平滑誤差共分散行列を入
力し、追尾対象の３次元位置及び速度の予測誤差を評価
する予測誤差共分散行列を算出する予測誤差評価器Ｂで
ある。１９は予測誤差評価器Ｂ１８により算出された予
測誤差共分散行列を１サンプリング時間だけ遅延して出
力する第３の遅延回路、４４は第３の遅延回路１９の出
力する予測誤差共分散行列と、距離・方位角観測行列算
出器Ｂ４２で算出した距離・方位角の観測行列と、予め
設定された２次元レーダ装置Ｂ３８の観測誤差共分散行
列を入力し、追尾対象の距離及び方位角の確率分布を算
出する距離・方位角確率分布算出器である。
【０１７７】２１は距離・方位角による平滑器７、距・
方位角による平滑器Ｂ４１のいずれかから出力される追
尾対象の３次元位置及び速度から成る３次元平滑ベクト
ルと、予め設定された追尾対象の運動モデルを用いて現
時刻より１サンプリング後の追尾対象の３次元予測位置
及び予測速度から成る３次元予測ベクトルを算出する位
置・速度予測器である。
方位角による平滑器Ｂ４１のいずれかから出力される追
尾対象の３次元位置及び速度から成る３次元平滑ベクト
ルと、予め設定された追尾対象の運動モデルを用いて現
時刻より１サンプリング後の追尾対象の３次元予測位置
及び予測速度から成る３次元予測ベクトルを算出する位
置・速度予測器である。
【０１７８】２２は位置・速度予測器２１により算出さ
れた追尾対象の３次元予測ベクトルを１サンプリング時
間だけ遅延して出力する第１の遅延回路、２３は第１の
遅延回路２２の出力する現時刻より１サンプリング前の
追尾対象の３次元予測ベクトルを距離及び方位角の予測
ベクトルに座標変換し、これと変換前の３次元予測ベク
トルを出力する第２の座標変換器、２４は第１の遅延回
路２２の出力する現時刻より１サンプリング前の追尾対
象の３次元予測ベクトルを距離及び方位角の予測ベクト
ルに座標変換し、これと変換前の３次元予測ベクトルを
出力する第３の座標変換器である。
れた追尾対象の３次元予測ベクトルを１サンプリング時
間だけ遅延して出力する第１の遅延回路、２３は第１の
遅延回路２２の出力する現時刻より１サンプリング前の
追尾対象の３次元予測ベクトルを距離及び方位角の予測
ベクトルに座標変換し、これと変換前の３次元予測ベク
トルを出力する第２の座標変換器、２４は第１の遅延回
路２２の出力する現時刻より１サンプリング前の追尾対
象の３次元予測ベクトルを距離及び方位角の予測ベクト
ルに座標変換し、これと変換前の３次元予測ベクトルを
出力する第３の座標変換器である。
【０１７９】次に動作について説明する。最初に、この
実施の形態４による移動体追尾装置及び方法の動作原理
を説明する。ただし、２次元レーダ装置１観測座標系の
定義、観測座標系と基準座標系の関係、移動体の運動モ
デル、２次元レーダ装置１の観測モデルの定義に関し
て、すなわち、式（１）、式（２）、式（５）〜式（１
９）に表される関係と、２次元レーダ装置１の相関処理
及び平滑処理に関して、すなわち、式（３７）〜式（４
２）に表される関係と、追尾初期化方法に関して、すな
わち、式（４９）〜式（５２）で表される関係とについ
ては実施の形態１と同様である。ただし、基準座標系に
おける２次元レーダ装置Ｂ３８の取り付け位置は、それ
ぞれ（ｘu，ｈu，ｚu）とする。
実施の形態４による移動体追尾装置及び方法の動作原理
を説明する。ただし、２次元レーダ装置１観測座標系の
定義、観測座標系と基準座標系の関係、移動体の運動モ
デル、２次元レーダ装置１の観測モデルの定義に関し
て、すなわち、式（１）、式（２）、式（５）〜式（１
９）に表される関係と、２次元レーダ装置１の相関処理
及び平滑処理に関して、すなわち、式（３７）〜式（４
２）に表される関係と、追尾初期化方法に関して、すな
わち、式（４９）〜式（５２）で表される関係とについ
ては実施の形態１と同様である。ただし、基準座標系に
おける２次元レーダ装置Ｂ３８の取り付け位置は、それ
ぞれ（ｘu，ｈu，ｚu）とする。
【０１８０】次に追尾対象の距離及び方位角がサンプリ
ング時刻ｔkに２次元レーダ装置Ｂ３８より観測される
場合の２次元レーダ装置Ｂ３８の観測モデルを式（９
７）とする。ただし、アンダバーｚ5 kはサンプリング時
刻ｔkにおけるレーダ観測装置Ｂ３８の観測ベクトルで
ある。ｈ5（アンダバーｘk）は２次元レーダ装置Ｂ３８
の観測ベクトルの真値で、式（１）及び式（２）より式
（９８）で表される。アンダバーν5 kはサンプリング時
刻ｔkにおける２次元レーダ装置Ｂ３８の観測雑音ベク
トルであり、平均アンダバーＯの２次元白色正規分布
で、式（９９）及び（１００）で表される。なお、Ｒ5 k
はサンプリング時刻ｔkにおける２次元レーダ装置Ｂ３
８の観測雑音共分散行列である。
ング時刻ｔkに２次元レーダ装置Ｂ３８より観測される
場合の２次元レーダ装置Ｂ３８の観測モデルを式（９
７）とする。ただし、アンダバーｚ5 kはサンプリング時
刻ｔkにおけるレーダ観測装置Ｂ３８の観測ベクトルで
ある。ｈ5（アンダバーｘk）は２次元レーダ装置Ｂ３８
の観測ベクトルの真値で、式（１）及び式（２）より式
（９８）で表される。アンダバーν5 kはサンプリング時
刻ｔkにおける２次元レーダ装置Ｂ３８の観測雑音ベク
トルであり、平均アンダバーＯの２次元白色正規分布
で、式（９９）及び（１００）で表される。なお、Ｒ5 k
はサンプリング時刻ｔkにおける２次元レーダ装置Ｂ３
８の観測雑音共分散行列である。
【０１８１】
【数３３】
【０１８２】２次元レーダ装置Ｂ３８の真値ｈ5（アン
ダバーｘk）の線形近似は式（１０１）より得られる。
式（１０２）は２次元レーダ装置Ｂ３８の観測行列であ
る。ここで、Ｒp、Ｅp及びＡｚpは３次元予測ベクトル
アンダバーハットｘk（−）を式（１）、式（２）及び
式（１９）の関係式を用いてレーダ観測座標系における
距離、俯角、方位角に変換したものである。
ダバーｘk）の線形近似は式（１０１）より得られる。
式（１０２）は２次元レーダ装置Ｂ３８の観測行列であ
る。ここで、Ｒp、Ｅp及びＡｚpは３次元予測ベクトル
アンダバーハットｘk（−）を式（１）、式（２）及び
式（１９）の関係式を用いてレーダ観測座標系における
距離、俯角、方位角に変換したものである。
【０１８３】
【数３４】
【０１８４】サンプリング時刻ｔkにおける追尾目標と
相関をとるべき目標予測存在範囲内の２次元レーダ装置
１または２次元レーダ装置Ｂ３８からの観測ベクトルの
総数をｍs k、観測ベクトルの全体をＺs kとし（式（１０
４）を参照）、サンプリング時刻ｔ1からｔkまでの２次
元レーダ装置１または２次元レーダ装置Ｂ３８からの観
測ベクトル全体をＺk,sとする（式（１０５）を参
照）。また、Ｚk,sに対する観測数の全体を式（１０
６）とし、さらにまた、式（１０７）とする。ここで、
ｓ＝１は２次元レーダ装置１を、ｓ＝５は２次元レーダ
装置Ｂ３８をそれぞれ表す。
相関をとるべき目標予測存在範囲内の２次元レーダ装置
１または２次元レーダ装置Ｂ３８からの観測ベクトルの
総数をｍs k、観測ベクトルの全体をＺs kとし（式（１０
４）を参照）、サンプリング時刻ｔ1からｔkまでの２次
元レーダ装置１または２次元レーダ装置Ｂ３８からの観
測ベクトル全体をＺk,sとする（式（１０５）を参
照）。また、Ｚk,sに対する観測数の全体を式（１０
６）とし、さらにまた、式（１０７）とする。ここで、
ｓ＝１は２次元レーダ装置１を、ｓ＝５は２次元レーダ
装置Ｂ３８をそれぞれ表す。
【０１８５】
【数３５】
【０１８６】次に、サンプリング時刻ｔk-1までの２次
元レーダ装置１または画像センサ装置により観測された
観測ベクトルＺs kが得られているときの、サンプリング
時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダ
バーハットｘk（−）、予測誤差共分散行列をＰk（−）
とすると、それぞれ条件付平均ベクトル、条件付共分散
行列で定義され、式（１０８）及び式（１０９）で表さ
れる。
元レーダ装置１または画像センサ装置により観測された
観測ベクトルＺs kが得られているときの、サンプリング
時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダ
バーハットｘk（−）、予測誤差共分散行列をＰk（−）
とすると、それぞれ条件付平均ベクトル、条件付共分散
行列で定義され、式（１０８）及び式（１０９）で表さ
れる。
【０１８７】
【数３６】
【０１８８】ここで、アンダバーハットｘk（＋）及び
Ｐk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベ
クトル及び平滑誤差共分散行列であり、前サンプリング
時刻ｔk-1に２次元レーダ装置１からの観測ベクトルを
処理した場合には式（１１０）及び式（１１１）、２次
元レーダ装置Ｂ３８からの観測ベクトルを処理した場合
には式（１１２）及び式（１１３）である。これらの算
出方法については後で述べる。
Ｐk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベ
クトル及び平滑誤差共分散行列であり、前サンプリング
時刻ｔk-1に２次元レーダ装置１からの観測ベクトルを
処理した場合には式（１１０）及び式（１１１）、２次
元レーダ装置Ｂ３８からの観測ベクトルを処理した場合
には式（１１２）及び式（１１３）である。これらの算
出方法については後で述べる。
【０１８９】
【数３７】
【０１９０】次に、移動体の距離及び方位角（観測ベク
トル）がサンプリング時刻ｔkにおいて、２次元レーダ
装置Ｂ３８より観測される場合の相関処理について述べ
る。追尾対象からの観測ベクトルは、式（１１４）で与
えられる予測位置ベクトルアンダバーｚ1 k（−）を平均
とし、式（１１５）で与えられるＳ5 kを共分散行列とす
る２次元正規分布に従うとする。そこで、式（１１６）
を満たす観測ベクトルアンダバーｚ5 k,iのうち、値の最
も小さい観測ベクトルアンダバーｚ5 kを追尾対象と相関
がある観測ベクトルと判定する。ここで、ｄ5は追尾対
象との相関範囲を決めるパラメータであり、自由度２の
χ自乗分布により算出する。
トル）がサンプリング時刻ｔkにおいて、２次元レーダ
装置Ｂ３８より観測される場合の相関処理について述べ
る。追尾対象からの観測ベクトルは、式（１１４）で与
えられる予測位置ベクトルアンダバーｚ1 k（−）を平均
とし、式（１１５）で与えられるＳ5 kを共分散行列とす
る２次元正規分布に従うとする。そこで、式（１１６）
を満たす観測ベクトルアンダバーｚ5 k,iのうち、値の最
も小さい観測ベクトルアンダバーｚ5 kを追尾対象と相関
がある観測ベクトルと判定する。ここで、ｄ5は追尾対
象との相関範囲を決めるパラメータであり、自由度２の
χ自乗分布により算出する。
【０１９１】
【数３８】
【０１９２】次に、追尾対象の位置ベクトルがサンプリ
ング時刻ｔkにおいて、２次元レーダ装置Ｂ３８により
観測される場合の平滑処理について述べる。ゲイン行
列、平滑ベクトル及び平滑誤差共分散行列は拡張カルマ
ンフィルタの理論により、式（１１７）〜（１１９）で
与えられる。
ング時刻ｔkにおいて、２次元レーダ装置Ｂ３８により
観測される場合の平滑処理について述べる。ゲイン行
列、平滑ベクトル及び平滑誤差共分散行列は拡張カルマ
ンフィルタの理論により、式（１１７）〜（１１９）で
与えられる。
【０１９３】
【数３９】
【０１９４】次にこの発明の移動体追尾装置の動作及び
方法を具体的に説明する。２次元レーダ装置１で、移動
体の距離及び方位角を観測し、距離・方位角確率分布算
出器１２で、現時刻での存在が予測される追尾対象の距
離及び方位角の確率分布Ｓ1 kを算出するまでは実施の形
態１と同一動作を行う。
方法を具体的に説明する。２次元レーダ装置１で、移動
体の距離及び方位角を観測し、距離・方位角確率分布算
出器１２で、現時刻での存在が予測される追尾対象の距
離及び方位角の確率分布Ｓ1 kを算出するまでは実施の形
態１と同一動作を行う。
【０１９５】２次元レーダ装置Ｂ３８では、レーダ観測
座標系で移動体の距離及び方位角を観測し、その結果
（観測ベクトル）を距離・方位角相関器Ｂ３９に出力す
る。距離・方位角相関器Ｂ３９では２次元レーダ装置Ｂ
３８から観測ベクトルを受け取ると、その観測ベクトル
から追尾対象と相関のある観測ベクトルを採用するた
め、距離・方位角確率分布算出器Ｂ４４により算出され
た距離及び方位角の確率分布Ｓ5 k（式（１１５）を参
照）を用いて、追尾対象と相関の可能性のある観測ベク
トルを式（１１６）を用いて選択する。そして、選択さ
れた観測ベクトルのうち式（１１６）の値の最も小さい
観測ベクトルアンダバーｚ5 kを追尾対象の観測ベクトル
と判定し、距離・方位角による平滑器Ｂ４１に出力する
（ステップＳＴ９）。
座標系で移動体の距離及び方位角を観測し、その結果
（観測ベクトル）を距離・方位角相関器Ｂ３９に出力す
る。距離・方位角相関器Ｂ３９では２次元レーダ装置Ｂ
３８から観測ベクトルを受け取ると、その観測ベクトル
から追尾対象と相関のある観測ベクトルを採用するた
め、距離・方位角確率分布算出器Ｂ４４により算出され
た距離及び方位角の確率分布Ｓ5 k（式（１１５）を参
照）を用いて、追尾対象と相関の可能性のある観測ベク
トルを式（１１６）を用いて選択する。そして、選択さ
れた観測ベクトルのうち式（１１６）の値の最も小さい
観測ベクトルアンダバーｚ5 kを追尾対象の観測ベクトル
と判定し、距離・方位角による平滑器Ｂ４１に出力する
（ステップＳＴ９）。
【０１９６】距離・方位角ゲイン行列算出器Ｂ４０で
は、第３の遅延回路１９から出力された予測誤差共分散
行列（現時刻より１サンプリング前に算出された予測誤
差共分散行列）と、距離・方位角観測行列算出器Ｂ４２
から出力された式（１０２）の２次元レーダ装置Ｂ３８
の観測行列と予め設定された式（１００）の２次元レー
ダ装置Ｂ３８の観測雑音共分散行列とから距離及び方位
角のゲイン行列Ｋ5 kを式（１１７）に従い算出する（ス
テップＳＴ１０）。
は、第３の遅延回路１９から出力された予測誤差共分散
行列（現時刻より１サンプリング前に算出された予測誤
差共分散行列）と、距離・方位角観測行列算出器Ｂ４２
から出力された式（１０２）の２次元レーダ装置Ｂ３８
の観測行列と予め設定された式（１００）の２次元レー
ダ装置Ｂ３８の観測雑音共分散行列とから距離及び方位
角のゲイン行列Ｋ5 kを式（１１７）に従い算出する（ス
テップＳＴ１０）。
【０１９７】距離・方位角による平滑器Ｂ４１では、距
離・方位角相関器Ｂ３９から出力された追尾対象の観測
ベクトルアンダバーｚ5 kと、第１の座標変換器２４から
出力された距離及び方位角の予測値（現時刻より１サン
プリング前に算出された予測値）と、距離・方位角ゲイ
ン行列算出器Ｂ４０により算出されたゲイン行列Ｋ5 kと
を用いて、追尾対象の位置及び速度の平滑ベクトルを式
（１１８）に従い算出する（ステップＳＴ１１）。距離
・方位角観測行列算出器Ｂ４２では、第１の座標変換器
２４から出力された距離と方位角の予測値（現時刻より
１サンプリング前に算出された予測値）を入力し、２次
元レーダ装置Ｂ３８の観測行列を式（１０２）に従い算
出する（ステップＳＴ１２）。
離・方位角相関器Ｂ３９から出力された追尾対象の観測
ベクトルアンダバーｚ5 kと、第１の座標変換器２４から
出力された距離及び方位角の予測値（現時刻より１サン
プリング前に算出された予測値）と、距離・方位角ゲイ
ン行列算出器Ｂ４０により算出されたゲイン行列Ｋ5 kと
を用いて、追尾対象の位置及び速度の平滑ベクトルを式
（１１８）に従い算出する（ステップＳＴ１１）。距離
・方位角観測行列算出器Ｂ４２では、第１の座標変換器
２４から出力された距離と方位角の予測値（現時刻より
１サンプリング前に算出された予測値）を入力し、２次
元レーダ装置Ｂ３８の観測行列を式（１０２）に従い算
出する（ステップＳＴ１２）。
【０１９８】距離・方位角データによる平滑誤差評価器
Ｂ４３では、第１の座標変換器２４から出力された距離
及び方位角の予測値（現時刻より１サンプリング前に算
出された予測値）と、距離・方位角観測行列算出器Ｂ４
２から出力された２次元レーダ装置Ｂ３８の観測行列
と、距離・方位角ゲイン行列算出器Ｂ４０から出力され
たゲイン行列Ｋ5 kと、第３の遅延回路１９から出力され
た予測誤差共分散行列（現時刻より１サンプリング前に
算出された予測誤差共分散行列）とから平滑誤差共分散
行列を式（１１９）に従い算出する（ステップＳＴ１
３）。
Ｂ４３では、第１の座標変換器２４から出力された距離
及び方位角の予測値（現時刻より１サンプリング前に算
出された予測値）と、距離・方位角観測行列算出器Ｂ４
２から出力された２次元レーダ装置Ｂ３８の観測行列
と、距離・方位角ゲイン行列算出器Ｂ４０から出力され
たゲイン行列Ｋ5 kと、第３の遅延回路１９から出力され
た予測誤差共分散行列（現時刻より１サンプリング前に
算出された予測誤差共分散行列）とから平滑誤差共分散
行列を式（１１９）に従い算出する（ステップＳＴ１
３）。
【０１９９】予測誤差評価器Ｂ１８では、距離・方位角
データによる平滑誤差評価器Ｂ４３から出力された平滑
誤差共分散行列を入力し、予測誤差共分散行列を式（１
０９）に従い算出する（ステップＳＴ１４）。
データによる平滑誤差評価器Ｂ４３から出力された平滑
誤差共分散行列を入力し、予測誤差共分散行列を式（１
０９）に従い算出する（ステップＳＴ１４）。
【０２００】距離・方位角確率分布算出器Ｂ４４は、第
３の遅延回路１９から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）と、距離・方位角観測行列算出器Ｂ４２から
出力された２次元レーダ装置Ｂ３８の観測行列と、予め
設定された式（１００）の２次元レーダ装置Ｂ３８の観
測雑音共分散行列とを入力し、現時刻での存在が予測さ
れる追尾対象の距離及び方位角の確率分布Ｓ5 kを式（１
１５）に従い算出する（ステップＳＴ１５）。
３の遅延回路１９から出力された予測誤差共分散行列
（現時刻より１サンプリング前に算出された予測誤差共
分散行列）と、距離・方位角観測行列算出器Ｂ４２から
出力された２次元レーダ装置Ｂ３８の観測行列と、予め
設定された式（１００）の２次元レーダ装置Ｂ３８の観
測雑音共分散行列とを入力し、現時刻での存在が予測さ
れる追尾対象の距離及び方位角の確率分布Ｓ5 kを式（１
１５）に従い算出する（ステップＳＴ１５）。
【０２０１】上記のようにして、距離・方位角による平
滑器７または距離・方位角による平滑器Ｂ４１または初
期値算出器５が追尾対象の位置及び速度の平滑値を算出
すると、位置・速度予測器２１はこれらのどちらかの平
滑値と予め設定された追尾対象の運動モデルを用いて、
現時刻より１サンプリング後の追尾対象の予測ベクトル
（位置及び速度）を式（１０８）に従い算出し、第１の
遅延回路２２に出力する（ステップＳＴ１６）。
滑器７または距離・方位角による平滑器Ｂ４１または初
期値算出器５が追尾対象の位置及び速度の平滑値を算出
すると、位置・速度予測器２１はこれらのどちらかの平
滑値と予め設定された追尾対象の運動モデルを用いて、
現時刻より１サンプリング後の追尾対象の予測ベクトル
（位置及び速度）を式（１０８）に従い算出し、第１の
遅延回路２２に出力する（ステップＳＴ１６）。
【０２０２】そして、次に追尾対象を観測するセンサが
２次元レーダ装置１の場合は、第３の座標変換器２３が
第１の遅延回路２２から予測ベクトルを入力し、式
（１）、式（２）、式（１９）を使用して２次元レーダ
装置１を基準とする極座標に変換する。一方、次に追尾
対象を観測するセンサが２次元レーダ装置Ｂ３８の場合
は、第１の座標変換器２４が第１の遅延回路２２から予
測ベクトルを入力し、式（１）、式（２）、式（１９）
を使用して２次元レーダ装置Ｂ３８を基準とする極座標
に変換する。以上、追尾終了になるまでこの一連の処理
をくり返す。
２次元レーダ装置１の場合は、第３の座標変換器２３が
第１の遅延回路２２から予測ベクトルを入力し、式
（１）、式（２）、式（１９）を使用して２次元レーダ
装置１を基準とする極座標に変換する。一方、次に追尾
対象を観測するセンサが２次元レーダ装置Ｂ３８の場合
は、第１の座標変換器２４が第１の遅延回路２２から予
測ベクトルを入力し、式（１）、式（２）、式（１９）
を使用して２次元レーダ装置Ｂ３８を基準とする極座標
に変換する。以上、追尾終了になるまでこの一連の処理
をくり返す。
【０２０３】以上で明らかなように、この実施の形態４
によれば、距離・方位角観測行列算出器８において、２
次元レーダ装置１の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、距離・方位角
による平滑器７で、この観測行列と、距離及び方位角の
観測ベクトルを用いて３次元位置及び速度の平滑ベクト
ルを算出し、また、距離・方位角観測行列算出器Ｂ４２
において、２次元レーダ装置Ｂ３８の観測モデルを３次
元予測ベクトルのまわりで線形近似した観測行列を算出
し、距離・方位角による平滑器Ｂ４１で、この観測行列
と、距離及び方位角の観測ベクトルを用いて３次元位置
及び速度の平滑ベクトルを算出する構成としている。
によれば、距離・方位角観測行列算出器８において、２
次元レーダ装置１の観測モデルを３次元予測ベクトルの
まわりで線形近似した観測行列を算出し、距離・方位角
による平滑器７で、この観測行列と、距離及び方位角の
観測ベクトルを用いて３次元位置及び速度の平滑ベクト
ルを算出し、また、距離・方位角観測行列算出器Ｂ４２
において、２次元レーダ装置Ｂ３８の観測モデルを３次
元予測ベクトルのまわりで線形近似した観測行列を算出
し、距離・方位角による平滑器Ｂ４１で、この観測行列
と、距離及び方位角の観測ベクトルを用いて３次元位置
及び速度の平滑ベクトルを算出する構成としている。
【０２０４】このため、追尾対象の追尾開始領域の道路
高の情報を利用することによって初期値算出器５により
算出した３次元位置及び速度の初期値をもとに、２次元
レーダ装置１の観測する距離及び方位角と、２次元レー
ダ装置Ｂ３８の観測する距離及び方位角とを用いて逐次
的に追尾対象の３次元位置及び速度を推定することがで
きる。従って、２つの２次元レーダ装置を併用すること
により、移動体の３次元運動を追尾できる効果を奏す
る。
高の情報を利用することによって初期値算出器５により
算出した３次元位置及び速度の初期値をもとに、２次元
レーダ装置１の観測する距離及び方位角と、２次元レー
ダ装置Ｂ３８の観測する距離及び方位角とを用いて逐次
的に追尾対象の３次元位置及び速度を推定することがで
きる。従って、２つの２次元レーダ装置を併用すること
により、移動体の３次元運動を追尾できる効果を奏す
る。
【０２０５】なお、この実施の形態４は、２次元レーダ
装置の代わりにレーザ装置や音波センサ装置を用いて追
尾対象の距離及び方位角を観測した場合にも適用可能で
ある。また、レーダ装置を更に増やしても同様の処理に
より追尾対象の３次元位置及び速度を推定することがで
きる。
装置の代わりにレーザ装置や音波センサ装置を用いて追
尾対象の距離及び方位角を観測した場合にも適用可能で
ある。また、レーダ装置を更に増やしても同様の処理に
より追尾対象の３次元位置及び速度を推定することがで
きる。
【０２０６】
【発明の効果】以上のように、この発明によれば、追尾
対象としての移動体の距離及び方位角を観測する第１の
観測手段またはステップと、俯角を観測する第２の観測
手段またはステップとを併用することにより、追尾対象
の３次元位置及び速度を推定することができる。
対象としての移動体の距離及び方位角を観測する第１の
観測手段またはステップと、俯角を観測する第２の観測
手段またはステップとを併用することにより、追尾対象
の３次元位置及び速度を推定することができる。
【０２０７】また、移動体の距離及び方位角を観測する
第１の観測手段またはステップと、方位角を観測する第
２の観測手段またはステップとを併用することにより、
追尾対象の３次元位置及び速度を推定することができ
る。
第１の観測手段またはステップと、方位角を観測する第
２の観測手段またはステップとを併用することにより、
追尾対象の３次元位置及び速度を推定することができ
る。
【０２０８】また、移動体の距離及び方位角を観測する
第１の観測手段またはステップと、俯角及び方位角を観
測する第２の観測手段またはステップとを併用すること
により、追尾対象の３次元位置及び速度を推定すること
ができる。
第１の観測手段またはステップと、俯角及び方位角を観
測する第２の観測手段またはステップとを併用すること
により、追尾対象の３次元位置及び速度を推定すること
ができる。
【０２０９】さらに、移動体の距離及び方位角を観測す
る第１の観測手段またはステップと、距離及び方位角を
観測する第２の観測手段またはステップとを併用するこ
とにより、追尾対象の３次元位置及び速度を推定するこ
とができる。
る第１の観測手段またはステップと、距離及び方位角を
観測する第２の観測手段またはステップとを併用するこ
とにより、追尾対象の３次元位置及び速度を推定するこ
とができる。
【図１】 この発明の実施の形態１による移動体追尾装
置及び方法を説明するための構成図である。
置及び方法を説明するための構成図である。
【図２】 図１の２次元レーダ装置１及び画像センサ装
置２による追尾対象の観測状態を示す概念図である。
置２による追尾対象の観測状態を示す概念図である。
【図３】 図１の２次元レーダ装置１を原点とするレー
ダ座標系ｏ(r)−ｘ( r)ｙ(r)ｚ(r)を示す説明図である。
ダ座標系ｏ(r)−ｘ( r)ｙ(r)ｚ(r)を示す説明図である。
【図４】 レーダ座標系を基準とする極座標（Ｒ，Ａｚ
r）をレーダ観測座標系とする説明図である。
r）をレーダ観測座標系とする説明図である。
【図５】 図１の画像センサ装置２の焦点を原点ｏ(c)
とし、画像センサ装置２前方をｚ(c)軸負の方向、画像
センサ装置２左手をｘ(c)軸負の方向、画像センサ装置
上方をｙ(c)軸正の方向とする座標系ｏ(c)−ｘ(c)ｙ(c)
ｚ(c)を画像センサ座標系とする説明図（上面図）であ
る。
とし、画像センサ装置２前方をｚ(c)軸負の方向、画像
センサ装置２左手をｘ(c)軸負の方向、画像センサ装置
上方をｙ(c)軸正の方向とする座標系ｏ(c)−ｘ(c)ｙ(c)
ｚ(c)を画像センサ座標系とする説明図（上面図）であ
る。
【図６】 図１の画像センサ装置２の焦点を原点ｏ(c)
とし、画像センサ装置２前方をｚ(c)軸負の方向、画像
センサ装置２左手をｘ(c)軸負の方向、画像センサ装置
上方をｙ(c)軸正の方向とする座標系ｏ(c)−ｘ(c)ｙ(c)
ｚ(c)を画像センサ座標系とする説明図（側面図）であ
る。
とし、画像センサ装置２前方をｚ(c)軸負の方向、画像
センサ装置２左手をｘ(c)軸負の方向、画像センサ装置
上方をｙ(c)軸正の方向とする座標系ｏ(c)−ｘ(c)ｙ(c)
ｚ(c)を画像センサ座標系とする説明図（側面図）であ
る。
【図７】 画像センサ座標系を基準とする極座標（Ｅ，
Ａｚc）を画像センサ観測座標系とする説明図である。
Ａｚc）を画像センサ観測座標系とする説明図である。
【図８】 この発明の実施の形態２による移動体追尾装
置及び方法を説明するための構成図である。
置及び方法を説明するための構成図である。
【図９】 この発明の実施の形態３による移動体追尾装
置及び方法を説明するための構成図である。
置及び方法を説明するための構成図である。
【図１０】 この発明の実施の形態４による移動体追尾
装置及び方法を説明するための構成図である。
装置及び方法を説明するための構成図である。
【図１１】 従来例による移動体追尾装置を説明するた
めの構成図である。
めの構成図である。
１ ２次元レーダ装置、２ 画像センサ装置、７ 距離
・方位角による平滑器、１５ 俯角による平滑器、２３
第３の座標変換器、２４ 第１の座標変換器、２７
方位角による平滑器、３３ 俯角・方位角による平滑
器、３８ ２次元レーダ装置Ｂ、４１ 距離・方位角に
よる平滑器。
・方位角による平滑器、１５ 俯角による平滑器、２３
第３の座標変換器、２４ 第１の座標変換器、２７
方位角による平滑器、３３ 俯角・方位角による平滑
器、３８ ２次元レーダ装置Ｂ、４１ 距離・方位角に
よる平滑器。
───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.7 識別記号 ＦＩ テーマコート゛(参考） // Ｇ０１Ｓ 7/295 Ｇ０１Ｓ 7/295 Ｃ (72)発明者 小菅 義夫 東京都千代田区丸の内二丁目２番３号 三 菱電機株式会社内 Ｆターム(参考） 5H180 AA01 AA21 CC03 CC04 CC11 CC14 DD03 5J070 AC01 AC06 AE01 AE09 AF01 AH04 AH19 AJ02 AK01 AK13 BB04 BB06 BD06
Claims (8)
- 【請求項１】 追尾対象としての移動体の距離及び方位
角を観測する第１の観測手段と、 追尾対象の俯角を観測する第２の観測手段と、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第１の観測手段によって観測された距離及び方位角とを
用いて追尾対象の３次元座標の平滑値を演算する第１の
平滑手段と、 追尾対象の３次元座標の俯角予測値と、前記第２の観測
手段によって観測された俯角を用いて追尾対象の３次元
座標の平滑値を演算する第２の平滑手段とを備えた移動
体追尾装置。 - 【請求項２】 追尾対象としての移動体の距離及び方位
角を観測する第１の観測手段と、 追尾対象の方位角を観測する第２の観測手段と、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第１の観測手段によって観測された距離及び方位角とを
用いて追尾対象の３次元座標の平滑値を演算する第１の
平滑手段と、 追尾対象の３次元座標の方位角予測値と、前記第２の観
測手段によって観測された方位角を用いて追尾対象の３
次元座標の平滑値を演算する第２の平滑手段とを備えた
移動体追尾装置。 - 【請求項３】 追尾対象としての移動体の距離及び方位
角を観測する第１の観測手段と、 追尾対象の俯角及び方位角を観測する第２の観測手段
と、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第１の観測手段によって観測された距離及び方位角を用
いて追尾対象の３次元座標の平滑値を演算する第１の平
滑手段と、 追尾対象の３次元座標の方位角予測値と、前記第２の観
測手段によって観測された俯角及び方位角を用いて追尾
対象の３次元座標の平滑値を演算する第２の平滑手段と
を備えた移動体追尾装置。 - 【請求項４】 追尾対象としての移動体の距離及び方位
角を観測する第１の観測手段と、 追尾対象の距離及び方位角を観測する第２の観測手段
と、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第１の観測手段によって観測された距離及び方位角を用
いて追尾対象の３次元座標の平滑値を演算する第１の平
滑手段と、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第２の観測手段によって観測された距離及び方位角を用
いて追尾対象の３次元座標の平滑値を演算する第２の平
滑手段とを備えた移動体追尾装置。 - 【請求項５】 追尾対象としての移動体の距離及び方位
角を観測する第１の観測ステップと、 追尾対象の俯角を観測する第２の観測ステップと、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第１の観測ステップによって観測された距離及び方位角
を用いて追尾対象の３次元座標の平滑値を演算する第１
の平滑ステップと、 追尾対象の３次元座標の俯角予測値と、第２の観測手段
によって観測された俯角を用いて追尾対象の３次元座標
の平滑値を演算する第２の平滑ステップとを備えた移動
体追尾方法。 - 【請求項６】 追尾対象としての移動体の距離及び方位
角を観測する第１の観測ステップと、 追尾対象の方位角を観測する第２の観測ステップと、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第１の観測ステップによって観測された距離及び方位角
を用いて追尾対象の３次元座標の平滑値を演算する第１
の平滑ステップと、 追尾対象の３次元座標の方位角予測値と、前記第２の観
測ステップによって観測された方位角を用いて追尾対象
の３次元座標の平滑値を演算する第２の平滑ステップと
を備えた移動体追尾方法。 - 【請求項７】 追尾対象としての移動体の距離及び方位
角を観測する第１の観測ステップと、 追尾対象の俯角及び方位角を観測する第２の観測ステッ
プと、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第１の観測ステップによって観測された距離及び方位角
を用いて追尾対象の３次元座標の平滑値を演算する第１
の平滑ステップと、 追尾対象の３次元座標の俯角予測値と、前記第２の観測
ステップによって観測された俯角及び方位角を用いて追
尾対象の３次元座標の平滑値を演算する第２の平滑ステ
ップとを備えた移動体追尾方法。 - 【請求項８】 追尾対象としての移動体の距離及び方位
角を観測する第１の観測ステップと、 追尾対象の距離及び方位角を観測する第２の観測ステッ
プと、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第１の観測ステップによって観測された距離及び方位角
を用いて追尾対象の３次元座標の平滑値を演算する第１
の平滑ステップと、 追尾対象の３次元座標の距離及び方位角予測値と、前記
第２の観測ステップによって観測された距離及び方位角
を用いて追尾対象の３次元座標の平滑値を演算する第２
の平滑ステップとを備えた移動体追尾方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2000230824A JP2002048860A (ja) | 2000-07-31 | 2000-07-31 | 移動体追尾装置及び方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2000230824A JP2002048860A (ja) | 2000-07-31 | 2000-07-31 | 移動体追尾装置及び方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2002048860A true JP2002048860A (ja) | 2002-02-15 |
Family
ID=18723730
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2000230824A Abandoned JP2002048860A (ja) | 2000-07-31 | 2000-07-31 | 移動体追尾装置及び方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2002048860A (ja) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007102639A (ja) * | 2005-10-06 | 2007-04-19 | Fuji Heavy Ind Ltd | 衝突判定装置 |
| JP2011039995A (ja) * | 2009-08-18 | 2011-02-24 | Nec Soft Ltd | 対象物検出装置、対象物検出方法、プログラムおよび記録媒体 |
| CN103576130A (zh) * | 2013-11-05 | 2014-02-12 | 西安电子科技大学 | 一种进动锥体的三维成像方法 |
| JP2017219377A (ja) * | 2016-06-06 | 2017-12-14 | 三菱電機株式会社 | 監視装置、監視方法及び空港監視システム |
| CN110045365A (zh) * | 2019-03-26 | 2019-07-23 | 西北工业大学 | 一种基于雷达信息的图像目标定位方法 |
-
2000
- 2000-07-31 JP JP2000230824A patent/JP2002048860A/ja not_active Abandoned
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2007102639A (ja) * | 2005-10-06 | 2007-04-19 | Fuji Heavy Ind Ltd | 衝突判定装置 |
| JP2011039995A (ja) * | 2009-08-18 | 2011-02-24 | Nec Soft Ltd | 対象物検出装置、対象物検出方法、プログラムおよび記録媒体 |
| CN103576130A (zh) * | 2013-11-05 | 2014-02-12 | 西安电子科技大学 | 一种进动锥体的三维成像方法 |
| CN103576130B (zh) * | 2013-11-05 | 2015-09-09 | 西安电子科技大学 | 一种进动锥体的三维成像方法 |
| JP2017219377A (ja) * | 2016-06-06 | 2017-12-14 | 三菱電機株式会社 | 監視装置、監視方法及び空港監視システム |
| CN110045365A (zh) * | 2019-03-26 | 2019-07-23 | 西北工业大学 | 一种基于雷达信息的图像目标定位方法 |
| CN110045365B (zh) * | 2019-03-26 | 2023-03-14 | 西北工业大学 | 一种基于雷达信息的图像目标定位方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Saho | Kalman filter for moving object tracking: Performance analysis and filter design | |
| JP4116898B2 (ja) | 目標追尾装置 | |
| WO2017207727A1 (en) | Enhanced object detection and motion state estimation for a vehicle environment detection system | |
| CN109716157B (zh) | 轴偏移估计装置 | |
| US7266042B1 (en) | Multi-stage maximum likelihood target estimator | |
| JP2008096137A (ja) | レーダ装置及び測角装置 | |
| CN111386476B (zh) | 确定车辆雷达系统中的对象运动和加速度矢量 | |
| US20210239825A1 (en) | Object tracking apparatus | |
| JP2012053062A (ja) | 合成開口ソナーのための方法及びシステム | |
| JP2009186353A (ja) | 物体検出装置および物体検出方法 | |
| JP2000241542A (ja) | 移動体追尾装置 | |
| JP2002048860A (ja) | 移動体追尾装置及び方法 | |
| JP5380267B2 (ja) | 誘導装置及び目標検出方法 | |
| JP3996080B2 (ja) | 目標追尾装置 | |
| JP7164327B2 (ja) | 物標検出装置 | |
| KR101837845B1 (ko) | 수중 표적 정보 획득 시스템 및 방법 | |
| JP3750860B2 (ja) | 画像レーダ装置 | |
| JP2009036539A (ja) | レーダ信号処理装置およびレーダ信号処理方法 | |
| JP3440010B2 (ja) | 目標追尾装置 | |
| JP2006329829A (ja) | レーダ装置 | |
| CN113008228A (zh) | 运行机动车传感器的方法及设备，传感器系统和机动车 | |
| JP4908599B2 (ja) | レーダ装置 | |
| JP6757227B2 (ja) | 運動パラメータ推定装置、運動パラメータ推定方法及びプログラム | |
| JP4097126B2 (ja) | 目標追尾装置及び方法 | |
| JP2595225B2 (ja) | 船舶用運動予測方式 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20041210 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20050308 |
|
| A762 | Written abandonment of application |
Free format text: JAPANESE INTERMEDIATE CODE: A762 Effective date: 20050422 |