JP6757227B2 - Motion parameter estimation device, motion parameter estimation method and program - Google Patents

Description

The present invention relates to a motion parameter estimation device, a motion parameter estimation method and a program.

Several techniques have been proposed for measuring the position of an object using a sensor such as sonar. For example, in the technique described in Patent Document 1, when a passive sonar is used, an active sonar is used at another frequency.

Special Fairness 03-060394 Gazette

It is preferable that the combination of the active sensor and the passive sensor can improve the estimation accuracy of the motion of the object as compared with the case where each sensor is used alone. On the other hand, Patent Document 1 does not show a specific method for improving the estimation accuracy of the motion of an object by combining the measurement result of the active method and the measurement result of the passive method.

The present invention provides a motion parameter estimation device, a motion parameter estimation method, and a program capable of improving the estimation accuracy of the motion of an object by combining an active sensor and a passive sensor.

According to the first aspect of the present invention, the motion parameter estimator measures the position of an object using an active sensor, and uses a passive sensor to measure the object in a period shorter than the measurement cycle of the active sensor. A measurement unit that measures the direction of the object, a drive noise sample setting unit that sets a plurality of drive noise samples in a state equation indicating the motion of the object based on a predetermined probability distribution, and each of the drive noise samples. The particle calculation unit that calculates a plurality of particles in the particle filter by substituting into the state equation and each of the particles calculated by the particle calculation unit are substituted into the observation equation to calculate the estimated value of the measurement value of the measurement unit. A selection unit that selects a part of the plurality of the particles based on the error between the first estimation unit and the measurement value of the measurement unit and the estimation value of the first estimation unit, and the selection unit selects the selection unit. It is provided with a second estimation unit that calculates an estimated value of a motion parameter indicating the motion of the object based on the particles.

The state equation shows the motion of the motion parameter estimator in addition to the motion of the object, and the observation equation is the measured value of the motion of the motion parameter estimator in addition to the equation of the measured value of the motion of the object. The second estimation unit may calculate an estimated value of the motion parameter indicating the motion of the object and the motion of the motion parameter estimation device.

The observation equation may include an equation for the measured value of the velocity of the motion parameter estimator.

The observation equation may include an equation of a velocity measurement of the object based on the Doppler effect in the active sensor.

The first estimation unit may calculate the estimated value of the measured value of the measurement unit by using the observation equations of the active sensor and the passive sensor.

According to the second aspect of the present invention, in the motion parameter estimation method, the position of the object is measured by using the active sensor, and the object is measured by using the passive sensor in a period shorter than the measurement cycle by the active sensor. A measurement step for measuring the direction of the object, a drive noise sample setting step for setting a plurality of drive noise samples in a state equation showing the motion of the object based on a predetermined probability distribution, and each of the drive noise samples are described above. Substituting into the state equation to calculate multiple particles in the particle filter, and substituting each of the particles calculated in the particle calculation step into the observation equation to calculate the estimated value of the measured value in the measurement step. A selection step of selecting a part of the plurality of the particles based on the error between the first estimation step to be performed, the measured value in the measurement step, and the estimated value in the first estimation step, and the selection. It includes a second estimation step of calculating an estimated value of a motion parameter indicating the motion of the object based on the particles selected in the step.

According to a third aspect of the present invention, the program measures the position of an object on a computer using an active sensor, and uses a passive sensor to measure the object in a period shorter than the measurement period of the active sensor. A measurement step for measuring the direction of the object, a drive noise sample setting step for setting a plurality of drive noise samples in a state equation indicating the motion of the object based on a predetermined probability distribution, and each of the drive noise samples are described above. Substituting into the state equation, the particle calculation step of calculating a plurality of particles in the particle filter, and substituting each of the particles calculated in the particle calculation step into the observation equation, the estimated value of the measured value in the measurement step is calculated. A selection step of selecting a part of the plurality of the particles based on the error between the first estimation step to be performed, the measured value in the measurement step, and the estimated value in the first estimation step, and the selection. It is a program for executing a second estimation step of calculating an estimated value of a motion parameter indicating the motion of the object based on the particles selected in the step.

According to the above-mentioned motion parameter estimation device, motion parameter estimation method and program, this is possible. The combination of the active sensor and the passive sensor can improve the estimation accuracy of the motion of the object.

It is a schematic block diagram which shows the functional structure of the motion parameter estimation apparatus which concerns on 1st Embodiment of this invention. It is a figure which shows the structural example of the measurement system which the motion parameter estimation apparatus which concerns on 1st Embodiment of this invention performs measurement. It is a figure which shows the measurement example of the direction of an object by the passive sensor which concerns on 1st Embodiment of this invention. It is a figure which shows the measurement example of the position of the object by the active sensor which concerns on 1st Embodiment of this invention. It is a flowchart which shows the example of the procedure of the process performed by the motion parameter estimation apparatus which concerns on 1st Embodiment of this invention. It is a figure which shows the example of convergence to the vicinity of the true value of the estimated value of the motion parameter by the motion parameter estimation device which concerns on 1st Embodiment of this invention.

Hereinafter, embodiments of the present invention will be described, but the following embodiments do not limit the inventions in the claims. Also, not all combinations of features described in the embodiments are essential to the means of solving the invention.

<First Embodiment>
FIG. 1 is a schematic block diagram showing a functional configuration of the motion parameter estimation device according to the first embodiment of the present invention. As shown in the figure, the motion parameter estimation device 100 includes a measurement unit 110, a storage unit 180, and a control unit 190. The measuring unit 110 includes a passive sensor 111 and an active sensor 112. The control unit 190 includes a drive noise sample setting unit 191, a particle calculation unit 192, a first estimation unit 193, a selection unit 194, a correction unit 195, and a second estimation unit 196. Further, the motion parameter estimation device 100 is mounted on the moving body 10.

The motion parameter estimation device 100 estimates the value of the motion parameter indicating the motion of the object. The motion parameter estimation device 100 is configured by using, for example, a computer equipped with a sensor.
FIG. 2 is a diagram showing a configuration example of a measurement system in which the motion parameter estimation device 100 measures. As shown in FIG. 2, the measurement system 1 includes a moving body 10 equipped with the motion parameter estimation device 100, and an object 20 on which the motion parameter estimation device 100 measures motion.
The line L11 shows an example of the locus of the moving body 10. Line L12 shows an example of the trajectory of the object 20. The arrow B11 shows an example of the velocity vector of the moving body 10. Arrow B12 shows an example of the velocity vector of the object 20.

The motion parameter estimation device 100 can set various moving objects as measurement targets (motion parameter estimation targets). For example, the measurement target area of the motion parameter estimation device 100 may be underwater such as in the sea, and the object 20 may be an underwater vehicle. Alternatively, the measurement target area of the motion parameter estimation device 100 may be on the water surface such as the sea, and the object 20 may be an object moving on the water surface such as a ship. Alternatively, the measurement target area of the motion parameter estimation device 100 may be the land (ground), and the object 20 may be an object traveling on land such as a vehicle. Alternatively, the measurement target area of the motion parameter estimation device 100 may be in the air, and the object 20 may be an object moving in the air such as an airplane. Alternatively, the measurement target of the motion parameter estimation device 100 may be the universe, and the object 20 may be an object moving in the universe such as a spaceship.

Like the object 20, the moving body 10 can be various objects. For example, the moving body 10 may be an underwater navigation body. Alternatively, the moving body 10 may be an object that moves on the water surface such as a ship. Alternatively, the moving body 10 may be a vehicle or the like traveling on land. Alternatively, the moving body 10 may be an object that moves in the air, such as an airplane. Alternatively, the moving body 10 may be an object that moves in space, such as a spaceship.
In the following, a case where the motion parameter estimation device 100 is mounted on the moving body 10 and moves will be described as an example, but the motion parameter estimation device 100 may be fixedly installed. For example, the motion parameter estimation device 100 may be provided in a non-moving structure such as in a building instead of the moving body 10.

Further, in the following description, a case where the measurement target region of the motion parameter estimation device 100 is a two-dimensional region and the motion parameter estimation device 100 uses xy coordinates (two-dimensional Cartesian coordinates) will be described as an example. The measurement target area of 100 may be three-dimensional. When the measurement target region of the motion parameter estimation device 100 is a three-dimensional region, the motion parameter estimation device 100 may use xyz coordinates (three-dimensional Cartesian coordinates).
When the measurement target region of the motion parameter estimation device 100 is a three-dimensional region, the motion parameter estimation device 100 also performs the processing related to the x-coordinate and the y-coordinate described below for the z-coordinate. Further, the motion parameter estimation device 100 performs processing on a three-dimensional angle or direction in the same manner as the processing on a two-dimensional angle or direction described below.

The measuring unit 110 measures the motion parameters.
The passive sensor 111 measures the direction of the object 20 as seen from the position of the measuring unit 110 (the position of the moving body 10) by measuring the physical energy that has reached the passive sensor 111 from the object 20. The direction of the object 20 is one of the motion parameters.

FIG. 3 is a diagram showing an example of measurement in the direction of the object 20 by the passive sensor 111. In the example of FIG. 3, the physical energy reached from the object 20 to the passive sensor 111 is indicated by the arrow B21.
For example, when the passive sensor 111 is a passive sonar, the passive sensor 111 measures the sound that reaches the passive sensor 111 from the object 20. This sound may be a sound emitted by the object 20, or a sound reflected by the object 20 from a sound emitted by an object other than the object 20.
Alternatively, when the passive sensor 111 is a passive radar, the passive sensor 111 measures the electromagnetic wave that has reached the passive sensor 111 from the object 20. This electromagnetic wave may be an electromagnetic wave emitted by the object 20, or may be an electromagnetic wave reflected by the object 20 from an electromagnetic wave emitted by an object other than the object 20.

The active sensor 112 measures the position of the object 20 by outputting physical energy toward the object 20 and measuring the physical energy reflected by the object 20 and reaching the passive sensor 111. The position of the object 20 is one of the motion parameters.

FIG. 4 is a diagram showing an example of measuring the position of the object 20 by the active sensor 112. In the example of FIG. 4, the physical energy output by the active sensor 112 toward the object 20 is indicated by an arrow B31. Further, the physical energy reflected by the object 20 and reaching the passive sensor 111 is indicated by an arrow B32.
For example, when the active sensor 112 is an active sonar, the active sensor 112 outputs a sound (sound wave) toward the object 20, and measures the sound that is reflected by the object 20 and reaches the active sensor 112.
Alternatively, when the active sensor 112 is an active radar, the active sensor 112 outputs an electromagnetic wave toward the object 20 and measures the electromagnetic wave reflected by the object 20 and reached the active sensor 112.
The active sensor 112 measures the relative position of the object 20 with respect to the position of the active sensor 112 (the position of the moving body 10). The active sensor 112 may determine the position of the object 20 with reference to a position other than the position of the active sensor 112 based on the relative position and the position of the active sensor 112 itself.

The storage unit 180 is configured by using a storage device included in the motion parameter estimation device 100, and stores various information.
The control unit 190 controls each unit of the motion parameter estimation device 100 to execute various processes. The control unit 190 is configured by, for example, a CPU (Central Processing Unit) included in the motion parameter estimation device 100 reading a program from the storage unit 180 and executing the program.

The control unit 190 estimates the motion parameter of the object 20 by using the measurement result by the passive sensor 111 and the measurement result by the active sensor 112. The control unit 190 estimates, for example, the position of the object 20.
Here, the passive sensor 111 can measure the direction of the object 20 as seen from the position of the measuring unit 110, but cannot measure the relative distance between the measuring unit 110 and the object 20. Therefore, it is difficult to estimate the position of the object 20 with high accuracy by using only the measurement result of the passive sensor 111.
On the other hand, the control unit 190 measures the position of the object 20 with higher accuracy than the case where only the measurement result of the passive sensor 111 is used by using the measurement result by the passive sensor 111 and the measurement result by the active sensor 112. can do.

Further, the active sensor 112 outputs physical energy to the object 20 and then waits for the arrival of the physical energy reflected by the object 20. Therefore, if only the measurement result of the active sensor 112 is used, it is necessary to increase the sampling period to some extent. When the sampling period becomes large to some extent, it may take time for the estimated value of the motion parameter to converge to a value close to the true value, for example, when the initial value of the motion parameter is unknown.
On the other hand, by using the measurement result by the passive sensor 111 and the measurement result by the active sensor 112, the control unit 190 sets the estimated value of the motion parameter to true more quickly than when using only the measurement result of the active sensor 112. It can be converged to a value close to the value.

The drive noise sample setting unit 191 sets a sample of drive noise. The driving noise referred to here is a parameter for matching the motion of the object 20 estimated using the motion model of the object 20 with the actual motion of the object 20. The control unit 190 uses the equation of state of the object 20 as a motion model of the object 20, and the equation of state includes driving noise.
The equation of state of the object 20 used by the control unit 190 is defined as, for example, the equation (1).

Here, k indicates the sampling timing index of the passive sensor 111. Specifically, with time 0 as the reference time, k indicates the number of samplings performed by the passive sensor 111 after the reference time. Denoting the sampling period of the passive sensor 111 at _{T S,} k-th sampling time is expressed as kT _S.
Hereinafter, the timing of the kth sampling after the reference time by the passive sensor (that is, the sampling timing indicated by the index k) is referred to as timing k.

The motion parameter vector q (k) indicates the motion parameter at the timing k. In particular, the motion parameter vector q (k) indicates the position, velocity and acceleration of the object at the timing k. The motion parameter vector q (k) is expressed by Eq. (2).

Here, x (k) and y (k) indicate the x-coordinate value and the y-coordinate value of the target position at the timing k, respectively. x'(k) and y'(k) indicate the x-coordinate value and the y-coordinate value of the target velocity at the timing k, respectively. The first derivative is indicated by'. x'' (k) and y'' (k) indicate the x-coordinate value and the y-coordinate value of the target acceleration at the timing k, respectively. The second derivative is indicated by''.
The superscript T to the right of the matrix or vector indicates the transpose of the matrix or vector.

The drive noise vector u (k) indicates the drive noise at the timing k. The drive noise vector u (k) is expressed as in Eq. (3) using _two scalar variables u ₁ (k) and u ₂ (k).

The matrix A of the equation (1) is a matrix showing the coefficients. The matrix A is represented by Eq. (4).

_{T S} as described above, shows a sampling period of the passive sensor 111.
The matrix B is a matrix showing the coefficients. The matrix B is represented by Eq. (5).

The equation of state shown in the equation (1) is the value of the motion parameter (target position, velocity, acceleration) and the driving noise value at the timing k, and the value of the motion parameter (target position, velocity, acceleration) at the timing k + 1. Shows the relationship with.

The drive noise sample setting unit 191 sets a plurality of drive noise samples based on a predetermined probability distribution. Here, the control unit 190 calculates an estimated value of the motion parameter using the particle filter. The drive noise sample setting unit 191 sets the drive noise by the number of particles in the particle filter.
The probability distribution used by the drive noise sample setting unit 191 for setting the drive noise sample is not limited to a specific probability distribution. For example, the drive noise sample setting unit 191 may set the drive noise based on the Gaussian distribution. Alternatively, the drive noise sample setting unit 191 may set the drive noise based on a distribution other than the Gaussian distribution.

The particle calculation unit 192 calculates a plurality of particles in the particle filter by substituting each of the drive noise samples set by the drive noise sample setting unit 191 into the equation of state. The particle calculation unit 192 substitutes the value of the motion parameter at the timing k-1 and the value of the driving noise into the equation of state, and calculates the value of the motion parameter at the timing k as a particle in the particle filter.

The first estimation unit 193 calculates the estimated value of the measured value of the motion parameter by substituting each of the particles calculated by the particle calculation unit 192 into the observation equation. The observation equation referred to here is an equation for estimating the measured value of the measuring unit 110 from the motion parameter, and an observation error (measurement noise) is added to the value of the motion parameter.
The observation equation in the active sensor 112 is defined as, for example, equation (6).

Here, the vector m (k) indicates the measured value of the motion parameter at the timing k. In the case of the active sensor 112, the vector m (k) is expressed as in equation (7).

Here, x _m (k) indicates a measured value of the x-coordinate value of the target position at the timing k. y m _(k) denotes a measured value of y-coordinate value of the position of the target at the timing k.

The matrix C of the equation (6) is a matrix showing the coefficients. The matrix C is represented by Eq. (8).

Further, w _A (k) of the equation (6) indicates an observation error of the active sensor 112 at the timing k.
On the other hand, the observation equation in the passive sensor 111 is defined as, for example, the equation (9).

Here, atan indicates an arc tangent. x _O (k) indicates the x coordinate value of the position of the measuring unit 110 at the timing k. y _O (k) indicates the y coordinate value of the position of the measuring unit 110 at the timing k.
w _P (k) indicates the observation error of the passive sensor 111 at the timing k.
Further, in the case of the passive sensor 111, m (k) is expressed as in the equation (10).

Here, θ _m (k) indicates a measured value (measured value of angle measurement) of the direction of the target with respect to the position of the measuring unit 110 at the timing k.

The selection unit 194 selects a part of the plurality of particles based on the error between the measured value of the motion parameter by the measuring unit 110 and the estimated value of the measured value of the motion parameter calculated by the first estimation unit 193. To do. Specifically, the selection unit 194 calculates the likelihood of particles from the reciprocal of the error, and selects a predetermined number of particles in descending order of likelihood.

The correction unit 195 applies an extended Kalman filter to each of the particles selected by the selection unit 194 to correct each of the particles. The correction unit 195 makes corrections so as to increase the likelihood of each particle.
The second estimation unit 196 obtains an estimated value of the motion parameter based on the particles corrected by the correction unit 195. Further, the second estimation unit 196 is based on the position and velocity of the object 20 indicated by the motion parameters, the distance between the measurement unit 110 and the object 20, and the object 20 as seen from the position of the measurement unit 110. The direction and the magnitude of the velocity of the object 20 are calculated.

Next, the operation of the motion parameter estimation device 100 will be described with reference to FIG. FIG. 5 is a flowchart showing an example of a processing procedure performed by the motion parameter estimation device 100. The motion parameter estimation device 100 repeats the processes of steps S101 to S110 of FIG. 5 for each sampling timing of the passive sensor 111.
Here, the active sensor 112 needs to wait until the physical energy output toward the object 20 is reflected and returned to the active sensor 112. On the other hand, in the passive sensor 111, there is no restriction on the waiting time as in the case of the active sensor 112. Therefore, the sampling cycle of the passive sensor 111 can be set shorter than the sampling cycle of the active sensor 112.
Therefore, the motion parameter estimation device 100 measures the motion parameters by setting the sampling cycle of the passive sensor 111 to be shorter than the sampling cycle of the active sensor 112. Specifically, every time the passive sensor 111 measures the motion parameter n times (n is a positive integer), the active sensor 112 measures the motion parameter once. As a result, there is a sampling timing in which the passive sensor 111 and the active sensor 112 measure the motion parameter, and a sampling timing in which only the passive sensor 111 measures the motion parameter.

(Step S101)
The drive noise sample setting unit 191 generates a sample of drive noise (drive noise vector u (k)) by the number of particles P. Hereinafter referred samples of driving noise at the timing k and ^u p (k).
After step S101, the transition to step S102 occurs.

(Step S102)
Particles calculation unit 192 substitutes the P samples generated in step S101 each equation of state (vector ^u p (k)) (Formula (1)), and calculates the P number of particles. In the following, the particle at the timing k is referred to as q ^p (k).
After step S102, the transition to step S103 occurs.

(Step S103)
The first estimation unit 193 substitutes each of the particles obtained in step S102 into the observation equation, and calculates the estimated value of the measured value by the measuring unit 110. As described above, there are an observation equation (Equation (6)) in the case of the active sensor 112 and an observation equation (Equation (9)) in the case of the passive sensor 111. Here, the first estimation unit 193 obtains an estimated value of the measured value of the orientation of the target with respect to the sensor at the timing k by using the observation equation (equation (9)) in the case of the passive sensor 111. In the following, the estimated value of the measured value of the target azimuth with respect to the sensor at the timing k is expressed as θ ^p (k).
Note that the observation error W _{A (k),} for example, sensor characteristics of (passive sensors 111 or active sensor 112), and, on the basis of the real environment (environment for measurement), the observation error follows a probability distribution and its parameters Is set in advance. Then, by generating random numbers based on the set probability distribution and parameters, the observation error corresponding to each particle can be generated.
After step S103, the process proceeds to step S104.

(Step S104)
The selection unit 194 calculates an error between the estimated value of the measured value obtained in step S103 and the measured value.
Here, the selection unit 194 subtracts the difference obtained by subtracting the measured value θ _m (k) of the target orientation with respect to the sensor at the timing k from the estimated value θ ^p (k) of the measured value of the target orientation with respect to the sensor at the timing k. Calculated as an error.
Hereinafter, it referred to the estimated value of the obtained measurement value in step S103, the error between the measured value by the measuring unit 110 e ^p a ^(k).
After step S104, the process proceeds to step S105.

(Step S105)
The selection unit 194 calculates the likelihood π ^p (k) at the timing k from the reciprocal e ^p (k) ^{-1 of} the error e ^p (k) calculated in step S104. The selection unit 194 normalizes the reciprocal of the error e ^p (k) ^{-1 so} that the sum is 1. The likelihood π ^p (k) at the timing k is expressed by Eq. (11).

After step S105, the process proceeds to step S106.

(Step S106)
The selection unit 194 selects (resampling) P particles (P is a positive integer) in order from particles having a high likelihood (particles having a large value of likelihood π ^p (k)). Here, P is a predetermined constant.
The selection unit 194 rejects particles other than the selected P particles.
After step S106, the process proceeds to step S107.

(Step S107)
The correction unit 195 applies an extended Kalman filter to each of the P particles selected in step S106 to correct the value of each particle. The correction unit 195 makes corrections so as to increase the likelihood of each particle. Therefore, the correction unit 195 recalculates the estimated value of the measured value by the sonar using each of the particles selected by the selection unit 194. Here, unlike the case of step S103, the correction unit 195 uses the observation equation (equation (6)) by the active sensor 112 at the time of active sensing. On the other hand, the correction unit 195 uses the observation equation (equation (9)) by the passive sensor 111 at the time of passive sensing. As a result, the distance measurement information by the active sensor 112 can be reflected in the likelihood correction of the particles.

Here, for passive sensing, the observation equation by passive sonar is a non-linear equation including arc tangent (atan). Therefore, based on the framework of the extended Kalman filter, the observation equation is Taylor-expanded and the approximate value truncated by the first-order argument is adopted. Specifically, the equation (9) is modified and used as in the equation (12).

Here, the vector C (k) is a vector indicating a coefficient, and is expressed as in the equation (13).

The partial derivative ∂h _k / ∂x (k) is expressed by Eq. (14).

α (k) indicates the ratio obtained by dividing the y-coordinate value of the relative position of the target with respect to the sensor by the x-coordinate value. α (k) is expressed by the equation (15).

The partial differential ∂h _k / ∂y (k) is expressed by Eq. (16).

By using the observation equation shown in the equation (12), the correction unit 195 can calculate the Kalman gain during passive sensing as well as during active sensing. The correction unit 195 makes corrections using Kalman gain for each particle.
The Kalman gain L (k) here is expressed by the equation (17).

P (k) indicates the estimation error covariance matrix at the timing k. W (k) indicates the variance matrix of the observation error at the timing k.
The particle correction using the Kalman gain L (k) here is expressed by Eq. (18).

Q ^p (k) on the left side of the equation (18) indicates the corrected particle at the timing k. The q ^p (k) on the right side indicates the particles before correction at the timing k. Further, q _e ^p (k) indicates an estimated value of the motion parameter at the timing k.
After step S107, the process proceeds to step S108.

(Step S108)
The second estimation unit 196 calculates the arithmetic mean of the corrected particles using Kalman gain. The second estimation unit 196 uses the obtained average value as the target motion parameter estimation value at the timing k. In the following, the estimated value of the motion parameter of the target at the timing k is expressed as q _e (k).
After step S108, the process proceeds to step S109.

(Step S109)
The second estimation unit 196 calculates the distance between the sensor and the target, the orientation of the target with respect to the sensor, and the relative velocity based on the motion parameter estimated value q _e (k) of the target at the timing k and the position of the sensor. The distance R (k) between the sensor and the target at the timing k is expressed by the equation (19).

As the x-coordinate value x (k) and the y-coordinate value y (k) of the target position here, the values in q _e (k) are used.
The azimuth θ (k) of the target with respect to the sensor at the timing k is expressed by the equation (20).

As the x-coordinate value x (k) and the y-coordinate value y (k) of the target position here, the values in q _e (k) are used.
The relative velocity v (k) of the object with respect to the sensor at the timing k is expressed by the equation (21).

As the x-coordinate value x'(k) and the y-coordinate value y'(k) of the target velocity here, the values in q _e (k) are used.
After step S109, the process proceeds to step S110.

(Step S110)
The control unit 190 sets k: = k + 1. That is, the value of the index indicating the sampling timing is advanced (increased) by 1.
After step S110, the process from step S101 is repeated at the next sampling timing.

FIG. 6 is a diagram showing an example of convergence of the estimated value of the motion parameter by the motion parameter estimation device 100 to the vicinity of the true value. The horizontal axis of the graph shown in FIG. 6 indicates the time, and the vertical axis indicates the value of the motor parameter. The line L21 shows the true value of the motion parameter. The line L22 shows the estimated value of the motion parameter by the motion parameter estimation device 100. Line L23 shows the estimated value of the motion parameter when only the active sensor is used. The time t11 indicates the sampling period when only the active sensor is used.
In the case of using only the active sensor, as described above, since the active sensor has a waiting time from outputting the physical energy to the object to receiving the physical energy reflected by the object, it is indicated by the time t11. As such, a certain sampling interval occurs. On the other hand, in the motion parameter estimation device 100, the passive sensor 111 can measure the motion of the object at a sampling interval shorter than the sampling interval of the active sensor 112. In this respect, according to the motion parameter estimation device 100, the motion of the object can be measured in a shorter cycle than when the active sensor alone is used, and it is expected that the motion of the object can be estimated with higher accuracy. In addition, it is expected that it will converge to near the true value more quickly.

As described above, the measuring unit 110 measures the position of the object using the active sensor 112, and measures the direction of the object 20 using the passive sensor 111 in a cycle shorter than the measurement cycle of the active sensor 112. .. The drive noise sample setting unit 191 sets a plurality of drive noise samples in the equation of state based on a predetermined probability distribution. The particle calculation unit 192 calculates a plurality of particles in the particle filter by substituting each of the driving noise samples into the equation of state. The first estimation unit 193 substitutes each of the particles calculated by the particle calculation unit 192 into the observation equation, and calculates the estimated value of the measured value of the measurement unit 110. The selection unit 194 selects a part of the plurality of particles based on the error between the measured value of the measuring unit 110 and the estimated value of the first estimation unit 193. The second estimation unit 196 calculates an estimated value of the motion parameter based on the particles selected by the selection unit 194.
In this way, in the motion parameter estimation device 100, the measurement result of the active sensor 112 and the measurement result of the passive sensor 111 can be applied to the same model by applying the particle filter to the model including the state equation and the observation equation. it can. As a result, the motion parameter estimation device 100 can use the measurement result of the position of the object 20 by the active sensor 112, and measures the motion of the object 20 in a shorter sampling cycle than the case where the active sensor 112 alone is used. be able to. In this respect, according to the motion parameter estimation device 100, the estimation accuracy of the motion of the object 20 can be improved by combining the active sensor 112 and the passive sensor 111.

<Second Embodiment>
In the first embodiment, the error of the sensor position ([x _O (k) y _O (k)]) is not considered. On the other hand, in any positioning such as positioning using GNSS (Global Navigation Satellite System) and dead reckoning, an error may occur in the positioning result of the sensor position.
Therefore, in the second embodiment, the motion parameters of the sensor itself are estimated in addition to the motion parameters of the target.

The configuration of the motion parameter estimation device 100 in the second embodiment is the same as in the case of the first embodiment described with reference to FIG. Further, the processing procedure in the second embodiment is the same as the processing procedure in the case of the first embodiment described with reference to FIG. However, in the second embodiment, the mathematical model is extended as follows.
The motion parameter vector q (k) includes the motion parameters of the sensor in addition to the motion parameters of the target. The motion parameter state vector q (k) at the timing k in the second embodiment is expressed by the equation (22).

In the second embodiment, the drive noise is also extended. The drive noise vector u (k) at the timing k in the second embodiment is expressed by the equation (23).

Like u ₁ (k) and u ₂ (k), both u ₃ (k) and u ₄ (k) are scalar variables.
In the second embodiment, the matrices A and B of the equation of state represented by the equation (1) are also extended. In the second embodiment, the matrix A ₁ is used instead of the matrix A in equation (1). The matrix A ₁ is represented by Eq. (24).

Here, R ^{12 × 12} indicates a real number matrix of 12 rows and 12 columns.
In the second embodiment, a matrix B ₁ in place of the matrix B in Equation (1). The matrix B ₁ is expressed by Eq. (25).

Here, 0 _{6 × 2} indicates a 6-by-2 zero matrix. R ^{12 × 4} represents a real number matrix of 12 rows and 4 columns.
In the second embodiment, the matrix C of the observation equation shown in the equation (6) is also extended. In the second embodiment, the matrix C ₁ is used in place of the matrix C in equation (6). The matrix C ₁ is expressed by Eq. (26).

Here, 0 _{2 × 6} indicates a zero matrix of 2 rows and 6 columns.
In the second embodiment, the observation value vector is extended by adding the measured value of the self-position. In the second embodiment, the equation (27) is used instead of the equation (7) as the observed value vector in the case of active sonar.

Here, x _Om indicates the x coordinate value of the measured value (positioning result) of the position of the sensor itself. y _Om indicates the y coordinate value of the measured value of the position of the sensor itself.
Since the position of the sensor can be measured separately from the active sonar and the passive sonar, the equation (28) is used as the observed value vector in the case of the passive sonar.

θ _m (k) is as described with reference to equations (9) and (12).
Equation (29) is used instead of equation (13) as an approximation of the non-linear function at the time of passive in step S107 of FIG.

Also, in the case of passive sensors, instead of the matrix C of the observation equation shown in equation (6), using the matrix C ₁ represented by the formula (30).

C ₂ is expressed as in equation (31).

As mentioned above, 0 _{2 × 6} represents a 2 row 6 column zero matrix. Further, R ^{2 × 12} indicates a real number matrix of 2 rows and 12 columns.

As described above, in the second embodiment, the equation of state shows the motion of the motion parameter estimation device 100 in addition to the motion of the object 20. Further, the observation equation includes the equation of the measured value of the motion of the motion parameter estimation device 100 in addition to the equation of the measured value of the motion of the object 20. The second estimation unit 196 calculates an estimated value of the motion parameter indicating the motion of the object 20 and the motion of the motion parameter estimation device 100.
As a result, the motion parameter estimation device 100 of the second embodiment can correct the measurement error of the motion of the motion parameter estimation device 100 itself. In this respect, the accuracy of estimating the motion of the object 20 can be further improved.

<Third Embodiment>
If a measured value of the sensor's speed can be obtained in addition to the sensor's own position, the estimated speed of the state, the focusing speed near the true value, or both can be further improved by using the measured value of the sensor's measurement. there is a possibility. Therefore, in the third embodiment, in addition to the expansion in the second embodiment, the speed of the sensor itself is further expanded to be included in the observed value vector.
The configuration of the motion parameter estimation device 100 in the third embodiment is the same as in the case of the first embodiment described with reference to FIG. Further, the processing procedure in the third embodiment is the same as the processing procedure in the case of the first embodiment described with reference to FIG. However, in the third embodiment, the mathematical model is further extended from the case of the second embodiment.
The observed value vector at the time of active sensing in the third embodiment is shown by Eq. (32).

x _Om (k)'indicates the measured value of the x-coordinate value of the sensor velocity at the timing k. y _Om (k)'indicates the measured value of the y coordinate value of the velocity of the sensor at the timing k.
Further, in the third embodiment, the matrix C ₂ is used instead of the matrix C in the equation (6). The matrix C ₂ is represented by Eq. (33).

Here, 0 _{2 × 6} indicates a zero matrix of 2 rows and 6 columns. 0 _{4 × 6} indicates a 4 row 6 column zero matrix. Further, the matrix C ₃ is expressed by the equation (34).

On the other hand, the measured value vector at the time of passive sensing is expressed by Eq. (35).

In this case, C ₄ (k) is used instead of the matrix C in the equation (6). C ₄ (k) is expressed as in equation (36).

Here, R ^{5 × 12} represents a real number matrix of 5 rows and 12 columns.
The matrix C ₅ is represented by Eq. (37).

As mentioned above, 0 _{4 × 6} represents a 4 row 6 column zero matrix. R4 ^{× 12} represents a real number matrix of 4 rows and 12 columns.

As described above, in the third embodiment, the observation equation includes the equation of the measured value of the velocity of the motion parameter estimation device 100. As a result, in the motion parameter estimation device 100 of the third embodiment, the motion estimation accuracy of the motion parameter estimation device 100 itself can be further improved, and thus the motion estimation accuracy of the object 20 can be further improved.

<Fourth Embodiment>
By measuring the Doppler effect by the active sensor 112, the velocity component in the sensor direction of the target velocity can be measured. By extending this to one of the measured values, the estimated speed of the state, the focusing speed near the true value, or both may be further improved. Therefore, in the fourth embodiment, the measured value of the velocity component in the sensor direction of the target velocity is extended to be included in the observed value vector. The fourth embodiment can be applied to any of the first to third embodiments.
The configuration of the motion parameter estimation device 100 in the fourth embodiment is the same as in the case of the first embodiment described with reference to FIG. Further, the processing procedure in the fourth embodiment is the same as the processing procedure in the case of the first embodiment described with reference to FIG. However, in the fourth embodiment, the mathematical model is extended as follows.

The Doppler effect of the active sensor 112 appears in the velocity component of the velocity of the object 20 in the direction of the measuring unit 110. Of the velocities of the object 20 at the timing k, the velocity component v _d (k) in the direction of the measuring unit 110 is expressed by the equation (38).

Here, v (k) indicates the speed of the target at the timing k. φ (k) indicates the traveling direction of the object as an angle with respect to the x-axis.
Using the state to be estimated (the component of the motion parameter vector q (k)), equation (38) is expressed as equation (39).

Equation (39) is added as one of the observation equations during active sensing.

As described above, in the fourth embodiment, the observation equation includes the equation of the measured value of the velocity of the object 20 based on the Doppler effect in the active sensor 112.
According to the motion parameter estimation device 100 in the fourth embodiment, the estimation accuracy of the motion of the object 20 can be further improved in that the measured value of the motion of the object 20 increases.

<Fifth Embodiment>
In the process shown in FIG. 5, the measured value by passive sensing (direction of the target as seen from the sensor) is always used in step S103. Therefore, in step S104, the error is always calculated using the measured value by passive sensing. On the other hand, at the time of active sensing, by using not only the azimuth of the target but also the measured value of the distance, the estimated speed of the state, the focusing speed near the true value, or both of them may be further improved.
Therefore, in the fifth embodiment, the error measurement method is switched between the active sensing and the passive sensing. At the time of active sensing, not only the azimuth of the target but also the measured value of the distance is used. Specifically, in step S103 of FIG. 5, the observation equation shown in the equation (6) is used at the time of active sensing.
On the other hand, at the time of passive sensing, the observation equation shown in Eq. (9) is used.
As for the fifth embodiment, the fifth embodiment can be applied to any of the first to fourth embodiments.

As described above, the first estimation unit 193 calculates the estimated value of the measured value of the measurement unit 110 by using the observation equations of the active sensor 112 and the passive sensor 111, respectively.
The accuracy of the estimated value calculated by the first estimation unit 193 is improved by the first estimation unit 193 calculating the estimated value of the measured value of the measuring unit 110 based not only on the direction of the object 20 but also on the position of the object 20. It is expected that this will be improved and the estimation accuracy of the motion of the object 20 will be further improved.

A program for realizing all or a part of the functions of the control unit 190 is recorded on a computer-readable recording medium, and the program recorded on the recording medium is read by the computer system and executed. May be processed. The term "computer system" as used herein includes hardware such as an OS and peripheral devices.
In addition, the "computer system" includes a homepage providing environment (or display environment) if a WWW system is used.
Further, the "computer-readable recording medium" refers to a portable medium such as a flexible disk, a magneto-optical disk, a ROM, or a CD-ROM, or a storage device such as a hard disk built in a computer system. Further, a "computer-readable recording medium" is a communication line for transmitting a program via a network such as the Internet or a communication line such as a telephone line, and dynamically holds the program for a short period of time. In that case, it also includes the one that holds the program for a certain period of time, such as the volatile memory inside the computer system that becomes the server or client. Further, the above-mentioned program may be a program for realizing a part of the above-mentioned functions, and may be a program for realizing the above-mentioned functions in combination with a program already recorded in the computer system.

Although the embodiments of the present invention have been described in detail with reference to the drawings, the specific configuration is not limited to this embodiment, and design changes and the like within a range not deviating from the gist of the present invention are also included.

1 Measurement system 10 Moving object 20 Object 100 Motion parameter estimation device 110 Measurement unit 111 Passive sensor 112 Active sensor 180 Storage unit 190 Control unit 191 Drive noise sample setting unit 192 Particle calculation unit 193 First estimation unit 194 Selection unit 195 Correction unit 196 Second estimation department

Claims (7)

A measuring unit that measures the position of an object using an active sensor and measures the direction of the object using a passive sensor in a cycle shorter than the measurement cycle of the active sensor.
A drive noise sample setting unit that sets a plurality of drive noise samples in the equation of state showing the motion of the object based on a predetermined probability distribution.
A particle calculation unit that calculates a plurality of particles in the particle filter by substituting each of the driving noise samples into the equation of state, and
The first estimation unit that calculates the estimated value of the measured value of the measurement unit by substituting each of the particles calculated by the particle calculation unit into the observation equation.
A selection unit that selects a part of the plurality of particles based on an error between the measured value of the measuring unit and the estimated value of the first estimation unit.
A second estimation unit that calculates an estimated value of a motion parameter indicating the motion of the object based on the particles selected by the selection unit.
A motor parameter estimator comprising. The equation of state shows the motion of the motion parameter estimator in addition to the motion of the object.
The observation equation includes the equation of the measured value of the motion of the motion parameter estimator in addition to the equation of the measured value of the motion of the object.
The second estimation unit calculates an estimated value of the motion parameter indicating the motion of the object and the motion of the motion parameter estimation device.
The motion parameter estimation device according to claim 1. The observation equation includes an equation for a measured value of the velocity of the motion parameter estimator.
The motion parameter estimation device according to claim 2. The observation equation includes an equation of a velocity measurement of the object based on the Doppler effect in the active sensor.
The motion parameter estimation device according to any one of claims 1 to 3. The first estimation unit calculates an estimated value of the measured value of the measurement unit using the observation equations of the active sensor and the passive sensor.
The motion parameter estimation device according to any one of claims 1 to 4. A measurement step of measuring the position of an object using an active sensor and measuring the direction of the object using a passive sensor in a cycle shorter than the measurement cycle of the active sensor.
A drive noise sample setting step of setting a plurality of drive noise samples in the equation of state showing the motion of the object based on a predetermined probability distribution, and
A particle calculation step of substituting each of the driving noise samples into the equation of state to calculate a plurality of particles in the particle filter, and
The first estimation step of substituting each of the particles calculated in the particle calculation step into the observation equation and calculating the estimated value of the measured value in the measurement step,
A selection step of selecting a part of the plurality of the particles based on the error between the measured value in the measurement step and the estimated value in the first estimation step, and
A second estimation step of calculating an estimated value of a motion parameter indicating the motion of the object based on the particles selected in the selection step, and
Exercise parameter estimation method including. On the computer
A measurement step of measuring the position of an object using an active sensor and measuring the direction of the object using a passive sensor in a cycle shorter than the measurement cycle of the active sensor.
A drive noise sample setting step of setting a plurality of drive noise samples in the equation of state showing the motion of the object based on a predetermined probability distribution, and
A particle calculation step of substituting each of the driving noise samples into the equation of state to calculate a plurality of particles in the particle filter, and
The first estimation step of substituting each of the particles calculated in the particle calculation step into the observation equation and calculating the estimated value of the measured value in the measurement step,
A selection step of selecting a part of the plurality of the particles based on the error between the measured value in the measurement step and the estimated value in the first estimation step, and
A second estimation step of calculating an estimated value of a motion parameter indicating the motion of the object based on the particles selected in the selection step, and
A program to execute.

Images