JP5595181B2 - 追尾装置 - Google Patents
追尾装置 Download PDFInfo
- Publication number
- JP5595181B2 JP5595181B2 JP2010187472A JP2010187472A JP5595181B2 JP 5595181 B2 JP5595181 B2 JP 5595181B2 JP 2010187472 A JP2010187472 A JP 2010187472A JP 2010187472 A JP2010187472 A JP 2010187472A JP 5595181 B2 JP5595181 B2 JP 5595181B2
- Authority
- JP
- Japan
- Prior art keywords
- processing unit
- bias error
- sensor
- motion model
- dynamic data
- 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.)
- Active
Links
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
ここで、図11を参照しながら、追尾装置に用いられるセンサのバイアス誤差を補正する目的について説明する。
図11(a)において、2つのセンサA、Bは、それぞれ、方位方向(破線座標参照)にバイアス誤差を有するものとし、3つの目標T1、T2、T3(○印、△印、□印参照)を同時に観測しているものとする。
つまり、各センサA、Bで観測した目標の状態量(目標の位置、速度、加速度など)を一致させることが必要となる。
また、バイアス誤差として、距離および方位角、仰角および方位角、距離および仰角、のいずれで構成してもよく、さらに、距離のみ、仰角のみ、または方位角のみで構成してもよい。
同様に、バイアス誤差は、直交座標X、Y、Zの成分からなるバイアス誤差としてもよく、また、バイアス誤差の成分は、上記距離、仰角および方位角の極座標の組合せと同様に、直交座標、または緯度、経度および高度からなる座標、または地球中心座標など、他の種々の座標で定義することができる。
しかし、従来装置においては、目標の運動を考慮して時刻同期を取るような処理は行われていないので、従来技術で時刻同期を取る方法を実現しようとする場合を考える。
ただし、実際には、目標T1、T2、T3の運動は未知であることから、等速直線運動の仮定が崩れると、各センサA、Bの観測値または航跡を外挿した結果が悪くなるので、かえってバイアス誤差推定精度が劣化するといった問題が考えられる。
以下、図面を参照しながら、この発明の好適な実施の形態について説明する。
図1はこの発明の実施の形態1に係る追尾装置を示すブロック構成図である。
図1において、追尾装置は、複数のセンサを含むセンサ群10と、補正処理部40と、一致度判定処理部50と、運動モデル設定処理部60と、カルマンフィルタによるバイアス誤差推定器(図示せず)を含むバイアス誤差推定処理部70と、バイアス誤差データベース80と、バイアス誤差補正処理部90と、制御工学における遅延部100とを備えている。
第1のセンサ11は、基準座標に対するバイアス誤差が存在しないセンサからなり、目標に搭載されたGPS(Global Positioning System:全地球測位システム)/IMU(Inertial Measurement Unit:慣性計測装置)からの目標Tの動態データを取得し、動態データを補正処理部40に入力する。
動態データは、以下の式(1)で表される状態ベクトルを有するものとする。
また、記号「’」は、ベクトルおよび行列の転置を表しており、このことは後述の数式においても同様である。
図2において、姿勢角[dx、dy、dz]は、x軸、y軸およびz軸周りの回転角に相当する。
図3において、UVW座標は、レーダ位置を原点Oとし、目標Tの速度ベクトル方向をu軸の正方向とし、水平面に平行な面内で速度ベクトルに対して垂直で右手の方向をv軸の正方向とし、速度ベクトルに対して垂直で下向きの方向をw軸の正方向とした座標系である。
たとえば、第2のセンサ12を原点とする北基準の直交座標系において、距離、仰角および方位角のバイアス誤差を有するものとする。
また、第2のセンサ12は、基準座標に対するバイアス誤差が付加された観測値を入力情報とした追尾を行い、追尾結果として、基準座標に対するバイアス誤差が付加された航跡を算出する。
また、第2のセンサ12の航跡は、状態ベクトルX(A)の誤差共分散行列を有する。
第2のセンサ12の航跡の誤差共分散行列は、ERRMAT(A)で表される。
また、ここでは図示しないが、前述と同様に、バイアス誤差は、緯度、経度および高度で定義することもでき、どのような座標でもバイアス誤差を定義することは可能である。
時刻合わせが行われた動態データX#(O)は、以下の式(3)のように定義される。
図7において、第1のセンサ11から補正処理部40への動態データの入力タイミングは時刻t1であり、第2のセンサ12の航跡の補正処理部40への入力タイミングは時刻t2であり、第2のセンサ12の航跡と動態データとの時刻差Δt(=t2−t1)が生じるが、補正処理部40の出力タイミングは、時刻t2に合わせられる。
このように、補正処理部40からは、第2のセンサ12の航跡すなわち状態ベクトルX(A)と、第2のセンサ12の航跡が得られた最新時刻まで時刻合わせが行われた動態データX#(O)とが出力される。
このとき、両者が一致しているか否かは、以下の式(5)、式(6)のように判定される。
|X(A)−X#(O)|>TH、であれば「不一致フラグ」を出力 ・・・(6)
なお、一致度は、式(5)、式(6)の左辺|X(A)−X#(O)|で定義される。
また、式(5)、式(6)の右辺は、事前に設定された閾値THを示している。
このように、カルマンフィルタによるバイアス誤差推定処理を行う際に、第1のセンサ11の動態データと、第2のセンサ12の航跡との一致度を比較して、予測ベクトル算出式を適切に選択することにより、目標Tが旋回運動を行う場合であっても、高精度のバイアス誤差推定値を得ることが可能となる。
なお、上記実施の形態1(図1)では、一致度判定処理部50において、第2のセンサ12の航跡(状態ベクトル)X(A)と、時刻合わせが行われた動態データX#(O)との比較に基づき一致度を判定したが、UVW座標における3次元加速度Aのユークリッドノルムによる大きさに基づき一致度を判定してもよい。
したがって、目標Tが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
なお、上記実施の形態2では、一致度判定処理部50において、UVW座標における3次元加速度Aのユークリッドノルムによる大きさに基づき一致度を判定したが、運動補償係数のユークリッドノルムによる大きさに基づき一致度を判定してもよい。
この場合、一致度判定処理部50は、運動補償係数のユークリッドノルムによる大きさが事前に決めた閾値TH以下の場合には、「一致フラグ」を出力し、閾値THよりも大きい場合には、「不一致フラグ」を出力する。
したがって、目標Tが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
なお、上記実施の形態1〜3(図1)では、一致度(一致フラグまたは不一致フラグ)に応答する運動モデル設定処理部60のみを用いて、バイアス誤差推定処理部70(カルマンフィルタによるバイアス誤差推定器)で使用される予測ベクトル算出用の制御信号を生成したが、図8のように、ユーザの設定条件にしたがう運動モデル制御処理部130を追加してもよい。
図8において、運動モデル設定処理部60には、運動モデル制御処理部130が接続されている。
なお、上記実施の形態4(図8)では、動態データを使用し続けるための制御信号を運動モデル設定処理部60に入力する運動モデル制御処理部130を追加したが、図9のように、第2のセンサ12から得られるサンプリングレートに基づく制御信号を運動モデル設定処理部60に入力する運動モデル制御処理部140を追加してもよい。
図9において、補正処理部40、一致度判定処理部50、運動モデル設定処理部60およびバイアス誤差推定処理部70には、運動モデル制御処理部140が接続されている。
また、運動モデル制御処理部140は、補正処理部40、一致度判定処理部50および運動モデル設定処理部60に対して、処理停止信号を入力して各処理を中止し、センサ群10からの観測値を、バイアス誤差推定処理部70で使用する処理のみ動作させるようにする。
さらに、前述と同様に、目標Tが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
なお、上記実施の形態1〜5(図1、図8、図9)では、言及しなかったが、図10のように、第1のセンサ11からの動態データに基づき、第2のセンサ12の航跡を上書きするセンサ航跡更新処理部150を追加してもよい。
図10において、補正処理部40には、第1のセンサ11の動態データに基づき、第2のセンサ12の航跡を上書きするセンサ航跡更新処理部150が接続されている。
さらに、前述と同様に、目標Tが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
Claims (6)
- 目標の基準座標に対するバイアス誤差が存在しない動態データを取得する第1のセンサと、
前記目標の前記基準座標に対するバイアス誤差を有する距離、仰角および方位角を取得する第2のセンサと、
前記動態データを等速直線運動モデルに従い外挿するとともに、前記動態データの加速度を含む情報を基に算出される運動補償係数を加算することにより、前記動態データを前記第2のセンサの航跡出力時刻に時刻合わせを行う補正処理部と、
前記補正処理部により前記時刻合わせが行われた前記動態データと前記第2のセンサの航跡とのノルムが予め設定された第1の閾値以下である場合には「一致フラグ」を出力し、前記第1の閾値より大きい場合には「不一致フラグ」を出力する一致度判定処理部と、
後記バイアス誤差推定処理部におけるバイアス誤差推定処理で使用する予測ベクトル算出式を、前記一致度判定処理部が前記「一致フラグ」を出力している場合には、前記等速直線運動モデルに従った予測ベクトル算出式を使用し、一方、前記一致度判定処理部が前記「不一致フラグ」を出力している場合には、前記等速直線運動モデルに従って外挿した後に前記運動補償係数を加算する予測ベクトル算出式を使用するように切替える運動モデル設定処理部と、
前記運動モデル設定処理部の設定結果に基づき、カルマンフィルタによるバイアス誤差推定器を用いて前記第2のセンサの航跡の前記バイアス誤差推定処理を行うバイアス誤差推定処理部と、
前記バイアス誤差推定処理部により得られるバイアス誤差ベクトル推定値を時系列に蓄積するバイアス誤差データベースと、
前記バイアス誤差データベース内の前記バイアス誤差ベクトル推定値を用いてバイアス誤差補正値を算出するバイアス誤差補正処理部と、
前記バイアス誤差補正処理部が出力する前記バイアス誤差補正値を前記第2のセンサの最新時刻まで遅延させて前記第2のセンサを含むセンサ群に出力することにより、前記第2のセンサの航跡の観測値のバイアス誤差補正を行うことを可能にする遅延部と、
を備えたことを特徴とする追尾装置。 - 前記一致度判定処理部は、前記動態データの前記加速度のノルムが、予め設定された第2の閾値以下である場合には前記「一致フラグ」を出力し、前記第2の閾値より大きい場合には前記「不一致フラグ」を出力することを特徴とする請求項1に記載の追尾装置。
- 前記一致度判定処理部は、前記動態データの加速度を含む情報を基に前記補正処理部によって算出される前記運動補償係数のノルムが、予め設定された第3の閾値以下である場合には前記「一致フラグ」を出力し、前記第3の閾値より大きい場合には前記「不一致フラグ」を出力することを特徴とする請求項1に記載の追尾装置。
- 前記運動モデル設定処理部に接続され、前記運動モデル設定処理部に対して、前記一致度判定処理部の出力値にかかわらず、予め設定した予測ベクトル算出式に切替えるよう指示するための制御信号を出力する運動モデル制御処理部を更に備え、
前記運動モデル設定処理部は、前記運動モデル制御処理部が前記制御信号を出力している間は、前記一致度判定処理部からの出力値にかかわらず、前記予め設定した予測ベクトル算出式に切替えることを特徴とする請求項1から請求項3までのいずれか1項に記載の追尾装置。 - 前記補正処理部、前記一致度判定処理部および前記運動モデル設定処理部に接続された運動モデル制御処理部を備え、
前記運動モデル制御処理部は、前記第2のセンサから得られるサンプリングレートが事前に決めたサンプリング閾値よりも小さい場合には、等速直線運動に基づく予測ベクトル算出処理によりバイアス誤差推定を行うための制御信号を前記運動モデル設定処理部に入力するとともに、前記補正処理部、前記一致度判定処理部および前記運動モデル設定処理部に処理停止信号を入力することを特徴とする請求項1から請求項3までのいずれか1項に記載の追尾装置。 - 前記動態データに基づき、前記第2のセンサの航跡を上書きするセンサ航跡更新処理部を備えたことを特徴とする請求項1から請求項5までのいずれか1項に記載の追尾装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2010187472A JP5595181B2 (ja) | 2010-08-24 | 2010-08-24 | 追尾装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2010187472A JP5595181B2 (ja) | 2010-08-24 | 2010-08-24 | 追尾装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2012047490A JP2012047490A (ja) | 2012-03-08 |
JP5595181B2 true JP5595181B2 (ja) | 2014-09-24 |
Family
ID=45902557
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2010187472A Active JP5595181B2 (ja) | 2010-08-24 | 2010-08-24 | 追尾装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5595181B2 (ja) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10853669B2 (en) | 2016-06-24 | 2020-12-01 | Mitsubishi Electric Corporation | Object recognition device, object recognition method and self-driving system |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6169146B2 (ja) * | 2015-10-16 | 2017-07-26 | 三菱電機株式会社 | 物体認識統合装置および物体認識統合方法 |
JP6702562B2 (ja) * | 2017-03-03 | 2020-06-03 | 株式会社東芝 | マルチセンサシステム、センサバイアス推定装置、センサバイアス推定方法及びセンサバイアス推定プログラム |
JP6759130B2 (ja) * | 2017-03-03 | 2020-09-23 | 株式会社東芝 | センサネットワークシステム、データ融合システム、センサバイアス推定装置、センサバイアス推定方法及びセンサバイアス推定プログラム |
JP6906409B2 (ja) * | 2017-09-21 | 2021-07-21 | 三菱電機株式会社 | 追尾処理装置及び追尾処理方法 |
JP6986980B2 (ja) * | 2018-01-18 | 2021-12-22 | 三菱電機株式会社 | 追尾処理装置及び追尾処理方法 |
KR101975462B1 (ko) * | 2018-06-14 | 2019-05-07 | 엘아이지넥스원 주식회사 | 다중 펄스 압축 기법 기반의 레이더 수신신호 처리 방법 및 그를 위한 장치 |
CN112033454B (zh) * | 2020-08-28 | 2022-07-12 | 杭州海康威视数字技术股份有限公司 | 轨迹数据处理方法和装置 |
CN115291526B (zh) * | 2022-09-30 | 2023-05-09 | 北京航天自动控制研究所 | 一种基于滚动时域的最优跟踪制导方法 |
CN117648001B (zh) * | 2024-01-29 | 2024-04-09 | 微网优联科技(成都)有限公司 | 基于云台机的追踪校正方法及系统 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5094195B2 (ja) * | 2006-04-20 | 2012-12-12 | 三菱電機株式会社 | システム航跡測定装置および航跡割当・航跡誤差補正処理方法 |
JP5383120B2 (ja) * | 2008-08-28 | 2014-01-08 | 三菱電機株式会社 | 追尾装置 |
JP2011064484A (ja) * | 2009-09-15 | 2011-03-31 | Mitsubishi Electric Corp | センサバイアス推定装置 |
-
2010
- 2010-08-24 JP JP2010187472A patent/JP5595181B2/ja active Active
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10853669B2 (en) | 2016-06-24 | 2020-12-01 | Mitsubishi Electric Corporation | Object recognition device, object recognition method and self-driving system |
Also Published As
Publication number | Publication date |
---|---|
JP2012047490A (ja) | 2012-03-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5595181B2 (ja) | 追尾装置 | |
US10306206B2 (en) | 3-D motion estimation and online temporal calibration for camera-IMU systems | |
CN111381217B (zh) | 基于低精度惯性导航系统的弹载sar运动补偿方法 | |
US8793090B2 (en) | Track information generating device, track information generating method, and computer-readable storage medium | |
US9037411B2 (en) | Systems and methods for landmark selection for navigation | |
JP2011064484A (ja) | センサバイアス推定装置 | |
CN104764452A (zh) | 一种基于惯性和光学跟踪系统的混合位姿跟踪方法 | |
JP2008116370A (ja) | 移動体位置測位装置 | |
JP2015148450A (ja) | センサ誤差補正装置、imu較正システム、imu較正方法、および、imu較正プログラム | |
JP2004239643A (ja) | ハイブリッド航法装置 | |
WO2018066754A1 (ko) | 라이다 센서를 이용한 차량의 자세 추정 방법 | |
CN110637209B (zh) | 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质 | |
KR101296190B1 (ko) | 지형 정보 활용 항법 및 그 구동 방법 | |
JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
JP2018044867A (ja) | ステレオ測距装置、ステレオ測距方法及びステレオ測距プログラム | |
JP4804371B2 (ja) | センサのバイアス誤差推定装置 | |
US20180259653A1 (en) | Method and device for calculating attitude angle | |
KR20120054347A (ko) | Sar 영상 형성을 위한 도플러 중심 주파수 정보 추정방법 및 그를 실행시키기 위한 프로그램을 기록한 컴퓨터 판독가능한 매체 | |
JP4343581B2 (ja) | 移動体の姿勢検出装置 | |
JP3439068B2 (ja) | センサ姿勢及び位置のバイアス誤差推定装置 | |
KR100522205B1 (ko) | 선박에 장착되는 조준 장치의 시차 보정 방법 | |
WO2020080405A1 (ja) | 表示システム、表示装置、及び表示制御方法 | |
JP5383120B2 (ja) | 追尾装置 | |
JP2017181195A (ja) | 測定装置、測定方法、及び、プログラム | |
EP1749183A1 (en) | Alignment of a flicht vehicle based on recursive matrix inversion |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20121119 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20130930 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20131008 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20140708 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20140805 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5595181 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |