JPH07151532A - Sensor bias error estimation device - Google Patents

Abstract

(57) [Summary] [Objective] When n sensors are used to observe k target positions, the object is to be able to estimate the bias error of each sensor. [Arrangement] An observation matrix generator 12 for generating an observation matrix from a target observation position, a sensor position, and a sensor bias error initial value is provided, and an estimated value evaluator 13 for obtaining a covariance matrix of bias error estimated values. An estimated value calculator 14 for calculating an estimated value is provided to estimate the bias error of the sensor. [Effect] Since the bias error is estimated by finding the solution of the equation in which the bias error of the sensor is the state variable, the error of each sensor can be obtained.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an apparatus for estimating a bias error of each sensor in a target position observation apparatus having a plurality of sensors.

[0002]

2. Description of the Related Art FIG. 4 is a diagram showing an embodiment of a conventional bias error calculating device for a sensor. In FIG. 4, 1 is a reference sensor, 2 is a first sensor, 3 is an n-th sensor with an arbitrary number of 2 or more as n, 4 is a reference observer, 5 is a first observer, and 6 is an n-th sensor. Observer, 7 is a sensor position input device for inputting the positions from the first sensor 2 to the nth sensor 3 with respect to the reference sensor 1, and 8 is a target position and sensor position input device 7 from the first observer 5. A first adder for adding the reference sensor positions from the above to calculate the target observation position, and 9 adds the target position from the nth observer 6 and the nth sensor position from the sensor position input unit 7. The n-th adder for calculating the target observation position by the reference numeral 22 is a first subtractor for calculating the difference between the target observation position from the reference observer 4 and the target observation position from the first adder 9, and 23. Is the difference between the target observation position from the reference observer 4 and the target observation position from the nth adder 10. The subtractor of the n, 24 is a reference sensor 1 and first sensor 2
And 25 is a bias error between the reference sensor 1 and the nth sensor 6. Further, FIG. 5 shows an example of the configuration of a position observation device having a plurality of sensors.
6 are the same as in FIG. 26 is the coordinate plane of the reference sensor,
Reference numeral 27 is an inertial surface, 28 is an observation target, and 29 is a bias error between the coordinate surface 26 of the reference sensor and the inertial surface 27.

Next, the operation will be described. In the conventional example, the target position is observed by the reference sensor 1 and the reference observer 4 and sent from the first subtractor 22 to the nth subtractor 23, and the target position is detected by the first sensor 2 and the first sensor 2. The observation is performed by the first observer 5 to the nth sensor 6 and the nth observer 10, and is sent from the first adder 9 to the nth adder 10. The sensor position input device 7 inputs the relative position from the first sensor 2 to the nth sensor 6 with respect to the reference sensor 1. At the target position from the first observer 5 to the nth observer 6, the relative position of the first sensor 2 to the nth sensor 3 with respect to the reference sensor from the sensor position input device 7 is calculated by the first adder 9 To the nth adder 10 to calculate the target observation position. First
The output from the adder 9 to the nth adder 10 is input to the first subtractor 22 to the nth subtractor 23 to calculate differences 24 to 25 from the observation position from the reference observer 4. If the bias error of the first sensor is set to zero, the calculation results 24 to 25 of the second subtractor 22 to the nth subtractor 23 become the bias error of the first sensor 2 to the nth sensor 3, Output this. That is, in FIG. 5, the alignment error of the first to nth sensors with respect to the coordinate plane 26 of the reference sensor is output as a bias error.

[0004]

Since the conventional sensor bias error calculating device is configured as described above, the bias error from the first sensor to the nth sensor with respect to the reference sensor, that is, the first bias error with respect to the reference sensor. Since only the alignment error from the sensor to the n-th sensor can be obtained, the bias error of each sensor cannot be calculated, and the reliability of the bias error calculated by the influence of the observation noise included in the observed value is low. There was a problem.

The present invention has been made to solve the above problems, and an object thereof is to estimate the bias error of each of the reference sensor and the first to nth sensors.

[0006]

A bias error estimating apparatus for a position observing device according to the present invention comprises a reference observing device and observation positions from a first observing device to a n-th observing device and first to first observing devices with respect to a reference sensor. The initial value of the bias error is calculated by creating an observation matrix from the alignment error up to n sensors and the initial value of the bias error of the reference sensor, and further calculating the covariance matrix of the estimated value from the observation matrix and the covariance matrix of the observation noise. And the covariance matrix of the estimated values, the bias error of the reference sensor is estimated, and the alignment error from the first sensor to the nth sensor with respect to the reference sensor is calculated.

[0007]

According to the present invention, a bias error estimating device for a position observing device calculates an observation matrix of the observing device, calculates a covariance matrix of estimated bias error values, and uses a bias error of a reference sensor as a state variable for a state equation. The bias error can be estimated as the solution of

[0008]

【Example】

Embodiment 1 FIG. 1 is a block diagram of a bias error calculation device for a sensor showing an embodiment of the present invention. In the figure, 1 to 6 and 8 to 10 are the same as the conventional device. 8 is the first for the reference sensor 1
Alignment error input device for inputting the alignment error of the n-th sensor 6 where n is an arbitrary number from 2 to 2 of the sensor 2, 11 is an initial bias setting device that determines the initial value of the bias error of the reference sensor, and 12 is the reference observation. From the target observation positions from the instrument 4 and the first adder 9 to the n-th adder 10 and the initial value of the bias error from the initial bias setter 11, let k be an arbitrary number of 2 or more and k targets. , An observation matrix generator for calculating the observation matrix of, and an estimated value evaluator for calculating the covariance matrix of the estimated value of the bias error from the observation matrix,
14 is an estimated value calculator that calculates a temporary value of the sensor bias error estimated value from the observation matrix from the observation matrix generator 12 and the covariance matrix of the estimated bias error value from the estimated value evaluator 13. A subtractor that subtracts a temporary value of the estimated sensor bias error value from the estimated value calculator 14 from the initial value of the sensor bias error from the initial bias setter 11.
6 is the calculation result of the subtracter 15 and is the bias error estimated value of the reference sensor 1, and 17 is the bias error estimated value 1 of the reference sensor 1.
6 and an adder for adding an alignment error from the first sensor 2 to the nth sensor 3 to the reference sensor 1 with respect to the reference sensor, and 18 is an output of the adder 17 from the first sensor 2 to the nth sensor 3. Is the bias error of the sensor.

Next, the operation will be described. Reference sensor 1
, And an arbitrary number of 1 to n is i, and an arbitrary number of 2 or more is k from an i-th sensor, and the correct positions of the targets are set as Pt and Pit, respectively. The observation positions obtained from the sensor are PtO and PitO, respectively.
far. In addition, the bias error of the reference sensor is d (dR,
dB, dE), the position vector of the i-th sensor viewed from the reference sensor 1 is set to Pip, and the reference sensor 1 and the i-th sensor
The alignment error of the th sensor is di (dRi, dE
i, dBi).

Pt, Pit, PtO, and PitO are each expressed in an orthogonal coordinate system.

[0011]

[Equation 1]

[0012]

[Equation 2]

[0013]

[Equation 3]

[0014]

[Equation 4]

[0015] Also, obviously,

[0016]

[Equation 5]

Is satisfied. Here as the function F

[0018]

[Equation 6]

Is defined as d (0) (dR (0), dB (0), d) as the initial value of the bias error of the reference sensor 1.
E (0)) is chosen in the vicinity of the equilibrium point of the function F, the function F is Taylor-expanded around (PtO, PitO, d (0)), and if terms of second or higher order are ignored,

[0020]

[Equation 7]

Is obtained. Gk, Nk, and Mk in Equation 7 are respectively

[0022]

[Equation 8]

[0023]

[Equation 9]

[0024]

[Equation 10]

It is expressed as The function F is the reference sensor 1 and the i-th
If there is no error in the position vector Pip of the th sensor, it will be equal to zero. Therefore, leave the right side of Equation 7 as zero.

[0026]

[Equation 11]

To obtain The left side of Expression 11 is considered to be an observation value determined by the initial value of the bias error and the observation position. The first term on the right side of Expression 11 is considered to be the product of the observation matrix G and the bias error to be obtained as a state variable, and the second and third terms are considered to be the product of the observation position error and the observation matrix, that is, noise. Therefore, Equation 11 can be considered as a linear equation of state in which the observation system contains noise.

Now, letting Z be an observed value, H be an observation matrix, x be a state variable to be obtained, and v be noise with covariance being R:

[0029]

[Equation 12]

It is expressed as If the least squares method is used, the square of the difference between the observed value and the state variable x may be used as the evaluation function J.
J is

[0031]

[Equation 13]

It becomes The x that minimizes the evaluation function J is such that the result obtained by expanding the right side of Expression 13 and differentiating x becomes zero. Therefore, the required state variable x is

[0033]

[Equation 14]

It is expressed as follows. Here, P (+) is the covariance matrix of the estimated value.

Next, the present invention will be described with reference to FIG.
The reference sensor 1 and the first sensor 1 to the n-th sensor 3 are sensors for observing the target position. The reference observer 4 and the first observer 5 to the n-th observer 6 measure the target position. Output. The alignment error input device 7 is the reference sensor 1
For inputting the alignment error of the first sensor 2 to the nth sensor 3 with respect to, the input alignment error is sent from the first observer 5 to the nth observer 6 to correct the target position. The sensor position input device 8 inputs the relative position of the first sensor 2 to the nth sensor 3 with respect to the reference sensor 1. In the first adder 9 to the nth adder 10, the positions of the first sensor 2 to the nth sensor 3 with respect to the reference sensor 1 are set at the target positions of the first observer 5 to the nth observer 6. Is added to calculate the target observation position by the first sensor 2 to the n-th sensor 3. Also, the initial bias setting device 11
Is for setting the bias error initial value of the reference sensor 1. The observation matrix generator 12 receives the target observation positions of the first adder 9 to the nth adder 10 and the bias error initial value from the initial bias setter 11 and is expressed by Equations 7 and 8. The observation matrix Gk is calculated and obtained. The estimated value evaluator 13 calculates P (+) in the equation 14, that is, the covariance matrix of the estimated value, from the observation matrix Gk from the observation matrix generator 12. The estimated value calculator 14 calculates the second term on the right side of Expression 14 from the covariance matrix from the estimated value evaluator 13 and the observation matrix from the observation matrix generator 12 as a temporary value of the bias error estimated value. Further, the subtracter 15 subtracts a temporary value of the bias error estimated value from the estimated value calculator 14 from the bias error initial value from the initial bias setter 11 to calculate the bias error estimated value 16 of the reference sensor 1 represented by the equation 14. Is output. The adder 17 adds the bias error estimated value 16 of the reference sensor 1 from the subtractor 15 and the alignment error of the first sensor 2 to the nth sensor 3 with respect to the reference sensor 1 from the alignment error setting unit 7 and adds The bias error 18 of the first sensor 2 to the nth sensor 3 is calculated.

When the target position is observed in this way,
Using the observation position and the set bias error initial value, the bias error is estimated by assuming a state equation in which the bias error of the reference sensor 1 is a state variable, and the bias error is estimated from the set alignment error from the first sensor 2 to the nth sensor. Since the bias error of the sensor 3 is calculated, the bias errors of the reference sensor 1 and the sensors from the first sensor 2 to the nth sensor 3 can be estimated. That is, in FIG. 5, a bias error 29 between the coordinate plane 26 and the inertial surface 27 of the reference sensor is calculated, and the bias error 29 is added to the alignment error of the first to n-th sensors with respect to the reference sensor. Bias error of the sensor is obtained.

Embodiment 2 FIG. 2 is a block diagram of a bias error calculating device for a sensor showing another embodiment of the present invention. In the figure, 1 to 18 are the same as in the first embodiment. An estimated value storage unit 19 stores the bias error estimated value 16 of the reference sensor 1 from the first subtractor 15.

Next, the operation will be described. Reference sensor 1
The process of observing the target position by the first sensor 2 to the nth sensor 3 and generating the target observation position by the first adder 9 to the nth adder 10 is the same as that in the first embodiment. Next, the previously calculated bias error estimated value is set to x (−), and the covariance matrix of the previous estimated value is set to P (−). If the least squares method is used, x is the estimated value of the bias error to be obtained, and x
Since the sum of the square of the difference between the estimated bias error value x (-) and the square of the difference between the observed value z and x (-) can be used as the evaluation function J1, J1 is

[0039]

[Equation 15]

It is expressed as The x that minimizes J1 is such that the result obtained by expanding the right side of J1 and differentiating with respect to x becomes zero. Therefore, the estimated value x to be obtained is

[0041]

[Equation 16]

The present invention will be described with reference to FIG. The operation from observing the target position by the reference sensor and the first sensor 2 to the nth sensor 3 and generating the target observation position by the first adder 9 to the nth observer 10 is the same as in the first embodiment. It is the same. The observation matrix generator 12 generates an observation matrix from the target observation positions of the first adder 9 to the n-th adder 10 and the bias error initial value from the initial bias setter 11.
The estimated value evaluator 13 calculates P (+) represented by Expression 16. Furthermore, in the estimated value calculator 14, the estimated value evaluator 13
The second term on the right side of Expression 16 is calculated using the covariance matrix P (+) of the estimated values from In the subtractor 15, the estimated value calculator 1
The bias error estimated value 16 of the reference sensor represented by the equation 16 is calculated from the output of No. 4 and the previous bias error estimated value from the initial bias setter 11, and is output to the estimated value storage unit 19 as well. The estimated value storage unit 19 stores the calculated bias error estimated value 16 of the reference sensor and sends the previously calculated sensor bias error estimated value 16 to the initial bias setting unit 11.

When the target position is observed in this way,
Since the bias error is converged and calculated from the observation position and the previously calculated estimated value, the accuracy is high.

Embodiment 3 FIG. 3 is an embodiment of a sensor bias error device showing still another embodiment of the present invention. In the figure, 1 to 19 are the same as in the second embodiment. Reference numeral 20 is an estimated value convergence determiner that compares the calculated bias error estimated value 14 of the reference sensor with the estimated value calculated last time to perform convergence determination, and 21 is an estimated value calculation result.

Next, the operation will be described. Reference sensor 1
The process from observing the target position by the first sensor 2 to the nth sensor 3 and calculating the bias error estimated value 16 of the reference sensor 1 is the same as in the second embodiment. The estimated value convergence determiner 20 compares the estimated value of the bias error calculated last time and the estimated value of the bias error calculated this time, and monitors the degree of convergence of the estimated bias error value 16 of the reference sensor 1.
When the estimated value of the bias error has converged to a certain value or less, the estimated value calculation result 21 is output.

In this way, the estimated value of the bias error is converged and calculated from the target observed position and the estimated value calculated last time, and the convergence judgment is performed, so the accuracy of the estimated value calculation result obtained is good.

[0047]

As described above, according to the present invention, the solution of the state equation with the bias error of the reference sensor as the state variable is calculated, and the alignment error of the first sensor to the nth sensor with respect to the reference sensor is calculated. Since the bias error is calculated using this, the bias error of each sensor can be obtained.

Further, when the sensor bias error is calculated by the convergence calculation, there is an effect that the sensor bias error can be accurately estimated.

[Brief description of drawings]

FIG. 1 is a configuration diagram of a bias error estimating apparatus for a sensor according to a first embodiment of the present invention.

FIG. 2 is a configuration diagram of a sensor bias error estimation device according to a second embodiment of the present invention.

FIG. 3 is a configuration diagram of a bias error estimating apparatus for a sensor according to a third embodiment of the present invention.

FIG. 4 is a configuration diagram of a conventional bias error calculation device for a sensor.

FIG. 5 is a configuration example of a position observation device having a plurality of sensors.

[Explanation of symbols]

 1 Reference sensor 2 1st sensor 3 nth sensor 4 Reference observer 5 1st observer 6 nth observer 7 Alignment error input device 8 Sensor position input device 9 1st adder 10 nth addition Device 11 Initial bias setting device 12 Observation matrix generator 13 Estimated value evaluator 14 Estimated value calculator 15 Subtractor 16 Estimated bias error of reference sensor 17 Adder 18 Bias error of sensor 19 Estimated value storage 20 Estimated value convergence judgment 21 Estimated value calculation result 22 First subtractor 23 Nth subtractor 24 Bias error of first sensor 25 Bias error of second sensor 26 Coordinate plane of reference sensor 27 Inertia plane 28 Observation target 29 Bias error

Claims (3)

[Claims] 1. A reference sensor for observing k (k is an arbitrary number of 2 or more) target positions, a reference observing device corresponding to the reference sensor, and a reference sensor installed at a place distant from the reference sensor. N sensors corresponding to the first to n-th (n is an arbitrary number of 2 or more) sensors whose relative alignment error is known, and n-sensors corresponding to the first to n-th sensors In the position observation device consisting of
Alignment error input device for inputting alignment error of each sensor from the first sensor to the nth sensor with respect to the reference sensor, and sensor position for inputting relative position from the first sensor to the nth sensor with respect to the reference sensor A target observation position is calculated by adding the target position from the input device and the observation devices from the first observation device to the nth observation device and the relative position of each sensor from the sensor position input device to the reference sensor. From the first adder to the nth adder, the initial bias setting device that sets the initial value of the bias error of the reference sensor, the target observation position from the reference observer, and the first adder to the nth adder To the observation matrix generator that calculates the observation matrix from the target observation position up to and the initial bias value of the reference sensor from the initial bias setter, and the bias error from the observation noise covariance and the observation matrix. An estimated value evaluator that calculates the covariance matrix of the estimated value, an estimation calculator that calculates a temporary value of the bias error of the reference sensor from the observation matrix and the covariance matrix of the bias error estimated value, and the bias error of the reference sensor And a first subtractor for calculating a bias error estimated value of the reference sensor by subtracting a temporary value of the bias error of the reference sensor from the initial value of the bias error of the reference sensor. Bias error estimation device. 2. An estimated value storage unit for storing an output of a first subtractor for calculating a bias error estimated value of a reference sensor, the content of the estimated value storage unit being sent to an initial bias setting unit. The bias error estimating device for a sensor according to claim 1, wherein 3. An estimation value determiner for determining the convergence of the estimated value from the output of a first subtractor for calculating the bias error estimated value of the reference sensor, and repeating the calculation until the bias error estimated value converges. The bias error estimation device for a sensor according to claim 1, which is characterized.