JP7725942B2 - Target motion analysis device, target motion analysis method, and target motion analysis program - Google Patents
Target motion analysis device, target motion analysis method, and target motion analysis programInfo
- Publication number
- JP7725942B2 JP7725942B2 JP2021139007A JP2021139007A JP7725942B2 JP 7725942 B2 JP7725942 B2 JP 7725942B2 JP 2021139007 A JP2021139007 A JP 2021139007A JP 2021139007 A JP2021139007 A JP 2021139007A JP 7725942 B2 JP7725942 B2 JP 7725942B2
- Authority
- JP
- Japan
- Prior art keywords
- particles
- particle
- state
- state quantity
- 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.)
- Active
Links
Landscapes
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Description
本発明は、ソーナーおよびレーダーの分野において、観測値から目標の運動を解析する目標運動解析装置、目標運動解析方法および目標運動解析プログラムに関するものである。 The present invention relates to a target motion analysis device, a target motion analysis method, and a target motion analysis program that analyze target motion from observed values in the fields of sonar and radar.
従来、ソーナーおよびレーダー等の分野においては、観測値から目標の運動を予測することが行われている。目標の運動を予測するためには、目標の位置および速度を推定する必要があり、そのような目標の運動を解析する方法として、種々の方法が提案されている。例えば、特許文献１～４には、パーティクルフィルタを用いた各種の目標運動解析方法が開示されている。 Traditionally, in fields such as sonar and radar, target motion has been predicted from observed values. To predict target motion, it is necessary to estimate the target's position and velocity, and various methods have been proposed for analyzing such target motion. For example, Patent Documents 1 to 4 disclose various target motion analysis methods that use particle filters.
パーティクルフィルタを用いた目標運動解析は、目標の位置および速度を含む状態を、パーティクルと称する粒子で表現する方法である。この目標運動解析では、まず、様々な目標の状態を有する粒子が大量に生成される。それぞれの粒子は、推定したい目標の状態と同じように速度、位置の状態を有しており、観測値に見合う粒子を増やし、観測値に見合わない粒子を減らすように処理が行われる。粒子の密度が高い地点は、目標が存在する可能性が高いことを示すため、目標が存在する可能性の高い場所に粒子を集めることにより、目標の位置および速度を推定することができる。 Target motion analysis using a particle filter is a method of representing the state of a target, including its position and velocity, using particles called "particles." In this target motion analysis, a large number of particles with various target states are first generated. Each particle has a velocity and position state similar to the state of the target to be estimated, and processing is performed to increase the number of particles that match the observed values and reduce the number of particles that do not. Points with a high density of particles indicate a high probability that a target is present, so by gathering particles in places where the target is likely to be present, the target's position and velocity can be estimated.
しかしながら、パーティクルフィルタを用いた目標運動解析では、例えば、目標が遠方に存在する場合または観測誤差が大きい場合などによって目標の状態の推定誤差が大きい場合に、目標の周辺に存在する粒子の数が少なくなる過疎状態が発生することがある。また、観測方位のバイアス誤差によっても、過疎状態が発生することがある。 However, when analyzing target motion using a particle filter, a sparse state can occur in which the number of particles around the target is reduced if the target's state estimation error is large, for example, due to a target being located far away or a large observation error. A sparse state can also occur due to a bias error in the observation direction.
過疎状態となると、目標の観測方位の近傍に存在する粒子の数が少なくなってしまうため、目標の状態を正確に推定することが困難となる。また、一旦過疎状態となると、以降の処理において、目標の状態を正しく推定することができない。そのため、過疎状態を発生させることなく、目標の状態を正しく推定することができる目標運動解析装置、目標運動解析方法および目標運動解析プログラムが望まれている。 When a sparsely populated state occurs, the number of particles present in the vicinity of the target's observation direction decreases, making it difficult to accurately estimate the target's state. Furthermore, once a sparsely populated state occurs, the target's state cannot be accurately estimated in subsequent processing. Therefore, there is a demand for a target motion analysis device, target motion analysis method, and target motion analysis program that can accurately estimate the target's state without causing a sparsely populated state.
本発明に係る目標運動解析装置は、目標の方位を示す観測方位と、前記観測方位の周辺に配置される複数の粒子の状態量および前記粒子の重みに基づき、前記目標の状態を推定する目標運動解析装置であって、所定の時刻に入力された前記観測方位と、前記粒子の状態量および前記粒子の重みとに基づき、前記粒子の重みを更新する重み更新部と、前記粒子の状態量と、更新された前記粒子の重みとから、前記目標の第１の状態量を推定し、推定結果として出力する状態推定部と、前記粒子の状態量および前記粒子の重みに基づき、次の時刻における推定に用いる前記粒子の状態量を算出する運動更新部と、前記所定の時刻以前の複数の時刻における前記観測方位から、前記目標の第２の状態量を推定状態量として推定するパラメータ推定部と、前記パラメータ推定部で推定された前記推定状態量と、前記運動更新部で算出された前記粒子の状態量および前記粒子の重みとに基づき、前記運動更新部で算出された前記粒子の状態量を置き換える粒子位置更新部とを備え、前記粒子位置更新部は、複数の前記粒子のうち前記重みが最も小さい前記粒子の状態量を、前記パラメータ推定部で推定された前記推定状態量に置き換えるものである。 The target motion analysis device according to the present invention is a target motion analysis device that estimates a state of a target based on an observation direction indicating the direction of the target, and state quantities of a plurality of particles arranged around the observation direction and weights of the particles, and includes a weight update unit that updates the weights of the particles based on the observation direction, state quantities of the particles, and weights of the particles input at a predetermined time, a state estimation unit that estimates a first state quantity of the target from the state quantities of the particles and the updated weights of the particles and outputs the result of estimation, and a state estimation unit that estimates a first state quantity of the target based on the state quantities of the particles and the weights of the particles to be used for estimation at the next time based on the state quantities of the particles and the weights of the particles. a parameter estimation unit that estimates a second state quantity of the target as an estimated state quantity from the observation orientation at a plurality of times before the predetermined time ; and a particle position update unit that replaces the state quantity of the particle calculated by the motion update unit based on the estimated state quantity estimated by the parameter estimation unit, the state quantity of the particle calculated by the motion update unit , and the weight of the particle, and the particle position update unit replaces the state quantity of the particle having the smallest weight among the plurality of particles with the estimated state quantity estimated by the parameter estimation unit.
また、本発明に係る目標運動解析方法は、目標の方位を示す観測方位と、前記観測方位の周辺に配置される複数の粒子の状態量および前記粒子の重みに基づき、前記目標の状態を推定する目標運動解析方法であって、所定の時刻に入力された前記観測方位と、前記粒子の状態量および前記粒子の重みとに基づき、前記粒子の重みを更新する重み更新ステップと、前記粒子の状態量と、更新された前記粒子の重みとから、前記目標の第１の状態量を推定し、推定結果として出力する状態推定ステップと、前記粒子の状態量および前記粒子の重みに基づき、次の時刻における推定に用いる前記粒子の状態量を算出する運動更新ステップと、前記所定の時刻以前の複数の時刻における前記観測方位から、前記目標の第２の状態量を推定状態量として推定するパラメータ推定ステップと、前記パラメータ推定ステップで推定された前記推定状態量と、前記運動更新ステップで算出された前記粒子の状態量および前記粒子の重みとに基づき、前記運動更新ステップで算出された前記粒子の状態量を置き換える粒子位置更新ステップとを備え、前記粒子位置更新ステップにおいて、複数の前記粒子のうち前記重みが最も小さい前記粒子の状態量を、前記パラメータ推定ステップで推定された前記推定状態量に置き換えるものである。 Furthermore, a target motion analysis method according to the present invention is a target motion analysis method for estimating a state of a target based on an observation direction indicating the direction of the target, state quantities of a plurality of particles arranged around the observation direction, and weights of the particles, and includes a weight updating step for updating the weights of the particles based on the observation direction input at a predetermined time, the state quantities of the particles, and the weights of the particles; a state estimation step for estimating a first state quantity of the target from the state quantities of the particles and the updated weights of the particles and outputting the result of estimation; and a step for calculating the state quantities of the particles to be used for estimation at the next time based on the state quantities of the particles and the weights of the particles. a parameter estimation step of estimating a second state quantity of the target as an estimated state quantity from the observation orientation at a plurality of times before the predetermined time ; and a particle position updating step of replacing the state quantity of the particle calculated in the movement updating step based on the estimated state quantity estimated in the parameter estimation step, the state quantity of the particle calculated in the movement updating step, and the weight of the particle, wherein in the particle position updating step, the state quantity of the particle having the smallest weight among the plurality of particles is replaced with the estimated state quantity estimated in the parameter estimation step.
本発明に係る目標運動解析プログラムは、目標の方位を示す観測方位と、前記観測方位の周辺に配置される複数の粒子の状態量および前記粒子の重みに基づき、前記目標の状態を推定する目標運動解析プログラムであって、所定の時刻に入力された前記観測方位と、前記粒子の状態量および前記粒子の重みとに基づき、前記粒子の重みを更新する重み更新ステップと、前記粒子の状態量と、更新された前記粒子の重みとから、前記目標の第１の状態量を推定し、推定結果として出力する状態推定ステップと、前記粒子の状態量および前記粒子の重みに基づき、次の時刻における推定に用いる前記粒子の状態量を算出する運動更新ステップと、前記所定の時刻以前の複数の時刻における前記観測方位から、前記目標の第２の状態量を推定状態量として推定するパラメータ推定ステップと、前記パラメータ推定ステップで推定された前記推定状態量と、前記運動更新ステップで算出された前記粒子の状態量および前記粒子の重みとに基づき、前記運動更新ステップで算出された前記粒子の状態量を置き換える粒子位置更新ステップとを目標運動解析装置のプロセッサに実行させ、前記粒子位置更新ステップにおいて、複数の前記粒子のうち前記重みが最も小さい前記粒子の状態量を、前記パラメータ推定ステップで推定された前記推定状態量に置き換えるものである。 A target motion analysis program according to the present invention is a target motion analysis program that estimates a state of a target based on an observation direction indicating the direction of the target, state quantities of a plurality of particles arranged around the observation direction, and weights of the particles, and includes a weight update step that updates the weights of the particles based on the observation direction input at a predetermined time, the state quantities of the particles, and the weights of the particles; a state estimation step that estimates a first state quantity of the target from the state quantities of the particles and the updated weights of the particles and outputs the result of estimation; and a motion update step that calculates the state quantities of the particles to be used for estimation at the next time based on the state quantities of the particles and the weights of the particles. a parameter estimation step of estimating a second state quantity of the target as an estimated state quantity from the observation azimuths at a plurality of times before the predetermined time , and a particle position update step of replacing the state quantity of the particle calculated in the movement update step based on the estimated state quantity estimated in the parameter estimation step and the state quantity of the particle and the weight of the particle calculated in the movement update step, and in the particle position update step, the state quantity of the particle having the smallest weight among the plurality of particles is replaced with the estimated state quantity estimated in the parameter estimation step.
以上のように、本発明によれば、パラメータ推定部で目標の状態量が推定され、粒子位置更新部で、複数の粒子のうち重みが最も小さい粒子の状態量が推定状態量に置き換えられる。これにより、観測方位の近傍に配置された粒子の重みが大きい状態に保たれるため、過疎状態を発生させることなく、目標の状態を正しく推定することができる。 As described above, according to the present invention, the parameter estimation unit estimates the state quantity of the target, and the particle position update unit replaces the state quantity of the particle with the smallest weight among multiple particles with the estimated state quantity. This keeps the weight of particles located near the observation direction high, allowing the state of the target to be accurately estimated without causing a sparse state.
以下、本発明の実施の形態について、図面を参照して説明する。本発明は、以下の実施の形態に限定されるものではなく、本発明の主旨を逸脱しない範囲で種々に変形することが可能である。また、本発明は、以下の各実施の形態に示す構成のうち、組合せ可能な構成のあらゆる組合せを含むものである。また、各図において、同一の符号を付したものは、同一のまたはこれに相当するものであり、これは明細書の全文において共通している。 Embodiments of the present invention will be described below with reference to the drawings. The present invention is not limited to the following embodiments, and various modifications are possible without departing from the spirit of the present invention. Furthermore, the present invention includes all possible combinations of the configurations shown in the following embodiments. In addition, parts with the same reference numerals in each drawing represent the same or equivalent parts, and this applies throughout the entire specification.
実施の形態１．
本実施の形態１に係る目標運動解析装置について説明する。本実施の形態１に係る目標運動解析装置は、ソーナーまたはレーダー等で観測される目標の方位を示す情報を含む観測値から目標の状態を予測するために、目標の運動を解析するものである。
Embodiment 1.
A description will be given of a target motion analysis device according to the present embodiment 1. The target motion analysis device according to the present embodiment 1 analyzes the motion of a target in order to predict the state of the target from observation values including information indicating the direction of the target observed by sonar, radar, or the like.
［従来の目標運動解析装置について］
まず、本実施の形態１に係る目標運動解析装置について説明する前に、従来の目標運動解析装置について説明する。背景技術の項で説明したように、パーティクルフィルタを用いた目標運動解析装置では、様々な目標の状態を有する粒子が大量に生成され、観測値に見合う粒子を増やし、観測値に見合わない粒子を減らすように、目標運動解析処理が行われる。
[Regarding conventional target motion analysis devices]
First, a conventional target motion analysis device will be described before describing the target motion analysis device according to the present embodiment 1. As described in the background art section, in a target motion analysis device using a particle filter, a large number of particles having various target states are generated, and target motion analysis processing is performed so as to increase particles that match the observed values and reduce particles that do not match the observed values.
（修正極座標）
それぞれの粒子の状態量Ｘｋ
（ｉ）は、デカルト座標で表す場合、式（１）のように定義される。式（１）において、「ｘ」は粒子の位置におけるｘ軸座標を示し、「ｙ」は粒子の位置におけるｙ軸座標を示す。「ｖｘ」はｘ軸方向の速度を示し、「ｖｙ」はｙ軸方向の速度を示す。また、「ｋ」は時刻を示すインデックスであり、「ｉ」は粒子番号を示すインデックスである。さらに、「Ｎ」は粒子の数を示す。
(modified polar coordinates)
The state quantity X k (i) of each particle is defined as shown in equation (1) when expressed in Cartesian coordinates. In equation (1), "x" indicates the x-axis coordinate of the particle's position, and "y" indicates the y-axis coordinate of the particle's position. "v x " indicates the velocity in the x-axis direction, and "v y " indicates the velocity in the y-axis direction. Furthermore, "k" is an index indicating time, and "i" is an index indicating the particle number. Furthermore, "N" indicates the number of particles.
また、各粒子は、当該粒子の信頼度を示す重みをそれぞれ有している。重みが大きい場合、その粒子が示す状態量が、推定したい目標の状態量である可能性が高いことを示す。 Each particle also has a weight that indicates the reliability of that particle. A large weight indicates that the state quantity indicated by that particle is likely to be the target state quantity to be estimated.
ところで、観測値が方位のみを示す場合、目標の位置および速度は、直接観測されたものではない。特に、観測者が変針する前の段階では、距離についての情報がなく、この場合には、目標の位置および速度を含むすべての状態量が不定となる。 However, when the observation value indicates only the direction, the target's position and velocity are not directly observed. In particular, before the observer changes course, there is no information about the distance, and in this case, all state quantities, including the target's position and velocity, become indeterminate.
そこで、このような場合には、方位のみの観測値から推定できる変数と、推定できない変数とを分離することができる座標系である修正極座標が用いられる。目標の状態を有する粒子の修正極座標上における状態量Ｙｋ （ｉ）は、式（２）で表される。式（２）において、「β」は方位を示し、「ｒ」は距離を示す。「文字上にドットが付されたβ」は方位変化を示し、「文字上にドットが付されたｒ／ｒ」は距離変化率を示す。 Therefore, in such cases, modified polar coordinates are used, which are a coordinate system that can separate variables that can be estimated from the observed value of only the orientation from variables that cannot be estimated. The state quantity Yk (i) on the modified polar coordinates of a particle having a target state is expressed by equation (2). In equation (2), "β" represents orientation and "r" represents distance. "β with a dot on it" represents the orientation change, and "r/r with a dot on it" represents the distance change rate.
式（２）で示される修正極座標を用いると、観測者が変針する前であっても、「Ｙ１，ｋ （ｉ）」～「Ｙ３，ｋ （ｉ）」は、観測方位から求めることができる。また、「Ｙ４，ｋ （ｉ）」は、観測者が変針することにより、求めることができる。 By using the modified polar coordinates shown in equation (2), "Y 1,k (i) " to "Y 3,k (i) " can be found from the observation direction even before the observer changes course. Also, "Y 4,k (i) " can be found when the observer changes course.
ここで、デカルト座標と修正極座標との関係は、式（３）および式（４）で表される。式（３）において、「ｆｃ ｍｐ」は、デカルト座標から修正極座標に変換するための変換関数を示す。また、式（４）において、「ｆｍｐ ｃ」は、修正極座標からデカルト座標に変換するための変換関数を示す。 Here, the relationship between Cartesian coordinates and modified polar coordinates is expressed by equations (3) and (4). In equation (3), "f c mp " represents a conversion function for converting from Cartesian coordinates to modified polar coordinates. Also, in equation (4), "f mp c " represents a conversion function for converting from modified polar coordinates to Cartesian coordinates.
（従来の目標運動解析装置１００）
図１は、従来の目標運動解析装置の構成の一例を示すブロック図である。従来の目標運動解析装置１００は、入力端子２０に入力された目標の観測方位に基づき目標の状態を推定する目標運動解析処理を行い、推定結果を出力端子３０から出力する。図１に示すように、目標運動解析装置１００は、粒子初期化部１１、重み更新部１２、状態推定部１３、縮退判定部１４、リサンプリング部１５および運動更新部１６を備えている。
(Conventional target motion analysis device 100)
Fig. 1 is a block diagram showing an example of the configuration of a conventional target motion analysis device. The conventional target motion analysis device 100 performs target motion analysis processing to estimate the state of a target based on the target observation direction input to an input terminal 20, and outputs the estimation result from an output terminal 30. As shown in Fig. 1, the target motion analysis device 100 includes a particle initialization unit 11, a weight update unit 12, a state estimation unit 13, a degeneracy determination unit 14, a resampling unit 15, and a motion update unit 16.
粒子初期化部１１は、修正極座標上の所定の範囲に、各粒子を一様に配置する初期化処理を行い、初期化されたｉ番目の粒子の状態量である初期状態量と、当該粒子の初期状態量の重みである初期重みを出力する。粒子初期化部１１による初期化処理は、目標運動解析処理を開始した際の１回だけ行われる。 The particle initialization unit 11 performs initialization processing to uniformly arrange each particle within a predetermined range on the modified polar coordinate system, and outputs the initial state quantity, which is the state quantity of the initialized i-th particle, and the initial weight, which is the weight of the initial state quantity of that particle. The initialization processing by the particle initialization unit 11 is performed only once, when the target motion analysis processing is started.
重み更新部１２は、所定の時刻に入力端子２０に入力された目標の方位を示す観測方位、運動更新部１６または粒子初期化部１１で生成された粒子の状態量および重みに基づき尤度を算出する。これにより、重み更新部１２は、粒子の重みを更新する重み更新処理を行う。また、重み更新部１２は、更新された粒子の重みを正規化する。 The weight update unit 12 calculates likelihood based on the observation direction indicating the target direction input to the input terminal 20 at a predetermined time, and the state quantities and weights of the particles generated by the motion update unit 16 or particle initialization unit 11. Based on this, the weight update unit 12 performs a weight update process to update the particle weights. The weight update unit 12 also normalizes the updated particle weights.
状態推定部１３は、重み更新部１２から出力された粒子の状態量と、正規化された重みとに基づき、修正極座標上における目標の状態量を推定する状態推定処理を行う。また、状態推定部１３は、修正極座標上の推定結果である推定状態量を、デカルト座標上の推定結果である推定状態量に変換する。 The state estimation unit 13 performs a state estimation process to estimate the state quantities of the target on modified polar coordinates based on the particle state quantities output from the weight update unit 12 and the normalized weights. The state estimation unit 13 also converts the estimated state quantities, which are the estimation results on modified polar coordinates, into estimated state quantities, which are the estimation results on Cartesian coordinates.
縮退判定部１４は、重み更新部１２から出力された粒子の重みに基づき、縮退を判定する。縮退判定部１４は、縮退を判定するための判定値を算出し、算出した判定値と予め設定された閾値とを比較し、粒子の縮退判定処理を行う。ここで、「縮退」とは、少数の粒子に大きな重みが集中し、他の大多数の粒子の重みが非常に小さくなることを示す。 The degeneracy determination unit 14 determines degeneracy based on the particle weights output from the weight update unit 12. The degeneracy determination unit 14 calculates a determination value for determining degeneracy, compares the calculated determination value with a preset threshold, and performs particle degeneracy determination processing. Here, "degeneracy" means that large weights are concentrated on a small number of particles, and the weights of the majority of other particles become very small.
リサンプリング部１５は、縮退判定部１４により縮退が発生していると判断された場合に、縮退判定部１４から出力された粒子の重みに基づき、当該粒子の状態量および粒子の重みをリサンプルするリサンプリング処理を行う。リサンプリング処理の詳細は、後述する。 When the degeneracy determination unit 14 determines that degeneracy has occurred, the resampling unit 15 performs a resampling process to resample the state quantities and particle weights of the particle based on the particle weights output from the degeneracy determination unit 14. Details of the resampling process will be described later.
運動更新部１６は、縮退判定部１４またはリサンプリング部１５から出力された粒子の状態量および正規化された粒子の重みに基づき、次の時刻における粒子の状態量を算出する運動更新処理を行う。そして、運動更新部１６は、算出した次の時刻における粒子の状態量を、重み更新部１２に対して出力する。 The motion update unit 16 performs a motion update process to calculate the particle state quantities at the next time based on the particle state quantities and normalized particle weights output from the degeneracy determination unit 14 or the resampling unit 15. The motion update unit 16 then outputs the calculated particle state quantities at the next time to the weight update unit 12.
（従来の目標運動解析処理）
上記の構成を有する従来の目標運動解析装置１００による目標運動解析処理について説明する。目標運動解析処理が開始されると、まず、粒子初期化部１１は、非特許文献２に記載されているように、修正極座標上において、各粒子を所定の範囲で一様に配置する。ここで、初期化された時点では、粒子に対する知見がないため、粒子初期化部１１は、各粒子の重みを一様とする。
(Conventional target motion analysis processing)
The target motion analysis process performed by the conventional target motion analysis device 100 having the above configuration will now be described. When the target motion analysis process is started, the particle initialization unit 11 first uniformly arranges each particle within a predetermined range on the modified polar coordinate system, as described in Non-Patent Document 2. Here, at the time of initialization, there is no knowledge about the particles, so the particle initialization unit 11 uniformly weights each particle.
初期化したｉ番目の粒子の状態量である初期状態量Ｙ１ （ｉ）、ならびに、ｉ番目の粒子の初期状態量Ｙ１ （ｉ）の重みである初期重みｗ１ （ｉ）は、重み更新部１２に対して出力される。なお、粒子初期化部１１による処理は、処理開始時に１回だけ実行される。 The initial state quantity Y 1 (i) , which is the initialized state quantity of the i-th particle, and the initial weight w 1 (i), which is the weight of the initial state quantity Y 1 (i) of the i-th particle, are output to the weight update unit 12. Note that the processing by the particle initialization unit 11 is executed only once at the start of processing.
重み更新部１２は、所定の時刻に入力端子２０に入力された観測方位、運動更新部１６または粒子初期化部１１で生成された粒子の状態量および重みを用いて尤度を算出し、粒子の重みを更新する重み更新処理を行う。 The weight update unit 12 calculates likelihood using the observation direction input to the input terminal 20 at a predetermined time, and the state quantities and weights of the particles generated by the motion update unit 16 or particle initialization unit 11, and performs weight update processing to update the particle weights.
図２は、重み更新部による重み更新処理について説明するための概略図である。図２（ａ）は、観測方位に基づき算出された尤度を示す。算出された尤度のうち、最も大きい値となる方位が、目標が観測される方位を示す観測方位となる。図２（ｂ）は、運動更新部１６から出力された粒子の重みを示す。通常、運動更新部１６から出力される粒子の重みは、観測方位の近傍で大きい値となる。図２（ｃ）は、重み更新部１２で更新された粒子の重みを示す。重み更新部１２は、図２（ａ）に示す尤度と、図２（ｂ）に示す粒子の重みとを乗算することにより、図２（ｃ）に示すように、粒子の重みを更新する重み更新処理を行う。 Figure 2 is a schematic diagram illustrating the weight update process performed by the weight update unit. Figure 2(a) shows the likelihood calculated based on the observation direction. The direction with the largest calculated likelihood value is the observation direction indicating the direction in which the target is observed. Figure 2(b) shows the particle weights output from the movement update unit 16. Typically, the particle weights output from the movement update unit 16 are large near the observation direction. Figure 2(c) shows the particle weights updated by the weight update unit 12. The weight update unit 12 performs weight update process to update the particle weights as shown in Figure 2(c) by multiplying the likelihood shown in Figure 2(a) by the particle weights shown in Figure 2(b).
具体的には、更新された粒子の重みｗｋ （ｉ）は、式（５）に基づき算出される。式（５）において、「ｚｋ」は観測方位を示し、「ｐ（ｚｋ｜Ｙ３，ｋ （ｉ））」は尤度関数を示す。また、「ｗｋ－１ （ｉ）」は時刻のインデックスが１つ前のときの粒子の重みを示す。 Specifically, the updated particle weight w k (i) is calculated based on equation (5). In equation (5), "z k " indicates the observation direction, "p(z k |Y 3,k (i) )" indicates the likelihood function, and "w k-1 (i) " indicates the particle weight when the time index is one point earlier.
各粒子に対する重み更新処理が行われると、重み更新部１２は、更新した各粒子の重みを正規化する。各粒子の正規化された重みｗ~ｋ （ｉ）は、式（６）に基づいて行われる。なお、以下の説明においては、数式中で「ｗ」などの文字の上に「~」がつけられた文字を「ｗ~」と記載するものとする。 After the weight update process for each particle is completed, the weight update unit 12 normalizes the updated weight of each particle. The normalized weight w~ k (i) of each particle is calculated based on equation (6). In the following description, a character with a "~" above it, such as "w", in an equation will be written as "w~".
各粒子の状態量Ｙｋ （ｉ）、および正規化した粒子の重みｗ~ｋ （ｉ）は、状態推定部１３および縮退判定部１４に対して出力される。このとき、粒子初期化部１１または運動更新部１６で生成された粒子の状態量は、保持したまま状態推定部１３および縮退判定部１４に対して出力される。 The state quantity Y k (i) of each particle and the normalized particle weight ŵ k (i) are output to the state estimation unit 13 and the degeneracy determination unit 14. At this time, the state quantities of the particles generated by the particle initialization unit 11 or the motion update unit 16 are output to the state estimation unit 13 and the degeneracy determination unit 14 while being retained.
状態推定部１３は、重み更新部１２から出力された粒子の状態量Ｙｋ （ｉ）と、正規化された重みｗ~ｋ （ｉ）とに基づき、修正極座標上における目標の状態量を推定する状態推定処理を行い、推定結果として推定状態量Ｙ＾ｋを出力する。推定結果である修正極座標上における目標の推定状態量Ｙ＾ｋは、非特許文献３に記載されているように、式（７）に基づき重み付け加算される。 The state estimation unit 13 performs a state estimation process to estimate the state quantity of the target on the modified polar coordinates based on the state quantity Yk (i) of the particle output from the weight update unit 12 and the normalized weights w~ k (i) , and outputs the estimated state quantity Y^ k as the estimation result. The estimated state quantity Y^ k of the target on the modified polar coordinates, which is the estimation result, is weighted and added based on equation (7), as described in Non-Patent Document 3.
そして、状態推定部１３は、式（４）に基づき、推定結果である修正極座標上における目標の推定状態量Ｙ＾ｋを、デカルト座標上における目標の推定状態量Ｘ＾ｋに変換する。状態推定部１３は、デカルト座標上における目標の推定状態量Ｘ＾ｋを推定結果として出力する。 Then, the state estimation unit 13 converts the estimated state quantity Y^ k of the target on the modified polar coordinates, which is the estimation result, into an estimated state quantity X^ k of the target on the Cartesian coordinates based on equation (4). The state estimation unit 13 outputs the estimated state quantity X^ k of the target on the Cartesian coordinates as the estimation result.
縮退判定部１４は、重み更新部１２から出力された粒子の重みｗ~ｋ （ｉ）に基づき判定値を算出する。また、縮退判定部１４は、非特許文献４に記載されているように、式（８）に基づき、算出した判定値と予め設定された閾値Ｎｔｈｒｅｓｈｏｌｄとを比較し、粒子に対する縮退判定処理を行う。 The degeneracy determination unit 14 calculates a determination value based on the particle weights w̃ k (i) output from the weight update unit 12. Furthermore, as described in Non-Patent Document 4, the degeneracy determination unit 14 compares the calculated determination value with a preset threshold N threshold based on Equation (8) to perform degeneracy determination processing for the particles.
判定値が閾値より小さい場合、すなわち、式（８）の関係が成立する場合、縮退判定部１４は、縮退が発生していると判断する。この場合、粒子の状態量および粒子の重みは、リサンプリング部１５に対して出力される。一方、判定値が閾値以上である場合、すなわち、式（８）の関係が成立しない場合、縮退判定部１４は、縮退が発生していないと判断する。この場合、粒子の状態量および粒子の重みは、運動更新部１６に対して出力される。 If the judgment value is smaller than the threshold, i.e., if the relationship in equation (8) holds, the degeneracy judgment unit 14 judges that degeneracy has occurred. In this case, the particle state quantities and particle weights are output to the resampling unit 15. On the other hand, if the judgment value is equal to or greater than the threshold, i.e., if the relationship in equation (8) does not hold, the degeneracy judgment unit 14 judges that degeneracy has not occurred. In this case, the particle state quantities and particle weights are output to the motion update unit 16.
縮退判定部１４により縮退が発生していると判断された場合、リサンプリング部１５は、縮退判定部１４から出力された粒子の状態量および粒子の重みをリサンプルするリサンプリング処理を行う。リサンプリング処理によって得られた粒子の状態量および粒子の重みは、運動更新部１６に対して出力される。 If the degeneracy determination unit 14 determines that degeneracy has occurred, the resampling unit 15 performs a resampling process to resample the particle state quantities and particle weights output from the degeneracy determination unit 14. The particle state quantities and particle weights obtained by the resampling process are output to the motion update unit 16.
図３は、リサンプリング部によるリサンプリング処理について説明するための概略図である。図３（ａ）は、縮退判定部１４から出力された粒子の重みを示す。図３（ａ）に示すように、粒子が観測方位の近傍に存在し、縮退が発生している。 Figure 3 is a schematic diagram illustrating the resampling process performed by the resampling unit. Figure 3(a) shows the particle weights output from the degeneracy determination unit 14. As shown in Figure 3(a), particles are present near the observation direction, causing degeneracy.
図３（ｂ）は、リサンプルされた粒子の位置を示す。図３（ｂ）に示すように、リサンプリング部１５は、縮退判定部１４から出力された粒子の重みに基づき、重みの小さい粒子（破線白丸）を削除する。また、リサンプリング部１５は、重みの大きい粒子を、当該粒子の重みの大きさに応じて分割し、粒子を複製する（実線白丸）。 Figure 3(b) shows the positions of resampled particles. As shown in Figure 3(b), the resampling unit 15 deletes particles with low weights (dashed white circles) based on the particle weights output from the degeneracy determination unit 14. The resampling unit 15 also divides particles with high weights according to the magnitude of the particle weights, and replicates the particles (solid white circles).
ここで、粒子を分割して複製する場合、リサンプリング部１５は、縮退判定部１４から出力された重みから累積分布関数を求め、求めた累積分布関数を粒子の数だけ等間隔にリサンプルすることにより、粒子を割り当てる。このとき、リサンプリング部１５は、割り当てた粒子の重みを均一にする。これにより、実質的に、重みの小さい粒子が削除され、重みの大きい粒子が分割されて複製されることになる。 When dividing and duplicating particles, the resampling unit 15 calculates a cumulative distribution function from the weights output from the degeneracy determination unit 14, and assigns particles by resampling the calculated cumulative distribution function at equal intervals for the number of particles. At this time, the resampling unit 15 equalizes the weights of the assigned particles. This essentially results in particles with small weights being deleted, and particles with large weights being divided and duplicated.
図３（ｃ）は、リサンプリングによって各粒子が観測方位の周りに集まり、粒子の重みが均一になっている様子を示す。このようにしてリサンプルされた粒子の状態量および粒子の重みは、運動更新部１６に対して出力される。 Figure 3(c) shows how resampling causes each particle to gather around the observation direction, making the particle weights uniform. The resampled particle state quantities and particle weights are output to the motion update unit 16.
運動更新部１６は、縮退判定部１４またはリサンプリング部１５から出力された粒子の状態量Ｙ１ （ｉ）および正規化された重みｗ~ｋ （ｉ）に基づき、目標の運動モデルに従って次の時刻における粒子の状態量を算出する。算出された粒子の状態量は、重み更新部１２に対して出力される。 The motion update unit 16 calculates the particle state quantity at the next time in accordance with the target motion model based on the particle state quantity Y 1 (i) and the normalized weights w̃ k (i) output from the degeneracy determination unit 14 or the resampling unit 15. The calculated particle state quantity is output to the weight update unit 12.
（従来の目標運動解析装置１００における問題点）
背景技術の項でも説明したように、上記のアルゴリズムを用いた目標運動解析装置１００は、目標の状態の推定誤差が大きい場合、あるいは、観測方位のバイアス誤差によって、目標の状態の周辺に粒子の数が少ない、もしくはなくなってしまう過疎状態が発生することがある。
(Problems with the conventional target motion analysis device 100)
As explained in the background art section, when the target motion analysis device 100 using the above algorithm has a large estimation error in the state of the target, or due to a bias error in the observation direction, a depopulated state may occur in which the number of particles around the state of the target is small or there is no particle at all.
図４は、過疎状態での重み更新部による重み更新処理について説明するための概略図である。図４（ａ）は、観測方位に基づき算出された尤度を示す。図４（ｂ）は、運動更新部１６から出力された粒子の重みを示す。図４（ｃ）は、重み更新部１２で更新された粒子の重みを示す。 Figure 4 is a schematic diagram illustrating the weight update process performed by the weight update unit in a depopulated state. Figure 4(a) shows the likelihood calculated based on the observation direction. Figure 4(b) shows the particle weights output from the movement update unit 16. Figure 4(c) shows the particle weights updated by the weight update unit 12.
この例では、観測方位の近傍に粒子が存在していないため、重み更新部１２から出力される粒子の重みは、図４（ｃ）に示すように、更新された粒子の重みも小さくなってしまう。その結果、従来の目標運動解析装置１００では、目標の状態を正しく推定することが困難となる。また、重みの大きい粒子が存在しないことから、リサンプリング部１５でリサンプリング処理が行われても、粒子が観測方位の周辺に集まらなくなってしまう。 In this example, because there are no particles near the observation direction, the particle weights output by the weight update unit 12 are small, even after the update, as shown in Figure 4(c). As a result, it becomes difficult for the conventional target motion analysis device 100 to accurately estimate the state of the target. Furthermore, because there are no particles with large weights, even when resampling processing is performed by the resampling unit 15, the particles do not gather around the observation direction.
図５は、過疎状態でのリサンプリング部によるリサンプリング処理について説明するための概略図である。図５（ａ）は、縮退判定部１４から出力された粒子の重みを示す。図５（ｂ）は、リサンプルされた粒子の位置を示す。図５（ｃ）は、リサンプリングによって各粒子が観測方位から離れた位置に偏り、粒子の重みが不均一になっている様子を示す。 Figure 5 is a schematic diagram illustrating the resampling process performed by the resampling unit in a sparsely populated area. Figure 5(a) shows the particle weights output from the degeneracy determination unit 14. Figure 5(b) shows the positions of the resampled particles. Figure 5(c) shows how resampling causes each particle to be biased away from the observation direction, resulting in uneven particle weights.
図５に示す例のように、観測方位の近傍に粒子が存在しない場合には、縮退判定部１４から出力された重みが小さくなる。そのため、リサンプリング部１５によるリサンプリング処理で重みの小さい粒子が削除され、重みの大きい粒子が複製されても、観測方位の周辺に粒子が集まらなくなってしまう。また、場合によっては、一旦過疎状態が発生すると、二度と目標の状態を正しく推定することができなくなってしまう。そこで、本実施の形態１では、粒子の過疎状態が発生して目標の状態推定の精度低下を抑制するための処理を行う。 As in the example shown in Figure 5, if there are no particles near the observation direction, the weight output from the degeneracy determination unit 14 will be small. Therefore, even if particles with small weights are deleted in the resampling process by the resampling unit 15 and particles with large weights are replicated, particles will not gather around the observation direction. Furthermore, in some cases, once a depopulation state occurs, it may never be possible to correctly estimate the state of the target again. Therefore, in this first embodiment, processing is performed to prevent a decrease in the accuracy of target state estimation due to the occurrence of a particle depopulation state.
［目標運動解析装置の構成］
図６は、本実施の形態１に係る目標運動解析装置の構成の一例を示すブロック図である。図６に示すように、目標運動解析装置１０は粒子初期化部１１、重み更新部１２、状態推定部１３、縮退判定部１４、リサンプリング部１５、運動更新部１６、パラメータ推定部１７および粒子位置更新部１８を備えている。このような目標運動解析装置１０は、ソフトウェアを実行することにより各種機能を実現するマイクロコンピュータ等の演算装置、もしくは各種機能に対応する回路デバイス等のハードウェアで構成されている。なお、本実施の形態１に係る目標運動解析装置１０において、粒子初期化部１１、状態推定部１３、縮退判定部１４およびリサンプリング部１５の構成については、従来の目標運動解析装置１００と共通している。そのため、ここでは、詳細な説明を省略する。
[Configuration of target motion analysis device]
6 is a block diagram showing an example of the configuration of a target motion analysis device according to the first embodiment. As shown in FIG. 6, the target motion analysis device 10 includes a particle initialization unit 11, a weight update unit 12, a state estimation unit 13, a degeneracy determination unit 14, a resampling unit 15, a motion update unit 16, a parameter estimation unit 17, and a particle position update unit 18. Such a target motion analysis device 10 is configured by a calculation device such as a microcomputer that realizes various functions by executing software, or hardware such as circuit devices corresponding to the various functions. Note that in the target motion analysis device 10 according to the first embodiment, the configurations of the particle initialization unit 11, the state estimation unit 13, the degeneracy determination unit 14, and the resampling unit 15 are common to those of the conventional target motion analysis device 100. Therefore, detailed description thereof will be omitted here.
重み更新部１２は、入力端子２０に入力された観測方位、粒子位置更新部１８または粒子初期化部１１で生成された粒子の状態量および重みに基づき尤度を算出する。これにより、重み更新部１２は、粒子の重みを更新する重み更新処理を行う。また、重み更新部１２は、更新された粒子の重みを正規化する。 The weight update unit 12 calculates likelihood based on the observation orientation input to the input terminal 20 and the particle state quantities and weights generated by the particle position update unit 18 or particle initialization unit 11. As a result, the weight update unit 12 performs a weight update process to update the particle weights. The weight update unit 12 also normalizes the updated particle weights.
運動更新部１６は、縮退判定部１４またはリサンプリング部１５から出力された粒子の状態量および正規化された粒子の重みに基づき、次の時刻における粒子の状態量を算出する運動更新処理を行う。そして、運動更新部１６は、算出した次の時刻における粒子の状態量を、粒子位置更新部１８に対して出力する。 The motion update unit 16 performs a motion update process to calculate the particle state quantity at the next time based on the particle state quantity and normalized particle weights output from the degeneracy determination unit 14 or the resampling unit 15. The motion update unit 16 then outputs the calculated particle state quantity at the next time to the particle position update unit 18.
パラメータ推定部１７は、入力端子２０から複数の時刻における観測方位が入力され、入力された複数の時刻における観測方位に基づき、目標の状態量を推定する。パラメータ推定部１７は、推定した目標の状態量を推定状態量として粒子位置更新部１８に対して出力する。このように、パラメータ推定部１７は、粒子を用いず、複数の時刻における観測方位のみに基づいて、目標の状態量を推定する。 The parameter estimation unit 17 receives observation orientations at multiple times from the input terminal 20 and estimates the state quantities of the target based on the input observation orientations at multiple times. The parameter estimation unit 17 outputs the estimated state quantities of the target as estimated state quantities to the particle position update unit 18. In this way, the parameter estimation unit 17 estimates the state quantities of the target based only on the observation orientations at multiple times, without using particles.
粒子位置更新部１８は、パラメータ推定部１７から出力された推定状態量と、運動更新部１６から出力された粒子の状態量および粒子の重みとに基づき、粒子の状態量を置き換える置換処理を行い、粒子の状態量および粒子の重みを更新する。粒子位置更新部１８は、更新した粒子の状態量および粒子の重みを重み更新部１２に対して出力する。 The particle position update unit 18 performs a replacement process to replace the particle state quantities based on the estimated state quantities output from the parameter estimation unit 17 and the particle state quantities and particle weights output from the motion update unit 16, and updates the particle state quantities and particle weights. The particle position update unit 18 outputs the updated particle state quantities and particle weights to the weight update unit 12.
（目標運動解析処理）
上記の構成を有する目標運動解析装置１０による目標運動解析処理について説明する。なお、この目標運動解析処理において、粒子初期化部１１、状態推定部１３、縮退判定部１４、リサンプリング部１５および運動更新部１６は、従来の目標運動解析装置１００による目標運動解析処理と同様の処理を行うため、ここでは詳細な説明を省略する。
(Target motion analysis processing)
The following describes the target motion analysis process performed by the target motion analysis device 10 having the above configuration. In this target motion analysis process, the particle initialization unit 11, state estimation unit 13, degeneracy determination unit 14, resampling unit 15, and motion update unit 16 perform the same processes as in the target motion analysis process performed by the conventional target motion analysis device 100, and therefore detailed descriptions thereof will be omitted here.
パラメータ推定部１７は、入力端子２０に入力された観測方位を複数保持し、保持した複数の観測方位から目標の状態量Ｙ´＾ｋを推定する。目標の状態量Ｙ´＾ｋの推定には、例えば非特許文献２に記載されている手法を用いることができる。その手法は、式（９）に示す非線形観測方程式を線形近似し、方程式を解く方式である。式（９）において、「Ｚｔ」は観測方位を示す。「ｔ」は得られた観測方位の時間を示すインデックスであり、「ｋ」は現在の時刻を示すインデックスである。「δｔ」は観測時間間隔を示し、「σｖ」は観測誤差を示す。また、「Ｎ（０，σｖ ２）は」平均が０であり、分散がσｖ ２である正規分布に従う乱数であり、ガウス雑音を示す。 The parameter estimation unit 17 holds a plurality of observation directions input to the input terminal 20, and estimates the state quantity Y'^ k of the target from the held plurality of observation directions. To estimate the state quantity Y'^ k of the target, for example, the method described in Non-Patent Document 2 can be used. This method linearly approximates the non-linear observation equation shown in equation (9) and solves the equation. In equation (9), "Z t " represents the observation direction. "t" is an index indicating the time of the obtained observation direction, and "k" is an index indicating the current time. "δ t " represents the observation time interval, and "σ v " represents the observation error. Furthermore, "N(0, σ v 2 )" is a random number following a normal distribution with a mean of 0 and a variance of σ v 2 , and represents Gaussian noise.
上記の式（９）において、「ｔａｎ－１（ｓｉｎθ／ｃｏｓθ）＝θ」とし、複数の保持された時間内において方位変化が線形であると仮定し、「ｃｏｓθ＝１」および「ｓｉｎθ＝θ」とおくことで、状態量Ｙ１，ｋ～Ｙ３，ｋは、式（１０）に示すように解くことができる。 In the above equation (9), by setting "tan -1 (sin θ/cos θ) = θ", assuming that the orientation change is linear within a plurality of held times, and setting "cos θ = 1" and "sin θ = θ", the state quantities Y 1,k to Y 3,k can be solved as shown in equation (10).
また、式（１０）における「Ｚ」および「Ｘ」は、式（１１）に基づき算出される。式（１０）において、「Ｙ´＾ｋ」は状態量の推定値である推定状態量を示し、「Ｚ」は観測方位を示し、「δｔ」は観測時間間隔を示す。このようにして推定された目標の推定状態量Ｙ´＾ｋは、粒子位置更新部１８に対して出力される。 Furthermore, "Z" and "X" in equation (10) are calculated based on equation (11). In equation (10), "Y'^ k " indicates an estimated state quantity which is an estimated value of the state quantity, "Z" indicates the observation direction, and " δt " indicates the observation time interval. The estimated state quantity Y'^ k of the target estimated in this way is output to the particle position update unit 18.
粒子位置更新部１８は、運動更新部１６から出力された粒子の状態量のうち、重みが小さい１または複数の粒子の状態量を、パラメータ推定部１７から出力された推定状態量Ｙ´＾ｋに置き換える置換処理を行う。粒子位置更新部１８は、推定状態量Ｙ´＾ｋに置き換えた粒子の状態量を含む状態量Ｙｋ （ｉ）と、粒子の状態の量の重みとを、重み更新部１２に対して出力する。 The particle position update unit 18 performs a replacement process to replace one or more particle state quantities with small weights among the particle state quantities output from the motion update unit 16 with the estimated state quantity Y'^ k output from the parameter estimation unit 17. The particle position update unit 18 outputs to the weight update unit 12 the state quantity Yk (i) including the particle state quantity replaced with the estimated state quantity Y'^ k and the weight of the particle state quantity.
図７は、粒子位置更新部による置換処理について説明するための概略図である。ここでは、重みが最も小さい１つの粒子の状態量を推定状態量に置き換える場合について説明する。図７（ａ）は、運動更新部１６から出力された粒子の重みを示す。この例では、観測方位の近傍に粒子が存在していない様子が示されている。図７（ｂ）は、粒子位置更新部１８から出力された重みを示す。図７（ｂ）では、運動更新部１６から出力された各粒子のうち、重みが最も小さい粒子における目標の状態量が、パラメータ推定部１７で算出された推定状態量Ｙ´＾ｋに置き換えられる様子が示されている。このとき、置き換えられた粒子の重みは、例えば、値が「１」にリセットされる。 FIG. 7 is a schematic diagram for explaining the replacement process by the particle position update unit. Here, a case will be described in which the state quantity of one particle with the smallest weight is replaced with an estimated state quantity. FIG. 7( a) shows the particle weights output from the motion update unit 16. In this example, a state in which no particles are present near the observation direction is shown. FIG. 7( b) shows the weights output from the particle position update unit 18. FIG. 7( b) shows a state in which the target state quantity of the particle with the smallest weight among the particles output from the motion update unit 16 is replaced with the estimated state quantity Y′^ k calculated by the parameter estimation unit 17. At this time, the weight of the replaced particle is reset to, for example, a value of “1”.
重み更新部１２は、入力端子２０に入力された観測方位、粒子位置更新部１８または粒子初期化部１１で生成された粒子の状態量および重みを用いて尤度を算出し、粒子の重みを更新する重み更新処理を行う。また、重み更新部１２は、更新した各粒子の重みを正規化し、正規化された各粒子の重みｗ~ｋ （ｉ）を出力する。 The weight update unit 12 calculates likelihoods using the observation orientation input to the input terminal 20 and the state quantities and weights of the particles generated by the particle position update unit 18 or the particle initialization unit 11, and performs weight update processing to update the weights of the particles. In addition, the weight update unit 12 normalizes the updated weights of each particle and outputs the normalized weights w~ k (i) of each particle.
以下、状態推定部１３、縮退判定部１４、リサンプリング部１５および運動更新部１６は、従来の目標運動解析装置１００による目標運動解析処理と同様に、各機種の処理を行う。これにより、状態推定部１３からデカルト座標上における目標の推定状態量Ｘ＾ｋが推定結果として出力される。 Thereafter, the state estimation unit 13, the degeneracy determination unit 14, the resampling unit 15, and the motion update unit 16 perform processing for each model in the same manner as the target motion analysis processing by the conventional target motion analysis device 100. As a result, the state estimation unit 13 outputs the estimated state quantity X^ k of the target on the Cartesian coordinates as the estimation result.
このように、従来の目標運動解析装置１００に対して、パラメータ推定部１７および粒子位置更新部１８が追加されることにより、重み更新部１２において粒子の重みが更新される際に、尤度の大きい方位に粒子が配置される。 In this way, by adding the parameter estimation unit 17 and particle position update unit 18 to the conventional target motion analysis device 100, when the particle weights are updated in the weight update unit 12, the particles are positioned in the direction with the highest likelihood.
図８は、本実施の形態１における、過疎状態での重み更新部による重み更新処理について説明するための概略図である。図８（ａ）は、観測方位に基づき算出された尤度を示す。図８（ｂ）は、粒子位置更新部１８から出力された粒子の重みを示す。図８（ｃ）は、重み更新部１２で更新された粒子の重みを示す。 Figure 8 is a schematic diagram illustrating the weight update process performed by the weight update unit in a depopulated state in this embodiment 1. Figure 8(a) shows the likelihood calculated based on the observation direction. Figure 8(b) shows the particle weights output from the particle position update unit 18. Figure 8(c) shows the particle weights updated by the weight update unit 12.
図４に示す従来の目標運動解析装置１００では、観測方位の近傍に粒子に数が少ない場合に、更新された粒子の重みも小さくなるという問題があった。これに対して、図８に示す本実施の形態１に係る目標運動解析装置１０では、重みの小さい粒子をパラメータ推定部１７で推定された粒子に置き換える。そのため、尤度が大きい位置に粒子を配置することができ、観測方位の近傍に配置された粒子の重みを大きい状態に保つことができる。その結果、リサンプリングによって、観測方位の周辺に粒子が再度集められる。これにより、過疎状態が発生することなく、目標の状態を正しく推定することができる。 The conventional target motion analysis device 100 shown in Figure 4 had a problem in that when there were few particles near the observation direction, the weights of the updated particles also became small. In contrast, the target motion analysis device 10 according to the first embodiment shown in Figure 8 replaces particles with small weights with particles estimated by the parameter estimation unit 17. This allows particles to be placed in positions with high likelihood, and the weights of particles placed near the observation direction can be maintained at a high level. As a result, particles are re-gathered around the observation direction by resampling. This prevents depopulation and allows the state of the target to be accurately estimated.
以上のように、本実施の形態１に係る目標運動解析装置１０において、パラメータ推定部１７は、複数の時刻における観測方位に基づき、目標の状態量を推定し、粒子位置更新部１８は、複数の粒子のうち重みが最も小さい粒子の状態量をパラメータ推定部１７で推定された目標の状態量に置き換える。これにより、観測方位の近傍に配置された粒子の重みが大きい状態に保たれるため、過疎状態が発生することなく、目標の状態を正しく推定することができる。 As described above, in the target motion analysis device 10 according to the first embodiment, the parameter estimation unit 17 estimates the state quantity of the target based on the observation direction at multiple times, and the particle position update unit 18 replaces the state quantity of the particle with the smallest weight among the multiple particles with the state quantity of the target estimated by the parameter estimation unit 17. This keeps the weight of particles located near the observation direction high, preventing depopulation and allowing the state of the target to be accurately estimated.
以上、本発明の実施の形態１について説明したが、本発明は、上述した実施の形態１に限定されるものではなく、本発明の要旨を逸脱しない範囲内で様々な変形または応用が可能である。例えば、実施の形態１では、パラメータ推定部１７において目標の状態量Ｙ´＾ｋを推定する方法として、式（９）の非線形方程式を線形近似して解く方法を適用した場合について説明したが、これはこの例に限られない。
例えば、目標の状態量Ｙ´＾ｋを推定する方法として、従来の文献（浅野太， 音のアレイ信号処理（第171～179頁）， コロナ社， 2011）に記載されたカルマンフィルタを用いる方法を適用してもよいし、非特許文献２に記載されたガウスニュートン法を適用してもよい。これらの推定方法を適用した場合でも、実施の形態１と同様の効果を得ることができる。
Although the first embodiment of the present invention has been described above, the present invention is not limited to the above-described first embodiment, and various modifications and applications are possible within the scope of the gist of the present invention. For example, in the first embodiment, a case has been described in which a method of solving the nonlinear equation of Equation (9) by linear approximation is applied as a method of estimating the state quantity Y'^ k of the target in the parameter estimating unit 17, but this is not limited to this example.
For example, as a method for estimating the target state quantity Y'^ k , a method using a Kalman filter described in a conventional document (Futoshi Asano, Array Signal Processing of Sound (pages 171-179), Corona Publishing, 2011) may be applied, or the Gauss-Newton method described in Non-Patent Document 2 may be applied. Even when these estimation methods are applied, the same effects as those of the first embodiment can be obtained.
また、実施の形態１では、粒子位置更新部１８において、運動更新部１６から得られ他粒子の状態量のうち、重みの小さい１つの粒子の状態量のみを推定状態量Ｙ´＾ｋに置き換える場合について説明したが、これに限られず、置き換える粒子の状態量を複数としてもよい。置き換える粒子の状態量を複数とすることにより、粒子の重みを更新する際に、観測方位の近傍に粒子がより集まりやすくなることが考えられる。ただし、置き換える粒子の数が多すぎると、縮退が発生し、推定結果が悪化する可能性がある。 Furthermore, in the first embodiment, the particle position update unit 18 replaces only the state quantity of one particle with a small weight among the state quantities of other particles obtained from the motion update unit 16 with the estimated state quantity Y'^ k . However, this is not limited to this, and a plurality of state quantities of the particle to be replaced may be used. By replacing a plurality of state quantities of the particle, it is considered that the particles are more likely to gather near the observation direction when updating the particle weights. However, if too many particles are replaced, degeneration may occur, which may deteriorate the estimation result.
さらに、実施の形態１では、粒子位置更新部１８において観測方位の近傍の粒子の重みを大きくするために、粒子の重みを値「１」にリセットするように説明したが、これに限られず、例えば粒子数分の１等の「１」以外の値にリセットしてもよい。 Furthermore, in embodiment 1, the particle position update unit 18 resets the particle weight to the value "1" to increase the weight of particles near the observation direction, but this is not limited to this, and the weight may be reset to a value other than "1", such as 1/the number of particles.
１０、１００ 目標運動解析装置、１１ 粒子初期化部、１２ 重み更新部、１３ 状態推定部、１４ 縮退判定部、１５ リサンプリング部、１６ 運動更新部、１７ パラメータ推定部、１８ 粒子位置更新部、２０ 入力端子、３０ 出力端子。 10, 100 Target motion analysis device, 11 Particle initialization unit, 12 Weight update unit, 13 State estimation unit, 14 Degeneracy determination unit, 15 Resampling unit, 16 Motion update unit, 17 Parameter estimation unit, 18 Particle position update unit, 20 Input terminal, 30 Output terminal.
Claims (7)
所定の時刻に入力された前記観測方位と、前記粒子の状態量および前記粒子の重みとに基づき、前記粒子の重みを更新する重み更新部と、
前記粒子の状態量と、更新された前記粒子の重みとから、前記目標の第１の状態量を推定し、推定結果として出力する状態推定部と、
前記粒子の状態量および前記粒子の重みに基づき、次の時刻における推定に用いる前記粒子の状態量を算出する運動更新部と、
前記所定の時刻以前の複数の時刻における前記観測方位から、前記目標の第２の状態量を推定状態量として推定するパラメータ推定部と、
前記パラメータ推定部で推定された前記推定状態量と、前記運動更新部で算出された前記粒子の状態量および前記粒子の重みとに基づき、前記運動更新部で算出された前記粒子の状態量を置き換える粒子位置更新部と
を備え、
前記粒子位置更新部は、
複数の前記粒子のうち前記重みが最も小さい前記粒子の状態量を、前記パラメータ推定部で推定された前記推定状態量に置き換える
目標運動解析装置。 A target motion analysis device that estimates a state of a target based on an observation direction indicating a direction of the target, and state quantities of a plurality of particles arranged around the observation direction and weights of the particles, comprising:
a weight update unit that updates the weights of the particles based on the observation direction input at a predetermined time, the state quantities of the particles, and the weights of the particles;
a state estimation unit that estimates a first state quantity of the target from the state quantities of the particles and the updated weights of the particles , and outputs the estimated result ;
a motion update unit that calculates a state quantity of the particle to be used for estimation at a next time based on the state quantity of the particle and the weight of the particle;
a parameter estimation unit that estimates a second state quantity of the target as an estimated state quantity from the observation azimuths at a plurality of times before the predetermined time ;
a particle position update unit that replaces the state quantity of the particle calculated by the motion update unit based on the estimated state quantity estimated by the parameter estimation unit and the state quantity of the particle and the weight of the particle calculated by the motion update unit ,
The particle position update unit
a target motion analysis device that replaces a state quantity of the particle with the smallest weight among the plurality of particles with the estimated state quantity estimated by the parameter estimation unit;
前記縮退が発生したと判定された場合に、前記縮退判定部から出力された前記粒子の重みに基づき、前記粒子の状態量および前記粒子の重みをリサンプルするリサンプリング部と
をさらに備え、
前記運動更新部は、
前記縮退判定部または前記リサンプリング部から出力された前記粒子の状態量および前記粒子の重みに基づき、次の時刻における前記粒子の状態量を算出する
請求項１に記載の目標運動解析装置。 a degeneracy determination unit that receives the state quantities of the particles and the weights of the particles output from the weight update unit and determines whether degeneracy has occurred based on the weights of the particles updated by the weight update unit;
a resampling unit that, when it is determined that the degeneration has occurred, resamples the state quantities of the particles and the weights of the particles based on the weights of the particles output from the degeneration determination unit,
The motion update unit
2. The target motion analysis device according to claim 1, wherein the state quantities of the particles at the next time are calculated based on the state quantities of the particles and the weights of the particles output from the degeneracy determination unit or the resampling unit.
入力された前記粒子の重みに基づき、前記縮退を判定するための判定値を算出し、
算出された前記判定値と予め設定された閾値とを比較して、前記縮退の発生を判定する
請求項２に記載の目標運動解析装置。 The degeneration determination unit
calculating a judgment value for judging the degeneracy based on the input weight of the particle;
The target motion analysis device according to claim 2 , wherein the calculated determination value is compared with a preset threshold value to determine whether or not the degeneration has occurred.
前記縮退が発生したと判定された場合に、前記重みの小さい前記粒子を削除し、前記重みの大きい前記粒子を分割して複製することにより、前記リサンプルを行う
請求項２または３に記載の目標運動解析装置。 The resampling unit
4. The target motion analysis device according to claim 2, wherein when it is determined that the degeneration has occurred, the resampling is performed by deleting the particles with small weights and dividing and duplicating the particles with large weights .
請求項１～４のいずれか一項に記載の目標運動解析装置。 5. The target motion analysis device according to claim 1, further comprising a particle initialization unit that, when the observation direction is input for the first time, uniformly arranges the plurality of particles having initial state quantities and initial weights around the observation direction.
所定の時刻に入力された前記観測方位と、前記粒子の状態量および前記粒子の重みとに基づき、前記粒子の重みを更新する重み更新ステップと、
前記粒子の状態量と、更新された前記粒子の重みとから、前記目標の第１の状態量を推定し、推定結果として出力する状態推定ステップと、
前記粒子の状態量および前記粒子の重みに基づき、次の時刻における推定に用いる前記粒子の状態量を算出する運動更新ステップと、
前記所定の時刻以前の複数の時刻における前記観測方位から、前記目標の第２の状態量を推定状態量として推定するパラメータ推定ステップと、
前記パラメータ推定ステップで推定された前記推定状態量と、前記運動更新ステップで算出された前記粒子の状態量および前記粒子の重みとに基づき、前記運動更新ステップで算出された前記粒子の状態量を置き換える粒子位置更新ステップと
を備え、
前記粒子位置更新ステップにおいて、
複数の前記粒子のうち前記重みが最も小さい前記粒子の状態量を、前記パラメータ推定ステップで推定された前記推定状態量に置き換える
目標運動解析方法。 A target motion analysis method for estimating a state of a target based on an observation direction indicating a direction of the target, and state quantities of a plurality of particles arranged around the observation direction and weights of the particles, comprising:
a weight updating step of updating the weights of the particles based on the observation orientations input at predetermined times, the state quantities of the particles, and the weights of the particles;
a state estimation step of estimating a first state quantity of the target from the state quantities of the particles and the updated weights of the particles , and outputting the result as an estimation result ;
a motion update step of calculating a state quantity of the particle to be used for estimation at a next time based on the state quantity of the particle and the weight of the particle;
a parameter estimation step of estimating a second state quantity of the target as an estimated state quantity from the observation azimuths at a plurality of times before the predetermined time ;
a particle position updating step of replacing the state quantity of the particle calculated in the motion updating step based on the estimated state quantity estimated in the parameter estimating step, and the state quantity of the particle and the weight of the particle calculated in the motion updating step ,
In the particle position updating step,
a state quantity of the particle with the smallest weight among the plurality of particles, the state quantity being replaced with the estimated state quantity estimated in the parameter estimation step;
所定の時刻に入力された前記観測方位と、前記粒子の状態量および前記粒子の重みとに基づき、前記粒子の重みを更新する重み更新ステップと、
前記粒子の状態量と、更新された前記粒子の重みとから、前記目標の第１の状態量を推定し、推定結果として出力する状態推定ステップと、
前記粒子の状態量および前記粒子の重みに基づき、次の時刻における推定に用いる前記粒子の状態量を算出する運動更新ステップと、
前記所定の時刻以前の複数の時刻における前記観測方位から、前記目標の第２の状態量を推定状態量として推定するパラメータ推定ステップと、
前記パラメータ推定ステップで推定された前記推定状態量と、前記運動更新ステップで算出された前記粒子の状態量および前記粒子の重みとに基づき、前記運動更新ステップで算出された前記粒子の状態量を置き換える粒子位置更新ステップと
を目標運動解析装置のプロセッサに実行させ、
前記粒子位置更新ステップにおいて、
複数の前記粒子のうち前記重みが最も小さい前記粒子の状態量を、前記パラメータ推定ステップで推定された前記推定状態量に置き換える
目標運動解析プログラム。 A target motion analysis program that estimates a state of a target based on an observation direction indicating a direction of the target, and state quantities of a plurality of particles arranged around the observation direction and weights of the particles, comprising:
a weight updating step of updating the weights of the particles based on the observation orientations input at predetermined times, the state quantities of the particles, and the weights of the particles;
a state estimation step of estimating a first state quantity of the target from the state quantities of the particles and the updated weights of the particles , and outputting the result as an estimation result ;
a motion update step of calculating a state quantity of the particle to be used for estimation at a next time based on the state quantity of the particle and the weight of the particle;
a parameter estimation step of estimating a second state quantity of the target as an estimated state quantity from the observation azimuths at a plurality of times before the predetermined time ;
a particle position updating step of replacing the state quantity of the particle calculated in the motion updating step based on the estimated state quantity estimated in the parameter estimation step and the state quantity of the particle and the weight of the particle calculated in the motion updating step,
In the particle position updating step,
a target motion analysis program for replacing a state quantity of the particle with the smallest weight among the plurality of particles with the estimated state quantity estimated in the parameter estimation step;
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2021139007A JP7725942B2 (en) | 2021-08-27 | 2021-08-27 | Target motion analysis device, target motion analysis method, and target motion analysis program |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2021139007A JP7725942B2 (en) | 2021-08-27 | 2021-08-27 | Target motion analysis device, target motion analysis method, and target motion analysis program |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2023032717A JP2023032717A (en) | 2023-03-09 |
| JP7725942B2 true JP7725942B2 (en) | 2025-08-20 |
Family
ID=85416110
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2021139007A Active JP7725942B2 (en) | 2021-08-27 | 2021-08-27 | Target motion analysis device, target motion analysis method, and target motion analysis program |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7725942B2 (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009199363A (en) | 2008-02-21 | 2009-09-03 | Toshiba Corp | Tracking system using particle filtering |
| JP2011149782A (en) | 2010-01-20 | 2011-08-04 | National Institute Of Advanced Industrial Science & Technology | Two-dimensional sound source mapping method from mobile robot by particle filtering |
| JP2012164089A (en) | 2011-02-04 | 2012-08-30 | Fujitsu Ltd | Robot, position estimation method and program |
| JP2015175700A (en) | 2014-03-14 | 2015-10-05 | 沖電気工業株式会社 | Position estimation device, position estimation method, and program |
| US20210173398A1 (en) | 2019-12-05 | 2021-06-10 | Aptiv Technologies Limited | Methods and systems for determining an initial ego-pose for initialization of self-localization |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH1062508A (en) * | 1996-08-26 | 1998-03-06 | Oki Electric Ind Co Ltd | Target motion analysis method |
-
2021
- 2021-08-27 JP JP2021139007A patent/JP7725942B2/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009199363A (en) | 2008-02-21 | 2009-09-03 | Toshiba Corp | Tracking system using particle filtering |
| JP2011149782A (en) | 2010-01-20 | 2011-08-04 | National Institute Of Advanced Industrial Science & Technology | Two-dimensional sound source mapping method from mobile robot by particle filtering |
| JP2012164089A (en) | 2011-02-04 | 2012-08-30 | Fujitsu Ltd | Robot, position estimation method and program |
| JP2015175700A (en) | 2014-03-14 | 2015-10-05 | 沖電気工業株式会社 | Position estimation device, position estimation method, and program |
| US20210173398A1 (en) | 2019-12-05 | 2021-06-10 | Aptiv Technologies Limited | Methods and systems for determining an initial ego-pose for initialization of self-localization |
Non-Patent Citations (1)
| Title |
|---|
| 光益義幸，雑音推定型パーティクルフィルタによるレーダ目標追尾，ロボティクス・メカトロニクス講演会 ’１２ 講演論文集 ，日本，一般社団法人日本機械学会，2012年05月27日，No,. 12-3，772～775 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2023032717A (en) | 2023-03-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Singh et al. | Pi-lstm: Physics-infused long short-term memory network | |
| CN104869126B (en) | A kind of network intrusions method for detecting abnormality | |
| US11195091B2 (en) | Reservoir computing system | |
| EP3716160A1 (en) | Learning parameters of a probabilistic model comprising gaussian processes | |
| JP2020086778A (en) | Machine learning model building device and machine learning model building method | |
| CN112388628A (en) | Apparatus and method for training a gaussian process regression model | |
| CN112232495A (en) | Prediction model training method, device, medium and computing equipment | |
| US12430581B2 (en) | Machine learning training device | |
| JP2020091791A (en) | Estimating device, optimizing device, estimating method, optimizing method, and program | |
| CN110135085A (en) | Fatigue Crack Evaluation Method Based on Adaptive Kernel Density Estimation Assisted Particle Filter | |
| JP2020154416A (en) | Track prediction device and track prediction method | |
| JP7725942B2 (en) | Target motion analysis device, target motion analysis method, and target motion analysis program | |
| WO2021157669A1 (en) | Regression analysis device, regression analysis method, and program | |
| JP7400819B2 (en) | Prediction device, prediction method, and prediction program | |
| KR101234797B1 (en) | Robot and method for localization of the robot using calculated covariance | |
| JP6986160B2 (en) | Image processing method and image processing equipment | |
| JP6954346B2 (en) | Parameter estimator, parameter estimator, and program | |
| CN115169184A (en) | Finite element model correction method adopting improved differential evolution Markov chain | |
| JP7176371B2 (en) | Estimation device, estimation method and program | |
| CN120781955A (en) | Parameter inference device and parameter inference method | |
| JP7444426B2 (en) | State estimation evaluation device, method, and program | |
| JP7500358B2 (en) | Information processing device | |
| US20210141980A1 (en) | Method and system for real-time simulations using convergence stopping criterion | |
| KR20210125732A (en) | Method and Apparatus for Adaptive Kernel Inference for Dense and Sharp Occupancy Grids | |
| Munoz-Minjares et al. | Jitter representation in SCNA breakpoints using asymmetric exponential power distribution |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20240508 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20250122 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20250304 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20250422 |
|
| 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: 20250708 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20250721 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 7725942 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |