JP2002189519A

JP2002189519A - 追跡システム、及び、追跡方法

Info

Publication number: JP2002189519A
Application number: JP2000388290A
Authority: JP
Inventors: Yuji Ota; 裕二太田
Original assignee: Mitsubishi Heavy Industries Ltd
Current assignee: Mitsubishi Heavy Industries Ltd
Priority date: 2000-12-21
Filing date: 2000-12-21
Publication date: 2002-07-05

Abstract

(57)【要約】【課題】観測周期よりも短い制御周期により追跡の精度
を高精度化すること。【解決手段】自律運動標的３，４２の第１時系列点上の
現実位置を離隔物体５，１３，４１で検出し、第１時系
列点上の現在位置から３回時間微分列を計算する。３回
時間微分列から第１時系列点に比べてより精細である第
２時系列点上で自律運動標的３，４２の位置、速度、加
速度を物理的に推定し、その物理的推定に基づいて、離
隔物体５，１３，４１の位置を制御する。物理的推定に
続いて、数学的推定が付加される。数学的推定は、物理
的に推定することにより得られた位置、速度、加速度を
カルマンフィルタ３１により更に推定することである。
過去の運動の動特性に基づいて、観測周期より短い制御
周期で高精細に追跡することが可能である。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、追跡システム、及
び、追跡方法に関し、特に、追跡される側の自律的運動
物体と追跡する側の物体との間の相対的直線距離、相対
的回転距離のような相対的位置関係を制御する追跡シス
テム、及び、追跡方法に関する。

【０００２】

【従来の技術】車間距離一定制御のような線長制御の技
術が、高速道路又は一般道路上の２車体間のような２自
律走行体間の相対距離制御のために必要である。このよ
うな技術の開発のためには、目標物体の認識技術とビジ
ュアル系のサンプリングをサーボ系のサンプリングに合
わせることが重要である。現在知られているそのような
技術は、画像取得、目標物抽出のようなビジュアル系の
処理時間が３０ｍｓｅｃで最高速である。この速度に比
べて、雲台、その他の周辺機器の制御演算は３０ｍｓｅ
ｃの３０分の１の１ｍｓｅｃ以下の周期であることが求
められる。目標物体の位置情報が３０ｍｓｅｃ毎にしか
更新されない技術状況では、その３０ｍｓｅｃ毎に得ら
れる追尾用データをフィードバックに使用すれば、その
３０ｍｓｅｃの間は位置指令が更新されず、従って、そ
の追跡性能は成立しない。

【０００３】現在は、現在位置の目標物体位置情報と１
サンプリング前の（３０ｍｓｅｃ前の）目標物体情報に
基づいて直線補間の外挿によって制御演算周期毎に目標
物体の位置を計算して追跡する制御が行われている。こ
のような直線補間制御は、最も簡単な制御技術である
が、画像から目標物体を抽出する抽出誤差に対して過度
に反応する特性を有し、追跡精度の誤差が大きくなって
高精度追跡のためには不満足である。

【０００４】観測周期よりも短い制御周期により制御さ
れる追跡の精度がより高精度化されることが求められ
る。特に、物理的推定がより適正化されて、観測周期よ
りも短い制御周期により制御される追跡の精度がより高
精度化され、且つ、滑らかな制御が可能であることが求
められる。

【０００５】

【発明が解決しようとする課題】本発明の課題は、観測
周期よりも短い制御周期により制御される追跡の精度が
より高精度化される追跡システム、及び、追跡方法を提
供することにある。本発明の他の課題は、物理的推定が
より適正化されて、観測周期よりも短い制御周期により
制御される追跡の精度がより高精度化される追跡システ
ム、及び、追跡方法を提供することにある。

【０００６】

【課題を解決するための手段】その課題を解決するため
の手段が、下記のように表現される。その表現中に現れ
る技術的事項には、括弧（）つきで、番号、記号等が添
記されている。その番号、記号等は、本発明の実施の複
数・形態又は複数の実施例のうちの少なくとも１つの実
施の形態又は複数の実施例を構成する技術的事項、特
に、その実施の形態又は実施例に対応する図面に表現さ
れている技術的事項に付せられている参照番号、参照記
号等に一致している。このような参照番号、参照記号
は、請求項記載の技術的事項と実施の形態又は実施例の
技術的事項との対応・橋渡しを明確にしている。このよ
うな対応・橋渡しは、請求項記載の技術的事項が実施の
形態又は実施例の技術的事項に限定されて解釈されるこ
とを意味しない。

【０００７】本発明による追跡システムは、追跡対象物
体（３）の現実位置列を第１時刻列点で測定する測定器
（１３）と、第１時系列点よりより高精細である第２時
系列点で追跡対象物体の物理的な動特性である第１精細
位置列（数列：ｘ_ｋ）を計算する計算機（２８）と、第
１精細位置列（ｘ_ｋ）から現実位置列の測定誤差を除去
する数学的処理を実行して第２精細位置列（ｘ^∧ _ｋ）を
計算する計算機（３１）と、第２精細位置列（ｘ^∧ _ｋ）
に基づいて、第１時刻列点の間で追跡対象物体と測定器
（１３）の間の相対的距離を高精細に制御する制御器
（３４）とから構成されている。

【０００８】時々刻々に加速度を変える自律運動物体
は、十分に高精細な時刻列点上の観測・検出データに基
づいて、その高精細度でフィードバック制御が行われる
ならば、十分な高精度でその自律運動物体を追跡するこ
とができるが、カメラの映像は情報量が多く時刻列点の
間隔を十分に小さくすることができない場合に、より高
精細な時刻列点上の制御を行うために、より高精細な時
刻列点上で推定的にデータを計算する必要がある。本発
明では、そのような推定的データが、加速度の変動（３
回時間微分に相当する加速度変動、２時刻点間の加速度
差分）に基づいて物理的に推定（計算）される。このよ
うな計算に基づく物理的推定値は、更に、カルマンフィ
ルタにより、自律運動物体の過去のデータに見られる動
特性が考慮されて測定誤差、測定時に入り込むノイズの
ような現実値と異なる数値上方を実質的に除去すること
ができるように、更に適正に数学的に再推定される。こ
のような再推定（物理的推定に基づく数学的推定）によ
り、自律的運動物体をより高精度に、且つ、滑らかに
（過度に反応することなく）追跡することができる。

【０００９】式（２）で示されるその動特性は、現実位
置列に基づく速度列と、速度列に基づく加速度列と、加
速度列に基づく加速度変動列とから形成されている。第
１時系列点の隣り合う２点間の時間がＴで表され、その
２点間がｎ分割され、ｋは０〜ｎの精細順序列数であ
り、ΔＴはＴ／ｎで表されて、第１精細位置列は、位置
列（ｘ_ｋ）、速度列（ｘ^・ _ｋ）、加速度列（ｘ^・・ _ｋ）
から形成され、加速度変動列（ｕ_ｋ）に基づいて、次
式：

【００１０】

【数４】により規定される。この式は、後述される式（２）を展
開した展開式である。加速度変動分（ｕ_ｋ）により、高
精細に、且つ、過去の加速度変動の特性が反映されて、
より現実に忠実な追跡制御が実行される。数学的推定精
細位置列は、後述される式（７）で表される。

【００１１】測定器（１３）は固定点に配置され、又
は、運動物体（４１）に固定されている。測定器（１
３）としてはカメラが好適であり、又は、測定器（１
３）としてはレーザー測距儀が好適である。現実位置列
は、カメラ（１３）を２軸回転させる２軸の回転角度位
置により記述され得る。追跡対象物体は第１自律運動物
体（４２）であり、測定器（１３）は第１自律運動物体
（４２）を追跡する第２自律運動物体（４１）に搭載さ
れることがある。

【００１２】この場合、現実位置列は、測定器（１３）
を回転させる軸の回転角度位置と、第１自律運動物体
（４２）と第２自律運動物体（４１）との間の相対的距
離、又は、相対位置間の位置ベクトルで記述される。第
１自律運動物体（４２）と第２自律運動物体（４１）が
自動車であれば、両自動車間の車間距離が自動的に高精
細に制御される。

【００１３】本発明による追跡方法は、自律運動標的
（３）の第１時系列点の現実位置列を自律運動標的から
離隔した離隔物体（２又は４）で測定すること、現実位
置列から、第１時系列点よりも高精細な第２時系列点の
物理的な高精細位置列（ｘ_ｋ）を計算すること、高精細
位置列（ｘ_ｋ）から高精細位置列（ｘ_ｋ）が含む測定誤
差を除去する数学的処理により推定高精細位置列（ｘ^∧
_ｋ）を計算すること、推定高精細位置列（ｘ^∧ _ｋ）に基
づいて自律運動標的（３）と離隔物体（４）の間の距離
ベクトルを制御することとから構成されている。

【００１４】

【発明の実施の形態】図に対応して、本発明による追跡
システムの実施の形態は、制御コントローラがビジュア
ルサーボとともに設けられている。その制御コントロー
ラ１は、図１に示されるように、ビジュアルサーボ２に
双方向に接続している。ビジュアルサーボ２は、雲台４
と、雲台４に支持されて旋回方向、俯仰方向に回転自在
であり目標物体（標的）３に向かう方向に光軸が制御さ
れる撮影機器５とを備えている。ビジュアルサーボ２
は、撮影機器５が取得する目標物体３の位置情報を画像
・サーボ位置情報６として制御コントローラ１に送信
し、制御コントローラ１は雲台４のサーボ制御に必要で
あるサーボコントロール信号７をビジュアルサーボ２に
送信する。

【００１５】図２は、制御コントローラ１を詳細に示し
ている。制御コントローラ１は、制御演算を実行するＤ
ＳＰボード８を備えている。制御コントローラ１は、Ｃ
ＰＵボード９と、Ｄ／Ａボード１１と、画像キャプチャ
１２とを更に備えている。ＣＰＵボード９は、Ｉ／Ｏコ
ントロールを実行する。画像キャプチャ１２は、ビジュ
アルサーボ２に付属する撮影機器５のカメラ１３が撮影
する画像１４を取得する。

【００１６】制御コントローラ１は、第１サーボドライ
バ１５と第２サーボドライバ１６とを更に備えている。
Ｄ／Ａボード１１は、第１サーボドライバ１５と第２サ
ーボドライバ１６に第１トルク指令１７と第２トルク指
令１８とをそれぞれに送信する。図１のサーボコントロ
ール信号７は、第１トルク指令１７と第２トルク指令１
８とから形成されている。画像キャプチャ１２は、図１
の画像・サーボ位置情報６をＣＰＵボード９に送信す
る。

【００１７】第１サーボドライバ１５と第２サーボドラ
イバ１６は、第１サーボモータ１９と第２サーボモータ
２１に接続している。制御コントローラ１は、更にエン
コーダカウンタ２２を備えている。エンコーダカウンタ
２２は、第１サーボモータ１９が出力する第１回転角度
位置信号２３と第２サーボモータ２１が出力する第２回
転角度位置信号２４を計数し、第１回転角度位置信号２
３を微分して第１モータ１９の第１回転速度を計算し、
第２サーボモータ２１の第２回転角度位置信号２４を微
分して第２モータ２１の第２回転速度を計算することが
できる。

【００１８】第１サーボモータ１９は雲台４の旋回方向
の回転を制御し、第２サーボモータ２１は雲台４の俯仰
方向の回転を制御する。画像キャプチャ１２は、ＰＣＩ
バス２５を介してＣＰＵボード９に接続し、ＣＰＵボー
ド９はＩＳＡバス２６を介してＤ／Ａボード１１とエン
コーダカウンタ２２とに双方向に接続している。

【００１９】図３は、制御コントローラ１とビジュアル
サーボ２の間で制御を実行する制御系２７を示してい
る。制御系２７は、ビジュアルサーボ２の雲台４とビジ
ュアルサーボ２の撮影機器５との間で主制御系を構成し
ている。目標物体３に対してカメラ１３が取得する取得
画像１２は、目標物体（標的）抽出画像処理２８で画像
処理を受ける。画像処理後に、処理された画像に基づい
て、測定周期で得られる目標物体位置ｘ_ｋが、撮影機器
５又は制御コントローラ１のＣＰＵボード９から出力さ
れる。

【００２０】目標物体位置ｘ_ｋ２９は、３０ミリ秒毎に
算出される３０ｍｓ単位第１回転位置と３０ｍｓ単位第
２回転位置であり、測定周期毎に測定値にリセットされ
る。測定値は、ｘ_０で表され、抽出画像処理２８により
測定周期毎に算出される。この算出は、座標系化された
画面の原点に対する画面上の標的点のＸ−Ｙ座標と、第
１回転角度位置信号２３の積分値と第２回転角度位置信
号２４の積分値とに基づいて算出される。

【００２１】制御系２７は、カルマンフィルタ３１を備
えている。カルマンフィルタ３１は、目標物体位置ｘ_ｋ
（ｋ＝０）２９に基づいて目標物体推定位置ｘ^∧ _ｋを出
力する。文章中の右肩添字”∧”は、数式表現では真上
添字で表される。目標物体推定位置ｘ^∧ _ｋは、第１位置
指令加減算器３２に加算値として入力される。

【００２２】第１サーボモータ１９と第２サーボモータ
２１が出力する第１回転角度位置信号２３と第２回転角
度位置信号２４（以下、Ｒ_ｋ）は、それぞれに減算値と
して第１位置指令加減算器３２に入力される。（ｘ^∧ _ｋ
−Ｒ_ｋ）は、速度指令として第２位置指令加減算器３３
に加算値として入力される。信号Ｒ_ｋは、速度変換器３
０で速度信号Ｖ_ｋに変換されて、第２位置指令加減算器
３３に減算値として入力される。

【００２３】第２位置指令加減算器３３から出力される
速度制御信号ＶＣ_ｋは、トルク指令を発生させる制御ブ
ロック３４に入力される。制御ブロック３４は一般的な
比例制御を行うフィードフォアワード制御部３５と、外
乱推定オブザーバ３６とで構成されている。その比例制
御には、制御機構の質量のイナーシャＪ_ｅ、流体圧シリ
ンダの粘性抵抗Ｄ_ｅ、モータ部分のクーロン摩擦Ｔ_Ｃｅ
が考慮され、ゲインＧ _ｖに加算され、又は、ゲインＧ_ｖ
から減算される。

【００２４】外乱推定オブザーバ３６には、フィードフ
ォアワード制御部３５が出力するトルク指令（Ｎｍ）３
７とモータ速度信号Ｖ_ｋが入力される。外乱推定オブザ
ーバ３６は、トルク指令（Ｎｍ）３７とモータ速度信号
Ｖ_ｋに基づいて外乱補正信号３８を出力する。外乱補正
信号３８は、減算値としてフィードフォアワード制御部
３５の出力点に入力され、トルク指令３７は補正された
状態でフィードフォアワード制御部３５の出力点から出
力される。

【００２５】２つのトルク指令３７は、第１サーボドラ
イバ１５と第２サーボドライバ１６にそれぞれに独立に
入力され、第１サーボドライバ１５と第２サーボドライ
バ１６は２様の旋回制御信号を第１サーボモータ１９と
第２サーボモータ２１にそれぞれに独立に出力する。ト
ルク指令３７は、１ミリ秒毎に算出される１ｍｓ単位第
１トルク指令と１ｍｓ単位第２トルク指令とから構成さ
れている。

【００２６】第１回転角度位置信号２３と第２回転角度
位置信号２４は、１ミリ秒毎に第１サーボモータ１９と
第２サーボモータ２１から出力されていて、１ｍｓ単位
第１トルク指令は１ミリ秒毎に第１サーボドライバ１５
に入力され、１ｍｓ単位第２トルク指令は１ミリ秒毎に
第２サーボドライバ１６に入力される。

【００２７】カルマンフィルタ３１は、標的の動特性か
らその動きを予測するアルゴリズムを持つフィルタであ
り、３０ミリ秒毎に画像キャプチャ１２から渡される目
標物体位置ｘ_ｋ（ｋ＝０）２９に基づいて、１ミリ秒毎
に現在の目標物体推定位置ｘ ^∧ _ｋを出力する。カルマン
フィルタ３１は、観測周期３０ミリ秒ごとに得られる画
像対応標的位置目標物体位置ｘ_０２９を制御周期１ミリ
秒毎の目標物体推定位置ｘ^∧ _ｋに変換する。画像対応標
的位置目標物体位置ｘ_０２９は、３０ｍ秒間隔の第１時
系列点の観測位置データである。

【００２８】次に、カルマンフィルタ３１が持つ推定機
能が例示される。標的の位置はｘ（ｔ）で表され、標的
の速度はｘ^・（ｔ）で表され、標的の加速度はｘ
^・・（ｔ）で表される。右肩添字の”・”と”・・”
は、時間の１回微分と２回微分を示し、数式表現では真
上添字として表されている。標的位置ｙは、記述の通り
画像データから観測周期ΔＶ毎（３０ｍｓ毎）に算出さ
れて得られている。観測周期ΔＶ毎に得られる標的位置
ｙ_ｋ（ｋ＝０）から制御周期ΔＴ毎（１ｍｓ毎）の標的
の推定位置ｘ^∧ _ｋ、推定速度ｘ^∧・ _ｋ、推定加速度ｘ
^∧・・ _ｋの数列を推定することが以下に考えられる。

【００２９】時刻又は基準時刻からの経過時間は、制御
周期ΔＴの整数倍のｋΔＴで表される。標的の位置ｘ
（ｋΔＴ）、速度ｘ^・（ｋΔＴ）、加速度ｘ^・・（ｋΔ
Ｔ）は、縦行列により次式で表される。

【数５】

【００３０】ΔＴ秒の間の加速度が一定である近似によ
り、次の位置・速度・加速度数列が成立する。

【数６】式（３）は、ｋ＝０又はｋ＝ｎで、ｙ_ｋ＝ｘ_ｋであるこ
とを示している。ここで、ｎはΔＶ／ΔＴであり、ｋは
０〜ｎである。ｋはｎ回目でリセットされ０に戻る。式
（３）のｗ_ｋは、測定位置誤差であり、ｕ_ｋは時刻ｋΔ
Ｔから時刻（ｋ＋１）ΔＴまでの標的の加速度変動であ
る。ｗ_ｋとｕ_ｋとが白色雑音とみなされれば、

【数７】で表される行列Ａ，Ｂ，Ｃが用いられて、ΔＴ秒毎の目
標物体推定位置ｘ^∧ _ｋは、次式で計算される。

【００３１】

【数８】

【００３２】式（７）と式（８−１，２，３）で、Ｕ_ｋ
はｕ_ｋの分散、Ｗ_ｋはｗ_ｋの分散、ｕ⁻ _ｋはｕ_ｋの平均
値、ｗ⁻ _ｋはｗ_ｋの平均値を示す。Ｐ_ｋは、初期値Ｐ_０
又はＰ_１から漸化式である式（８−２）から順次に求め
られる。カルマンフィルタ３１は、式（７）の変換を行
う機能を有するコンピュータである。１ｍｓ毎に、目標
物体推定位置ｘ^∧ _ｋが式（８−１，２，３）によって数
列化され、式（７）の第２項の因数｛ｙ_ｋ−（Ｃｘ^〜 _ｋ
−ｗ⁻ _ｋ）｝に基づいて、１ｍｓ毎に、測定誤差ｗ_ｋを
漸化的に除去する数学的フィルタリングが実行される。
この数学的フィルタリングは、物理的推定のｘ_ｋのより
現実的に高精度な数学的推定である。

【００３３】このように推定的に得られる目標物体推定
位置ｘ^∧ _ｋは、図３の第１位置指令加減算器３２に入力
される。目標物体推定位置ｘ^∧ _ｋと１ミリ秒毎のリアル
タイムの既述の回転位置信号Ｒ_ｋとに基づく制御指令信
号のゲインＧ_ｖは、一般的な比例制御のイナーシャ
Ｊ_ｅ、粘性抵抗Ｄ_ｅ、クーロン摩擦Ｔ_Ｃが考慮される補
正を受け、更に、外乱が考慮され、トルク指令信号に変
換されて制御ブロック３４から出力され、第１サーボド
ライバ１５と第２サーボドライバ１６に入力される。

【００３４】第１サーボモータ１９と第２サーボモータ
２１は、既述の差分信号（ｘ^∧ _ｋ−Ｒ_ｋ）に基づいて、
第１サーボドライバ１５と第２サーボドライバ１６とに
より制御されて駆動し、１ミリ秒毎に、ビジュアルサー
ボ２の撮影機器５の基準光軸が標的に向かって修正さ
れ、撮影機器５はダイナミックに運動する標的を高精細
に追尾・追跡する。この高精細な追尾・追跡は、推定位
置に向かって１ｍｓ単位で制御され、過激な加速度制
御、過激な運動方向制御を受けず、その制御はより滑ら
かである。

【００３５】このように小刻みに追跡して回転運動する
質量物体であるビジュアルサーボ２は、小さい運動幅で
小刻みに運動し、修正幅、修正中の速度、修正中の加速
度が小さいので、制御目標位置に対して高精度に且つ滑
らかに姿勢が制御され、ビジュアルサーボ２の運動部位
は、１ミリ秒毎の繰り返し制御により追跡物体に対して
静止するほどに高精度追跡状態になって安定している。
変動が起きた場合にも、従来との比較で、（１／３０）
の何乗かの短い時間で速やかにその安定状態に戻ること
ができる。

【００３６】標的を障害物等で観測できない状態になっ
た場合、標的観測位置を最終観測点に固定すれば、カル
マンフィルタ３１の推定は標的が静止してしまったと見
なしてしまうので、カルマンフィルタ３１のアルゴリズ
ムに代替されて他のアルゴリズムが用いられる。他のア
ルゴリズムとしては、例えば、観測された標的の過去の
位置情報（過去の位置、速度、加速度と運動軌跡）を数
十秒間保存し、その保存された位置の全点を通る補間公
式から外挿して標的の位置を推定するアルゴリズムが適
用され得る。

【００３７】このようなアルゴリズムでは、推定位置に
大きなバラツキがある場合（後述される実施の形態の自
動車が高速道路上で蛇行したような場合）、記憶してい
る位置情報のうち標的の平均的運動とかけ離れた運動を
示す点（速度と加速度とにより判別される）を取り除い
た位置情報に基づいてその補間公式を計算する。

【００３８】次に、計算の実証例が既述される。標的の
加速度が、次式で与えられる：

【数９】

【００３９】式（９）と式（１０）に、ビジュアルサー
ボ２からの目標物体位置ｙ_ｖ（ｍ）が０．０１秒毎に観
測誤差ｗ”ｖ”（ｍ）を持つ観測が可能である条件が設
定される。カルマンフィルタにより推定した標的推定位
置（本発明）と直線補間の外挿により推定した標的推定
位置（公知技術）の比較が、図４に示されている。図４
は、横軸が秒数（５秒間）を示し、縦軸が標的位置を示
している。

【００４０】図４（ａ）は、式（９）を実質的に連続的
である時間軸上で計算された標的位置を示し、図４
（ｃ）は０．０１秒毎に直線補間の外挿により推定した
標的位置を示し、図４（ｂ）は０．０１秒毎にカルマン
フィルタにより推定した標的位置を示している。カルマ
ンフィルタにより推定した標的位置は、標的の実際位置
に忠実に再現されている。直線補間の外挿により推定し
た標的位置は、観測点の実際の値に対してある幅で正負
に揺れ動いている。０．００１秒間隔の時刻点列による
計算では、直線補間の標的位置は、更に大幅に揺れ動く
ことになる。

【００４１】図５は、推定位置と実際位置の差を示して
いる。図５（ａ）は、カルマン推定値と実際値との差を
示し、図５（ｂ）は、直線補間推定値と実際値との差を
示している。図６の表に示されるように、本発明による
差の絶対値の平均が０．４程度であることに対して、公
知技術による差の絶対値の平均は１．３程度である。公
知技術による瞬間的揺れ幅は、本発明による瞬間的揺れ
幅に対して非常に大きい。

【００４２】図７は、本発明による追跡システムの実施
の他の形態を示している。本実施の形態は、図８に示さ
れる２台の自律走行車輌４１，４２の間の車間距離を主
制御対象にしている。図３のカメラ１３に対応して、第
１カメラ１３ａと第２カメラ１３ｂが用いられ、追尾す
る側の第１自律走行車輌４１に２位置で搭載されてい
る。図３の画像キャプチャ１２に対応して、第１画像キ
ャプチャ１２ａと第２画像キャプチャ１２ｂが用いら
れ、第１自律走行車輌４１に搭載されている。

【００４３】図３の制御系２７に対応して、第１制御系
２７ａと第２制御系２７ｂが用いられ、第１自律走行車
輌４１に搭載されている。第１制御系２７ａは図３のカ
ルマンフィルタ３１に対応する第１カルマンフィルタ３
１ａを備え、第２制御系２７ｂは図３のカルマンフィル
タ３１に対応する第２カルマンフィルタ３１ｂを備えて
いる。

【００４４】第１制御系２７ａは更に第１制御器４３ａ
を備え、第２制御系２７ｂは更に第２制御器４３ｂを備
えている。第１制御器４３ａは、図３の速度変換器３
０、第１位置指令加減算器３２、第２位置指令加減算器
３３、制御ブロック３４に対応する諸機器（図示され
ず）を備えている。第２制御器４３ｂは、図３の速度変
換器３０、第１位置指令加減算器３２、第２位置指令加
減算器３３、制御ブロック３４に対応する諸機器（図示
されず）を備えている。

【００４５】本実施の形態は、既述の実施の形態の制御
系が２つ存在し、その制御系の一つは、第１自律走行車
輌４１の車間距離を第１カルマンフィルタ３１ａを用い
て制御し、その制御系の他の一つは、第１自律走行車輌
４１の方位を第２カルマンフィルタ３１ｂを用いて制御
する。車間距離制御の制御対象は第１自律走行車輌４１
の駆動装置４４であり、方位制御の制御対象は第１自律
走行車輌４１の操舵装置４５である。

【００４６】方位、車間距離の推定値は、式（７）のｘ
^∧ _ｋがそのままに用いられる。第１自律走行車輌４１駆
動装置４４が電気モータであり、第１自律走行車輌４１
の操舵装置が電気モータ式ハンドリング装置であれば、
図３の第１サーボドライバ１５と第２サーボドライバ１
６は、図７の駆動装置４４と操舵装置４５にそのまま対
応する。車間距離は、２台のカメラの画面上の２座標系
上の２標的画像座標に基づいて、零差フィルタ法により
算出される。

【００４７】図３の第１位置指令加減算器３２に対応し
て、第１制御系２７ａに第１制御系側第１位置指令加減
算器３２ａが設けられている。図３の第１位置指令加減
算器３２に対応して、第２制御系２７ｂに第２制御系側
第１位置指令加減算器３２ｂが設けられている。第１制
御系側第１位置指令加減算器３２ａには、図３の第２回
転角度位置信号２３（Ｒ_ｋ）に代替されて車間距離指令
４５が入力される。第２制御系側第１位置指令加減算器
３２ｂには、図３の第２回転角度位置信号２４（Ｒ_ｋ）
に同等である操舵装置４５のエンコーダの出力信号が入
力される。

【００４８】既述の実施の形態と全く同じに、第１自律
走行車輌４１と第２自律走行車輌４２の相対的関係であ
る車間距離と方位とが、第１カルマンフィルタ３１ａと
第２カルマンフィルタ３１ｂにより推定される推定車間
距離と推定方位に基づいて高精細に制御される。車間距
離制御は、駆動装置のモータの回転速度制御を伴うこと
は有意義である。実際の車間距離と指令される車間距離
との差分がより大きい場合には回転速度がより速く、そ
の差分がより小さい場合には回転速度がより遅い傾向を
持つ速度制御は、公知技術に従って実行され得る。

【００４９】第２自律走行車輌４２を見失った場合に
は、第１自律走行車輌４１は既述の補間公式に従って第
２自律走行車輌４２の推定現在方位と推定車間距離に基
づいて第２自律走行車輌４２の再発見を試みる。

【００５０】

【発明の効果】本発明による追跡システム、及び、追跡
方法は、観測周期よりも短い制御周期で、追跡対象物体
の過去の動特性が反映される高精細な追跡制御が可能で
ある。物理的推定に更に数学的推定が加味され、結果的
に究極的には、連続時間上の制御に近づく超高精細な追
跡が可能になる。

【図面の簡単な説明】

【図１】図１は、本発明による追跡システムの実施の形
態を示すシステムブロック図である。

【図２】図２は、制御系を示す回路ブロック図である。

【図３】図３は、制御系を更に詳細に示す回路ブロック
図である。

【図４】図４は、制御性能のデータを示す関数図であ
る。

【図５】図５は、制御性能の他のデータを示す関数図で
ある。

【図６】図６は、平均値比較を示す表である。

【図７】図７は、本発明による追跡システムの実施の形
態を示す回路ブロック図である。

【図８】図８は、制御対象を示す平面図である。

【符号の説明】

３，４２…追跡対象物体（自律運動標的、自律運動物
体）５，１３，４１…離隔物体１３…カメラ３１…カルマンフィルタ３５，４３ａ，４３ｂ…制御器４１…第２自律運動物体（実施の形態では、第１自律運
動物体）４２…第１自律運動物体（実施の形態では、第２自律運
動物体）

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁷ 識別記号ＦＩテーマコート゛(参考）Ｂ６２Ｄ 6/00 Ｂ６２Ｄ 6/00 Ｆ０２Ｄ 29/02 ３０１Ｆ０２Ｄ 29/02 ３０１Ｄ // Ｂ６２Ｄ 113:00 Ｂ６２Ｄ 113:00 137:00 137:00 Ｆターム(参考） 3D020 BA20 BB01 BC03 BD03 BE03 3D032 CC20 DA04 DA77 DC38 DD02 EB04 EC34 GG01 3D044 AA25 AA45 AB01 AC59 AE03 3G093 AA01 BA04 BA27 CB01 DB16 EC01 FA01 5H301 AA01 AA10 BB20 CC06 CC08 GG09 GG14 GG19 QQ08

Claims

【特許請求の範囲】

【請求項１】追跡対象物体の現実位置列を第１時刻列点
で測定する測定器と、前記第１時系列点よりより高精細である第２時系列点で
前記追跡対象物体の物理的な動特性である第１精細位置
列を計算する計算機と、前記第１精細位置列から前記現実位置列の測定誤差を除
去する数学的処理を実行して第２精細位置列を計算する
計算機と、前記第２精細位置列に基づいて、前記第１時刻列点の間
で前記追跡対象物体と前記測定器の間の相対的距離を高
精細に制御する制御器とを含む追跡システム。
【請求項２】前記動特性は、前記現実位置列に基づく速
度列と、前記速度列に基づく加速度列と、前記加速度列
に基づく加速度変動列とから形成されている請求項１の
追跡システム。
【請求項３】前記第１時系列点の隣り合う２点間の時間
がＴで表され、前記２点間がｎ分割され、ｋは０〜ｎの
精細順序列数であり、ΔＴはＴ／ｎで表されて、前記第１精細位置列は、位置列ｘ_ｋ、速度列ｘ^・ _ｋ、加
速度列ｘ^・・ _ｋから形成され、前記加速度変動列ｕ_ｋに
基づいて、次式：【数１】により規定される請求項２の追跡システム。
【請求項４】前記第２精細位置列は、次式：【数２】で表され、前記式中、ＡとＢとＣとは、次式：【数３】で定義されている請求項３の追跡システム。
【請求項５】前記測定器は固定点に配置されている請求
項１〜４から選択される１請求項の追跡システム。
【請求項６】前記測定器はカメラであり、前記現実位置列は、前記カメラを２軸回転させる２軸の
回転角度位置により記述される請求項５の追跡システ
ム。
【請求項７】前記追跡対象物体は第１自律運動物体であ
り、前記測定器は前記第１自律運動物体を追跡する第２自律
運動物体に搭載されている請求項１〜４から選択される
１請求項の追跡システム。
【請求項８】前記現実位置列は、前記測定器を回転させ
る軸の回転角度位置と、前記第１自律運動物体と前記第
２自律運動物体との間の相対的距離とで記述される請求
項７の追跡システム。
【請求項９】前記第１自律運動物体と前記第２自律運動
物体は自動車である請求項８の追跡システム。
【請求項１０】自律運動標的の第１時系列点の現実位置
列を前記自律運動標的から離隔した離隔物体で測定する
こと、前記現実位置列から、前記第１時系列点よりも高精細な
物理的な第２時系列点の高精細位置列を計算すること、前記高精細位置列から前記高精細位置列が含む測定誤差
を除去する数学的処理により推定高精細位置列を計算す
ること、前記推定高精細位置列に基づいて前記自律運動標的と前
記離隔物体の間の距離ベクトルを制御することとを含む
追跡方法。