JP2015197403A - 目標追尾装置 - Google Patents
目標追尾装置 Download PDFInfo
- Publication number
- JP2015197403A JP2015197403A JP2014076606A JP2014076606A JP2015197403A JP 2015197403 A JP2015197403 A JP 2015197403A JP 2014076606 A JP2014076606 A JP 2014076606A JP 2014076606 A JP2014076606 A JP 2014076606A JP 2015197403 A JP2015197403 A JP 2015197403A
- Authority
- JP
- Japan
- Prior art keywords
- observation
- noise matrix
- matrix
- target
- calculation unit
- 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.)
- Pending
Links
Images
Abstract
【課題】 移動体の運動状態により観測誤差の大きさが変化する状況にあっても、目標を高精度に追尾する目標追尾装置を提供する。
【解決手段】 目標の運動モデルと、モデル誤差の大きさを表すプロセスノイズ行列と、センサによる観測誤差の大きさを表す観測ノイズ行列の値を用いたカルマンフィルタにおいて、観測ノイズ行列に自らの移動体の運動状態に合わせて変化する値を設定する。観測ノイズ行列計算処理によってフィルタ特性をその場に応じて適応的に変化させることができ、結果として、目標の位置等の状態量を高精度に推定可能となる。
【選択図】 図1
【解決手段】 目標の運動モデルと、モデル誤差の大きさを表すプロセスノイズ行列と、センサによる観測誤差の大きさを表す観測ノイズ行列の値を用いたカルマンフィルタにおいて、観測ノイズ行列に自らの移動体の運動状態に合わせて変化する値を設定する。観測ノイズ行列計算処理によってフィルタ特性をその場に応じて適応的に変化させることができ、結果として、目標の位置等の状態量を高精度に推定可能となる。
【選択図】 図1
Description
この発明は、車両や飛しょう体等の移動体に搭載され、自身の運動量に基づいて、観測ノイズ行列の値を更新可能な目標追尾装置に関するものである。
従来、車両や飛しょう体等の移動体において目標を追尾する際、目標の運動を推定する計算としてカルマンフィルタが広く用いられている。
目標を捕捉するためのセンサであるシーカから得られる目標の観測結果には、観測誤差が含まれており、その観測誤差を取り除く手段として、目標の運動モデルに基づいて状態量を推定するカルマンフィルタは非常に有効である。カルマンフィルタには、線形カルマンフィルタ(例えば、非特許文献1のP.163〜P.184参照)や、非線形カルマンフィルタ(例えば、非特許文献1のP.373〜P.388参照)等の種類がある。
また、従来、目標の状態量を推定する装置が開示されている(例えば、特許文献1参照)。
目標を捕捉するためのセンサであるシーカから得られる目標の観測結果には、観測誤差が含まれており、その観測誤差を取り除く手段として、目標の運動モデルに基づいて状態量を推定するカルマンフィルタは非常に有効である。カルマンフィルタには、線形カルマンフィルタ(例えば、非特許文献1のP.163〜P.184参照)や、非線形カルマンフィルタ(例えば、非特許文献1のP.373〜P.388参照)等の種類がある。
また、従来、目標の状態量を推定する装置が開示されている(例えば、特許文献1参照)。
"Tactical and Strategic Missile Guidance Fifth Edition", Paul Zarchan, ISBN-10:1-56347-874-9
カルマンフィルタの特性は、目標の運動モデルに加えて、モデル誤差の大きさを表すプロセスノイズ行列と、シーカによる観測誤差の大きさを表す観測ノイズ行列の値によって決定される。このうち、目標の観測結果に含まれる観測誤差の大きさは、装置の搭載された移動体の運動状態によって変化しうる。そのため、カルマンフィルタの観測ノイズ行列の値も、移動体の運動状態に合わせて変化させるのが望ましい。
しかしながら、従来は移動体の運動状態に関わらず、観測ノイズ行列の値は固定されており、観測誤差の大きさの変化が考慮されていなかった。
そのため、必ずしもその場に応じた適切なフィルタ特性となっていないことがあり、このことが、目標状態量の推定精度の向上を図る上での課題となっていた。
しかしながら、従来は移動体の運動状態に関わらず、観測ノイズ行列の値は固定されており、観測誤差の大きさの変化が考慮されていなかった。
そのため、必ずしもその場に応じた適切なフィルタ特性となっていないことがあり、このことが、目標状態量の推定精度の向上を図る上での課題となっていた。
この発明は係る課題を解決するためになされたものであり、移動体の運動状態により観測誤差の大きさが変化する状況にあっても、目標を高精度に追尾する目標追尾装置を提供することを目標とする。
この発明に係る目標追尾装置は、移動体に搭載され、目標を追尾する目標追尾装置であって、目標の方向、距離を観測した観測量を観測更新計算部に出力する第1のセンサ部と、前記移動体の運動状態を計測し、運動状態量を観測ノイズ行列計算部に出力する第2のセンサ部と、前記運動状態量を入力し、前記移動体の運動状態を表す観測ノイズ行列を観測更新計算部に出力する観測ノイズ行列計算部と、観測更新計算部が出力する状態量(+)と誤差共分散行列(+)を入力し、所定のモデルに基づき計算した時間外挿計算後の状態量(−)と誤差共分散行列(−)を観測更新計算装置に出力する時間外挿計算部と、前記観測ノイズ行列計算部が出力する観測ノイズ行列と、前記第1のセンサ部が出力する観測量と、前記時間外挿計算部が出力する時間外挿計算後の状態量(−)と誤差共分散行列(−)を入力し、前記目標の観測量の観測更新計算を行い、観測更新計算後の状態量(+)と誤差共分散行列(+)を出力する観測更新計算部とを備える。
この発明に係る目標追尾装置によれば、カルマンフィルタのフィルタ特性を状態に応じて適切に変化させることが可能であり、目標追尾性能の向上を図ることができる。
実施の形態1.
図1は、本実施の形態に係る目標追尾装置100の構成を示す図である。
目標追尾装置100は、車両や飛しょう体等の移動体に搭載され、目標の方向・距離等を観測するためのシーカ1と、自身の速度・加速度等の状態量を計測するためのセンサ2と、観測更新計算装置3と、時間外挿計算装置4と、観測ノイズ行列計算装置5から構成される。
以下、図1を参照しながら、実施の形態1における目標追尾装置100の動作を説明する。
図1は、本実施の形態に係る目標追尾装置100の構成を示す図である。
目標追尾装置100は、車両や飛しょう体等の移動体に搭載され、目標の方向・距離等を観測するためのシーカ1と、自身の速度・加速度等の状態量を計測するためのセンサ2と、観測更新計算装置3と、時間外挿計算装置4と、観測ノイズ行列計算装置5から構成される。
以下、図1を参照しながら、実施の形態1における目標追尾装置100の動作を説明する。
シーカ1は、目標に向けて電波を送信しその反射波を受信し、あるいは目標が放射する赤外線を受信すること等により、目標の動きを観測する。
シーカ1は、観測された目標の観測量6を観測更新計算装置3へ出力する。
シーカ1は、観測された目標の観測量6を観測更新計算装置3へ出力する。
センサ2は、自身の目標追尾装置100が搭載される移動体の運動状態量10を計測する。センサ2は、計測により得られた運動状態量10を観測ノイズ行列計算装置5に出力する。
観測更新計算装置3は、シーカ1から得られる観測量6と、時間外挿装置から得られる「状態量(−)・誤差共分散行列(−)」9を用い、観測ノイズ行列計算装置5から得られる観測ノイズ行列7の値に基づいて観測更新計算を行う。
観測更新計算の処理を下記の式(1)に示す。
式(1)において、シーカ1から観測量6が出力される時刻と、観測ノイズ行列計算装置5から観測ノイズ行列7が出力される時刻はそれぞれ異なる。そのため、観測更新計算は新たな観測量6が得られた時点で行うものとし、その際に使用する観測ノイズ行列7は、その時点で得られている最新の値を使用するものとする。
式(1)において、シーカ1から観測量6が出力される時刻と、観測ノイズ行列計算装置5から観測ノイズ行列7が出力される時刻はそれぞれ異なる。そのため、観測更新計算は新たな観測量6が得られた時点で行うものとし、その際に使用する観測ノイズ行列7は、その時点で得られている最新の値を使用するものとする。
ここで、
出力方程式:h
観測量:z
観測ノイズ行列:R
状態量(観測更新計算前):x−
状態量(観測更新計算後):x+
誤差共分散行列(観測更新計算前):P−
誤差共分散行列(観測更新計算後):P+
とする。
出力方程式:h
観測量:z
観測ノイズ行列:R
状態量(観測更新計算前):x−
状態量(観測更新計算後):x+
誤差共分散行列(観測更新計算前):P−
誤差共分散行列(観測更新計算後):P+
とする。
出力方程式hは、シーカの特性をあらかじめ適当に同定した数式モデルから構成される。誤差共分散行列Pの初期値は、事前の想定に基づいて適当に設定されるものとする。
以上の計算で得られた観測更新計算後の状態量(X+)と観測更新計算後の誤差共分散行列(P+)を「状態量(+)・誤差共分散行列(+)」8として時間外挿計算装置4へ出力する。
時間外挿計算装置4は、観測更新計算装置3からの入力「状態量(+)・誤差共分散行列(+)」8を用いて時間外挿計算を行う。次の式(2)に時間外挿計算の処理を示す。
ここで、
運動方程式:f
プロセスノイズ:w
プロセスノイズ行列:Q
現在時間:tk
とする。
運動方程式:f
プロセスノイズ:w
プロセスノイズ行列:Q
現在時間:tk
とする。
運動方程式fは、対象をあらかじめ適当に同定した数式モデルから構成される。プロセスノイズ行列Qの値はあらかじめ想定されるモデル化誤差から適当に設定される。
時間外挿計算装置4は、時間外挿計算の結果として得られる時間外挿計算後の状態量と時間外挿計算後の誤差共分散行列を、観測更新計算前の「状態量(−)・誤差共分散行列(−)」9として観測更新計算装置3へ出力する。
ここで、観測ノイズ行列計算装置5は、センサ2から得られた自身の目標追尾装置100が搭載される移動体の運動状態量10を用いて、観測ノイズ行列7の値を計算する。
観測ノイズ行列7の値は次の式(3)に示す処理で計算される。
観測ノイズ行列7の値は次の式(3)に示す処理で計算される。
ここで
観測ノイズ行列:R
観測ノイズ行列初期値:R0
運動状態量:wk
ゲイン行列:G
現在時間:tk
サンプリング数:M
とする。
観測ノイズ行列:R
観測ノイズ行列初期値:R0
運動状態量:wk
ゲイン行列:G
現在時間:tk
サンプリング数:M
とする。
Vは与えられたデータ列から分散を算出する関数、mは与えられたデータ列から平均を算出する関数である。各時刻において、センサ2から得られた運動状態量wkの過去Mサンプリング分の分散を算出する。これにゲイン行列Gを乗じた上で、観測ノイズ行列初期値R0に乗じて観測ノイズ行列Rを計算する。これにより、移動体の運動状態を反映した観測ノイズ行列Rを算出することができる。
なお、観測ノイズ行列初期値R0は、シーカ1のS/N比(singal-noise ratio)等をもとに設定される初期値である。
この計算はセンサ2から新たな値が送信される毎に行われ、式(3)により計算された観測ノイズ行列Rは、観測ノイズ行列7として観測更新計算装置3に送信される。
この計算はセンサ2から新たな値が送信される毎に行われ、式(3)により計算された観測ノイズ行列Rは、観測ノイズ行列7として観測更新計算装置3に送信される。
以上の数式1〜数式3の処理を繰り返し計算し、「状態量(−)・誤差共分散行列(−)」9を毎サンプリング計算する。サンプリング毎に、「状態量(−)・誤差共分散行列(−)」9を目標追尾装置の最終的な出力とする。
このように、本実施の形態に係る目標追尾装置によれば、移動体の運動状態により観測誤差の大きさが変化する状況にあっても、フィルタ特性をその場に応じて適応的に変化させることができ、ロバストな推定が可能になる。その結果、目標の位置等の状態量を高精度に推定することができ、ひいてはその推定された状態量を用いて、目標を高精度に追尾することができる。
例えば、自身の目標追尾装置100が搭載される移動体が車両であり、目標を追尾している場合に、目標の追尾精度は移動体が走行している路面の状態に大きく影響される。
従来は路面の状態に関わらず観測ノイズ行列が固定されていたが、本発明によれば固定されていた観測ノイズ行列の値を観測ノイズの大きさと相関のある自らの移動体の運動状態を示す値によって変化させることが可能であるため、フィルタ特性をその場に応じて適応的に変化させることができ、目標の追尾精度を格段に向上させることが可能となる。例えば舗装路面を走行する際は観測ノイズが小さく目標の方向・距離等を観測する観測量6に対する応答性能が高い特性となる。一方、不整地を走行する際には観測ノイズが大きく観測量6に対する外乱抑制性能が高い特性となる。
従来は路面の状態に関わらず観測ノイズ行列が固定されていたが、本発明によれば固定されていた観測ノイズ行列の値を観測ノイズの大きさと相関のある自らの移動体の運動状態を示す値によって変化させることが可能であるため、フィルタ特性をその場に応じて適応的に変化させることができ、目標の追尾精度を格段に向上させることが可能となる。例えば舗装路面を走行する際は観測ノイズが小さく目標の方向・距離等を観測する観測量6に対する応答性能が高い特性となる。一方、不整地を走行する際には観測ノイズが大きく観測量6に対する外乱抑制性能が高い特性となる。
1 シーカ、2 センサ、3 観測更新計算装置、4 時間外挿計算装置、5 観測ノイズ行列計算装置、6 観測量、7 観測ノイズ行列、8 状態量(+)・誤差共分散行列(+)、9 状態量(−)・誤差共分散行列(−)、10 運動状態量、100 目標追尾装置。
Claims (4)
- 移動体に搭載され、目標を追尾する目標追尾装置であって、
目標の方向、距離を観測した観測量を観測更新計算部に出力する第1のセンサ部と、
前記移動体の運動状態を計測し、運動状態量を観測ノイズ行列計算部に出力する第2のセンサ部と、
前記運動状態量を入力し、前記移動体の運動状態を表す観測ノイズ行列を観測更新計算部に出力する観測ノイズ行列計算部と、
観測更新計算部が出力する状態量(+)と誤差共分散行列(+)を入力し、所定のモデルに基づき計算した時間外挿計算後の状態量(−)と誤差共分散行列(−)を観測更新計算装置に出力する時間外挿計算部と、
前記観測ノイズ行列計算部が出力する観測ノイズ行列と、前記第1のセンサ部が出力する観測量と、前記時間外挿計算部が出力する時間外挿計算後の状態量(−)と誤差共分散行列(−)を入力し、前記目標の観測量の観測更新計算を行い、観測更新計算後の状態量(+)と誤差共分散行列(+)を出力する観測更新計算部と、
を備えることを特徴とする目標追尾装置。 - 前記観測ノイズ行列初期値R0は、前記第1のセンサ部のS/N比を用いて設定される初期値であることを特徴とする請求項2記載の目標追尾装置。
- 前記観測更新計算部は、前記第1のセンサ部から前記観測量を入力する時刻と、前記観測ノイズ行列計算部から前記観測ノイズ行列を入力する時刻が異なる場合に、前記第1のセンサ部から観測量を入力したタイミングにおいて、当該観測量と、前記タイミングにおける最新の観測ノイズ行列を用いて観測更新計算を行うことを特徴とする請求項1記載の目標追尾装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2014076606A JP2015197403A (ja) | 2014-04-03 | 2014-04-03 | 目標追尾装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2014076606A JP2015197403A (ja) | 2014-04-03 | 2014-04-03 | 目標追尾装置 |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2015197403A true JP2015197403A (ja) | 2015-11-09 |
Family
ID=54547183
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2014076606A Pending JP2015197403A (ja) | 2014-04-03 | 2014-04-03 | 目標追尾装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2015197403A (ja) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2016161269A (ja) * | 2015-03-05 | 2016-09-05 | 三菱電機株式会社 | 目標状態量推定装置 |
CN108535720A (zh) * | 2017-03-02 | 2018-09-14 | 通用汽车环球科技运作有限责任公司 | 用于改进的卡尔曼滤波目标跟踪的自适应过程噪声描述 |
JP2019056642A (ja) * | 2017-09-21 | 2019-04-11 | 三菱電機株式会社 | 追尾処理装置及び追尾処理方法 |
JP2019124637A (ja) * | 2018-01-18 | 2019-07-25 | 三菱電機株式会社 | 追尾処理装置及び追尾処理方法 |
JPWO2021009852A1 (ja) * | 2019-07-16 | 2021-01-21 |
-
2014
- 2014-04-03 JP JP2014076606A patent/JP2015197403A/ja active Pending
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2016161269A (ja) * | 2015-03-05 | 2016-09-05 | 三菱電機株式会社 | 目標状態量推定装置 |
CN108535720A (zh) * | 2017-03-02 | 2018-09-14 | 通用汽车环球科技运作有限责任公司 | 用于改进的卡尔曼滤波目标跟踪的自适应过程噪声描述 |
JP2019056642A (ja) * | 2017-09-21 | 2019-04-11 | 三菱電機株式会社 | 追尾処理装置及び追尾処理方法 |
JP2019124637A (ja) * | 2018-01-18 | 2019-07-25 | 三菱電機株式会社 | 追尾処理装置及び追尾処理方法 |
JPWO2021009852A1 (ja) * | 2019-07-16 | 2021-01-21 | ||
WO2021009852A1 (ja) * | 2019-07-16 | 2021-01-21 | 日本電気株式会社 | 推定装置、制御システム、推定方法及びプログラム |
JP7151900B2 (ja) | 2019-07-16 | 2022-10-12 | 日本電気株式会社 | 推定装置、制御システム、推定方法及びプログラム |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP2015197403A (ja) | 目標追尾装置 | |
CN104132662B (zh) | 基于零速修正的闭环卡尔曼滤波惯性定位方法 | |
CN105043348A (zh) | 基于卡尔曼滤波的加速度计陀螺仪水平角度测量方法 | |
JP6271080B2 (ja) | 荷重状態推定器を含む地ならし機 | |
RU2564380C1 (ru) | Способ коррекции бесплатформенной инерциальной навигационной системы | |
US8812225B2 (en) | Electronic navigation device for a human and related methods | |
RU2013127127A (ru) | Компенсация задержки | |
CN105547291B (zh) | 室内人员自主定位系统的自适应静止检测方法 | |
Klein et al. | Dead reckoning for trajectory estimation of underwater drifters under water currents | |
RU2564379C1 (ru) | Бесплатформенная инерциальная курсовертикаль | |
CN103954288B (zh) | 一种卫星姿态确定系统精度响应关系确定方法 | |
KR101833007B1 (ko) | 도플러 비컨을 이용한 수중 이동체의 위치 및 속도 추정 방법 및 시스템 | |
CN106092141B (zh) | 一种改善相对位置传感器性能的方法及装置 | |
RU2625603C2 (ru) | Способ минимаксной фильтрации параметров движения беспилотного летательного аппарата с коррекцией от спутниковой навигационной системы | |
RU2014144061A (ru) | Способ управления инерционным приводом антенны, обеспечивающий устойчивое сопровождение интенсивно маневрирующих и высокоскоростных летательных аппаратов с повышенной адаптацией к маневру носителя и система для его реализации | |
Senyurek et al. | A modified adaptive Kalman filter for fiber optic gyroscope | |
KR20130082327A (ko) | 가속도 센서가 구비된 오브젝트의 진동 제어 방법 및 장치 | |
JP6248967B2 (ja) | 目標状態量推定装置 | |
KR101900564B1 (ko) | 표적 정보 획득 장치 | |
RU2406098C1 (ru) | Способ определения наклонной дальности до движущейся цели по минимальному числу пеленгов | |
TWI422825B (zh) | 高精度速度估測方法及其裝置 | |
RU2581743C1 (ru) | Бесплатформенная аппаратура счисления координат | |
Kim et al. | Relative azimuth estimation algorithm using rotational displacement | |
Semenov et al. | Investigation of the Kalman filter in problems of increasing the accuracy of measurements using MEMS (9 axis) motion sensors | |
KR100763973B1 (ko) | 무인비행체의 실시간 상대거리 추정방법 |