JPH0281794A - Stabilizer device for navigating vehicle - Google Patents

Abstract

PURPOSE:To improve responsibility for disturbance in an invisible spot by detecting acceleration and an angular speed of a navigating vehicle in its principal three axes and correcting a gravitational acceleration component by an attitude of the navigating vehicle so as to be output to a mixing amplifier, in case of the captioned device for a drug scattering unmanned helicopter. CONSTITUTION:An output of three accelerometers 304 to 306 and three angular velocity sensors 301 to 303, with an input axis agreeing with the principal three axes of a navigating vehicle, is input to an arithmetic part 309. The arithmetic part 309 performs arithmetic processing in accordance with a predetermined procedure and a stabilization control. Here the arithmetic part 309, in its vehicle body coordinate component arithmetic part not shown, corrects a gravitational acceleration component by an attitude of the navigating vehicle from output signals of the accelerometers 304 to 306. And the result is input through a signal output part 310 to a mixing amplifier controlling the navigating vehicle. By this constitution, a stabilization control can be performed in good responsibility for disturbance even in an invisible condition with no necessity for particular skill.

Description

[Detailed Description of the Invention] [Field of Industrial Application] The present invention relates to a stabilizing device for safely operating a navigation vehicle such as an unmanned helicopter without requiring any skill, and in particular a stabilization device for controlling the attitude (roll and pitch angle) of the vehicle. ) and its heading, and also relates to a stabilizing device for a navigation vehicle that allows the navigation vehicle to fly safely and stably even against disturbances such as wind.

[Conventional technology]

A conventional example of a control device for a navigation vehicle, for example, an unmanned helicopter (hereinafter abbreviated as unmanned helicopter) will be described with reference to FIG. 8, which is a system block diagram thereof.

An operator (100) on the ground determines the direction, roll/pitch of the radio control transmitter (101) for the unmanned helicopter.

When the altitude handle or stick is operated, the transmitter (101) modulates these commands into radio waves (high frequency signals) and transmits them. Then, this radio wave is transmitted to a receiver (10) attached to the body (105) of the unmanned helicopter.
2) receives it, demodulates it, and sends its output to the servo actuator (103) via the mixing amplifier (200).
). The servo actuator (103) has a pitch control mechanism and a roll control mechanism (cyclic pitch control) in response to the input signal.
, azimuth control mechanism (tail rotor pitch control) and altitude control mechanism (main rotor pitch control (collective pitch control)
) consisting of a helicopter operation control mechanism (104
) to control the attitude, speed, altitude, etc. of the aircraft (105).

In addition, the mixing amplifier (200) is equipped with the aircraft (105).
An azimuth angular velocity signal around the azimuth axis is supplied from an azimuth axis angular velocity sensor (106). This azimuth angular velocity signal is supplied to the azimuth control mechanism via the mixing amplifier (200) to stabilize the movement of the aircraft (105) around the azimuth axis.

(Problems to be Solved by the Invention) However, with such conventional unmanned helicopter control devices, the unmanned helicopter itself lacks stability and is easily affected by crosswinds, etc. It takes a great deal of skill to fly safely and stably, and if an unmanned helicopter is to be used for things like spraying chemicals or taking aerial photographs, it is necessary to fly the unmanned helicopter to a location far away from the operator. It becomes impossible for the operator to visually detect changes in the attitude, altitude, speed, etc. of the unmanned helicopter, making it virtually impossible to stabilize the attitude, etc. There were drawbacks such as the impossibility of flight.

The present invention has been made in view of the above-mentioned conventional problems, and its purpose is to provide a novel stabilizing device for a navigation vehicle that eliminates the above-mentioned conventional problems.

[Means to solve the problem]

According to the present invention, in the stabilization device for an unmanned vehicle, which includes an angular velocity sensor and a mixing amplifier,
Three accelerometers (304) with input axes aligned in the axial direction
, (305), (306), and three angular velocity sensors (301), (302), (303),
A calculation unit (309) to which these outputs are input is provided, and a body coordinate component calculation unit (359) of the navigation vehicle which inputs the attitude angle signal among the outputs of the calculation unit. A stabilizing device for a navigation object is obtained, which corrects and outputs the gravitational acceleration component due to the attitude of the navigation object from the output signal, and inputs this output to the mixing amplifier (200).

[Effect]

According to the present invention configured as described above, the azimuth, attitude angle, angular velocity signal, and acceleration signal regarding the three main axes of the navigation object are detected by the azimuth/attitude detection device (201), and the detection results are supplied to the mixing amplifier (200). Then, signal processing corresponding to the motion characteristics of the vehicle is performed, and the resulting output is used to control the attitude and acceleration of the vehicle in the three main axes via the helicopter operation control mechanism (104).

〔Example〕

Hereinafter, one embodiment of the present invention will be described based on the drawings.

FIG. 1 is a block diagram showing an embodiment of a control system for a navigation object, such as an unmanned helicopter, according to the present invention. Furthermore, the first
In the figure, the same reference numerals as in FIG. 8 indicate the same parts.

The main difference between the example of the present invention shown in FIG. 1 and the conventional control system shown in FIG.
), a strap-down type orientation/attitude detection device (201
) from this orientation/attitude detection device (201).
Attitude angle signal (roll angle, pitch angle) signal, azimuth angle signal,
The purpose is to input angular velocity signals (three axes) and acceleration signals (three axes) to the mixing amplifier (200). The fuselage (105) of the unmanned helicopter is equipped with a speedometer (202) that detects the speed in its three main axis directions and an altimeter (203) that detects the altitude. The velocity signals (front/rear, lateral, and vertical velocity signals) are supplied to the azimuth/attitude detection device (201), and the altitude signal from the altimeter (203) is supplied to the mixing amplifier (20
0). In the example of Figure 1, the rest is
This is the same as the conventional example shown in the figure.

FIG. 2 is a detailed block diagram of the mixing amplifier (200) according to the present invention shown in FIG. As shown in the figure (
, mixing amplifier (200), roll mixing amplifier (200-1), pitch mixing amplifier (200
-2) consists of an azimuth mixing amplifier (200-3) and an altitude mixing amplifier (200-4).

The roll mixing amplifier (200-1) is connected to the transmitter (1
In addition to the lateral velocity command signal from 01), the roll angular velocity signal, roll attitude angle signal, lateral acceleration signal, and lateral velocity signal are received from the azimuth and attitude detection device (201), and the motion characteristics of the aircraft (105) are received. After performing signal processing corresponding to the above, these signals are input to the roll servo of the cyclic pitch mechanism that generates torque or force in the Y-axis direction around the longitudinal axis (X-axis) of the aircraft (105). do.

The pitch mixing amplifier (200-2) is connected to the transmitter (1
In addition to the longitudinal direction velocity command signal from the azimuth and attitude detection device (201), the pitch angular velocity signal, pitch attitude angle signal, longitudinal direction acceleration signal, and longitudinal direction velocity signal are received from the azimuth and attitude detection device (201), and the output thereof is sent to the cyclic pitch mechanism. The signal is sent to the pitch servo to stabilize the angular movement of the aircraft (105) around the Y axis, and control the longitudinal speed of the aircraft (105) to match the command speed signal.

The azimuth mixing amplifier (200-3) is connected to the transmitter (10
In addition to the direction command signal from 1), the direction and attitude detection device (2
01), the output is sent to the pitch servo of the tail rotor, and the output is sent to the pitch servo of the tail rotor.
105).

The advanced mixing amplifier (200-4) is connected to the transmitter (10
In addition to the altitude command signal from 1), the direction and attitude detection bag! (2
01), the vertical velocity signal, and the altitude signal from the altimeter (203), the output is input to the collective pitch servo that controls the pitch of the main rotor, and the altitude of the aircraft (105) is determined. Hold or make the aircraft (105) follow the altitude command.

FIG. 3 is a block diagram showing the overall configuration of an example of the strap-down type orientation/posture detection device (201) shown in FIG. 1. In the example shown in the figure, there are an X gyro (301), a Y gyro (302), a Z gyro (303), which are non-rotating gyros such as a vibration gyro, an X accelerometer (304), and a Y accelerometer (305). , X accelerometer (
306) and a magnetic orientation sensor (307) in the three main orthogonal axes of the unmanned helicopter (see Figure 1), namely X and Y.
.

Install each input shaft so that it matches the Z-axis (
(See arrow in Figure 3). Output signals from these are input to the calculation section (309) via the signal conversion section (308).

In the calculation unit (309), a coordinate transformation matrix (CT
After performing the calculations of M), gyro drift correction calculations, acceleration correction calculations, etc., the roll angle, pitch angle, and azimuth angle necessary for the control and operation of the navigation object are determined.

Signals such as angular velocities around the x, y, and z axes, X, Y, and Z (axial) accelerations, and x, y, and z (axial) velocities are sent out via the signal output section (310).

FIG. 4 is a block diagram showing the configuration of an example of the arithmetic unit (309) in FIG. 3. Signal converter (30B) shown in Figure 3
X, Y and Z gyros from (301), (302
).

Gyro signal (XG) from (303), (Y(1,
) and (ZC), respectively, a bias corrector (350),
(351) and (352) as X rate (roll angular velocity signal), Y rate (pitch angular velocity signal), and Z rate (azimuth angular velocity signal), and are supplied to the signal output section (310) shown in Fig. 3. , CTM (coordinate transformation matrix) is input to the calculation unit (353), and the CTM is calculated.

On the other hand, X from the signal converter (30B) shown in FIG.

Y and X accelerometers (304), (305) and (3
Acceleration signals (XA), (YA) and (ZA) from
) is the CTM signal (CS
) is input to the earth coordinate horizontal component calculation unit (354), where the horizontal components of the gravitational acceleration (
α) and (β) are calculated. This is the horizontal component (α).

(β) is the speedometer (202) of the unmanned helicopter (105).
) (see Figure 1) is input to the acceleration correction calculation section (355), where the component of the motion acceleration of the aircraft is removed, and then the upright torque calculation section (356
), and after performing a standing torque calculation, it is input to a CTM calculation unit (353), where the CTM is torqued so that the horizontal components (α) and (β) become zero.

An azimuth signal (As) from the CTM calculation unit (353),
The magnetic azimuth signal (MAS) from the signal converter (308) (see Figure 3) is the magnetic azimuth signal (MAS) from the azimuth constraint torque calculation unit (357).
There, a comparison calculation and a restraining torque calculation are performed, and the output torque signal (AT) is applied to the above CTM.
The CTM orientation is constrained to the magnetic orientation by feeding back to the calculation unit (353) and torqueing the CTM mainly around the orientation axis.

Outputs (AT), (ETI), (ET2) of the azimuth restraint torque calculation unit (357) and the standing torque calculation unit (356)
is input to the gyro bias calculation unit (358) together with the CTM signal (CS) of the CTM calculation unit (353), where the X, Y, Z gyro (301), (
The bias correction signals (302) and (303) are calculated, and the output thereof, that is, the bias correction signal (BC), is sent to a bias correction unit (350).

(351), (352).

Incidentally, when the speed signal (SS) with the desired accuracy cannot be obtained, the magnetic azimuth signal (MAS) or X.

A cutoff signal (CO3) made from Y, Z acceleration signals (XA), (YA), (ZA), etc. is supplied to the acceleration correction calculation unit (355), and when acceleration is applied, it is sent to the standing torque calculation unit (356). The input may be cut off.

Three x, y, and z gyros (301), (302), installed with input axes aligned with the three main axes of the aircraft.
(303) output signals (XIG), (YG), (ZG
), bias corrector (350), (351)
, (352) via the coordinate transformation matrix (CTM)
is input to the CTM calculation unit (353), which calculates the CTM. The three gyros x, y, and z installed on the aircraft so that the input axis of the above gyro and its input axis are parallel to each other.
Accelerometer (304), (305), (306)
The horizontal component calculation unit (354) calculates the horizontal component (α) of the gravitational acceleration from the output of
(β) is calculated. If CTM is correct, the horizontal component (α
) and (β) are zero, but if there is an error in the CTM, the horizontal components (α) and (β) will have finite values. Torque signal (ETI) that has the correct value, (ET2
) and sends it to the CTM calculation unit (353), which rotates it so that it faces in the correct direction.

On the other hand, the azimuth signal (AS
) and the magnetic azimuth signal (MAS) from the magnetic azimuth sensor (307), the azimuth constraint torque calculation unit (357) performs comparison calculations, etc. to create a torque signal (AT) around the azimuth axis, which is then C so that there is no difference between the two
Feedback is provided to the TM calculation unit (353).

On the other hand, if there is gyro drift, the standing torque calculation section (
356) and the azimuth constraint torque calculation unit (357) are not zero, but have finite values corresponding to the gyro drift. These signals are processed by the gyro bias calculation unit (35B
) is input together with the CTM signal (CS) and the calculated bias correction signal (BC) is input to each gyro (301), (
302), bias corrector (350) of (303)
.

(351) and (352) are manually corrected so that the gyro drift becomes zero.

From the CTM calculation unit (353), the roll angle, pitch angle, and azimuth angle signals of the aircraft and the bias corrector (350), (
351).

From (352), the angular velocity of the vehicle, X ray) t Y sea.

Output each Z rate.

When the aircraft is performing acceleration motion such as turning, increasing/decelerating, etc., the input to the standing torque calculation section (356) is cut off to eliminate the influence of acceleration. Also, speedometer (202) (
When the speed signal (SS) etc. are obtained from the signal (see FIG. 1), this and the CTM signal (CS) are supplied to the acceleration correction calculation section (355) to remove the influence of acceleration.

Figure 5 shows the aircraft coordinate component calculation unit (359) shown in Figure 4.
- shows a block diagram of an example. As shown in the figure, this aircraft coordinate component calculation section (359) is a gravity tilt component calculation section (359).
59-1) and its correction section (359-2).

This gravity tilt component calculation unit (359-1) inputs the roll attitude angle signal (θ) and pitch attitude angle signal (θF) calculated by the CTM calculation unit (353), and calculates the , Y, Z direction gravitational acceleration (station component is calculated.Here, the X, Y, Z direction gravitational acceleration (6) component is
g-sinθp, g-sinθ and g', respectively
It is calculated as cos θ,·cos θ8.

Directions, X, Y and Z accelerometers (304), (305)
and the acceleration signal (XA
), (YA).

(ZA) is the adder (
ADI), (AD2) and (AD3), and there, the X
, are compared with the corresponding components of the gravitational acceleration (8) in the Y and Z directions, and as a result, the aircraft coordinate component calculation unit (35
9) outputs the gravitational acceleration (true longitudinal acceleration signal (αX) corrected for the barrel component) + lateral acceleration signal (α) and vertical acceleration signal (α2) due to the tilt of the aircraft.

Next, the speed signal calculation correction section (360) in FIG. 4 will be explained. The source of the target speed signal is the unmanned helicopter, that is, the speedometer (20
Examples of 2) include a Doppler ground speed meter or air speed meter using a microwave system.

A Doppler ground speed meter is easily affected by the unevenness of a reflecting surface (such as the ground surface), and these unevenness appear as noise in its output signal. In other words, even though the aircraft (unmanned helicopter) is flying at a constant speed, due to the effects of unevenness on the ground surface, etc.
This Doppler ground speed meter outputs a signal that gives the impression that the speed is changing.

On the other hand, airspeed meters generate errors due to factors such as wind speed, so they cannot be expected to be highly accurate speedometers.

Therefore, the speed signal calculation correction section (360) corrects the above-mentioned defects of each speedometer and obtains a highly accurate speed signal.

As shown in FIG. 4, the speed signal calculation correction section (360)
Front and back, which is the output of the aircraft coordinate component calculation unit (359).

Acceleration signals in the lateral and vertical directions (αX), (α7)
(α8) and the speedometer (2
The longitudinal, lateral, and vertical velocity signals (vllX), (■1lV), and (VOZ), which are the outputs of The husband outputs velocity signals Vx, V, , V2 in the directions.

The speed signal calculation correction unit (360) is configured for each of the X, Y, and Z axes, but in reality, its configuration is the same for each of the X, Y, and Z axes. Therefore, in FIG. 6, only the speed signal calculation correction section (360X) for one axis, for example, the X-axis direction, is shown and explained.

In the speed signal calculation correction section (360X) shown in FIG.
I), the signal is input to an integrator (360-1), an integral operation is performed, and the signal is converted into a speed signal (VX). The speed signal (Vx) is compared with the speed signal (vex) which is the output of the speedometer (202) in an adder (AD21), and the output is sent to the adder via a coefficient unit (360-2). (
It is fed back to A D 11).

The input/output characteristics of the speed signal calculation correction section (360X) having the above-described configuration are shown in the following equation.

As is clear from the equation ( ), the speed signal (VDX) from the speed hand (202) is filtered with a time constant T, so high-frequency noise due to unevenness of the ground surface, etc. is included in the speed signal. The component can be removed by selecting an appropriate value for the time constant TF of the filter. Also, the speed signal (
The delay in response caused by inserting a filter into Vll) can be removed because it is corrected using a term that integrates the acceleration signal (αX). In addition, the output speed signal (V) when an error occurs in the acceleration signal (α on α)
The steady-state error of l) is T, ·α. This is a finite value, and by performing acceleration integration, the speed error does not increase over time, and a stable speed signal can be obtained over a long period of time.

FIG. 7 shows a specific circuit example of the speed signal computation and correction section (360).

Note that since the circuits are the same in the x, Y, and Z directions, only one axis, for example, the circuit (360X) in the X axis direction is shown, and the others are omitted.

In Fig. 7, the acceleration signal (α,) is transmitted through the operational amplifier (
A1), resistor (R1) and capacitor (C1)
It is integrated with a gain determined by . The operational amplifier (A2) connects the output signal of the operational amplifier (AI) to the resistor (R2).
and (R3), and output speed signal (
V, ) is output. The operational amplifier (A3) connects the output speed signal (Vx) and speed signal (VDX) to the resistor (R4).
).

A comparison operation is performed using the gains determined by (R5), (R6), and (R7), and an output signal is output. This output signal is attenuated by a variable resistor (VRI) and then fed back to the operational amplifier (A1) via a resistor (R8).

In the circuit configuration shown in FIG. 7, each resistor (R1)
~ (R7) and each resistance value and capacity of capacitor (C1)!
(Use the same code) For example, R1=R8, R1・C1
= 1 second, R2 = R3, R4 = R5 = R6 = R7, and the attenuation rate of the variable resistor (VRI) = β is selected, and the output speed signal ( VX), the following formula is obtained.

Here, S is a Laplace operator.

In the equation (2), if we set T as the equation (1) described above, the time constant Tr can be appropriately selected by adjusting the variable resistor (VRI).

In the above description, an unmanned helicopter was cited as an example of a navigation vehicle to which the present invention is applied, but the present invention also applies to other navigation vehicles, such as manned,
Helicopters, unmanned submarines, unmanned target aircraft.

It goes without saying that it can be applied to missiles and the like, and the same effects can be achieved.

〔Effect of the invention〕

According to the present invention having the above-described configuration, it is possible to perform attitude control regarding the three main axes of the navigation object and eliminate instability inherent in the navigation object, and to detect acceleration signals purely due to the movement of the navigation object, Since the movement of the vehicle caused by disturbances such as wind is quickly controlled, the attitude and position of the vehicle can be controlled even in places where the operator cannot see the vehicle.
In addition to being able to stabilize the heading without any skill, it is also possible to improve the responsiveness of the navigation vehicle to disturbances.

Furthermore, in the present invention, since the strap-down type orientation and attitude detection device is only added to the conventional control device,
It can be applied to conventional equipment with only simple changes.

[Brief explanation of the drawing]

FIG. 1 is a block diagram of a system according to an example of the present invention, FIG. 2 is a detailed block diagram of an example of its mixing amplifier, and FIG. 3 is a block diagram of an example of the orientation and orientation detection device of FIG. 1.
Fig. 4 is a block diagram of an example of the calculation section, Fig. 5 is a detailed block diagram of an example of the aircraft coordinate component calculation section, and Fig. 6 is a detailed block diagram of an example of the speed signal calculation correction section of Fig. 4. 7 is a circuit diagram of an example thereof, and FIG. 8 is a block diagram of a conventional unmanned helicopter control device. In the figure, (101) is a radio control transmitter, (102)
) is a radio control receiver, (103) is a servo actuator, (104) is a control mechanism for helicopter operation, (105) is a navigation vehicle body, (200) is a mixing amplifier, and (201) is a strap-down type direction. The attitude detection device, (202) shows a speedometer, and (203) shows an altimeter.

Claims (1)

[Claims] 1. In a navigation vehicle stabilization device consisting of an angular velocity sensor and a mixing amplifier, three accelerometers whose input axes are aligned with the three main axes of the navigation vehicle, and three angular velocity sensors; sensor and
In addition to providing a calculation unit into which these outputs are input, a calculation unit for calculating body coordinates of the navigation object receives the attitude angle signal from among the outputs of the calculation unit, and calculates the output signal of the navigation object from the output signal of the accelerometer. A stabilizing device for a navigation object, characterized in that the gravitational acceleration component due to the attitude of the vehicle is corrected and output, and the output is input to the mixing amplifier. 2. The navigation object according to claim 1, wherein the coordinate component calculation section of the navigation object comprises a gravitational inclination component calculation section and a correction section that corrects the output signal of the accelerometer. Stabilizer.