JP5291610B2 - Azimuth estimation apparatus, method, and program - Google Patents

Description

  The present invention relates to a technique for estimating an azimuth angle of an object.

  In recent years, research on autonomous mobile robots that can be used indoors and outdoors has been actively conducted, and their application destinations are expanding. As types of autonomous mobile robots, there are a type that moves on the ground surface using wheels, an autonomous aerial robot that moves by flying in the sky, an underwater type robot that navigates underwater, and the like. In order to establish an algorithm for controlling the behavior of an autonomous mobile robot with high accuracy, a technique for measuring the position and posture angle of the autonomous mobile robot with high accuracy in real time is essential.

  As a technique for measuring the position of an autonomous mobile robot, a position measurement technique using a GPS method or a wireless LAN method is known, and a technique using a gyro sensor or the like is known as a technique for measuring an attitude angle or an azimuth angle. However, the measurement accuracy of these position / angle measurement techniques may be insufficient to control an autonomous mobile robot. Therefore, a filter technique has been introduced in order to estimate a position with higher accuracy using measurement values obtained by these position measurement techniques (see, for example, Non-Patent Document 1). Most of these filter technologies are due to Bayesian filter applications. In order to estimate a position with high accuracy using a Bayes filter, a highly accurate movement model of an automatic mobile robot is required.

Ryuichi Ueda, Tamio Arai, Kazunori Asanuma, Kazunobu Umeda, Hisashi Osumi, “Recovery from fatal estimation errors caused by self-location estimation using particle filter”, Journal of the Robotics Society of Japan Vol. 23, No. 4, pp.466-473, 2005

  There are small gyro sensors that measure angular acceleration, but laser gyros and other devices that accurately measure absolute angles are often heavy, making it difficult to mount them on small autonomous airship robots. . In addition, some autonomous mobile robots have a limited payload weight, such as autonomous airship robots, and may not be equipped with sensors that meet the required performance due to the size of the robot itself. is there.

  The technique of Non-Patent Document 1 is based on the premise that a sensor for measuring the azimuth angle of the robot is mounted. Therefore, the azimuth angle is measured and estimated in a situation where such a sensor is not mounted. There is a problem that cannot be done.

  In order to solve the above-described problems, in the present invention, the azimuth angle of the object is estimated using the measurement value of the sensor that measures the position mounted on the object.

  Even without a sensor for measuring the azimuth, the azimuth of the object can be estimated.

The functional block diagram of the example of an azimuth angle estimation apparatus. The flowchart of the example of an azimuth angle estimation method. The figure which illustrates the target object used as the object of azimuth angle presumption. The figure which illustrates the indication value to an actuator. The figure which illustrates the change of azimuth angular velocity (PHI) 'corresponding to the instruction | indication value of FIG. The figure for demonstrating the calculation method 2. FIG.

[Object]
The target object whose azimuth angle is to be estimated is, for example, an autonomous mobile robot such as the autonomous airship illustrated in FIG. This autonomous airship is equipped with a rudder 2, a main propulsion device 3, and a vertical propulsion device 4. Although not shown, a thruster in the lateral direction (azimuth angle direction) is provided. Since the center of gravity of the autonomous airship is set low, the restoring force with respect to the attitude inclination is large, and the attitude angle of the autonomous airship is maintained at 0 except for the azimuth angle. Hereinafter, a case where the target object is an autonomous mobile robot will be described as an example. Note that the target object for state estimation is not limited to the autonomous mobile robot.

  Five wireless LAN type position measuring sensors 61, 62, 63, 64, 65 are attached to the gondola portion of the autonomous mobile robot of FIG. In this example, these position measurement sensors 61, 62, 63, 64, 65 are centered on the center of gravity position (Xg (t), Yg (t), Zg (t)) in the horizontal plane (XY plane) of the autonomous mobile robot. It is assumed that the point object is arranged in the horizontal plane. The position measurement sensors 61, 62, 63, 64, 65 are Φs1, Φs2, Φs3, Φs4, and Φs5 with azimuth angles from the center-of-gravity position (Xg (t), Yg (t), Zg (t)) in the horizontal plane. Are fixed at positions separated by distances Rs1, Rs2, Rs3, Rs4, and Rs5. Each position measurement sensor 61, 62, 63, 64, 65 can measure a horizontal plane position (XY) and altitude (Z).

  The measured values of the position measurement sensors 61, 62, 63, 64, and 65 at time step t are Z1 (t), Z2 (t), Z3 (t), Z4 (t), and Z5 (t), respectively.

Z1 (t) = (Zx1 (t), Zy1 (t), Zz1 (t))
Z2 (t) = (Zx2 (t), Zy2 (t), Zz2 (t))
Z3 (t) = (Zx3 (t), Zy3 (t), Zz3 (t))
Z4 (t) = (Zx4 (t), Zy4 (t), Zz4 (t))
Z5 (t) = (Zx5 (t), Zy5 (t), Zz5 (t))
Then, these average values (Zxa (t), Zya (t), Zza (t)) can be calculated as follows.

Zxa (t) = [Zx1 (t) + Zx2 (t) + Zx3 (t) + Zx4 (t) + Zx5 (t)] / 5 (a)
Zya (t) = [Zy1 (t) + Zy2 (t) + Zy3 (t) + Zy4 (t) + Zy5 (t)] / 5 (b)
Zza (t) = [Zz1 (t) + Zz2 (t) + Zz3 (t) + Zz4 (t) + Zz5 (t)] / 5

  Since the sensors are arranged as point objects, the above average values (Zxa (t), Zya (t), Zza (t)) are handled as measured values of the center of gravity (Xg, Yg, Zg) of the autonomous airship. Can do. Hereinafter, the average value (Zxa (t), Zya (t), Zza (t)) will be described as the measured value of the gravity center position (Xg, Yg, Zg) of the autonomous airship.

  It is assumed that the autonomous airship is stable in rotational movement in the azimuth direction due to the effect of the vertical tail mounted on the stern.

[Control of autonomous airship]
In the present invention, the instruction value given to the actuator (for example, the lateral propulsion device) of the autonomous airship is controlled so that the azimuth velocity at each action selection time becomes a constant value 0. Further, the control is performed so as to assume that the forward speed is always a constant value Vx. That is, at each time step, the instruction value given to the actuator of the autonomous airship is controlled as shown in FIG. Here, Ta is a time for giving a constant value (azimuth direction torque M) in the acceleration direction, Td is a time for giving a constant value (azimuth direction torque M) in the deceleration direction, and Ts is an azimuth direction of the autonomous airship itself. This is the time for adjusting the azimuth velocity to zero using the velocity stability characteristic.

  4 is given to the actuator of the autonomous airship, the azimuth velocity Φ ′ of the autonomous airship becomes a step response as shown in FIG. The time corresponding to one time step is T = Ta + Td + Ts. The ratio of the length of time of Ta and Td is determined based on the relationship between the indication value given to the actuator and the azimuth velocity separately measured. In addition, the length of time between Ta and Td is set so that the azimuth velocity of the autonomous airship is as close to 0 as possible at the end of Td. Further, in order to keep the forward speed constant, the actuator force in the forward direction has a constant value as an instruction value.

[Bayes filter]
Hereinafter, the Bayes filter will be briefly described. See Reference 1 for details on Bayes filters.
[Reference 1] Llan Sebastian (Author), Bargard Wolfram (Author), Fox Dieter (Author), Ryuichi Ueda (Translation), "Stochastic Robotics", Mainichi Communications, 2007/10/25

  The Bayes filter is an algorithm that repeats (1) prediction and (2) measurement update described below. Hereinafter, the estimation of the azimuth angle Φ (t) will be described as an example.

(1) Prediction
bel0 (Φ (t)) = ∫p (Φ (t) | u (t), Φ (t-1)) ・ bel (Φ (t-1)) dΦ (t-1)
(2) Measurement update
bel (Φ (t)) = ηp (Zφ (t) | Φ (t)) ・ bel0 (Φ (t))

  bel (Φ (t)) is the probability that the azimuth angle of the autonomous airship is Φ (t) at time step t. In order to obtain bel (Φ (t)) at time step t, first, in (1) prediction step, bel (Φ (t−1)) one time step before is estimated by an azimuth transition model. The estimated value bel0 (Φ (t)) of bel (Φ (t)) at time step t is multiplied by the state transition probability p (Φ (t) | u (t), Φ (t-1)) of the mobile robot. Ask for. Here, u (t) is the control input of the autonomous mobile robot at time step t.

  Next, in (2) the measurement update step, the estimated value bel0 (Φ (t)) is corrected using the measured value Zφ (t) of the azimuth sensor actually measured at time step t, and the time Bel (Φ (t)) of step t is obtained. Specifically, when the state of the autonomous mobile robot is Φ (t), correction of bel0 is performed using a probability value by which the measured value Zφ (t) is obtained by the position measurement sensor. Note that η is a positive constant.

[Binary Bayes filter]
The Bayesian filter described above is only a mathematical concept, so another representation is required to implement it on a computer. Known implementations of Bayes filters include particle filters, Monte Carlo localization, and binary Bayes filters. Hereinafter, a binary Bayes filter deeply related to the method of the present invention will be briefly described.

  The binary Bayes filter is a Bayesian filter written in a discrete representation that is suitable for use in a computer. Each step of (1) prediction and (2) calculation update in the binary Bayes filter is described as follows, for example.

(1-1) Prediction
Bel0 (Φs (t)) = Σp (Φs (t) | a (t), Φs (t-1)) ・ Bel (Φs (t-1))
(1-2) Measurement update
Bel (Φs (t)) = ηP (Zφs (t) | Φs (t)) ・ Bel0 (Φs (t))

  Here, Φs (t) represents the azimuth angle of the autonomous airship at time step t as a discrete value. For example, when the angle of one rotation from 0 degrees to 360 degrees is divided into 36, the value of Φs (t) takes one of 36 discrete values. For example, an integer from 1 to 36 is assigned to 36 Φs (t).

  Bel (Φs (t)) is the probability that the azimuth angle of the autonomous airship is Φs (t) at time step t. In order to obtain Bel (Φs (t)) at time step t, first, in (1-1) prediction step, Bel (Φs (t−1)) one time step before is estimated by an azimuth transition model. The estimated value Bel0 (Φs (t)) of Bel (Φs (t)) at time step t is obtained by multiplying the state transition probability of the autonomous mobile robot. Here, the azimuth transition model is represented by an azimuth transition probability p (Φs (t) | a (t), Φs (t−1)). Here, a (t) is a discrete expression of the action taken by the robot at time step t, and is an identifier for identifying a control input value given to the autonomous mobile robot at that time step. That is, the robot selects one action at each time step among positive integer number of actions, and gives the control input value associated with the action to the actuator of the autonomous mobile robot.

  Next, in the (1-2) measurement update step, the estimated value Bel0 (Φs (t)) is calculated using the discrete value Zφs (t) of the measured value of the azimuth sensor actually measured at time step t. Correction is performed to obtain Bel (Φs (t)) at time step t. Here, P (Zφs (t) | Φs (t)) is a discrete value of the measured value of the azimuth sensor when the true value of the azimuth angle of the autonomous airship is Φs (t) at time step t. Zφs (t) is a probability value to be measured.

  If the binary Bayes filter described above is used, the Bayes filter can be implemented on a computer. However, since the present invention presupposes a situation in which an azimuth sensor for measuring the azimuth angle cannot be mounted, the “discrete value Zφs (t) of the measured value of the azimuth angle sensor” necessary for the above calculation method is obtained. I can't. Therefore, this method cannot be used as it is. Therefore, in the present invention, the measurement values (average values of the measurement values of the five sensors) Zxa (t), Zya (t), Zxa (t−1), Zya (t) at the time steps t and t−1. (1-2) Perform measurement update processing using (-1).

[basic way of thinking]
In the present invention, the measured values (average values of the measured values of the five sensors) at the time steps t and t−1 are Zxa (t), Zya (t), Zxa (t−1), Zya (t− 1), and instead of P (Zφs (t) | Φs (t)), the probability P (Zxa (t), Zya (t), Zxa (t−1), Zya (t−1) | Φs ( t-1) and a (t-1)) are used. That is, when the object having the azimuth angle Φs (t−1) takes the action a (t−1) at time step t−1, (Zxa (t−1), Zya ( t-1)), and the probability of obtaining a measurement result of being located at (Zxa (t), Zya (t)) at time step t is used. A method of calculating the probability P (Zxa (t), Zya (t), Zxa (t−1), Zya (t−1) | Φs (t−1), a (t−1)) will be described later.

  This displacement is appropriate because the position of the autonomous airship at each time is uniquely determined by the azimuth and control input of the autonomous airship at each time. When this replacement is performed, the binary Bayes filter is as follows.

(2-1) Prediction before one time step
Bel0 (Φs (t-1)) = Σp (Φs (t-1) | a (t-2), Φs (t-2)) ・ Bel (Φs (t-2))
(2-2) Measurement update
Bel (Φs (t-1)) = ηP (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1))・ Bel0 (Φs (t-1))
(2-3) Prediction of current time
Bel0 (Φs (t)) = Σp (Φs (t) | a (t-1), Φs (t-1)) ・ Bel (Φs (t-1))

  What is important here is not the probability Bel (Φs (t)) that the azimuth angle of the autonomous airship is Φs (t) at the time step t, which is obtained when the (2-2) measurement update step is completed. The time point t-1 is the point at which the azimuth angle of the autonomous airship becomes a probability Bel (Φs (t-1)) that is Φs (t-1). This is because the update using P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1)) This is caused by being performed for the azimuth angle Φs (t−1) before the time step (time step t−1).

  Therefore, in order to obtain the probability Bel (Φs (t)) that the azimuth angle of the autonomous airship is Φs (t) at the current time step t, (2-3) the current The time prediction step is calculated. Then, (0-3) Bel0 (Φs (t)) obtained in the current time prediction step is used to calculate the estimated value of the azimuth angle.

  Here, considering that the above calculation is implemented on a computer and repeated for each time, the calculations of (2-1) and (2-3) are redundant at adjacent times. The redundant part is organized as follows.

(3-1) Measurement update
Bel (Φs (t-1)) = ηP (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1))・ Bel0 (Φs (t-1))
(3-2) Prediction of current time
Bel0 (Φs (t)) = Σp (Φs (t) | a (t-1), Φs (t-1)) ・ Bel (Φs (t-1))

  In one embodiment of the azimuth angle estimation device described below, the azimuth angle of the autonomous airship is estimated according to the steps (3-1) and (3-2) described above.

[Embodiment]
FIG. 1 is a functional block diagram of an example of an azimuth estimation device according to the present invention. FIG. 2 is a flowchart of an example of an azimuth angle estimation method according to the present invention.

  As illustrated in FIG. 1, the azimuth estimation device includes a storage unit 11, an action input unit 12, an initial value setting unit 13, a time setting unit 14, a position measurement probability calculation unit 15, an estimated displacement amount calculation unit 16, and a comparison unit. 17, measurement update unit 18, control unit 19, prediction unit 20, azimuth angle estimation unit 21, normalization unit 22, position measurement sensors 61, 62, 63, 64, 65, centroid calculation unit 66, and lattice position acquisition unit 67. For example.

  The storage unit 11 stores the action a (t) given to the actuator of the target object at each time step t. Moreover, the estimated value of the azimuth angle of the target object in each past time step t calculated by the azimuth angle estimation unit 21 may be stored.

The action a (t) of the target object at each time step t is automatically determined based on the position of the target object in the XY plane and the estimated azimuth angle at each time step t, for example. Alternatively, the person who remotely operates the object may be determined by the operation.
[Reference Document 2] Japanese Patent Application Laid-Open No. 2007-317165

<Step S1>
The initial value setting unit 13 sets the time step t to the initial value 0, and sets the initial value of Bel0 (Φs (0)) at the time step t = 0 (step S1). When the azimuth angle of the object is determined in advance and the symmetrical object is arranged at the azimuth angle, for example, Bel0 (Φs (0)) = 1 of Φs corresponding to the azimuth angle is set and other Φs are set. Bel0 (Φs (0)) = 0.

<Step S2>
The time setting unit 14 sets t = t + 1. That is, the time step t is incremented by 1 (step S2).

<Step S3>
The position measurement probability calculation unit 15 determines (Zxa (t) at time step t-1 when the object having the azimuth angle Φs (t-1) takes action a (t-1) at time step t-1. -1), Zya (t-1)) and a probability P (Zxa (t), Zya (t) that a measurement result is obtained that is located at (Zxa (t), Zya (t)) at time step t. ), Zxa (t−1), Zya (t−1) | Φs (t−1), a (t−1)) are calculated (step S3). As will be described later, the processing in step S3 and step S4 is performed for discrete values Φs (t−1) of all azimuth angles. The calculated probabilities P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1)) are measured and updated by the measurement updating unit 18. Sent to.

  The position measurement probability calculation unit 15 reads the action a (t−1) used for the above calculation from the storage unit 11, and Zxa (t), Zya (t), Zxa (t−1), Zya (t−1). ) Is acquired from the center of gravity calculation unit 66. The center-of-gravity calculation unit 66 obtains Z1 (t), Z2 (t), Z3 (t), Z4 (t), Z5 (t), Z1 acquired from the position measurement sensors 61, 62, 63, 64, 65, respectively. From (t-1), Z2 (t-1), Z3 (t-1), Z4 (t-1), and Z5 (t-1), Zxa (t ), Zya (t), Zxa (t−1), Zya (t−1) are calculated and sent to the position measurement probability calculation unit 15. Zxa (t) and Zya (t) are also sent to the lattice position acquisition unit 67 and the comparison unit 17 as necessary.

  The probability P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1)) is, for example, the following << calculation method 1 >> to << Calculation method 4 >> can be calculated.

  Note that the XY plane on which the object is located is divided by a grid having an area Δs, and the grid corresponding to the centroid based on the centroid (Zxa (t), Zya (t)) calculated by the centroid calculation unit 66. The lattice position acquisition unit 67 acquires s (t) and the coordinates (Xs (t), Ys (t)) of the lattice s (t). The lattice corresponding to the center of gravity is, for example, a lattice including the center of gravity. The coordinates (Xs (t), Ys (t)) are output to the position measurement probability calculation unit 15. Further, if necessary, the coordinates (Xs (t-1), Ys (t-1)) of the lattice s (t), the lattice s (t-1), and the lattice s (t-1) are also position measurement probabilities. It is sent to the calculation unit 15. The size of the lattice is determined in consideration of the measurement accuracy of the position measurement sensors 61, 62, 63, 64, 65. The higher the measurement accuracy, the smaller the grid size.

  Further, the estimated displacement amount on the XY plane when the object having the azimuth angle Φs (t−1) takes the action a (t−1) at the time step t−1 is expressed as (Dx (Φs (t−1), a (T-1)), Dy (Φs (t-1), a (t-1)), and based on the azimuth angle Φs (t-1) and the action a (t-1), the estimated displacement calculation unit 16 calculates an estimated displacement amount (Dx (Φs (t−1), a (t−1)), Dy (Φs (t−1), a (t−1)). For example, based on the following equation: The estimated displacement amount is output to the position measurement probability calculation unit 15.

  In the above equation, Vx is the forward speed of the object, Φs (t−1) r is a representative value of the azimuth angle (a discrete value thereof) Φs (t−1), for example, the median value of Φs (t−1), dΦ (Τ) is the amount of change in azimuth at time τ when the object takes action a (t−1). Note that any point of the lattice Φs (t−1) may be a representative value of Φs (t−1). τ represents the elapsed time in each time step t. That is, when each time step t starts, τ = 0 is set, and thereafter increases with the passage of time. When τ = T is reached, time step t + 1 is set, and τ = 0 is set again. An instruction value to be given to the actuator of the object is determined based on the action a (t−1) as shown in FIG. 4, for example, and an azimuth velocity change amount dΦ (τ) is determined based on this instruction value as shown in FIG. .

≪Calculation method 1≫
The calculation method 1 is to obtain a position measurement probability using a normal distribution.

  The position measurement probability calculation unit 15 calculates a value of the following formula and uses the calculation result as a probability P (Zxa (t), Zya (t), Zxa (t−1), Zya (t−1) | Φs ( t-1) and a (t-1)).

Here, R ₁ = (Xs (t) − (Dx (Φs (t−1), a (t−1)) + Xs (t−1)) ² + (Ys (t) − (Dy (Φs (t −1), a (t−1)) + Ys (t−1)) ² ), and the variance σ ² is a predetermined constant.

≪Calculation method 2≫
The position measurement probability calculation unit 15 uses the lattice s (t−1) corresponding to the measurement value (Zxa (t−1), Zya (t−1)) of the center of gravity of the object at time t−1 as (Dx (Φs (T-1), a (t-1)), Dy (Φs (t-1), a (t-1))), the lattice s ′ (t−1) obtained by translating the XY plane by time A value obtained by dividing an area overlapping the grid s (t) corresponding to the measured value (Zxa (t), Zya (t)) of the center of gravity of the object at t (area of the hatched portion in FIG. 6) by Δs is calculated. The calculation result is a probability P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1)).

The calculation method 2 is based on the assumption that the object is present in the previous lattice where the object overlaps with a probability proportional to the area where the lattice s ′ (t−1) overlaps with another lattice. ≪Calculation method 3≫
The position measurement probability calculation unit 15 calculates a value of the following formula and uses the calculation result as a probability P (Zxa (t), Zya (t), Zxa (t−1), Zya (t−1) | Φs ( t-1) and a (t-1)).

In the above equation, Φtan = arctan ((Zya (t) −Zya (t−1)) / (Zxa (t) −Zxa (t−1))), and Φs (t−1) r is changed to Φs (t −1) as a representative value, and DΦ (a (t−1)) as an azimuthal displacement when the object takes action a (t−1), R ₂ = (Φ tan− (DΦ (a (T−1)) + Φs (t−1) r)), R ₂ = (Φtan−Φs (t−1) r) or R ₂ = (Φtan− (0.5 × DΦ (a (t−1))) + Φs (T-1) r)), and the variance σ is a predetermined constant.

  When the object is an autonomous airship, the azimuth angle often coincides with the traveling direction. This is particularly noticeable when the speed of the autonomous airship in the traveling direction is sufficiently high. Calculation method 3 is based on this property.

≪Calculation method 4≫
When the displacement vector ((Zya (t) -Zya (t-1)), (Zxa (t) -Zxa (t-1))) of the autonomous airship becomes smaller (for example, the airship turns around on the spot) The accuracy of the calculation method 3 rapidly decreases.

  Therefore, in the calculation method 4, when the displacement vector ((Zya (t) −Zya (t−1)), (Zxa (t) −Zxa (t−1))) is small, the calculation is performed by the calculation method 1. If it is larger, the calculation method 3 is used.

Specifically, the comparison unit 17 determines the magnitude of the displacement vector ((Zxa (t) −Zxa (t−1)) ² + (Zya (t) −Zya (t−1)) ² ) ^{(1/2 )} And a predetermined threshold value, and if the predetermined threshold value is larger, the probability P (Zxa (t), Zya (t), Zxa (t-1), Zya (t− 1) When | Φs (t−1) is calculated and the predetermined threshold value is smaller, the probability P (Zxa (t), Zya (t), Zxa (t−1), Zya is calculated by the calculation method 3. (T−1) | Φs (t−1) is calculated.

<Step S4>
The measurement update unit 18 includes the probabilities P (Zxa (t), Zya (t), Zxa (t−1), Zya (t−1) | Φs (t−1), calculated by the position measurement probability calculation unit 15. a (t-1)) is used to perform measurement update corresponding to the above (3-1) (step S4).

  Specifically, the measurement update unit 18 calculates the probability P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1 )) And the pre-update probability Bel0 (Φs (t−1)) in which the azimuth angle of the object is Φs (t−1) at time step t−1, Bel (Φs (t−1)) = P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1)) · Bel0 (Φs (t-1) ), The updated probability Bel (Φs (t−1)) where the azimuth angle of the object is Φs (t−1) is calculated at time step t−1. The pre-update probability Bel0 (Φs (t−1)) is read from the storage unit 11. The calculated post-update probability Bel (Φs (t−1)) is sent to the prediction unit 20.

<Step S5>
The control unit 19 determines whether or not the processing of step S3 and step S4 has been performed for the discrete values Φs (t−1) of all azimuth angles (step S5). If it is determined that the processing is not performed for all the azimuth discrete values Φs (t−1), the azimuth discrete values Φs (t−1) that have not been processed are selected and the step is performed. Return to S3. If it is determined that the processing has been performed on the discrete values Φs (t−1) of all azimuths, the process proceeds to step S6.

<Step S6>
The prediction unit 20 uses the post-update probability Bel (Φs (t−1)) calculated by the measurement update unit 18 to perform prediction processing of the current time corresponding to the above (3-2) (step S6). .

Specifically, when the object having the azimuth angle Φs (t−1) takes action a (t−1) at time step t−1, the prediction unit 20 determines the object at time step t−1. Using the probability p (Φs (t) | a (t−1), Φs (t−1)) that the azimuth is Φs (t) and the updated probability Bel (Φs (t−1)), Bel0 (Φs (t)) = _{ΣΦs (t−1)} p (Φs (t) | a (t−1), Φs (t−1)) · Bel (Φs (t−1)) Then, the pre-update probability Bel0 (Φs (t)) in which the azimuth angle of the object is Φs (t) at time step t is calculated. The pre-update probability Bel0 (Φs (t)) is sent to the azimuth angle estimating unit 21 and stored in the storage unit 11.

  The probability p (Φs (t) | a (t−1), Φs (t−1)) can be calculated by the following equation, for example.

In the above formula, R ₃ = (Φs (t) − (DΦ (a (t−1)) + Φs (t−1))) ² . DΦ (a (t−1)) is the displacement amount of the azimuth when the object takes action a (t−1). Let τ be the elapsed time in each time step t, T be the length of one time step, and Φ ′ (τ) be the azimuth velocity at each time τ when the object takes action a (t−1). , DΦ (a (t−1)) can be calculated by the following equation.

<Step S61>
The control unit 19 determines whether or not the process of step S6 has been performed for the discrete values Φs (t) of all azimuth angles (step S61). If it is determined that processing has not been performed for all azimuth angle discrete values Φs (t), the azimuth angle discrete value Φs (t) that has not yet been processed is selected, and the process returns to step S6. If it is determined that the processing has been performed on the discrete values Φs (t) of all azimuths, the process proceeds to step S7.

  Thereby, the process of step S6 is performed for discrete values Φs (t) of all azimuth angles.

<Step S7>
The azimuth angle estimation unit 21 _{calculates ΣΦs (t)} Φs (t) · Bel0 (Φs (t)) using the pre-update probability Bel0 (Φs (t)), and the calculation result is time step t. The estimated value of the azimuth angle of the object at (step S7).

The azimuth angle estimation unit 21 uses the updated probability Bel (Φs (t−1)) at time step t−1 to calculate _{ΣΦs (t−1)} Φs (t−1) · Bel0 (Φs (t -1)) may be calculated, and the calculation result may be an estimated value of the azimuth angle of the object at time step t-1.

<Step S8>
The control unit 19 determines whether or not to continue prediction for the next time step (step S8), and returns to step S2 when prediction is made for the next time step. If no more predictions are made, the process ends.

  In this way, by performing measurement update corresponding to the above (3-1) using the measurement values of the position measurement sensors 61, 62, 63, 64, and 65, the azimuth angle without using the azimuth angle measurement sensor. Can be estimated.

[Modifications, etc.]
The normalization unit 22 may normalize Bel (Φs (t−1)) so that _{ΣΦs (t−1)} Bel (Φs (t−1)) = 1 after Step S5. That is, each Bel (Φs (t−1)) may be replaced with Bel (Φs (t−1)) / _{ΣΦs (t−1)} Bel (Φs (t−1)). Similarly, the normalization unit 22 may normalize Bel0 (Φs (t)) so that _{ΣΦs (t)} Bel0 (Φs (t)) = 1 after step S61. That is, each Bel0 (Φs (t)) may be replaced with Bel0 (Φs (t)) / _{ΣΦs (t)} Bel0 (Φs (t)). By performing normalization, the azimuth angle estimation accuracy is increased.

  Data exchange between the units of the azimuth estimation device may be performed directly or may be performed via the storage unit 11.

  The azimuth angle estimation device can be realized by a computer. In this case, the processing contents of each function that the apparatus should have are described by a program. Then, by executing this program on a computer, each processing function in this apparatus is realized on the computer.

  The program describing the processing contents can be recorded on a computer-readable recording medium. In this embodiment, these devices are configured by executing a predetermined program on a computer. However, at least a part of these processing contents may be realized by hardware.

  The present invention is not limited to the above-described embodiment, and can be modified as appropriate without departing from the spirit of the present invention.

15 Position measurement probability calculation unit 17 Comparison unit 18 Measurement update unit 20 Prediction unit 21 Azimuth angle estimation units 61, 62, 63, 64, 65 Position measurement sensor

Claims (7)

When the object having the azimuth angle Φs (t−1) takes action a (t−1) at time step t−1, (Zxa (t−1), Zya (t−1) at time step t−1. )) And a probability P (Zxa (t), Zya (t), Zxa (t−1) that a measurement result that the position is located at (Zxa (t), Zya (t)) is obtained at time step t. ), Zya (t−1) | Φs (t−1), a (t−1));
The probability P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1)) and time step t-1 Bel (Φs (t−1)) = P (Zxa (t), Zya () using the pre-update probability Bel0 (Φs (t−1)) where the azimuth angle of the object is Φs (t−1). t), Zxa (t−1), Zya (t−1) | Φs (t−1), a (t−1)) · Bel0 (Φs (t−1)) A measurement update unit that calculates an updated probability Bel (Φs (t−1)) in which the azimuth angle of the object is Φs (t−1) in 1;
Probability that the azimuth angle of the object becomes Φs (t) at time step t-1 when the object of azimuth angle Φs (t-1) takes action a (t-1) at time step t-1. Using p (Φs (t) | a (t−1), Φs (t−1)) and the updated probability Bel (Φs (t−1)), Bel0 (Φs (t)) = Σ _Orientation of the object at time step t satisfying the relationship of _{Φs (t−1)} p (Φs (t) | a (t−1), Φs (t−1)) · Bel (Φs (t−1)) A prediction unit for calculating a pre-update probability Bel0 (Φs (t)) whose angle is Φs (t);
Using the pre-update probability Bel0 (Φs (t)) or the post-update probability Bel (Φs (t−1)), _{ΣΦs (t)} Φs (t) · Bel0 (Φs (t)) or _{ΣΦs ( t-1)} Calculate Φs (t-1) · Bel (Φs (t-1)), and use the calculation result as an estimated value of the azimuth of the object at time step t or time t-1. An estimation unit;
An azimuth estimation device including In the azimuth angle estimating device according to claim 1,
Assume that the XY plane on which the object is located is divided by a grid of area Δs, and the coordinates of the grid s (t) on which the object is located at time step t are (Xs (t), Ys (t)) In step t-1, the estimated displacement in the XY plane when the object having the azimuth angle Φs (t−1) takes action a (t−1) is expressed as (Dx (Φs (t−1), a (t− 1)), Dy (Φs (t−1), a (t−1)),) and R ₁ = (Xs (t) − (Dx (Φs (t−1), a (t−1))) + Xs (t−1)) ² + (Ys (t) − (Dy (Φs (t−1), a (t−1)) + Ys (t−1)) ² ), and the dispersion σ is predetermined. As a constant

The position measurement probability calculation unit calculates the value of the above formula, and calculates the result as the probability P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (T-1), a (t-1))
An azimuth estimation device characterized by the above. In the azimuth angle estimating device according to claim 1,
Assume that the XY plane on which the object is located is divided by a grid of area Δs, and that the lattice on which the object is located at time step t is s (t), and (Dx (Φs (t−1), a (t− 1)), Dy (Φs (t-1), a (t-1))) at time step t-1, the object of azimuth angle Φs (t-1) took action a (t-1) As an estimated displacement amount in the XY plane,
The lattice s (t-1) is translated on the XY plane by (Dx (Φs (t-1), a (t-1)), Dy (Φs (t-1), a (t-1))). The value obtained by dividing the area where the lattice s ′ (t−1) overlaps the lattice (s) by Δs is calculated, and the calculation result is expressed as the probability P (Zxa (t), Zya (t), Zxa (t -1), Zya (t-1) | Φs (t-1), a (t-1))
An azimuth estimation device characterized by the above. In the azimuth angle estimating device according to claim 1,
It is assumed that the XY plane on which the object is located is divided by a grid having an area Δs, the position of the object at time step t is (Zxa (t), Zya (t)), and Φtan = arctan ((Zya (t) −Zya (t−1)) / (Zxa (t) −Zxa (t−1))), the median value of Φs (t−1) is Φs (t−1) r, and DΦ (a (t− 1)) is the amount of displacement of the azimuth when the object takes action a (t-1), and R ₂ = (Φ tan− (DΦ (a (t−1)) + Φs (t−1) r) ), R ₂ = (Φ tan−Φ s (t−1) r) or R ₂ = (Φ tan− (0.5 × DΦ (a (t−1)) + Φ s (t−1) r)), As a fixed constant,

The position measurement probability calculation unit calculates the value of the above formula, and calculates the result as the probability P (Zxs (t), Zys (t), Zxs (t−1), Zys (t−1) | Φs. (T-1), a (t-1))
An azimuth estimation device characterized by the above. In the azimuth angle estimating device according to claim 1,
A comparison unit that compares the displacement vector ((Zxa (t) −Zxa (t−1)) ² + (Zya (t) −Zya (t−1)) ² ) ^(1/2) with a predetermined threshold;
If the displacement vector is smaller, it is assumed that the XY plane on which the object is located is divided by a grid of area Δs, and the coordinates of the lattice s (t) at which the object is located at time step t are (Xs (t) , Ys (t)), and the estimated displacement amount in the XY plane when the object having the azimuth angle Φs (t−1) takes the action a (t−1) at time step t−1 is (Dx (Φs ( t-1), a (t-1)), Dy (Φs (t-1), a (t-1)),), and R ₁ = (Xs (t)-(Dx (Φs (t-1) ), A (t−1)) + Xs (t−1)) ² + (Ys (t) − (Dy (Φs (t−1), a (t−1)) + Ys (t−1)) ² ) And the variance σ is a predetermined constant,

The position measurement probability calculation unit calculates the value of the above formula, and calculates the result as the probability P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (T-1), a (t-1))
If the displacement vector is larger, the XY plane on which the object is located is divided by a grid having an area Δs, and the position of the object at time step t is (Zxa (t), Zya (t)). Φtan = arctan ((Zya (t) −Zya (t−1)) / (Zxa (t) −Zxa (t−1))), and DΦ (a (t−1)) is the action a ( The displacement amount of the azimuth angle when t-1) is taken, and R ₂ = (Φ tan− (DΦ (a (t−1)) + Φs (t−1) r)), R ₂ = (Φ tan−Φs ( t−1) r) or R ₂ = (Φ tan− (0.5 × DΦ (a (t−1)) + Φs (t−1) r)), and the variance σ is a predetermined constant,

The position measurement probability calculation unit calculates the value of the above formula, and calculates the result as the probability P (Zxs (t), Zys (t), Zxs (t−1), Zys (t−1) | Φs. (T-1), a (t-1))
An azimuth estimation device characterized by the above. When the position measurement probability calculation unit takes the action a (t-1) at the time step t-1, the object having the azimuth angle Φs (t-1) takes the action (Zxa (t-1 ), Zya (t-1)) and the probability P (Zxa (t), Zya (t), Zxa (t-1) located at (Zxa (t), Zya (t)) at time step t. , Zya (t−1) | Φs (t−1), a (t−1))
The measurement updating unit calculates the probability P (Zxa (t), Zya (t), Zxa (t-1), Zya (t-1) | Φs (t-1), a (t-1)) and time Bel (Φs (t−1)) = P (Zxa () using the pre-update probability Bel0 (Φs (t−1)) in which the azimuth angle of the object is Φs (t−1) in step t−1. t), Zya (t), Zxa (t−1), Zya (t−1) | Φs (t−1), a (t−1)) · Bel0 (Φs (t−1)) A measurement update step of calculating an updated probability Bel (Φs (t−1)) that the azimuth angle of the object is Φs (t−1) at time step t−1;
When the prediction unit takes the action a (t-1) at the time step t-1, the azimuth angle of the object is Φs (t-1) at the time step t-1. ) Using the probability p (Φs (t) | a (t−1), Φs (t−1)) and the updated probability Bel (Φs (t−1)), Bel0 (Φs (t )) = Σ _{Φs (t−1)} p (Φs (t) | a (t−1), Φs (t−1)) · Bel (Φs (t−1)) A prediction step of calculating a pre-update probability Bel0 (Φs (t)) in which the azimuth angle of the object is Φs (t);
The azimuth angle estimation unit uses the pre-update probability Bel0 (Φs (t)) or the post-update probability Bel (Φs (t−1)) to calculate _{ΣΦs (t)} Φs (t) · Bel0 (Φs (t )) Or [Sigma] [ _{Phi] s (t-1) [} Phi] s (t-1) .Bel ([Phi] s (t-1)) and calculate the result of the azimuth of the object at time step t or time t-1. Azimuth angle estimation step as an estimated value;
Azimuth estimation method including   An azimuth estimation program for causing a computer to function as each part of the azimuth estimation apparatus according to claim 1.

Images