JP2016109608A - Posture estimation device and control program for posture estimation device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To estimate a posture while suppressing a processing load.SOLUTION: A posture estimation device, provided in a portable apparatus 1 equipped with an acceleration sensor 80 for periodically detecting acceleration in three directions and successively outputting the detection result as acceleration data a and an angular velocity sensor 90 for periodically detecting angular velocity in three directions and successively outputting the detection result as angular velocity data ω, for estimating the posture q of the portable apparatus 1, the posture estimation device being characterized by including: a posture prediction unit 21 for predicting, on the basis of the angular velocity data ω, the posture q of the portable apparatus 1 at the current time of day from the posture q of the portable apparatus 1 at a time of day past the current time of day; a posture correction unit 22 for correcting the prediction result of the posture prediction unit 21 on the basis of the acceleration data a; and an estimated posture output unit 23 for outputting either the prediction result of the posture prediction unit 21 or the correction result of the posture correction unit 22 as the estimated value of the posture q of the portable apparatus 1.SELECTED DRAWING: Figure 4

Description

  The present invention relates to a posture estimation device and a control program for the posture estimation device.

  When performing calculations that estimate the state of a dynamic system, such as the posture of an object, it is shorter to calculate by integrating the output results of multiple sensors than to calculate based on the output results of one type of sensor. An accurate estimate of time can be made.

  A method using a Kalman filter is known as a method for estimating the state of a dynamic system by integrating the outputs of a plurality of sensors that measure different physical quantities. For example, Patent Document 1 includes a posture by integrating outputs from a plurality of sensors such as a three-axis angular velocity sensor and a three-axis acceleration sensor using a sigma point Kalman filter which is a kind of nonlinear Kalman filter. An estimation device for estimating the state of a dynamic system is disclosed.

JP 2012-132713 A

  By the way, in the calculation of the Kalman filter, in addition to executing the calculation for periodically estimating the state of the system including the posture, it is necessary to periodically calculate the covariance of the estimation error of the estimated value of the state. . For this reason, there has been a problem that the processing load increases when the posture is estimated using the Kalman filter. Further, since the posture is estimated using the state model in the calculation of the Kalman filter, there is a problem that it is difficult to estimate the posture accurately if the state model is not accurate.

  The present invention has been made in view of the above points, and an object of the present invention is to provide a technique for estimating a posture while suppressing a processing load.

  In order to solve the above-described problem, the posture estimation apparatus according to the present invention periodically detects acceleration in three directions and sequentially outputs a detection result as acceleration data, and periodically detects angular velocity in three directions. An angular velocity sensor that sequentially outputs detection results as angular velocity data, and based on the angular velocity data, based on the angular velocity data, the posture of the device at a time earlier than the current time is provided. And a prediction unit that predicts the attitude of the device at the current time, and a correction unit that corrects a prediction result of the prediction unit based on the acceleration data.

According to the posture estimation apparatus according to the present invention, the device posture is estimated by calculating a predicted value of the device posture based on the angular velocity data and correcting the predicted value of the device posture based on the acceleration data. In other words, the posture estimation apparatus according to the present invention estimates the posture of the device without calculating the covariance of the estimation error of the estimated value of the posture of the device. Therefore, in the posture estimation apparatus according to the present invention, the calculation load can be reduced as compared with the case of estimating the posture while calculating the covariance of the estimation value estimation error, such as a Kalman filter.
Further, according to the present invention, since the posture of the device can be estimated based on both the acceleration data and the angular velocity data, it is compared with the case where the posture of the device is estimated based on either the acceleration data or the angular velocity data. Thus, an estimated value close to the true attitude of the device can be calculated.

The posture estimation device described above includes a determination unit that determines whether or not the movement of the device is stable based on the acceleration data, and the correction unit includes the determination unit when the determination result of the determination unit is positive. It is preferable that the prediction result of the prediction unit is corrected.
According to this aspect, when the movement of the device is unstable and there is a high possibility that noise is superimposed on the acceleration data, the correction calculation using the acceleration data can not be executed. For this reason, it is possible to reduce the influence of noise superimposed on the acceleration data on the estimation of the posture of the device. That is, according to this aspect, it is possible to estimate the posture with higher accuracy than in the case of executing the correction calculation regardless of the determination result of the determination unit.

In the posture estimation apparatus described above, the correction unit outputs the acceleration sensor when the value indicated by the acceleration data output by the acceleration sensor at the current time and the posture indicated by the prediction result of the prediction unit are present. It is preferable that the prediction result of the prediction unit is corrected based on a weighted average value with a value indicated by acceleration data.
According to this aspect, since the posture of the device is estimated based on both the acceleration data and the angular velocity data, the accuracy of the device is more accurate than in the case where the posture of the device is estimated based on either the acceleration data or the angular velocity data. A high posture can be estimated.

  In addition, the control program for the posture estimation apparatus according to the present invention periodically detects acceleration in three directions and sequentially outputs detection results as acceleration data, and periodically detects and detects angular velocity in three directions. A control program for an attitude estimation device provided in a device including an angular velocity sensor that sequentially outputs a result as angular velocity data and a computer, the computer based on the angular velocity data, A prediction unit that predicts the posture of the device at a current time from a posture of the device at a time, and a correction unit that corrects a prediction result of the prediction unit based on the acceleration data. And

  According to the present invention, it is possible to estimate the posture of the device while suppressing the calculation load as compared to the case of estimating the posture while calculating the covariance of the estimation value estimation error as in the Kalman filter. Moreover, according to this invention, compared with the case where the attitude | position of an apparatus is estimated based on either one of acceleration data or angular velocity data, the attitude | position of an apparatus can be estimated with a sufficient precision.

It is a block diagram which shows the structure of the portable apparatus 1 which concerns on embodiment of this invention. 1 is a perspective view showing an external appearance of a mobile device 1. 3 is a block diagram illustrating a posture estimation unit 2. FIG. 3 is a block diagram illustrating a calculation unit 20. FIG. It is an explanatory diagram for explaining correction acceleration vector a _C.

Hereinafter, embodiments for carrying out the present invention will be described with reference to the drawings. However, in each figure, the size and scale of each part are appropriately changed from the actual ones. Further, since the embodiments described below are preferable specific examples of the present invention, various technically preferable limitations are attached thereto. However, the scope of the present invention is particularly limited in the following description. Unless otherwise stated, the present invention is not limited to these forms.
<A. Embodiment>
Embodiments of the present invention will be described with reference to the drawings.

<1. About device configuration and software configuration>
FIG. 1 is a block diagram of a portable device 1 (an example of “device”) according to an embodiment of the present invention, and FIG. 2 is a perspective view showing an appearance of the portable device 1. The mobile device 1 realizes the orientation represented by the image by rotating an image such as a map displayed on a screen (display unit 62 described later) included in the mobile device 1 according to the orientation of the mobile device 1. A function to follow the direction of the space is provided. This function is realized by estimating the attitude of the mobile device 1 based on the outputs of various sensors.

  The mobile device 1 stores various programs and data such as a CPU 10 that is connected to various components via a bus and controls the entire apparatus, a RAM (Random Access Memory) 51 that functions as a work area of the CPU 10, and a posture estimation program 100. A stored ROM (Read Only Memory) 52, a communication unit 61 that executes communication, a display unit 62 that displays an image, and a GPS unit 63 are provided. The portable device 1 also includes an acceleration sensor 80 that detects acceleration and outputs acceleration data a, and an angular velocity sensor 90 that detects angular velocity and outputs angular velocity data ω.

  The display unit 62 displays an image such as an arrow indicating the azimuth or the like based on the estimated value of the posture q of the mobile device 1 estimated by the CPU 10 executing the posture estimation program 100. The GPS unit 63 receives a signal from a GPS satellite and generates position information (latitude, longitude) of the mobile device 1.

The acceleration sensor 80 includes an X-axis acceleration sensor 81, a Y-axis acceleration sensor 82, and a Z-axis acceleration sensor 83. Examples of the acceleration sensor include detection type sensors such as a piezoresistive type, a capacitance type, and a heat detection type. The acceleration sensor I / F 84 AD-converts output signals from the sensors and periodically outputs acceleration data a every unit time. The acceleration data a is a vector having three components of an x-axis, a y-axis, and a z-axis representing the resultant force of inertia and gravity in a coordinate system fixed to the portable device 1 that moves integrally with the acceleration sensor 80. As shown in FIG. More specifically, the acceleration data a represents the output value from the X-axis acceleration sensor 81 as an x-axis component and the output value from the Y-axis acceleration sensor 82 in the coordinate system fixed to the portable device 1 as the y-axis. This is three-dimensional vector data represented by a component and an output value from the Z-axis acceleration sensor 83 being represented by a z-axis component.
Therefore, if the mobile device 1 is in a stationary state or a constant velocity linear motion state, the acceleration data a is three-dimensional vector data indicating the magnitude and direction of gravitational acceleration in a coordinate system fixed to the mobile device 1.

  The angular velocity sensor 90 includes an X-axis angular velocity sensor 91, a Y-axis angular velocity sensor 92, and a Z-axis angular velocity sensor 93. The angular velocity sensor I / F 94 AD-converts output signals from the sensors and periodically outputs angular velocity data ω every unit time. The angular velocity data ω is three-dimensional vector data indicating respective angular velocities around three axes (X axis, Y axis, Z axis) extending in three directions in a coordinate system fixed to the mobile device 1.

  The CPU 10 periodically estimates the posture q of the mobile device 1 every unit time by executing the posture estimation program 100 stored in the ROM 52. That is, the CPU 10 provided in the mobile device 1 functions as the posture estimation unit 2 (an example of “posture estimation device”) by executing the posture estimation program 100.

<2. About posture estimation unit>
FIG. 3 is a functional block diagram showing functions of the posture estimation unit 2. The posture estimation unit 2 estimates the posture q of the mobile device 1 per unit time based on the acceleration data a and the angular velocity data ω output every unit time. Hereinafter, the posture of the mobile device 1 estimated by the posture estimation unit 2 is referred to as an estimated posture q _OUT .

In the present embodiment, the posture q of the mobile device 1 is expressed using quaternions. A quaternion is a four-dimensional number representing the posture of an object. For example, a posture in which each axis of the coordinate system fixed to the mobile device 1 and each axis of the coordinate system fixed to the ground coincide with each other is defined as a reference posture, and the reference posture is defined as q = [0, 0, 0, 1] Expressed as ^T. At this time, the arbitrary posture q of the mobile device 1 is determined by setting the mobile device 1 that is the reference posture as an angle ψ with the axis extending in the direction of the three-dimensional unit vector ρ in the coordinate system fixed on the ground as the rotation axis. It can be expressed as a rotated posture. That is, the attitude q of the mobile device 1 is expressed by the following formula (1) using a quaternion.

As illustrated in FIG. 3, the posture estimation unit 2 determines whether the movement of the mobile device 1 is stable based on the calculation unit 20 that performs a calculation for estimating the estimated posture q _OUT and the acceleration data a. A determination unit 30 that outputs determination information JD indicating the determination result, and a switch SW that controls whether or not the acceleration data a is supplied to the calculation unit 20 based on the determination information JD of the determination unit 30. Prepare.

As described above, the determination unit 30 determines whether or not the movement of the mobile device 1 is stable based on the acceleration data a, that is, whether or not a large vibration is generated in the mobile device 1. Specifically, the determination unit 30 determines that the condition exemplified in the following formula (2), that is, “the absolute value of the difference value between the magnitude of the vector indicated by the acceleration data a and the magnitude of the gravitational acceleration vector g is a predetermined value. It is determined whether or not the condition “smaller than threshold ε _A ” is satisfied. Here, the gravitational acceleration vector g is a three-dimensional vector indicating the direction and magnitude of gravitational acceleration in a coordinate system fixed on the ground.
Note that the subscript k attached to codes representing various variables, vectors, matrices, and the like is a natural number that satisfies 0 ≦ k, which means that the variables, vectors, matrices, and the like are values at time T _k . Here, the time T _{k + 1} represents the time after the unit time than the time T _k, the time T _k-1 is set to indicate a past time by a unit time than the time T _k. In the following, it referred to three-dimensional vector indicating the acceleration data a at time T _k and an acceleration vector a _k, a 3-dimensional vector indicating the angular velocity data omega at time T _k is referred to as angular velocity vector omega _k. Further, the acceleration vector a _k may be referred to as an observed value observed by the acceleration sensor 80, and the angular velocity vector ω _k may be referred to as an observed value observed by the angular velocity sensor 90.

  The determination unit 30 determines that the movement of the mobile device 1 is stable when the condition shown in Expression (2) is satisfied, and the movement of the mobile device 1 is stable with respect to the determination information JD. For example, “1” is set. Further, the determination unit 30 determines that the movement of the mobile device 1 is unstable when the condition shown in Expression (2) is not satisfied, and the movement of the mobile device 1 is unstable with respect to the determination information JD. For example, “0” is set.

The switch SW is provided between the acceleration sensor 80 and the calculation unit 20 and controls whether or not the acceleration data a is supplied to the calculation unit 20 based on the determination information JD.
Specifically, the switch SW is turned on when the determination result in the determination unit 30 is positive, that is, when the determination information JD output from the determination unit 30 indicates “1”. In this case, the computing unit 20 calculates the estimated posture q _OUT using the acceleration data a output from the acceleration sensor 80 and the angular velocity data ω output from the angular velocity sensor 90.
On the other hand, the switch SW is turned off when the determination result in the determination unit 30 is negative, that is, when the determination information JD output from the determination unit 30 indicates “0”. In this case, the calculation unit 20 calculates the estimated posture q _OUT using the angular velocity data ω output from the angular velocity sensor 90 without using the acceleration data a output from the acceleration sensor 80.
Hereinafter, the operation of the calculation unit 20 will be described in detail.

<3. About the calculation unit>
FIG. 4 is a functional block diagram of the calculation unit 20. As shown in this figure, the calculation unit 20 includes a posture prediction unit 21 (an example of a “prediction unit”), a posture correction unit 22 (an example of a “correction unit”), and an estimated posture output unit 23 (an “output unit”). For example).

Orientation prediction unit 21 on the basis of the angular velocity vector omega _k-1 indicated by the angular velocity data omega outputted at time T _k-1, the estimated position of the mobile device 1 at time T _k-1 which is estimated by the posture estimating unit 2 From q _OUT , a predicted value of posture q of portable device 1 at time T _k (hereinafter referred to as “predicted posture q _G ”) is calculated.
The posture correcting unit 22 corrects the predicted posture q _G based on the acceleration vector a _k indicated by the acceleration data a output at the time T _k , thereby correcting the predicted value of the posture q of the mobile device 1 at the time T _k . A value (hereinafter referred to as “correction posture q _A ”) is calculated.
The estimated posture output unit 23 outputs the predicted posture q _G calculated by the posture prediction unit 21 or the corrected posture q _A calculated by the posture correction unit 22 as the estimated posture q _OUT of the mobile device 1 at time T _k .
Hereinafter, the posture prediction unit 21, the posture correction unit 22, and the estimated posture output unit 23 will be described in detail.

As described above, the posture prediction unit 21 performs a calculation for predicting the predicted posture q _G from the estimated posture q _OUT of the mobile device 1 at time T _k−1 based on the angular velocity vector ω _k−1 .
Hereinafter, as illustrated in FIG. 4, when the estimated posture q _OUT of the mobile device 1 at time T _k−1 is used for calculation of the predicted posture q _G at time T _k by the posture prediction unit 21, For convenience of explanation, the estimated posture q _OUT may be referred to as a pre-prediction posture q _G-IN .

Orientation prediction unit 21, for example, according to equation (3) of the rotational movement below, calculates the predicted attitude _{q G} from the prediction before the attitude _{q G-IN.}
Here, the operator Ω in Expression (3) is defined by Expression (4). Further, the matrix I _{3 × 3} in the equation (4) is a unit matrix of 3 rows and 3 columns. The operator Γ in equation (4) is defined by equation (5) using a three-dimensional vector L = (L ₁ , L ₂ , L ₃ ). A symbol Δt indicates a unit time from time T _{k −1} to time T _k . Further, the component Ψ _k of the formula (4) is defined by the formula (6).

As described above, the posture correction unit 22 corrects the predicted posture q _G based on the acceleration vector a _k and calculates a corrected posture q _A.
As illustrated in FIG. 4, the posture correction unit 22 includes a predicted observation value calculation unit 221, a corrected observation value calculation unit 222, a correction amount calculation unit 223, and a correction posture calculation unit 224. Hereinafter, each component of the posture correction unit 22 will be described.

The predicted observation value calculation unit 221 calculates an acceleration vector a _k that is an observation value observed by the acceleration sensor 80 at the time T _k when the posture of the mobile device 1 at the time T _k is the predicted posture q _G. Execute. Then, the predicted observation value calculating unit 221 outputs the predicted acceleration vector a _O representing the result of the prediction.
Specifically, the predicted observation value calculating unit 221, according to equation (7) shown below to calculate the predicted acceleration vector a _O. Here, the matrix R (q) in Expression (7) is a 3-by-3 matrix for coordinate conversion from a coordinate system fixed on the ground to a coordinate system fixed to the mobile device 1 in the posture q. And is represented by the following equation (8).

As described above, the acceleration vector _ak is supplied to the arithmetic unit 20 when the determination result in the determination unit 30 is positive, that is, when the movement of the mobile device 1 is stable. When the movement of the mobile device 1 is stable, it can be considered that the acceleration vector a _k that is an observation value by the acceleration sensor 80 represents the gravitational acceleration. Therefore, the predicted observation value calculating unit 221, as shown in equation (7), the attitude q of the portable device 1 at time T _k upon which a predicted position q _G, fixed to the gravitational acceleration vector g in the portable device 1 is the coordinate transformation to the coordinate system, the vector obtained by the coordinate transformation, the predicted acceleration vector a _O is the predicted value of the acceleration vector a _k.

The corrected observation value calculation unit 222 performs a weighted average of the predicted acceleration vector a _O calculated by the prediction observation value calculation unit 221 and the acceleration vector a _k indicated by the acceleration data a supplied from the acceleration sensor 80, thereby correcting the corrected acceleration. A vector a _C is generated.
Specifically, the corrected observed value calculation unit 222 generates a corrected acceleration vector a _C based on the following equation (9). In Equation (9), the coefficient α _A is a real number that satisfies 0 <α _A <1.

FIG. 5 is an explanatory diagram for explaining the relationship among the acceleration vector a _k , the predicted acceleration vector a _O , and the corrected acceleration vector a _C. As shown in this figure, the correction acceleration vector a _C is a vector perpendicular to the acceleration vector a _k and the predicted acceleration vector a _O perpendicular unit vector d _A, of the acceleration vector a _k and the predicted acceleration vector a _O It becomes a vector facing the direction between. Hereinafter, as shown in this figure, the angle of the predicted acceleration vector a _O and correction acceleration vector a _C, referred to as the angle theta _A.

Correction amount calculation unit 223, based on the corrected acceleration vector a _C and predicted acceleration vector a _O, and calculates the posture change amount P _A is a quaternion for correcting the predicted attitude q _G. Specifically, the correction amount calculation unit 223, based on equation (10) shown below, calculates the posture change amount P _A. Note that the unit vector d _A can be obtained by Expression (11). In Expression (11), “a _O × a _C ” means a cross product of the predicted acceleration vector a _O and the corrected acceleration vector a _C.

As is clear from the equation (1), the quaternion can represent the posture change amount in addition to the posture itself. When the change in posture is rotation about a certain axis, the change amount of the posture (for example, the posture change amount P _A ) is set to the direction in which the axis extends (for example, the unit vector d _A ) It can be expressed using a quaternion indicating the magnitude of rotation about the axis (for example, the angle θ _A ).
That is, the posture change amount P _A that is a quaternion represents the amount of change in posture when the predicted acceleration vector a _O is changed to the corrected acceleration vector a _C. In other words, the posture change amount P _A is calculated based on the posture q of the mobile device 1 from the predicted posture q _G where the observed value of the acceleration sensor 80 is the predicted acceleration vector a _O, and the observed value of the acceleration sensor 80 is the corrected acceleration vector a. _This is a quaternion indicating the amount of change in the attitude of the mobile device 1 when changing to the corrected attitude q _A that becomes _C.

Correcting the posture computing unit 224, a predicted position _{q G,} by changing only the posture variation _{P A,} calculates the correction posture _{q A.}
In the following, as shown in FIG. 4, the predicted attitude q _G is, when used to calculate the correction posture q _A by the correction position calculating section 224, for convenience of explanation, before correcting the predicted position q _G posture It may be referred to as q _A-IN .

Correcting the posture computing unit 224 according to the following equation (12), by executing the correction of changing the uncorrected attitude _{q A-IN} only posture variation _{P A,} calculates the correction posture _{q A.}
Here, the operator [×] is a quaternion product. A quaternion product refers to the attitude expressed as one quaternion as the other quaternion by considering one of the two quaternions as the attitude and the other quaternion as the attitude change amount. This is a calculation for changing the posture by the posture change amount.

As described above, the posture correction unit 22 determines the predicted acceleration vector a _O determined according to the angular velocity vector ω _k−1 that is the output value from the angular velocity sensor 90 and the acceleration that is the output value from the acceleration sensor 80. A corrected acceleration vector a _C is calculated as a weighted average with the vector a _k (see formula (9)). Then, the predicted acceleration vector a _O, obtains a predicted acceleration vector a _O and posture variation P _A in the case of the changing the posture to the correction acceleration vector a _C oriented at between acceleration vector a _k, the posture variation P _A corrected posture q _A is calculated by correcting the predicted posture q _G by _A. That is, in the present embodiment, the corrected posture q _A is determined based on both the angular velocity data ω output from the angular velocity sensor 90 and the acceleration data a output from the acceleration sensor 80.
By the way, if the coefficient α _A is “1” in equation (9), the corrected acceleration vector a _C is the acceleration vector a _k . In this case, correction posture q _A is not affected by the angular velocity data omega, is determined based only on the acceleration data a. In this case, the corrected posture q _A may be a posture deviated from the true posture due to the noise superimposed on the acceleration data a or the measurement error in the acceleration sensor 80.
If the coefficient α _A is “0” in the equation (9), the corrected acceleration vector a _C is the predicted acceleration vector a _O. In this case, correction posture q _A, without being affected by the acceleration vector a _k, is determined based on the angular velocity data omega. In this case, the corrected posture q _A may be a posture deviated from the true posture due to the noise superimposed on the angular velocity data ω or the measurement error in the angular velocity sensor 90.
On the other hand, in the present embodiment, as shown in Expression (9), the corrected acceleration vector a _C is calculated as a weighted average of the predicted acceleration vector a _O and the acceleration vector a _k, and the corrected posture q _A is calculated as the acceleration. It is determined based on both data a and angular velocity data ω. Therefore, in the present embodiment, the correction posture q _A, as compared to the case determined based on one of the acceleration data a or the angular velocity data omega, the influence of noise superimposed on the acceleration data a and the angular velocity data omega, The influence of measurement errors in the acceleration sensor 80 and the angular velocity sensor 90 can be reduced. Thus, in the present embodiment, it is possible to calculate an accurate correction posture q _A close to the true position.

Estimating the attitude output unit 23, as described above, the predicted attitude _{q G} or correction posture _{q A,} and outputs as the estimated attitude _{q OUT} of the portable device 1 at time _{T k.}
More specifically, when the determination information JD indicates “1”, that is, when the determination result in the determination unit 30 is affirmative, the estimated posture output unit 23 sets the predicted posture q _G to calculate the correct attitude _{q a} correction to, and outputs the correction posture _{q a,} as the estimated attitude _{q OUT} of the portable device 1 at time _{T k.} On the other hand, the estimated position output unit 23, if the determination information JD indicates "0", i.e., if the determination result in the determination unit 30 is negative is not performed is corrected for the predicted attitude q _G by the posture correcting portion 22 Therefore, the predicted attitude _{q G} calculated by the orientation prediction unit 21, and outputs as the estimated attitude _{q OUT} of the portable device 1 at time _{T k.}

Thus, the posture estimating unit 2 based on the angular velocity data ω the angular velocity sensor 90 outputs per unit time, and generates a predicted attitude q _G is a prediction value of the orientation q of the portable equipment 1. Further, the posture estimating unit 2, if the value indicated by the acceleration data a by the acceleration sensor 80 outputs is stable for each unit time, that on the basis of the acceleration data a, corrects the predicted position q _{G A} corrected posture qA is calculated. Then, the posture estimating unit 2, if the predicted position q _G is corrected to a corrected posture q _A outputs the correction posture q _A as the estimated attitude q _OUT of the portable device 1, also predicted position q _G If not corrected, the predicted posture q _G is output as the estimated posture q _OUT of the mobile device 1.
As described above, the posture estimation unit 2 according to the present embodiment is based on both the angular velocity data ω output from the angular velocity sensor 90 and the acceleration data a output from the acceleration sensor 80, and the estimated posture at time T _k−1 . By calculating the estimated posture q _OUT at time T _k from q _OUT , the posture q of the mobile device 1 is estimated for each unit time.

<4. Conclusion of Embodiment>
As described above, the posture estimation unit 2 according to the present embodiment uses the predicted posture q _G calculated based on the angular velocity data ω output from the angular velocity sensor 90 based on the acceleration data a output from the acceleration sensor 80. by correcting Te, and calculates the correction posture q _a. That is, in the present embodiment, the posture q of the mobile device 1 is estimated based on both the angular velocity data ω and the acceleration data a.

Incidentally, in general, acceleration data a is superimposed with noise caused by vibration of the mobile device 1 in addition to the gravitational acceleration to be detected by the acceleration sensor 80 in order to estimate the posture q of the mobile device 1. Probability is high. On the other hand, since the angular velocity data ω generally detects only the posture change of the mobile device 1 to be detected by the angular velocity sensor 90 in order to estimate the posture q of the mobile device 1, noise is generated compared to the acceleration data a. The possibility of overlapping is low. From these, in the estimation of the posture q of the mobile device 1, only the highly reliable data is used, that is, the predicted posture q _G calculated based on the angular velocity data ω is output as the estimated posture q _OUT . manner that does not perform the correction of the predicted position q _G based on the acceleration data a (hereinafter, referred to as "comparison example") also be envisaged.

However, the predicted attitude _{q G} is a prediction result of the portable device 1 posture q at time _{T k} is the estimated pose _{q OUT} of the portable device 1 at time _{T k-1,} from time _{T k-1} to time _{T k} The posture is changed by the posture change amount of the mobile device 1 in unit time (see Expression (3)). That is, the calculation (hereinafter referred to as “prediction calculation”) for predicting the posture q of the mobile device 1 executed by the posture prediction unit 21 is a calculation for obtaining a relative change amount of the posture q of the mobile device 1 in unit time. Not too much.
Therefore, in contrast, for example, when there is an error between the estimated posture q _OUT of the mobile device 1 in the initial state (time T ₀ ) and the true posture q of the mobile device 1 at the time T ₀ , Even if the prediction calculation by the prediction unit 21 is repeatedly executed, the error is not eliminated.
Further, in the comparative example, the predicted attitude q _G of the portable device 1 at time T _k, obtained in the k times of the prediction computation orientation prediction unit 21 has performed during the initial state (time T ₀₎ to the time T _k It is calculated based on a total value of _k posture change amounts Ω (ω ₀ ), Ω (ω ₁ ),..., Ω (ω _k−1 ). For this reason, even if the reliability of the angular velocity sensor 90 is high and the noise superimposed on the angular velocity data ω is small, the time length between the time T ₀ and the time T _k becomes long, and the number of times of execution of the prediction calculation is increased. with increasing, also accumulates effects on predicted position q _G of noise superimposed on the angular velocity data omega. That is, as the time length between the time T ₀ and the current time T _k increases, the error between the predicted posture q _G of the mobile device 1 and the true posture q of the mobile device 1 also accumulates and increases.

On the other hand, when the determination result in the determination unit 30 is affirmative, that is, when it can be considered that the acceleration vector _ak represents gravitational acceleration, the posture estimation unit 2 according to the present embodiment has an acceleration vector. to correct the prediction attitude _{q G} by a _k.
When the posture q of the mobile device 1 is changing, the direction of the coordinate system (X-axis, Y-axis, and Z-axis directions) fixed to the mobile device 1 also changes with the passage of time. In this case, the coordinate system fixed to the mobile device 1 at time T _k−1 is a different coordinate system from the coordinate system fixed to the mobile device 1 at time T _k . Therefore, when the posture q of the portable device 1 is changing, the posture of the portable device 1 at the certain time and the portable device 1 at the certain time as viewed from the coordinate system fixed to the portable device 1 at the certain time. The posture of the mobile device 1 at a time different from the certain time as seen from the fixed coordinate system may be different. The posture of the mobile device 1 as viewed from the coordinate system fixed to the mobile device 1 at a certain time is expressed based on the angular velocity vector ω _k . Specifically, the angular velocity vector ω _k−1 indicates a so _- called relative posture of the mobile device 1 at time T _k viewed from the coordinate system fixed to the mobile device 1 at time T _k−1 (formula ( 3)).
On the other hand, the acceleration vector _ak indicates a so-called absolute attitude of the mobile device 1 viewed from a coordinate system fixed on the ground. For this reason, as in the present embodiment, by executing the correction for the predicted posture q _G using the acceleration vector a _k , the error of the estimated posture q _OUT that has occurred in the initial state can be reduced, The accumulated error can be reduced by repeating the prediction calculation. Thereby, the posture estimation unit 2 according to the present embodiment can accurately estimate the posture q of the mobile device 1.

By the way, as described above, the posture correction unit 22 included in the calculation unit 20 confirms that the acceleration vector _ak and the vector _representing the gravitational acceleration vector g in the coordinate system fixed to the mobile device 1 are substantially the same. to correct the prediction attitude q _G as a premise. However, when the movement of the mobile device 1 is unstable, such as when the mobile device 1 vibrates, acceleration other than gravitational acceleration is superimposed as noise on the acceleration vector _ak . In this case, the acceleration vector _ak and the vector _representing the gravitational acceleration vector g in the coordinate system fixed to the portable device 1 cannot be regarded as substantially the same. Therefore, when the movement of the mobile device 1 is unstable, the corrected posture q _A obtained by correcting the predicted posture q _G using the acceleration vector a _k is greatly deviated from the true posture of the mobile device 1. There is a high possibility. That is, in this case, when the posture estimation unit 2 outputs the corrected posture q _A as the estimated posture q _OUT , the posture estimation unit 2 outputs the estimated posture q _OUT greatly deviating from the true posture of the mobile device 1. Become.
On the other hand, the posture estimation unit 2 according to the present embodiment supplies the acceleration data a to the calculation unit 20 only when the determination result in the determination unit 30 is affirmative and the movement of the mobile device 1 is stable. Then, correction for the predicted posture q _G by the acceleration vector a _k is executed. For this reason, in the posture estimation unit 2 according to the present embodiment, it is possible to prevent the estimated posture q _OUT greatly deviating from the true posture of the mobile device 1.

Thus, the posture estimation unit 2 according to the present embodiment calculates the estimated posture q _OUT based on both the angular velocity data ω output from the angular velocity sensor 90 and the acceleration data a output from the acceleration sensor 80. as compared with the case of calculating the estimated posture q _OUT based on only one of the angular velocity data ω or acceleration data a, it can be calculated closer estimated attitude q _OUT by the true attitude of the mobile device 1.
Furthermore, since the posture estimation unit 2 according to the present embodiment does not need to calculate the covariance of the estimation error of the estimated value of the posture q every unit time unlike the calculation by the Kalman filter, the portable device 1 uses the Kalman filter. The processing load can be reduced as compared with the case where the posture q is estimated.

<B. Modification>
The above embodiment can be variously modified. Specific modifications are exemplified below. Two or more aspects arbitrarily selected from the following examples can be appropriately combined within a range that does not contradict each other.
In addition, about the element which an effect | action and a function are equivalent to embodiment in the modification illustrated below, the code | symbol referred by the above description is diverted and each detailed description is abbreviate | omitted suitably.

<Modification 1>
The determination unit 30 according to the embodiment described above determines whether or not the difference between the magnitude of the acceleration vector a _{k and} the magnitude of the gravitational acceleration vector g is smaller than the threshold value ε _A , as illustrated in the above-described equation (2). However, the present invention is not limited to such a form, and the determination unit 30 may have any configuration as long as it can determine whether the movement of the mobile device 1 is stable. Also good.
For example, the determination unit 30 determines whether or not the output value of the HPF is smaller than a predetermined value when each component of the acceleration vector _ak included in the acceleration data a is transmitted through an HPF (High-pass filter). You may do. In this case, if vibration is generated in the mobile device 1, the output value of the HPF is equal to or greater than a predetermined value.

<Modification 2>
In the embodiment and the modification described above, the posture estimation unit 2 includes the switch SW. However, the present invention is not limited to such a form, and the posture estimation unit 2 may not include the switch SW. Good.

When the posture estimation unit 2 does not include the switch SW, the acceleration vector _ak may be supplied to the calculation unit 20 regardless of the determination result of the determination unit 30. In this case, the posture correction unit 22 may calculate the corrected posture q _A by correcting the predicted posture q _G with the acceleration vector a _k regardless of the determination result of the determination unit 30, or the estimated posture output unit. 23 may output the corrected posture q _A as the estimated posture q _OUT regardless of the determination result of the determination unit 30.

When the posture estimation unit 2 is not provided with the switch SW, the coefficient α which is a weight added to the acceleration vector a _k in the calculation of the weighted average for calculating the corrected acceleration vector a _C shown in the above equation (9). _A may be changed based on the determination result of the determination unit 30. For example, when the determination result of the determination unit 30 is affirmative and the operation of the mobile device 1 can be regarded as stable, the corrected acceleration vector a _C is calculated using the coefficient α _A as shown in Equation (9). When the determination result of the determination unit 30 is negative and the operation of the mobile device 1 is unstable, a real number satisfying 0 ≦ α _B <α _A <1 instead of the coefficient α _A in the equation (9) The corrected acceleration vector a _C may be calculated using the coefficient α _B as follows.
Operation of the mobile device 1 is unstable, if the acceleration vector a _k comprises a noise component other than the component indicating the gravitational acceleration, the correction posture q _A calculated based on the acceleration vector a _k, the acceleration vector a _k It is affected by the included noise. In this case, the corrected posture q _A is calculated as being greatly deviated from the true posture of the mobile device 1.
In this modification, when the determination result of the determination unit 30 is negative, in the calculation of the weighted average for calculating the corrected acceleration vector a _C shown in Expression (9), the weight corresponding to the acceleration vector a _k is expressed as the coefficient instead of alpha _a, for a small coefficient alpha _B than the coefficient alpha _a, as compared with the case of the weight corresponding to the acceleration vector a _k a coefficient alpha _a, noise included in the acceleration vector a _k correction posture q _The influence on _A can be reduced. That is, in the present modification, the coefficient (1-α _B ) that is the weight corresponding to the predicted acceleration vector a _O is compared with the case where the weight corresponding to the acceleration vector a _k is set to the coefficient α _A in Equation (9). Can be increased. For this reason, even if the operation of the mobile device 1 is unstable, it is possible to calculate the corrected posture q _A that is close to the true posture of the mobile device 1.

In the embodiment and the modification described above, the posture estimation unit 2 includes the determination unit 30, but the present invention is not limited to such a form, and the posture estimation unit 2 does not include the determination unit 30. It may be. In this case, the posture estimation unit 2 may be configured without the switch SW.
When the posture estimation unit 2 does not include the determination unit 30, the acceleration vector a _k output from the acceleration sensor 80 may be always supplied to the calculation unit 20. In this case, the arithmetic unit 20, the correction posture _{q A} obtained by correcting the predicted position _{q G} based on the acceleration vector _{a k,} may be output as the estimated attitude _{q OUT.}
When the posture estimation unit 2 does not include the determination unit 30, a corrected acceleration α is used by using a coefficient α _C that is a real number satisfying 0 <α _C <α _A <1 instead of the coefficient α _A in the equation (9). The vector a _C may be calculated. In this case, the coefficient (1-α _C ) that is a weight corresponding to the predicted acceleration vector a _O can be increased. For this reason, even if the operation of the mobile device 1 is unstable, it is possible to calculate the corrected posture q _A that is close to the true posture of the mobile device 1.

<Modification 3>
In the embodiment and the modification described above, the mobile device 1 includes the acceleration sensor 80 and the angular velocity sensor 90. However, the present invention is not limited to such a form, and the mobile device 1 includes, You may provide the magnetic sensor which detects the magnetism of 3 directions periodically and outputs a detection result.
In this case, the posture estimation unit 2 is portable from the vertical direction (the direction in which the gravitational acceleration vector g extends) based on the output results from the acceleration sensor 80 and the angular velocity sensor 90, as in the above-described embodiments and modifications. In addition to estimating the inclination of the device 1, the inclination of the portable device 1 from the direction toward the north of the magnetic pole may be estimated based on the output results from the magnetic sensor and the angular velocity sensor 90.

<Modification 4>
In the embodiment and the modification described above, sensors such as the acceleration sensor 80 and the angular velocity sensor 90 output observation values periodically detected every unit time, but the present invention is not limited to such a form. There is no limitation as long as the observation value is output at an arbitrary timing. In this case, the posture predicting portion 21, at the timing when the angular velocity data ω is output may be executed an operation for generating a predicted attitude q _G, posture correction section 22 at the timing of the acceleration data a is output The calculation for generating the corrected posture q _A may be executed. That is, the calculation unit 20 performs processing of the posture prediction unit 21 that generates the predicted posture q _G based on the angular velocity data ω and processing of the posture correction unit 22 that generates the corrected posture q _A based on the acceleration data a. It may be executed independently and asynchronously.
Further, when the posture prediction unit 21 and the posture correction unit 22 operate asynchronously, the posture correction unit 22 may generate a corrected posture q _A based on the estimated posture q _OUT instead of the predicted posture q _G. . That is, the calculation unit 20 sets the estimated posture q _OUT as the pre _- prediction posture q _G-IN and generates a predicted posture q _G based on the pre-prediction posture q _G-IN , and the estimated posture q _OUT. Is executed at the mutually exclusive timing and independently from each other, with the pre _- correction posture q _A-IN as the pre-correction posture q _A-IN and generating the corrected posture q _A based on the pre-correction posture q _A-IN It may be.

DESCRIPTION OF SYMBOLS 1 ... Portable apparatus, 2 ... Attitude estimation part, 10 ... CPU, 20 ... Calculation part, 21 ... Attitude prediction part, 22 ... Attitude correction part, 23 ... Estimated attitude output part, 221 ... Predicted observation value calculation part, 222 ... Correction Observation value calculation unit, 223 ... correction amount calculation unit, 224 ... correction posture calculation unit, 30 ... determination unit, 80 ... acceleration sensor, 90 ... angular velocity sensor.

Claims (4)

An acceleration sensor that periodically detects acceleration in three directions and sequentially outputs detection results as acceleration data;
An angular velocity sensor that periodically detects angular velocities in three directions and sequentially outputs the detection results as angular velocity data;
A posture estimation device provided in a device comprising:
Based on the angular velocity data, a prediction unit that predicts the posture of the device at the current time from the posture of the device at a time earlier than the current time;
A correction unit that corrects a prediction result of the prediction unit based on the acceleration data;
Comprising
A posture estimation apparatus characterized by that. A determination unit that determines whether the movement of the device is stable based on the acceleration data;
The correction unit is
When the determination result of the determination unit is positive, the prediction result of the prediction unit is corrected.
The posture estimation apparatus according to claim 1, wherein: The correction unit is
A weighted average value of the value indicated by the acceleration data output by the acceleration sensor at the current time and the value indicated by the acceleration data output by the acceleration sensor when the device is in the posture indicated by the prediction result of the prediction unit. Based on the prediction result of the prediction unit,
The posture estimation apparatus according to claim 1 or 2, characterized in that An acceleration sensor that periodically detects acceleration in three directions and sequentially outputs detection results as acceleration data;
An angular velocity sensor that periodically detects angular velocities in three directions and sequentially outputs the detection results as angular velocity data;
A computer,
A control program for a posture estimation device provided in a device comprising:
The computer,
Based on the angular velocity data, a prediction unit that predicts the posture of the device at the current time from the posture of the device at a time earlier than the current time;
A correction unit that corrects a prediction result of the prediction unit based on the acceleration data;
Make it work,
A control program for a posture estimation device.

Images