JPH0518753A - Strap-down type gyro device - Google Patents

Abstract

PURPOSE:To output azimuth-attitude angle signals based on actual azimuth- attitude angle fluctuation without being influenced by the arithmetic speed limit of a strap-down arithmetic part in a strap-down type gyro device. CONSTITUTION:An interpolation arithmetic part 6 is provided on the output side of a strap-down arithmetic part 3 so as to compute predictive azimuth- attitude angle signals predicted at the output time instant. The strap-down arithmetic part 3 is of either a system using a low-pass filter 62 or a system using an alpha-beta filter 65.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a strapdown gyro device mounted on an aircraft, a ship, a vehicle (hereinafter referred to as a navigation body), and outputting azimuth and attitude angle signals of the navigation body.

[0002]

2. Description of the Related Art FIG. 4 shows a block diagram of a conventional strapdown gyro device. In FIG. 4, 1 is an accelerometer that detects independent 3-axis accelerations, and 2 is a gyro that detects independent 3-axis angular velocities. Here, the angular velocity output of a normal rate gyro is integrated to output an angle signal. To be done. The accelerometer 1 and the gyro 2 are directly attached to the body of the navigation body.

Acceleration signals and angle signals from the accelerometer 1 and the gyro 2 are input to the strapdown calculator 3,
Then, the converted acceleration signal, the azimuth signal and the attitude angle signal of the machine body are calculated. A detailed configuration diagram of the strapdown calculation unit 3 is shown in FIG.

The angle signal from the gyro 2 is input to the input terminal 15
Is input to the gyro correction unit 33, and the gyro correction unit 33 corrects the gyro scale factor, bias error, input axis error, and the like, and the corrected angle signal is converted into a coordinate conversion matrix (CTM) calculation unit 34. Sent to.

In the CTM calculator 34, the rotation of the airframe with respect to the inertial space is calculated based on the corrected angle signal,
At the same time, the rotation component due to the rotation of the earth, the motion of the navigation body, etc. is corrected using the control signal (torking signal) from the control calculation unit 4 described later, and the rotation angle of the machine body coordinate system with respect to the local coordinate system is calculated. . Such calculations are usually done in a special form called a quaternion.

The quaternion from the CTM calculator 34 is converted by the posture angle calculator 35 into the roll and pitch angles which are the azimuth and posture angles normally used.

The acceleration signal from the accelerometer 1 is input to the accelerometer correction unit 31 via the input terminal 14, and the accelerometer correction unit 31 corrects the accelerometer scale factor, bias error, input axis error and the like. Then, the corrected acceleration signal is sent to the coordinate conversion unit 32.

The coordinate conversion section 32 converts the corrected acceleration signal into an acceleration signal for the earth coordinate system using the coordinate information calculated by the CTM calculation section 34.

Returning to FIG. 4 again, the converted acceleration signal, azimuth angle signal and attitude angle signal sent from the strapdown calculation unit 3 are input to the control calculation unit 4 and the output conversion unit 5, respectively.

The control calculation unit 4 performs a control calculation of compass or inertia based on the acceleration signal from the strapdown calculation unit 3, and a torqueing signal as a control signal is fed back to the strapdown calculation unit 3.

The output conversion unit 5 converts the azimuth angle signal and the attitude angle signal from the strapdown calculation unit 3 into a signal and outputs the signal to the outside.

The strapdown arithmetic unit 3 and the control arithmetic unit 4 perform input / output by signals from a timing control circuit (not shown). The strapdown calculation unit 3 takes in the outputs of the accelerometer 2 and the gyro 1 at the cycle of the first clock 7 (T1) and performs the strapdown calculation. In the control calculation unit 4, the cycle of the second clock 8 (T2, which is longer than the first clock 7).
In the above), the acceleration signal from the strapdown calculation unit 3 is fetched and the calculation is performed. Usually, the period of this first clock 7 is on the order of a few hundred Hz and the second clock 8
The period is from several Hz to several tens Hz.

[0013]

However, in the conventional strapdown type gyro device, it is difficult to shorten the cycle of the first clock 7 because the strapdown arithmetic unit has a limited arithmetic capacity. . Therefore, there is a problem that the output of the azimuth angle signal and the attitude angle signal is delayed by the period of the first clock 7.

The problem will be described with reference to FIG. 6 by taking the attitude angle as an example. In FIG. 6, the actual attitude angle of the navigation body is represented by a solid line, and the output from the strapdown calculation unit 3 is represented by a dotted line. If the current time is T0, the gyro 2 integrates the input angular velocity from time T0-T1 to time T0 and outputs an angle signal. Then, using this angle signal, the strapdown calculation unit 3 outputs a posture angle signal at time T0 + TS delayed by the time TS required for the strapdown calculation. Then, this value is held for the sampling time T1 of the strapdown calculation unit 3.
Therefore, even when the actual angle continuously increases as shown by the solid line, only the stepwise output having the delay shown by the dotted line is obtained.

Further, when a servo control circuit such as an autopilot or a stabilizer is built by using the azimuth / attitude angle, there is a problem that optimum control cannot be performed due to the delay of the azimuth angle signal and the attitude angle signal.

In view of the above problems, it is an object of the present invention to provide a strap-down type gyro device that can output an azimuth angle signal and an attitude angle signal without delay and continuously as much as possible. .

[0017]

In order to achieve such an object, according to the present invention, an accelerometer 1 for detecting independent triaxial accelerations, a gyro 2 for detecting independent triaxial angular velocities,
A strapdown calculator 3 that outputs an acceleration signal, an azimuth signal, and an attitude angle signal based on an input signal from the accelerometer 1 and an input signal from the gyro 2, and a strapdown calculator 3 that outputs a control signal based on the acceleration signal. In the strap-down type gyro device having the control calculation unit 4 supplied to

On the output side of the strapdown calculation unit 3, an interpolation calculation unit 6 for outputting an azimuth angle signal and a posture angle signal indicating a predicted azimuth and a posture angle predicted at the output time is provided.

Further, the interpolation calculation section 6 differentiates the azimuth angle signal and the attitude angle signal and outputs a rate signal, and a low pass filter 62, and outputs from the rate signal, the azimuth angle signal and the attitude angle signal. It may be configured to include an interpolation calculator 63 that outputs an azimuth signal and an attitude angle signal indicating a predicted azimuth angle and an attitude angle predicted at time.

Further, the interpolation calculating section 6 outputs an αβ filter 65 which outputs a smoothed rate signal and a smoothed azimuth angle signal and an attitude angle signal from the azimuth angle signal and the attitude angle signal, and the smoothed αβ filter 65. And the interpolation calculator 66 that outputs the azimuth angle signal and the attitude angle signal indicating the predicted azimuth angle and the attitude angle predicted at the output time from the rate signal and the smoothed azimuth angle signal and the attitude angle signal. .

Further, the interpolation calculation section 6 may be configured to output the azimuth signal and the attitude angle signal indicating the predicted azimuth angle and the attitude angle in a cycle shorter than the output cycle from the strapdown calculation section 3.

[0022]

According to the present invention, in the interpolation calculation unit 6, the predicted azimuth angle and predicted posture angle predicted at the output time are calculated based on the azimuth angle signal and the posture angle signal output from the strapdown calculation unit 3. The predicted azimuth signal and the predicted attitude angle signal shown are calculated.

The output signal from the interpolation calculation unit 6 is not affected by the limit of the calculation speed of the strapdown calculation unit 3, and the azimuth signal and the posture angle signal follow the fluctuation of the actual azimuth angle and the posture angle. Becomes

According to one configuration example of the interpolation calculation unit 6, the differentiator 61 and the low pass filter 62 calculate the rate signal ω _S based on the azimuth angle signal and the attitude angle signal from the strapdown calculation unit 3, The predicted azimuth signal and the predicted attitude angle signal are calculated based on the rate signal ω _S.

According to another configuration example of the interpolation calculation unit 6, the rate signal smoothed by the αβ filter 65 and the smoothed direction are calculated based on the azimuth angle signal and the attitude angle signal from the strapdown calculation unit 3. The angle signal and the attitude angle signal are calculated, and the predicted azimuth angle signal and the predicted attitude angle signal are calculated based on the smoothed signal.

In the interpolation calculation unit 6, by providing the interpolation calculation unit 63 that calculates the predicted azimuth angle signal and the predicted attitude angle signal in a cycle shorter than the output cycle of the strapdown calculation unit 3, the actual azimuth angle and There is no delay with respect to the posture angle, and a signal that is closer to the actual value is output as continuously as possible.

[0027]

1 is a block diagram of a strap-down type gyro device according to the present invention. Here, the same components as those of FIG. 4 are designated by the same reference numerals, and the description thereof will be omitted.

According to the present invention, the interpolation calculation unit 6 is newly provided between the strapdown calculation unit 3 and the output conversion unit 5. A detailed explanatory diagram of the first embodiment of the interpolation calculation unit 6 is shown in FIG.

In FIG. 2, 61 is a differentiator, 62 is a low-pass filter, and 63 is an interpolation calculator.

Time T0 from the strapdown calculator 3
The azimuth / orientation angle signal θ _m (T0) (in which azimuth angle, roll angle, pitch angle, etc. are generically represented by θ _m ) is input to the differentiator 61 with a delay of the strapdown calculation time TS and the rate signal thereof is obtained. ω _a (T0) is calculated. That is,
The output ω _a (T0) of the differentiator 61 is calculated by the following equation from the azimuth / attitude angle signal θ _m (T0-T1) at the previous time T0-T1.

## EQU1 ## ω _a (T0) = {θ _m (T0) -θ _m (T0-T1)} / T1.

The azimuth / attitude angle signal output from the strapdown computing unit 3 often contains random components due to irregular motions of the body, vibrations, quantization of gyro output, and the like. Therefore, the rate signal obtained by the differentiator 61 is smoothed by the low pass filter 62. Rate signal ω _S (T0) smoothed by the low-pass filter 62
Is

[Expression 2] ω _S (T0) = ω _S (T0-T1) + {ω _a (T0) −ω _S (T0-T1)} × T1 / Tf Here, Tf is a filter time constant of the low pass filter 62.

The output θ _O (T0 + T) of the interpolation calculator 63 after the lapse of time T from the time T0 is obtained by the following equation.

(3) θ _O (T0 + T) = θ _m (T0) + ω _S (T0) · T

This θ _O (T0 + T) becomes a predicted azimuth / attitude angle signal at time T0 + T.

The differentiator 61 and the low-pass filter 62 perform an operation in the same cycle T1 as the strapdown operation unit 3 and output the operation result. On the other hand, the interpolation calculator 63 performs calculation in a cycle T3 shorter than the cycle T1 and outputs the result of the equation 3 in each cycle.

FIG. 7 shows a part of the output result. The value indicated by the black circle is the output of the interpolation calculator 63. In this embodiment, the rate signal (indicated by a black arrow) at the time T0 obtained by the differentiator 61 is obtained until the time T0 + T1 and is used to output the predicted interpolation value from the time T0 + T1 to the time T0 + 2T1. Similarly, the strapdown calculation unit 3
Of the rate signal at time T0 + T1 of time T0 + 2T
Up to 1 and using this, from time T0 + 2T1 to time T0
The predicted interpolation value up to + 3T1 is output.

By operating in this manner, the time T0 is not affected by the delay in the operation time of the strapdown operation unit 3.
It is possible to output an approximate value of the actual azimuth / attitude angle at + T1 at the same time. Further, since the interpolation calculator 63 itself can be operated in a shorter cycle than the strapdown calculator 3 due to the difference in the number of calculation steps, if T is the time of the cycle T3 in the equation (3), the interpolation calculator 63 is more continuous. Can be output.

Next, a second embodiment of the interpolation calculation unit 6 is shown in FIG. In FIG. 3, reference numeral 65 is an αβ filter, and 66 is an interpolation calculator.

Time T0 from the strapdown calculator 3
The azimuth / attitude angle signal θ _m (T0) at is input to the αβ filter 65 after a strapdown calculation time TS, and the smoothed azimuth / attitude angle signal θ _S (T0) and rate signal ω _S (T0) are input. It is calculated.

That is, in the αβ filter 65, the azimuth / attitude angle signal θ calculated in the sampling time one time before is calculated.
_{Using S} (T0-T1) and rate signal ω _S (T0-T1), the predicted azimuth / attitude angle signal θ at time T0 is calculated by the following equation.
_P (T0) and the predicted rate signal ω _P (T0) are calculated.

[Number 4] _{θ P (T0) = θ S} (T0-T1) + ω S (T0-T1) · T1

(5) ω _P (T0) = ω _S (T0-T1)

Such predicted values θ _P (T0) and ω _P (T
0) and the time T supplied from the strapdown calculation unit 3
Using the azimuth / attitude angle signal θ _m (T0) at ₀ , the smoothed azimuth / attitude angle signal θ _S (T0) and rate signal ω _S (T0) at the time T ₀ are calculated by the following equations. Is calculated.

[Equation 6] θ _S (T0) = θ _P (T0) + α {θ _m (T0) −θ _P (T0)}

Ω _S (T0) = ω _P (T0) + β {θ _m (T0) −θ _P (T0)} / T1

Here, α is a smoothing constant of the azimuth / attitude angle signal, and β is a smoothing constant of the rate signal.

Azimuth / attitude angle signal θ _S calculated in this way
(T0) and the rate signal ω _S (T0) are output from the αβ filter 65 and supplied to the interpolation calculator 66.

In the interpolation calculator 66, the smoothed azimuth
Using the attitude angle signal θ _S (T0) and the rate signal ω _S (T0), the time T after the time T has elapsed from the time T ₀ is calculated by the following equation.
Predicted azimuth / attitude angle signal at ₀ + T θ ₀ (T0 + T)
Is calculated.

## EQU8 ## θ ₀ (T0 + T) = θ _S (T0) + ω _S (T0) · T

With this operation, similar to the first embodiment, the approximate value of the actual azimuth / orientation angle can be output at the same time without being affected by the delay in the calculation time of the strapdown calculation unit 3. .

By obtaining the smoothed rate signal and the azimuth / attitude angle signal using the αβ filter, a more accurate predicted value of the azimuth / attitude angle can be obtained.

FIG. 8 shows the difference in output results between the case of the second embodiment using the αβ filter and the case of the first embodiment using the low pass filter. In the figure, white circles indicate the actual azimuth / orientation angle, black triangles indicate the output value from the interpolation calculation unit 63 when the low-pass filter is used, and black squares indicate the interpolation calculation when the αβ filter is used. The output value from the device 66 is shown. The arrow a indicates the rate signal from the low pass filter, and the arrow b indicates the rate signal from the αβ filter. Since the azimuth / posture angle signal itself is also smoothed by the αβ filter, it follows an average angle change 59, and thus changes in the entire posture angle compared to the first embodiment using the actual angle change. It is possible to obtain a suitable predicted value.

[0047]

According to the present invention, the interpolation calculation unit is provided on the output side of the strapdown calculation unit 3, so that the azimuth signal and the azimuth signal which are more suitable for the fluctuation of the actual value can be obtained without delaying the actual value. The attitude angle signal is obtained. Further, according to the present invention, in the interpolation calculation unit 6, the strapdown calculation unit 3
Since the interpolation calculation is performed in a cycle shorter than the output cycle from, the azimuth angle signal and the attitude angle signal are obtained as continuously as possible.

[Brief description of drawings]

FIG. 1 is a block diagram showing the configuration of a strapdown gyro device according to the present invention.

FIG. 2 is a block diagram showing an embodiment of an interpolation calculation unit included in the strapdown gyro device according to the present invention.

FIG. 3 is a block diagram showing another embodiment of the interpolation calculation unit included in the strapdown gyro device according to the present invention.

FIG. 4 is a block diagram showing a configuration of a strapdown gyro device according to a conventional technique.

FIG. 5 is a block diagram showing a configuration of a strapdown calculation unit.

FIG. 6 is a graph showing an output from a strapdown calculation unit.

FIG. 7 is a graph showing an output from an interpolation calculation unit according to the present invention.

FIG. 8 is a graph showing an output from an interpolation calculation unit when a low-pass filter is used and when an αβ filter is used.

[Explanation of symbols]

1 accelerometer 2 gyro 3 Strap-down calculator 4 Control calculation section 5 Output converter 6 Interpolation calculation section 7,8 clock 11 Azimuth / Attitude angle signal output terminal 12 Azimuth / Attitude angle signal input terminal 13 Azimuth / Attitude angle signal output terminal 14 Acceleration signal input terminal 15 Angle signal input terminal 16 Acceleration signal output terminal 17 Azimuth / Attitude angle signal output terminal 18 Control signal input terminal 31 Accelerometer correction section 32 coordinate converter 33 Gyro correction unit 34 CTM operation unit 35 Posture angle calculator 50 Actual angle curve 51 Output curve of the strapdown calculator 59 Center line of angle change 61 differentiator 62 low pass filter 63 Interpolation calculator 65 αβ filter 66 Interpolation calculator

   ─────────────────────────────────────────────────── ─── Continued front page    (72) Inventor Toshihiko Nakatani             2-16-46 Minami Kamata, Ota-ku, Tokyo Stocks             Company Tokimetsu

Claims (4)

[Claims] 1. An acceleration signal based on an accelerometer for detecting independent triaxial acceleration, a gyro for detecting independent triaxial angular velocity, an input signal from the accelerometer and an input signal from the gyro. And a strapdown gyro having a strapdown calculator for outputting an azimuth angle signal and an attitude angle signal, and a control calculator for supplying a control signal to the strapdown calculator based on an acceleration signal from the strapdown calculator. In the device, a strapdown type characterized in that an interpolation calculation unit for outputting an azimuth angle signal and a posture angle signal indicating a predicted azimuth angle and a predicted posture angle predicted at an output time is provided on the output side of the strapdown calculation unit. Gyro device. 2. The differentiating unit and the low-pass filter for differentiating the azimuth angle signal and the attitude angle signal to output a rate signal, the interpolation calculation unit, the output time from the rate signal, the azimuth angle signal and the attitude angle signal. The strap-down gyro device according to claim 1, further comprising: an interpolation calculator that outputs an azimuth signal and an attitude angle signal indicating the predicted azimuth angle and the predicted attitude angle. 3. The interpolation calculator outputs an αβ filter for outputting a rate signal smoothed from the azimuth angle signal and the attitude angle signal and a smoothed azimuth angle signal and the attitude angle signal,
An interpolation calculator which outputs an azimuth angle signal and an attitude angle signal indicating a predicted azimuth angle and a predicted attitude angle predicted at an output time from the smoothed rate signal and the smoothed azimuth angle signal and attitude angle signal The strap-down gyro device according to claim 1, further comprising: 4. The interpolation calculation section outputs the azimuth signal and the attitude angle signal indicating the predicted azimuth angle and the predicted attitude angle signal in a cycle shorter than the output cycle from the strapdown calculation section. Item 4. A strapdown gyro device according to items 1 to 3.