JP2004163443A - Attitude angle detector - Google Patents

Abstract

PROBLEM TO BE SOLVED: To correctly obtain an attitude angle without using an azimuth sensor having no drift even if a gyro bias error is large, and even if there is a deviation in a mounting state.
When the angular velocity bias estimator 43 determines that the input angular velocity ω _B is equal to or less than a determination value, the angular velocity input to the apparatus is determined to be zero, and switches 45 and 47 are switched to the 0 registers 44 and 46. cumulatively adds the input omega _B of, and updates the angular velocity bias error [Delta] [omega _BS.
[Selection diagram] Fig. 1

Description

  The present invention relates to a device for detecting a posture angle by mounting a gyro and an accelerometer.

For example, this kind of posture angle detection device was attached to a human head, the movement of the head was detected, and the virtual space image displayed on the display was changed according to the movement of the head. There is something.
As shown in FIG. 7, in a conventional attitude angle detecting device, for example, each output of a gyro 11 having three axes orthogonal to each other as input axes is converted into digital values by an AD converter 12, and these digital values are converted by a subtractor 13. initial bias reset is subtracted, is the body angular velocity omega _B is further corrected by the correction unit 14.
On the other hand, the output of the accelerometer 15 to an input shaft and an axis perpendicular to its output when the analog signal is converted into a digital value by the AD converter 16, and the output as a body acceleration A _B is corrected by the correction unit 17 Is done.

Direction cosine matrix C and the body acceleration A _B than attitude reference computation unit 18 is input to the horizontal acceleration calculator 19, calculates the horizontal acceleration A _L, the horizontal acceleration A _L is the angular velocity error Δω in horizontal acceleration damping unit 21 _BA is estimated, the angular velocity error Δω _BA is subtracted from the body angular velocity ω _B by the subtractor 22, and the subtraction force is calculated by integrating the angular velocity in the attitude reference calculation unit 18 to calculate the attitude reference. The criterion is converted into an Euler angle by the Euler angle calculation unit 23, and is output as a posture angle in the local coordinate system.
The correction units 14 and 17 perform filter processing after AD conversion and correction based on a misalignment correction value, a scale factor correction, a bias correction value, and the like, which are obtained at the time of adjustment performed at the time of shipping the sensor.

In horizontal acceleration damping unit 21, a horizontal acceleration A _L sought is the direction cosine matrix C and the body acceleration A _B as shown in FIG. 8 is multiplied by the horizontal acceleration calculation unit 19, gravitational acceleration (0 from horizontal acceleration A _L, (0, G) is subtracted by the subtracter 25, and the horizontal component ΔA _L = (ΔA _LX , ΔA _LY , 0) is calculated by the acceleration calculator 26 as angular velocity Δω _L1 = (Δω _L1X , Δω _L1Y , 0). , Δω _L1 are converted into body coordinate components Δω _B1 by the direction cosine matrix C in the coordinate conversion unit 27. The .DELTA.A _L the acceleration calculating unit 28 at an angular velocity _{Δω L2 = (Δω L2X, Δω} L2Y, 0) is calculated, [Delta] [omega _L2 is integrated by the integrator 31 to be converted in the body coordinate component in the coordinate transformation unit 29. The integration result and Δω _B1 are added by the adder 32 to obtain an angular velocity error Δω _BA .
The horizontal acceleration damping calculation unit 21 may be configured as shown in FIG. 9, parts different from FIG. 8 is a horizontal acceleration component .DELTA.A _L is the error angle component by the angle conversion section 33 (Δφ, Δθ, 0) is calculated, the error angle component with respect to the output of the attitude reference computation unit 18, The error correction is performed by performing rotation (torking) based on the posture by the subtractor 34.

When a posture angle detecting device is mounted on a human head, it is necessary to make it small and light with a size of several cm3 and a weight of about 100 g or less. From this point, a gyro (angular velocity sensor) or accelerometer used has to be used with low accuracy and large variation in error. When such a low-accuracy gyro is used, an angular velocity error occurs, causing a phenomenon in which the attitude angle drifts. In order to suppress this, a loop for estimating a gyro error is configured using an angle sensor such as an inclinometer having no angle drift.
Also, at the time of reset, that is, at the time of initial gyro bias estimation, for a gyro having a large gyro bias error, the AD conversion value of zero angular velocity is greatly shifted from the center of the AD converter, and the effective AD conversion range of the gyro output is narrowed. That is, conventionally, as shown in FIG. 10A, the gyro output is directly input to the AD converter 12, and the output of the AD converter 12 passes through the subtractor 13 and is turned on at the time of reset (at the time of gyro bias estimation). Is input to the gyro bias estimating unit 422, and the value is held as an initial bias and supplied to the subtractor 13, so that the output of the subtractor 13 becomes zero.

  As described above, since the gyro sensor output is directly input to the AD converter 12, in a place having a different temperature environment, or in a case of a sensor having a large error due to a gyro bias or a fluctuation in the bias, as shown in FIG. The AD converter 12 has an input range (conversion range) of 0 to 5 V and an input having an angular velocity of zero when the input is 2.5 V at the center of the input range. However, since the gyro bias and the drift error are large, these are used. Assuming that the gyro output at the time when the angular velocity is zero is 3.2 V, the range (−side angular velocity range) that can respond to the input change on the − side is 3.2 V, but the + side angular velocity range is only 1.8 V. Absent. Therefore, the effective range is only ± 1.8 V, that is, it is restricted to a narrower range. As a countermeasure, it is necessary to eliminate an angular velocity sensor having poor performance in advance, or to attach an adjusting resistance element to adjust the sensor before shipment, and to input the bias error value to the subtractor 13 in FIG. All of these are expensive and cannot cope with the case where the gyro bias greatly varies depending on the temperature environment.

For example, when a posture angle detection device is mounted on a human head and used, it is difficult to accurately mount the vertical axis of the posture angle detection device in parallel with the vertical axis of the human head. There is a risk that rotation will be input around. That is, as shown in FIG. 11A, when the attitude angle detecting device 39 is mounted on the human head 38, the horizontal axis of the device 39 can be mounted horizontally, that is, both the roll angle and the pitch angle become 0 degrees. 11A, when the device 39 is mounted at an angle of [Delta] [theta] in the pitch direction and the true angular velocity [omega] about the vertical axis is input, the angular speed [omega] (Δθ) is input, and the angular velocity ωcos (Δθ) is also input to the Z axis, so that a correct attitude angle cannot be detected.
JP-A-61-86613 U.S. Pat. No. 4,212,443

The first problem is that the attitude angle detection device does not require adjustment of the gyro bias before shipping, and can sufficiently secure the range of the AD converter even when a large gyro bias is input.
A second problem that a gyro output can be used to suppress drift of an azimuth angle (yaw angle) without using an angle sensor such as a magnetic azimuth sensor having no angle drift;
There is a third problem that the attitude angle can be easily determined even if the vertical axis of the device is not mounted exactly parallel to the vertical axis of the object to be mounted. It aims to solve the problem of.

  According to the technique for solving the first problem, first and second correctors are provided, these are operated by a reset command, a gyro output is directly input to the first corrector, and an estimated gyro bias error is provided. Is subtracted, and the gyro bias error is estimated from the subtracted analog value such that the subtraction result is reduced. The gyro output corrected by the first corrector is subtracted from the gyro bias correction residual of the digital value estimated by the second corrector, and the gyro bias is set so that the digital value of the subtracted result becomes smaller. A correction residual is estimated, and a gyro output of a digital value obtained by subtracting the gyro bias correction residual is used to obtain an attitude angle.

According to the present invention for solving the second problem, when the angular velocity obtained from the gyro output becomes equal to or less than the specified value in the angular velocity bias estimating unit, it is determined that the input angular velocity to the apparatus is zero, and the horizontal acceleration damping is performed. The input of the calculation unit is set to zero, the gyro bias error estimated from the gyro output is subtracted, and the gyro bias error is estimated from the result of the subtraction.
The following technology can be considered to solve the third problem. The initial attitude angle calculation unit is operated by the reset command, the initial attitude angle is calculated from the output of the accelerometer, the attitude standard is corrected by the initial attitude angle in the attitude angle setting unit, and the corrected attitude standard is used for the Euler angle calculation. Used.

  According to the present invention, even if there is a large angular velocity bias error, the effective AD conversion range of the gyro output is not narrowed, and the angular velocity bias error can be estimated without using an azimuth sensor that does not cause drift.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings. FIG. 1 shows an embodiment of the present invention, and portions corresponding to FIG. 7 are denoted by the same reference numerals. In this embodiment, a gyro bias correction unit 40 is provided, and a gyro bias error is subtracted from the output of the gyro 11. In the gyro bias correction section 40, the output of the gyro 11 is first corrected by the first corrector 41 by subtracting the estimated gyro bias error at the time of reset by analog calculation, and the corrected gyro output is converted into a digital value by the AD converter 12. The gyro output of the digital value is corrected by the second corrector 42 by subtracting the estimated gyro bias correction residual at the time of reset by digital calculation. The corrected gyro output is corrected by the correction unit 14 and supplied to the subtractor 22.

  The output of the subtractor 22 is used to estimate a gyro bias error from the output of the subtracter 22 when the angular velocity input to the device is regarded as being zero in the angular velocity bias estimator 43, and the estimated gyro bias error is The subtracter 22 subtracts the gyro output from the gyro output corrected by the correction unit 14 and corrects the gyro output. The input side of the horizontal acceleration damping unit 21 is switched and connected to the output side of the horizontal acceleration calculation unit 19 and the 0 register 44 by a switch 45, and the angular velocity bias estimating unit 43 detects that the angular velocity input to the device is zero. When the state is regarded as the state, the switch 45 is switched to the 0 register 44 side.

Further, in this example, the input side of the attitude reference calculation unit 18 is switched and connected to the output side of the subtractor 22 and the 0 register 46 by a switch 47.
The initial attitude angle at the time of reset is calculated by the initial attitude angle calculation unit 48 based on the output of the accelerometer, and the direction cosine matrix from the attitude reference calculation unit 18 is rotated and corrected by the output attitude angle setting unit 49 by the initial attitude angle, The rotation-corrected direction cosine matrix is supplied to the Euler angle calculation unit 23.

Next, a specific example of each unit newly provided in this embodiment will be described.
First, examples of the first and second correctors 41 and 42 that perform bias correction at the time of reset will be described with reference to FIG. 2A. Each time the first and second correctors 41 and 42 are used, the first and second compensators 41 and 42 are operated by a reset switch or a reset command from a personal computer that controls the attitude angle detector. (When the angular velocity input is zero) Obtain the bias error value.
In the first corrector 41, the gyro output is directly input to the subtractor 411, the gyro bias error B _i from the DA converter 412 is subtracted, and the subtracted gyro output is converted into the digital value Δ by the AD converter 12. _i , and the digital value is input to the error estimator 414 through the switch 413 which is turned on by the reset command. Digital value input in the error estimating unit 414 a noise component is removed by the noise removal filter 415, the error correcting unit 416 to the gyro bias error B _i of the previous and summed output delta _i of the subtracter 411, The gyro bias error is updated to B _{i + 1,} and the updated B _{i + 1} is converted into an analog signal by the DA converter 412 and supplied to the subtractor 411. This updating loop is repeated several times so that the output of the AD converter 12 becomes zero, and the process is repeated until the output value of the AD converter 12 becomes within the LSB (minimum digit bit) of the DA converter 412.

As the noise removal filter 415, a low-pass filter or a filter that performs a moving average is used. The finally obtained bias error at the time of reset is held in a register in the error correction unit 416, converted into an analog signal by the DA converter 412, and supplied to the subtractor 411.
After the value of the bias error B _i at the time of resetting is determined by the first corrector 41, the output of the first corrector 41, in this example, the output of the AD converter 12 is supplied to the subtractor 421 of the second corrector. The gyro bias correction residual from the correction residual estimator 422 is subtracted, and the correction is performed so that the correction residual (the value equal to or less than the LSB of the DA converter 412) in the first corrector 41 becomes zero. The output of the subtractor 421 is input to the correction residual estimator 422 through a switch 423 that is turned on by a reset command. In the correction residual estimating unit 422, a filtering process for taking an average value of, for example, about 0.3 seconds is performed to remove noise, and the correction residual of the gyro bias error by the first corrector 41 is estimated. The data is supplied to the calculator 421. The gyro bias correction value for the correction residual is also stored in, for example, a register and is continuously supplied to the subtractor 421.

  When the conversion range of the AD converter 12 is 0 to 5.0 V, as shown in FIG. 2B, when the gyro output at rest is 3.2 V, the conversion range for the + angular velocity is 1.8 V, and for the-angular velocity. The conversion range is 3.2 V, and the effective angular velocity range is only ± 1.8 V as described with reference to FIG. 10B. However, when 0.7 V is output as the gyro bias error from the DA converter 412 in FIG. 2A, the output of the subtractor 411 becomes 2.5 V in the stationary state, and as shown in FIG. The −angular velocity range is also 2.5, and the effective angular velocity range is maximum in the + direction and the − direction. Since the D / A converter 412 actually has a conversion error within the LSB, the output of the subtractor 411 has a magnitude within the LSB of the D / A converter 412 with respect to the center 2.5 V of the input range of the A / D converter 12. Is output from the subtractor 411. This is corrected by the second corrector 42. As described above, even if the gyro bias error greatly changes due to the temperature environment or the like, the gyro bias error value at the time of resetting is obtained each time use is performed, so that the detection range of the angular velocity can always be maximized, and The gyro bias error can be corrected.

  It is desirable to increase the resolution of the AD converter 12 from the point that the attitude reference calculation (integration) is performed by the attitude reference calculator 18 in FIG. When the resolution of the AD converter 12 is increased, its convertible range is narrowed. Therefore, the configuration shown in FIG. 2A cannot remove a large initial gyro bias. In such a case, the configuration shown in FIG. That is, the output of the subtractor 411 is converted into a digital value by the AD converter 417 having a lower resolution than the AD converter 12, and the converted digital value is supplied to the noise removal filter 415 through the switch 413. In this way, the gyro output when the angular velocity is zero is corrected by the subtractor 411 even with a large gyro bias error by using the AD converter 417 having a wide range. It is made to be the center value. Thereafter, the corrected gyro output of the subtractor 411 is converted into a digital value by the AD converter 12 having a higher resolution (for example, about 20 times higher) than the AD converter 417, and the first correction is performed by the second corrector 42. A correction value for the correction residual of the gyro bias error at the time of reset in the unit 41 is obtained. As shown in FIGS. 2A and 3, in the gyro bias correction unit 40, the first correction unit 41 first performs correction using an analog amount. Therefore, even when there is a large gyro bias error, when the correction is performed using a digital value. Due to the saturation of the AD converter, there is no possibility that the correction is insufficient and the effective operating range is narrowed. In this specification, the AD converters such as the AD converters 12 and 417 are not narrowly defined, but may include, for example, an amplifier such as an amplifier. In other words, the AD converters convert an analog value into a digital value. Similarly, the DA converter 412 is not in a narrow sense, and may include an amplifier or the like. In other words, the DA converter 412 only needs to convert a digital value into an analog value.

Next, the angular velocity bias estimating process in the angular velocity bias estimator 43 in FIG. 1 will be described with reference to FIG. The corrected gyro output ω _B from the correction unit 14 in FIG. 1 is input to an angular velocity error determination processing unit 432 after a noise component is removed by a noise removal filter 431. The noise removal filter 431 removes noise components by moving average processing or low-pass processing. The characteristics of the noise removal filter 431 are determined according to the environment in which the attitude angle detection device is used.
The angular velocity error determination processing unit 432 refers to the determination threshold table 433, determines that an error is present if the input angular velocity is within a predetermined angular velocity threshold range, turns on the switches 434 and 435, and The switch 45 in 1 is switched to the 0 register 44 side, and the switch 47 is also switched to the 0 register 46 side as necessary. Output deemed error of the noise removal filter 431 is input to the angular velocity bias estimation processor 436 via the switch 434, is an angular velocity bias value [Delta] [omega _i at that time. The angular velocity bias value [Delta] [omega _i are input to the angular velocity bias integrator 437 through the switch 435, the previous angular velocity bias [Delta] [omega _BS, [Delta] [omega _i is added to _i-1 and the angular velocity bias value [Delta] [omega _BS is updated. That is, the following equation is calculated.
Δω _{BS, i} = Δω _{BS, i-1} + Δω _i
In this case, since 0 is input to the horizontal acceleration damping unit 21 through the switch 45 in FIG. 1, there is no possibility that the angular velocity error Δω _BA in FIGS. 8 and 9 changes, and the angular velocity bias value Δω _BS can be correctly obtained. it can.

When the determination of the error in the angular velocity error determination processing unit 432 is continuously performed N times within the angular velocity threshold range, the output of the noise removal filter 431 is the error generated when the angular velocity input to the apparatus is zero. May be determined. For this purpose, FIG. 5 shows an example of processing for one j by setting the X axis when j = 0, the Y axis when j = 1, and the Z axis when j = 2. The flag F _j = 0 and (S1) It is checked whether the gyro output (output of the filter 431) ω _{j of the} j-axis input is within the range of the determination threshold ω _jm , that is, −ω _jm <ω _j <+ ω _jm ( S2), not within the threshold range n _j 0 and (S3), this if within a threshold range to update the n _j by +1 (S4), its updated n _j n It is checked whether the above is true (S5). If it is N or more, it is in a stationary state, ω _j at that time is determined as the j-axis angular velocity error Δω, and the flag F _{j is set} to 1 (S6). Although not shown in the drawing, j is updated and the same processing is performed to successively obtain errors in angular velocities around each axis.

Various values are stored in the determination threshold table 433 for each of the input axes X, Y, and Z of each gyro, and the sensitivity to the input angular velocity is changed by the user selecting the determination threshold according to the use state. It may be possible to do so. The sensitivity to the input angular velocity increases as the determination threshold decreases. In the illustrated example, the determination threshold value is 2 ゜ / sec common to all axes, but may be set for each axis. As shown in FIG. 5, it was determined whether or not the input was within the threshold value for each axis, but only when the input ω _{j of} all axes was simultaneously within the threshold value, the gyro output of each axis was determined. May be determined as an error. Depending on the case, it may be performed for any combination of axes. These may be determined according to the situation of use of the attitude angle detecting device.

When the angular velocity error determination processing unit 432 determines that the input is outside the range of the determination threshold, the switches 434 and 435 are turned off, and the switches 45 and 47 in FIG. It is switched as follows. The output of the angular velocity bias integrator 437 when the switch 435 is turned off is output as the latest angular velocity bias Δω _BS . When the angular velocity input to the apparatus is determined to be zero and the switches 434 and 435 are turned off and the switches 45 and 47 are connected to the 0 registers 44 and 46, the posture reference calculation unit 18 in FIG. Since the input becomes zero and the Euler angle calculation unit 23 continues to output the posture angle at that time, for example, when displaying an image corresponding to this posture angle, the image does not fluctuate and become unsightly.

  According to the angular velocity bias estimation processing shown in FIG. 4, the angular velocity bias of the gyro output in each axis can be estimated. That is, when the attitude angle detecting device is used on a substantially horizontal plane, the Z-axis gyro is not corrected by horizontal acceleration damping. For this reason, the yaw angle drifts due to an error of the angular velocity sensor used normally, but the yaw angle drift can be suppressed by the angular velocity bias estimation processing shown in FIG. Conventionally, yaw angle error correction is performed by combining a direction sensor other than a gyro such as a magnetic direction sensor. However, according to this embodiment, such a direction sensor other than the gyro is not required.

Next, the output attitude angle setting unit 49 in FIG. 1 will be described. As shown in FIG. 6, the output of the accelerometer is input to the noise removal filter 492 through a switch 491 that is turned on at the time of resetting. For example, averaging is performed N times. Is entered. The posture angle calculator 493 calculates a posture angle based on the input acceleration, and outputs the initial posture angle as an initial direction cosine matrix C ₀ . In the attitude reference calculation unit 18 in FIG. 1, the attitude angle is calculated from the acceleration as an initial value of the integration. However, if the attitude angle calculation unit 493 calculates the initial attitude angle in the same manner, Good. The initial direction cosine matrix C ₀ is stored in a storage unit in the posture angle calculation unit 493.

Using the initial direction cosine matrix C ₀ obtained in this way, the direction cosine vector C which is the attitude reference obtained by the attitude reference calculation unit 18 in FIG.
C ′ = C · C ₀ ^T , C ₀ ^T are calculated by the transposition matrix output attitude angle setting unit 49 of C ₀ , and a direction cosine vector C ′ rotated by the reset attitude angle is obtained. To calculate the Euler angles. In this way, even if the attitude angle detecting device 39 is attached while being tilted by, for example, Δθ in the pitch direction as shown in FIG. 11A, the attitude reference is rotated by Δθ by the initial attitude angle C ₀ as shown in FIG. 11B. There is no possibility that the corrected angular velocity is input to an unexpected axis, and a correct attitude angle can be output accordingly.
As described above, the initial attitude angle is obtained from the output of the accelerometer. Therefore, if there is a bias error in the accelerometer, a correct horizontal plane cannot be detected and an error occurs in the initial attitude angle. It is difficult to install the attitude angle detecting device horizontally after each reset after attaching it to the head 38 or the like. Further, a bias error correction mode that can be set by a user may be prepared so that an error can be corrected even after shipping.

That is, the attitude angle detector 39 is placed as close as possible to the horizontal plane, and as shown in FIG. 6, in this correction mode, the switch 494 is turned on and the output of the accelerometer is supplied to the subtractor 495 to supply the gravitational acceleration (0 , 0, 1G), the difference is averaged N times by an acceleration bias estimator 496 as needed to estimate the acceleration bias, and the estimated value is stored in the nonvolatile memory 497. Thereafter, the switch 498 is turned on, the acceleration bias correction value from the memory 497 is input to the subtractor 499, the acceleration bias correction value is subtracted from the accelerometer output, and the result is converted to the gravitational acceleration (0, 0, 1G). To be. In this way, the correct acceleration can be obtained from the output of the subtractor 497 from the next power ON.
In the above description, each unit may be caused to function by executing a program by a computer.

FIG. 1 is a block diagram showing a functional configuration of an embodiment of the present invention. A is a diagram showing a specific example of the first and second correctors 41 and 42 in FIG. 1, and B and C are diagrams for explaining the same. FIG. 2 is a diagram showing another specific example of the first and second correctors 41 and 42 in FIG. 1. FIG. 2 is a diagram illustrating a specific example of an angular velocity bias estimating unit 43 in FIG. 1. 5 is a flowchart showing an example of a part of a processing procedure in an angular velocity error determination processing unit 432 in FIG. 4. FIG. 2 is a diagram showing a specific example of an initial attitude calculation unit 48 in FIG. 1. FIG. 9 is a block diagram showing a conventional attitude angle detection device. FIG. 8 is a diagram illustrating a specific example of a horizontal acceleration damping unit 21 in FIG. 7. The figure which shows other examples. A is a diagram showing a configuration of a conventional gyro bias correction, and B is a diagram for explaining the configuration. FIG. 7A is a diagram illustrating a state where an angular velocity is generated on a necessary axis when the mounting state is inclined, and FIG.

Claims (1)

A gyro and an accelerometer are provided, a direction cosine matrix is calculated using the output of the gyro, a horizontal acceleration is detected from the output of the accelerometer, and an angular velocity error is calculated from the direction cosine matrix and the acceleration by a horizontal acceleration damping unit. Is estimated, and the angular velocity error is subtracted from the gyro output to obtain an attitude angle.
When the angular velocity obtained from the output of the gyro is equal to or less than a predetermined value, it is determined that the angular velocity input to the apparatus is zero, and the input of the horizontal acceleration damping unit is set to zero, and a gyro bias error is estimated from the output of the gyro. An angular velocity bias estimator;
And a subtraction unit that subtracts the estimated gyro bias error from the gyro output and outputs the result to the attitude reference calculation unit.

Images