Automatic driving platform of small unmanned helicopter
By using high-precision sensors, rotary quaternion method, complementary filtering algorithm and cascade PID controller on the small unmanned helicopter autonomous driving platform, dynamic coupling, sensor accuracy and real-time problems in attitude control are solved, and high-precision and real-time attitude control is achieved.
Patent Information
- Application Number
- CN202510081321.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-01-16
- Publication Date
- 2025-05-06
AI Technical Summary
In attitude control, small unmanned helicopters have problems such as obvious dynamic coupling characteristics, limited sensor accuracy and poor real-time attitude solution.
The high-precision micromechanical sensor module is used to obtain angular velocity, acceleration and inclination data in real time, combined with the attitude solution module, the rotation quaternion method and the fourth-order Longge-Kutta numerical integral method are used to perform attitude solution, and the multi-sensor data is fused based on the complementary filtering algorithm through the data fusion module. At the same time, a cascade PID controller is used for attitude control, and the three-axis attitude independent control is achieved through the PWM signal driving the servo.
It significantly improves the accuracy and real-time performance of attitude solution, enhances the system's dynamic response performance and disturbance resistance, and ensures the flight stability and control accuracy of the unmanned helicopter in complex environments.
Smart Images
Figure CN119937402A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the technical field of automatic driving of small unmanned helicopters, and in particular to an automatic driving platform for small unmanned helicopters. Background Art
[0002] With the rapid development of UAV technology, the application of UAV in military, agriculture, surveying and mapping, logistics and other fields is gradually increasing. Among them, the autopilot platform, as the core control system of the UAV, directly determines the flight performance and mission execution capability of the UAV. Especially in the field of small unmanned helicopters, due to their complex flight attitude and high dynamic response requirements, the autopilot platform needs to have high-precision attitude solution and control functions to ensure the stability and safety of the aircraft in various working environments.
[0003] At present, for the automatic driving control of small unmanned helicopters, the traditional PID control algorithm is usually used to achieve the control of attitude angle and angular velocity. However, due to the coupling characteristics between the helicopter attitude angles, the traditional PID algorithm is difficult to effectively deal with the dynamic coupling problem of attitude control, resulting in slow dynamic response and poor anti-disturbance ability of the control system. In addition, during the attitude solution process, due to the limited accuracy of micromechanical (MEMS) sensors, the measurement data is easily affected by noise and environmental interference, which reduces the accuracy and reliability of attitude solution. At the same time, the traditional attitude solution has a large amount of calculation and poor real-time performance, which is difficult to meet the high real-time attitude control requirements of small unmanned helicopters.
[0004] Therefore, in the attitude control of small unmanned helicopters, the obvious dynamic coupling characteristics, limited sensor accuracy and poor real-time performance of attitude solution have become problems that need to be solved urgently. Summary of the invention
[0005] The present application provides an automatic driving platform for a small unmanned helicopter, aiming to solve the problems of obvious dynamic coupling characteristics, limited sensor accuracy and poor real-time performance of attitude solution in the attitude control of existing small unmanned helicopters.
[0006] A small unmanned helicopter automatic driving platform, the platform comprising:
[0007] High-precision micromechanical sensor modules for obtaining real-time angular velocity, acceleration, and inclination data of small unmanned helicopters;
[0008] The attitude calculation module is used to calculate the attitude through the rotation quaternion method and update the rotation quaternion in real time;
[0009] A data fusion module, used for fusing the angular velocity, acceleration and inclination data;
[0010] An attitude control module includes a cascade PID controller, an outer loop for calculating an angular velocity reference value according to an attitude angle error, and an inner loop for calculating a control signal according to the angular velocity error;
[0011] The actuator control module is used to drive the steering gear according to the control signal to adjust the pitch, roll and yaw attitude of the small unmanned helicopter.
[0012] In the above solution, optionally, the attitude calculation module performs attitude calculation based on the rotation quaternion method, specifically using the following formula:
[0013]
[0014] In the above formula, is the derivative of the state vector, which indicates the change of the system state over time and is used to describe the change of the object's posture; λ is the real part of the quaternion, p 1 、p 2 、p 3 is the imaginary part of the quaternion, i.e. q = λ + p 1 i+p 2 j+p 3 k;ω=ω x i+ω y j+ω z k is the projection of the angular velocity of the carrier coordinate system relative to the navigation coordinate system in the carrier coordinate system, ω x ,ω y ,ω z are the rotational velocity components of the angular velocity on the x-axis, y-axis, and z-axis.
[0015] In the above solution, optionally, the posture calculation module performs coordinate transformation on the angular velocity to obtain the following formula:
[0016]
[0017] Among them, ω x ,ω y ,ω z is the angular velocity component in the carrier coordinate system, [ω xx ω yy ω zz ] T is the gyro output angular velocity, is the attitude matrix, [ω 1 ω 2 ω 3 ] T It is the sum of the projection of the earth's rotation angular velocity in the navigation coordinate system and the projection of the navigation coordinate system's rotation angular velocity relative to the earth's coordinate system in the navigation coordinate system.
[0018] In the above solution, optionally, the attitude solving module integrates the quaternion derivative by a fourth-order Runge-Kutta method to update the quaternion in real time.
[0019] In the above solution, optionally, the attitude calculation module normalizes the updated quaternion to ensure that the norm of the quaternion is 1.
[0020] In the above solution, optionally, the data fusion module uses a complementary filtering algorithm to fuse the gyroscope, accelerometer and inclinometer data to correct the attitude solution error.
[0021] In the above scheme, optionally, the cascade PID controller includes:
[0022] The outer loop PID controller is used to calculate the angular velocity reference value according to the attitude angle error;
[0023] The inner loop PID controller is used to calculate the output control signal based on the angular velocity error.
[0024] In the above scheme, optionally, the calculation period of the inner loop and the outer loop of the cascade PID controller are both 50 Hz.
[0025] In the above solution, optionally, the actuator control module drives the servo through a PWM signal to adjust the pitch, roll and yaw attitude of the small unmanned helicopter.
[0026] In the above solution, optionally, the platform is suitable for solving the coupling problem in the three-axis attitude control of pitch, roll and yaw.
[0027] Compared with the prior art, this application has at least the following beneficial effects:
[0028] Based on further analysis and research of the existing technical problems, this application recognizes that the attitude control of the existing small unmanned helicopter has obvious dynamic coupling characteristics, limited sensor accuracy and poor real-time performance of attitude solution. The high-precision micromechanical sensor module is used to obtain angular velocity, acceleration and inclination data in real time. The attitude solution module adopts the rotation quaternion method and the fourth-order Runge-Kutta numerical integration method to update the quaternion in real time and perform normalization processing, which effectively improves the accuracy and real-time performance of attitude solution and solves the problems of insufficient real-time performance and low accuracy of attitude solution in the background technology. The data fusion module fuses the gyroscope, accelerometer and inclinometer data based on the complementary filtering algorithm, corrects the attitude solution error, compensates for the defects of low sensor accuracy and susceptibility to noise interference, and ensures the long-term stability and dynamic accuracy of attitude information. The attitude control module adopts a cascade PID controller to hierarchically process the outer loop attitude angle control and the inner loop angular velocity control, solves the dynamic coupling problem in attitude control, and significantly improves the dynamic response performance and anti-disturbance ability of the system. The actuator control module drives the servo through PWM signals to achieve independent control of the three-axis attitude of pitch, roll and yaw, ensuring the flight stability and control accuracy of the unmanned helicopter in complex environments.
[0029] In summary, the present invention effectively solves the problems mentioned in the background technology, such as low sensor accuracy, poor real-time performance of attitude solution, and dynamic coupling of attitude control, and significantly improves the stability, accuracy, and real-time response performance of the unmanned helicopter automatic driving platform. BRIEF DESCRIPTION OF THE DRAWINGS
[0030] Figure 1 A block diagram of the module architecture of a small unmanned helicopter automatic driving platform device provided in one embodiment of the present application;
[0031] Figure 2 A schematic diagram of a serial digital PID controller provided for one embodiment of the present application. DETAILED DESCRIPTION
[0032] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.
[0033] In one embodiment, Figure 1 As shown, a small unmanned helicopter automatic driving platform is provided, including:
[0034] High-precision micromechanical sensor modules for obtaining real-time angular velocity, acceleration, and inclination data of small unmanned helicopters;
[0035] The attitude calculation module is used to calculate the attitude through the rotation quaternion method and update the rotation quaternion in real time;
[0036] A data fusion module, used for fusing the angular velocity, acceleration and inclination data;
[0037] An attitude control module includes a cascade PID controller, an outer loop for calculating an angular velocity reference value according to an attitude angle error, and an inner loop for calculating a control signal according to the angular velocity error;
[0038] The actuator control module is used to drive the steering gear according to the control signal to adjust the pitch, roll and yaw attitude of the small unmanned helicopter.
[0039] The high-precision micromechanical sensor module of this embodiment includes a gyroscope, an accelerometer and an inclinometer, which are mainly used to obtain the real-time angular velocity, acceleration and inclination data of a small unmanned helicopter. These sensors have different measurement functions:
[0040] The gyroscope provides the angular velocity information of the aircraft and is suitable for short-term dynamic attitude detection;
[0041] Accelerometers and inclinometers provide attitude angle data, which are suitable for long-term static attitude stability detection.
[0042] This embodiment can collect the status data of the aircraft in real time through the high-precision sensor module, and provide high dynamic performance and high-precision attitude angle and angular velocity measurement. Although MEMS sensors have certain limitations in accuracy, they can effectively reduce sensor noise interference and error effects when used in combination with subsequent data fusion modules, solving the problems of low sensor accuracy and susceptibility to interference mentioned in the background technology.
[0043] In this embodiment, the attitude calculation module performs attitude calculation based on the rotation quaternion method, specifically using the following formula:
[0044]
[0045] In the above formula, is the derivative of the state vector, which indicates the change of the system state over time and is used to describe the change of the object's posture; λ is the real part of the quaternion, p 1 、p 2 、p 3 is the imaginary part of the quaternion, i.e. q = λ + p 1 i+p 2 j+p 3 k;ω=ω x i+ω y j+ω z k is the projection of the angular velocity of the carrier coordinate system relative to the navigation coordinate system in the carrier coordinate system, ω x ,ω y ,ω zare the rotational velocity components of the angular velocity on the x-axis, y-axis, and z-axis.
[0046] In this embodiment, the posture calculation module performs coordinate transformation on the angular velocity to obtain the following formula:
[0047]
[0048] Among them, ω x ,ω y ,ω z is the angular velocity component in the carrier coordinate system, [ω xx ω yy ω zz ] T is the gyro output angular velocity, is the attitude matrix, [ω 1 ω 2 ω 3 ] T It is the sum of the projection of the earth's rotation angular velocity in the navigation coordinate system and the projection of the navigation coordinate system's rotation angular velocity relative to the earth's coordinate system in the navigation coordinate system.
[0049] In this embodiment, the attitude calculation module integrates the quaternion derivative by the fourth-order Runge-Kutta method to update the quaternion in real time.
[0050] In this embodiment, the attitude calculation module normalizes the updated quaternion to ensure that the norm of the quaternion is 1.
[0051] In this embodiment, the data fusion module uses a complementary filtering algorithm to fuse the gyroscope, accelerometer and inclinometer data to correct the attitude solution error.
[0052] In this embodiment, the cascade PID controller includes:
[0053] The outer loop PID controller is used to calculate the angular velocity reference value according to the attitude angle error;
[0054] The inner loop PID controller is used to calculate the output control signal based on the angular velocity error.
[0055] In this embodiment, the calculation cycles of the inner loop and the outer loop of the cascade PID controller are both 50 Hz.
[0056] In this embodiment, the actuator control module drives the servo through a PWM signal to adjust the pitch, roll and yaw attitude of the small unmanned helicopter.
[0057] In this embodiment, the platform is suitable for solving the coupling problem in the three-axis attitude control of pitch, roll and yaw.
[0058] This embodiment uses the rotation quaternion method for attitude solution, which can effectively solve the singularity problem that occurs when the traditional Euler angle method changes at large angles. In particular, when the aircraft performs complex attitude maneuvers, the attitude information can still maintain continuity and accuracy. The quaternion method avoids redundant calculations in three-dimensional coordinate rotation, thereby greatly reducing the amount of calculations in the solution process, allowing the system to run efficiently in resource-constrained embedded environments. In addition, since the formula is directly updated based on the angular velocity component, the rotation quaternion method can quickly respond to changes in attitude angles, meeting the high requirements of small unmanned helicopters for real-time performance and dynamic accuracy.
[0059] Combined with the Runge-Kutta method for numerical integration, the calculation accuracy of quaternion updates can be significantly improved, the cumulative error can be reduced, and the stability and consistency of attitude solution results during long-term flight can be ensured. In addition, since the unit of the quaternion is maintained, there will be no numerical divergence problem during the solution process, thereby improving the reliability of the system. In general, through the attitude solution method, the small unmanned helicopter automatic driving platform can achieve high-precision and real-time attitude information output in a high-dynamic flight environment, effectively solving the problems of poor real-time performance and low accuracy of attitude solution in the background technology.
[0060] This embodiment solves the coordinate mismatch problem between the angular velocity output by the MEMS sensor and the attitude solution model by performing coordinate transformation on the angular velocity data, ensuring that the attitude solution module can accurately process the data output by the sensor. Since there is a constantly changing relationship between the navigation coordinate system and the carrier coordinate system during flight, the coordinate system transformation process can dynamically adapt to the real-time changes in the system attitude, further improving the accuracy of the solution results.
[0061] In addition, the coordinate transformation formula can eliminate the influence caused by sensor installation error or coordinate system offset, ensure the uniformity and consistency of angular velocity data, and provide reliable input data for attitude solution. Combined with high-precision numerical calculation and filtering optimization technology, the coordinate system transformation process will not introduce additional errors, ensuring that the system has high calculation accuracy and stability in real-time operation. Overall, the angular velocity coordinate transformation method effectively solves the problem of coordinate differences between sensor data and solution models, improves the accuracy and real-time performance of attitude solution, and meets the requirements of small unmanned helicopters in high-dynamic flight missions.
[0062] This embodiment can effectively improve the real-time performance and control coordination of the system by setting the calculation period of the inner and outer loops of the cascade PID controller to 50Hz. The outer loop attitude angle control and the inner loop angular velocity control are updated in the same time step, ensuring the synchronization of data transmission and response between the two control loops, thereby avoiding lag in the control process. In addition, the unified calculation period simplifies the design and implementation of the controller, reduces the complexity of the system, and helps to run stably in an embedded environment.
[0063] The 50Hz calculation cycle meets the real-time control requirements of small unmanned helicopters, and can quickly respond to attitude changes during dynamic flight, ensuring the real-time and stability of attitude control. At the same time, this calculation frequency also balances the control accuracy of the system and the consumption of computing resources, ensuring that the controller runs efficiently on the embedded hardware platform and does not cause system delays or crashes due to excessive computing loads.
[0064] In summary, by setting the calculation period of the inner and outer loops of the cascade PID controller to 50 Hz, the problems of control output lag and incoordination between different control loops are solved, the real-time response capability and control performance of the system are effectively improved, and the attitude stability and dynamic control accuracy of the small unmanned helicopter in complex flight environments are ensured.
[0065] In one embodiment, the control center of the drone is an autopilot platform. Therefore, when designing the autopilot platform, the design function of the autopilot should be determined according to the type of aircraft and mission requirements. The autopilot control of small unmanned helicopters includes flight trajectory control and flight attitude control. The main tasks are the collection and processing of sensor data, real-time calculation of attitude angles, and output of PWM signals according to the control algorithm to drive the servo to control the flight attitude of the aircraft in real time and realize closed-loop control. Among them, flight trajectory control is realized through flight attitude control. Therefore, attitude control is the core issue of the design of the autopilot platform for micro unmanned helicopters.
[0066] The hardware design of the unmanned helicopter autopilot platform uses high-precision micromechanical (MEMS) sensors. At the same time, the attitude update solution algorithm is the core of the platform system. The conventional method to determine the attitude is to calculate the direction cosine matrix associated with the vehicle coordinate system and the navigation coordinate system, or to use an efficient numerical integration algorithm to calculate the equivalent quaternion. This platform uses the rotation quaternion method to update the attitude matrix in real time and the multi-sensor information fusion technology based on complementary filtering to correct the attitude solution error. It is also used to compensate for the low accuracy and susceptibility to interference of the MEMS sensor itself. In order to improve the real-time performance of the attitude solution, the fourth-order Runge Kutta method is used to update the rotation quaternion in combination with the cascade PID control algorithm of the helicopter's pitch, roll, and yaw axes.
[0067] After the initial quaternion is determined according to the conversion formula between Euler angles and quaternions, the quaternion can be updated in real time using the following calculation formula.
[0068]
[0069] In the above formula, ω=ω x i+ω y j+ω z k is the projection of the angular velocity of the carrier coordinate system relative to the navigation coordinate system in the carrier coordinate system, and the output angular velocity of the gyroscope on the carrier measures the projection of the angular velocity of the carrier coordinate system relative to the inertial system in the carrier coordinate system, so it must be transformed before it can be used for the update calculation of the attitude matrix. The transformation is as follows:
[0070]
[0071] In order to ensure the "unit" nature of the quaternion, normalization processing is also added to the autonomous driving program to ensure that the norm of the four elements is 1. The output value of the accelerometer is then used to fuse the data complementation method of the gyroscope attitude angle and the inclinometer attitude angle, providing long-term high-precision and good dynamic performance attitude information for subsequent attitude control.
[0072] The attitude control of unmanned helicopters requires high precision, and there are certain coupling characteristics between the attitude angles. The traditional digital PID algorithm cannot meet its control requirements, while the cascade digital PID control algorithm can better meet its accuracy requirements. The cascade digital PID controller connects two digital PID controllers in series, and the output of one controller is used as the input of the other controller. Compared with the single-stage digital PID controller, the cascade digital PID controller improves the dynamic characteristics of the control process, improves the system control quality, can quickly overcome the secondary disturbance entering the secondary loop, increases the system's operating frequency, and has strong adaptability to load changes.
[0073] The autopilot platform system uses a cascade PID control algorithm to maintain the attitude of the roll, pitch and yaw channels. Because the angular velocity updates faster than the pitch angle value and pitch angular velocity, the angular velocity control with a faster response speed is placed in the inner loop, and the attitude control with a slower response speed is placed in the outer loop, thus forming a cascade digital PID controller. Figure 2 shown.
[0074] The cascade digital PID control is the outer loop (attitude loop) first, and then the inner loop (angular velocity loop). The reference pitch angle is first given by the calibrated hovering pitch angle value, and the reference value of the pitch axis angular velocity is obtained after the outer loop digital PID calculation. Then, after the inner loop digital PID calculation output and control of the servo system, the helicopter can follow the reference angular velocity and then follow the reference value of the pitch angle. In order to meet the faster inner loop control, the inner and outer loop calculation design cycles are the same, both 50Hz, which fully meets the control frequency requirements of the unmanned helicopter inertia system. According to the characteristics of helicopter control, the traditional PID algorithm is improved, and the cascade PID is used to realize the inner and outer loops - cascade control of angular velocity and attitude angle.
[0075] Based on further analysis and research of the existing technical problems, this application recognizes that the attitude control of the existing small unmanned helicopter has obvious dynamic coupling characteristics, limited sensor accuracy and poor real-time performance of attitude solution. The high-precision micromechanical sensor module is used to obtain angular velocity, acceleration and inclination data in real time. The attitude solution module adopts the rotation quaternion method and the fourth-order Runge-Kutta numerical integration method to update the quaternion in real time and perform normalization processing, which effectively improves the accuracy and real-time performance of attitude solution and solves the problems of insufficient real-time performance and low accuracy of attitude solution in the background technology. The data fusion module fuses the gyroscope, accelerometer and inclinometer data based on the complementary filtering algorithm, corrects the attitude solution error, compensates for the defects of low sensor accuracy and susceptibility to noise interference, and ensures the long-term stability and dynamic accuracy of attitude information. The attitude control module adopts a cascade PID controller to hierarchically process the outer loop attitude angle control and the inner loop angular velocity control, solves the dynamic coupling problem in attitude control, and significantly improves the dynamic response performance and anti-disturbance ability of the system. The actuator control module drives the servo through PWM signals to achieve independent control of the three-axis attitude of pitch, roll and yaw, ensuring the flight stability and control accuracy of the unmanned helicopter in complex environments.
[0076] In summary, this embodiment effectively solves the problems mentioned in the background technology, such as low sensor accuracy, poor real-time performance of attitude solution, and dynamic coupling of attitude control, and significantly improves the stability, accuracy, and real-time response performance of the unmanned helicopter automatic driving platform.
[0077] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
Claims
1. A small unmanned helicopter automatic driving platform, characterized in that: The platform includes: High-precision micromechanical sensor modules for obtaining real-time angular velocity, acceleration, and inclination data of small unmanned helicopters; The attitude calculation module is used to calculate the attitude through the rotation quaternion method and update the rotation quaternion in real time; A data fusion module, used for fusing the angular velocity, acceleration and inclination data; An attitude control module includes a cascade PID controller, an outer loop for calculating an angular velocity reference value according to an attitude angle error, and an inner loop for calculating a control signal according to the angular velocity error; The actuator control module is used to drive the steering gear according to the control signal to adjust the pitch, roll and yaw attitude of the small unmanned helicopter.
2. The small unmanned helicopter automatic driving platform according to claim 1 is characterized in that: The attitude calculation module performs attitude calculation based on the rotation quaternion method, specifically using the following formula: In the above formula, is the derivative of the state vector, which indicates the change of the system state over time and is used to describe the change of the object's posture; λ is the real part of the quaternion, and p1, p2, and p3 are the imaginary parts of the quaternion, that is, q = λ + p1i + p2j + p3k; ω = ω x i+ω y j+ω z k is the projection of the angular velocity of the carrier coordinate system relative to the navigation coordinate system in the carrier coordinate system, ω x ,ω y ,ω z are the rotational velocity components of the angular velocity on the x-axis, y-axis, and z-axis.
3. The small unmanned helicopter automatic driving platform according to claim 2 is characterized in that: The attitude calculation module performs coordinate transformation on the angular velocity to obtain the following formula: Among them, ω x ,ω y ,ω z is the angular velocity component in the carrier coordinate system, [ω xx ω yy ω zz ] T is the gyro output angular velocity, C b n is the attitude matrix, [ω1ω2ω3] T It is the sum of the projection of the earth's rotation angular velocity in the navigation coordinate system and the projection of the navigation coordinate system's rotation angular velocity relative to the earth's coordinate system in the navigation coordinate system.
4. The small unmanned helicopter automatic driving platform according to claim 1 is characterized in that: The attitude solving module integrates the quaternion derivatives by the fourth-order Runge-Kutta method and updates the quaternion in real time.
5. The small unmanned helicopter automatic driving platform according to claim 1 is characterized in that: The attitude solving module normalizes the updated quaternion to ensure that the norm of the quaternion is 1.
6. The small unmanned helicopter automatic driving platform according to claim 1 is characterized in that: The data fusion module adopts complementary filtering algorithm to fuse the gyroscope, accelerometer and inclinometer data to correct the attitude solution error.
7. The small unmanned helicopter automatic driving platform according to claim 1 is characterized in that: The cascade PID controller comprises: The outer loop PID controller is used to calculate the angular velocity reference value according to the attitude angle error; The inner loop PID controller is used to calculate the output control signal based on the angular velocity error.
8. The small unmanned helicopter automatic driving platform according to claim 7 is characterized in that: The calculation cycles of the inner loop and the outer loop of the cascade PID controller are both 50 Hz.
9. The small unmanned helicopter automatic driving platform according to claim 1, characterized in that: The actuator control module drives the steering gear through PWM signals to adjust the pitch, roll and yaw attitude of the small unmanned helicopter.
10. The small unmanned helicopter automatic driving platform according to claim 1, characterized in that: The platform is suitable for solving the coupling problem in three-axis attitude control of pitch, roll and yaw.