JP2005331265A - Attitude angle detection device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To have an algorithm not lowering accuracy in any attitude, and to acquire an ideal attitude angle detection device by the algorithm. <P>SOLUTION: This attitude angle detection device is constituted of three gyroscopes, a moving angle operation device for operating a moving angle in a unit time by two kinds of Euler angles based on an output corresponding to an angular velocity of the gyroscope, an acceleration sensor for detecting a biaxial acceleration, an earth magnetism sensor for detecting a biaxial earth magnetism, a static angle operation device for operating rotation angles around the X-axis, the Y-axis and the Z-axis by the two kinds of Euler angles based on outputs from the acceleration sensor and the earth magnetism sensor, a discrimination device for discriminating genuineness of an operation result, an attitude angle operation device for operating the attitude angle by the two kinds of Euler angles according to the operation result from the discrimination device, and an Euler angle conversion operation device for selecting an Euler angle to be used having a small error from between the two kinds of Euler angles and converting it into the other Euler angle. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

  The present invention relates to a posture of a measurement object in a three-dimensional space, such as a posture of a moving body, a tracker for detecting a posture angle of a head on a 3D game pad, a head mounted display used for posture control / virtual reality, etc. The present invention relates to a posture angle detection device particularly suitable for detecting an angle (a rotation angle of three axes).

  The Euler angle has often been used as a method of expressing a three-dimensional posture angle. The Euler angles determine the order of rotation of the three axes, and there are twelve different orders in total, such as ZYX Euler angles and ZYZ Euler angles. The most common Euler angle used in the attitude angle detection device is the ZY-X Euler angle. The angle around the Z axis is represented as the yaw angle α, the angle around the Y axis as the pitch angle β, and the angle around the X axis as the roll angle γ.

  Patent Document 1 describes a posture angle detection device that calculates and outputs a posture angle using a ZYX Euler angle.

  However, when the above method is used, there is a problem that an error increases when the posture of the posture angle detection device becomes a singular point of Euler angles. Table 1 shows typical examples of each Euler angle and singularity.

  Taking the ZY-X Euler angle as an example, when the X axis is vertical, that is, when the pitch angle β is ± 90 degrees, it becomes a singular point, and the yaw angle α and the roll angle γ are indefinite. Even if the pitch angle β is not exactly 90 degrees, when the pitch angle β is close to 90 degrees, for example, the accuracy is deteriorated at 60 degrees or more. For example, the posture of (180, 89, 180) and the posture of (0, 89, 0) at the ZY-X Euler angle are both just shifted from the singular point by 1 degree, and the Z-X-Y Euler In the angle, the difference between the two is only 2 degrees, but in the ZY-X Euler angle, both the yaw angle α and the roll angle γ are as much as 180 degrees.

  In Patent Document 1, two types of temporary posture angles, a motion angle and a static angle, are calculated, and then the posture angle, which is the final output angle, is calculated by combining them. At this time, if the motion angle and the static angle are the above postures (180, 89, 180) and (0, 89, 0), the final posture angle also becomes a large error angle.

JP-A-11-212474

  An object of the present invention is to solve the problems of the conventional posture angle detection device and to provide an ideal posture angle detection device with an algorithm that does not reduce the accuracy in any posture. is there.

  The posture angle detection device of the present invention is a posture angle detection device having an algorithm that does not reduce the accuracy in any posture. That is, according to the present invention, the axes orthogonal to each other in the horizontal plane are the X axis and the Y axis, the axes orthogonal to the X axis and the Y axis are the Z axes, and the X axis, the Y axis, and the Z axis are fixed. Three gyroscopes that detect angular velocities around the Xs, Ys, and Zs axes when the coordinate axes consisting of the Xs axis, the Ys axis, and the Zs axis that move with the attitude angle detection device are the sensor coordinates. Scope, motion angle calculation device that calculates the angle moved in unit time based on the output according to the angular velocity of the gyroscope with two types of Euler angles, and detection of two-axis acceleration orthogonal to each other on the Xs-Ys plane An X-axis, Y-axis based on outputs from the acceleration sensor and the geomagnetic sensor, an acceleration sensor arranged to detect the geomagnetic sensor arranged to detect two axes of geomagnetism orthogonal to each other in the Xs-Ys plane, , A static angle calculation device that calculates the rotation angle around the Z axis with two types of Euler angles, a determination device that determines the authenticity of the calculation result by the static angle calculation device, and a motion according to the calculation result of the determination device The attitude angle calculation device that calculates the posture angle to be output from the calculation result of the angle calculation device and the calculation result of the static angle calculation device with two types of Euler angles, and the Euler angle that should be used with a small error from the two types of Euler angles. It is an attitude angle detection device that includes an Euler angle conversion arithmetic device that selects and converts it to another Euler angle.

  According to the present invention, a high-accuracy posture angle detection device can be realized by an algorithm that obtains an accurate output in any posture by combining two types of Euler angles with different singularities.

  Hereinafter, an attitude angle detection device according to an embodiment of the present invention will be described with reference to the drawings. The output of the ZY-X Euler angle is represented by the yaw angle α, the pitch angle β, the roll angle γ, (α, β, γ), and the output of the Z-X-Y Euler angle is the yaw angle φ, the roll angle. It is represented by θ, pitch angle ψ, (φ, θ, ψ).

  FIG. 1 is a configuration diagram of an example of a posture angle detection device according to the present invention. As shown in FIG. 1, an arrangement of a gyro, an acceleration sensor, and a magnetic sensor in the attitude angle detection device of the present invention is shown. In the sensor coordinate system shown in FIG. 1, a first gyro 11, a second gyro 12, and a third gyro 13 for detecting angular velocities around three axes (Xs axis, Ys axis, and Zs axis) orthogonal to each other. Are arranged in parallel to the Xs axis, the Ys axis, and the Zs axis, that is, orthogonal to each other. The first acceleration sensor 14, the second acceleration sensor 15, and the third acceleration sensor 16 are arranged in parallel to the three axes that are orthogonal to each other, the Xs axis, the Ys axis, and the Zs axis. Similarly, the first magnetic sensor 17, the second magnetic sensor 18, and the third magnetic sensor 19 are disposed in parallel to the three axes that are orthogonal to each other, the Xs axis, the Ys axis, and the Zs axis.

  FIG. 2 is an explanatory diagram relating to the overall configuration of the attitude angle detection device according to the present invention. As shown in FIG. 2, the angular velocities (ωx, ωy, ωz) and accelerations (Ax, Ay, Az) are obtained from the gyros 22 a, 22 b, 22 c, the acceleration sensors 23 a, 23 b, 23 c and the magnetic sensors 24 a, 24 b, 24 c, respectively. , Magnetism (Mx, My, Mz) is output. The motion angle calculation device 26a calculates and outputs motion angles (Δα, Δβ, Δγ) and (Δφ, Δθ, Δψ) from the output of the gyro 22. The static angle calculation device 26b calculates and outputs the static angles (α ′, β ′, γ ′) and (φ ′, θ ′, ψ ′) from the outputs of the acceleration sensor 23 and the magnetic sensor 24. The discriminating device 27 discriminates whether or not the stationary angle is correct from the motion angle and the stationary angle, and outputs it to the posture angle computing device 26c. Based on the determination from the determination device, the posture angle calculation device 26c combines the motion angle and the stationary angle to create temporary posture angles (α ″, β ″, γ ″) and (φ ″, θ ″, ψ ″) is output. The Euler angle conversion calculation device 28 calculates a final posture angle to be output from the temporary posture angle, and outputs Euler angles (α, β, γ).

  Next, the contents of the motion angle calculation device will be described. The outputs (ωx, ωy, ωz) of each gyro according to the angular velocity applied to the attitude angle detection device are output by converting the coordinates α (n−1), β (n −1), γ (n−1), φ (n−1), θ (n−1), and ψ (n−1), the current movement angle (movement angle) in the reference coordinate system is ) (Δα, Δβ, Δγ) and (Δφ, Δθ, Δψ).

Δα (n) = ωy · sinγ (n−1) / cosβ (n−1) + ωz · cosγ (n−1) / cosβ (n−1),
Δβ (n) = ωy · cosγ (n−1) −ωz · sinγ (n−1),
Δγ (n) = ωx + ωy · tanβ (n−1) · sinγ (n−1) + ωz · tanβ (n−1) · cosγ (n−1),
Δφ (n) = − ωx · sinψ (n−1) / cosθ (n−1) + ωz · cosψ (n−1) / cosθ (n−1),
Δθ (n) = ωx · cos ψ (n−1) + ωz · sin ψ (n−1),
Δψ (n) = ωx · tan θ (n−1) · sinψ (n−1) + ωy−ωz · tan θ (n−1) · cos φ (n−1)

  Next, the static angle calculation device will be described. The acceleration sensors 14, 15 and 16 shown in FIG. 1 detect component component forces Ax (n), Ay (n) and Az (n) of the Xs, Ys and Zs axes of gravity acceleration in the sensor coordinate system, respectively. Is arranged. Further, the magnetic sensors 17, 18 and 19 shown in FIG. 1 are arranged to detect geomagnetic component forces Mx (n), My (n) and Mz (n) on the Xs, Ys and Zs axes, respectively. .

  Outputs Ax (n), Ay (n), and Az (n) of the acceleration sensor are provisional roll angles γ ′ (n), θ that are inclination angles with respect to the horizontal plane (XY plane) of the reference coordinate system according to the following formula. '(N) and provisional pitch angle β' (n), ψ '(n) are calculated.

β ′ (n) = sin ⁻¹ Ax (n)
γ ′ (n) = tan ⁻¹ [−Ay (n) / − Az (n)]
θ ′ (n) = − sin ⁻¹ Ay (n)
ψ ′ (n) = tan ⁻¹ [Ax (n) / − Az (n)]

  In order to obtain the provisional yaw angle α ′ (n) in the reference coordinate system, the geomagnetic components HX1 and HY1 in the reference coordinate system are assumed to be temporary and the outputs Mx (n), My (n) and Mz (n) of the geomagnetic sensor. Is converted using the pitch angle β ′ (n) and the temporary roll angle γ ′ (n).

HX1 (n) = Mx · cos β ′ (n) + [My · sin γ ′ (n) + Mz · cos γ ′ (n)] · sin β ′ (n)
HY1 (n) = My · cos γ ′ (n) −Mz · sin γ ′ (n)

The provisional yaw angle α ′ (n) in the reference coordinate system is
α ′ (n) = − tan ⁻¹ [HY1 (n) / HX1 (n)] − α (0)
It is represented by Where α (0) is the azimuth angle at the origin.

Similarly, in order to obtain the provisional yaw angle φ ′ (n) in the reference coordinate system, the geomagnetic components HX2 and HY2 in the reference coordinate system are output as the outputs Mx (n), My (n) and Mz (n) of the geomagnetic sensor. ), A temporary pitch angle ψ ′ (n), and a temporary roll angle θ ′ (n), coordinate conversion is performed as in the following equation.
HX2 (n) = Mx · cos ψ ′ (n) + Mz · sin ψ ′ (n)
HY2 (n) = My · cos θ ′ (n) + [Mx · sin ψ ′ (n) −Mz · cos ψ ′ (n)] · sin θ ′ (n)

The temporary yaw angle φ ′ (n) in the reference coordinate system is φ ′ (n) = − tan ⁻¹ [HY2 (n) / HX2 (n)] − α (0).
It is represented by

Next, the discrimination device will be described. The output Ax, Ay, and Az of the acceleration sensor is a composite vector of the Xs, Ys, and Zs axis direction components of the gravitational acceleration and the Xs, Ys, and Zs axis direction components of the motion acceleration. The temporary pitch angle β ′ calculated by the angle calculation device does not become a correct pitch angle. Also, the temporary yaw angle α ′ may not be a correct yaw angle depending on the magnetic field environment. Whether the discriminating device is correct whether α ′ (n), β ′ (n), γ ′ (n), φ ′ (n), θ ′ (n), ψ ′ (n), which are outputs of the static angle calculation device, are correct Is to discriminate.
When the motion acceleration is not applied, the combined vector of Ax, Ay and Az is 1G.
| √ (Ax ² + Ay ² + Az ² ) −1 | <ε
If the above holds, it is determined that α ′, β ′, γ ′, φ ′, θ ′, and ψ ′ are correct.

Next, a calculation method performed by the attitude angle calculation device that calculates the output of the motion angle calculation device and the output of the static angle calculation device according to the determination result of the determination device to obtain the final output will be described.
When it is determined that α ′, β ′, γ ′, φ ′, θ ′, ψ ′ is correct in the discriminator,
α ″ (n) = α (n−1) + Δα−k [α (n−1) + Δα−α ′ (n)]
β ″ (n) = β (n−1) + Δβ−k [β (n−1) + Δβ−β ′ (n)]
γ ″ (n) = γ (n−1) + Δγ−k [γ (n−1) + Δγ−γ ′ (n)]
φ ″ (n) = φ (n−1) + Δφ−k [φ (n−1) + Δφ−φ ′ (n)]
θ ″ (n) = θ (n−1) + Δθ−k [θ (n−1) + Δθ−θ ′ (n)]
ψ ″ (n) = ψ (n−1) + Δψ−k [ψ (n−1) + Δψ−ψ ′ (n)]
To calculate the attitude angle.

When it is determined that α ′, β ′, γ ′, φ ′, θ ′, ψ ′ is incorrect in the discrimination device,
α ″ (n) = α (n−1) + Δα
β ″ (n) = β (n−1) + Δβ
γ ″ (n) = γ (n−1) + Δγ
φ ″ (n) = φ (n−1) + Δφ
θ ″ (n) = θ (n−1) + Δθ
ψ ″ (n) = ψ (n−1) + Δψ
To calculate the attitude angle. However, k is a correction coefficient and 0 <k <1.

  Finally, the Euler angle conversion arithmetic device will be described. The outputs from the attitude angle calculation device are ZYX Euler angles (α ″, β ″, γ ″) and ZXY Euler angles (φ ″, θ ″, ψ ″). It is. When | β ″ |> 60 degrees, the ZY-X Euler angle is less accurate, and when | θ ″ |> 60 degrees, the Z-X-Y Euler angle is less accurate. '|> 60 ° ZY-X Euler angles (α ″, β ″, γ ″), and | θ ″ | ≦ 60 ° Z-X-Y Euler angles (φ ″) , Θ ″, ψ ″).

  When the ZY-X Euler angles (α ″, β ″, γ ″) are adopted, they are output as they are as final outputs (α, β, γ) (α = α ″, β = β ″, γ = γ ″), and further converted into (φ, θ, ψ).

  When the Z-X-Y Euler angles (φ ″, θ ″, ψ ″) are adopted, they become (φ, θ, ψ) (φ = φ ″, θ = θ ″, ψ = Ψ ″), and further converted into (α, β, γ) that is the final output.

  Hereinafter, a method for converting the ZY-X Euler angles (α ″, β ″, γ ″) to the Z-X-Y Euler angles (φ, θ, ψ) will be described. How the two points on the sensor coordinate axis, P (1,0,0), Q (0,1,0) are represented on the reference coordinate axis will be described with reference to FIGS. When the ZY-X Euler angles (α ″, β ″, γ ″) are employed, the points P and Q are rotated as shown in FIG. 3 to calculate the position on the reference coordinate axis. Equation 1 is a calculation formula for expressing P and Q on the sensor coordinate axis in reference coordinates using ZY-X Euler angles.

Next, as shown in FIG. 4, considering the Q ′ point that is the intersection of the perpendicular from the Q point to the XY plane and the XY plane, the yaw angle φ and the roll angle θ are calculated by the following equations.
φ = tan ⁻¹ (−Qx / Qy)
θ = tan ⁻¹ (Qz / √ (Qx ² + Qy ² ))

Next, as shown in FIG. 4, P ′ (Px ′, Py ′, 0), which is the intersection of the perpendicular to the X ′ axis and the X ′ axis, which is obtained by rotating the X axis from the point P by φ around the Z axis. Ask. Since the line segment PP ′ and the X ′ axis are orthogonal, when Px ′ and Py ′ are calculated,
Px ′ = Px cos ² φ + Py sin φ cos φ
Py ′ = Px sinφ cosφ + Py sin ² φ
It becomes.

Therefore, ψ is from the point P ′,
ψ = ± cos ⁻¹ √ (Px ′ ² + Py ′ ² )
Is required. The sign is negative when Pz> 0 and positive when Pz <0.

  In the following, a method for converting the Z-X-Y Euler angles (φ ″, θ ″, ψ ″) to Z-Y-X Euler angles (α, β, γ) will be described. How the two points on the sensor coordinate axis, P (1,0,0), Q (0,1,0) are represented on the reference coordinate axis will be described with reference to FIGS. When the Z-X-Y Euler angles (φ ″, θ ″, ψ ″) are adopted, the points P and Q are rotated as shown in FIG. 5 to calculate the position on the reference coordinate axis. Equation 2 is a calculation formula for expressing P and Q on the sensor coordinate axis in reference coordinates using the ZXY Euler angles.

Next, considering a point P ′ that is an intersection of a perpendicular line from the point P to the XY plane and the XY plane as shown in FIG. 6, the yaw angle α and the pitch angle β are calculated by the following equations.
α = tan ⁻¹ (Py / Px)
β = tan ⁻¹ (−Pz / √ (Px ² + Py ² ))

Next, as shown in FIG. 6, Q ′ (Qx ′, Qy ′, 0), which is the intersection of the perpendicular to the Y ′ axis and the Y ′ axis, which is obtained by rotating the Y axis from the Q point by α around the Z axis. Ask. Since Qx ′ and Qy ′ are calculated because the line segment QQ ′ and the Y ′ axis are orthogonal, Qx ′ = Qxsin ² α−Qy sinα cosα
Qy ′ = − Qxsin α cos α + Qy cos ² α
It becomes. Therefore, γ is from the point Q ′,
γ = ± cos ⁻¹ √ (Qx ′ ² + Qy ′ ² )
Is required. The sign is positive when Qz> 0 and negative when Qz <0.

The block diagram of an example of the attitude | position angle detection apparatus in this invention. Explanatory drawing regarding the whole structure of the attitude | position angle detection apparatus in this invention. Explanatory drawing which rotates P point and Q point by ZYX Euler angle. Explanatory drawing which calculates | requires ZXY Euler angle from P point and Q point. Explanatory drawing which rotates P point and Q point by a ZXY Euler angle. Explanatory drawing which calculates | requires a ZYX Euler angle from P point and Q point.

Explanation of symbols

11 1st gyro 12 2nd gyro 13 3rd gyro 14 1st acceleration sensor 15 2nd acceleration sensor 16 3rd acceleration sensor 17 1st magnetic sensor 18 2nd magnetic sensor 19 3rd magnetism Sensors 22a, 22b, 22c Gyros 23a, 23b, 23c Acceleration sensors 24a, 24b, 24c Magnetic sensor 26a Motion angle computing device 26b Stationary angle computing device 26c Attitude angle computing device 27 Discriminating device 28 Euler angle conversion computing device 29 Output

Claims (1)

  The axes orthogonal to each other in the horizontal plane are the X-axis and the Y-axis, the axes orthogonal to the X-axis and the Y-axis are the Z-axis, and the coordinate axes composed of these fixed X-axis, Y-axis, and Z-axis are the reference coordinates, Three gyroscopes for detecting angular velocities around the Xs axis, the Ys axis, and the Zs axis, when the coordinate axis composed of the Xs axis, the Ys axis, and the Zs axis that moves together with the attitude angle detection device is used as the sensor coordinate, and the gyroscope It is arranged to detect the motion angle calculation device that calculates the angle moved in unit time based on the output according to the angular velocity with two types of Euler angles, and the biaxial acceleration orthogonal to each other on the Xs-Ys plane. An acceleration sensor, a geomagnetic sensor arranged to detect two axes of geomagnetism orthogonal to each other in the Xs-Ys plane, and the outputs of the acceleration sensor and the geomagnetic sensor, about the X axis, the Y axis, and the Z axis Times A static angle calculation device that calculates the angle with two types of Euler angles, a determination device that determines whether the calculation result is true or false, and a calculation result of the motion angle calculation device according to the calculation result of the determination device And the posture angle calculation device that calculates the posture angle to be output from the calculation result of the static angle calculation device with two types of Euler angles, and the Euler angle that should be used with a small error from the two types of Euler angles. A posture angle detection device comprising: an Euler angle conversion arithmetic device that converts an Euler angle.

Images