JPH07234126A - Attitude angle detecting method for angular velocity sensor type attitude angle detector - Google Patents

Abstract

PURPOSE:To enhance a detecting accuracy of an angular velocity sensor while preventing an increase in cost. CONSTITUTION:Before an attitude angle detector having angular velocity sensors 12, 14, 16 mounted at X-, Y-and Z-axes is placed at a body 1, an erroneous angular velocity is detected, and stored in a non-volatile memory 24. The erroneous velocity is obtained from an output vale of angular velocity sensors of two shafts perpendicularly crossed at a detecting shaft by rotating it around the detection shaft by a rotary table. After the angle detector is placed at the body, the erroneous value is subtracted from the output value of the sensors of the two shafts when it is rotated around the detection shaft. In order to correct the error of the angular velocity generated from the fact that axes of the three angular velocity sensors are not perpendicularly crossed, it can be electrically executed, and its coat can be reduced by enhancing a mechanical accuracy.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an attitude angle detecting device for detecting an attitude angle and a traveling direction of a radio-controlled unmanned helicopter, a ship traveling on water or underwater, a shield excavator, and the like. It relates to a detection method.

[0002]

2. Description of the Related Art Conventionally, for example, a radio-controlled unmanned helicopter for spraying agricultural chemicals has a structure in which the azimuth, inclination angle, etc. of the machine body are controlled by remote control in response to a pilot command signal from the operator. Then, in this type of unmanned helicopter, the actual azimuth, tilt angle, etc. of the airframe are subject to disturbances such as winds with respect to the azimuth, tilt angle, etc. determined by the pilot command signal so that the maneuver can be performed easily It has been desired to provide an attitude control device that automatically corrects the attitude of the airframe even if it changes.

It is conceivable that this attitude control device is configured to determine how many times the machine body is rotating with respect to its left, right, front and rear, and up and down axes by using an angular velocity sensor for each axis.

[0004]

However, according to the experiments conducted by the inventors, even if a highly accurate angular velocity sensor is used, the attitude angle of the machine body cannot be detected with high accuracy. This was due to the low positional accuracy when the angular velocity sensor was attached to the machine body.

That is, the axis lines in the left-right, front-rear, and up-down directions of the machine body do not match the axis lines of the angular velocity sensors arranged on these axes, and the detection axis lines of the three angular velocity sensors are not orthogonal to each other. there were. Therefore, for example, when the aircraft is rotated only around the vertical axis, the angular velocity sensor on the horizontal axis and the angular velocity sensor on the longitudinal axis, which should not normally detect the angular velocity, are caused by the positional deviation. Will detect the angular velocity.

Such a problem can be solved by increasing the accuracy of the mounting position of each angular velocity sensor, but the angular velocity sensor mounting portion must be machined with strict accuracy to perform more precise position adjustment. The cost will be high.

In addition, the angular velocity sensor is usually assembled by housing the sensor main body in the case, and the detection axis of the sensor main body is displaced from the reference axis of the case, as described above. Even if the case is assembled to the machine body with high accuracy as described above, the axis will eventually be displaced. If the accuracy of positioning the sensor body with respect to the case is also increased, the cost will be further increased.

The present invention has been made in order to solve such a problem, and detects an attitude angle with high accuracy by using an angular velocity sensor without adopting a cost-increasing method of increasing the mounting position accuracy. The purpose is to be able to.

[0009]

According to a first aspect of the present invention, there is provided an attitude angle detecting method for an angular velocity sensor type attitude angle detecting apparatus, comprising: a vertical axis angular velocity sensor for detecting an angular velocity about a vertical axis; An attitude angle detection device equipped with an X-axis angular velocity sensor and a Y-axis angular velocity sensor for detecting angular velocities around the X and Y directions orthogonal to the vertical direction is rotated about the vertical axis. The axis of the vertical axis angular velocity sensor is placed on the rotary table in the vertical direction and rotated, and the output values of the X-axis angular velocity sensor and the Y-axis angular velocity sensor when the rotary table is rotating are set vertically. An error value around the axis is stored in a non-volatile memory, and after the attitude angle detection device is mounted on a moving object, when the attitude angle detection apparatus rotates about the vertical axis, the angular velocity sensor for the X axis and the Y axis Obtains a true angular velocity from the output value of the speed sensor by subtracting each said error value, in which determining the angle based on the true angular velocity.

The attitude angle detecting method of the angular velocity sensor type attitude angle detecting apparatus according to the second invention is the same as the attitude angle detecting method of the angular velocity sensor type attitude angle detecting apparatus according to the first invention. Before mounting on the, the axis of the X-axis angular velocity sensor is turned vertically by the rotary table, and the output values of the vertical-axis angular velocity sensor and the Y-axis angular velocity sensor are measured when the rotary table is rotating. It is stored in the non-volatile memory as an error value around the X axis,
After the attitude angle detecting device is mounted on a moving object, the true value is obtained by subtracting the error value around the X axis from the output values of the vertical axis angular velocity sensor and the Y axis angular velocity sensor when the device rotates around the X axis. The angular velocity is obtained, and the angle is obtained based on this true angular velocity.

An attitude angle detecting method of an angular velocity sensor type attitude angle detecting device according to a third aspect of the invention is the same as the attitude angle detecting method of the angular velocity sensor type attitude angle detecting device of the second aspect of the invention.
Before the attitude angle detection device is mounted on a moving object, the axis of the Y-axis angular velocity sensor is turned vertically by a rotary table, and the vertical-axis angular velocity sensor and X-axis are used when the rotary table is rotating. The output values of the angular velocity sensor are stored in a non-volatile memory as error values around the Y-axis, and after the attitude angle detection device is mounted on a moving object, the angular velocity sensor for vertical axis and X
The true angular velocity is obtained by subtracting the error value around the Y axis from the output value of the axial angular velocity sensor, and the angle is obtained based on this true angular velocity.

[0012]

In the present invention, the angular velocity error caused by the fact that the axes of the three angular velocity sensors are not orthogonal to each other is electrically corrected.

[0013]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described in detail below with reference to FIGS. In the present embodiment, an example of detecting the attitude angle of the radio-controlled unmanned helicopter will be described. FIG. 1 is a schematic configuration diagram of an unmanned helicopter that detects a posture angle by the posture angle detection method according to the present invention, and FIG. 2 shows an overall configuration of a posture angle detection device that detects a posture angle by the posture angle detection method according to the present invention. 3 is a block diagram, FIG. 3 is a block diagram showing a configuration of a main part of the attitude angle detecting device, and FIG. 4 is a perspective view showing a schematic configuration of the attitude angle detecting device.

FIG. 5 is a block diagram for explaining the error value detecting step on the rotary table, FIG. 6 is a graph for explaining the error value detecting procedure on the rotary table, and FIG. 7 is an attitude angle detecting device. It is a figure for demonstrating the angular velocity detection procedure after mounting in an unmanned helicopter.

In these figures, 1 is a body of an unmanned helicopter as a moving object, 2 is a main rotor, 3 is a tail rotor, 4 is the main rotor 2 and a tail rotor 3.
Is an engine that drives the rotation of. 5 is an engine controller servomotor for controlling the rotation speed of the engine 4,
Reference numeral 6 is a collective servo motor for controlling the tilt angle and pitch angle of the main rotor 2, 7 is a ladder servo motor for controlling the pitch angle of the tail rotor 3, and these servo motors 5 to 7 are attitude angle detection according to the present invention. It has a structure controlled by a controller 8 as a device.

Reference numeral 9 denotes a receiver mounted on the machine body 1. The receiver 9 receives the pilot command signal transmitted from the transmitter 10 by the receiver 9a and outputs it to the controller 8.
The control signal from the controller 8 is sent to the servomotor 5 to
7 has an amplifier 9b built in. The machine body 1 is equipped with a pesticide spraying device (not shown) for spraying pesticides from the air.

The controller 8 detects the angles of the main body 1 with respect to three mutually orthogonal main azimuths (horizontal, front-back and up-down directions), the altitude of the airframe, the acceleration in the vertical direction, etc. by using various sensors described later, The structure is such that the target flight state set by the pilot command signal sent from the transmitter 10 is controlled. here,
As the sensor, the axis line in the left-right direction of the machine body 1 (X axis)
Acceleration sensor 11 as an inclinometer for detecting the angle of rotation
And the angular velocity sensor 12, the acceleration sensor 13 and the angular velocity sensor 14 as an inclinometer for detecting the angle around the longitudinal axis (Y axis) of the machine body 1, and the angle around the vertical axis line (Z axis) of the machine body 1. A geomagnetic direction sensor 15 and an angular velocity sensor 16 for detecting the vertical direction, and a vertical direction acceleration sensor 17 for detecting the acceleration of the body 1 in the vertical direction.
And an altitude sensor 18 for detecting the altitude of the aircraft 1,
It is an engine speed detection sensor 19 (FIG. 2) that detects the speed of the engine 4.

Of these sensors, the X-axis acceleration sensor 11 detects from the acceleration in the Y-axis direction how many times the Y-axis of the machine body 1 is inclined with respect to the vertical direction, and the X-axis angular velocity sensor 12 is It is configured to detect an angular velocity when the machine body 1 rotates about the X axis. Further, the Y-axis acceleration sensor 13 detects how many times the X-axis of the machine body 1 is inclined with respect to the vertical direction from the acceleration in the X-axis direction, and the Y-axis angular velocity sensor 14 detects the machine body 1 around the Y-axis. It is configured to detect the angular velocity when rotating.

Further, the geomagnetic direction sensor 15 detects, for example, how many times the Y axis of the airframe is rotating with respect to the north direction,
The Z-axis angular velocity sensor 16 is configured to detect the angular velocity when the machine body 1 rotates about the Z-axis. In addition, the vertical acceleration sensor 17 is configured to detect acceleration in the Z-axis direction of the aircraft 1 in the same direction, and the altitude sensor 18 optically detects the distance between the aircraft 1 and the ground surface. Is configured to. Furthermore, the engine speed detection sensor 19 is configured to detect the rotation of a crankshaft (not shown) of the engine 4.

The angular velocity sensor 12 for each axis is
14, 16, the acceleration sensors 11, 13, 17 and the geomagnetic direction sensor 15 are attached to the base block 20 shown in FIG. 4 in this embodiment, and are supported and fixed to the machine body 1 via the base block 20. Note that FIG.
In FIG. 1, the geomagnetic direction sensor 15 is not shown, but is fixed to the base block 20 with the Z axis as the detection axis.

The base block 20 includes a bottom plate 20a formed parallel to the XY plane, a rear plate 20b formed parallel to the XZ plane, and a side plate 20c formed parallel to the YZ plane. Are integrally provided, and the bottom plate 20a has angular velocity sensors 12, 14, 16 and a vertical acceleration sensor 1
7 is fixed, and the acceleration sensor 1 in the X-axis direction is attached to the rear plate 20b.
1 is fixed, and the acceleration sensor 1 in the Y-axis direction is attached to the side plate 20c.
3 is fixed. In addition, this base block 20
Is attached to the machine body 1 such that the ridge lines of the corners formed by joining adjacent ones of the bottom plate 20a, the rear plate 20b, and the side plates 20c coincide with the X axis, Y axis, and Z axis of the machine body 1. . Then, each angular velocity sensor 12, 14,
Reference numeral 16 is attached so that the detection axes are parallel to the X axis, the Y axis and the Z axis. An optical fiber gyro is adopted as the angular velocity sensors 12, 14, 16 in this embodiment.

As shown in FIG. 2, the controller 8 calculates the actual posture angle of the machine body 1 from the output values of the various sensors, and the machine body 1 obtained by the posture angle calculation device 21. Of the servo motor 5 so that the actual attitude angle of the vehicle approaches a target attitude angle described later, and the flight direction, engine speed, and altitude approach the target values.
It is composed of a flight state control CPU 22 and the like for controlling the to. The CPU 22 for controlling the flight state is the transmitter 1
Target value calculation processing means for calculating target values such as a target attitude angle, a target flight direction and a target flight speed based on a pilot command signal sent from 0, and each of the servo motors 5 to 7 for controlling the servo motors 5 to 7 as described above. An actuator control amount calculation processing means is provided.

As shown in FIG. 3, the attitude angle computing device 21 comprises a CPU 23 for performing various computations and an interface for connecting the sensors and the flight state control CPU 22 to the CPU 23.

The CPU 23 uses the angular velocity sensors 12, 1
Non-volatile memory 24 and the angular velocity actually detected by 4, 16
The corrected angular velocity calculation processing means 25 for obtaining the corrected angular velocity as the true angular velocity using the error angular velocity stored in the acceleration angular velocity calculation processing means 25 and the corrected angular velocity calculated by the corrected angular velocity calculation processing means 25. 15
Attitude angle calculation processing means 26 is added to the angle detected by to obtain the current attitude angle.

The non-volatile memory 24 includes the angular velocity sensors 12, which are set in advance by an error detection process described later.
The error angular velocities of 14 and 16 are stored. If the angular velocity sensor 12 detects the angular velocity around the X-axis, the error angular velocity means that the angular velocity sensor 12 is rotated together with the base block 20 around the Z-axis while being attached to the base block 20 shown in FIG. Angular velocity KXZ detected when rotating and the angular velocity KXY detected when rotating around the Y-axis
That is. Originally, the angular velocity sensor 12 around the X-axis should not detect the angular velocity even if it is rotated around the Z-axis or the Y-axis, but the angular velocity sensor 12 has the axis of the sensor body 12a in the sensor case 12b. The above-described error angular velocities KXZ and KXY are generated due to the fact that the sensor case 12b is inclined with respect to the reference line and the reference line of the sensor case 12b is inclined with respect to the X axis of the base block 20.

Similarly to this, the angular velocity sensor 1 around the Y-axis
Since the angular velocity sensor 16 around the 4th axis and the Z-axis also causes an error, the error is also stored in the nonvolatile memory 24.
The error angular velocity output by the angular velocity sensor 14 around the Y axis is
Error angular velocity KYZ when it is rotated around the Z axis, and X
It is the error angular velocity KYX when it is rotated around the axis. Also,
The error angular velocity output by the angular velocity sensor 16 around the Z axis is
Error angular velocity KZX when it is rotated around the X axis and Y
It is the error angular velocity KZY when rotated about the axis.

The above-mentioned error angular velocity is set as a value per unit angular velocity when it rotates around the axis where an error occurs in the error detection step described later. For example, the error angular velocity KXZ when the angular velocity sensor 12 around the X axis is rotated around the Z axis is obtained by dividing by the angular velocity detected by the angular velocity sensor 16 around the Z axis at that time. That is, the greater the angular velocity around the Z axis, the greater the error angular velocity. Therefore, the error angular velocity KXZ
Is multiplied by the output value of the angular velocity sensor 16 around the Z axis. This multiplication is performed by the corrected angular velocity calculation processing means 25 described later.

The correction angular velocity calculation processing means 25 subtracts the difference between the actual angular velocity ωX, ωY, ωZ detected by the respective angular velocity sensors 12, 14, 16 and the error angular velocity multiplied by the angular velocity around the error generating axis to obtain a true value. Is configured to determine the angular velocity of. More specifically, for example, when the Z-axis rotation angular velocity sensor 16 detects the angular velocity, X
The error angular velocities KXZ and KYZ around the Z-axis are calculated from the output values of the axial-rotational angular velocity sensor 12 and the Y-axis rotational angular velocity sensor 14, respectively.
Subtract what is multiplied by the output ωZ of the angular velocity sensor 16 around the Z axis. With this, the angular velocity sensor 1 around the X-axis
2. The true value of the angular velocity output by the angular velocity sensor 14 around the Y axis is obtained. The true angular velocity is output to the posture angle calculation processing means 26.

Similarly, the correction angular velocity calculation processing means 2
When the angular velocity sensor 12 around the X axis detects the angular velocity, the posture angle calculation processing means 26 indicates the true value of the angular velocity detected by the angular velocity sensor 14 around the Y axis and the angular velocity sensor 16 around the Z axis. Output to. Further, when the angular velocity sensor 14 about the Y axis is detecting the angular velocity, the posture angle calculation processing means 26 is provided with the true value of the angular velocity detected by the angular velocity sensor 12 about the X axis and the angular velocity sensor 16 about the Z axis. Output.

That is, the current true angular velocities of the machine body 1 around the respective axes are output to the respective attitude calculation processing means 26 as ω'X, ω'Y, ω'Z.

Each of the attitude calculation processing means 26 detects the initial value of the tilt angle and the azimuth of the body 1 when the body is stationary before takeoff and stores the values in a memory (not shown), and after the takeoff, the corrected angular velocity calculation is performed. True angular velocity ω obtained by the processing means 15
A true angle obtained by integrating X ', ωY', and ωZ 'is added to the initial value to obtain the current posture angle. Accelerometers 11, 1 are used to determine the tilt angle before takeoff.
3 is used, and the azimuth uses the output value of the geomagnetic azimuth sensor 15. Further, in order to detect that the airframe 1 is stationary, the angular velocity sensors 12, 14, 16
Is detected by continuously outputting a value smaller than a predetermined value during a certain time.

That is, the current attitude angle obtained by the attitude angle calculation processing means 26 is input to the flight state control CPU 22, and the flight state control CPU 22 determines the flight direction, altitude, and engine rotation based on the attitude angle. Will control the number.

Next, a method for obtaining the error value will be described. First, as shown in FIG. 4, the base block 20 to which the angular velocity sensors 12, 14, 16 are attached is placed and fixed on the rotary table 27. This turntable 27
Has a structure that rotates around a vertical axis at a predetermined rotation speed. In mounting the base block 20 on the rotary table 27, first, the axis Z of the base block 20 is oriented in the vertical direction. That is, the base block 20 in the state shown in FIG.
Is fixed to the rotary table 27. In the present embodiment, the base block 27 has a C of the controller 8 as shown in FIG.
The PU 23, the non-volatile memory 24 and the like are attached together with the angular velocity sensor. Then, each angular velocity sensor 12, 1
4, 16 are connected to the CPU 23 via the A / D converter 28. That is, in this embodiment, the base block 20
Also constitutes a part of the attitude angle detection device.

Next, after mounting the base block 20 on the rotary table 27 in this manner, as shown in FIG. 6, the angular velocity is detected by each angular velocity sensor for 10 seconds while the rotary table 27 is stationary. And this 10
The average value of the angular velocities detected in each second is stored in a memory (not shown) of the CPU 23 as the initial angular velocities ωX0, ωY0, ωZ0 of each angular velocity sensor. This is performed based on the error detection program stored in the CPU 23 in advance. That is, the CPU 23, in addition to the program for calculating the attitude angle,
A program for detecting an error of the angular velocity sensor in the error detecting step and storing it in the nonvolatile memory 24 as described later is also stored.

After that, the rotary table 27 is rotated at a predetermined rotational speed, and the angular velocity is detected by each angular velocity sensor when the rotary table 27 is rotating at a constant rotational speed. At this time, the average value of the angular velocities detected in 10 seconds is stored in the memory of the CPU 23 as the actual angular velocities ωX, ωY, ωZ of the respective angular velocity sensors. After detecting the real angular velocities ωX, ωY, ωZ in this way, the rotary table 27 is stopped.

Next, the actual angular velocity ωX of each angular velocity sensor,
The initial angular velocities ωX0, ωY0, ωZ0 are subtracted from ωY, ωZ to obtain the angular velocity increments ΔωX, ΔωY, ΔωZ, respectively. Then, the angular velocity increment ΔωX of the angular velocity sensor 12 around the X axis and the angular velocity increment ΔωY of the angular velocity sensor 14 around the Y axis are divided by the angular velocity increment ΔωZ at the angular velocity sensor 16 around the Z axis, respectively. The error angular velocities KXZ and KYZ of are calculated. Thus, the error angular velocity KX
After obtaining Z and KYZ, the CPU 23 stores them in the non-volatile memory 24.

After the error angular velocity about the Z axis is obtained in this way, the error angular velocity about the X axis is obtained. At this time,
Remove the base block 20 from the rotary table 27 once,
The base block 20 is fixed again so that the X axis is oriented in the vertical direction. Then, in the same manner as described above, the initial angular velocities ωX0, ωY0, ωZ0 are detected in the stationary state, and thereafter the rotary table 27 is rotated in the same manner as described above to obtain the real angular velocities ωX, ωY,
ωZ is detected. The angular velocity detection method is the same as when detecting the error around the Z axis.

After that, the initial angular velocities ωX0, ωY0, ωZ0 are subtracted from the real angular velocities ωX, ωY, ωZ, and the angular velocity increase Δω
X, ΔωY, ΔωZ are obtained, and the angular velocity increase ΔωY of the Y-axis rotation angular velocity sensor 14 and the angular velocity increase ΔωZ of the Z-axis rotation angular velocity sensor 16 are calculated.
The error angular velocities KYX and KZX per unit angular velocity are obtained by dividing by the angular velocity increment ΔωX of 2. After obtaining the error angular velocities KYX and KZX in this way, the CPU 23 stores them in the non-volatile memory 24.

After obtaining the error angular velocities about the Z axis and the X axis, the error angular velocities about the Y axis are obtained. Also at this time, similarly to the above, the base block 20 is once removed from the rotary table 27 and fixed again so that the Y axis of the base block 20 is oriented in the vertical direction. Then, the initial angular velocities ωX0, ωY0, ωZ0 are detected in a stationary state, and then the rotary table 27 is rotated to detect the real angular velocities ωX, ωY, ωZ. The angular velocity detection method is the same as that for detecting an error around the Z axis and the X axis.

After that, the initial angular velocities ωX0, ωY0, ωZ0 are subtracted from the actual angular velocities ωX, ωY, ωZ, and the angular velocity increase Δω
X, ΔωY, ΔωZ are obtained, and the angular velocity increase ΔωX of the X-axis rotational angular velocity sensor 12 and the angular velocity increase ΔωZ of the Z-axis rotational angular velocity sensor 16 are calculated.
The error angular velocities KXY and KZY per unit angular velocity are obtained by dividing by the angular velocity increment ΔωY of 4. After obtaining the error angular velocities KXY and KZY in this way, the CPU 23 stores them in the non-volatile memory 24.

By storing the error angular velocities around the X, Y, and Z axes in the nonvolatile memory 24 in this way, the error detection step is completed. The base block 20 to which the angular velocity sensor and the like are attached is mounted on the machine body 1 after this error detection process.

When the controller 8 in which the error angular velocities are stored in the above-described error detecting step is used, the actual angular velocity ω X detected by the X-axis rotational angular velocity sensor 12 is calculated as shown in FIG. , When the Z-axis rotation angular velocity sensor 16 outputs the angular velocities ωY and ωZ, the value of ωY × KYX and the value of ωZ × KZX are subtracted from each other to be changed to the true angular velocity ωX ′. Similarly, the actual angular velocity ωY detected by the Y-axis rotation angular velocity sensor 14 is the value of ωX × KXY when the X-axis rotation angular velocity sensor 12 and the Z-axis rotation angular velocity sensor 16 output the angular velocities ωX and ωZ. And the value of ωZ × KZY are subtracted from each other to be changed to the true angular velocity ωY ′. Further, the actual angular velocities ωZ detected by the Z-axis rotational angular velocity sensor 16 are also measured by the X-axis rotational angular velocity sensor 12 and the Y-axis rotational angular velocity sensor 14, respectively.
Is being output, the value of ωX × KXZ and the value of ωY × KYZ are subtracted from each other to be changed to the true angular velocity ωZ ′.

Therefore, the three angular velocity sensors 12, 1
It is possible to electrically correct the angular velocity error caused by the fact that the detection axes of the sensor bodies 12a, 14a, 16a of 4, 16 are not orthogonal to each other. Therefore, the sensor bodies 12a, 14 of the angular velocity sensors 12, 14, 16 are
a, 16a are attached to the cases 12b, 14b, 16b with strict accuracy, and the angular velocity sensors 12, 14, 1
It is not necessary to position 6 on the base block 20 with high precision.

In this embodiment, the error angular velocity is X,
Although the example of obtaining the Y-axis and Z-axis around three axes has been shown, only the error angular velocity around the Z-axis is obtained and the X-axis around angular velocity sensor 1 is provided.
2, the angular velocity ωX detected by the Y-axis rotation angular velocity sensor 14,
If only ωY is corrected, the attitude of the helicopter can be controlled with relatively high accuracy. Further, the base block 20 for attaching the angular velocity sensors 12, 14, 16 to the machine body 1 is not limited to the structure in which a plate-like object is extended in three directions as shown in the present embodiment. The structure can be changed appropriately.

Further, in the present embodiment, an example in which the attitude angle detecting method according to the present invention is applied to the attitude angle detecting method of an unmanned helicopter is shown. However, the attitude angle of a ship traveling on or under water is detected, It can also be applied when detecting the traveling direction of the shield type excavator.

[0046]

As described above, in the attitude angle detecting method of the angular velocity sensor type attitude angle detecting apparatus according to the present invention, the moving body is equipped with the attitude angle detecting apparatus to which the angular velocity sensors are attached for every three axes orthogonal to each other. Before rotating, it is rotated around the detection axis by the rotary table and the other two
The output value of the angular velocity sensor of the axis is stored as an error value in a non-volatile memory, and after the attitude angle detection device is mounted on a moving object, the output value of the angular velocity sensor of the two axes is changed when the device rotates around the detection axis. Since the error value is subtracted, 3
It is possible to electrically correct the angular velocity error that occurs due to the axes of the individual angular velocity sensors not being orthogonal to each other.

Therefore, it becomes unnecessary to mount the main body of the angular velocity sensor on the case with strict accuracy and to position each angular velocity sensor on the attitude angle detecting device with high accuracy. Therefore, it is not necessary to increase the accuracy more than necessary when manufacturing the angular velocity sensor or when processing the angular velocity sensor mounting portion of the attitude angle detection device, so that it is possible to detect the angular velocity with high accuracy while preventing cost increase. You will be able to.

[Brief description of drawings]

FIG. 1 is a schematic configuration diagram of an unmanned helicopter that detects an attitude angle by an attitude angle detection method according to the present invention.

FIG. 2 is a block diagram showing an overall configuration of an attitude angle detection device that detects an attitude angle by the attitude angle detection method according to the present invention.

FIG. 3 is a block diagram showing a configuration of a main part of an attitude angle detection device.

FIG. 4 is a perspective view showing a schematic configuration of an attitude angle detection device.

FIG. 5 is a block diagram for explaining an error value detection process on a rotary table.

FIG. 6 is a graph for explaining an error value detection procedure on a turntable.

FIG. 7 is a diagram for explaining an angular velocity detection procedure after mounting the attitude angle detection device on an unmanned helicopter.

[Explanation of symbols]

 8 Controller 12 Angular Velocity Sensor 14 Angular Velocity Sensor 16 Angular Velocity Sensor 20 Base Block 21 Posture Angle Calculator 24 Nonvolatile Memory 25 Corrected Angular Velocity Calculator 27 Rotation Table

Claims (3)

[Claims] 1. An angular velocity sensor for a vertical axis for detecting an angular velocity when rotating about an axis in the vertical direction, and an angular velocity sensor for rotating around an axis in the X and Y directions orthogonal to each other and orthogonal to the vertical direction. X-axis angular velocity sensor for detecting angular velocity,
The attitude angle detection device to which the Y-axis angular velocity sensor is attached is placed on a rotary table that rotates around an axis in the vertical direction so that the axis line of the vertical-axis angular velocity sensor is placed in the vertical direction and rotated. The output values of the X-axis angular velocity sensor and the Y-axis angular velocity sensor while rotating is stored in the nonvolatile memory as error values around the vertical axis, and this attitude angle detection device is mounted on a moving object. After the rotation, the true angular velocity is obtained by subtracting the error values from the output values of the X-axis angular velocity sensor and the Y-axis angular velocity sensor when turning around the vertical axis, and the angle is obtained based on the true angular velocity. An attitude angle detection method of an angular velocity sensor type attitude angle detection device, comprising: 2. The attitude angle detecting method for an angular velocity sensor type attitude angle detecting apparatus according to claim 1, wherein the axis of the X-axis angular velocity sensor is oriented vertically before the attitude angle detecting apparatus is mounted on a moving object. This attitude angle detecting device is rotated by a rotary table, and the output values of the vertical axis angular velocity sensor and the Y axis angular velocity sensor when the rotary table is rotating are stored in a non-volatile memory as error values around the X axis. After being mounted on a moving object, the true angular velocity is obtained by subtracting the error value around the X-axis from the output values of the vertical-axis angular velocity sensor and the Y-axis angular velocity sensor when rotating around the X-axis axis. An attitude angle detection method for an attitude angle sensor type attitude angle detection device, characterized in that an angle is obtained based on a true angular speed. 3. The method for detecting an attitude angle of an angular velocity sensor type attitude angle detecting device according to claim 2, wherein the axis of the Y-axis angular velocity sensor is oriented vertically before the attitude angle detecting device is mounted on a moving object. This attitude angle detecting device is rotated by a rotary table, and the output values of the vertical axis angular velocity sensor and the X axis angular velocity sensor when the rotary table is rotating are stored in a non-volatile memory as error values around the Y axis. After being mounted on a moving object, the true angular velocity is obtained by subtracting the error value around the Y-axis from the output values of the vertical-axis angular velocity sensor and the X-axis angular velocity sensor when turning around the Y-axis axis. An attitude angle detection method for an attitude angle sensor type attitude angle detection device, characterized in that an angle is obtained based on a true angular speed.