JPS63315909A - inertial navigation device - Google Patents

Abstract

PURPOSE:To remove the influence of a drift due to the gravitation by removing a signal of extremely low frequency by a filtering means. CONSTITUTION:The difference between the outputs of angular velocities omega1 and omega2 of servo dry gyros is calculated and amplified by an amplifier 20a. Then a band-pass filter 21a removes the signal of extremely low frequency of variation in the attitude angle of a robot, etc., to eliminate a drift signal due to the gravitation. Then a rotational angle calculating means 13 integrates a rotational speed and outputs a rotational speed theta. Accelerometers 4a and 4b output signals corresponding to (x)- and (y)-directional acceleration values. Those are amplified by amplifiers 20b and 20c and then band-pass filters 21b and 21c remove low-frequency components.

Description

DETAILED DESCRIPTION OF THE INVENTION [Summary] The present invention relates to an inertial navigation device that detects at least one translational displacement and rotational angle, and is ideal for measuring the position of a mobile object such as a self-propelled robot.

The purpose of the present invention is to provide an inertial navigation device that has high measurement accuracy, can be made smaller and lower in cost, and can be installed as an independent unit.

A rotational speed detection means for detecting the rotational speed of the moving body; and an acceleration detection means fixed to the same coordinate system as the rotational speed detection means and for detecting the acceleration of the moving body.

filtering means for removing low frequency components regarding the outputs of the rotation speed detection means and the acceleration detection means, and integrating the outputs of the rotation speed detection means;
Regarding the output of the rotation angle calculation means for determining the rotation angle of the moving body and the acceleration detection means, based on the output of the rotation angle calculation means.

The apparatus is configured to include a coordinate transformation means that performs transformation to a reference coordinate system, and a displacement calculation means that performs second-order integration of the result of the coordinate transformation and calculates the displacement of the moving body.

[Industrial application field]

The present invention relates to an inertial navigation device that detects at least one translational displacement and rotational angle, and is ideal for measuring the position of a moving object such as a self-propelled robot.

Autonomous robots and the like are often used to transport goods and the like. Requirements for the precision of the motion trajectories of these robots are becoming stricter year by year. Furthermore, it is desired that the degree of freedom of movement of the robot will be increased, and that the robot will be able to measure its own position without the need for special incidental equipment. Additionally, there is a need for a navigation device that can be easily attached and detached as an independent unit without depending on the structure of the robot.

[Conventional technology] FIG. 8 is an explanatory diagram of the prior art.

In Figure 8, l is a robot, 80 is a jai 0.81
82 represents a landmark sensor, 83 represents a driving wheel, and 84 represents a measurement wheel.

Conventionally, as a method for measuring the self-position of a moving object such as RoboScope 1 while moving, a gyro 80 is mounted to detect the rotation angle, and as shown in FIG.
Some devices use a landmark sensor 81 to optically detect a landmark 82 attached to the ground to confirm the current position.

In addition, as shown in FIG. 8(b), there is a system that is provided with a measuring wheel 84 that rotates according to the amount of movement, detects the number of rotations with an encoder, and calculates the amount of movement corresponding to one number of rotations. . Other methods that use electric wires strung across the road to guide the vehicle are also used.

(Problems to be Solved by the Invention) The conventional system shown in Fig. 8 has limitations such as requiring ground equipment and being usable only on certain uneven surfaces.

Another problem is that it is difficult to obtain high accuracy without such ancillary equipment.

As a self-position measuring means, means using a gyroscope are promising, but those with a stable platform mechanism, such as those often used in inertial navigation systems of aircraft, are
There are problems in that miniaturization is difficult and expensive.

When a rate gyro and an accelerometer are combined 2For example, when a robot stops diagonally to a horizontal plane, a drift due to gravity occurs, making it appear as if it is moving even though it is actually stationary. The problem is that output like this appears.

The present invention aims to solve the above-mentioned problems, and aims to provide an inertial navigation device that has high measurement accuracy, can be made smaller and lower in cost, and can be installed as an independent unit 7).

[Means for solving problems]

FIG. 1 is a diagram explaining the principle of the present invention.

In FIG. 1, 1 is a robot having an autonomous running function, 2 is an INS unit, and 3a, 3b are servo dry controls: 1. 4 (4a, 4b) are accelerometers, 5 is an INS controller, 10 is a rotation speed detection means, 11 is an acceleration detection means, 12 is a filtering means, 13 is a rotation angle calculation means, 14 is a coordinate conversion means,
15 is a displacement calculation means.

203 to 20C represent amplifiers, and 21a to 21c represent bandpass filters.

The inertial navigation device of the present invention is mounted on a robot 1 as shown in FIG.
Measure the translational displacement in the direction.

The INS unit 2 is as shown in FIG. 1 (b).

For example, two servo dry gyros 3a and 3b.

Together with these, it is composed of an X-direction accelerometer 4 fixed to the robot 1, and an X-direction accelerometer 4.

As shown in FIG. 1(C), the servo dry gyro 3a
, 3b's angular velocity ω1. The difference between the outputs of ω2 is taken and amplified by the amplifier 20a. The reason for taking the difference in output is to reduce the influence of acceleration disturbance. The bandpass filter 21a removes extremely low frequency signals, such as changes in the posture angle of the robot, in order to eliminate drift signals caused by the influence of one gravitational force. The rotation angle calculation means 13 integrates one rotation speed and outputs one rotation angle θ.

The accelerometers 4a and 4b output signals corresponding to accelerations in the X direction and the y direction, respectively. This is the amplifier 20b, 20
After amplification by C, bandpass filters 21b, 21
C removes low frequency components. Coordinate conversion means 1
4 performs coordinate transformation on this output using a coordinate transformation matrix that stores transformation information according to the angle in advance in order to transform from the robot coordinate system to the basic (absolute) coordinate system. Displacement calculation means! 5 performs second-order integration of the coordinate transformation results to calculate the translational displacement of the robot 1.

[Effect]

Since it does not use a gyroscope, etc. of a stable platform mechanism that uses a means to detect the rotational speed and directly measures the angle of one rotation, there is no need to control the unit in a floating state, and it is used for Bn by being fixed to a moving object such as a robot. can.

Also, an accelerometer 4a.

4b is used to obtain the acceleration factor, y, and the second-order integral/iii calculation is used to obtain the digits 1x, y, so there is no need for ground facilities such as landmarks.

Nervo-dry gyros and accelerometers have the disadvantage that they output signals due to the influence of gravity even when they are stationary, and they cannot be distinguished from signals generated by movement, so they are all drift signals. As a result, there is a possibility that measurement accuracy will be greatly affected, but since the filtering means 12 removes signals of extremely low frequencies, it is possible to eliminate the influence of such drift due to gravity.

〔Example〕

Fig. 2 is an example of a servo dry gyro used in an embodiment of the present invention, Fig. 3 is a block diagram of the control system of the gyro shown in Fig. 2, Fig. 4 is an example of a rotation speed detection means, and Fig. 5 is a FIG. 6 shows a configuration of an embodiment of the present invention, FIG. 6 shows an example of a processing configuration of the present invention, and FIG. 7 shows a frequency characteristic diagram of a filter according to an embodiment of the present invention.

For example, a servo dry gyro as shown in FIG. 2 is used as the rotational speed detecting means 10 shown in FIG.

In FIG. 2, 30 is a rotor as an inertia gyro, 31 is a spin axis, and 32 is a gimbal.

Nutation axes 33a and 33b are perpendicular to the spin axis 31.

3.1 is a flat plate motor, and 34a is its movable part.

Reference numeral 34b is a fixed part of the flat plate motor, on which a magnet is provided, and 34c and 34d are coils. This movable portion 34a is fixed to the first nutation shaft 33b. 35a, 35
b is a radial (cruciform) spring; 1 each has a nutation shaft 33a;
33b.

36 is a strain gauge.

1 nutation axis 33b due to the rotational force (torque) of the gimbal 32
When the radial spring 235b is deformed via the radial spring 35b, the deformation (2) is detected by the outputs of several strain gauges 36 attached to the radial spring 35b. Based on the detection signal, the torque of the gimbal 32 and, in turn, the rotational movement of the moving body are recognized. Furthermore, feedback to the flat plate motor 34 that controls the rotation of the gimbal 32 is provided in accordance with the angle detection result.

The control system is as shown in FIG. 3, for example.

In Figure 3,! 7 is the inductance of the flat plate motor,
R is the flat plate motor), Bff is the torque constant of the flat plate motor, i is the current flowing through the flat plate motor, J is the inertia of the moving part, and D is the viscous braking coefficient.

K is a spring constant, θ is a rotation angle, and r is a command rotation angle.

k, , , k are the current feedback gain, velocity feedback gain, and rotational feedback gain, respectively, and 4 is the integral feedback gain +k
S is the open loop gain of the power amplifier. S is a Laplace operator.

In this embodiment, the power amplifier to which at least the current and position JM multiplied signals are input is of a completely integral type.

The transfer characteristic when a flat plate motor is connected to the power amplifier falls within the range shown by PT in FIG.

The transfer function from the angle command value R(S) to the rotation angle θ(S) by Laplace transform is as follows.

R(S) k4ks B 1, Bβ
k.

k, BJ k, k, B7! to 4
Bn ksBffi Positive feedback of the gimbal 320 rotation angle signal shown in Fig. 2 results in a negative d
It has the effect of canceling the reaction force of the spring given as the Uji signal. As a result, the flat plate motor 34 suppresses torque having a frequency component that causes resonance of the spring.

Spring resonance can be prevented. Also, 5 command rotation angle r (=O)
Since the difference between the angle and the angle is completely integrated and positive feedback is performed, double oscillations do not occur and the steady angle deviation can always be zero.

It is possible to suppress the rotation of the gimbal up to high frequencies, making it possible to achieve high precision.

By the way, when a servo dry gyro as shown in Fig. 2 is mounted on a moving object, especially in a self-propelled robot, the servo dry gyro as shown in FIG.

Disturbances such as vibrations associated with movement are applied to the gimbal as rotational force. Since this disturbance causes an error in detecting the turning angle of the moving body, in this embodiment, as shown in FIG.
a, 3b are used.

The output currents tl and i of each control circuit 40a and 40b flowing to the flat plate motors of the servo dry gyros 3a and 3b are
By taking the difference in f, disturbance components can be removed and rotational angular velocity can be detected with high accuracy.

Filtering means 121 rotation angle calculation means 13 shown in FIG. Coordinate transformation means 14. The displacement calculation means 15 uses one or more microprocessors, etc.
It can be digitally processed. FIG. 5 shows an example of its configuration.

In FIG. 5, the same symbols as in FIG. 1 correspond to those shown in FIG. 1, and 50a to 50C are A/D converters;
51 represents a microprocessor.

Various methods are known for realizing bandpass filters using digital circuits.

Trying to create a bandpass filter that removes extremely low frequency signals using an analog circuit.

A capacitor with a large capacity is required, and the accuracy of the capacitor, resistor, etc. becomes a problem, and there is a possibility that it will be difficult to miniaturize the device due to the physical size. In this respect, if it is constructed using a digital circuit.

It has advantages such as easier adjustment and miniaturization.

The microprocessor 51 manually converts the output analog signals of the amplifiers 20a to 20C into digital signals by the A/D converters 50a to 50c, and performs the processes (1) to (4) as shown in FIG. 6, for example.

(2) Digitalize the output signal from the INS unit 2 to obtain the angular velocity ω, acceleration value (y) C-+ numbering.

(2) Filtering is performed for the angular velocity ω to remove very low frequency components below 1, for example, LO-'Ilz. Note that the digital filter technology is widely known, so a detailed explanation will be omitted.

■ Integrate the angular velocity ω with low frequency components removed to obtain one rotation angle θ. Note that a well-known technique can also be applied to this integral calculation.

■ Next, based on the obtained rotation angle θ, the acceleration factor 5
Convert y to the reference coordinate system.

■ Same as the angular velocity ω for the acceleration factor y.

Perform filtering to remove low frequency components. Note that this filtering may be performed before coordinate transformation.

■ By performing second-order integral calculation on acceleration L y.

Calculate the translational displacements x and y.

■ Output the obtained rotation angle θ and translational displacements x and y to the robot control device. This information is used in the robot control device to check the current position, etc.

The frequency characteristics of the filter in the filtering processing performed in the above processing (1) and (2) can be arbitrarily selected depending on the application of the robot or the like. In this example.

For example, filtering processing with steep frequency characteristics as shown in FIG. 7 is performed. The gain of signals below 10-'Hz and above lO''llz is made to decrease rapidly.

Although a self-propelled robot was described as an example, the inertial navigation device according to the present invention can be mounted not only on robots but also on various moving bodies.

〔Effect of the invention〕

As explained above, according to the present invention, the device can be miniaturized;
It becomes possible to provide a high-precision inertial navigation device that is easy to reduce in price. Since it can be constructed as an independent unit, handling and installation become easier.

It is especially suitable for trackless robots because it does not require any additional equipment such as landmarks.

[Brief explanation of the drawing]

FIG. 1 is a diagram explaining the principle of the present invention. FIG. 2 is an example of a servo dry gyro used in an embodiment of the present invention. FIG. 3 is a block diagram of the control system of the gyro shown in FIG. 2. FIG. 4 shows an example of rotational speed detection means. FIG. 5 shows the configuration of an embodiment of the present invention. FIG. 6 shows an example of the processing configuration of the present invention. FIG. 7 is a frequency characteristic diagram of a filter according to an embodiment of the present invention. FIG. 8 shows an explanatory diagram of the prior art. In the figure, 10 represents rotational speed detection means, 11 represents acceleration detection means, 12 represents filtering means 213, rotation angle calculation means, 14 represents coordinate conversion means, and 15 represents displacement calculation means.

Claims (1)

[Claims] An inertial navigation device mounted on a moving object, comprising: a rotational speed detection means (10) for detecting the rotational speed of the moving object; and fixed to the same coordinate system as the rotational speed detection means (10). an acceleration detection means (11) for detecting the acceleration of a moving object; a filtering means (12) for removing low frequency components related to the outputs of the rotational speed detection means (10) and the acceleration detection means (11); Rotation angle calculation means (13) which integrates the output of the rotation speed detection means (10) and calculates the rotation angle of the moving object.
), coordinate conversion means (14) for converting the output of the acceleration detection means (11) to a reference coordinate system based on the output of the rotation angle calculation means (13), and Displacement calculation means (15) that integrates and calculates the displacement of the moving body
).