JPH06201863A - Strapped-down attitude detection apparatus - Google Patents

Abstract

PURPOSE:To eliminate that an acceleration error is caused in the circling motion of a navigating body and to output a sideslip-angle component to the outside by a method wherein an operation part operates a roll-angle signal, a pitch-angle signal and a magnetic-declination signal as well as the sideslip-angle component. CONSTITUTION:In a mounting shaft 10, an X-accelerometer 11A and a speedometer 11V which respectively detect an acceleration an a velocity in the X-axis direction and an X-gyro 11G which detects an angular velocity around the X-axis are attached to the X-axis. A Y-accelerometer 12A which detects an acceleration in the Y-axis direction and a Y-gyro 12G which detects an angular velocity around the Y-axis are attached to the Y-axis. A Z-accelerometer 13A which detects an acceleration in the Y-axis direction, a Z-gyro 13G which detects an angular velocity around the Y-axis and a magnetic-declination detector 13Y are attached to the Z-axis. An operation part 2' performs an operation by using output signals from the accelerometers, the gyros and the like for the three axes, it outputs a roll-angle signal ROL, a pitch-angle signal PIT and a magnetic-declination signal YAW for the mounting shaft 10 as attitude angles, and it outputs also a sideslip-angle component delta.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a strapdown-type attitude detecting device capable of outputting a side slip angle component of a navigation body by using a speedometer, a gyro, an accelerometer and a magnetic direction detector.

[0002]

2. Description of the Related Art As a conventional strap-down type posture detecting device, one shown in FIGS. 4 and 5 has been proposed. FIG. 4 is a block diagram showing an entire example of a conventional strapdown-type posture detection device. This strapdown-type posture detection device includes a sensor block unit 1 and a calculation unit 2.

This sensor block unit 1 has a mounting shaft 10 which coincides with the right-handed orthogonal three-axis coordinate system (in FIG. 4, the Z-axis direction is the downward vertical direction, and the arrow in the figure indicates the detection amount of each sensor (+ ), The X accelerometer 11 detects the acceleration and the velocity in the X axis direction on the X axis (this X axis is the main direction with respect to the movement).
A and X velocity meters 11V and an X gyro 11G for detecting angular velocity around the X axis are attached, and a Y accelerometer 12A for detecting acceleration in the Y axis direction and a Y for detecting angular velocity around the Y axis are attached to the Y axis. A gyro 12G is attached, and a Z accelerometer 13A that detects acceleration in the Z axis direction, a Z gyro 13G that detects angular velocity around the Z axis, and an azimuth angle around a vertical axis in the gravity acceleration direction are detected on the Z axis. A magnetic azimuth detector 13Y is attached. Each of these three-axis accelerometers 11A, 12A, 13 of the sensor block unit 1
Output signals from the A, 3-axis gyros 11G, 12G, 13G, the magnetic azimuth detector 13Y, and the speedometer 11V are input to the calculation unit 2.

The calculation unit 2 calculates using this input signal to calculate the roll angle signal ROL, the pitch angle signal PIT, and the azimuth angle signal Y of the mounting shaft 10 of the sensor block unit 1.
AW is output as the attitude angle.

FIG. 5 is a block diagram showing the configuration of an example of the arithmetic unit 2 of FIG. The output signals of the three-axis gyros 11G, 12G, and 13G of the sensor block unit 1 are X gyro signal GX, Y gyro signal GY, and Z gyro signal G.
Z (hereinafter, these three gyro signals may be referred to as Gi) may be input to the CTM calculation unit 20 via a gyro bias correction unit 26 of the calculation unit 2 which will be described later. This C
In the TM operation unit 20, the input gyro signal Gi
From the coordinate system of this mounting axis 10 to the local coordinate system (X
A 3 × 3 matrix for conversion into a coordinate system in which the axial direction is the north direction and the Z-axis direction coincides with the gravity direction) is calculated.

Furthermore, the attitude angle with respect to the local coordinate system is determined by the elements of this 3 × 3 matrix, and the roll angle signal R with respect to the X axis is used.
The pitch angle signal PIT for the OL and Y axes and the azimuth angle signal YAW for the Z axis are calculated and output as the calculation unit 2. However, 3 × 3 calculated by the CTM calculator 20
An attitude angle error occurs in the matrix due to a difference between the initial value of the 3 × 3 matrix used for the calculation and the initial value of the mounting shaft 10 of the sensor block unit 1. The horizontal component of the attitude angle error generated in the CTM calculation unit 20 is corrected using the horizontal error calculation unit 21, the acceleration error correction unit 22, and the correction amount calculation unit 24.

A 3 × 3 matrix signal C from the CTM calculator 20
TMS and 3-axis accelerometer 1 of this sensor block unit 1
The output signals of 1A, 12A, and 13A are X acceleration signals A
The X, Y acceleration signals AY, Z acceleration signals AZ (hereinafter, three acceleration signals may be referred to as Ai) are input to the horizontal error calculation unit 21 of the calculation unit 2. In the horizontal error calculation unit 21, the X-axis direction attitude angle error ERRX ′ and the Y-axis direction attitude angle error of the attitude angle error with respect to the local coordinate system are calculated using the input three acceleration signals Ai and the matrix signal CTMS. ERRY 'is calculated and output. The attitude angle errors ERRX ′ and ERRY ′ are calculated by the acceleration error correction unit 2
Entered in 2. In the acceleration error correction unit 22, the motion acceleration component included in the posture angle errors ERRX ′ and ERRY ′ is X attached to the X axis of the sensor block unit 1.
X-axis speed signal VX and CT which are output signals of speedometer 11V
The corrected motion acceleration component of the local coordinate system is calculated by using the azimuth angle signal YAW of the M calculation unit 20 and subtracted to output the corrected X and Y axis attitude angle errors ERRX and ERRY. The X and Y axis attitude angle errors ERRX and ERRY are input to the correction amount calculation unit 24.

In the correction amount calculation unit 24, the X and Y axis posture angles to which the X axis correction signal CX and the Y axis correction signal CY which are the coordinate correction amounts of the X axis and the Y axis of the CTM calculation unit 20 are input. The error is calculated using the errors ERRX and ERRY and output.

On the other hand, the vertical component of the attitude angle error generated in the CTM calculating section 20 is corrected using the azimuth error calculating section 23 and the correction calculating section 24. Heading error calculator 2
3, the CTM along with the input direction signal H
The azimuth signal YAW calculated and output by the calculator 20 is also input, the deviation amount between the azimuth signal H and the azimuth signal YAW is calculated, and the azimuth error ERRZ is output.

The azimuth angle error ERRZ is calculated by the correction amount calculator 24.
Entered in. The correction amount calculation unit 24 calculates and outputs the Z-axis correction signal CZ, which is the Z-axis coordinate correction amount of the CTM calculation unit 20, using the azimuth angle error ERRZ. This X
Axis correction signal CX, Y-axis correction signal CY and Z-axis correction signal C
Z is input to the CTM calculator 20 to form a feedback loop that corrects the error in the attitude angle.

The attitude angle generated in the CTM calculator 20 by the bias components contained in the three gyro signals Gi,
Regarding the error of the azimuth angle, the gyro bias calculation unit 2
5. Correction is performed using the gyro bias correction unit 26.

[0012]

[0013]

More specifically, when there is a difference between the initial value of the mounting shaft 10 of the sensor block unit 1 and the initial value of the CTM calculating unit 20 of the calculating unit 2, the reason why the error included in the attitude angle transmission finally disappears is explained. Describe. However, here, for simplicity, it is assumed that there is no gyro bias component Bi. The detailed operation of the acceleration error correction unit 22 is omitted.
At this time, the mounting shaft 10 rotates γ ₀ around the Z axis, then β ₀ around the Y axis, and finally α ₀ around the X axis with respect to the local coordinate system. It is assumed that the initial value of the calculation unit 20 matches the local coordinate system. The acceleration signal Ai at this time is approximately represented by the following equation.

[0015]

[Equation 1]

Here, g is the gravitational acceleration T _{O, which} is a 3 × 3 matrix, as shown in Equation 2.

[0017]

[Equation 2]

Here, α _O , β _O , and γ _O are the mounting shaft 10
It is a small angle with respect to the local coordinate system of. The initial value of the matrix signal CTMS of the CTM calculator 20 is given by the following equation.

[0019]

[Equation 3]

Horizontal error calculation unit 21 and acceleration error correction unit 2
2, the error angle ERR in the X and Y axis directions of the local coordinate system
Input matrix signal C for calculating X, ERRY
The TMS and the acceleration signal Ai are multiplied by the following equation.

[0021]

[Equation 4] Therefore, the X and Y axis error angles ERRX and ERRY are given by the following equations.

[0022]

[Equation 5] ERRX = g · α ₀ ÷ g = α ₀

## EQU00006 ## ERROR =-. Beta. ₀ Here, since the error angle is an angle unit, the gravitational acceleration g
Is divided by.

The error angle E about the Z axis of the local coordinate system
In order to calculate RRZ, the azimuth signal H output from the magnetic azimuth detector 13Y attached to the Z axis of the attachment shaft 10 and the azimuth angle signal YAW transmitted from the CTM calculator 20.
And are input to the azimuth error calculation unit 23.

Since the magnetic azimuth detector 13Y attached to the Z-axis of the attachment shaft 10 detects the azimuth around the vertical axis in the direction of gravitational acceleration, this azimuth is an azimuth signal relative to the local coordinate system. It has the same coordinate system as YAW.

In the azimuth error calculation unit 23, the azimuth signal H
And the azimuth signal YAW are calculated by the following equation to obtain the azimuth error ERRZ.

[0026]

(7) H = γ ₀

[0027]

YAW = 0 (from the initial value of the azimuth angle of the CTM calculator 20) ERRZ = H-YAW The calculated 3-axis error ERRi is input to the correction amount calculator 24.

In the correction amount calculation unit 24, the input 3
After the following equation is calculated using the axis error ERRi, the X-axis correction signal CX, the Y-axis correction signal CY, and the Z-axis correction signal CZ are output.

[0029]

[Equation 9] Here, τ is a constant having a unit of time constant.

The X-axis correction signal CX, the Y-axis correction signal CY, and the Z-axis correction signal CZ are obtained from the correction amount calculation unit 24, and CT
It is input to the M calculation unit 20. In the CTM calculation unit 20, a correction matrix CTMC is created using the input triaxial correction signals CX, CY and CZ, and the correction matrices CTMC and C are generated.
By using the TM signal CTMS and the calculation of the following equation, the attitude angle error can be corrected.

[0031]

CTMS (t + Δt) = CTMS (t) · CTMC (t, Δt) where t represents the current time, Δt represents the time update interval, and (t + Δt) represents the current time. The next time (t + Δt) from t is represented, and (t, Δt) represents the amount of change between time t and time t + Δt.

[0032]

[Equation 11]

[Mathematical formula-see original document] Since the equation 11 is described by a discrete matrix, if the equation is newly rewritten in the form of a differential equation, the error of the attitude angle is as follows.

[0034]

[Equation 12] Therefore, if you solve equation 12,

[0035]

[Equation 13] Becomes

Here, the subscript O represents an initial value. Therefore, when the CTM calculation unit 20 corrects the formula 11, the posture angle error included in the matrix signal CTMS is represented by the formula 13, and its steady value becomes zero, and the posture angle (roll Angle ROL, pitch angle PIT and azimuth YA
The component due to the error due to the difference between the initial value of the mounting axis included in W) and the initial value of the CTM calculator 20 finally disappears, and the attitude angle becomes a value with respect to the local coordinate system of the mounting axis 10. Next, a method of calculating a 3 × 3 coordinate conversion matrix for converting the coordinate system of the mounting axis 10 into the local coordinate system by using the input 3-axis gyro signal Gi in the CTM calculator 20 will be described. .

The 3-axis gyro signal Gi has a time (t, t +
It is an angle obtained by integrating the angular velocity by Δt). Expressed as a formula,

[Equation 14]

Here, ωi is the angular velocity detected by the X-axis, Y-axis and Z-axis gyros. 3-axis gyro signal Gi
Is the change amount at the interval Δt, and this change amount is calculated as the interval Δt.
The coordinate conversion matrix is obtained by accumulating each time as in the following equation. That is,

[0039]

[Mathematical formula-see original document] CTMS (t + Δt) = CTMS (t) · Γ (t, Δt)

Here, in CTMS, the 3 × 3 coordinate transformation matrix Γ (t, Δt) is an angle matrix that changes in time [t, t + Δt], and

[0041]

[Equation 16] Is.

When t = 0, CTMS (0) is an initial value, and from Equation 3,

[0043]

## EQU17 ## CTMS (0) = CTMS _O.

In practice, the CTM arithmetic unit 20 executes the arithmetic operation of Γ (t, Δt) of the mathematical expression 15 and CTM (t, Δt) of the mathematical expression 10 described above. Newly, the operation is described as follows.

[0045]

[Equation 18]

Further, the roll angle signal ROL, the pitch angle signal PIT, and the azimuth angle signal Y transmitted from the CTM calculator 20.
The method of calculating the AW is as follows. First, the coordinate axes of the coordinate conversion matrix are made to coincide with the local coordinate axes, the azimuth angle φ is rotated about the Z axis, and then the pitch angle θ is rotated about the Y axis.
The 3 × 3 coordinate conversion matrix CTMS when rotated and finally rotated by the roll angle φ is as follows.

[0047]

[Formula 19]

Therefore, the posture angle output signals from the equations 19 are given by the following equations.

[0049]

[Equation 20]

Further, the operations of the gyro bias calculating section 25 and the gyro bias correcting section 26 will be specifically described. First, the error correction model of the attitude detection device shown in FIG. 4 is as follows. However, for simplicity,
The motion acceleration component is corrected by the function of the acceleration error correction unit 22 and does not act.

[0051]

[Equation 21]

[0052]

[0053]

[Equation 22]

[Equation 23] Is.

[0054]

[0055]

[Equation 24]

[0056]

[Equation 25]

[0057]

[Equation 26] Is.

[0058]

[Equation 27] , The gyro bias component Bi
The effect of will disappear.

[0059]

The operations of the gyro bias calculation unit 25 and the gyro bias correction unit 26 will be described below.

[0061]

[0062]

The above-mentioned conventional device is
In order to prevent an attitude angle error caused by motion acceleration, an X velocity meter 11V is prepared, and an X velocity signal VX which is an output signal thereof is prepared.
A countermeasure is taken by inputting the azimuth signal YAW and the azimuth signal YAW to the acceleration error correction unit 22. However,
As shown in FIG. 7, at a certain point of time when the turning motion is performed, the traveling direction speed VXB and the actual trajectory speed VP do not match. This is because the navigation body cannot support the centrifugal force generated during turning.

The symbols in FIG. 7 will be explained (here, (+) Z.
Axial direction is downward with respect to the paper surface), O-XLYL is a local (or local) coordinate system, subscript L O-XBYB is a body (or body) coordinate system, and subscript B YAW is CTM. The azimuth angle YAW 'of the body coordinate system of the calculation unit is the azimuth angle of the actual navigation vehicle, or the trajectory angle δ is the side slip angle. As a hypothesis, centrifugal force and centripetal force act in the direction orthogonal to the trajectory speed VP. The X velocity signal VX is treated as detecting the X axis velocity VXB in the body coordinate system.

In this situation, the velocity of the machine coordinate system is
An X-axis velocity VXB and a Y-axis velocity VYB are generated, and their components are approximately represented by the trajectory velocity VP and the sideslip angle δ. That is,

VXB = VP cos δ VYB = −VP sin δ

However, since the acceleration error correction unit 22 uses only the X-axis speed signal VX that detects the X-axis speed (that is, the Y-axis speed VYB cannot be detected), the Y caused by the side slip angle δ is used. Since the acceleration component cannot be corrected by the axial velocity VYB, a posture angle error (so-called side slip angle error) is caused by it.

Therefore, according to the present invention, the lateral slip angle error does not occur only with the X-axis velocity signal, that is, the acceleration error does not occur in any acceleration motion even in the turning motion of the navigation body. In addition, the present invention provides a strap-down type posture detection device capable of outputting the component of the sideslip angle to the outside.

[0067]

As shown in FIGS. 1 and 2, for example, a strap-down type posture detecting device of the present invention has a mounting shaft 1 which coincides with a right-handed orthogonal triaxial coordinate system O-XYZ.
0, an X accelerometer 11A and an X velocimeter 11 for detecting acceleration and velocity in the X axis direction are provided on the X axis.
V and an X gyro 11G that detects an angular velocity around the X axis are attached, and a Y accelerometer 12A that detects acceleration in the Y axis direction and a Y gyro 12G that detects an angular velocity around the Y axis are attached to the Y axis. The Z axis has a Z accelerometer 13A for detecting acceleration in the Z axis direction, a Z gyro 13G for detecting angular velocity around the Z axis, and a magnetic azimuth detector 13Y for detecting an azimuth angle around the vertical axis in the direction of gravity acceleration. And a sensor block unit 1 to which is attached, X, Y, and Z accelerometers output from X, Y, and Z accelerometers 11A, 12A, and 13A of the sensor block unit 1, and its X, Y, and Z gyro 11G. , 12G, 13G output X, Y, Z
By inputting the gyro signal and the azimuth signal output from the magnetic azimuth detector 13Y, the roll angle signal, the pitch angle signal, the azimuth angle signal and the side slip angle signal, which are the attitude angles, are calculated,
It is composed of an output operation unit 2 '.

Further, in the strap-down type posture detecting device of the present invention, as shown in FIGS. 1 and 2, for example, in the above description, the computing unit 2'includes the motion acceleration included in the X and Y axis errors of the local coordinate system. The conventional attitude detection device is newly provided with a function of detecting and calculating a component and outputting the information, and a function of inputting the information and calculating so that the influence of the motion acceleration does not act on the CTM calculation. For physical exercise,
This is a posture detection device in which an error does not occur due to motion acceleration.

[0069]

According to the present invention, the side slip angle component is detected and calculated from the motion acceleration component included in the X and Y axis errors of the local coordinate system, and the information is used to determine the influence of the motion acceleration on the coordinate conversion matrix calculation. Since the calculation is performed so as not to act, the error due to the motion velocity does not occur, and the information on the sideslip angle can be output to the outside.

[0070]

DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the strapdown type posture detecting apparatus of the present invention will be described below with reference to FIGS. 1, 2 and 3. In FIGS. 1 and 2, parts corresponding to those in FIGS. 4 and 5 are designated by the same reference numerals, and description thereof will be omitted.

The example of FIG. 1 differs from the example of the prior art of FIG. 4 in that the side slip angle component δ ′ is added to the transmission signal of the arithmetic unit 2 ′. The difference is that in the calculation unit 2 ′, the sideslip angle calculation unit 30
And the sideslip angle correcting unit 40 is newly provided.

FIG. 1 is a block diagram showing an entire example of a strap-down type posture detecting device of this example. This strap-down type posture detecting device is a sensor block unit 1.
And an arithmetic unit 2 '. The sensor block unit 1 of FIG.
On the mounting axis 10 that matches the right-handed orthogonal three-axis coordinate system, X
An X accelerometer 11A that detects acceleration and velocity in the X axis direction, an X velocity meter 11V, and an X gyro 11G that detects angular velocity around the X axis are attached to the axis, and the Y axis has a Y axis direction. Y accelerometer 12A and Y for detecting acceleration
A Y gyro 12G that detects an angular velocity around the axis is attached, and a Z accelerometer 13A that detects an acceleration in the Z axis direction, a Z gyro 13G that detects an angular velocity around the Z axis, and a vertical direction in the gravitational acceleration direction are attached to the Z axis. A magnetic azimuth detector 13Y for detecting the azimuth angle around the axis is attached.

The three-axis accelerometers 11A, 12A, 13A of the sensor block unit 1 and the three-axis gyro 11 are provided.
Output signals from the G, 12G and 13G, the magnetic azimuth detector 13Y and the X-axis speedometer 11V are input to the calculation unit 2 '.
The calculation unit 2'calculates using this input signal to calculate the roll angle signal R of the mounting shaft 10 of the sensor block unit 1.
The OL, the pitch angle signal PIT, and the azimuth angle signal YAW are output as the attitude angle, and the sideslip angle component δ ′ is also output.

A more detailed operation of this example will be described with reference to FIGS. 2 and 3. FIG. 2 is a block diagram of the arithmetic unit 2'of this example. In this calculation unit 2 ', a CTM calculation unit 20, a horizontal error calculation unit 21, an acceleration error correction unit 22,
The operations of the azimuth error calculation unit 23, the correction calculation unit 24, the gyro bias calculation unit 25, and the gyro bias correction unit 26 are the same as those in FIG. 5, and thus the description thereof will be omitted, and the newly provided side slip angle calculation unit 30 and side slip angle correction unit 40 will be omitted. Will be described.

As described above, the attitude angle error due to the sideslip angle is the actual trajectory angle YAW 'and azimuth angle YA of the navigation vehicle.
It occurs because W and W are rotated by the side slip angle δ, and therefore the acceleration error correction in the acceleration error correction unit 22 is not performed correctly. Therefore, if the input signal of the azimuth angle of the acceleration error correction unit 22 uses the trajectory angle YAW ', the attitude angle error due to the side slip angle does not occur.

Description will be made with reference to FIG. The sideslip angle calculation unit 30 calculates the X-axis velocity signal VX, the azimuth angle signal YAW, and the X and Y-axis attitude angle error ERR output from the horizontal error calculation unit 21.
X ', ERRY' and the output of the CTM calculator 20 are 3
The matrix signal CTMS of 3 rows is input, the side slip angle component β ′ is calculated and output, and the acceleration calculation unit 31, the acceleration error detection unit 32, and the coordinate conversion calculation unit 33 are configured.

The acceleration calculator 31 determines the X-axis velocity signal VX.
Using the azimuth angle signal YAW, X and Y axis acceleration components AXL 'and AYL' to be corrected in the local coordinate system are calculated and output. The acceleration error detector 32 has
By deducting the X- and Y-axis posture angle errors ERRX 'and ERRY' and the X and Y-axis acceleration components AXL 'and AYL' respectively for the corresponding axes, the acceleration components that cannot be corrected on the X and Y axes, that is, the slip angle. The acceleration errors εALX and εALY due to the acceleration component are detected by and output.

The coordinate conversion calculation unit 33 uses the X and Y axis acceleration errors εAXL and εAYL and the matrix signal CTMS to calculate
Calculates and outputs the sideslip angle component. Then, the sideslip angle correcting section 40 receives the azimuth angle signal YAW of the body coordinate system and the sideslip angle component δ ', and adds them to calculate the actual azimuth angle of the navigation body, that is, the trajectory angle YAW'. ,Output. By using the trajectory angle YAW ′ as the input of the azimuth angle of the acceleration error correction unit 22, the acceleration component due to the slip angle can be removed from the error correction model (Equation 21).

Next, a more detailed description will be given using mathematical expressions.
For the sake of simplicity, the motion of the navigation body will be treated as a constant turn (both velocity and angular velocity are constant), and the sideslip angle generated at this time will also be treated as constant. X and Y included in the X and Y axis errors ERRX 'and ERRY' output from the horizontal error calculation unit 21.
The axial motion acceleration components AXL and AYL are as shown in FIG.
It becomes the following formula.

[0080]

[Equation 29]

Now, assuming that there is no error other than the motion acceleration component,

EQUATION 30 ERRX '= AXL ERRRY' = AYL.

On the other hand, the X and Y axis motion acceleration component AX calculated in the acceleration calculator 31 of the side slip calculator 30.
L'and AYL 'are given by the following equations.

[0083]

[Equation 31]

Therefore, the residual error of the acceleration component calculated by the acceleration error detecting section 32 of the side slip calculating section 30 is given by the following equation using the equations (29), (30), (31) and (28).

[0085]

[Equation 32]

[0086]

[Expression 33]

Where δ is the sideslip angle VP is the trajectory velocity VXB is the X-axis velocity of the airframe,

VXB = VX ω is the turning angular velocity YAW ′ is the trajectory angle Further, the following assumptions are made for approximate calculation.

[0088]

[Equation 35]

Next, in the coordinate transformation calculation unit 33 of the sideslip angle calculation unit 30, the actual sideslip angle component δ'is developed as in the following equation. That is,

[Equation 36] Here, CTMS uses only an azimuth signal for simplicity.

Therefore, the calculated sideslip angle component β '
Is calculated by the following equation using the Y-axis value of Equation 36, the known X-axis velocity signal VXB, and the angular velocity ω. That is,

[0091]

[Equation 37]

Since the side slip angle δ is an actual value, the calculated side slip angle is the side slip angle component δ '. Therefore, from equations 36 and 37

Δ ′ = δ Then, in the side slip angle correcting section 40, the trajectory angle YA
W'is

## EQU39 ## YAW '= YAW-.delta.'

In the acceleration error correction unit 22, the locus angle Y
Using AW 'and the X-axis velocity signal VXB, the result is as follows. That is, when the YAW in Equation 31 is replaced with YAW 'and the residual of the acceleration component is calculated,

[Formula 40]

[0094]

[Formula 41]

Therefore, by using the trajectory angle YAW ', the motion acceleration component due to the side slip angle δ can be removed,
Moreover, since the sideslip angle component δ ′ is constantly calculated, it can be output to the outside.

The present invention is not limited to the above-mentioned embodiments, and it goes without saying that various other configurations can be adopted without departing from the gist of the present invention.

[0097]

According to the present invention, it is possible to realize a posture detecting device which does not cause a posture angle error due to a motion acceleration component even in a motion in which a navigation body also causes a sideslip angle, and the accuracy thereof can be dramatically improved. .

Further, this method of calculating the side slip angle and the method of correcting the side slip angle has a merit in that it is a configuration in which only a relatively simple calculation is performed without adding or changing the equipment.

Further, according to the present invention, the side slip angle component is detected and calculated from the motion acceleration component included in the X and Y axis errors of the local coordinate system, and the influence of the motion acceleration acts on the coordinate conversion matrix calculation by using the information. Since there is no error in the motion acceleration, the side slip angle information can be output to the outside.

[Brief description of drawings]

FIG. 1 is a configuration diagram showing an embodiment of a strap-down type posture detection device of the present invention.

FIG. 2 is a configuration diagram showing an example of a calculation unit 2'of the present invention.

FIG. 3 is a diagram provided for explaining a sideslip angle calculation unit 30 and a sideslip angle correction unit 40 in FIG.

FIG. 4 is a configuration diagram showing an example of a conventional attitude detection device.

FIG. 5 is a configuration diagram showing an example of a conventional calculation unit 2.

6 is a diagram provided for explaining a gyro bias calculation unit 25 and a gyro bias correction unit 26 of FIG.

FIG. 7 is a diagram illustrating the occurrence of a sideslip angle.

[Explanation of reference numerals] 1 sensor block section 2'calculation section 11V speedometer 11A X accelerometer 11G X gyro 12A Y accelerometer 12G Y gyro 13A Z accelerometer 13G Z gyro 13Y magnetic direction detector 20 CTM calculation section 21 horizontal error calculation Part 22 Acceleration error correction part 23 Azimuth error calculation part 24 Correction amount calculation part 25 Gyro bias calculation part 26 Gyro bias correction part 30 Side slip angle calculation part 31 Acceleration calculation part 32 Acceleration error detection part 33 Coordinate conversion calculation part 40 Side slip angle correction part

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Kanushi Yamamoto 2-16-46 Minami Kamata, Ota-ku, Tokyo Within Tokimec Co., Ltd.

Claims (4)

[Claims] 1. An X-axis, an X-accelerometer, and an X-accelerometer for detecting a velocity and an acceleration in the X-axis direction, respectively, on an X-axis of a mounting axis that coincides with a right-handed orthogonal three-axis coordinate system O-XYZ.
An X gyro that detects the angular velocity around the axis is attached, and a Y accelerometer that detects the acceleration in the Y axis direction and a Y gyro that detects the angular velocity around the Y axis are attached to the Y axis, and the Z axis thereof is attached. Is a sensor block unit to which a Z accelerometer for detecting acceleration in the Z axis direction, a Z gyro for detecting angular velocity around the Z axis, and a magnetic azimuth detector for detecting azimuth angle around the axis in the direction of gravity acceleration are attached. , An X velocity signal output from the X velocity meter of the sensor block unit, an X velocity signal output from the X, Y and Z accelerometers,
The Y and Z acceleration signals, the X, Y and Z gyro signals output from the X, Y and Z gyros, and the azimuth signal output from the magnetic azimuth detector are input, and a roll angle signal and a pitch angle signal azimuth angle are input. A strapdown-type posture detection device comprising a calculation unit that calculates and outputs a signal and a sideslip angle component. 2. The strap-down type posture detection device according to claim 1, wherein the arithmetic unit detects and calculates X, Y and Z axis errors of the local coordinate system, and uses the information to detect the posture angle error. A strap-down type posture detection device characterized in that the roll angle, pitch angle and azimuth angle signals do not include errors by having a correction function. 3. The strapdown-type attitude detection device according to claim 1, wherein the computing unit includes X, Y and Z gyro bias components included in the X, Y and Z gyro signals as X in a local coordinate system. , A Y-axis and a Z-axis error are calculated, and a function of correcting them is provided so that an attitude angle error due to the above-mentioned three gyro bias components is not generated. 4. The strapdown-type posture detection device according to claim 1, wherein the calculation unit includes motion acceleration components including acceleration due to a side slip angle generated during a rotational motion in X, Y and Z of a local coordinate system. Axis error, coordinate matrix and X
It has a function of performing calculation using a speedometer and outputting it to the outside. A strapdown-type posture detection device, which has a function of correcting a motion acceleration component included in the Y and Z axes to correct an error due to motion acceleration.