JP6358006B2 - Target tracking device - Google Patents

Description

  The present invention relates to a target tracking device that estimates a motion specification of a target and tracks the target.

  Conventional target tracking devices that estimate target motion parameters using observation results of distance differences from a target by a plurality of sensors include a synchronous target tracking device and an asynchronous target tracking device. In synchronous target tracking, target tracking is performed using observation information obtained at the same time by multiple sensors, and in asynchronous target tracking, target tracking is performed using observation information obtained at different times by multiple sensors. .

ここで、z_i,j(t)は時刻tにおける目標とセンサi、センサjとの距離差の観測値、ｒ_i,j(x(t))は時刻tにおける目標とセンサi、センサjとの距離差真値を表す観測方程式、v_i,j(t)は時刻tにおける目標とセンサi、センサjとの距離差の観測誤差、x(t)は目標の状態ベクトル（式(4)）を表す。観測誤差v_i,j(t)は受信時の雑音の影響等により発生する。 Conventional synchronous target tracking devices are described in Patent Document 1 and Non-Patent Document 1, for example. The operation of this conventional synchronous target tracking device will be described. First, the observation model is assumed as shown in Equation (1).

Here, z _{i, j} (t) is the observed value of the distance difference between the target and sensor i and sensor j at time t, and r _{i, j} (x (t)) is the target and sensor i and sensor j at time t. The observation equation that expresses the true value of the distance difference between and v _{i, j} (t) is the observation error of the distance difference between the target and sensor i and sensor j at time t, and x (t) is the target state vector (equation (4 )). The observation error v _{i, j} (t) occurs due to the influence of noise during reception.

ここで

であり、I_nxnはn行n列の単位行列、O_nxnはn行n列の零行列、上付き添え字のTは転置の記号を意味する。 For example, it is assumed that an observation value of a distance difference between the target position in the xy two-dimensional orthogonal coordinate system and the sensors i and j is obtained. At this time, the distance difference true value r _{i, j} (x (t)) is defined as in equation (2). Here, Si and Sj represent position vectors of sensors i and j, respectively.

here

_Where I _nxn is an n-by-n unit matrix, O _nxn is an n-by-n zero matrix, and the superscript T is a transpose symbol.

  Under the assumption of the observation model as described above, the conventional target tracking apparatus estimates the state vector by the algorithm shown below.

  First, when there are three or more sensors, two or more equations of the formula (2) are obtained. Therefore, the state vector can be obtained by solving the simultaneous equations. However, since equation (2) is a nonlinear equation, the position vector estimated value is calculated using the nonlinear least square method.

で近似すると、真値への修正ベクトルδxとの関係式が式(5)により得られる。

ここで、距離差観測値として、センサ1を基準とし、センサ2〜Mまでの距離差観測値を使用する場合、δrはM-1行1列の残差ベクトル、GはM-1行2列のヤコビアン、δxは2行1列の修正ベクトルとなる。残差とは、観測値と推定値の差を意味する。また、各変数は下記式で表現される。

Using equation (2), the target position is the position vector estimate (or initial value)

Is approximated by (5), the relational expression with the correction vector Δx to the true value is obtained.

Here, when using the distance difference observation value from sensor 2 to M as the distance difference observation value as a reference, δr is a residual vector of M-1 rows and 1 column, and G is M-1 row 2 The column Jacobian, Δx, is a 2 × 1 correction vector. Residual means the difference between the observed value and the estimated value. Each variable is expressed by the following formula.

以上の処理を修正ベクトルδxが収束する（例えば、所定値以下となる）まで繰り返すと、位置ベクトルが推定される。ここまでの処理を測位処理とし、さらに、各時刻で得られる前記位置ベクトルを用いて追尾処理を実施することで位置及び速度を含む目標状態ベクトルを更新する。 The correction vector is calculated by the equation (11) using the least square method from the equation (5), and the target position vector is updated as in the equation (12).

When the above processing is repeated until the correction vector Δx converges (for example, becomes a predetermined value or less), the position vector is estimated. The process so far is a positioning process, and further, a tracking process is performed using the position vector obtained at each time to update a target state vector including a position and a velocity.

Further, the estimation error covariance matrix P of the position vector is as shown in Equation (13). Here, σ is the accuracy of the distance difference observation value.

上記に示した従来の同期型目標追尾装置は、複数のセンサが同じ時刻に観測した場合に成立する。しかし、実際には観測タイミングのずれ、マルチパス干渉による観測値欠落などにより、複数センサ間で同時に観測できない状況が想定される。 Further, the horizontal plane position accuracy σ _H can be calculated from HDOP which is DOP (Dilution of Precision) of the horizontal plane.

The conventional synchronous target tracking device described above is established when a plurality of sensors observe at the same time. However, in reality, it is assumed that a plurality of sensors cannot be observed at the same time due to a difference in observation timing or missing observation values due to multipath interference.

  On the other hand, as an asynchronous target tracking device, for example, there is Non-Patent Document 2. Non-Patent Document 2 discloses a method of updating a target state vector by directly inputting a distance difference observation value without performing positioning processing as in the case of a synchronous target tracking device.

  Since Non-Patent Document 2 directly uses distance difference observation values observed at different times as in the asynchronous target tracking device, the target state vector is updated even when the distance difference observation values cannot be obtained at the same time and positioning cannot be performed. It becomes possible.

  In this case, the tracking process is performed using the distance difference instead of the position vector as in the synchronous target tracking device.

JP 2002-267732 A

D. J. Torrieri, “Statistical Theory of Passive Location Systems,” IEEE Trans. Aersp. Syst. , vol. AES-20, pp. 183-198, Mar. 1984. N. Okello, “Comparison of Recursive Algorithms for Emitter Localization using TDOA Measurements from a Pair of UAVs,” IEEE Trans. Aersp. Syst. , vol. 47, no. 3, pp. 1723-1732, Jul. 2011.

In a conventional target tracking device including a synchronous target tracking device and an asynchronous target tracking device, all the distance difference observation values observed by a plurality of sensors are input to the tracking processing, and tracking is performed by updating the target position and velocity. Process. In this case, since the tracking process is performed including observation values with low observation accuracy and low contribution to the tracking process, it is necessary to perform calculations on observation values with a low contribution to the tracking process. There was a problem that would increase.
The present invention has been made to solve the above-described problems, and an object of the present invention is to reduce processing load while ensuring tracking accuracy in tracking processing in a target tracking device.

The target tracking device according to the present invention is included in the plurality of sensors based on an observation value output unit that outputs observation values of the target observed by a plurality of sensors, and an observation value output from the observation value output unit. A distance difference calculation unit for calculating a distance difference between a sensor set, which is a distance difference between the two sensors constituting the sensor set and the target, using two sensors as sensor sets, and each of the different sensor sets. An observation accuracy calculation unit for calculating the observation accuracy for each distance difference of the two, and a difference calculated by the distance difference calculation unit based on the observation accuracy for each distance difference of each of the different sensor sets calculated by the observation accuracy calculation unit a distance difference observed value selection portion including a priority selection unit for selecting a distance difference of one sensor set even without inside or al low the sensor set respective distance differences, is selected by the distance difference observed value selector Based on at least one sensor set of distance difference, characterized in that and a tracking processing unit for performing tracking processing of calculating the position and velocity of the target.

  According to this invention, since the distance difference observation value with high observation accuracy is selected and used preferentially, it is possible to reduce the tracking processing load while ensuring the tracking accuracy.

The block diagram which shows the target tracking apparatus of Embodiment 1 of this invention. The conceptual diagram of the tracking process in the target tracking apparatus of Embodiment 1 of this invention. The figure which shows the processing flow of the distance difference observation value selection part 4 and the asynchronous tracking process part 5 in the target tracking apparatus of Embodiment 1 of this invention. The conceptual diagram of the tracking process which selects the distance difference observation value by a priority sensor group in each distance difference observation time in the target tracking apparatus of Embodiment 1 of this invention. The block diagram which shows the target tracking apparatus of Embodiment 2 of this invention. The block diagram which shows the target tracking apparatus of Embodiment 3 of this invention.

Embodiments of the present invention will be described below.
Embodiment 1 FIG.
FIG. 1 is a configuration diagram illustrating a target tracking device 1 according to the first embodiment. As shown in FIG. 1, the target tracking device 1 of the present embodiment includes an observation value output unit 2, a distance difference calculation unit 3, a distance difference observation value selection unit 4, and an asynchronous tracking processing unit 5 that is a tracking processing unit. Yes. In the following drawings, the same reference numerals indicate the same or corresponding parts.

  The observation value output unit 2 includes sensors 11 to 1M (M is an integer greater than 1). The distance difference calculation unit 3 uses the target and any two sensors among the sensors 11 to 1M based on the observation value output from the observation value output unit 2, and sets the two sensors included in the sensor set and the target. The difference between the distances is calculated as the distance difference between the sensor groups. The distance difference calculation unit 3 outputs the distance difference for each sensor set to the distance difference observation value selection unit 4. The distance difference observation value selection unit 4 includes a prediction unit 41, a pseudo DOP (Dilution of Precision) calculation unit 42 for the sensor group, and a sensor group priority selection unit 43. The pseudo DOP calculation unit 42 can be generally handled as an observation accuracy calculation unit. Further, the sensor group priority selection unit 43 can be generally handled as a priority selection unit.

  Next, the operation of the target tracking device 1 according to the first embodiment will be described. The observation value output unit 2 outputs all or part of the observation values of the sensors 11 to 1M. The distance difference calculation unit 3 is based on the observation value output from the observation value output unit 2, and any two sensors out of the sensors 11 to 1M are included in the sensor set for each of the different sensor sets. The difference between the distances between the two sensors and the target is calculated as a distance difference. In FIG. 1, sensor 11 and sensor 12 are sensor set 1,..., Sensor 1 (M-1) and sensor 1M are sensor set M-1, and the difference in distance between the two sensors and the target in each sensor set is shown. Calculated as the distance difference. Here, the distance difference calculation unit 3 may calculate the distance difference for each sensor set at the same time or at different times. The distance difference for each sensor set calculated at the same time is output to the distance difference observation value selection unit 4. The distance difference observation value obtained by each sensor set input at the same time may be a distance difference observation value obtained by each sensor set input in a certain predetermined time zone (for example, within several seconds). Absent. Further, the distance difference calculation unit 3 may output the distance difference value for each different sensor set to the distance difference observation value selection unit 4 at different times.

  The prediction unit 41 in the distance difference observation value selection unit 4 calculates the predicted position after the predicted time ΔTi (i = 1,..., L) seconds from the estimated position and speed calculated by the tracking process performed by the asynchronous tracking processing unit 5. To do. The pseudo DOP calculation unit 42 includes a calculation unit corresponding to each sensor set (sensor set 1,..., Sensor set M-1 in FIG. 1). The pseudo DOP calculation unit 42 receives the predicted position information after the prediction time ΔTi (i = 1,..., L) seconds from the prediction unit 41, and calculates the pseudo DOP at the predicted position for each sensor set. Here, unlike the DOP of the positioning position, the pseudo DOP is a value representing the position accuracy for each sensor set defined by the present embodiment. Although details of the pseudo DOP will be described later, the smaller the value of the pseudo DOP, the higher the position accuracy.

  The sensor group priority selection unit 43 receives the distance difference observation value obtained by each sensor group input from the distance difference calculation unit 3 at the same time. In addition, when the sensor group priority selection unit 43 receives the pseudo DOP of each sensor group output from the pseudo DOP calculation unit 42, the asynchronous tracking processing unit displays the distance difference observation values obtained by each sensor group in ascending order of the pseudo DOP. Output to 5. Asynchronous tracking processing unit 5 performs tracking processing using the distance difference observation value obtained by each sensor set having a small pseudo DOP. By such processing, it is possible to efficiently perform tracking processing with a small amount of computation using information on observation values with high observation accuracy. The sensor group priority selection unit 43 outputs the distance difference observation values obtained by each sensor group to the asynchronous tracking processing unit 5 in ascending order of the pseudo DOP. However, the asynchronous tracking processing preferentially performs the observation value with the small pseudo DOP. You may output to the part 5.

  Hereinafter, details of the pseudo DOP defined in the present embodiment will be described.

  First, HDOP of Expression (15) is known as a parameter representing the overall position accuracy, but this HDOP is transformed using approximation and decomposed into each element. Specifically, in the positioning of the two-dimensional position, the HDOP in the equation (15) becomes the second equation in the equation (16), and further approximates the third equation in the equation (16). In the third equation of Equation (16), the first term in the square root is the DOP in the X direction (hereinafter referred to as XDOP), and the second term is the DOP in the Y direction (YDOP). Therefore, the relational expression of HDOP, XDOP, and YDOP is as shown in Expression (17).

  Following this idea, pay attention to a pair of sensors i and j. The pseudo DOP in the X direction (hereinafter referred to as XDOPtrk (i, j)) for sensor set i and j is expressed by equation (18) and the pseudo DOP in the Y direction. (Hereinafter referred to as YDOPtrk (i, j)) is defined by equation (19). Furthermore, HDOPtrk (i, j), which is a pseudo-DOP on the horizontal plane, is defined by equation (20) following the notation of equation (17). When the pseudo DOP is defined in this way, it can be seen that the HDOP represented by the third equation of Equation (16) can be expressed using the pseudo DOP of the sensor set.

ここで、

である。ここで、

はri、jのxに関する勾配と呼んでも構わない。

here,

It is. here,

May be called the gradient of ri, j with respect to x.

  In this way, HDOP, which is a parameter representing the overall position accuracy, can be represented by a pseudo DOP for each sensor set defined in the present embodiment. In other words, the parameters representing the overall position accuracy are determined by the pseudo DOP for each sensor set, and by selecting the observation values for each sensor set with a small pseudo DOP and high observation accuracy, a small number of observation values are used. It can be seen that the target position can be estimated with high accuracy.

  In the present embodiment, the pseudo DOP output from the pseudo DOP calculation unit 42 for each sensor set is any one of XDOPtrk (i, j), YDOPtrk (i, j), and HDOPtrk (i, j). When the sensor group priority selection unit 43 receives the pseudo DOP output from the pseudo DOP calculation unit 42 of each sensor group, the distance difference observation obtained by each sensor group input at the same time received from the distance difference calculation unit 3. The values are output to the asynchronous tracking processing unit 5 in ascending order of pseudo DOP, for example, HDOPtrk.

There is also a method in which the pseudo DOP calculation unit 42 outputs the direction in which the accuracy is to be improved to the asynchronous tracking processing unit 5 in the order of decreasing XDOPtrk or YDOPtrk and determines the priority of the sensor set. At this time, the position error variance in the X direction of the estimated error covariance matrix (Equation (24)) output from the asynchronous tracking processing unit 5 at the subsequent stage is used to determine whether to improve the X or Y direction. It is also possible to compare σ _xx ² and position error variance σ _yy ² in the Y direction to improve the direction in which the error is large.

σ_xx ²：X方向の位置誤差分散
σ_yy ²：Y方向の位置誤差分散
σ_vxvx ²：X方向の速度誤差分散
σ_vyvy ²：Y方向の速度誤差分散 As an example, FIG. 2 shows a conceptual diagram of the tracking process when the target tracking process in the present embodiment is performed. In FIG. 2, the error distribution in the X and Y directions at each time is shown, and the estimation error in the Y direction is larger than the estimation error in the X direction. For example, the accuracy in the Y direction is improved by using the distance difference observation value that contributes to the improvement in the accuracy in the Y direction from the estimated error covariance matrix of Equation (24) calculated by the tracking process in the asynchronous tracking processing unit 5. It becomes possible.

σ _xx ² : Position error variance in X direction σ _yy ² : Position error variance in Y direction σ _vxvx ² : Speed error variance in X direction σ _vyvy ² : Speed error variance in Y direction

疑似DOP算出部４２は前記予測位置における疑似DOPを算出する（ST4）。疑似DOP算出部４２は前記推定誤差共分散行列のX方向誤差分散とY方向誤差分散を比較し、X方向誤差分散がY方向誤差分散より大きい場合（ST5）は、X方向の疑似DOPが小さい順に追尾処理に入力するようにTDOA観測値を優先的に並び替える（ST6）。ST４において、Y方向誤差分散がX方向誤差分散より大きい場合は、疑似DOP算出部４２はY方向の疑似DOPが小さい順に追尾処理に入力するようにTDOA観測値を優先的に並び替える（ST7）。また、ST5では距離差観測時刻t_i(i=1、…、L)に跨がった全距離差観測値を並び替えても良いこととする。疑似DOP算出部４２はセンサ組の優先選択を行う。具体的には、ST6またはST7で並び替えた順に非同期追尾処理部５へ入力する（ST8）。ここで、予め処理上限数を決めておけば上限数に達した時点で処理を打ち切っても良い。 最後に、パラメータiがLであれば(ST9)終了し、パラメータiがそれ以外の値であれば、ST3の処理に戻る。 FIG. 3 shows a processing flow of the distance difference observation value selection unit 4 and the asynchronous tracking processing unit 5 when the priority of the sensor set is determined using the estimated error covariance matrix of Expression (24). First, the asynchronous tracking processing unit 5 is carried out tracking processing, to calculate the estimated value and the estimation error covariance matrix of the target state vector at the current time point t _s (ST1). In the subsequent processing, assuming that multiple distance difference observation values are obtained at different times within the tracking processing cycle, the predicted position at each acquisition time of the distance difference observation values is calculated, and the following ST3 to ST8 A series of processing is repeated. Alternatively, the predicted time ΔTi (i = 1,..., L) may be set in advance, and the processing of ST2 to ST6 after ΔTi seconds may be repeated L times. That is, first, i = 1 is set (ST2). Next, the prediction unit 41 calculates a time difference (expression (25)) between the time t _{i at} which the distance difference observation value is acquired and the latest time t _s, and the distance difference observation value is acquired by expression (26). time t calculated position in _i x _p (t _i), calculates the _{y p (t i) (ST3} ).

The pseudo DOP calculating unit 42 calculates a pseudo DOP at the predicted position (ST4). The pseudo DOP calculation unit 42 compares the X-direction error variance and the Y-direction error variance of the estimated error covariance matrix. If the X-direction error variance is larger than the Y-direction error variance (ST5), the pseudo-DOP in the X direction is small. The TDOA observations are sorted preferentially so as to be input to the tracking process in order (ST6). In ST4, when the Y-direction error variance is larger than the X-direction error variance, the pseudo DOP calculation unit 42 preferentially rearranges the TDOA observation values so that the Y-direction pseudo DOP is input to the tracking process in ascending order (ST7). . In ST5, all distance difference observation values straddling the distance difference observation times t _i (i = 1,..., L) may be rearranged. The pseudo DOP calculation unit 42 performs priority selection of the sensor set. Specifically, it inputs to the asynchronous tracking processing unit 5 in the order rearranged in ST6 or ST7 (ST8). Here, if the upper limit number is determined in advance, the process may be terminated when the upper limit number is reached. Finally, if the parameter i is L (ST9), the process ends. If the parameter i is any other value, the process returns to ST3.

  With this process, as shown in FIG. 4, at each distance difference observation time, the distance difference observation value by the priority sensor set is selected and the tracking process is performed. In FIG. 4, when calculating the predicted position after Δt1 seconds and Δt2 seconds, observation values from different sensor groups are selected.

  The asynchronous tracking processing unit 5 receives the distance difference of each sensor set output from the distance difference observation value selection unit 3, performs tracking processing by a non-linear filter, and outputs the position and velocity. Here, an extended Kalman filter, an unscented Kalman filter, a particle filter, or the like may be used as the nonlinear filter.

  As described above, according to the target tracking device 1 according to the first embodiment, the distance difference observation values that are simultaneously input to the tracking process are selected and input in the order that contributes to the tracking position accuracy. The tracking accuracy can be ensured even when there is a restriction on the calculation load. Particularly in a multi-target environment, the number of distance difference observation values to be processed is large, and this embodiment is effective.

  In addition, although the description has been made with the two-dimensional position so far, the three-dimensional position can be easily extended with the same concept. Further, when not only the distance difference but also the Doppler speed difference is obtained, the same idea may be applied.

  As described above, the target tracking device according to the present embodiment includes a plurality of observation value output units 2 that output observation values of targets observed by a plurality of sensors, and a plurality of observation values output from the observation value output unit 2. A distance difference calculation unit 3 that calculates two sensor included in the sensor as a sensor set, and calculates a distance difference between the sensor sets, which is a distance difference between the two sensors constituting the sensor set and a target, for different sensor sets; A pseudo DOP calculation unit 42 that is an observation accuracy calculation unit that calculates the observation accuracy of the distance difference between different sensor sets, and a distance difference calculation unit based on the observation accuracy of the distance difference between different sensor sets calculated by the pseudo DOP calculation unit 42 The distance difference observation value selection unit 4 including a priority selection unit that selects the distance difference of at least one sensor set from the distance differences of the different sensor sets calculated by 3 and the distance difference observation value selection unit 4 are selected. At least One of the basis of the sensor suite of the distance difference, and wherein the tracking processing unit for performing tracking processing of calculating the position and velocity of the target, further comprising: a. With such a configuration, since the distance difference observation value that contributes to tracking accuracy is selected and used, it is possible to reduce the tracking processing load while ensuring tracking accuracy.

  In the target tracking device of the present embodiment, the distance difference observation value selection unit 4 includes a prediction unit 41 that calculates a predicted position at a target future time, and different sensor sets at the predicted position calculated by the prediction unit 41. Based on the observation accuracy of the distance difference between different sensor sets calculated by the pseudo DOP calculation unit 42 and the pseudo DOP calculation unit 42 that is an observation accuracy calculation unit that calculates the observation accuracy of the distance difference, And a priority selection unit 43 for selecting a distance difference between at least one sensor set. With such a configuration, it is possible to select a distance difference observation value that contributes to tracking accuracy assuming a future time required for tracking processing. As a result, it is possible to efficiently select an observation value required for tracking.

  In the target tracking device according to the present embodiment, the observation accuracy of the distance difference between the sensor sets is the observation accuracy in one spatial direction or a horizontal plane. Here, typical examples of one spatial direction include the X direction and the Y direction. A typical example of the observation accuracy on the horizontal plane is HDOP, which is the DOP on the horizontal plane. With such a configuration, it is possible to perform tracking processing for improving observation accuracy by narrowing down in one spatial direction or horizontal direction.

  In the target tracking device according to the present embodiment, the observation accuracy of the distance difference between the sensor sets is a parameter defined based on the positional relationship between the two sensors included in the sensor set and the target. This relationship is expressed by equations (21) and (22). With such a configuration, it is possible to efficiently select a distance difference observation value that contributes to tracking accuracy using observation accuracy determined from sensors and targets included in the sensor set. In addition, this parameter may be a gradient with respect to one spatial direction or a horizontal plane of an estimation error of a distance difference from a target of two sensors included in the sensor set. With such a configuration, it is possible to perform tracking processing for improving observation accuracy by narrowing down in one spatial direction or horizontal direction.

This parameter can also be a pseudo DOP defined for the sensor set. With such a configuration, the distance difference observation value that contributes to tracking accuracy can be expressed smoothly in the same manner as HDOP, which has been known as a parameter that represents the overall position accuracy. The observation accuracy can be calculated efficiently.

  In the target tracking device of the embodiment, the tracking processing unit includes means for calculating the estimated position error of the target for a plurality of spatial directions, and the distance difference observation value selection unit 4 is output from this tracking processing unit. A spatial direction having a large estimated position error is selected from the target estimated position errors for a plurality of spatial directions, and the distance difference of the at least one sensor set is based on the observation accuracy of the distance difference of the sensor set with respect to the selected spatial direction. It comprises the structure which selects. With such a configuration, the tracking processing unit can perform the tracking process using the distance difference observation value of the sensor set with good observation accuracy in the direction in which the accuracy is desired to be improved.

Embodiment 2. FIG.
In the second embodiment, in order to simplify the process of calculating the pseudo DOP for each predicted position in the first embodiment, the pseudo DOP is predicted, and the priority of the distance difference observation value is estimated using the predicted pseudo DOP. Determine the degree.

  FIG. 5 is a configuration diagram illustrating the target tracking device 1 according to the second embodiment. As shown in FIG. 5, the target tracking device 1 of this embodiment includes an observation value output unit 2, a distance difference calculation unit 3, a distance difference observation value selection unit 6, and an asynchronous tracking processing unit 5.

を式(30)のように定義する。式(31)のように時刻Δt秒後にX軸方向にΔX、Y軸方向にΔY分位置が変化した場合、本来は式(21)を用いて算出する必要がある。

In this embodiment, the gradient with respect to x of XDOPtrk (i, j), which is a pseudo DOP of sensor set i, j

Is defined as in equation (30). When the position changes by ΔX in the X-axis direction and by ΔY in the Y-axis direction after time Δt seconds as in Expression (31), it is originally necessary to calculate using Expression (21).

さらに、位置変化量ΔXおよびΔYを追尾処理から得たサンプル時刻kにおける推定速度ベクトル

と時間差Δtで表す。このとき、式(33)、 (34)が成り立つ。

式(32)、 (33)、 (34)より、式(35)が導出される。

ここで、式(35)のA、Bは式(36)、 (37)に示すように時間差Δtに依存しない固定値である。

Here, a method for calculating the equation (31) in a simplified manner is shown below. First, the gradient at the position shifted by ΔX and ΔY for the two-dimensional positions x and y in equation (31) is approximated by equation (32).

Further, the estimated velocity vector at the sampling time k obtained from the tracking process by the position change amounts ΔX and ΔY.

And the time difference Δt. At this time, equations (33) and (34) hold.

Equation (35) is derived from Equations (32), (33), and (34).

Here, A and B in the equation (35) are fixed values that do not depend on the time difference Δt as shown in the equations (36) and (37).

  As described above, the gradient with respect to x of XDOPtrk (i, j) at an arbitrary time difference Δt is calculated using Equation (35). Further, the gradient of YDOPtrk (i, j) with respect to y is calculated in the same procedure.

  Next, the operation of the target tracking device will be described in this embodiment. Here, since the observed value output unit 2, the distance difference calculation unit 3, and the asynchronous tracking processing unit 5 are the same as those in the first embodiment, the description thereof is omitted.

The distance difference observation value selection unit 6 includes a gradient prediction unit 61, a pseudo DOP calculation unit 62, and a sensor group priority selection unit 63. The gradient prediction unit 61 first obtains a sampling time k obtained by the tracking process of the asynchronous tracking processing unit 5. Assuming that the estimated position vectors x _s and y _{s at} are input, the gradient initial value of each sensor set is calculated. Furthermore, the gradient is predicted using the gradient initial value and Equation (35). The pseudo DOP calculation unit 62 calculates the pseudo DOP based on the gradient predicted by the gradient prediction unit 61. Others are the same as in the embodiment, and will be omitted.

  Although the description has been made with the two-dimensional position so far, the three-dimensional position may be calculated in the same way.

  In the configuration of the present embodiment, in order to simplify the process of calculating the pseudo DOP as compared with the first embodiment, the pseudo DOP is predicted, and the priority of the distance difference observation value is predicted using the predicted pseudo DOP. To decide. As a result, it is possible to further reduce the calculation load as compared with the first embodiment.

  As described above, in the target tracking device according to the present embodiment, the gradient prediction unit 61 that calculates the gradient with respect to one spatial direction or horizontal plane of the observation accuracy of the distance difference of the sensor set at the target future time, and the gradient prediction unit 61 A pseudo DOP calculation unit 62 that is an observation accuracy calculation unit that calculates the observation accuracy of the distance difference between different sensor sets based on the calculated gradient, and the observation of the distance difference between the different sensor sets calculated by the pseudo DOP calculation unit 62 And a sensor group priority selection unit 63 which is a priority selection unit for selecting a distance difference of the at least one sensor group from the distance differences of the different sensor groups based on accuracy. By using such a configuration, it is possible to further reduce the calculation load as compared with the first embodiment.

Embodiment 3 FIG.
The third embodiment is different from the first embodiment in configuration, and shows a configuration including a positioning unit and a synchronous tracking process instead of the asynchronous tracking process.

  FIG. 6 is a configuration diagram illustrating the target tracking device 1 according to the second embodiment. As shown in FIG. 6, the target tracking device 1 of this embodiment includes an observation value output unit 2, a distance difference calculation unit 3, a distance difference observation value selection unit 4, a positioning unit 7, and a synchronization tracking processing unit 8. .

  As described in the background art, the positioning unit 7 constructs the equation (8) using all the distance observation values observed at the same time, and calculates the correction vector by the equation (11). However, when there are many distance observation values, the load at the time of matrix calculation becomes large. In the third embodiment, the distance difference observation value selection unit 4 determines the priority of the distance difference observation value for each sensor set using the contribution of the distance difference observation value described in the first embodiment, and the positioning unit 7 The calculation load is reduced by limiting the top N distance observation values in order of priority and performing the positioning process.

  Next, the operation will be described. In the positioning unit 7 of the third embodiment, the distance difference observation value output from the distance difference observation value selection unit 4 is limited to the observation value from the distance difference observation value having a small pseudo DOP value to the top N observation values. Then, the positioning position can be calculated by reducing the Jacobian matrix size of Equation (8). By reducing the matrix size, the calculation load of the positioning unit 7 is suppressed. The synchronization tracking processing unit 8 receives the positioning position output from the positioning unit 7 and calculates the target position and speed.

  The other processes are the same as those in the first and second embodiments, and will be omitted.

  As described above, the target tracking device 1 according to the present embodiment executes the synchronous tracking process by limiting the observed value from the distance difference observation value having a small pseudo DOP value to the top N observation values.

  That is, in the target tracking device of the present embodiment, the distance difference observation value selection unit 4 selects a distance difference between two or more predetermined numbers of sensor sets, and the tracking processing unit is a synchronous tracking processing unit 8. And By using such a configuration, as a result, it is possible to reduce the amount of calculation while maintaining the same position observation accuracy compared to the conventional target tracking process that uses the distance difference observation values of all sensor sets. It is.

1: target tracking device, 2: observation value output unit, 3: distance difference calculation unit, 4: distance difference observation value selection unit, 5: asynchronous tracking processing unit, 6: distance difference observation value selection unit, 7: positioning unit, 8: synchronous tracking processing unit, 41: prediction unit, 42: pseudo DOP calculation unit, 43: sensor group priority selection unit, 61: gradient prediction unit, 62: pseudo DOP calculation unit, 63: sensor group priority selection unit

Claims (10)

An observation value output unit for outputting the observation value of the target observed by a plurality of sensors;
Based on the observation values output from the observation value output unit, two sensors included in the plurality of sensors as sensor sets, and for each of the different sensor sets, two sensors constituting the sensor set and the target A distance difference calculation unit for calculating a distance difference of the sensor set, which is a distance difference of
The observation accuracy computing unit calculates the observation accuracy for each different sensor sets each distance difference and the observation accuracy calculating unit the distance difference calculation section based on the calculated said different sensor set respective distance differences per observation accuracy in a distance difference observed value selection portion including a priority selection unit that selects at least one sensor set of distance differences among the calculated different sensor suite respective distance differences by,
A tracking processing unit that performs a tracking process for calculating the position and speed of the target based on a distance difference of at least one sensor set selected by the distance difference observation value selection unit;
A target tracking device comprising: The distance difference observation value selection unit includes a prediction unit that calculates a predicted position of the target at a future time,
Said observation accuracy for each of the different sensor set respective distance differences calculated by the measurement accuracy calculating section is the observation accuracy for each of the different sensor set each distance difference at the predicted position calculated by the prediction unit The target tracking device according to claim 1, wherein the target tracking device is characterized in that: The target tracking device according to claim 1, wherein the observation accuracy is an observation accuracy in one spatial direction or a horizontal plane. 4. The parameter according to claim 1, wherein the observation accuracy is a parameter defined based on a positional relationship between two sensors included in a sensor set corresponding to the observation accuracy and the target. 5. The target tracking device described. 5. The parameter according to claim 4, wherein the parameter is a gradient with respect to one spatial direction or a horizontal plane of an estimation error of a distance difference from the target of two sensors included in a sensor set corresponding to the observation accuracy . Target tracking device. The target tracking device according to claim 4, wherein the parameter is a pseudo DOP defined for a sensor set corresponding to the observation accuracy . The tracking processing unit includes means for calculating an estimated position error of the target with respect to a plurality of spatial directions, and the distance difference observation value selecting unit estimates the target with respect to the plurality of spatial directions output from the tracking processing unit. A spatial direction having a large estimated positional error is selected from among position errors, and a distance difference of the at least one sensor set is selected based on observation accuracy for each distance difference of the different sensor sets with respect to the selected spatial direction. The target tracking device according to any one of claims 1 to 6, wherein The distance difference observation value selection unit calculates a gradient with respect to one spatial direction or a horizontal plane of observation accuracy for each distance difference of the different sensor sets at the target future time; and
An observation accuracy computing unit for calculating the observation accuracy for each of the different sensor set respective distance differences based on the gradient calculated by the gradient estimation unit,
Based on said observation accuracy calculating section observation accuracy of each of the calculated different sensor suite respective distance differences, the priority selection unit for selecting a distance difference of the at least one sensor set from among the different sensor sets each distance difference When,
The target tracking device according to claim 1, further comprising: The distance difference of the at least one sensor set selected by the distance difference observation value selection unit is a distance difference of a predetermined number of sensor sets of 2 or more,
The target tracking device according to claim 1, wherein the tracking processing unit is a synchronous tracking processing unit. The target tracking device according to claim 1, wherein the tracking processing unit is an asynchronous tracking processing unit.

Images