Aircraft control method based on dual-sensor fusion barometric altitude compensation and attitude calculation

Abstract

The invention discloses an aircraft control method based on barometric altitude compensation and attitude calculation of dual-sensor fusion, and the method comprises the steps: carrying out the temperature compensation and attitude calculation through collecting the data of a barometric sensor and an inertial measurement unit, and obtaining an original altitude and an Euler angle; processing the original height based on an alpha-beta filter to obtain an air pressure estimation height, and calculating an inertial navigation height and a vertical speed through attitude conversion integral; a self-adaptive complementary filtering strategy is adopted, the fusion weight is dynamically adjusted according to the air pressure estimation height variance, the high-frequency inertial data and the low-frequency air pressure data are fused, and accurate fusion height and vertical speed are output; finally, constructing a cascade controller, and calculating a target vertical speed instruction by an outer ring according to the expected height deviation; the inner ring generates an actuator signal based on the deviation to drive a motor; height stable control is achieved, so that drifting is effectively restrained, the precision and the response speed are improved, and reliable height control of the aircraft under vibration or air pressure changes is achieved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of aircraft control, in particular to an aircraft control method based on double-sensor fusion for air pressure height compensation and attitude calculation. BACKGROUND

[0002] In the dynamic operation of an aircraft, adverse disturbances such as air flow, vibration interference of various types of sensors on board, etc. are often encountered, and higher requirements are put forward for the real-time data acquisition of the aircraft with high precision, so that the real-time data acquisition of the aircraft with high precision has become the core development demand of the current technology.

[0003] The height measurement based on the air pressure sensor is easily affected by temperature changes and environmental noise, resulting in large data fluctuations, which is difficult to meet the stability requirements of long-time flight; while the inertial measurement unit can provide short-term dynamic response, but cannot support the accuracy of height estimation alone due to long-term drift. The contradictory height control brought by it makes it difficult for a single sensor to effectively cope with the demand for height control in complex flight scenarios, especially under the rapid height command of the aircraft. There are obvious deficiencies in the dynamic balance of data and the response speed of control in the prior art.

[0004] Chinese patent CN201910270669.5 proposes a combined navigation algorithm fusing optical flow position and velocity information, which uses an extended Kalman filter to fuse data through optical flow sensors, inertial measurement units, barometric altimeters and other devices, and attempts to calculate the position, velocity and attitude information of the carrier. However, this method fails to effectively solve the contradiction between air pressure data fluctuations and inertial data drift in height estimation, especially in the balance between dynamic response and long-term stability in complex environments, making it difficult to meet the dual requirements of real-time and accuracy of the aircraft Therefore, there is a lack of an aircraft control method based on double-sensor fusion for air pressure height compensation and attitude calculation, which is used to solve the volatility problem of air pressure data affected by environmental interference. SUMMARY

[0005] The present application provides an aircraft control method based on double-sensor fusion for air pressure height compensation and attitude calculation, which is used to solve the volatility problem of air pressure data affected by environmental interference, while suppressing the long-term drift of inertial data to obtain high-precision fused height information; and in the height control process, how to construct an efficient control mechanism based on fused data to cope with rapid changes in flight requirements and ensure the stability and safety of the aircraft.

[0006] The present application provides an aircraft control method based on double-sensor fusion for air pressure height compensation and attitude calculation, comprising: Step S1, obtaining the ambient pressure data collected by the air pressure sensor and the three-axis acceleration and three-axis angular velocity data collected by the inertial measurement unit; performing temperature compensation on the ambient pressure data and converting it into a raw height value; Step S2, performing attitude solving using the three-axis acceleration and three-axis angular velocity data to obtain fused attitude quaternions; converting the attitude quaternions into Euler angles for flight control, wherein the Euler angles include a pitch angle and a roll angle; Step S3, processing the raw height value to obtain a barometric estimated height; converting the three-axis acceleration in the body coordinate system to the geographic coordinate system based on the attitude quaternions and calculating an inertial navigation height and a vertical velocity; fusing the barometric estimated height and the inertial navigation height and the vertical velocity, wherein the fusion weight is dynamically adjusted according to the variance of the barometric estimated height, and the fused height value and the vertical velocity value are output; Step S4, constructing a cascade controller including an outer loop and an inner loop, wherein the outer loop is a height control loop and the inner loop is a vertical velocity control loop; the height control loop calculates a target vertical velocity command according to the deviation between the expected height and the fused height value; the vertical velocity control loop generates an actuator control signal according to the deviation between the target vertical velocity command and the fused vertical velocity, and drives the aircraft motor for flight control using the actuator control signal.

[0007] Preferably, step S1 includes: obtaining temperature sensor data of the air pressure sensor and correcting the air pressure value to obtain a compensated air pressure value; converting the compensated air pressure value into a raw height value according to a standard atmospheric model; wherein the air pressure sensor is used for height measurement, and the inertial measurement unit is used for attitude sensing; the standard atmospheric pressure at sea level is used as reference data for the standard atmospheric model.

[0008] Preferably, step S2 includes: sequentially performing zero drift calibration, scale transformation and normalization processing on the three-axis acceleration and three-axis angular velocity data to obtain preprocessed data; wherein the zero drift calibration is calibrated by statically collecting multiple samples at power-on and calculating their average value as the zero offset value; based on the preprocessed angular velocity data and the quaternion of the last period, the predicted attitude quaternion of the current period is obtained by updating calculation through the quaternion differential equation.

[0009] Preferably, the optimal quaternion is obtained by using the gradient descent method to assist in correcting the predicted attitude quaternion based on the gravity vector error determined by the cross product of the normalized acceleration vector and the predicted gravity vector, wherein the gradient descent method processes the gravity vector error based on a preset correction step and adjusts the predicted attitude quaternion using the processing result; the optimal quaternion is converted into Euler angles including a roll angle, a pitch angle and a heading angle for flight control, wherein the roll angle is obtained by an inverse tangent function based on a specific element of the optimal quaternion, the pitch angle is obtained by an inverse sine function based on an element of the optimal quaternion, and the heading angle is obtained by an inverse tangent function based on an element of the optimal quaternion.

[0010] Preferably, the step S2 comprises: calculating a projection of a theoretical gravity vector in the body coordinate system based on the optimal quaternion; obtaining an actually measured gravity vector based on the preprocessed acceleration data; constructing an objective function for optimization based on the difference between the theoretical gravity vector and the actually measured gravity vector; and obtaining the optimal quaternion that suppresses long-term drift by iteratively solving the objective function based on the gradient descent method, wherein in each iteration, the quaternion is adjusted based on a preset correction step and a gradient direction of the objective function, and the optimal quaternion is obtained.

[0011] Preferably, the step S3 comprises: processing the original height value comprises: obtaining a predicted height value of the current period based on the obtained estimated height at the last moment, the estimated vertical velocity at the last moment and the sampling time; using the estimated vertical velocity at the last moment as the predicted value of the current period; obtaining a height measurement residual value based on the predicted height value and the current original height value; calculating the predicted height, the height measurement residual value and a position gain coefficient a to obtain the estimated height at the current moment; calculating the predicted vertical velocity, the height measurement residual value, a velocity gain coefficient β and the sampling time to obtain the estimated vertical velocity at the current moment; wherein the position gain coefficient a and the velocity gain coefficient β are preset as fixed values, a = 0.2 and β = 0.05; and the sampling time is the sensor sampling rate.

[0012] Preferably, in step S3, the high-frequency component higher than the preset frequency threshold in the fused barometric pressure estimated height data is the inertial navigation height and vertical velocity, and the low-frequency component lower than the preset frequency threshold is the barometric pressure estimated height, the variance of the barometric pressure estimated height in the latest time period is calculated through a sliding window, the fusion weight is dynamically adjusted based on the comparison result of the variance and the preset frequency threshold, when the variance exceeds the preset frequency threshold, the weight of the inertial navigation height and vertical velocity is increased, when the variance does not exceed the preset frequency threshold, the weight of the barometric pressure estimated height is increased, the weight of the barometric pressure estimated height is adjusted through linear interpolation, wherein the data weight value of the inertial measurement unit is positively correlated with the size of the variance, and the data weight value of the barometric pressure sensor is negatively correlated with the size of the variance, and the preset frequency threshold is dynamically fine-tuned based on the battery voltage or vibration amplitude of the aircraft.

[0013] Preferably, in step S3, the estimated height and the estimated vertical velocity at the last time are initialized as the prediction basis, the displacement change amount in the current period is calculated based on the estimated vertical velocity at the last time and the sampling time, the predicted height of the current period is obtained based on the estimated height at the last time and the displacement change amount, and the estimated vertical velocity at the last time is directly used as the predicted vertical velocity of the current period to maintain the continuity of the motion state.

[0014] Preferably, in step S4, the deviation value of the outer loop based on the expected height and the fused height value is obtained, and a target vertical velocity instruction is output, the deviation value of the inner loop based on the target vertical velocity instruction and the fused vertical velocity is obtained, and an execution signal adjustment amount is generated, the execution signal adjustment amount is obtained by weighted fusion based on the proportional term, the integral term and the differential term of the deviation, the execution signal adjustment amount and the basic driving value are superimposed to generate an actuator control signal, the actuator control signal is a pulse width modulation signal, the pulse width modulation signal is applied to the actuator motor of the aircraft through a driving circuit, and the attitude and height of the aircraft are controlled by adjusting the motor lift.

[0015] Preferably, the method realizes aircraft control through an aircraft control system based on barometric pressure height compensation and attitude calculation of double-sensor fusion, and the system comprises a sensing module comprising a barometric pressure sensor and an inertial measurement unit connected with a controller through a bus, used for collecting environmental barometric pressure data, acceleration and angular velocity data, and an execution module controlled by a driving signal generated by the controller.

[0016] The working principle and beneficial effects of the application are as follows: The application provides a kind of aircraft control method based on dual sensor fusion barometric height compensation and attitude solution, comprising: step S1, obtaining the ambient pressure data collected by barometric pressure sensor, and the three-axis acceleration and three-axis angular velocity data collected by inertial measurement unit;Temperature compensation is carried out on the ambient pressure data, and it is converted into original height value;Step S2, attitude solution is carried out using three-axis acceleration and three-axis angular velocity data, and fused attitude quaternion is obtained;Attitude quaternion is converted into Euler angle used for flight control, wherein, Euler angle includes pitch angle and roll angle;Step S3, original height value is processed to obtain barometric estimated height;Three-axis acceleration under body coordinate system is converted to geographic coordinate system based on attitude quaternion, and inertial navigation height and vertical velocity are calculated;Fusion barometric estimated height and inertial navigation height and vertical velocity, wherein, according to the variance of barometric estimated height, the fusion weight is dynamically adjusted, and the fused height value and vertical velocity value are output;Step S4, a cascade controller including outer loop and inner loop is constructed, the outer loop is height control loop, and the inner loop is vertical velocity control loop, and the target vertical velocity command is calculated according to the deviation of expected height and fused height value in height control loop;The actuator control signal is generated according to the deviation of target vertical velocity command and fused vertical velocity in vertical velocity control loop, and the flight control is carried out by driving aircraft motor using actuator control signal.

[0017] In the application, the barometric pressure data obtained from the barometric pressure sensor is temperature compensated and processed, and then the corresponding original height value is calculated. Based on attitude solution, the three-axis acceleration value and three-axis angular velocity value collected by the inertial measurement unit are calculated to generate fused attitude quaternion. The attitude quaternion is converted in coordinates and the Euler angle data used for flight control are calculated.

[0018] After the original height value is filtered by an α-β filter, the barometric estimated height is generated. The acceleration value under the body coordinate system is converted to the geographic coordinate system using the attitude quaternion, and the corresponding navigation height and vertical velocity value are determined by integral operation.

[0019] Further, the barometric estimated height, navigation height and vertical velocity value are data fused. If the fluctuation of the barometric estimated height exceeds the preset threshold, the fusion weight of the barometric estimated height is reduced and the weight of the navigation height is increased, and finally the fused height value and vertical velocity value are output.

[0020] Then, a cascade control structure is constructed, the outer loop is set as the height control loop, and the inner loop is set as the vertical velocity control loop. The target vertical velocity command is calculated according to the deviation of the fused height value and the expected height. Further specifically, in the inner loop, the corresponding actuator control signal is generated according to the deviation of the target vertical velocity command and the fused vertical velocity value. The aircraft motor is driven by the control signal to adjust the aircraft attitude to stabilize the height.

[0021] In the application, in the process of realizing the height stable control of the aircraft, first, the ambient pressure data is collected by the air pressure sensor, and the ambient temperature is obtained, the air pressure is corrected by using the temperature compensation formula, and the compensated air pressure value is obtained; then the air pressure is converted into the original height value according to the international standard atmospheric model, and the original height is obtained; then, the three-axis acceleration data and three-axis angular velocity data collected by the inertial measurement unit are fused to obtain the attitude quaternion through the Kalman filtering algorithm, and then the attitude quaternion is converted into Euler angle, and the pitch angle and roll angle are calculated, which are used for subsequent flight control. After filtering the original height value, the sliding average filter is used to take the average of multiple height values in the past, and the air pressure estimated height value is obtained, and the acceleration in the body coordinate system is converted to the geographic coordinate system based on the attitude quaternion, and the vertical acceleration is obtained, and the inertial navigation height value and the vertical velocity value are calculated by twice integration; further, the adaptive complementary filtering strategy is used to dynamically adjust the fusion weight to 0.7 (air pressure) and 0.3 (inertia), and finally the height value and the vertical velocity value are fused. Finally, a cascade controller is constructed, the outer loop height control loop calculates the target vertical speed command according to the deviation value of the expected height value and the fused height value through the proportional integral differential control algorithm, the inner loop vertical speed control loop generates the actuator control signal according to the deviation value of the target speed value and the fused speed value, and the proportional gain is assumed to be 2.0, and the output control amount is 1.6, which drives the aircraft motor to adjust the rotating speed, and realizes the height stable control.

[0022] Other features and advantages of the present application will be set forth in the following description, and in part will become apparent to those skilled in the art from the description, or can be learned by practice of the present application. The objects and other advantages of the present application can be realized and attained by means of the instrumentalities particularly pointed out in the written description and the appended drawings.

[0023] The technical solutions of the present application will be further described in detail below with the help of the drawings and examples. BRIEF DESCRIPTION OF DRAWINGS

[0024] The accompanying drawings are included to provide a further understanding of the present application, and constitute a part of the specification, and are used to explain the present application together with the embodiments of the present application, and do not constitute a limitation on the present application. In the drawings: Fig. 1 It is a flowchart of the present application; Fig. 2 It is a system framework schematic diagram of the present application. DETAILED DESCRIPTION

[0025] The preferred embodiments of the present application will be described below in combination with the drawings, and it should be understood that the preferred embodiments described herein are only used to illustrate and explain the present application, and do not limit the present application.

[0026] According to Figs. 1-2 The embodiment of the application provides a kind of aircraft control method based on dual sensor fusion's air pressure height compensation and attitude solution, including: Step S1, the environmental air pressure data collected by barometric pressure sensor is acquired, and the three-axis acceleration and three-axis angular velocity data collected by inertial measurement unit;Temperature compensation is carried out to environmental air pressure data, and it is converted into original height value; Step S2, attitude solution is carried out using three-axis acceleration and three-axis angular velocity data, and fused attitude quaternion is obtained;Attitude quaternion is converted into Euler angle for flight control, wherein Euler angle includes pitch angle and roll angle; Step S3, original height value is processed to obtain air pressure estimated height;Three-axis acceleration under body coordinate system is converted to geographic coordinate system based on attitude quaternion, and inertial navigation height and vertical velocity are calculated;Air pressure estimated height, inertial navigation height and vertical velocity are fused, wherein fusion weight is dynamically adjusted according to the variance of air pressure estimated height, and fused height value and vertical velocity value are output; Step S4, a cascade controller including outer loop and inner loop is constructed, the outer loop is height control loop, and the inner loop is vertical velocity control loop, height control loop calculates target vertical velocity command according to the deviation of expected height and fused height value;Vertical velocity control loop generates actuator control signal according to the deviation of target vertical velocity command and fused vertical velocity, and drives aircraft motor using actuator control signal to carry out flight control.

[0027] In the application, the air pressure data obtained from the barometric pressure sensor is temperature compensated and processed, and then the corresponding original height value is calculated.Based on attitude solution, the three-axis acceleration value and three-axis angular velocity value collected by inertial measurement unit are calculated to generate fused attitude quaternion;Coordinate conversion is carried out on attitude quaternion, and Euler angle data used for flight control are calculated.

[0028] After filtering the original height value through α-β filter, air pressure estimated height is generated;Acceleration value under body coordinate system is converted to geographic coordinate system using attitude quaternion, and corresponding navigation height and vertical velocity value are determined by integral operation.

[0029] Further, data fusion is carried out on air pressure estimated height, navigation height and vertical velocity value;If the fluctuation of air pressure estimated height exceeds the preset threshold, the fusion weight of air pressure estimated height is reduced, and the weight of navigation height is increased, and finally the fused height value and vertical velocity value are output.

[0030] Then, a cascade control structure is constructed, an outer loop is set as a height control loop, and an inner loop is set as a vertical speed control loop;According to the deviation between the fused height value and the expected height, the target vertical speed command is calculated.

[0031] In the process of realizing the height stable control of the aircraft, in the application, firstly, the environmental air pressure data is collected by the air pressure sensor, and the environmental temperature is obtained, the air pressure is corrected by using the temperature compensation formula, and the compensated air pressure value is obtained;Then, according to the international standard atmospheric model, the air pressure is converted into the original height value, and the original height is obtained;Then, the three-axis acceleration data and three-axis angular velocity data collected by the inertial measurement unit are used for attitude solution through Kalman filtering algorithm, and the attitude quaternion is fused, and then converted into Euler angle, and the pitch angle and roll angle are calculated, which are used for subsequent flight control. After filtering the original height value, the sliding average filter is used to take the average value of multiple height values in the past, and the air pressure estimated height value is obtained, and the acceleration in the body coordinate system is converted to the geographic coordinate system based on the attitude quaternion, and the vertical acceleration is obtained, and the inertial navigation height value and the vertical speed value are calculated by twice integral;Further, the adaptive complementary filtering strategy is used to dynamically adjust the fusion weight to 0.7 (air pressure) and 0.3 (inertia) according to the variance of the air pressure estimated height, and finally the height value and the vertical speed value are fused. Finally, a cascade controller is constructed, the outer loop height control loop calculates the target vertical speed command according to the deviation between the expected height value and the fused height value through the proportional integral differential control algorithm, and the inner loop vertical speed control loop generates the actuator control signal according to the deviation between the target speed value and the fused speed value, assuming that the proportional gain is 2.0, the output control quantity is 1.6, the aircraft motor is driven to adjust the rotating speed, and the height stable control is realized.

[0032] In one embodiment, step S1 includes: obtaining the data of the temperature sensor built-in the air pressure sensor, and correcting the air pressure value to obtain the compensated air pressure value;According to the standard atmospheric model, the compensated air pressure value is converted into the original height value;The air pressure sensor is used for height measurement, and the inertial measurement unit is used for attitude sensing;The standard atmospheric pressure at sea level is used as the reference data of the standard atmospheric model.

[0033] The temperature data is acquired by the temperature sensor built in the air pressure sensor, and the air pressure value is temperature-corrected to obtain a compensated air pressure value; the compensated air pressure value is converted into an original height value by using a standard atmospheric model; attitude data is collected by using an inertial measurement unit, and an attitude correction coefficient is determined based on the attitude data; wherein, if the attitude correction coefficient exceeds a preset threshold, the attitude data and the compensated air pressure value are fused to obtain an attitude-compensated height value; the attitude-compensated height value is compared with reference data of sea level standard atmospheric pressure to obtain a height deviation value, and a final compensated height value is obtained based on the height deviation value.

[0034] The temperature data of the air pressure sensor is acquired, the air pressure value collected by the air pressure sensor is temperature-corrected based on the temperature data to obtain a compensated air pressure value; the compensated air pressure value is converted into an original height value by using a standard atmospheric model, attitude data is collected by using an inertial measurement unit, and an attitude correction coefficient is determined based on the attitude data; wherein, if the attitude correction coefficient exceeds a preset threshold, the attitude data and the compensated air pressure value are fused to obtain an attitude-compensated height value; the attitude-compensated height value is compared with reference data of sea level standard atmospheric pressure to obtain a height deviation value, and a final compensated height value is obtained based on the height deviation value.

[0035] Through the joint action of the built-in accelerometer and gyroscope, the inertial measurement unit can not only accurately detect the change of the inclination of the device and the motion state thereof, but also can better reflect various external powers Since temperature changes will affect the accuracy of air pressure readings, the air pressure value is corrected by temperature data to improve the accuracy of the air pressure value. Specifically, the air pressure value is adjusted by a linear or polynomial function to correct the air pressure deviation caused by temperature.

[0036] The standard atmospheric model takes sea level standard atmospheric pressure as reference data, and the height conversion is calculated by the approximate relationship of temperature gradient and gravity influence to obtain the sea level standard atmospheric pressure as the reference to calibrate the air pressure difference of different places.

[0037] Since the airflow in the mountain environment may be unstable and cause the aircraft to tilt, the compensated air pressure value is obtained by correcting the collected data, and the original height value relative to the sea level is obtained by using the standard atmospheric model to convert the height based on the compensated air pressure value; at the same time, the error caused by temperature fluctuation is compensated in real time to provide stable height readings for the aircraft.

[0038] Further, the temperature compensation process further includes: calculating a correction coefficient after comparing the current temperature value output by the temperature sensor with the standard temperature, and applying the correction coefficient to the original air pressure value; thereby realizing the applicability in the variable temperature environment in the high mountain area; Further, when the multi-temperature is higher than the standard value, the air pressure reading can also be higher; therefore, the correction coefficient is reduced to obtain an accurate compensation coefficient, so that the accuracy of the compensated air pressure value can reach a certain threshold, that is, the purpose of reliable height conversion is achieved.

[0039] Further, the standard atmosphere model is divided into different layers, and each layer has a specific pressure decay rate; based on the sea level standard atmospheric pressure, the compensation air pressure is converted into height through iterative calculation or table lookup.

[0040] When the device detects the inclined posture, the inertial measurement unit is used to provide a correction vector, and the height value based on the air pressure is adjusted based on the correction vector, so as to avoid errors caused by changes in posture.

[0041] In one embodiment, step S2 includes: sequentially performing zero drift calibration, scale transformation and normalization processing on the three-axis acceleration and three-axis angular velocity data to obtain preprocessed data; wherein the zero drift calibration is calibrated by statically collecting a plurality of samples at power-on and calculating the average value as the zero offset value; based on the preprocessed angular velocity data and the quaternion of the last period, the prediction attitude quaternion of the current period is obtained by updating calculation through the quaternion differential equation.

[0042] Using the preprocessed acceleration data, based on the gravity vector error determined by the cross product of the normalized acceleration vector and the predicted gravity vector, the gradient descent method is used to assist in correcting the predicted attitude quaternion to obtain the optimal quaternion; wherein the gradient descent method processes the gravity vector error based on the pre-set correction step, and adjusts the predicted attitude quaternion using the processing result; the optimal quaternion is converted into Euler angles including roll angle, pitch angle and heading angle for flight control; wherein the roll angle is calculated based on a specific element of the optimal quaternion by the arctangent function, the pitch angle is calculated based on the element of the optimal quaternion by the arcsine function, and the heading angle is calculated based on the element of the optimal quaternion by the arctangent function.

[0043] Step S2 includes: calculating the projection of the theoretical gravity vector in the body coordinate system based on the optimal quaternion; obtaining the actually measured gravity vector based on the preprocessed acceleration data; based on the difference between the theoretical gravity vector and the actually measured gravity vector, constructing a target function for optimization; based on the gradient descent method, the target function is iteratively solved, and in each iteration, the quaternion is adjusted based on the pre-set correction step and the gradient direction of the target function to obtain the optimal quaternion that suppresses long-term drift; wherein the iterative calculation process of the gradient descent method is realized by fixed-point number operation and table lookup method, which is used to optimize the calculation efficiency.

[0044] In this embodiment, by statically collecting multiple samples, the average value is calculated as the zero bias value, the three-axis acceleration data and the three-axis angular velocity data are calibrated for zero drift, and the calibrated acceleration data and the calibrated angular velocity data are obtained. According to the calibrated acceleration data and the calibrated angular velocity data, the data range is adjusted by using the scale transformation method to obtain the transformed acceleration data and the transformed angular velocity data. The transformed acceleration data and the transformed angular velocity data are obtained, normalized, and the preprocessed acceleration data and the preprocessed angular velocity data are obtained. For the preprocessed acceleration data and the preprocessed angular velocity data, if the transformed range exceeds the preset threshold, the current period prediction attitude quaternion is obtained by updating the last period quaternion through the quaternion differential equation.

[0045] Among them, the scale transformation after zero drift calibration includes: mapping the original sensor data to a standard unit range, for example, converting the acceleration from the original voltage value to meters per second square; Further, the transformation is realized by multiplying the original data by a pre-defined scale factor, thereby unifying the output difference between different sensors and improving the data consistency in attitude estimation. In addition, the scale factor can be manually set according to the sensor specification or determined through a calibration experiment, thereby adapting to different aircraft models or application scenarios. Further, the transformed data is mapped to the [-1, 1] interval, then the minimum-maximum normalization method of subtracting the minimum value from the data and dividing by the range difference is used for normalization processing, which eliminates the dimension effect and facilitates the input of subsequent algorithms. Further, for angular velocity data, the normalization processing is adjusted in combination with the device-specific threshold to optimize the performance in motion monitoring.

[0046] Further, using the quaternion of the last period as the initial value, the current attitude is updated through the quaternion differential equation, which includes: regarding the preprocessed angular velocity as the rotation rate, calculating the derivative of the quaternion through the differential equation, and then integrating to obtain the change amount, thereby ensuring the continuity and accuracy of the flight attitude, especially suitable for real-time tracking. And the quaternion differential equation is updated by numerical integration, which includes: calculating the cross product of the angular velocity vector and the current quaternion to obtain the derivative term, and then adding it to the last quaternion; After updating, the quaternion is normalized to maintain its unit length and avoid cumulative errors. Further, when applied to the attitude tracking of the aircraft, 200 samples are statically collected to calculate the zero bias value at power-on, the collected angular velocity data is scaled to radian per second unit, and normalized to [-1, 1]; Then, the last period quaternion is updated iteratively through the differential equation to obtain the current predicted attitude; thereby achieving the purpose of improving the accuracy of attitude estimation and reducing the drift effect in continuous monitoring.

[0047] Further, the integrated acceleration data can also assist in correcting the quaternion drift, and a quaternion differential equation is used to convert the angular velocity into the rate of change of the quaternion. Specifically, the angular velocity vector ω is multiplied by half of the current quaternion to obtain the derivative, which is then integrated by the time step. This scheme takes into account the vector nature of rotation and ensures that the updated quaternion accurately represents the device attitude.

[0048] In this embodiment, the original acceleration data collected by the sensor is obtained, the original acceleration data is filtered to obtain a preprocessed acceleration vector, and a predicted gravity vector and a predicted attitude quaternion are determined from the gyroscope data. The cross product is calculated according to the preprocessed acceleration vector and the predicted gravity vector, and the cross product is obtained by vector multiplication to obtain a perpendicular vector, and the gravity vector error is determined. The gravity vector error is processed using a gradient descent method, which minimizes the error function by iteration, and the predicted attitude quaternion is adjusted by a predetermined correction step to obtain a corrected quaternion. The magnetic sensor data is fused from the corrected quaternion, the magnetic sensor data is obtained from the sensor, the magnetic field vector deviation is calculated, the deviation is obtained by comparing the measured magnetic field with the reference magnetic field, and the optimal quaternion is obtained. The arctangent function and the arcsine function are calculated by the elements of the optimal quaternion, the arctangent function is for a specific element ratio, and the arcsine function is for an element product, to determine the roll angle, the pitch angle and the heading angle.

[0049] Further, the attitude correction of the acceleration data includes: normalizing the original acceleration data to obtain a normalized acceleration vector; and calculating a predicted gravity vector based on the current attitude quaternion, which is used to obtain an expected vector parameter of the gravity direction in the device coordinate system. The gravity vector error is calculated by the cross product of the normalized acceleration vector and the predicted gravity vector, and is used to quantify the gravity vector error.

[0050] Further, the predicted attitude quaternion is assisted to be corrected based on the gravity vector error, and the error function of the gravity vector error is minimized by adjusting the parameters along the negative gradient direction. The adjustment amplitude of each iteration is controlled by a predetermined correction step, specifically, the gradient of the error with respect to the quaternion elements is calculated, the step is combined with the quaternion value to complete the update, and the purpose of real-time processing of the acceleration data in the unmanned aerial vehicle attitude tracking is achieved; the optimal quaternion is obtained by iterating multiple times until the error is lower than a threshold; and the above correction method can effectively compensate the influence of sensor noise and improve the accuracy of attitude estimation.

[0051] Further, based on the specific elements of the optimal quaternion, Euler angles are calculated through the inverse tangent function; through the conversion of Euler angles, data generalization purposes in different flight tasks are achieved; for example, in autonomous navigation, through the combination of Euler angles and GPS data, the control response speed can be improved, and accurate processing of attitude data can be achieved; in a multi-sensor fusion environment, through the combination of Euler angles and gyroscope data, the system robustness can be enhanced.

[0052] In this embodiment, a theoretical gravity vector is obtained from a preset initial quaternion, and the projection component of the theoretical gravity vector in the body coordinate system is calculated through matrix transformation. The acceleration sensor data is processed by a low-pass filtering method, and the actual measured gravity vector is obtained by combining the projection component calibration processing result. According to the Euclidean distance between the actual measured gravity vector and the theoretical gravity vector, a target function is constructed, and the Euclidean distance is obtained by calculating the square root of the sum of the square differences of the corresponding components of the two vectors. If the value of the target function exceeds a preset threshold, the gradient direction of the target function is calculated by the gradient descent method, the gradient of the target function is taken as the gradient of the partial derivative of the target function with respect to the quaternion, and the quaternion is adjusted by combining a preset correction step. On the basis of the adjusted quaternion, fixed-point number operation is used to perform iterative calculation combined with a lookup table method, the lookup table method pre-stores trigonometric function values for matrix operation, and the optimal quaternion that suppresses long-term drift is obtained.

[0053] Further, the quaternion is used to represent the rotation of the three-dimensional space, which can avoid the gimbal lock situation of Euler angles; therefore, in the attitude estimation system, the quaternion describes the orientation of the body relative to the reference coordinate system, and the known gravity vector is defined as a unit vector in the earth coordinate system, and the theoretical projection vector is obtained by rotating and projecting it to the body coordinate system. The reference gravity vector is transformed to the body perspective, thereby providing a reference for subsequent comparison.

[0054] Furthermore, when optimizing quaternion adjustments by minimizing the objective function is applied to aircraft attitude systems, if the angle between the theoretical projection and the actual measurement vector is large, the function value increases, necessitating correction of the quaternion parameters. This correction employs gradient descent to iteratively solve the objective function. In each iteration, the quaternion is adjusted based on a preset correction step size and the gradient direction of the objective function to obtain the optimal quaternion that suppresses long-term drift. By calculating the gradient of the objective function relative to the quaternion parameters, the function is moved in the negative gradient direction to reduce error. The preset correction step size controls the magnitude of each adjustment, avoiding excessively large step sizes that cause oscillations or excessively small step sizes that lead to slow convergence. During the iteration process, the quaternion is gradually updated until the function value converges to its minimum, thereby suppressing the long-term impact of sensor drift on attitude estimation. In real-time attitude tracking scenarios for UAVs, multiple iterations are performed per second to ensure flight stability. The iterative calculation process of the gradient descent method is implemented using fixed-point arithmetic and table lookup methods to optimize computational efficiency. Fixed-point arithmetic uses integers to represent decimals, avoiding the resource consumption of floating-point arithmetic; the lookup table method pre-stores common function values, such as trigonometric functions or quaternion normalization constants, and reduces the real-time computing burden by performing lookup and replacement calculations.

[0055] Furthermore, by quantizing the geomagnetic field vector into magnetometer data and using it as an additional reference for the geomagnetic field, a multidimensional difference function was constructed in conjunction with the gravity vector. The magnetometer data was then used to assist in correcting the accuracy of the gravity vector, further enhancing the robustness of the objective function. This method has significant potential for widespread application in UAV take-off and landing scenarios, helping to suppress UAV drift in complex electromagnetic environments and thus providing more accurate attitude estimation for UAVs.

[0056] Furthermore, by setting the initial quaternion from a static attitude, calculating the theoretical gravity projection, and comparing it with the actual acceleration, the algorithm uses the objective function as the adjustment target for gradient descent. Each iteration can quickly and stably adjust the UAV's attitude through fixed-point calculations, ultimately enabling the UAV to smoothly traverse unstable airflow regions. By pre-computing the quaternion, the elements of its complex rotation matrix are obtained in advance through table lookup, greatly reducing matrix multiplication overhead. Simultaneously, setting the termination condition of the iteration as a threshold of the function value or setting the maximum number of iterations ensures that the algorithm provides good drift suppression in various applications with different aircraft attitudes. Through efficient integration with gyroscope data, the quaternion-based attitude fusion algorithm not only provides continuous orientation information for the aircraft but also maintains attitude accuracy at a high level for several hours without requiring any external calibration.

[0057] In one embodiment, the step S3 comprises: processing the original height value comprises: obtaining a predicted height value of the current period based on the obtained estimated height of the last moment, the estimated vertical velocity of the last moment and the sampling time; the predicted vertical velocity uses the estimated vertical velocity of the last moment as the predicted value of the current period; obtaining a height measurement residual value based on the predicted height value and the current original height value; calculating the predicted height, the height measurement residual value and the position gain coefficient a to obtain the estimated height of the current moment; calculating the predicted vertical velocity, the height measurement residual value, the speed gain coefficient β and the sampling time to obtain the estimated vertical velocity of the current moment; wherein the position gain coefficient a and the speed gain coefficient β are preset as fixed values, a = 0.2 and β = 0.05; the sampling time is the sensor sampling rate; wherein the filter for filtering the original height value is an a-β filter, and the parameters of the a-β filter are cooperatively set through a joint debugging method to optimize the aircraft model.

[0058] In the fused barometric estimated height data, the high-frequency component higher than the preset frequency threshold comprises: the inertial navigation height and the vertical velocity; and the low-frequency component lower than the preset frequency threshold comprises: the barometric estimated height. The variance of the barometric estimated height in the recent time period is calculated through a sliding window, the variance in the recent time period is compared with the preset frequency threshold, and the obtained variance comparison result is used to dynamically adjust the fusion weight; Wherein, when the variance exceeds the preset frequency threshold, the weight of the inertial navigation height and the vertical velocity is increased; when the variance does not exceed the preset frequency threshold, the weight of the barometric estimated height is increased; the weight of the barometric estimated height is adjusted through linear interpolation, wherein the data weight value of the inertial measurement unit is positively correlated with the size of the variance, and the data weight value of the barometric sensor is negatively correlated with the size of the variance; the preset frequency threshold is dynamically fine-tuned based on the battery voltage or the vibration amplitude of the aircraft.

[0059] The step S3 comprises: initializing the estimated height and the estimated vertical velocity of the last moment as the prediction basis; calculating the displacement change amount in the current period based on the estimated vertical velocity of the last moment and the sampling time; obtaining the predicted height of the current period based on the estimated height of the last moment and the displacement change amount; directly using the estimated vertical velocity of the last moment as the predicted vertical velocity of the current period to maintain the continuity of the motion state.

[0060] In this embodiment, the estimated height value and the estimated vertical velocity value of the previous time are obtained, the predicted height value of the current period is obtained by adding the sampling time value to the estimated height value, and the estimated vertical velocity value is used as the predicted vertical velocity value. Based on the predicted height value and the current raw height value obtained from the sensor, a height measurement residual value is determined, wherein the height measurement residual value is the current raw height value minus the predicted height value. The estimated height value at the current time is calculated by adding the predicted height value to the height measurement residual value multiplied by the position gain coefficient. Based on the predicted vertical velocity value plus the height measurement residual value divided by the sampling time value and multiplied by the velocity gain coefficient, the estimated vertical velocity value at the current time is calculated, and the aircraft model is optimized by jointly adjusting the PID parameter values of the cascade controller based on the estimated height value and the estimated vertical velocity value.

[0061] Based on the estimated height and the estimated vertical velocity of the previous time and the sampling time, the predicted height of the current period is calculated. Specifically, the predicted height can be obtained by adding the estimated height of the previous time to the estimated vertical velocity multiplied by the sampling time, thereby ensuring the continuity of height prediction. Further, the sampling time is determined by the sensor sampling rate of the aircraft, and continuous sampling and height prediction help to reduce the influence of noise on the trajectory estimation of the aircraft, which is particularly suitable in the scene of multi-rotor aircraft. Further, the predicted vertical velocity directly uses the estimated vertical velocity of the previous time as the predicted value of the current period, thereby maintaining the stability of the velocity estimation and avoiding unnecessary complex calculation.

[0062] For the use environment of fixed-wing aircraft, when the flight environment is relatively stable, the accuracy of velocity prediction can be effectively maintained by predicting the height. Under complex airflow conditions, the prediction result of the predicted height is optimized by combining residual correction to improve the stability of flight. Based on the predicted height and the current raw height value, the height measurement residual is obtained. Further, the residual value is obtained by subtracting the predicted height value from the current raw height value. This residual value represents the difference between the measurement bias and the prediction bias, which plays a key role in aircraft height control. For example, in the height maintenance task of a quad-rotor unmanned aerial vehicle, if the original height collected by the sensor fluctuates due to wind disturbance, residual calculation can quantify this deviation and provide basic data for subsequent estimation. In this way, environmental noise or sensor errors can be identified, thereby improving the robustness of the overall estimation. Further specifically, the estimated height at the current time is calculated based on the predicted height, the height measurement residual, and the position gain coefficient a. Specifically, the estimated height is obtained by adding the predicted height and the position gain coefficient a and then multiplying the result by the residual value, wherein the position gain coefficient a is preset to 0.2. Further, when the application scenario is a vertical take-off and landing aircraft, the position gain coefficient a is set to a fixed value, thereby effectively smoothing the height estimation curve and reducing the control instability caused by sharp fluctuations.

[0063] In the simulation flight environment, the cooperative debugging of the position gain coefficient a and the PID parameters of the cascade controller is repeatedly tested, the response time and stability indexes of the aircraft are optimized, and the model of the aircraft is further optimized The iterative parameter adjustment and optimization of the filter are continuously performed to match the PID input, so that the tracking effect of the height is more accurate.

[0064] The estimated vertical speed at the current time is calculated by using the predicted vertical speed, β, residual error and sampling time, wherein β is preset as 0.05. With the introduction of the sampling time, the sensitivity of the speed estimation to the change of the residual error is greatly improved.

[0065] When it is applied to a control scene with high requirements for space and time, such as hovering of a rotor aircraft, if the sampling time is relatively short, the deviation of the speed caused by the rapid change of the height is quickly corrected. The joint optimization of the parameters of a, β and PID is performed to cooperatively optimize the position gain coefficient a and the PID parameters of the cascade controller, so that the adaptability of the aircraft and the rapid and correct response to complex scenes are improved.

[0066] In the prediction height calculation, the residual error is generated by combining the GPS and barometer data, and then a and β are used for estimation and updating. The fixed value of the parameter is matched with the PID parameter through joint debugging, so that the trajectory planning capability of the aircraft is optimized.

[0067] The joint debugging method comprises: cooperative debugging of filter parameters and PID parameters of a cascade controller. Specifically, a dynamic model of an aircraft is established, the response data of the input disturbance signal of the aircraft are collected, and a series of work such as optimization and adjustment of the parameters of the aircraft are provided to provide a reliable theoretical basis. Based on continuous debugging and optimization, the simulation parameters of the wind speed are gradually adjusted, the accuracy of the prediction is continuously evaluated, and the corresponding fine adjustment of β is performed, so that the response of the speed is improved. Not only the cruising performance of the aircraft is improved, but also the applicability of various flight obstacle avoidance and different application scenarios is also improved. When the sampling rate of the sensor is increased to 20 times per second, the accuracy of the prediction calculation is also improved, especially the timely reflection of the residual error on the position change of the aircraft is more obvious. Through the continuous and repeated execution of the filtering process of the whole aircraft control cycle, the real-time optimization effect of the filtering process is ensured. Based on the fine estimation and prediction of the height and speed, the aircraft can maintain a stable running state in a complex external environment.

[0068] In the embodiment, height and vertical velocity data are acquired from an inertial navigation device, and estimated height data are acquired from a barometric pressure sensor, the height and vertical velocity data are converted into frequency domain signals by Fourier transform, high frequency components above a preset frequency threshold and low frequency components below the preset frequency threshold are separated, the high frequency components are derived from the inertial navigation height and vertical velocity, and the low frequency components are derived from the barometric estimated height, and a preliminary fused height sequence is obtained by combination. For the preliminary fused height sequence, a sliding window method is used to calculate a variance value of the barometric estimated height in a recent time period, and a comparison result of the variance value and a preset threshold is determined. According to the comparison result, if the variance value exceeds the preset threshold, a linear interpolation method is used to increase a weight value of the inertial navigation height and vertical velocity and to decrease a weight value of the barometric estimated height, so as to obtain an adjusted fused weight combination. If the variance value does not exceed the preset threshold, a linear interpolation method is used to increase the weight value of the barometric estimated height and to decrease the weight value of the inertial navigation height and vertical velocity, and a current voltage level is acquired based on a voltage of a vehicle battery and a current amplitude level is acquired based on a vibration amplitude, so as to dynamically fine-tune the preset threshold and obtain an updated threshold. The preliminary fused height sequence is subjected to weighted average by using the adjusted fused weight combination, and the updated threshold is used to verify the fusion stability, so as to obtain final fused height data.

[0069] The fused height data are optimized by frequency component separation; the high frequency components above a preset frequency threshold are inertial navigation height and vertical velocity, which capture rapid dynamic change data or parameters of instantaneous response of the vehicle in sharp turns or acceleration through high frequency components; The low frequency components below the threshold are barometric estimated height, which is used to provide a long-term stable height reference; low frequency separation is calculated based on Fourier transform or filter method, so that the fused data take into account both short-term accuracy and long-term reliability.

[0070] By correcting the high frequency components of the unmanned aerial vehicle, the interference of external wind disturbance on the navigation of the unmanned aerial vehicle can be better eliminated, and the influence of atmospheric pressure drift on the unmanned aerial vehicle is corrected through the low frequency components, so that the navigation of the unmanned aerial vehicle is more stable and reliable Further, the variance of the barometric estimated height in a recent time period is calculated by using a sliding window, so as to quantify the fluctuation degree of the barometric data; the corresponding variance can also reflect the noise level of the barometric sensor, and when the vehicle flies at high altitude, if the atmospheric conditions are stable, the variance is low; otherwise, in the turbulent area, the variance increases.

[0071] Based on the comparison between the variance and the set threshold, the fused weight is dynamically adjusted; if the variance exceeds the preset threshold range, the reliability of the barometric data decreases.

[0072] At this time, the weight of the inertial navigation height and vertical speed is adjusted to make the IMU signal more stable.

[0073] When the variance does not exceed the threshold, the weight of the barometric pressure estimated height is increased; the low frequency advantage is used to realize the application versatility of the aircraft in different stages; for example, the variance may be high during the take-off stage, at which time the weight is biased towards the IMU, and during the smooth flight, the weight is biased towards the barometric pressure.

[0074] The weight adjustment is performed by linear interpolation, and the weight value of the inertial measurement unit data is positively correlated with the size of the variance, and the weight value of the barometric pressure sensor data is negatively correlated with the size of the variance; wherein, if the variance range is from 0 to the maximum value, the weight is mapped through a linear function; According to the real-time variance smooth transition weight, the estimation error of the height caused by the sudden IMU data can be avoided, and according to the different flight environments, the IMU data is preferentially used for flight control calculation in the environment with strong vibration; and in the calm condition of flight, the data of the barometric pressure is relied on for flight control calculation, thereby greatly improving the accuracy and stability of the overall height estimation.

[0075] When the battery voltage decreases, the corresponding threshold also decreases, so that the noise generated by the sensor increases. Similarly, the vibration amplitude is measured based on the accelerometer, and once the vibration amplitude increases, the threshold will be adjusted upwards accordingly, so that the change rate of the variance can be detected more sensitively; When applied to the height control system scene of a fixed-wing aircraft, the frequency components are separated, the variance is calculated, and the weight is adjusted, then the amplitude change caused by the engine vibration is introduced to fine-tune the threshold, so as to realize the purpose of reducing the height drift, thereby improving the safety of navigation; further, when the aircraft encounters sudden barometric pressure changes, the variance monitoring and linear interpolation are used to quickly optimize the data source weight, thereby realizing more robust height estimation without additional hardware.

[0076] In this embodiment, the estimated height and the vertical speed are subjected to average value smoothing processing by a preset noise filter to obtain a filtered height and a filtered speed. The displacement change amount is calculated by multiplying the filtered speed by the sampling time, the predicted height of the current period is obtained by adding the filtered height to the displacement change amount, and the predicted speed of the current period is obtained by the filtered speed.

[0077] The estimated height and the estimated vertical velocity of the last time are initialized as the prediction basis. Specifically, in the unmanned aerial vehicle height tracking system, the height value and the vertical velocity value of the last time provided by the barometer or the GPS module are used as the starting point (i.e., the initial value is obtained from sensor data or the calculation result of the previous period), so as to ensure the continuity and accuracy of the prediction, and to realize the rapid establishment of the reference benchmark of the current period, and to avoid the delay caused by starting from zero.

[0078] The displacement change is calculated by the uniform motion model (i.e., the displacement change is calculated by the vertical velocity of the last time and the sampling time interval), so as to reflect the displacement trend of the object in the vertical direction in real time.

[0079] Further, when the predicted height of the current period is obtained based on the estimated height of the last time and the displacement change, the displacement change calculated is directly added to the last height, so as to obtain the predicted value; this is used to maintain the logical continuity of the height estimation, is suitable for various uniform or approximate uniform motion scenarios, and ensures that the prediction result is consistent with the actual physical state.

[0080] The estimated vertical velocity of the last time is directly used as the predicted vertical velocity of the current period, so as to maintain the continuity of the motion state; the unnecessary speed recalculation is avoided by direct reuse, so as to maintain the stability of the overall motion model.

[0081] In one embodiment, the step S4 includes: obtaining a bias value of the outer loop based on the expected height and the fused height value, and outputting a target vertical velocity instruction; obtaining a bias value of the inner loop based on the target vertical velocity instruction and the fused vertical velocity, and generating an execution signal adjustment amount; obtaining the execution signal adjustment amount by weighted fusion based on the proportional term, the integral term and the differential term of the bias; superimposing the execution signal adjustment amount and the basic driving value to generate an actuator control signal, the actuator control signal being a pulse width modulation signal; and applying the pulse width modulation signal to the actuator motor of the aircraft through a driving circuit, so as to control the attitude and the height of the aircraft by adjusting the motor lift.

[0082] In this embodiment, the bias between the expected height and the fused height value is obtained, and the target vertical velocity instruction is calculated by a proportional-integral-differential controller, wherein the proportional-integral-differential controller obtains the vertical velocity control basis by weighted fusion based on the proportional term, the integral term and the differential term of the bias. The execution signal adjustment amount is generated by combining the proportional-integral controller and the bias between the target vertical velocity instruction and the fused vertical velocity, wherein the execution signal adjustment amount is obtained by weighted fusion based on the proportional term and the integral term of the bias. The proportional-integral controller is used to integrate and accumulate the vertical velocity bias, the execution signal adjustment amount and the basic driving value are superimposed to generate a pulse width modulation signal, The input of the motor drive is determined by adjusting the duty cycle of the pulse width modulation signal fused with the adjustment amount, and finally the pulse width modulation signal is applied to the execution motor through the drive circuit. The drive circuit converts the signal into current output mode to adjust the motor lift, so as to realize the purpose of attitude and height control of the aircraft.

[0083] In this embodiment, the purpose of precise control of the aircraft is achieved through attitude adjustment of the inner ring and height adjustment of the outer ring. Specifically, the outer ring part calculates the deviation of the expected height and the fused height value. If the deviation is positive, it means that the current height is lower than the expectation. At this time, the outer ring calculates the target vertical speed command through the proportional-integral-derivative controller.

[0084] Among them, the expected height is the target value set by the user; the fused height value is the current real-time flight state, which is obtained by weighted average of multi-sensor data such as barometer and GPS module.

[0085] In this embodiment, the proportional-integral-derivative controller quickly responds to the deviation through the proportional term, eliminates the steady-state error through the integral term, and predicts the trend through the derivative term, so as to obtain a suitable speed command, and further realize the stable and precise guidance of the aircraft rising.

[0086] Further, the fused vertical speed is calculated by combining the accelerometer and gyroscope data to reduce noise interference.

[0087] Further, after calculating the deviation between the expected height and the fused height value, the execution signal adjustment amount is generated by the proportional-integral controller. The execution signal adjustment amount is obtained by weighted fusion of the proportional term, the integral term and the derivative term of the deviation value calculated after the deviation calculation. The proportional term is used for immediate correction, the integral term is used for cumulative historical deviation to realize no steady-state error, and the derivative term monitors the speed change rate to avoid overshoot flight. In the hovering scene of the aircraft, if the target vertical speed is 0 meters per second and the actual fused speed is 0.5 meters per second downward, the inner ring will calculate a positive adjustment amount to compensate.

[0088] In this embodiment, the pulse width modulation signal is generated by superimposing the execution signal adjustment amount and a basic drive value. The basic drive value is a motor reference signal corresponding to the self-weight balance speed value of the aircraft. By fine-tuning the reference, the purpose of accurate signal control is achieved. In a multi-rotor unmanned aerial vehicle, the lift change can be smooth. In the process of applying the pulse width modulation signal to the execution motor of the aircraft, the drive circuit includes a power amplifier and a switching element, which converts the digital signal into a current available to the motor; adjust the motor lift to control the attitude and height of the aircraft, for example, in a wind disturbance environment, increase the motor speed to increase the lift and maintain the horizontal attitude; thereby reducing external interference and ensuring the stability of flight.

[0089] In one embodiment, the method is implemented by an aircraft control system based on dual-sensor fusion barometric altitude compensation and attitude solution, the system comprising: a sensing module comprising a barometric pressure sensor and an inertial measurement unit connected with a microcontroller through a bus, for collecting environmental air pressure data, acceleration and angular velocity data at a preset sampling rate; a communication module connected with the microcontroller, for receiving external instructions; a power module comprising a boost circuit and a voltage stabilizing circuit, for providing stable voltage for the system; an execution module comprising a motor and its driving circuit controlled by the driving signal generated by the microcontroller; an interaction module connected with the microcontroller, for displaying flight parameters calculated based on raw data.

[0090] In the present application, the microcontroller is configured to perform the following operations: obtaining environmental air pressure data from the barometric pressure sensor, and collecting three-axis acceleration values and three-axis angular velocity values from the inertial measurement unit; performing temperature compensation processing on the environmental air pressure data, and calculating corresponding raw altitude values through a preset conversion model; for the collected three-axis acceleration values and three-axis angular velocity values, generating fused attitude quaternions using an attitude solution method, and performing coordinate conversion based on the attitude quaternions to calculate Euler angle data for flight control; filtering the raw altitude values to generate a barometric estimated altitude; and converting acceleration values in the body coordinate system to the geographic coordinate system based on the attitude quaternions, and determining corresponding navigation altitude and vertical velocity values through integral operation; using an adaptive complementary filtering strategy to fuse the barometric estimated altitude, navigation altitude and vertical velocity values, wherein if the fluctuation of the barometric estimated altitude exceeds a preset threshold, the fusion weight of the barometric estimated altitude is reduced and the fusion weight of the navigation altitude is increased, to output fused altitude values and vertical velocity values; constructing a cascade control structure, setting the outer loop as the altitude control loop and the inner loop as the vertical velocity control loop, calculating the target vertical velocity command according to the deviation of the fused altitude values from the expected altitude; in the inner loop, generating corresponding actuator control signals according to the deviation of the target vertical velocity command from the fused vertical velocity value; driving the motor through the control signals to adjust the attitude of the aircraft to stabilize the altitude; continuously monitoring the fused altitude values and vertical velocity values, and if the altitude value deviates from the expected range, recalculating the target vertical velocity command and updating the actuator control signals to maintain the altitude stabilization state of the aircraft.

[0091] Obviously, those skilled in the art can make various modifications and variations to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalent technologies, the present application also intends to include these modifications and variations.

Claims

1. An aircraft control method based on dual-sensor fusion for barometric altitude compensation and attitude solution, characterized in that, The method comprises the following steps: Step S1, acquiring environmental air pressure data collected by an air pressure sensor and three-axis acceleration and three-axis angular velocity data collected by an inertial measurement unit; Step S2, performing attitude solving on the three-axis acceleration and three-axis angular velocity data to obtain fused attitude quaternions; and converting the attitude quaternions into Euler angles for flight control, wherein the Euler angles comprise a pitch angle and a roll angle; Step S3, processing the raw height value to obtain an air pressure estimated height; converting three-axis acceleration in a body coordinate system to a geographic coordinate system based on the attitude quaternions and calculating an inertial navigation height and a vertical velocity; and fusing the air pressure estimated height, the inertial navigation height and the vertical velocity, wherein a fusion weight is dynamically adjusted according to a variance of the air pressure estimated height, and a fused height value and a vertical velocity value are output; Step S4, constructing a cascade controller comprising an outer loop and an inner loop, wherein the outer loop is a height control loop and the inner loop is a vertical velocity control loop; the height control loop calculates a target vertical velocity command according to a deviation between an expected height and the fused height value; and the vertical velocity control loop generates an actuator control signal according to a deviation between the target vertical velocity command and the fused vertical velocity, and drives a motor of the aircraft to perform flight control by using the actuator control signal.

2. The aircraft control method based on dual-sensor fusion for air pressure height compensation and attitude solving according to claim 1, wherein in step S1, the following steps are further included: acquiring temperature sensor data of the air pressure sensor, and correcting an air pressure value to obtain a compensated air pressure value; and converting the compensated air pressure value into a raw height value according to a standard atmospheric model. In the method, the air pressure sensor is used for height measurement, and the inertial measurement unit is used for attitude sensing; and a sea level standard atmospheric pressure is used as reference data of the standard atmospheric model.

3. The aircraft control method based on dual-sensor fusion for air pressure height compensation and attitude solving according to claim 2, wherein in step S2, the following steps are further included: sequentially performing zero drift calibration, scale transformation and normalization processing on the three-axis acceleration and three-axis angular velocity data to obtain preprocessed data; wherein the zero drift calibration is calibrated by collecting multiple samples statically when powered on and calculating an average value as a zero offset value. Based on the preprocessed angular velocity data and the quaternion of the last period, the predicted attitude quaternion of the current period is obtained by updating calculation through a quaternion differential equation.

4. The aircraft control method based on dual-sensor fusion for air pressure height compensation and attitude solving according to claim 3, wherein the following steps are further included: using the preprocessed acceleration data, based on a gravity vector error determined by a cross product of a normalized acceleration vector and a predicted gravity vector, the predicted attitude quaternion is modified by using a gradient descent method to obtain an optimal quaternion; wherein the gradient descent method processes the gravity vector error based on a preset correction step, and adjusts the predicted attitude quaternion by using the processing result. ​ ​ ​ ​ convert the optimal quaternion into Euler angles including a roll angle, a pitch angle and a heading angle for flight control; wherein the roll angle is calculated based on a specific element of the optimal quaternion by an inverse tangent function, the pitch angle is calculated based on an element of the optimal quaternion by an inverse sine function, and the heading angle is calculated based on an element of the optimal quaternion by an inverse tangent function.

5. The dual-sensor fusion based barometric altitude compensation and attitude computation method for aircraft control according to claim 4, wherein, The step S2 comprises: calculating a projection of a theoretical gravity vector in a body coordinate system based on the optimal quaternion; obtaining an actually measured gravity vector based on the preprocessed acceleration data; and constructing an objective function for optimization based on a difference between the theoretical gravity vector and the actually measured gravity vector; iterative solving of the objective function based on a gradient descent method, wherein in each iteration, the quaternion is adjusted based on a preset correction step and a gradient direction of the objective function to obtain the optimal quaternion for suppressing long-term drift; wherein the iterative calculation process of the gradient descent method is realized by fixed-point number operation and lookup table method to improve the optimization calculation efficiency.

6. The dual-sensor fusion based barometric altitude compensation and attitude computation method for aircraft control according to claim 1, wherein, The step S3 comprises: processing the original height value, including: obtaining a predicted height value of the current period based on the estimated height of the previous moment, the estimated vertical velocity of the previous moment and the sampling time; the predicted vertical velocity uses the estimated vertical velocity of the previous moment as the predicted value of the current period; obtaining a height measurement residual value based on the predicted height value and the current original height value; calculating the predicted height, the height measurement residual value and the position gain coefficient a to obtain the estimated height at the current moment; calculating the predicted vertical velocity, the height measurement residual value, the velocity gain coefficient β and the sampling time to obtain the estimated vertical velocity at the current moment; wherein the position gain coefficient a and the velocity gain coefficient β are preset as fixed values, a = 0.2 and β = 0.05; the sampling time is the sensor sampling rate.

7. The aircraft control method based on dual-sensor fusion for barometric altitude compensation and attitude calculation according to claim 6, wherein, The step S3 comprises: in the fused barometric estimated height data, the high-frequency component higher than a preset frequency threshold is the inertial navigation height and the vertical velocity; and the low-frequency component lower than the preset frequency threshold is the barometric estimated height; calculating the variance of the barometric estimated height in the recent time period through a sliding window; based on the comparison result of the variance and the preset frequency threshold, dynamically adjusting the fusion weight; wherein when the variance exceeds the preset frequency threshold, the weight of the inertial navigation height and the vertical velocity is increased; and when the variance does not exceed the preset frequency threshold, the weight of the barometric estimated height is increased; adjusting the weight of the barometric estimated height through linear interpolation, wherein the data weight value of the inertial measurement unit is positively correlated with the size of the variance, and the data weight value of the barometric sensor is negatively correlated with the size of the variance; the preset frequency threshold is dynamically fine-tuned based on the battery voltage or the vibration amplitude of the aircraft.

8. The aircraft control method based on dual-sensor fusion for barometric altitude compensation and attitude calculation according to claim 6, wherein, The step S3 comprises: initializing the estimated height and the estimated vertical velocity of the previous moment as the prediction basis; Based on the estimated vertical velocity of the last moment and the sampling time, the displacement change amount in the current period is calculated; Based on the estimated height of the last moment and the displacement change amount, the predicted height of the current period is obtained; the estimated vertical velocity of the last moment is directly used as the predicted vertical velocity of the current period to maintain the continuity of the motion state.

9. The dual-sensor fusion based barometric altitude compensation and attitude computation method for aircraft control as claimed in claim 8, wherein, Step S4 includes: obtaining the deviation value of the outer loop based on the expected height and the fused height value, and outputting the target vertical velocity instruction; Obtain the deviation value of the inner loop based on the target vertical velocity instruction and the fused vertical velocity, and generate the execution signal adjustment amount; The weighted fusion is carried out based on the proportional term, integral term and differential term of the deviation to obtain the execution signal adjustment amount; the execution signal adjustment amount is superimposed with the basic driving value to generate the actuator control signal, and the actuator control signal is a pulse width modulation signal; The pulse width modulation signal is applied to the actuator motor of the aircraft through the driving circuit, and the attitude and height of the aircraft are controlled by adjusting the motor lift.

10. The dual-sensor fusion based barometric altitude compensation and attitude computation method for aircraft control according to any one of claims 1-9, wherein, The method realizes aircraft control through an aircraft control system based on double-sensor fusion air pressure height compensation and attitude calculation, the system includes: a sensing module including an air pressure sensor and an inertial measurement unit connected with a controller through a bus, for collecting environmental air pressure data, acceleration and angular velocity data; An execution module controlled by the driving signal generated by the controller.

Images