WO2019223271A1 - Correction method and device for yaw of aircraft, and aircraft - Google Patents

Abstract

A correction method and device for a yaw of an aircraft, and an aircraft. The method comprises: acquiring initial attitude information of an aircraft, the initial attitude information of the aircraft comprising an initial angle of the aircraft (101); acquiring a yaw rate of the aircraft, i.e., a first yaw rate, according to the initial angle of the aircraft (102); acquiring an angular rate of a pan-tilt base (103); acquiring a yaw rate of the pan-tilt base, i.e., a second yaw rate according to the angular rate of the pan-tilt base (104); comparing the first yaw rate with the second yaw rate (105); and when the difference between the first yaw rate and the second yaw rate is larger than or equal to a preset threshold, correcting the yaw of the aircraft according to the first yaw rate and the second yaw rate, so as to obtain the corrected yaw of the aircraft (106). The correction method of a yaw of an aircraft can effectively improve the accuracy of the estimation of a yaw of an aircraft.

Description

Method, device for correcting yaw angle of aircraft and aircraft

Cross-reference to related applications

This application claims priority from a Chinese patent application filed with an application number of 201810501521.3 and an application date of May 23, 2018, the entire contents of which are incorporated herein by reference.

Technical field

Embodiments of the present invention relate to the technical field of aircraft, and in particular, to a method for correcting a yaw angle of an aircraft, a device for correcting a yaw angle of an aircraft, and an aircraft.

Background technique

In recent years, aircraft, such as unmanned aerial vehicles (UAVs), also known as unmanned aerial vehicles (UAVs), have been used more and more widely. UAV is a new concept equipment under rapid development. It has the advantages of small size, light weight, flexible maneuverability, fast response, unmanned driving, and low operating requirements. The various actions (or attitudes) of UAV are usually realized by controlling different speeds of multiple drive motors in the power unit of UAV. Among them, the UAV yaw angle is an important parameter in controlling the attitude of the UAV (such as front, rear, left, right, up, down, etc.), that is, the correct estimation of the yaw angle of the UAV is particularly important for the attitude control of the UAV. Importantly, if the yaw angle of the UAV is incorrectly estimated, the UAV may not be able to fly in a preset direction or trajectory, and in the worst case it may be unstable and cause a bomber. Therefore, how to improve the accuracy of the yaw angle estimation is of great significance.

At present, the yaw angle of an aircraft (such as UAV) is usually obtained based on data collected by a magnetometer, but the yaw angle obtained by this method is easily affected by external factors, especially when the magnetometer is in a strong magnetic interference environment. The magnetometer data may be seriously wrong, leading to a large deviation in the yaw angle estimation, and the accuracy of the yaw angle estimation of the aircraft is low.

Summary of the Invention

Embodiments of the present invention provide an aircraft yaw angle correction method and device, and an aircraft, which can effectively improve the accuracy of estimating the aircraft yaw angle.

The embodiments of the present invention disclose the following technical solutions:

According to a first aspect, an embodiment of the present invention provides a method for correcting a yaw angle of an aircraft. The aircraft is connected with a gimbal. The gimbal includes a gimbal base, a gimbal motor connected to the gimbal base, and The shooting device connected to the PTZ motor, the method includes:

Acquiring initial attitude information of the aircraft, where the initial attitude information of the aircraft includes an initial angular velocity of the aircraft;

Obtaining the yaw angular velocity of the aircraft according to the initial angular velocity of the aircraft, wherein the yaw angular velocity of the aircraft is a first yaw angular velocity;

Obtaining the angular velocity of the gimbal base;

Obtaining the yaw angular velocity of the gimbal base according to the angular velocity of the gimbal base, wherein the yaw angular velocity of the gimbal base is a second yaw angular velocity;

Comparing the first yaw angular velocity with the second yaw angular velocity;

When the difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, the yaw of the aircraft is based on the first yaw angular velocity and the second yaw angular velocity. The heading angle is corrected to obtain a corrected yaw angle of the aircraft.

In an embodiment of the present invention, the preset threshold is determined by whether the aircraft receives a yaw operation instruction.

In an embodiment of the present invention, the preset threshold is determined by whether the aircraft receives a yaw operation instruction, and includes:

When the aircraft receives a yaw operation instruction, the preset threshold is a first preset threshold;

When the aircraft does not receive a yaw operation instruction, the preset threshold is a second preset threshold.

In an embodiment of the present invention, when a difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, the according to the first yaw angular velocity and the Said second yaw angular velocity and correcting the yaw angle of the aircraft include:

Obtaining a yaw angle compensation value according to a difference between the first yaw angular velocity and the second yaw angular velocity;

Correct the yaw angle of the aircraft according to the yaw angle compensation value.

In an embodiment of the present invention, the yaw angle compensation value Δψ _p is:

Where t ₀ is the time corresponding to the initial attitude information of the aircraft, and ΔT is the period for correcting the yaw angle of the aircraft,

Is the first yaw angular velocity,

Is the second yaw angular velocity.

In an embodiment of the present invention, the yaw angle ψ ′ _p of the aircraft after the correction is:

ψ ′ _p = ψ _p + Δψ _p

Wherein, ψ _p is the yaw angle of the aircraft, and Δψ _p is the yaw angle compensation value.

In an embodiment of the present invention, the obtaining the angular velocity of the gimbal base includes:

Obtaining an angle of the gimbal motor;

Determining the angular velocity of the gimbal motor according to the angle of the gimbal motor;

Obtaining an angular velocity of the photographing device;

Determine the angular velocity of the pan-tilt base according to the angular velocity of the pan-tilt motor, the angle of the pan-tilt motor, and the angular velocity of the photographing device.

In an embodiment of the present invention, determining the angular velocity of the gimbal motor according to the angle of the gimbal motor includes:

Taking the angle of the gimbal motor as an input, the angular velocity of the gimbal motor is calculated by a second-order discrete nonlinear tracking differentiator.

In an embodiment of the present invention, the expression of the second-order discrete nonlinear tracking differentiator is:

r ₁ (k + 1) = r ₁ (k) + T · r ₂ (k)

r ₂ (k + 1) = r ₂ (k) + T · fst (r ₁ (k) -P (k), r ₂ (k), δ, h)

Among them, T is a sampling period for obtaining the angle of the gimbal motor, and P (k) = [φ (k) θ (k) ψ (k)] ^T is the angle of the gimbal motor at the k-th time, r ₁ (k) is a value determined by P (k) for tracking P (k) by the second-order discrete nonlinear tracking differentiator, r ₂ (k) is a derivative of P (k), and k + 1 is the first The value corresponding to time k + 1, fst () is the highest speed control function, δ is a parameter located at the third position of the highest speed control function, and h is a parameter located at the fourth position of the fastest control function.

In an embodiment of the present invention, determining the angular velocity of the gimbal base according to the angular velocity of the gimbal motor, the angle of the gimbal motor, and the angular velocity of the photographing device includes:

Determining a rotation transformation matrix according to the angle of the gimbal motor, the rotation transformation matrix being a rotation matrix of a gimbal base coordinate system to a gimbal motor coordinate system;

Determine the angular velocity of the pan-tilt base according to the angular velocity of the pan-tilt motor, the rotation transformation matrix, and the angular velocity of the photographing device.

In an embodiment of the present invention, the calculation formula of the rotation transformation matrix is:

Where D is the rotation transformation matrix; (φ, θ, ψ) is the angle of the gimbal motor, φ is the roll angle in the angle of the gimbal motor, and θ is the pitch in the angle of the gimbal motor. Angle, ψ is the yaw angle among the angles of the gimbal motor.

In an embodiment of the present invention, the calculation formula of the angular velocity of the gimbal base is:

among them,

Is the angular velocity of the gimbal base,

Is the angular velocity of the photographing device, D is the rotation transformation matrix, and r ₂ is the angular velocity of the gimbal motor.

According to a second aspect, the present invention also provides an aircraft yaw angle correction device. The aircraft is connected with a gimbal. The gimbal includes a gimbal base, a gimbal motor connected to the gimbal base, and The shooting device connected to the PTZ motor, the device includes:

An initial attitude information acquisition module, configured to obtain initial attitude information of the aircraft, wherein the initial attitude information of the aircraft includes an initial angular velocity of the aircraft; and

Configured to obtain a yaw angular velocity of the aircraft according to an initial angular velocity of the aircraft, wherein the yaw angular velocity of the aircraft is a first yaw angular velocity;

The base angular velocity obtaining module is configured to obtain the angular velocity of the gimbal base and to obtain the yaw angular velocity of the gimbal base according to the angular velocity of the gimbal base, wherein the The yaw angular velocity is the second yaw angular velocity;

A comparison module, configured to compare the first yaw angular velocity with the second yaw angular velocity;

A correction module, configured to: when the comparison module determines that a difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, according to the first yaw angular velocity and the second yaw Angular velocity, correcting the yaw angle of the aircraft to obtain a corrected yaw angle of the aircraft.

In an embodiment of the present invention, the device further includes a preset threshold determination module, configured to determine the preset threshold according to whether the aircraft receives a yaw operation instruction.

In an embodiment of the present invention, the preset threshold determination module is specifically configured to:

When the aircraft receives a yaw operation instruction, determine that the preset threshold is a first preset threshold;

When the aircraft does not receive a yaw operation instruction, it is determined that the preset threshold is a second preset threshold.

In an embodiment of the present invention, when the difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, the correction module is based on the first yaw angular velocity and the Said second yaw angular velocity and correcting the yaw angle of the aircraft include:

Obtaining a yaw angle compensation value according to a difference between the first yaw angular velocity and the second yaw angular velocity;

Correct the yaw angle of the aircraft according to the yaw angle compensation value.

In an embodiment of the present invention, the yaw angle compensation value Δψ _p is:

Where t ₀ is the time corresponding to the initial attitude information of the aircraft, and ΔT is the period for correcting the yaw angle of the aircraft,

Is the first yaw angular velocity,

Is the second yaw angular velocity.

In an embodiment of the present invention, the yaw angle ψ ′ _p of the aircraft after the correction is:

ψ ′ _p = ψ _p + Δψ _p

Wherein, ψ _p is the yaw angle of the aircraft, and Δψ _p is the yaw angle compensation value.

In an embodiment of the present invention, the base angular velocity obtaining module is specifically configured to:

Obtaining an angle of the gimbal motor;

Determining the angular velocity of the gimbal motor according to the angle of the gimbal motor;

Obtaining an angular velocity of the photographing device;

Determine the angular velocity of the pan-tilt base according to the angular velocity of the pan-tilt motor, the angle of the pan-tilt motor, and the angular velocity of the photographing device.

In an embodiment of the present invention, the base angular velocity acquisition module determining the angular velocity of the pan / tilt motor according to the angle of the pan / tilt motor includes:

Taking the angle of the gimbal motor as an input, the angular velocity of the gimbal motor is calculated by a second-order discrete nonlinear tracking differentiator.

In an embodiment of the present invention, the expression of the second-order discrete nonlinear tracking differentiator is:

r ₁ (k + 1) = r ₁ (k) + T · r ₂ (k)

r ₂ (k + 1) = r ₂ (k) + T · fst (r ₁ (k) -P (k), r ₂ (k), δ, h)

Among them, T is a sampling period for obtaining the angle of the gimbal motor, and P ( ^k ) = [φ (k) θ (k) ψ (k)] ^T is the angle of the gimbal motor at the k-th time, r ₁ (k) is a value determined by P (k) for tracking P (k) by the second-order discrete nonlinear tracking differentiator, r ₂ (k) is a derivative of P (k), and k + 1 is the first The value corresponding to time k + 1, fst () is the highest speed control function, δ is a parameter located at the third position of the highest speed control function, and h is a parameter located at the fourth position of the fastest control function.

In an embodiment of the present invention, the base angular velocity obtaining module determines the angular velocity of the gimbal base according to the angular velocity of the gimbal motor, the angle of the gimbal motor, and the angular velocity of the photographing device. include:

Determining a rotation transformation matrix according to the angle of the gimbal motor, the rotation transformation matrix being a rotation matrix of a gimbal base coordinate system to a gimbal motor coordinate system;

Determine the angular velocity of the pan-tilt base according to the angular velocity of the pan-tilt motor, the rotation transformation matrix, and the angular velocity of the photographing device.

In an embodiment of the present invention, the calculation formula of the rotation transformation matrix is:

Where D is the rotation transformation matrix; (φ, θ, ψ) is the angle of the gimbal motor, φ is the roll angle in the angle of the gimbal motor, and θ is the pitch in the angle of the gimbal motor. Angle, ψ is the yaw angle among the angles of the gimbal motor.

In an embodiment of the present invention, the calculation formula of the angular velocity of the gimbal base is:

among them,

Is the angular velocity of the gimbal base,

Is the angular velocity of the photographing device, D is the rotation transformation matrix, and r ₂ is the angular velocity of the gimbal motor.

According to a third aspect, the present invention also provides an aircraft including:

body;

A machine arm connected to the fuselage;

A power unit provided on the machine arm;

At least one processor provided in the fuselage; and

A memory connected in communication with the at least one processor; wherein,

The memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor, so that the at least one processor can execute the aircraft yaw angle correction method described above.

In the embodiment of the present invention, the difference between the yaw angular velocity in the initial angular velocity of the aircraft and the yaw angular velocity in the angular velocity of the gimbal base is used to correct the yaw angle in the initial angle of the aircraft, which can effectively improve the estimated aircraft yaw The accuracy of the heading angle. This estimation method can avoid the interference of external factors, that is, it has high estimation accuracy under the environment of weak GPS signal and strong magnetic interference, thereby effectively improving the safety and stability of the aircraft flight.

BRIEF DESCRIPTION OF THE DRAWINGS

One or more embodiments are exemplified by the pictures in the accompanying drawings. These exemplary descriptions do not constitute a limitation on the embodiments. Elements with the same reference numerals in the drawings are denoted as similar elements. Unless otherwise stated, the drawings in the drawings do not constitute a limitation on scale.

FIG. 1 is a schematic flowchart of an aircraft yaw angle correction method according to an embodiment of the present invention;

2 is a schematic diagram of a position setting of an attitude sensor component according to an embodiment of the present invention;

3 is a schematic diagram of an aircraft yaw angle correction device according to an embodiment of the present invention;

4 is a schematic diagram of an aircraft hardware structure provided by an embodiment of the present invention;

5 is a connection block diagram of an aircraft provided by an embodiment of the present invention;

FIG. 6 is a schematic diagram of the power plant in FIG. 5.

Detailed ways

The following describes the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are a part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by a person of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

The method for correcting the yaw angle of an aircraft provided by the embodiment of the present invention can be applied to various movable objects driven by a motor or a motor, including but not limited to an aircraft, a robot, and the like. The aircraft may include an unmanned aerial vehicle (UAV), an unmanned aerial vehicle, and the like. Take UAV as an example for illustration. The UAV includes a fuselage, a boom connected to the fuselage, a controller, and a power unit. The controller is connected to a power device, and the power device is installed on the arm and is used to provide flying power for the aircraft. Specifically, the controller is configured to execute the above-mentioned yaw angle correction method of the aircraft to correct the yaw angle of the aircraft, and generate a control instruction according to the corrected yaw angle of the aircraft, and send the control instruction to the ESC of the power unit (electronic Governor). The ESC controls the drive motor of the power unit through this control instruction. Alternatively, the controller is configured to execute the yaw angle correction method of the aircraft to correct the yaw angle of the aircraft, and send the corrected yaw angle of the aircraft to the ESC, and the ESC generates a control instruction based on the corrected yaw angle of the aircraft, The driving motor of the power unit is controlled by the control instruction.

An unmanned aerial vehicle may include one or more arms that extend radially from the fuselage. The connection between the arm and the fuselage may be an integral connection or a fixed connection.

The controller is configured to execute the above-mentioned yaw angle correction method of the aircraft to correct the yaw angle of the aircraft, and generate a control instruction according to the corrected yaw angle of the aircraft, and send the control instruction to the ESC of the power unit so that the ESC passes The control instruction controls a drive motor of the power unit. The controller is a device with certain logic processing capabilities, such as a control chip, a single-chip microcomputer, and a Microcontroller Unit (MCU).

Power unit includes: ESC, drive motor and propeller. The ESC is located inside the arm or body. The ESC is connected to the controller and the drive motor respectively. Specifically, the ESC is electrically connected to the driving motor and is used to control the driving motor. The driving motor is installed on the arm, and the rotating shaft of the driving motor is connected to the propeller. The propeller generates a force that moves the UAV under the driving of a drive motor, for example, a lift force or a thrust force that moves the UAV.

UAV completes each of the specified speeds, actions (or attitudes) by controlling the drive motor to achieve this. The full name of the ESC is electronic speed governor, which adjusts the speed of the driving motor of the UAV according to the control signal. Among them, the controller is the execution subject that executes the yaw angle correction method of the aircraft, and the ESC controls the driving motor based on the control command generated by the corrected yaw angle of the aircraft. The principle of ESC controlling the driving motor is roughly as follows: The driving motor is an open-loop control element that converts electrical pulse signals into angular displacement or linear displacement. Under non-overload conditions, the speed and stop position of the drive motor depend only on the frequency and number of pulse signals, and are not affected by the load change. When the driver receives a pulse signal, it drives the drive motor of the power unit Rotate a fixed angle in the set direction, and its rotation runs at a fixed angle. Therefore, the ESC can control the amount of angular displacement by controlling the number of pulses, thereby achieving the purpose of accurate positioning; at the same time, the speed and acceleration of the driving motor can be controlled by controlling the pulse frequency, thereby achieving the purpose of speed regulation.

At present, the main functions of UAV are aerial photography, real-time image transmission, and detection of high-risk areas. In order to realize the functions of aerial photography, real-time image transmission, and detection of high-risk areas, a camera component is connected to the UAV. Specifically, the UAV and the camera component are connected through a connection structure, such as a vibration reduction ball. This camera module is used to obtain the shooting picture during the aerial photography of UAV.

Specifically, the camera component includes: a gimbal and a shooting device. The gimbal is connected to the UAV. The photographing device is mounted on the pan / tilt head. The photographing device may be an image acquisition device for acquiring images. The photographing device includes, but is not limited to, a camera, a video camera, a camera, a scanner, a camera phone, and the like. The pan / tilt is used to carry a photographing device, so as to fix the photographing device, or adjust the posture of the photographing device (for example, change the height, inclination, and / or direction of the photographing device), and keep the photographing device in a set posture. on. For example, when the UAV performs aerial photography, the pan / tilt is mainly used to keep the shooting device in a set posture stably, prevent the shooting screen of the shooting device from shaking, and ensure the stability of the shooting screen.

The gimbal includes: gimbal motor and gimbal base. The gimbal motor is installed on the gimbal base. The controller of the aircraft can also control the PTZ motor through the ESC of the power unit. Specifically, the controller of the aircraft is connected to the ESC, and the ESC is electrically connected to the PTZ motor. The controller of the aircraft generates the PTZ motor control instructions. Adjust the PTZ motor control instruction to control the PTZ motor.

The gimbal base is connected to the body of the UAV, and is used to fix the camera module on the body of the UAV.

The gimbal motor is connected to the gimbal base and camera. The gimbal can be a multi-axis gimbal. In response to this, there are multiple gimbal motors, that is, one gimbal motor is provided for each axis. On the one hand, the PTZ motor can drive the rotation of the shooting device, so as to meet the horizontal rotation and tilting angle adjustment of the shooting shaft. Manually remotely control the rotation of the PTZ motor or use the program to rotate the motor automatically, so as to achieve all-round scanning monitoring; On the other hand, during the aerial photography of the UAV, the disturbance of the shooting device is cancelled in real time by the rotation of the gimbal motor to prevent the shooting device from shaking and ensure the stability of the shooting picture.

The photographing device is mounted on the pan / tilt head, and an inertial measurement unit is provided on the photographing device, and the inertial measurement unit is configured to acquire attitude information of the photographing device.

In the process of controlling the attitude of the UAV, the yaw angle of the UAV is an important parameter in controlling the attitude of the UAV, and the driving motor needs to be controlled based on the yaw angle of the UAV. The yaw angle of the UAV is obtained in real time through the controller of the aircraft, and the necessary attitude information is provided for the attitude control of the UAV. That is, the correct estimation of the yaw angle of the UAV is particularly important for the attitude control of the UAV. If the yaw angle of the UAV is incorrectly estimated, the UAV cannot fly in a preset direction or trajectory in the light, and may be unstable due to the bomber.

At present, the yaw angle of the UAV is usually obtained based on the data collected by the magnetometer. However, the yaw angle obtained by this method is easily affected by external factors. Especially when the magnetometer is in a strong magnetic interference environment, the magnetometer data may be Severe errors lead to a large deviation in the yaw angle estimation, and the accuracy of the yaw angle estimation of the aircraft is low.

In order to improve the accuracy of the yaw angle estimation of the UAV, it is common to use an external GPS module to estimate a yaw angle based on the GPS to correct the yaw angle value based on a magnetometer. However, the GPS signal may be unstable sometimes, so that in some cases, the yaw angle estimated by the magnetometer cannot be effectively corrected even if there is a deviation. That is, using an external GPS module to correct the yaw angle of the UAV can improve the accuracy of the yaw angle estimation of the aircraft to a certain extent, but the effect is not good, especially when the GPS signal is weak, using the external GPS The module failed to perform effective course correction.

Therefore, based on the above problems, the main purpose of the embodiments of the present invention is to provide an aircraft yaw angle correction method, device and aircraft, which can correct the aircraft yaw angle based on the attitude information provided by the gimbal, and effectively improve the estimated aircraft yaw angle. Accuracy, thereby improving the safety and stability of aircraft flight.

Among them, the idea of the present invention is: first, an attitude sensor component is provided on the aircraft, and the initial attitude information of the aircraft is collected through the attitude sensor component, and the attitude sensor component sends the attitude sensor component to the controller of the aircraft to Make the controller of the aircraft obtain the initial attitude information of the aircraft, wherein the initial attitude information of the aircraft includes the initial angular velocity of the aircraft and the initial angle of the aircraft; then, the controller of the aircraft acquires the angular velocity of the gimbal base; then , The controller of the aircraft can compare the obtained yaw angular velocity in the initial angular velocity of the aircraft with the yaw angular velocity in the angular velocity of the gimbal base; finally, based on the comparison result, the aircraft controller's The heading angle is corrected. Specifically, when the difference between the yaw angular velocity in the initial angular velocity of the aircraft and the yaw angular velocity in the angular velocity of the gimbal base is greater than or equal to a preset threshold, the initial angle of the aircraft is calculated based on the difference. To correct the yaw angle in Yaw angle of the aircraft.

In the embodiment of the present invention, the difference between the yaw angular velocity in the initial angular velocity of the aircraft and the yaw angular velocity in the angular velocity of the gimbal base is used to correct the yaw angle in the initial angle of the aircraft, which can effectively improve the estimated aircraft yaw The accuracy of the heading angle. This estimation method can avoid the interference of external factors, that is, it has high estimation accuracy under the environment of weak GPS signal and strong magnetic interference, thereby effectively improving the safety and stability of the aircraft flight.

The embodiments of the present invention will be further described below with reference to the accompanying drawings.

Example 1:

FIG. 1 is a schematic flowchart of an aircraft yaw angle correction method according to an embodiment of the present invention. The yaw correction method of the aircraft may be executed by various electronic devices with certain logic processing capabilities, such as an aircraft, a control chip, and the like. The aircraft may include a drone, an unmanned ship, and the like. The following electronic devices are described using an aircraft as an example. Among them, the aircraft is connected with a gimbal. The gimbal includes a gimbal motor and a gimbal base. Among them, the gimbal can be a multi-axis gimbal, such as a two-axis gimbal and a three-axis gimbal. The following three-axis gimbal is used as an example. Instructions. The aircraft is provided with an attitude sensor assembly. For the description of the specific structure of the aircraft and the gimbal, reference may be made to the above description, and therefore, it will not be repeated here.

Please refer to FIG. 1, the yaw angle correction method of the aircraft includes:

101: Obtain initial attitude information of the aircraft.

Obtaining the initial attitude information of the aircraft by the aircraft specifically includes: first acquiring the initial attitude information by an attitude sensor component provided on the aircraft, and sending the initial attitude information to the aircraft, so that the aircraft obtains the initial attitude information. The initial attitude information of the aircraft includes the initial angular velocity of the aircraft and the initial angle of the aircraft.

Among them, the attitude sensor component includes: a magnetometer, a first Inertial Measurement Unit (IMU), a GPS (Global Positioning System), and the like. The magnetometer can be used to collect the initial angle of the aircraft, and the first inertial measurement unit can be used to collect the initial angular velocity of the aircraft.

For the specific setting position of the attitude sensor assembly, refer to FIG. 2. Among them, FIG. 2 includes four coordinate systems: an aircraft coordinate system, a gimbal base coordinate system, a gimbal motor coordinate system, and a photographing device coordinate system. In Figure 2, the top of the aircraft is the fuselage of the aircraft. The attitude sensor assembly is located on the fuselage. The bottom of the aircraft's fuselage and the closest to the fuselage is the gimbal base. A vibrating ball (not shown) is connected. A gimbal motor is located at the lower end of the gimbal base, and a photographing device is positioned at the bottom. The photographing device is provided with a second inertial measurement unit for collecting the angular velocity of the photographing device. The gimbal base and the camera are connected by a three-axis motor in the ZXY Euler angle sequence, that is, from top to bottom, the yaw axis Yaw (Z axis), the roll axis Roll (X axis), and the pitch axis Pitch ( Y axis) three motor shafts.

Among them, the Inertial Measurement Unit (IMU) is a device that measures the three-axis attitude angle (or angular rate) and acceleration of an object. Generally, the IMU is a six-axis IMU. Among the six-axis IMUs, one IMU contains three single-axis accelerometers and three single-axis gyroscopes. The accelerometer detects the acceleration signals of the object in the carrier coordinate system independently of the three axes, and the gyroscope detects the relative Navigating the angular velocity signal of the coordinate system, measuring the angular velocity and acceleration of the object in three-dimensional space, and using this solution to calculate the attitude of the object.

The six-axis IMU can detect its own angle information in the inertial system. Specifically, the first inertial measurement unit provided on the aircraft acquires the initial angular velocity of the aircraft, and the initial angular velocity of the aircraft can be used as a vector.

Means the initial angular velocity of the aircraft

Represented as the coordinate vector of the angular velocity of the aircraft relative to the inertial system in the aircraft coordinate system, the initial angular velocity of the aircraft includes: the initial roll angular velocity of the aircraft, the initial pitch angular velocity of the aircraft, and the initial yaw angular velocity of the aircraft. among them,

Represents the initial roll angular velocity of the aircraft,

Represents the initial pitch angular velocity of the aircraft,

Expressed as the initial yaw rate of the aircraft. Similarly, the second inertial measurement unit provided on the photographing device acquires the angular velocity of the photographing device, and the angular velocity of the photographing device can be obtained by using a vector

Indicates the angular velocity of the camera

Expressed as the coordinate vector of the angular velocity of the imaging device relative to the inertial system in the imaging device coordinate system. among them,

Indicates the roll angular velocity of the camera,

Indicates the pitch angular velocity of the camera,

Expressed as the yaw rate of the camera. Among them, the inertial system, also known as the inertial coordinate system, the inertial reference system, the geodetic coordinate system, or the world coordinate system. Since the unmanned aerial vehicle can be placed at any position, a reference coordinate is selected to describe the The position of each part and use it to describe the position of any object in the environment. This coordinate system is called the inertial system.

Magnetometers, also called geomagnetism and magnetic sensors, can be used to test the strength and direction of magnetic fields and locate the orientation of equipment. Based on the advantages of low price, light weight, and compact structure of the magnetometer, it is widely used in the collection of aircraft angles. The initial angle of the aircraft is obtained through magnetometer acquisition. The initial angle of the aircraft can be represented by a vector (φ _p , θ _p , ψ _p ), that is, the initial angle of the aircraft (φ _p , θ _p , ψ _p ) is expressed as the relative of the aircraft. The coordinate vector of the angle of the inertial system in the aircraft coordinate system, where φ _p represents the initial roll angle of the aircraft, θ _p represents the initial pitch angular velocity of the aircraft, and ψ _p represents the initial yaw angle of the aircraft.

102. Obtain a yaw angular velocity of the aircraft according to an initial angular velocity of the aircraft, wherein the yaw angular velocity of the aircraft is a first yaw angular velocity.

For example, the initial angular velocity of the aircraft is

Then the yaw rate of the aircraft, that is, the first yaw rate is

103: Obtain the angular velocity of the gimbal base.

In order to avoid the phenomenon of gimbal lock during the attitude solution process, the quaternion is used to describe the attitude of the aircraft, the gimbal base, the gimbal motor, and the shooting device. Among them, the root cause of the universal joint lock phenomenon is that the rotation matrix is sequentially performed. It is assumed that the rotation is about the x-axis, then the y-axis, and finally the z-axis. This causes the object to actually rotate around its own coordinate system. The x-axis rotation is not the x-axis rotation of the inertial frame. The performance is that under an Euler angle (x1, y1, z1), changing the value of x1, the object will rotate around the x-axis of the object's own coordinate system, instead of the x-axis of the world's inertial system. Finally, when When the x-axis of the object is rotated to coincide with the z-axis of the inertial system, the x1 and z1 rotation results of the Euler angle are the same, and one dimension is lost. This is the universal joint lock phenomenon. To sum up, it can be said that a certain rotation around an axis of the object coordinate system, such as + (-) 90 degrees of the y axis, makes the previous rotation about the x axis of the object coordinate system and the rotation The two rotation axes of the last rotation around the z-axis of the object coordinate system are the same (the same means that in the inertial system, the two rotation axes are coaxial but opposite directions), resulting in a loss of rotational freedom, That is the gimbal lock phenomenon.

The system that uses three quantities to represent the orientation of the three-dimensional space will have the problem of the gimbal lock phenomenon, and the description by the quaternion can effectively avoid the gimbal lock phenomenon. Specifically, it can be assumed that the attitude quaternion of the aircraft relative to the inertial system is q _ip = [q _ip0 q _ip1 q _ip2 q _ip3 ] ^T , and the attitude quaternion of the _gimbal base relative to the inertial coordinate system is q _ib = [ q _ib0 q _ib1 q _ib2 q _ib3 ] ^T , the attitude quaternion of the _gimbal motor relative to the inertial coordinate system is q _bc = [q _bc0 q _bc1 q _bc2 q _bc3 ] ^T , the attitude of the camera relative to the inertial coordinate system is four The quaternion is q _ic = [q _ic0 q _ic1 q _ic2 q _ic3 ] ^T.

Obtaining the angular velocity of the gimbal base by the aircraft specifically includes: acquiring the angle of the gimbal motor; determining the angular velocity of the gimbal motor according to the angle of the gimbal motor; and acquiring the angular velocity of the photographing device, the The angular velocity of the photographing device is acquired by the inertial measurement unit; the angular velocity of the base of the gimbal is determined according to the angular velocity of the gimbal motor, the angle of the gimbal motor, and the angular velocity of the photographing device.

The specific process of the aircraft according to the angle of the gimbal motor may be: setting a linear Hall sensor on the gimbal motor, acquiring the angle of the gimbal motor through the linear hall sensor, and sending the angle of the gimbal motor to the aircraft So that the aircraft can obtain the angle of the gimbal motor.

In order to save costs and reduce the complexity of the algorithm for controlling the PTZ motor, usually the speed measurement element is not directly set on the PTZ motor, and the angular velocity of the PTZ motor cannot be directly measured. Therefore, in the embodiment of the present invention, in order to obtain the angular velocity of the gimbal motor, since the angle of the gimbal motor can be acquired by a linear Hall sensor, the angular velocity of the gimbal motor can be obtained by using a differentiator. Specifically, determining the angular velocity of the pan / tilt motor according to the angle of the pan / tilt motor includes: taking the angle of the pan / tilt motor as an input, and calculating the pan / tilt motor through a second-order discrete nonlinear tracking differentiator. Angular velocity.

The expression of the second-order discrete nonlinear tracking differentiator is:

r ₁ (k + 1) = r ₁ (k) + T · r ₂ (k)

r ₂ (k + 1) = r ₂ (k) + T · fst (r ₁ (k) -P (k), r ₂ (k), δ, h)

Among them, T is the sampling period for obtaining the angle of the pan / tilt motor, that is, the sampling period of the linear Hall sensor, usually T = 0.001s, P (k) = [φ (k) θ (k) ψ (k) ] ^T is the angle of the gimbal motor at time k, r ₁ (k) is the value determined by P (k) for tracking P (k) by the second-order discrete nonlinear tracking differentiator, r ₂ (k) is the derivative of P (k), k + 1 is the value corresponding to the k + 1th time, fst () is the highest speed control function, δ is the third parameter in the highest speed control function, and δ is used to determine Tracking speed, h is the fourth parameter in the highest speed control function. Among them, the fastest control function, also known as the fast control function, is the optimal control function that can complete the prescribed control action in the shortest time.

The expression of the fastest control function fst () is:

Among them, x ₁ is a parameter at the first position of the fastest control function, x ₂ is a parameter at the fourth position of the fastest control function, δ is a parameter at the third position of the fastest control function, and δ is used to determine the tracking speed. Based on the trade-off between tracking speed and noise amplification, and after experimental analysis, take δ = 20, h is the fourth parameter of the fastest control function.

sgn () is the step function, a is the operation parameter of the step function, and d = δ · T is the first operation parameter of the fastest control function. According to the noise characteristics of the linear Hall sensor, d = 0.5 and d ₀ = T · d is the second operation parameter of the fastest control function, and y = x ₁ + T · x ₂ is the third operation parameter of the fastest control function,

It is the fourth operation parameter of the fastest control function.

The functions that can be achieved by a second-order discrete nonlinear tracking differentiator are:

r ₁ (k) → P (k)

That is, the function implemented by the second-order discrete nonlinear tracking differentiator is to track P (k) through r ₁ (k), to obtain the differential r ₂ (k) of P (k), and the differential of P (k) is P derivative of (k)

The angular velocity of the gimbal motor

The aircraft determining the angular velocity of the gimbal base according to the angular velocity of the gimbal motor, the angle of the gimbal motor, and the angular velocity of the photographing device specifically includes: determining a rotation transformation matrix according to the angle of the gimbal motor The rotation transformation matrix is a rotation matrix of a gimbal base coordinate system to a gimbal motor coordinate system; the gimbal is determined according to an angular velocity of the gimbal motor, the rotation transformation matrix, and an angular velocity of the photographing device. The angular velocity of the base.

First, the rotation transformation matrix D is determined according to the angle (φ, θ, ψ) of the motor. Specifically, let R _z (ψ), R _x (φ), and R _y (θ) be unit rotation arrays rotating around the Z, X, and Y axes, respectively. According to the basic principles of inertial navigation, their R _z (ψ), R The values of _x (φ) and R _y (θ) are as follows:

The relationship between the angular velocity of the shooting device, the angular velocity of the gimbal motor and the angular velocity of the gimbal base can be described by the following dynamic equation of attitude:

among them,

Is the angular velocity of the gimbal base,

Is the angular velocity of the shooting device,

Is the angular velocity of the gimbal motor.

Based on the above equation, the calculation formula for determining the rotation transformation matrix according to the angle of the gimbal motor is:

Where D is the rotation transformation matrix; (φ, θ, ψ) is the angle of the motor, φ is the roll angle in the angle of the gimbal motor, and θ is the pitch in the angle of the gimbal motor. Angle, ψ is the yaw angle among the angles of the gimbal motor.

Then, according to the angular velocity r _{2 of} the gimbal motor, the rotation transformation matrix D, and the angular velocity of the imaging device

Determine the angular velocity of the gimbal base

Specifically, the formula for calculating the angular velocity of the gimbal base is:

among them,

Represents the angular velocity of the gimbal base. The angular velocity of the gimbal base includes the roll angular velocity of the gimbal base, the pitch angular velocity of the gimbal base, and the yaw angular velocity of the gimbal base. E.g,

then,

Represents the initial roll angular velocity of the aircraft,

Represents the initial pitch angular velocity of the aircraft,

Expressed as the initial yaw rate of the aircraft.

104: Obtain a yaw angular velocity of the gimbal base according to the angular velocity of the base of the gimbal, wherein the yaw angular velocity of the gimbal base is a second yaw angular velocity.

For example, the angular velocity of the gimbal base is

Then the yaw angular velocity of the gimbal base, that is, the second yaw angular velocity is

105. Compare the first yaw angular velocity

With the second yaw angular velocity

By comparing the first yaw angular velocity

With the second yaw angular velocity

You can determine the size relationship between the two, the difference between the two, and so on.

106: When the difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, the aircraft is determined according to the first yaw angular velocity and the second yaw angular velocity. The yaw angle of the aircraft is corrected to obtain a corrected yaw angle of the aircraft.

The preset threshold is determined by whether the aircraft receives a yaw operation instruction. Specifically, the preset threshold determined by whether the aircraft receives a yaw operation instruction includes:

When the aircraft receives a yaw operation instruction, the preset threshold is a first preset threshold;

When the aircraft does not receive a yaw operation instruction, the preset threshold is a second preset threshold, wherein the second preset threshold is smaller than the first preset threshold.

Usually during the flight of the aircraft, the yaw angle of the gimbal base moves in real time following the yaw angle of the aircraft, that is, the second yaw angular velocity

With first yaw rate

It is almost synchronous. Considering the yaw angle error rotation caused by unstable flight control during the flight of the aircraft,

versus

Satisfaction relationship:

Among them, ζ represents a preset threshold. When the user turns the course, that is, when there is a yaw operation, such as when the aircraft receives a yaw operation instruction generated by the user's yaw operation, because the aircraft has an acceleration movement in the heading direction, although the gimbal base tries to Keep up with the movement of the aircraft's yaw angle in time, but there is always a small lag, so at this time, the preset threshold is the first preset threshold, such as ζ = ζ ₁ = 5. Wherein, ζ ₁ represents a first preset threshold. When the user does not turn the heading stick, that is, when there is no yaw operation, the aircraft does not accelerate in the heading direction. At this time, the gimbal base can follow the heading motion of the aircraft in a timely manner, that is, the preset threshold value is Two preset thresholds, such as ζ = ζ ₁ = 2. Wherein, ζ ₂ represents a second preset threshold. And, the second preset threshold ζ _{2 is} smaller than the first preset threshold ζ ₁ .

If at some point the aircraft ’s magnetometer is subject to strong interference and the GPS signal is weak, GPS fails to perform an effective yaw angle correction. Rapid rotation of the gimbal base produces an angular velocity difference

Wherein, η represents a difference between the first yaw angular velocity and the second yaw angular velocity, and η ≧ ζ, it can be judged that the magnetometer is disturbed. At this time, the aircraft needs to be corrected.

Specifically, when the first yaw angular velocity

With the second yaw angular velocity

When the difference η is greater than or equal to the preset threshold ζ, the aircraft is based on the first yaw angular velocity

With the second yaw angular velocity

The yaw angle ψ _p in the initial angle of the aircraft is modified to obtain a corrected yaw angle ψ ′ _p of the aircraft.

Wherein, the aircraft is based on the first yaw angular velocity

With the second yaw angular velocity

Correcting the yaw angle ψ _p in the initial angle of the aircraft includes: according to the first yaw angular velocity

With the second yaw angular velocity

To obtain a yaw angle compensation value Δψ _p ; correct the yaw angle ψ _p in the initial angle of the aircraft according to the yaw angle compensation value Δψ _p .

The calculation formula for the yaw angle compensation value based on the difference between the first yaw angular velocity and the second yaw angular velocity is:

Among them, Δψ _p is the yaw angle compensation value, t ₀ is the time corresponding to obtaining the initial attitude information of the aircraft, and ΔT is the period for correcting the yaw angle in the initial angle of the aircraft. According to experience, usually ΔT = 5T, that is, yaw angle correction is performed once every 5 sampling intervals,

Is the first yaw angular velocity,

Is the second yaw angular velocity.

The calculation formula for the aircraft to obtain the revised yaw angle of the aircraft is:

ψ ′ _p = ψ _p + Δψ _p

Among them, ψ ′ _p is a modified yaw angle, ψ _p is a yaw angle in an initial angle of the aircraft, and Δψ _p is a yaw angle compensation value.

In the embodiment of the present invention, the yaw angle in the initial angle of the aircraft is corrected by the difference between the yaw angular speed in the initial angular speed of the aircraft and the yaw angular speed in the angular speed of the gimbal base, which can effectively improve The accuracy of estimating the yaw angle of the aircraft. This estimation method can avoid the interference of external factors, that is, it has a high estimation accuracy under the environment of weak GPS signals and strong magnetic interference, thereby effectively improving the safety and flight of the aircraft. stability.

Example 2:

FIG. 3 is a schematic diagram of an aircraft yaw angle correction device according to an embodiment of the present invention. The aircraft yaw correction device 30 may be configured in various electronic devices, such as an aircraft and a control chip. The aircraft may include an unmanned aerial vehicle, an unmanned ship, and the like. The following electronic devices are described using an aircraft as an example. The aircraft is connected with a gimbal. The gimbal includes a gimbal base, a gimbal motor connected to the gimbal base, and a camera device connected to the gimbal motor. The gimbal can be a multi-axis gimbal, such as a two-axis gimbal. PTZ and 3-axis PTZ. The following three-axis PTZ is used as an example. The aircraft is provided with an attitude sensor assembly. For the description of the specific structure of the aircraft and the gimbal, reference may be made to the above description, and therefore, it will not be repeated here.

Referring to FIG. 3, the aircraft yaw correction device 30 includes:

An initial attitude information acquisition module 301 is configured to obtain initial attitude information of the aircraft and to obtain a yaw angular velocity of the aircraft according to an initial angular velocity of the aircraft, wherein the yaw angular velocity of the aircraft is a first yaw Angular velocity.

Obtaining the initial attitude information of the aircraft by the initial attitude information acquisition module 301 specifically includes: first acquiring initial attitude information by an attitude sensor component provided on the aircraft, and sending the initial attitude information to the initial attitude information acquisition module 301, The initial posture information acquisition module 301 acquires the initial posture information. The initial attitude information of the aircraft includes the initial angular velocity of the aircraft and the initial angle of the aircraft. After obtaining the initial angular velocity of the aircraft, the first yaw angular velocity may be further obtained. For example, the initial angular velocity of the aircraft is

Then the first yaw angular velocity is

The base angular velocity obtaining module 302 is configured to obtain the angular velocity of the gimbal base and obtain the yaw angular velocity of the gimbal base according to the angular velocity of the gimbal base, wherein the gimbal base The yaw angular velocity is the second yaw angular velocity. The angular velocity of the gimbal base is

Then the second yaw angular velocity is

The pedestal angular velocity acquisition module 302 acquiring the angular velocity of the PTZ base specifically includes: acquiring the angle of the PTZ motor; determining the angular velocity of the PTZ motor according to the angle of the PTZ motor; The angular velocity of the photographing device is acquired by the inertial measurement unit; the angular velocity of the gimbal base is determined according to the angular velocity of the gimbal motor, the angle of the gimbal motor, and the angular velocity of the photographing device. Angular velocity.

The specific process of the base angular velocity acquisition module 302 according to the angle of the gimbal motor may be: setting a linear Hall sensor on the gimbal motor, acquiring the angle of the gimbal motor through the linear hall sensor, and integrating the gimbal motor The angle is sent to the base angular velocity acquisition module 302, so that the base angular velocity acquisition module 302 can obtain the angle of the gimbal motor.

In order to save costs and reduce the complexity of the algorithm for controlling the PTZ motor, usually the speed measurement element is not directly set on the PTZ motor, and the angular velocity of the PTZ motor cannot be directly measured. Therefore, in the embodiment of the present invention, in order to obtain the angular velocity of the gimbal motor, since the angle of the gimbal motor can be acquired by a linear Hall sensor, the angular velocity of the gimbal motor can be obtained by using a differentiator. Specifically, the base angular velocity acquisition module 302 determines the angular velocity of the pan / tilt motor according to the angle of the pan / tilt motor including: using the angle of the pan / tilt motor as an input, and calculating through a second-order discrete nonlinear tracking differentiator. Get the angular velocity of the gimbal motor.

The expression of the second-order discrete nonlinear tracking differentiator is:

r ₁ (k + 1) = r ₁ (k) + T · r ₂ (k)

r ₂ (k + 1) = r ₂ (k) + T · fst (r ₁ (k) -P (k), r ₂ (k), δ, h)

Among them, T is the sampling period for obtaining the angle of the pan / tilt motor, that is, the sampling period of the linear Hall sensor, usually T = 0.001s, P (k) = [φ (k) θ (k) ψ (k) ] ^T is the angle of the gimbal motor at time k, r ₁ (k) is the value determined by P (k) for tracking P (k) by the second-order discrete nonlinear tracking differentiator, r ₂ (k) is the derivative of P (k), k + 1 is the value corresponding to the k + 1th time, fst () is the highest speed control function, δ is the third parameter in the highest speed control function, and δ is used to determine Tracking speed, h is the fourth parameter in the highest speed control function.

The functions that can be achieved by a second-order discrete nonlinear tracking differentiator are:

r ₁ (k) → P (k)

That is, the function implemented by the second-order discrete nonlinear tracking differentiator is to track P (k) through r ₁ (k), to obtain the differential r ₂ (k) of P (k), and the differential of P (k) is P derivative of (k)

The angular velocity of the gimbal motor

The base angular velocity acquisition module 302 determines the angular velocity of the gimbal base according to the angular velocity of the gimbal motor, the angle of the gimbal motor, and the angular velocity of the photographing device, and specifically includes: The angle determines a rotation transformation matrix, the rotation transformation matrix being a rotation matrix of a gimbal base coordinate system to a gimbal motor coordinate system; according to the angular velocity of the gimbal motor, the rotation transformation matrix, and the angular velocity of the photographing device, Determine the angular velocity of the gimbal base.

The calculation formula of the base angular velocity obtaining module 302 to determine the rotation transformation matrix according to the angle of the gimbal motor is:

Where D is the rotation transformation matrix; (φ, θ, ψ) is the angle of the motor, φ is the roll angle in the angle of the gimbal motor, and θ is the pitch in the angle of the gimbal motor. Angle, ψ is the yaw angle among the angles of the gimbal motor.

Then, the base angular velocity acquisition module 302 is based on the angular velocity r _{2 of} the gimbal motor, the rotation transformation matrix D, and the angular velocity of the photographing device.

Determine the angular velocity of the gimbal base

Specifically, the formula for calculating the angular velocity of the pedestal angular velocity obtaining module 302 is:

The comparison module 303 is configured to compare the first yaw angular velocity with the second yaw angular velocity.

The first yaw angular velocity is compared by the comparison module 303

With second yaw rate

By comparison, you can determine the size relationship between the two, the difference between the two, and so on.

A correction module 304 is configured to, when a difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, according to the first yaw angular velocity and the second yaw angular velocity, Correct the yaw angle of the aircraft to obtain a corrected yaw angle of the aircraft.

A preset threshold determination module 305 is configured to determine the preset threshold according to a determination as to whether the aircraft has received a yaw operation instruction.

The preset threshold determination module 305 is specifically configured to: when the aircraft receives a yaw operation instruction, determine that the preset threshold is a first preset threshold; when the aircraft does not receive a yaw operation instruction, It is determined that the preset threshold is a second preset threshold, wherein the second preset threshold is smaller than the first preset threshold.

Usually during the flight of the aircraft, the yaw angle of the gimbal base moves in real time following the yaw angle of the aircraft, that is, the second yaw angular velocity

With first yaw rate

It is almost synchronous. Considering the yaw angle error rotation caused by unstable flight control during the flight of the aircraft,

versus

Satisfaction relationship:

Among them, ζ represents a preset threshold. When the user turns the yaw, that is, when there is a yaw operation, because the aircraft has an acceleration movement in the heading direction, although the gimbal base tries to keep up with the movement of the aircraft's yaw angle, there is always a small lag. Therefore, at this time, the preset threshold determination module 305 determines the preset threshold as the first preset threshold, such as ζ = ζ ₁ = 5. Wherein, ζ ₁ represents a first preset threshold. When the user does not turn the heading stick, that is, when there is no yaw operation, the aircraft does not accelerate in the heading direction. At this time, the gimbal base can follow the heading motion of the aircraft in a timely manner. The preset threshold is determined as a second preset threshold, such as ζ = ζ ₁ = 2. Wherein, ζ ₂ represents a second preset threshold. And, the second preset threshold ζ _{2 is} smaller than the first preset threshold ζ ₁ .

If at some point, the aircraft's magnetometer is subject to strong interference and the GPS signal is weak, and GPS fails to perform an effective yaw angle correction, the yaw angle estimation of the aircraft will suddenly be wrong, with the result that the aircraft will generate a relative Rapid rotation of the gimbal base produces an angular velocity difference

Wherein, η represents a difference between the first yaw angular velocity and the second yaw angular velocity, and η ≧ ζ, it can be judged that the magnetometer is disturbed. At this time, the correction module 304 needs to correct the yaw angle of the aircraft.

Specifically, when the first yaw angular velocity

With the second yaw angular velocity

When the difference η is greater than or equal to the preset threshold ζ, the correction module 304 is based on the first yaw angular velocity.

With the second yaw angular velocity

The yaw angle ψ _p in the initial angle of the aircraft is modified to obtain a corrected yaw angle ψ ′ _p of the aircraft.

Wherein, the correction module 304 is based on the first yaw angular velocity.

With the second yaw angular velocity

Correcting the yaw angle ψ _p in the initial angle of the aircraft includes: according to the first yaw angular velocity

With the second yaw angular velocity

To obtain a yaw angle compensation value Δψ _p ; correct the yaw angle ψ _p in the initial angle of the aircraft according to the yaw angle compensation value Δψ _p .

The correction module 304 calculates the yaw angle compensation value according to the difference between the first yaw angular velocity and the second yaw angular velocity as:

Among them, Δψ _p is the yaw angle compensation value, t ₀ is the time corresponding to obtaining the initial attitude information of the aircraft, and ΔT is the period for correcting the yaw angle in the initial angle of the aircraft. According to experience, usually ΔT = 5T, that is, yaw angle correction is performed once every 5 sampling intervals,

Is the first yaw angular velocity,

Is the second yaw angular velocity.

The correction formula for the yaw angle of the aircraft obtained by the correction module 304 is:

ψ ′ _p = ψ _p + Δψ _p

Among them, ψ ′ _p is a modified yaw angle, ψ _p is a yaw angle in an initial angle of the aircraft, and Δψ _p is a yaw angle compensation value.

It should be noted that, in the embodiment of the present invention, the aircraft yaw angle correction device 30 can execute the aircraft yaw angle correction method provided by the method embodiment, and has corresponding function modules and beneficial effects of the execution method. For technical details that are not described in detail in the embodiment of the aircraft yaw angle correction device 30, reference may be made to the aircraft yaw angle correction method provided in the embodiment of the method invention.

Example 3:

FIG. 4 is a schematic diagram of an aircraft hardware structure according to an embodiment of the present invention. As shown in FIG. 4, the aircraft 40 includes:

One or more processors 401 and a memory 402. One processor 401 is taken as an example in FIG. 4.

The processor 401 and the memory 402 may be connected through a bus or other manners. In FIG. 4, the connection through the bus is taken as an example.

The memory 402 is a non-volatile computer-readable storage medium, and can be used to store non-volatile software programs, non-volatile computer executable programs, and modules. Program instructions / modules (for example, the initial attitude information acquisition module 301, the base angular velocity acquisition module 302, the comparison module 303, the correction module 304, and the preset threshold determination module 405 shown in FIG. 3). The processor 401 executes various functional applications and data processing of the aircraft by running non-volatile software programs, instructions, and units stored in the memory 402, that is, an aircraft yaw angle correction method of the method embodiment is implemented.

The memory 402 may include a storage program area and a storage data area, wherein the storage program area may store an operating system and application programs required for at least one function; the storage data area may store data created according to aircraft use, and the like. In addition, the memory 402 may include a high-speed random access memory, and may further include a non-volatile memory, such as at least one magnetic disk storage device, a flash memory device, or other non-volatile solid-state storage device. In some embodiments, the memory 402 may optionally include a memory remotely set relative to the processor 401, and these remote memories may be connected to the aircraft through a network. Examples of the network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

The one or more units are stored in the memory 402, and when executed by the one or more processors 401, execute the aircraft yaw angle correction method in the method embodiment, for example, execute the above-described The method steps 101 to 104 in FIG. 1 implement the functions of the modules 301-305 in FIG. 3.

5 and FIG. 6, the aircraft 40 further includes a power unit 403. The power unit 403 is used to provide flight power for the aircraft. The power unit 403 is connected to the processor 401. The power device 403 includes a driving motor 4031 and an ESC 4032. The ESC 4032 is electrically connected to the driving motor 4031 and is used to control the driving motor 4031. Specifically, the ESC 4032 generates a control instruction based on the corrected yaw angle of the aircraft obtained after the processor 401 executes the aircraft yaw angle correction method, and controls the driving motor 4032 through the control instruction.

The aircraft 40 can execute the aircraft yaw angle correction method provided in Embodiment 1 of the present invention, and has the corresponding functional modules and beneficial effects of the execution method. For technical details that are not described in detail in the embodiment of the aircraft, reference may be made to the aircraft yaw angle correction method provided in Embodiment 1 of the present invention.

An embodiment of the present invention provides a computer program product. The computer program product includes a computer program stored on a non-volatile computer-readable storage medium. The computer program includes program instructions. When the program instructions are executed by a computer, At that time, the computer is caused to execute the aircraft yaw angle correction method as described above. For example, steps 101 to 104 of the method in FIG. 1 described above are performed to implement the functions of the modules 301-305 in FIG. 3.

An embodiment of the present invention provides a non-volatile computer-readable storage medium. The computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to cause a computer to execute the aircraft yaw as described above. Angle correction method. For example, steps 101 to 104 of the method in FIG. 1 described above are performed to implement the functions of the modules 301-305 in FIG. 3.

It should be noted that the device embodiments described above are only schematic, and the modules described as separate components may or may not be physically separated, and the components displayed as modules may or may not be physical Modules can be located in one place or distributed to multiple network modules. Some or all of the modules may be selected according to actual needs to achieve the objective of the solution of this embodiment.

Through the description of the above embodiments, those skilled in the art can clearly understand that the embodiments can be implemented by means of software plus a general hardware platform, and of course, they can also be implemented by hardware. Those of ordinary skill in the art can understand that all or part of the processes in the method of the embodiment can be completed by computer program instructions related hardware. The program can be stored in a computer-readable storage medium, and the program is being executed. In this case, the process of the embodiment of each method may be included. The storage medium may be a magnetic disk, an optical disc, a read-only memory (Read-Only Memory, ROM), or a random access memory (Random, Access Memory, RAM).

Finally, it should be noted that the above embodiments are only used to describe the technical solution of the present invention, but not limited thereto; under the idea of the present invention, the technical features in the above embodiments or different embodiments can also be combined, The steps can be implemented in any order, and there are many other variations of the different aspects of the invention as described above, for the sake of brevity, they are not provided in the details; although the invention has been described in detail with reference to the foregoing embodiments, it is common in the art The skilled person should understand that it can still modify the technical solutions described in the foregoing embodiments, or equivalently replace some of the technical features; and these modifications or replacements do not deviate from the essence of the corresponding technical solutions from the implementation of the present invention. Examples of technical solutions.

Claims (25)

1. An aircraft yaw angle correction method, characterized in that the aircraft is connected with a gimbal, the gimbal comprises a gimbal base, a gimbal motor connected to the gimbal base, and a gimbal motor A connected shooting device, the method includes:

   Acquiring initial attitude information of the aircraft, where the initial attitude information of the aircraft includes an initial angular velocity of the aircraft;

   Obtaining the yaw angular velocity of the aircraft according to the initial angular velocity of the aircraft, wherein the yaw angular velocity of the aircraft is a first yaw angular velocity;

   Obtaining the angular velocity of the gimbal base;

   Obtaining the yaw angular velocity of the gimbal base according to the angular velocity of the gimbal base, wherein the yaw angular velocity of the gimbal base is a second yaw angular velocity;

   Comparing the first yaw angular velocity with the second yaw angular velocity;

   When the difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, the yaw of the aircraft is based on the first yaw angular velocity and the second yaw angular velocity. The heading angle is corrected to obtain a corrected yaw angle of the aircraft.
2. The method according to claim 1, wherein the preset threshold is determined by whether the aircraft receives a yaw operation instruction.
3. The method according to claim 2, wherein the preset threshold is determined by whether the aircraft receives a yaw operation instruction, comprising:

   When the aircraft receives a yaw operation instruction, the preset threshold is a first preset threshold;

   When the aircraft does not receive a yaw operation instruction, the preset threshold is a second preset threshold.
4. The method according to any one of claims 1 to 3, wherein when a difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, the according to Correcting the yaw angle of the aircraft by the first yaw angular velocity and the second yaw angular velocity includes:

   Obtaining a yaw angle compensation value according to a difference between the first yaw angular velocity and the second yaw angular velocity;

   Correct the yaw angle of the aircraft according to the yaw angle compensation value.
5. The method as claimed in claim 4, wherein said yaw angle Δψ _p compensation value is:

   

   Where t ₀ is the time corresponding to the initial attitude information of the aircraft, and ΔT is the period for correcting the yaw angle of the aircraft,

   

   Is the first yaw angular velocity,

   

   Is the second yaw angular velocity.
6. The method according to claim 5, characterized in that the yaw angle ψ ' _p of the aircraft after the correction is:

   ψ ′ _p = ψ _p + Δψ _p

   Wherein, ψ _p is the yaw angle of the aircraft, and Δψ _p is the yaw angle compensation value.
7. The method according to claim 1, wherein the obtaining the angular velocity of the gimbal base comprises:

   Obtaining an angle of the gimbal motor;

   Determining the angular velocity of the gimbal motor according to the angle of the gimbal motor;

   Obtaining an angular velocity of the photographing device;

   Determine the angular velocity of the pan-tilt base according to the angular velocity of the pan-tilt motor, the angle of the pan-tilt motor, and the angular velocity of the photographing device.
8. The method according to claim 7, wherein determining the angular velocity of the gimbal motor according to the angle of the gimbal motor comprises:

   Taking the angle of the gimbal motor as an input, the angular velocity of the gimbal motor is calculated by a second-order discrete nonlinear tracking differentiator.
9. The method according to claim 8, wherein the expression of the second-order discrete nonlinear tracking differentiator is:

   r ₁ (k + 1) = r ₁ (k) + T · r ₂ (k)

   r ₂ (k + 1) = r ₂ (k) + T · fst (r ₁ (k) -P (k), r ₂ (k), δ, h)

   Among them, T is a sampling period for obtaining the angle of the gimbal motor, and P (k) = [φ (k) θ (k) ψ (k)] ^T is the angle of the gimbal motor at the k-th time, r ₁ (k) is a value determined by P (k) for tracking P (k) by the second-order discrete nonlinear tracking differentiator, r ₂ (k) is a derivative of P (k), and k + 1 is the first The value corresponding to time k + 1, fst () is the highest speed control function, δ is a parameter located at the third position of the highest speed control function, and h is a parameter located at the fourth position of the fastest control function.
10. The method according to any one of claims 7 to 9, wherein the determining of the gimbal base is based on an angular velocity of the gimbal motor, an angle of the gimbal motor, and an angular velocity of the photographing device. The angular velocity of the seat includes:

    Determining a rotation transformation matrix according to the angle of the gimbal motor, the rotation transformation matrix being a rotation matrix of a gimbal base coordinate system to a gimbal motor coordinate system;

    Determine the angular velocity of the pan-tilt base according to the angular velocity of the pan-tilt motor, the rotation transformation matrix, and the angular velocity of the photographing device.
11. The method according to claim 10, wherein the calculation formula of the rotation transformation matrix is:

    

    Where D is the rotation transformation matrix; (φ, θ, ψ) is the angle of the gimbal motor, φ is the roll angle in the angle of the gimbal motor, and θ is the pitch in the angle of the gimbal motor. Angle, ψ is the yaw angle among the angles of the gimbal motor.
12. The method according to claim 11, wherein the calculation formula of the angular velocity of the gimbal base is:

    

    among them,

    

    Is the angular velocity of the gimbal base,

    

    Is the angular velocity of the photographing device, D is the rotation transformation matrix, and r ₂ is the angular velocity of the gimbal motor.
13. An aircraft yaw angle correction device, characterized in that the aircraft is connected with a gimbal, the gimbal includes a gimbal base, a gimbal motor connected to the gimbal base, and the gimbal motor A connected shooting device, the device comprising:

    An initial attitude information acquisition module, configured to obtain initial attitude information of the aircraft, wherein the initial attitude information of the aircraft includes an initial angular velocity of the aircraft; and

    Configured to obtain a yaw angular velocity of the aircraft according to an initial angular velocity of the aircraft, wherein the yaw angular velocity of the aircraft is a first yaw angular velocity;

    The base angular velocity obtaining module is configured to obtain the angular velocity of the gimbal base and to obtain the yaw angular velocity of the gimbal base according to the angular velocity of the gimbal base, wherein the The yaw angular velocity is the second yaw angular velocity;

    A comparison module, configured to compare the first yaw angular velocity with the second yaw angular velocity;

    A correction module, configured to: when the comparison module determines that a difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, according to the first yaw angular velocity and the second yaw Angular velocity, correcting the yaw angle of the aircraft to obtain a corrected yaw angle of the aircraft.
14. The device according to claim 13, further comprising a preset threshold determining module, configured to determine the preset threshold according to whether the aircraft receives a yaw operation instruction.
15. The apparatus according to claim 14, wherein the preset threshold determination module is specifically configured to:

    When the aircraft receives a yaw operation instruction, determine that the preset threshold is a first preset threshold;

    When the aircraft does not receive a yaw operation instruction, it is determined that the preset threshold is a second preset threshold.
16. The device according to any one of claims 13 to 15, wherein when a difference between the first yaw angular velocity and the second yaw angular velocity is greater than or equal to a preset threshold, the correction module is based on Correcting the yaw angle of the aircraft by the first yaw angular velocity and the second yaw angular velocity includes:

    Obtaining a yaw angle compensation value according to a difference between the first yaw angular velocity and the second yaw angular velocity;

    Correct the yaw angle of the aircraft according to the yaw angle compensation value.
17. The method according to claim 16, wherein said yaw angle Δψ _p compensation value is:

    

    Where t ₀ is the time corresponding to the initial attitude information of the aircraft, and ΔT is the period for correcting the yaw angle of the aircraft,

    

    Is the first yaw angular velocity,

    

    Is the second yaw angular velocity.
18. The method according to claim 17, wherein the corrected yaw angle ψ ' _p of the aircraft is:

    ψ ′ _p = ψ _p + Δψ _p

    Wherein, ψ _p is the yaw angle of the aircraft, and Δψ _p is the yaw angle compensation value.
19. The device according to claim 13, wherein:

    The base angular velocity obtaining module is specifically configured to:

    Obtaining an angle of the gimbal motor;

    Determining the angular velocity of the gimbal motor according to the angle of the gimbal motor;

    Obtaining an angular velocity of the photographing device;

    Determine the angular velocity of the pan-tilt base according to the angular velocity of the pan-tilt motor, the angle of the pan-tilt motor, and the angular velocity of the photographing device.
20. The apparatus according to claim 19, wherein the base angular velocity obtaining module determines an angular velocity of the pan / tilt motor according to an angle of the pan / tilt motor, comprising:

    Taking the angle of the gimbal motor as an input, the angular velocity of the gimbal motor is calculated by a second-order discrete nonlinear tracking differentiator.
21. The method according to claim 20, wherein the expression of the second-order discrete nonlinear tracking differentiator is:

    r ₁ (k + 1) = r ₁ (k) + T · r ₂ (k)

    r ₂ (k + 1) = r ₂ (k) + T · fst (r ₁ (k) -P (k), r ₂ (k), δ, h)

    Among them, T is a sampling period for obtaining the angle of the gimbal motor, and P (k) = [φ (k) θ (k) ψ (k)] ^T is the angle of the gimbal motor at the k-th time, r ₁ (k) is a value determined by P (k) for tracking P (k) by the second-order discrete nonlinear tracking differentiator, r ₂ (k) is a derivative of P (k), and k + 1 is the first The value corresponding to time k + 1, fst () is the highest speed control function, δ is a parameter located at the third position of the highest speed control function, and h is a parameter located at the fourth position of the highest speed control function.
22. The device according to any one of claims 19 to 21, wherein the base angular velocity acquisition module is based on the angular velocity of the gimbal motor, the angle of the gimbal motor, and the angular velocity of the photographing device, Determining the angular velocity of the gimbal base includes:

    Determining a rotation transformation matrix according to the angle of the gimbal motor, the rotation transformation matrix being a rotation matrix of a gimbal base coordinate system to a gimbal motor coordinate system;

    Determine the angular velocity of the pan-tilt base according to the angular velocity of the pan-tilt motor, the rotation transformation matrix, and the angular velocity of the photographing device.
23. The method according to claim 22, wherein the calculation formula of the rotation transformation matrix is:

    

    Where D is the rotation transformation matrix; (φ, θ, ψ) is the angle of the PTZ motor, ^φ is the roll angle in the angle of the PTZ motor, and θ is the pitch in the angle of the PTZ motor Angle, ψ is the yaw angle among the angles of the gimbal motor.
24. The method according to claim 23, wherein the calculation formula of the angular velocity of the gimbal base is:

    

    among them,

    

    Is the angular velocity of the gimbal base,

    

    Is the angular velocity of the photographing device, D is the rotation transformation matrix, and r ₂ is the angular velocity of the gimbal motor.
25. An aircraft is characterized by comprising:

    body;

    A machine arm connected to the fuselage;

    A power unit provided on the machine arm;

    At least one processor provided in the fuselage; and

    A memory connected in communication with the at least one processor; wherein,

    The memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor, so that the at least one processor can execute any one of claims 1-12. Methods.

Images