JP2000111634A - Target position observation device - Google Patents

Abstract

(57) [Summary] When observing the same target position using three sensors, each sensor has an observation error. An object of the present invention is to accurately estimate and calculate the position of a target by estimating the observation error. SOLUTION: An observation matrix generator 4 for generating an observation matrix from a target observation position is provided, and an estimation value evaluator 5 for obtaining a covariance matrix of an observation error estimation value and an estimation value calculator 6 for calculating an estimation value. Is provided to estimate the observation error of the sensor, and then the target position is calculated.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a target position observing apparatus for observing a distance from a sensor to a target by using three sensors and estimating and calculating the position of the target. The present invention relates to an apparatus for estimating and calculating a target position in consideration of an error to be taken.

[0002]

2. Description of the Related Art First, a target position observing apparatus to which the present invention is applied will be described with reference to FIG. FIG. 6 shows an example of a configuration of a target position observing device including three sensors (for example, radar). In FIG. 6, reference numeral 1 denotes a first sensor that observes a target position and outputs a distance to the target position; A second sensor, which is installed at a location distant from the first sensor and outputs the distance to the target position by observing the target position, 3 is installed at a location distant from the first sensor 1 and the second sensor 2 A third sensor that observes the position of the target and outputs the distance to the target position, 12 is the target to be observed, 13 is the target position calculation result, 20 is the distance to the target 12 observed by the first sensor 1, Reference numeral 21 denotes a distance to the target 12 observed by the second sensor 2, and reference numeral 22 denotes a distance to the target 12 observed by the third sensor 3. When a target observation device having three sensors is configured as described above and observes the distance from each sensor of the same target, the observation value includes an observation error of the distance.
When the distance from the first sensor 1 to the target observed by the third sensor 3 is drawn as circles centered on the respective sensors, the three circles do not become one intersection.
Therefore, in the target position observation device as described above, when the target position is calculated, the calculated target position is inaccurate.

FIG. 7 is a diagram showing a conventional target position observation device. 6 are the same as those in FIG. 14 is a first intersection calculator that calculates the intersection of circles from the distances of the target positions from the first sensor 1 and the second sensor 2 and outputs the first target position, 15 is the first target position, 16 is the second sensor 2
And a second intersection calculator that calculates the intersection of the circles from the distance of the target position from the third sensor 3 and outputs a second target position, 17 is a second target position, and 18 is a first target position. Fifteen
An average calculator for calculating the average of the second target position 17 and
Reference numeral 19 denotes an output of the average value calculator 18 which is a target position.

Next, the operation will be described. In FIG. 7, the conventional device observes the target position with the first sensor 1, the second sensor 2, and the third sensor 3 and calculates the target distance from the first sensor 1 and the second sensor 2 in FIG. The second sensor 2 and the third sensor 3 are output to the first intersection calculator 14.
Is output to the second intersection calculator 16.
The first intersection calculator 14 calculates the target position as the intersection of a circle centered on the first sensor 1 and the second sensor 2 from the distance between the first sensor 1 and the second sensor 2. Then, the first target position 15 is output. The second intersection calculator 16 calculates the target position from the distance between the second sensor 3 and the third sensor 3 to the target position as the intersection of a circle centered on the second sensor 3 and the third sensor 3. Second
Is output as the target position 17. The average calculator 18 calculates the average of the first target position 15 and the second target position 17 from the first intersection calculator 14 and the second intersection calculator 16 and outputs the average as the target position 19. I do.

[0005]

Since the conventional target position observing apparatus is configured as described above, the distance from the sensor to the target on which the noise component during observation is superimposed is output to the observation value from the sensor. Therefore, there is a problem that the reliability of the target position calculated by the influence of the distance error is low.

SUMMARY OF THE INVENTION The present invention has been made to solve the above-described problems. An error included in an observation distance to a target by a first sensor, a second sensor, and a third sensor is determined by calculating the error of the target position. The purpose is to incorporate it into the estimation formula.

[0007]

According to a first aspect of the present invention, there is provided a target position observing apparatus for forming an observation matrix from target observation positions from a first sensor, a second sensor, and a third sensor. And the covariance matrix of the target estimated position is calculated from the covariance matrix, and the estimated value of the target position is calculated using the observation matrix of the target and the covariance matrix of the target estimated position.

A target position observing apparatus according to a second aspect of the present invention forms an observation matrix from target observation positions from a first sensor, a second sensor, and a third sensor, and obtains an observation matrix from a covariance matrix of an observation error and an observation error. Calculates the covariance matrix of the target estimated position, further sets the initial value when calculating the estimated value of the target position, and calculates the estimated value of the target position from the target observation matrix and the initial value of the estimated value. is there.

A target position observing apparatus according to a third aspect of the present invention forms an observation matrix from target observation positions from a first sensor, a second sensor, and a third sensor, and obtains an observation matrix from a covariance matrix of an observation error and an observation error. Calculates the covariance matrix of the target estimated position, stores the calculated estimated value of the target position, and estimates the target position using the target observation matrix, the covariance matrix of the estimated value, and the previously calculated estimated value of the target position. The value is calculated.

A target position observing apparatus according to a fourth aspect of the present invention forms an observation matrix from target observation positions from a first sensor, a second sensor, and a third sensor, and obtains an observation matrix from a covariance matrix of an observation error and an observation error. Calculate the covariance matrix of the target estimated position, further set an initial value when calculating the estimated value of the target position, and store the calculated estimated value of the target position. The estimated value of the target position is calculated from the observation matrix, the covariance matrix of the estimated value, and the initial value of the estimated value.

A target position observing apparatus according to a fifth aspect of the present invention forms an observation matrix from target observation positions from a first sensor, a second sensor, and a third sensor, and obtains an observation matrix from the observation matrix and a covariance matrix of observation errors. Calculate the covariance matrix of the target estimated position, set the initial value when calculating the estimated value of the target position, and calculate the estimated value of the target position from the target observation matrix, the covariance matrix of the estimated value, and the initial value. After the calculation, the degree of convergence of the calculated estimated value is determined.

[0012]

Embodiment 1 FIG. 1 is a configuration diagram of a target position observing apparatus according to Embodiment 1 of the present invention. 1 to 3 in the figure are the same as in the conventional apparatus. Reference numeral 4 denotes an observation matrix generator which inputs a distance to a target observed by the first sensor 1, the second sensor 2, and the third sensor 3 and calculates an observation matrix to be converted into a target position. An estimator estimator 6 for calculating a covariance matrix of the estimated value of the target position. An estimated value calculator 7 for calculating the estimated value is an estimated value of the target position.

Next, the operation will be described. The positions of the first sensor 1 and the second sensor 2 are represented by P1 (X
1, Y1), P2 (X2, Y2), P3 (X3, Y3)
The target observation value (distance to the target) obtained from each sensor at the time of the k-th observation is set to R1k, R2k, and R3k, respectively, where k is an integer of 1 or more.

The first sensor 1, the second sensor 2 and the third sensor
Radius R1k, R2k and R3k around the sensor 3 of
Equations on the xy plane of the circle are represented by Equations 1, 2, and 3, respectively.

[0015]

(Equation 1)

[0016]

(Equation 2)

[0017]

(Equation 3)

The position of the target calculated from the distance obtained from the sensor at the time of the k-th observation is the intersection of the circles represented by Equations 1 and 2 and the intersection of the circles represented by Equations 1 and 3 or It can be considered as the intersection of the circles represented by Equations 2 and 3.
If the intersections of Equations 1 and 2 and the intersections of Equations 1 and 3 match, the intersections of Equations 2 and 3 naturally match, so the intersection P12kt of Equations 1 and 2 and the intersection P13kt of Equations 1 and 3 Focus on it. Normally, observation value (distance) obtained from each sensor
Is superimposed with the observation error. The observation error is calculated as dR
1, dR2, dR3, the first sensor 1 and the second sensor 1
The position P12kto on the xy plane of the target obtained from the distance observed by the sensor 2 and the position P13kto on the xy plane obtained from the distance observed by the first sensor 1 and the third sensor 3 are respectively Equation 4 is calculated as an intersection of circles represented by Equations 1 and 2, and Equation 5 is calculated as an intersection of circles represented by Equations 1 and 3.

[0019]

(Equation 4)

[0020]

(Equation 5) Equation 6 clearly holds.

[0021]

(Equation 6)

Here, the function F (xfk, yfk) is defined as shown in Expression 7.

[0023]

(Equation 7)

The function F expressed by the equation (7) is converted to the first sensor 1
And the initial value of the observation error of the second sensor 2 and the third sensor 3 as d (0) = (dR1, dR2, dR3) = 0, and the Taylor around the point (P12kto, P13kto, d (0)) By expanding and ignoring terms of second and higher order, Equation 8 is obtained.

[0025]

(Equation 8)

The function F becomes zero if the observation values from the second sensor 3 and the third sensor 3 do not include an observation error. Therefore, Equation 9 is obtained by setting the right side of Equation 8 to zero.

[0027]

(Equation 9)

The left side of Equation 9 is considered to be an observation value determined by the initial value of the observation error of the second sensor 3 and the third sensor 3. The first term on the right side of Equation 9 is considered to be the product of the state variable and the observation matrix when the observation error of the sensor is used as the state variable,
The second and third terms are the product of the observation error of the sensor and the observation matrix, and Equation 9 can be considered as a linear state equation in which the observation system includes the observation error.

Now, if an observation value is Z, an observation matrix is H, a state variable to be obtained is x, and an observation error at the time of target observation is v, expression 10 is obtained.

[0030]

(Equation 10)

If the least squares method is used, the observed value and the state variable x
May be used as the evaluation function J. If the covariance of the observation error v at the time of the target observation is R, J is represented by Formula 11.

[0032]

[Equation 11]

The value x that minimizes the evaluation function J is such that the result of expanding the right side of Equation 11 and differentiating x becomes zero. Accordingly, the state variable x to be obtained is expressed by Expression 12.

[0034]

(Equation 12)

Here, P (+) is a covariance matrix of the estimated value.

Next, the present invention will be described with reference to FIG.
The first sensor 1, the second sensor 2, and the third sensor 3 are sensors for observing the same target and observing the distance to the target, and output the distance from each sensor to the target.
The observation matrix generator 4 receives the distances from the first sensor 1, the second sensor 2, and the third sensor 3 to the target, and calculates and obtains an observation matrix G1k expressed by Expression 8. In the estimated value evaluator 5, the observation matrix G1 from the observation matrix generator 4 is obtained.
P (+) in Equation 12 from k, that is, the covariance matrix of the estimated value is calculated. The estimated value calculator 6 calculates the second value on the right side of the estimated value x of the observation error represented by Expression 12 from the covariance matrix of the estimated value from the estimated value evaluator 5 and the observation matrix from the observation matrix generator 4.
The term is calculated, and the position obtained by adding the observation error is output as the target position. In the process of deriving Equation 12, d (0) = (0,
(0, 0), the output of the estimated value calculator 6 is the estimated value 7 of the target position to be obtained.

As described above, the first sensor 1, the second sensor 2, and the third sensor 3 are used by using the observed value of the distance to the target.
Since the observation error is estimated and the target position is calculated by assuming a state equation using the observation error as a state variable, the observation position is hardly affected by the observation error of the sensor included in the observation value.

Second Embodiment FIG. 2 is a configuration diagram of a target position observation device according to a second embodiment of the present invention. 1 to 7 in the figure are the same as those in the first embodiment. Reference numeral 8 denotes an initial value setting device for setting an initial value for performing estimation calculation of an observation error.

Next, the operation will be described. The operation performed by the first sensor 1, the second sensor 2, and the third sensor 3 to observe the distance to the target and calculate the covariance matrix of the observed error estimated value by the estimated value evaluator 5 is described in the embodiment. Same as 1. In the initial value setting unit 8, an initial value when the estimated value calculator 6 calculates the estimated value of the observation error, that is, d in Expression 12
(0) is set. The estimated value calculator 6 calculates the estimated value x represented by the equation 12 from the observation matrix from the observation matrix generator 4, the covariance matrix of the estimated value from the estimated value evaluator 5, and the initial value from the initial value setting device 8. Is calculated.

Since the initial value of the observation error estimation calculation can be set as described above, if the initial value of the observation error is obtained by any method and set as shown in this embodiment, the sensor accuracy can be improved. And the target observation position can be calculated.

Third Embodiment FIG. 3 is a configuration diagram of a target position observation apparatus according to a third embodiment of the present invention. 1 to 7 in the figure are the same as those in the first embodiment. Reference numeral 9 denotes an estimated value storage that stores the observation error estimated value from the estimated value calculator 6 and the covariance matrix P (+) of the estimated value.

Next, the operation will be described. The processing from the observation of the distance to the target by the first sensor 1, the second sensor 2 and the third sensor 3 to the calculation of the observation matrix by the observation matrix generator 4 is the same as in the first embodiment. Next, the observation error estimation value of the previous calculation is set to x (-), and the covariance matrix of the previous estimation value is set to P (-). If the least square method is used, the estimated value of the observation error to be obtained is x, and the square of the difference between x and the previously calculated observation error estimated value x (−), and the observed values z and x
And the sum of the square of the difference and the square may be used as the evaluation function J1.
J1 is represented by Expression 13.

[0043]

(Equation 13)

X which minimizes J1 is such that the result of expanding the right side of J1 and differentiating x becomes zero.
Therefore, the estimated value x to be obtained is expressed by Expression 14.

[0045]

[Equation 14]

The present invention will be described with reference to FIG. The operation from the first sensor 1, the second sensor 2, and the third sensor 3 observing the distance to the target until the observation matrix generator 4 calculates the target observation matrix is the same as in the first embodiment. is there. The estimated value evaluator 5 expresses the covariance matrix of the previously calculated estimated value as P (−), the observation matrix from the observation matrix generator 4 and the covariance matrix R of the observation error v at the time of target observation, as shown in Expression 14. The covariance matrix P (+) of the estimated value is calculated. In the estimated value calculator 6, the covariance matrix P (+) of the observation matrix from the observation matrix generator 4 and the estimated value from the estimated value evaluator 5 and the previously calculated observation error estimated value x ( -) And from Equation 1
An observation error estimated value represented by x of 4 is calculated. The calculated estimated value of the observation error and the covariance matrix P (+) of the estimated value are sent to the estimated value storage 9 to calculate the estimated value of the next observation error, and calculate the previous estimated value x (−) and the previous calculated value. Is used as the covariance matrix P (−) of

When the target position is observed as described above,
Since the observation error is converged from the target observation value and the previously calculated estimation value and the target observation position is calculated, the accuracy is high.

Fourth Embodiment FIG. 4 is a configuration diagram of a target position observation device according to a fourth embodiment of the present invention. In the figure, 1 to 7 and 9 are the same as those of the third embodiment. Reference numeral 8 denotes an initial value setting device for setting an initial value of the observation error estimation calculation.

Next, the operation will be described. The processing up to calculating the estimated value 7 of the target position by observing the distance to the target by the first sensor 1, the second sensor 2, and the third sensor 3 is the same as that of the third embodiment. The initial value setting unit 8 sets an initial value when the estimated value calculator 6 calculates the estimated value of the observation error, that is, sets an initial value of x (−) in the equation (14). Default value. The observation error initial value from the initial value setting unit 8 is sent to and stored in the estimated value storage 9 and is used for the first (first) observation error convergence calculation. In the estimated value calculator 6, the covariance matrix of the observed matrix from the observed matrix generator 4 and the estimated value from the estimated value evaluator 5 and the estimated value of the observation error at the previous calculation from the estimated value storage unit 9, or for the first time ( At the time of the first estimation calculation, an estimated value x represented by Expression 14 is calculated from the initial value of the observation error and the estimated value of the observation error is calculated, and is output as the estimated value 7 of the target position.

As described above, since the initial value at the time of performing the convergence calculation of the estimated value can be set, if the initial value of the observation error is set by any method, the estimated value of the observation error can be quickly converged, and the accuracy can be further improved. The target position can be estimated.

Fifth Embodiment FIG. 5 is a configuration diagram of a target position observation device according to a fifth embodiment of the present invention. In the figure, 1 to 9 are the same as in the fourth embodiment. Reference numeral 10 denotes an estimated value convergence determiner that determines the degree of convergence of the estimated value of the observation error.

Next, the operation will be described. Processing until the first sensor 1, the second sensor 2, and the third sensor 3 observe the distance to the target, calculate an observation error estimated value, and store the observation error calculated by the estimated value storage 9. Is the same as in the fourth embodiment. The estimated value convergence determiner 10 monitors the degree of convergence of the estimated value 7 of the target position by comparing the previously calculated estimated value of the target position with the currently calculated estimated value of the target position. When the estimated value of the target position converges to a certain value or less, the estimated value convergence determiner 10 outputs the observation error estimated value 7 as the target position estimated value calculation result 11 assuming that the estimated value calculation has converged. .

As described above, since the convergence calculation of the estimated value of the target position is performed and the convergence is determined, the accuracy of the obtained estimated value calculation result is high.

[0054]

According to the first and second aspects of the present invention, a state equation using the observation error of the sensor as a state variable is derived and its solution is calculated, whereby the observation error of the sensor is calculated, and the target position can be estimated and calculated. This has the effect.

According to the third and fourth aspects of the present invention, a state equation using the sensor observation error as a state variable is derived, and each time a solution is calculated, the solution is stored. Is calculated, the target position can be accurately estimated.

According to the fifth aspect of the present invention, a state equation using the observation error of the sensor as a state variable is derived, the solution is stored each time a solution is calculated, and the target position is converged using the solution,
There is an effect that the degree of convergence of the solution can be determined.

[Brief description of the drawings]

FIG. 1 is a configuration diagram of a target position observation device according to a first embodiment of the present invention.

FIG. 2 is a configuration diagram of a target position observation device according to a second embodiment of the present invention.

FIG. 3 is a configuration diagram of a target position observation device according to a third embodiment of the present invention.

FIG. 4 is a configuration diagram of a target position observation device according to a fourth embodiment of the present invention.

FIG. 5 is a configuration diagram of a target position observation device according to a fifth embodiment of the present invention.

FIG. 6 is a diagram illustrating a configuration example of a target position observation device including three sensors.

FIG. 7 is a configuration diagram of a conventional sensor observation error calculation device.

[Explanation of symbols]

1 1st sensor, 2nd sensor, 3rd sensor, 4 observation matrix generator, 5 estimated value evaluator, 6 estimated value calculator, 7 estimated value of target position, 8 initial value setting device,
9 Estimated value storage, 10 Estimated value convergence determiner, 11 Target position estimated value calculation result, 12 target, 13 Target position calculation result, 14 First intersection calculator, 15 First target position, 16 Second Intersection calculator, 17 second target position, 18 average value calculator, 19 target position, 20 first
Distance to the target observed by the second sensor, 21 distance to the target observed by the second sensor, and 22 distance to the target observed by the third sensor.

Claims (5)

[Claims] 1. A first sensor for observing a target position and outputting a distance from the target position, and a first sensor arranged at a position distant from the first sensor to observe the same target and output a distance from the target position. A second sensor, and a third sensor which is arranged at a position apart from the first sensor and the second sensor, observes the same target, and outputs a distance of the target position, and outputs a signal from the three sensors. In a target position observing device that calculates a target position from a distance of the target position, an observation matrix generator that calculates an observation matrix of the target position by using the distances of the target positions observed by the first, second, and third sensors as inputs. An estimation value estimator for calculating a covariance matrix of a target position estimated value from a covariance of an observation error included in a distance between the first to third sensor-observed target positions and an observation matrix; And from the estimator estimator A target position observing apparatus, comprising: an estimated value calculator for estimating and calculating a target position from the covariance matrix. 2. The apparatus according to claim 1, further comprising an initial value setting device for setting an initial value when estimating and calculating the target position, and transmitting an initial value of the estimating calculation of the target position to the estimated value calculating device. Target position observation device. 3. An estimated value storage for storing an output of an estimated value calculator for calculating an estimated position of a target, wherein the contents of the estimated value storage are sent to the estimated value evaluator and the estimated value calculator. The target position observation device according to claim 1, wherein: 4. The target according to claim 3, further comprising an initial value setting device for setting an initial value when calculating an estimated position of the target, and transmitting an initial value of the estimation calculation to the estimated value storage. Position observation device. 5. An estimated value convergence determiner that monitors an output of an estimated value calculator that calculates an estimated position of a target and determines convergence of the estimated value, and calculates an estimated value after the estimated position of the target converges. The target position observing device according to claim 4, wherein the target position is output.