JP2012242121A - Tracking apparatus - Google Patents

Abstract

PROBLEM TO BE SOLVED: To exhibit tracking maintenance performance when a distance between a sensor and a target is long, and to improve tracking accuracy to each tracking target when the distance between the sensor and the target is short.SOLUTION: A tracking apparatus includes: an angle calculation part 21 for calculating an angle formed by two measurement values obtained by setting a sensor position as a reference point when two measurement values are acquired through the sensor; an angle reference centroid calculation part 22 for calculating a weight coefficient for two measurement values based on the angle formed by the two measurement values calculated by the angle calculation part 21 and calculating the centroids of respective measurement values between respective measurement values based on the weight coefficient; and a smooth processing part 24 for calculating a smoothing value of each tracking target based on the centroids of respective measurement values calculated by the angle reference centroid calculation part 22 and a prediction value for each tracking target.

Description

  The present invention relates to a tracking device that performs tracking by acquiring observation values of motion specifications for a plurality of adjacent tracking targets via a sensor and calculating a smooth value and a predicted value for the tracking target.

2. Description of the Related Art Conventionally, a tracking device that tracks a plurality of adjacent tracking targets using a radar sensor is known (see, for example, Non-Patent Document 1). In the tracking device disclosed in Non-Patent Document 1, first, the reliability of each hypothesis indicating the correspondence between each tracking target and a plurality of observed values obtained in the tracking gate is calculated. At this time, as the reliability of each hypothesis, a weighting coefficient for each observed value is calculated based on the distance between the predicted value (predicted position) of each tracking target and each observed value (observed position). Then, a weighted average value of each observation value is calculated based on each reliability, and a plurality of target tracking is performed by associating with each tracking target.
Here, among the plurality of targets, for example, when tracking the i-th target, the observed value number within the tracking gate and N _i, the observed value _{z i, j (j = 1} , ···, N _i ), and the reliability when these observation values are the i-th tracking target is β _{i, j} (j = 1,..., N _i ), each associated with the i-th tracking target. The weighted average value of the observed values is expressed by equation (1).

Fortmann, Bar-Shalom and Scheffe, “Multi-Target Tracking Using Joint Probabilistic Data Association,” Proc. of the IEEE Conf. on Decision and Control, 2, pp. 807-812, Feb. , 1980.

Here, in the conventional tracking apparatus disclosed in Non-Patent Document 1, although the weighted average value of N _i number of observations as in the above equation (1) in association with the i-th tracking target, the weighting The average value is always located near the middle of all observed values in the tracking gate. For this reason, the conventional tracking device can maintain the tracking, but there is a problem that the tracking accuracy for each tracking target cannot be improved even when the distance between the radar sensor and the target becomes small. It was.

  The present invention has been made to solve the above-described problems, and detects a plurality of observation values in a tracking device that performs tracking by acquiring observation values for a plurality of adjacent tracking targets via a sensor. If the distance between the sensor to be detected and the target is large, tracking maintenance performance can be achieved by stabilizing the position of the tracking target candidate, and if the distance between the sensor and the target is small, An object of the present invention is to provide a tracking device that can improve the tracking accuracy for each tracking target by associating each observed value with each tracking target.

  The tracking device according to the present invention includes an angle calculation unit that calculates an angle formed by the two observation values with the sensor position as a reference point when two observation values are acquired via the sensor, and an angle calculation unit. An angle reference for calculating a weighting coefficient for the two observation values based on the angle formed by the two observation values and calculating a centroid between the observation values for the observation values based on the weighting coefficient. A centroid calculating unit and a smoothing processing unit that calculates a smooth value for each tracking target based on the centroid of each observation value calculated by the angle reference centroid calculating unit and the predicted value for each tracking target.

  According to the present invention, since it is configured as described above, when the distance between the sensor and the target is large and the angle formed by the two observation values obtained by the sensor is small, the position as the tracking target candidate is stabilized. Tracking can be maintained for each tracking target, and when the distance between the sensor and the target is small and the observation value obtained by the sensor is large, it is possible to associate each observation value with each tracking target. In addition, the tracking accuracy for each tracking target can be improved.

It is a block diagram which shows an example of a structure of the tracking apparatus which concerns on Embodiment 1 of this invention. It is a flowchart which shows the flow of the tracking process by the tracking apparatus which concerns on Embodiment 1 of this invention. It is explanatory drawing which shows the observation condition which the tracking apparatus which concerns on Embodiment 1 of this invention assumes. It is explanatory drawing which showed an example of the function used in Embodiment 1 of this invention. It is the conceptual diagram which showed the effect implement | achieved by the tracking apparatus which concerns on Embodiment 1 of this invention. It is a block diagram which shows an example of a structure of the tracking apparatus which concerns on Embodiment 2 of this invention. It is a flowchart which shows the flow of the tracking process by the tracking apparatus which concerns on Embodiment 2 of this invention. It is the conceptual diagram which showed the effect implement | achieved by the tracking apparatus which concerns on this Embodiment 2 of this invention. It is a block diagram which shows an example of a structure of the tracking apparatus which concerns on Embodiment 3 of this invention. It is a flowchart which shows the flow of the tracking process by the tracking apparatus which concerns on Embodiment 3 of this invention. It is explanatory drawing which shows the observation condition which the tracking apparatus which concerns on Embodiment 3 of this invention assumes.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1 FIG.
1 is a block diagram showing an example of the configuration of a tracking device according to Embodiment 1 of the present invention.
The tracking device acquires observation values for a plurality of adjacent tracking targets via a radar sensor (not shown), and performs tracking by calculating a smooth value and a predicted value for the tracking target. As shown in FIG. 1, the tracking device includes a target observation unit 1 and a tracking unit 2. In the tracking device according to the first embodiment described below, description will be made assuming a situation in which observation values for two tracking targets are detected by a radar sensor, as shown in FIG.
The radar sensor may be any radar sensor that can detect an observation value such as the position and speed of the tracking target. For example, a radar for ranging or a radar for measuring speed using a pulse Doppler frequency may be used. Use. Further, an optical sensor such as an infrared sensor, an image radar, or the like may be used.

  The target observation means 1 performs signal processing by a known method on a reception signal acquired from a radar sensor via an antenna / reception system (not shown) to generate two observation values. Information indicating each observation value generated by the target observation unit 1 is output to the angle calculation unit 21 in the tracking unit 2.

  The tracking unit 2 performs a tracking process on the tracking target based on each observation value generated by the target observation unit 1. The tracking unit 2 includes an angle calculation unit 21, an angle reference gravity center calculation unit 22, a prediction processing unit 23, a smoothing processing unit 24, and a smooth specification file 25.

  The angle calculation unit 21 calculates an angle formed by two observation values with the radar sensor position as a reference point, based on the two observation values generated by the target observation unit 1. Information indicating the angle formed by the two observation values calculated by the angle calculation unit 21 is output to the angle reference gravity center calculation unit 22.

  The angle reference center-of-gravity calculation unit 22 calculates a weighting coefficient for the two observation values based on the angle formed by the two observation values calculated by the angle calculation unit 21, and based on the weighting coefficient, The center of gravity between the observed values is calculated. Information indicating the centroid of each observation value calculated by the angle reference centroid calculation unit 22 is output to the smoothing processing unit 24.

  The prediction processing unit 23 acquires information indicating the smoothed values (smooth position and smoothing speed) of each tracking target at the observation time before one observation period from the smoothing specification file 25, and based on the smoothed values, the current observation time The predicted value (predicted position and predicted speed) of each tracking target is calculated. In addition, the prediction processing unit 23 obtains the smoothing error covariance matrix of each tracking target at the observation time one observation period before from the smoothing specification file 25, and based on this smoothing error covariance matrix, A prediction error covariance matrix of the tracking target is calculated. Information indicating the predicted value and prediction error covariance matrix of each tracking target at the current observation time calculated by the prediction processing unit 23 is output to the smoothing processing unit 24.

  The smoothing processing unit 24 is based on the centroid of each observation value calculated by the angle reference centroid calculation unit 22 and the predicted value of each tracking target at the current observation time calculated by the prediction processing unit 23 at the current observation time. The smooth value of each tracking target is calculated. Further, the smoothing processing unit 24, based on the prediction error covariance matrix at the current observation time calculated by the prediction processing unit 23, and the Kalman gain for each tracking target, the smoothing error covariance of each tracking target at the current observation time. Calculate the matrix. Information indicating the smoothing value and smoothing error covariance matrix of each tracking target at the current observation time calculated by the smoothing processing unit 24 is output to the smoothing specification file 25.

  The smoothing specification file 25 stores information indicating the smoothing value and smoothing error covariance matrix of each tracking target at the current observation time calculated by the smoothing processing unit 24. In addition, the smoothing specification file 25 outputs, to each prediction time, information indicating the smoothing value and smoothing error covariance matrix of each tracking target at the stored observation time before one observation period at each observation time.

Next, a flow of tracking processing by the tracking device configured as described above will be described. FIG. 2 is a flowchart showing the flow of tracking processing by the tracking device according to Embodiment 1 of the present invention. In the following description, as shown in FIG. 3, description will be made assuming a situation in which observation values (observation positions) z ₁ and z ₂ for two tracking targets are detected by a radar sensor.

In the tracking processing by the tracking device, as shown in FIG. 2, first, the target observation means 1 performs signal processing by a known method on the received signal acquired from the radar sensor via the antenna / reception system. Two observation values z ₁ and z ₂ are generated (step ST21). Information indicating the observation values z ₁ and z ₂ generated by the target observation means 1 is output to the angle calculation unit 21.

Then, the angle calculating unit 21, based on the target observation means each observation z ₁ generated by _1, z _2, the angle θ of the two on the basis point radar sensor position observations z _1, z ₂ Calculate (step ST22). Information indicating the angle θ formed by the two observation values z ₁ and z ₂ calculated by the angle calculation unit 21 is output to the angle reference gravity center calculation unit 22.

この角度基準重心算出部２２により算出された各観測値ｚ_１，ｚ_２の重心ｚ_１θ，ｚ_２θを示す情報は平滑処理部２４に出力される。 Next, the angle reference center-of-gravity calculation unit 22 uses the function f (θ) whose variable is the angle θ formed by the two observation values z ₁ and z ₂ calculated by the angle calculation unit 21, and uses the two observation values z. _1, calculates the weight factor for _{z 2.} Based on this weighting factor, the centroids z _1θ and z _2θ between the observed values z ₁ and z _{2 with} respect to the observed values z ₁ and z ₂ are calculated from the equation (2) (step ST23).

Information indicating the centroids z _1θ and z _2θ of the observation values z ₁ and z ₂ calculated by the angle reference centroid calculation unit 22 is output to the smoothing processing unit 24.

Here, as a function f (θ) used when calculating a weighting coefficient for each of the observation values z ₁ and z ₂ , for example, as shown in FIG. 4, an angle θ formed by _two observation values z ₁ and z ₂ is set. Use functions that change accordingly. The function f (θ) is not limited to the function shown in FIG. 4, and other functions may be used. The same applies to the following embodiments.

By the angle reference gravity center calculation process in step ST23, as shown in FIG. 5A, the distance between the radar sensor and each tracking target is large, and two observation values z ₁ and z ₂ (the positions marked with x in FIG. 5). Is small, the centroids z _1θ and z _2θ (the positions marked by □ and ■ in FIG. 5) of the observation values z ₁ and z ₂ associated with the tracking targets are the observation values z ₁ and z. _{It is in the} middle position with respect to ₂ . Therefore, the position that becomes a tracking target candidate can be stabilized, and tracking can be maintained for each tracking target.
Further, as shown in FIG. 5B, as the distance between the radar sensor and each tracking target becomes smaller and the angle θ formed by the _two observation values z ₁ and z ₂ becomes larger, each tracking target is supported. each observation _z 1, _{z 2} of the center of gravity _{z 1Shita} to be attached, _{z 2 [Theta]} approaches each observation _z 1, _{z 2} side. Therefore, tracking accuracy for each tracking target can be improved.
Note ○ in FIG. 5, ● mark positions are predicted position x of each tracking target at the current measurement time _{t k (hat) i, k} | a k-1 (i = 1,2) .

On the other hand, the prediction processing unit 23 performs a prediction process (step ST24). Specifically, the prediction processing unit 23 firstly smoothes each smoothing target (smooth position and smoothing speed) x (hat) _{i, k−1 | k− at} the observation time tk ₋₁ one observation period before. Information indicating ₁ (i = 1, 2) is acquired from the smooth specification file 25. Then, the smoothed value x _{(hat) i, k-1 |} based on _k-1, equation (3) the prediction value of each tracking target at the current observation time _{t k} from the (predicted position and predicted velocity) x (hat) _{i, k | k−1} is calculated.

Next, the prediction processing unit 23 acquires the smoothing error covariance matrix of each tracking target from the smoothing specification file 25 at the time tk ₋₁ one observation period before. Then, based on the error covariance matrix to calculate the prediction error covariance matrix of each tracking target at the current measurement time t _k.
Each predicted value x (hat) _i of the tracking _{target, k} at the current measurement time t _k which is calculated by the prediction processing section 23 _| information indicating the _k-1 and a prediction error covariance matrix is outputted to the smoothing processing section 24 .

Next, the smoothing processing unit 24 performs smoothing processing (step ST25). Specifically, the smoothing processing unit 24 first calculates the current observations calculated by the prediction processing unit 23 using the centroids z _1θ and z _2θ of the observation values z ₁ and z ₂ calculated by the angle reference centroid calculation unit 22. The Kalman gain K _{i, k} (i = 1, 2) for each tracking target is calculated in association with the predicted value x (hat) _{i, k | k−1} of each tracking target at time t _k . The current observation time _t smoothed value of each tracking target in _k x _{(hat) i, k} according to the Kalman filter algorithm expressed by the equations (4) _| calculates the _k-1.

Next, the smoothing processing unit 24 smoothes each tracking target at the current observation time t _k based on the prediction error covariance matrix calculated by the prediction processing unit 23 and the Kalman gain K _{i, k} for each tracking target. Calculate the error covariance matrix.
The smoothing processing unit of the tracking target in the current observation time t _k which is calculated by 24 smoothed value x (hat) _{i, k |} information indicating the _k-1 and error covariance matrix is outputted to the smoothing specifications file 25 Is remembered.

Then, smoothing specifications file 25 is 1 at the observation time t _{k + 1} after the observation period, smoothed value x (hat) _i of each tracking target at the measurement time t _k which is _{stored, k | k} and error covariance matrix Is output to the prediction processing unit 23 (step ST26).

  As described above, according to the first embodiment, the weighting coefficient for the two observation values is calculated based on the angle formed by the two observation values with the laser sensor position as the reference point, and based on the weighting coefficient. Since the calculated gravity center of each observation value is configured to be associated with each tracking target, if the distance between the radar sensor and each tracking target is large and the angle between the two observation values is small, the position as a tracking target candidate Can be tracked for each tracking target, and when the distance between the radar sensor and each tracking target is small and the angle between the two observations is large, a plurality of observation values are obtained. By associating each with each tracking target, the tracking accuracy with respect to each tracking target can be improved.

Embodiment 2. FIG.
FIG. 6 is a block diagram showing an example of the configuration of the tracking device according to Embodiment 2 of the present invention. The configuration of the tracking device according to the second embodiment shown in FIG. 6 is obtained by adding a weighted average unit 26b to the tracking device according to the first embodiment shown in FIG. 1, and the description of the same configuration is omitted. . In the tracking device according to the second embodiment described below, the description will be made on the assumption that the observed values for the two tracking targets are detected by the radar sensor, as shown in FIG.

  The weighted average unit 26b calculates each observation value for each tracking target based on each observation value generated by the target observation unit 1b and the predicted value of each tracking target at the current observation time calculated by the prediction processing unit 23b. The reliability with respect to is calculated. Further, the weighted average unit 26b calculates a weighted average value of each observation value for each tracking target based on each observation value generated by the target observation means 1 and the reliability for each calculated observation value. Information indicating the weighted average value for each tracking target calculated by the weighted average unit 26b is output to the angle reference centroid calculating unit 22b.

  Note that the angle reference centroid calculation unit 22b is based on a function that uses the angle formed by the two observation values calculated by the angle calculation unit 21b as a variable by the same processing as the angle reference centroid calculation unit 22 in the first embodiment. The weighting coefficient for these two observation values is calculated. Further, the angle reference centroid calculating unit 22b calculates the centroid with reliability of each observation value based on the weighted average value for each tracking target calculated by the weighted average unit 26b and the calculated weighting factor for each observation value. To do. Information indicating the centroid with reliability of each observation value calculated by the angle reference centroid calculating unit 22b is output to the smoothing processing unit 24b.

Next, a flow of tracking processing by the tracking device configured as described above will be described. FIG. 7 is a flowchart showing the flow of tracking processing by the tracking device according to Embodiment 2 of the present invention. In the following description, as shown in FIG. 3, description will be made assuming a situation in which observation values (observation positions) z ₁ and z ₂ for two tracking targets are detected by a radar sensor.

In the tracking processing by the tracking device, as shown in FIG. 7, first, the target observation unit 1b receives the reception acquired from the radar sensor via the antenna / reception system by the same processing as the target observation unit 1 in the first embodiment. Two observation values z ₁ and z ₂ are generated from the signal (step ST71). Information indicating the observed values z ₁ and z ₂ generated by the target observation unit 1b is output to the angle calculation unit 21b and the weighted average unit 26b.

Next, the angle calculation unit 21b uses the radar sensor position as a reference point based on the observation values z ₁ and z ₂ generated by the target observation unit 1b by the same processing as the angle calculation unit 21 in the first embodiment. An angle θ formed by the two observed values z ₁ and z ₂ is calculated (step ST72). Information indicating the angle θ formed by the two observation values z ₁ and z ₂ calculated by the angle calculation unit 21b is output to the angle reference gravity center calculation unit 22b.

On the other hand, the weighted average unit 26b performs an observed value weighted average calculation process (step ST73). Specifically, the weighted average unit 26b, first, each observation z ₁ generated by the target observation means _1b, z _2, and, for each tracked target at the current measurement time t _k which is calculated by the prediction processing section 23b Based on the predicted value (predicted position and predicted speed) x (hat) _{i, k | k−1} (i = 1, 2), the reliability β _i, for each observed value z ₁ , z ₂ for each tracking target _j (i = 1, 2, j = 1, 2) is calculated. Here, for example, using a function whose variable is the distance between the predicted position x (hat) _{i, k | k−1} of each tracking target and the observed values z ₁ and z ₂ at the current observation time t _k , ), The reliability β _{i, j} for each observation value z ₁ , z ₂ is calculated for each tracking target.

Here, in the above equation (5), each predicted position x (hat) _{i, k | k} is a function used when calculating the reliability β _{i, j} for each observation value z ₁ , z ₂ for each tracking target. _Although a function using a distance between ₋₁ and each of the observed values z ₁ and z ₂ as a variable is used, the present invention is not limited to this, and other functions may be used.

この加重平均部２６ｂにより算出された追尾目標ごとの加重平均値ｚ（バー）_１，ｚ（バー）_２を示す情報は角度基準重心算出部２２ｂに出力される。 Then, the weighted average unit 26b, each observation _z 1 generated by the target observation means 1b, _{z 2,} and the reliability beta _{i, j} for each observation _z 1, _{z 2} per calculated tracking target based on, from equation (6), for each tracked target, to calculate a weighted average value z _(bar) 1, z _{(bar) 2} each observation _z 1, _{z 2.}

Information indicating the weighted average values z (bar) ₁ and z (bar) ₂ for each tracking target calculated by the weighted average unit 26b is output to the angle reference centroid calculating unit 22b.

この角度基準重心算出部２２ｂにより算出された各観測値ｚ_１，ｚ_２の信頼度付き重心ｚ（バー）_１θ，ｚ（バー）_２θを示す情報は平滑処理部２４ｂに出力される。 Next, the angle reference centroid calculation unit 22b changes the angle θ formed by the two observation values z ₁ and z ₂ calculated by the angle calculation unit 21b by the same processing as the angle reference centroid calculation unit 22 in the first embodiment. Is used to calculate a weighting coefficient for each of the observed values z ₁ and z ₂ . Based on the weighted average value z (bar) ₁ and z (bar) ₂ for each tracking target calculated by the weighting coefficient and the weighted average unit 26b, the observed values z ₁ and z ₂ are calculated from the equation (7). The center of gravity z (bar) _1θ and z (bar) _2θ with reliability are calculated (step ST74).

Information indicating the centroids z (bar) _1θ and z (bar) _2θ with reliability of the observation values z ₁ and z ₂ calculated by the angle reference centroid calculation unit 22b is output to the smoothing processing unit 24b.

By the angle reference gravity center calculation process in step ST74, as shown in FIG. 8A, the distance between the radar sensor and each tracking target is large, and two observation values z ₁ and z ₂ (the positions marked with x in FIG. 8). Is small, the center of gravity z (bar) _1θ , z (bar) _2θ with reliability of each observation value z ₁ , z ₂ associated with each tracking target (□, ■ mark positions in FIG. 8) ) Is a position closer to the middle between the two weighted average values z (bar) ₁ and z (bar) ₂ (the positions of Δ and ▲ in FIG. 8). Therefore, the position that becomes a tracking target candidate can be stabilized, and tracking can be maintained for each tracking target.
Further, as shown in FIG. 8B, as the distance between the radar sensor and each tracking target becomes smaller and the angle θ formed by the _two observation values z ₁ and z ₂ becomes larger, it corresponds to each tracking target. The centroids z (bars) _1θ and z (bars) _2θ with reliability of the observed values z ₁ and z ₂ to be attached _approach the respective weighted average values z (bars) ₁ and z (bars) ₂ . Therefore, tracking accuracy for each tracking target can be improved.
Note ○ in FIG. 8, ● mark positions are predicted position x of each tracking target at the current measurement time _{t k (hat) i, k} | a k-1 (i = 1,2) .

On the other hand, the prediction processing section 23b performs prediction processing by the same processing as the prediction processing unit 23 according to the first embodiment, the predicted value x (hat) _i of each tracking target at the current measurement time t _{k, k | k-1} Then, a prediction error covariance matrix is calculated (step ST75). Predicted value x (hat) _{i, k} of the tracking target in the current observation time t _k which is calculated by the prediction processing section 23b _{| k-1} and the information indicating the prediction error covariance matrix smoothing processing unit 24b and the weighted averager 26b.

Then, the smoothing processing unit 24b performs smoothing processing by the same processing as the smoothing processing section 24 in the first embodiment, the smoothing value x (hat) _i of each tracking target at the current measurement time t _{k, k | k} and smooth An error covariance matrix is calculated (step ST76). Smoothed value of each tracking target at the current measurement time t _k which is calculated by the smoothing processing unit 24b x (hat) _{i, k |} information indicating _k and error covariance matrix is outputted to the smoothing specification file 25b stores Is done.

Then, smoothing specifications file 25b carries out the same processing as smoothing specifications file 25 in the first embodiment, in one observation time t _{k + 1} after the period, the smoothing value x of each tracking target at time t _k which is stored (Hat) Information indicating _{i, k | k} and the smoothing error covariance matrix is output to the prediction processing unit 23b (step ST77).

  As described above, according to the second embodiment, the reliability with respect to each observation value is calculated for each tracking target, and the center of gravity with the reliability of each observation value is calculated based on this reliability to each tracking target. Since the distance between the radar sensor and each tracking target is large and the angle formed by the two observation values is small with respect to the effect in the first embodiment, the position that becomes the tracking target candidate is further increased. When the distance between the radar sensor and each tracking target is small and the angle formed by the two observation values is large, the tracking accuracy for each tracking target can be further improved.

Embodiment 3 FIG.
FIG. 9 is a block diagram showing an example of the configuration of the tracking device according to Embodiment 3 of the present invention. The configuration of the tracking device according to the third embodiment shown in FIG. 9 is obtained by changing the angle calculation unit 21b of the tracking device according to the second embodiment shown in FIG. 6 to a multi-target angle calculation unit 21c. The description of is omitted. Note that the tracking device according to Embodiment 3 described below is described assuming a situation in which observed values for N tracking targets are detected by a radar sensor, as shown in FIG.

  The target observation means 1c performs signal processing by a known method on the received signal acquired from the radar sensor via an antenna / reception system (not shown), and generates N observation values. Information indicating each observation value generated by the target observation means 1c is output to the multi-target angle calculation unit 21c.

  The multi-target angle calculation unit 21c calculates an angle formed by two observation values with the laser sensor position as a reference point for all combinations based on N observation values generated by the target observation unit 1c. . Information indicating the angle formed by the two observation values in each combination calculated by the multi-target angle calculation unit 21c is output to the angle reference centroid calculation unit 22c.

Next, a flow of tracking processing by the tracking device configured as described above will be described. FIG. 10 is a flowchart showing a flow of tracking processing by the tracking device according to Embodiment 3 of the present invention. In the following description, as shown in FIG. 11, a description will be given assuming a situation in which observation values z _j (j = 1, 2,..., N) for N tracking targets are detected by a radar sensor.

In the tracking processing by the tracking device, as shown in FIG. 10, first, the target observation means 1c performs signal processing by a known method on the received signal acquired from the radar sensor via the antenna / reception system, and N The observed values z _j are generated (step ST101). Information indicating each observation value z _j generated by the target observation means 1c is output to the multi-target angle calculation unit 21c and the weighted average unit 26c.

Next, the multi-target angle calculation unit 21c, based on the N observation values z _j generated by the target observation unit 1c, forms an angle θ _{j, m} (j between two observation values with the laser sensor position as a reference point. = 1, 2,..., N, m = 1, 2,..., N) are calculated for all combinations (step ST102). Information indicating the angle θ _{j, m} formed by the two observation values in each combination calculated by the multi-target angle calculation unit 21c is output to the angle reference centroid calculation unit 22c.

On the other hand, the weighted average unit 26c performs an observation value weighted average calculation process (step ST103). Specifically, the weighted average unit 26c, first, the target observation means 1c each observation z _j is generated _by, and the predicted values of the tracking target in the current observation time t _k which is calculated by the prediction processing section 23c ( predicted position and predicted velocity) x _{(hat) i, k | k-1} (i = 1,2, ···, based on N), each tracking target, reliability beta _i for each observation _{z j, j} (i = 1, 2,..., N j = 1, 2,..., N) is calculated. Here, for example, current observation time t predicted position of each tracking target in _k x (hat) _{i, k |} using a function of the distance between the _k-1 and the observation value z _j and variable, from equation (8), calculates the reliability beta _{i, j} for each observation z _j for each tracking target.

Here, in the above equation (8), each predicted position x (hat) _{i, k | k−1} is a function used when calculating the reliability β _{i, j} for each observation value z _j for each tracking target. A function using the distance from each observation value z _j as a variable is used, but the present invention is not limited to this, and other functions may be used.

この加重平均部２６ｃにより算出された追尾目標ごとの加重平均値ｚ（バー）_ｉを示す情報は角度基準重心算出部２２ｃに出力される。 Next, the weighted average unit 26c calculates the equation (9) based on the observation values z _j generated by the target observation unit 1c and the reliability β _{i, j} for the calculated observation values z _j for each tracking target. ), The weighted average value z (bar) _i (i = 1, 2,..., N) of each observation value z _j is calculated for each tracking target.

Information indicating the weighted average value z (bar) _i for each tracking target calculated by the weighted average unit 26c is output to the angle reference centroid calculating unit 22c.

この角度基準重心算出部２２ｃにより算出された各観測値ｚ_ｊの信頼度付き重心ｚ（バー）_ｉ，θを示す情報は平滑処理部２４ｃに出力される。 Next, the angle reference centroid calculation unit 22c is a function having the angle θ formed by the two observation values calculated by the multi-target angle calculation unit 21c as a variable by the same processing as the angle reference centroid calculation unit 22 in the first embodiment. Using f (θ), weighting coefficients for these two observation values are calculated for all combinations. Then, based on these weighting factors and the weighted average value z (bar) _i for each tracking target calculated by the weighted average unit 26c, the center of gravity z (bar) with reliability of each observation value z _j from Equation (10). _{i and θ} are calculated (step ST104).

Information indicating the centroids z (bars) _{i, θ} with reliability of the respective observation values z _j calculated by the angle reference centroid calculating unit 22c is output to the smoothing processing unit 24c.

When the distance between the radar sensor and each tracking target is large and the angle θ formed by the two observation values is small by the angle reference centroid calculation process in step ST104, each observation value z _j associated with each tracking target is calculated. The center of gravity z (bar) _{i, θ} with reliability is a position closer to the middle of each weighted average value z (bar) _i . Therefore, the position that becomes a tracking target candidate can be stabilized, and tracking can be maintained for each tracking target.
Further, as the distance between the radar sensor and each tracking target decreases and the angle θ formed by the two observation values increases, the center of gravity z (bar with reliability) of each observation value z _i associated with each tracking target. ) _{I and θ} approach each weighted average value z (bar) _i side. Therefore, tracking accuracy for each tracking target can be improved.

On the other hand, the prediction processing unit 23c performs a prediction process (step ST105). Specifically, the prediction processing unit 23c firstly smoothes each tracking target (smooth position and smoothing speed) x (hat) _{i, k-1 | k-1} at time tk _-1 one observation period before. Information indicating (i = 1, 2,..., N) is acquired from the smooth specification file 25c. Then, the smoothed value x _{(hat) i, k-1 |} based on _k-1, wherein the predicted value of each tracking target at the current observation time _{t k} from (11) (predicted position and predicted velocity) x (hat) _{i, k | k−1} (i = 1, 2,..., N) is calculated.

Next, the prediction processing unit 23c calculates a prediction error covariance matrix for each tracking target by the same process as the prediction processing unit 23 in the first embodiment.
Predicted value x (hat) _{i, k} of the tracking target in the current observation time t _k which is calculated by the prediction processing section 23c _{| k-1} and the information indicating the prediction error covariance matrix smoothing processing section 24c and the weighted averager 26c.

Next, the smoothing processor 24c performs a smoothing process (step ST106). Specifically, the smoothing processing unit 24c first calculates the centroid z (bar) _{i, θ} with reliability of each observation value z _j calculated by the angle reference centroid calculating unit 22c from the prediction processing unit 23c. Kalman gain K _{i, k} (i = 1, 2,..., N) corresponding to each tracking target in correspondence with the predicted value x (hat) _{i, k | k−1} of each tracking target at the current observation time t _k Is calculated. The current observation time _t smoothed value of each tracking target in _k x _{(hat) i, k} according to the Kalman filter algorithm expressed by the equations (12) _| calculates the _k-1.

Next, the smoothing processing unit 24c smoothes each tracking target at the current observation time t _k based on the prediction error covariance matrix calculated by the prediction processing unit 23c and the Kalman gain K _{i, k} for each tracking target. Calculate the error covariance matrix.
The smoothed values of the tracking target in the current observation time t _k which is calculated by the smoothing processing section 24c x (hat) _{i, k |} information indicating the _k-1 and error covariance matrix is outputted to the smoothing specifications file 25c Is remembered.

Then, smoothing specifications file 25c carries out the same processing as smoothing specifications file 25 in the first embodiment, in one observation time t _{k + 1} after the period, the smoothing value x of each tracking target at time t _k which is stored (Hat) Information indicating _{i, k | k−1} and the smoothing error covariance matrix is output to the prediction processing unit 23c (step ST107).

  As described above, according to the third embodiment, based on the N observation values, the angles formed by the two observation values based on the laser sensor position are calculated for all combinations, and these angles are calculated. Since the weighting factor is calculated from this, and the centroid with reliability of each observation value calculated based on this weighting factor is associated with the tracking target, the distance between the radar sensor and each tracking target is large, and the two observation values If the angle between the tracking target is small, the tracking target can be maintained by stabilizing the position of the tracking target candidate, the distance between the radar sensor and each tracking target is small, and the angle formed by the two observation values When is large, it is possible to improve the tracking accuracy with respect to each tracking target by associating each of the obtained observation values with each tracking target.

The tracking device according to the third embodiment shown in FIG. 9 is obtained by changing the angle calculation unit 21b of the tracking device according to the second embodiment to a multi-target angle calculation unit 21c, but according to the first embodiment. The same applies to the case where the angle calculation unit 21 of the tracking device is changed to the multi-target angle calculation unit 21c.
In the present invention, within the scope of the invention, any combination of the embodiments, or any modification of any component in each embodiment, or omission of any component in each embodiment is possible. .

  1, 1b, 1c target observation means, 2, 2b, 2c tracking means, 21, 21b angle calculation section, 21c multi-target angle calculation section, 22, 22b, 22c angle reference centroid calculation section, 23, 23b, 23c prediction processing section , 24, 24b, 24c Smoothing processing unit, 25, 25b, 25c Smoothing specification file, 26b, 26c Weighted average unit.

Claims (4)

In a tracking device that performs tracking by obtaining observed values for a plurality of tracking targets that are close to each other via a sensor and calculating a smooth value and a predicted value for the tracking target based on the observed values,
An angle calculation unit that calculates an angle formed by the two observation values with the sensor position as a reference point when two observation values are acquired via the sensor;
A weighting coefficient for the two observation values is calculated based on the angle formed by the two observation values calculated by the angle calculation unit, and the center of gravity between the observation values of the observation values is calculated based on the weighting coefficient. Angle reference center of gravity calculation unit for calculating each
A tracking apparatus comprising: a smoothing unit that calculates a smooth value for each tracking target based on the center of gravity of each observation value calculated by the angle reference centroid calculation unit and the predicted value for each tracking target. The angle reference center-of-gravity calculation unit uses a function having an angle formed by the two observation values as a variable, and the center of gravity of each observation value increases from the intermediate side of each observation value as the angle increases. The tracking device according to claim 1, wherein the weighting coefficient that approaches the side is calculated. For each tracking target, a reliability for each observation value is calculated based on the predicted value, and a weighted average unit that calculates a weighted average value for each observation value based on the reliability is provided.
The angle reference centroid calculating unit calculates a centroid with reliability of each observation value based on a weighting factor for each observation value and a weighted average value for each tracking target calculated by the weighted average unit. The tracking device according to claim 1 or 2. When the angle calculation unit acquires three or more observation values via the sensor, the angle calculation unit calculates the angle formed by the two observation values with the sensor position as a reference point for all combinations,
The angle reference center-of-gravity calculation unit calculates a weighting factor for the two observation values based on an angle formed by the two observation values calculated by the angle calculation unit for all combinations, and based on each weighting factor, 4. The tracking device according to claim 1, wherein a center of gravity of each observation value acquired via the sensor is calculated between the observation values. 5.

Images