A method for pose and heading control of an unmanned vehicle

Abstract

The application discloses a pose and heading control method of unmanned equipment, and belongs to the technical field of unmanned equipment control. The method comprises the following steps: performing data fusion on collected multi-source heterogeneous data of the unmanned equipment, and outputting pose estimation data of the unmanned equipment; predicting the influence of an external environment on the unmanned equipment based on a preset dynamic model, a preset model credibility and a current wind disturbance coefficient of the unmanned equipment, so as to obtain an optimal heading control strategy of the unmanned equipment; based on the optimal heading control strategy and the pose estimation data, calculating a heading deviation and a roll pose of the unmanned equipment at a future time for the purpose of offsetting wind disturbance, and outputting a final control torque of the unmanned equipment; and controlling the pose and the heading of the unmanned equipment through the final control torque. Therefore, by implementing the application, the problem that the prior art is difficult to control the heading when facing a strong wind, and the problem that the pose control and the heading control are not coordinated due to separate design can be solved.

Description

Technical Field

[0001] This application belongs to the field of unmanned equipment control technology, specifically relating to a method for attitude and heading control of unmanned equipment. Background Technology

[0002] With the widespread application of intelligent unmanned equipment such as drones and unmanned surface vessels in military and civilian fields, their autonomous control technology has gradually become a research hotspot. Currently, the mainstream control strategy for these unmanned equipment mostly adopts proportional-integral-derivative (PID) controllers, which are widely used in attitude and heading control systems due to their simple structure and ease of implementation. At the same time, some modern control methods, such as model predictive control technology, are also gradually being introduced to improve the performance of unmanned equipment in dynamic environments. Furthermore, attitude control and heading control are usually designed as two independent subsystems, supplemented by a combination of various sensors such as inertial measurement units, GPS, and magnetometers to achieve system state estimation.

[0003] While the aforementioned methods meet basic control requirements to some extent, they exhibit significant limitations in complex dynamic environments. Proportional-integral-derivative (PID) controllers, due to their fixed parameters, struggle to adapt to time-varying disturbances such as sudden winds and waves, leading to decreased control performance. Model predictive control, while highly accurate, suffers from high computational complexity, making real-time operation difficult on resource-constrained embedded platforms. Furthermore, the separate design of attitude and heading control ignores the dynamic coupling effect between them, making them prone to system oscillations or instability during rapid maneuvers. In addition, existing technologies lack effective dynamic compensation mechanisms for practical issues such as sensor errors and actuator response delays, further limiting the control accuracy and stability of unmanned equipment. Summary of the Invention

[0004] This application proposes a method for attitude and heading control of unmanned equipment, which can solve the problems of difficulty in heading control when facing strong winds in the prior art, and the lack of coordination in the control due to the separate design of attitude control and heading control.

[0005] The first aspect of this application provides a method for attitude and heading control of an unmanned equipment, the method comprising: Data fusion is performed on the multi-source heterogeneous data collected from unmanned equipment to output pose estimation data of the unmanned equipment; wherein, the multi-source heterogeneous data includes inertial data, geomagnetic data and satellite navigation data; By using a pre-defined dynamic model, the impact of the external environment on the unmanned equipment is predicted based on the pre-defined model confidence level and the current wind disturbance coefficient of the unmanned equipment, and the optimal heading control strategy of the unmanned equipment is obtained; wherein, the dynamic model is constructed based on the heading angular velocity, control torque and damping coefficient of the unmanned equipment control system. Based on the optimal heading control strategy and the attitude estimation data, with the goal of counteracting wind disturbance, the heading deviation and roll attitude of the unmanned equipment at future moments are calculated, and the final control torque of the unmanned equipment is output. The attitude and heading of the unmanned equipment are controlled by the final control torque.

[0006] The above scheme collects flight data from three types of unmanned equipment with different attributes, resulting in multi-source heterogeneous data. This multi-source heterogeneous data is fused together to achieve mathematical coupling between attitude and heading, resolving the control incoordination problem caused by separate design of attitude and heading. When constructing the optimal heading control strategy for the unmanned equipment, the impact of external environmental factors such as strong winds and waves on the unmanned equipment is considered, along with the errors in the dynamic model itself. Real-time perception and adaptive adjustment of environmental changes are achieved, significantly improving the stability and robustness of the unmanned equipment's attitude and heading control under complex conditions. Then, the specific current wind disturbance coefficient is converted into a corresponding control torque, and the heading deviation of the unmanned equipment is converted into a corresponding correction torque. These two torques generate the final control torque of the unmanned equipment, pre-balancing the wind load before a substantial deviation in the unmanned equipment's attitude occurs. This significantly reduces the impact of strong winds on the unmanned equipment's attitude and avoids severe roll caused by emergency control, thus preventing the risk of the equipment tipping over.

[0007] In one possible implementation of the first aspect, data fusion is performed on the multi-source heterogeneous data collected from the unmanned equipment to output the pose estimation data of the unmanned equipment, specifically as follows: Acquire the pose and heading information of the unmanned equipment and construct the system state vector of the unmanned equipment; Based on the satellite navigation data, the motion acceleration and gravity components of the unmanned equipment are calculated, and the attitude components of the system state vector are corrected using the motion acceleration and gravity components. The heading component of the system state vector is corrected using geomagnetic data; Based on the inertial data, the pose estimation data and the confidence matrix of the pose estimation data are obtained through the corrected system state vector.

[0008] The above scheme collects high-frequency inertial data, low-frequency geomagnetic data, and medium-low frequency satellite navigation data respectively, and fuses these multi-source heterogeneous data together to achieve unified fusion of attitude and heading, improve the integrity and accuracy of the data, and transform the attitude and heading of the unmanned equipment into a unified system state vector.

[0009] In one possible implementation of the first aspect, the inertial data is obtained by acquiring the three-axis angular velocity and three-axis acceleration of the unmanned equipment through an inertial measurement unit. The geomagnetic data were obtained by collecting triaxial geomagnetic field strength using a magnetometer. The absolute position and ground velocity vector of the unmanned equipment are collected through a satellite navigation system to obtain the satellite navigation data. The inertial data, geomagnetic data, and satellite navigation data are subjected to coordinate system transformation, magnetometer calibration, and outlier removal.

[0010] In one possible implementation of the first aspect, the impact of the external environment on the unmanned equipment is predicted based on a preset dynamic model, the preset model confidence level, and the current wind disturbance coefficient of the unmanned equipment, thereby obtaining the optimal heading control strategy for the unmanned equipment, specifically as follows: The control torque and the heading angular velocity of the previous moment are input into the preset wind disturbance prediction model to calculate the deviation between the model prediction value and the sensor measurement value. The preset initial wind disturbance coefficient is updated by the deviation change to obtain the current wind disturbance coefficient. The performance of the dynamic model is evaluated to obtain the model's reliability. Based on the current wind disturbance coefficient, the model reliability, and the preset target heading error, the influence of the external environment on the trajectory of the unmanned equipment is analyzed, and a comprehensive evaluation value is calculated. The optimal heading control strategy is obtained by adjusting the weights of the heading control strategy based on the magnitude of the comprehensive evaluation value.

[0011] The above scheme updates the wind disturbance coefficient in real time by comparing the deviation between the model's predicted values ​​and the sensor's measured values. The current wind disturbance coefficient measures the combined force of wind and waves on the unmanned equipment, providing strong data support for subsequent wind disturbance mitigation. Furthermore, it innovatively considers the impact of the model's performance on the accuracy of the prediction results, combining the current wind disturbance coefficient with the model's reliability to jointly analyze the impact of the external environment on the unmanned equipment's trajectory. This allows for a more accurate estimation of the external environment's disturbance to the unmanned equipment's heading, resulting in a comprehensive evaluation value that better reflects the actual hydrological and meteorological environment, thus providing a foundation for obtaining the optimal heading control strategy.

[0012] In one possible implementation of the first aspect, based on the current wind disturbance coefficient, the model reliability, and the preset target heading error, the influence of the external environment on the trajectory of the unmanned equipment is analyzed, and a comprehensive evaluation value is calculated, specifically as follows: Calculate the ratio of the current wind disturbance coefficient to the preset maximum wind resistance capacity to obtain the first ratio; Calculate the ratio of the model's credibility to a preset credibility threshold to obtain a second ratio; The first ratio, the second ratio, and the target heading error are weighted and calculated to obtain the comprehensive evaluation value.

[0013] The above scheme does not use a single threshold, but uses a normalization formula to evaluate the instability caused by wind disturbance from three dimensions, more accurately identifying whether the unmanned equipment is in a stable or significantly disturbed state, and obtaining a comprehensive evaluation value that can accurately describe the current working condition.

[0014] In one possible implementation of the first aspect, the heading control strategy weights are adjusted according to the magnitude of the comprehensive evaluation value to obtain the optimal heading control strategy, specifically as follows: If the comprehensive evaluation value is less than the first threshold, the heading control strategy weight is increased with the goal of reducing the energy consumption of the unmanned equipment and ensuring the smoothness of the trajectory, so as to obtain the optimal heading control strategy. If the comprehensive evaluation value is greater than or equal to the first threshold, then with the goal of resisting the impact of wind speed from the external environment, the weight of the heading control strategy is reduced to obtain the optimal heading control strategy.

[0015] In the above scheme, if the comprehensive evaluation value is small, it indicates that the unmanned equipment is in a stable operating condition. Therefore, priority should be given to reducing navigation energy loss and the smoothness of the trajectory. If the comprehensive evaluation value is large, it indicates that the unmanned equipment will be subject to strong external interference and the operating environment is harsh. Therefore, priority should be given to resisting external shocks to prevent the unmanned equipment from deviating significantly or going out of control.

[0016] In one possible implementation of the first aspect, based on the optimal heading control strategy and the attitude estimation data, with the optimization objective of counteracting wind disturbance, the heading deviation and roll attitude of the unmanned equipment at future moments are calculated, and the final control torque of the unmanned equipment is output, specifically as follows: The pose estimation data is compared with the expected pose data to obtain the pose error; wherein, the expected pose data comes from the navigation system of the unmanned equipment. Based on the pose error and the optimal heading control strategy, and considering the heading deviation and roll attitude of the unmanned equipment, a feedback control torque is generated. Based on the current wind disturbance coefficient, a counter-compensation torque of equal magnitude and opposite direction to the wind disturbance is generated; the counter-compensation torque is used to counteract the wind disturbance. The final control torque is obtained by combining the feedback control torque and the reverse compensation torque.

[0017] The above scheme first calculates the pose error of the unmanned equipment in three-dimensional coordinates. By analyzing the pose error and the optimal heading control strategy, while considering the heading deviation of the unmanned equipment, it also takes into account the rollover risk caused by excessive heading reversal, and obtains a feedback control torque that can both correct the heading and ensure the safe flight of the unmanned equipment. Then, it generates a reverse compensation torque to resist wind disturbance. The feedback control torque and the reverse compensation torque are superimposed to obtain the final control torque that can balance wind load and significantly reduce the impact of sudden heading changes on the UAV's pose, thereby improving the flight stability of the unmanned equipment under complex conditions.

[0018] In one possible implementation of the first aspect, based on the pose error and the optimal heading control strategy, considering the heading deviation and roll attitude of the unmanned equipment, a feedback control torque is generated, specifically as follows: If the unmanned equipment is in a stable operating condition, then based on the imaginary part vector of the pose error and the optimal heading control strategy, a feedback control torque is generated with the goal of minimizing the energy consumption of the unmanned equipment. If the unmanned equipment is in a strong wind disturbance condition, a feedback control torque is generated based on the sliding surface parameters and torque switching gain of the unmanned equipment to resist the wind disturbance.

[0019] In one possible implementation of the first aspect, the maximum permissible torque of the unmanned equipment is adjusted according to the current battery voltage of the unmanned equipment; if the feedback control torque is greater than the maximum permissible torque, the calculation method of the feedback control torque is adjusted.

[0020] In one possible implementation of the first aspect, the attitude and heading of the unmanned equipment are controlled by the final control torque, specifically as follows: If the current battery voltage of the unmanned equipment is less than the second threshold, the maximum permissible torque of the unmanned equipment is reduced. The limiting threshold of the final control torque control is adjusted based on the reduced maximum permissible torque.

[0021] The above solution monitors the battery voltage changes of the unmanned equipment in real time and automatically adjusts the limiting threshold of the final control torque, effectively solving the problem of inconsistent actuator response characteristics caused by battery voltage fluctuations. It prevents actuator saturation failure under low voltage conditions and ensures maximum control efficiency under high voltage conditions, enabling the unmanned equipment to achieve stable and reliable control performance across the entire operating range. This significantly improves the adaptability and safety of the unmanned equipment control system in complex power supply environments. Attached Figure Description

[0022] To more clearly illustrate the technical solution of this application, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0023] Figure 1 This is a schematic diagram illustrating the specific process of a pose and heading control method for unmanned equipment provided in one embodiment of this application. Detailed Implementation

[0024] The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0025] It should be understood that the step numbers used in the text are for ease of description only and are not intended to limit the order in which the steps are performed.

[0026] First Embodiment Unmanned equipment, such as drones and unmanned surface vessels (USVs), is often affected by waves and strong winds when operating at sea or in the air, causing severe yaw. Existing technologies, such as traditional PID controllers, typically use fixed parameters to control the heading of unmanned equipment, thus lacking quantitative assessment of environmental disturbances and failing to adapt to sudden wind disturbances. Furthermore, existing technologies often design heading control and attitude (roll / pitch) control as independent subsystems, ignoring the dynamic coupling effects during rapid maneuvers, resulting in control performance that differs significantly from the expected results.

[0027] To address the aforementioned technical shortcomings, the embodiments of this application can calculate the wind disturbance coefficient in real time and consider the reliability of model predictions. It does not solely rely on wind speed but analyzes the heading changes of the unmanned equipment from multiple dimensions, combining the wind disturbance coefficient with the reliability of model predictions to intelligently and dynamically adjust the optimal heading control strategy based on the current operating conditions of the unmanned equipment. Furthermore, it establishes a unified pose and heading coupling logic, effectively eliminating control incoordination during multi-axis motion and avoiding system oscillations caused by neglecting coupling terms. This makes it particularly suitable for highly maneuverable and complex flight or navigation scenarios of unmanned equipment.

[0028] like Figure 1As shown, to address the difficulties in heading control when facing strong winds in existing technologies, and the incoordination caused by the separate design of attitude control and heading control, the first embodiment of this application provides a detailed flowchart of an attitude and heading control method for unmanned equipment. The attitude and heading control method for unmanned equipment in this embodiment includes steps S1 to S4, which are detailed below: Step S1: Perform data fusion on the collected multi-variable heterogeneous data of the unmanned equipment and output the pose estimation data of the unmanned equipment.

[0029] For unmanned equipment, inertial data, geomagnetic data and satellite navigation data are collected separately. Then, these multi-source heterogeneous data are fused to achieve unified fusion of pose data and heading data, so as to obtain pose estimation data that can accurately describe the three-dimensional full attitude of the unmanned equipment.

[0030] The inertial data is obtained by acquiring the three-axis angular velocity and three-axis acceleration of the unmanned equipment through an inertial measurement unit. The inertial data is updated at a high frequency, typically 200Hz, but it has a zero-bias error that diverges over time.

[0031] The geomagnetic data is obtained by collecting triaxial geomagnetic field strength using a magnetometer. This geomagnetic data is used to provide an absolute heading reference, but it is susceptible to interference from soft / hard iron.

[0032] The satellite navigation data is obtained by collecting the absolute position and ground velocity vector of the unmanned equipment through a satellite navigation system. This type of data belongs to low-to-medium frequency data.

[0033] Before data fusion, the aforementioned multi-dimensional heterogeneous data is first preprocessed, including coordinate system alignment, magnetometer calibration, and outlier removal.

[0034] Specifically, the coordinate systems of the inertial measurement unit and magnetometer are initialized with the navigation coordinate system of the satellite navigation system through a transformation matrix to achieve coordinate system alignment; the original magnetic field data is corrected using an ellipsoid fitting algorithm to eliminate hard iron interference generated by the motor current of the unmanned equipment to achieve magnetometer calibration; the signal-to-noise ratio of the satellite navigation data is detected, and when the number of satellites is less than 4 or the positioning accuracy factor is too large, the data is automatically marked as invalid, and only the inertial measurement unit is used for inertial calculation.

[0035] After data preprocessing, the pose and heading information of the unmanned equipment are used to predefine the system state vector of the unmanned equipment through an improved unscented Kalman filter. The specific expression is as follows: ; in, X Let be the system state vector. q 0 , q 1 ,q 2 , q 3 To represent the three-dimensional full attitude (including pose and heading) of unmanned equipment, For the dynamic zero bias of a three-axis gyroscope, x , y and z This is the coordinate system of the three-axis gyroscope. The three-axis gyroscope is a core component of the navigation system of unmanned equipment, and its measurement accuracy directly affects the accuracy of the attitude calculation of the navigation system.

[0036] Based on the ground speed information provided by the preprocessed satellite navigation data, the motion acceleration of the unmanned equipment is calculated, and the gravity component of the unmanned equipment is calculated using the motion acceleration. The attitude component of the system state vector is then accurately corrected using the gravity component.

[0037] An observation equation is constructed based on the preprocessed geomagnetic data, and the heading component in the four-digit unit of the system state vector is corrected by the observation equation.

[0038] Based on the angular velocity data provided by the preprocessed inertial data, the pose estimation data and the confidence matrix of the pose estimation data are obtained through the corrected system state vector. At this time, the pose (roll / pitch) and heading (yaw) are mathematically coupled and evolve synchronously.

[0039] The pose estimation data is actually the corrected four-digit unit of the system state vector. These corrected four-digit unit numbers can also be called the optimal estimated quaternions, which can be converted into intuitive Euler angles (roll, pitch, yaw / heading) for use by the control system of unmanned equipment.

[0040] The confidence matrix is ​​the diagonal element of the state covariance matrix output in real time by the controller based on unscented Kalman filtering, representing the uncertainty of the current pose estimation.

[0041] To avoid the control incoordination problem caused by the separate design of attitude and heading in traditional control systems, this application embodiment uses data fusion to perform unified modeling and coupling processing of attitude and heading data. It adopts a composite control method combining quaternions and angular velocity feedforward to improve the overall dynamic performance of unmanned equipment in maneuvering flight or complex navigation.

[0042] Step S2: Using a preset dynamic model, based on the preset model confidence level and the current wind disturbance coefficient of the unmanned equipment, predict the impact of the external environment on the unmanned equipment, and obtain the optimal heading control strategy of the unmanned equipment.

[0043] This application establishes a dynamic model based on autoregressive moving average and discrete time to characterize the heading dynamics of a UAV. The expression of the dynamic model is as follows: ; in, for k The angular velocity of the heading at time t, for k Control torque at all times The system damping coefficient is... The wind disturbance coefficient for unmanned equipment. b The input gain coefficient is used to characterize the intensity of the effect of unit control torque on the heading angular velocity of unmanned equipment.

[0044] Based on the aforementioned dynamic model, this application embodiment needs to focus on calculating the wind disturbance coefficient of the unmanned equipment and the model reliability.

[0045] The wind disturbance coefficient characterizes the equivalent additional disturbance torque generated by the external airflow on the unmanned equipment. The control torque and the heading angular velocity at the previous moment are input into the preset wind disturbance prediction model. The deviation between the model prediction value and the sensor measurement value is calculated by using the recursive least squares method with a forgetting factor (actually, the deviation between the theoretical predicted heading angular velocity of the dynamic model and the measured angular velocity provided by the inertial measurement unit). The preset initial wind disturbance coefficient is updated by the deviation change to obtain the current wind disturbance coefficient of the unmanned equipment.

[0046] Optionally, the forgetting factor can be set in the range of [0.95, 0.99]. Its function is to perform exponential weighted decay on historical data, that is, to give higher weight to the latest measurement data. When a sudden gust of wind strikes, a smaller forgetting factor can allow the algorithm to quickly "forget" the previous stable state and converge to the new wind disturbance coefficient within 5 to 10 control cycles, thereby solving the problem of slow response of traditional algorithms to sudden environmental changes.

[0047] The current wind disturbance coefficient actually reflects the combined effect of wind and water flow in the external environment on unmanned equipment.

[0048] Simultaneously, the performance of the dynamic model is evaluated to obtain the model confidence level. The model confidence level reflects the "confidence" of the recursive least squares method with forgetting factor in the output results of the dynamic model.

[0049] When the model's confidence level approaches 0, it indicates that the model parameters have converged and the model is extremely accurate.

[0050] When the reliability of the model suddenly increases, it indicates that an unmodeled dynamic (such as structural damage or extreme airflow) has occurred, and the model parameters are unreliable.

[0051] Furthermore, in order to overcome the instability caused by relying solely on the current wind disturbance coefficient for torque switching, this embodiment of the application also sets up a multi-dimensional state evaluator. By using the current wind disturbance coefficient, the model credibility, and the preset target heading error, it analyzes the impact of the external environment on the trajectory of the unmanned equipment, calculates a comprehensive evaluation value, and thereby determines the optimal heading control strategy of the unmanned equipment.

[0052] The input data for the multidimensional state evaluator are the current wind disturbance coefficient, the model confidence level, and the preset target heading error.

[0053] Based on the input data, a continuous comprehensive evaluation value is calculated in real time using a normalization formula. The specific calculation formula is as follows: ; in, J severity This is the comprehensive evaluation value; w 1. w 2. w 3 are all weighting coefficients. D max This is the preset maximum wind resistance capacity. The current wind disturbance coefficient, Tr ( P The confidence level of the model is represented by ). The confidence threshold is... Let be the target heading error.

[0054] The comprehensive evaluation value is then mapped to the (0,1) interval to generate a dynamic weight. When the comprehensive evaluation value is small, the dynamic weight approaches 1; when the comprehensive evaluation value is large, i.e., the unmanned equipment is in a harsh working condition or the dynamic model is distorted, the dynamic weight approaches 0.

[0055] Therefore, if the comprehensive evaluation value is less than the first threshold, the heading control strategy weight is increased by using channel A through the dynamic weight to reduce the energy consumption of the unmanned equipment and ensure the smoothness of the trajectory, so as to obtain the optimal heading control strategy and ensure the smoothness and energy saving of the trajectory tracking.

[0056] If the comprehensive evaluation value is greater than or equal to the first threshold, then the dynamic weighting of channel B is used to reduce the heading control strategy weight with the goal of resisting the impact of wind speed in the external environment, so as to obtain the optimal heading control strategy and prevent the UAV equipment from deviating significantly or losing control.

[0057] The formula for calculating the optimal heading control strategy is as follows: ; in, ufinal This is the optimal heading control strategy. For the dynamic weight, u LQR This is the output of channel A. u SMC This is the output of channel B.

[0058] The above scheme adjusts the optimal heading control strategy according to the actual working conditions of the unmanned equipment, realizing real-time perception and adaptive adjustment of environmental changes, thereby significantly improving the stability and robustness of the control system under complex working conditions.

[0059] Furthermore, for step heading commands sent from the ground station (such as "turn right 90 degrees immediately"), the command is not directly sent to the UAV's control system. Instead, a third-order trajectory planning algorithm is used to generate an S-shaped reference curve with continuous position, velocity, and acceleration. This ensures that the error signal input to the control system is smooth and continuous, eliminating the motor current surges and overshoot caused by sudden changes in attitude and heading control parameters.

[0060] Step S3: Based on the optimal heading control strategy and the attitude estimation data, with the aim of counteracting wind disturbance, calculate the heading deviation and roll attitude of the unmanned equipment at future moments, and output the final control torque of the unmanned equipment.

[0061] This application embodiment adopts an error feedback architecture based on the four-digit number of the unit to achieve strong coupling control of the three-axis attitude. Combined with the optimal heading control strategy and the current wind disturbance coefficient, environmental disturbances are offset through a feedforward mechanism to output the final control torque that can accurately correct the attitude and heading of the unmanned equipment.

[0062] The optimal estimated quaternion is extracted from the pose estimation data, and the optimal estimated quaternion is compared with the expected pose data provided by the navigation system of the unmanned equipment to obtain the pose error.

[0063] Specifically, the pose error is calculated using the quaternion conjugate penalty, and the calculation formula is as follows: ; in, q err For pose error, q curr To estimate the quaternion, The desired pose data; q e0 , q e1 , q e2 , q e3These are the four components of the unit four-digit number representing the pose error, where... q e0 For real scalar components, q e1 , q e2 , q e3 These are the imaginary error components along the x, y, and z axes of the unmanned equipment's body coordinate system, respectively. The imaginary part of the pose error directly represents the error components that need to rotate around the three axes in the current coordinate system of the unmanned equipment. The imaginary part can be used to solve the heading deviation and roll attitude in a unified manner. When outputting the turning command to the unmanned equipment, the current roll angle will be taken into account to prevent the attitude of the unmanned equipment from becoming unstable due to over-turning. Therefore, it reflects the strong coupling of the three-axis attitude.

[0064] The embodiments of this application mainly calculate the feedback control torque and the reverse compensation torque. The feedback control torque is used to reduce the energy cost of high-precision navigation and resist the impact of wind disturbance on unmanned equipment. The reverse compensation torque is used to balance the wind load on the unmanned equipment, thereby significantly reducing the impact of gusts on the attitude of the unmanned equipment.

[0065] If the unmanned equipment is in a stable operating condition, then based on the imaginary part vector of the pose error and the optimal heading control strategy, a feedback control torque is generated with the goal of minimizing the energy consumption of the unmanned equipment. The specific calculation formula is as follows: ; in, T fb For feedback control torque, K lqr The state feedback gain matrix is ​​the output of the optimal heading control strategy using channel A. q vec Let be the imaginary part vector of the pose error. This is the angular velocity error vector.

[0066] If the unmanned equipment is operating under strong wind disturbance conditions, a feedback control torque is generated based on the sliding surface parameters and torque switching gain of the unmanned equipment, with the goal of resisting wind disturbance. The specific calculation formula is as follows: ; in, T fb For feedback control torque, For torque switching gain, S Let Sgn be the parameter of the sliding surface. Sgn(·) is the sign function.

[0067] For the reverse compensation moment, based on the current wind disturbance coefficient, a reverse compensation moment with the opposite direction and equal magnitude to the wind disturbance is generated.

[0068] The final control torque of the unmanned equipment is obtained by calculating the vector sum of the feedback control torque and the reverse compensation torque. By directly converting the current wind disturbance coefficient into a reverse torque and adding it to the control output, the system can "pre-balance" the wind load before a substantial deviation in attitude occurs, thereby significantly reducing the impact of gusts on the attitude of the unmanned equipment.

[0069] As an improvement to the above scheme, after synthesizing the final control torque, further adjustments are made based on the current battery voltage of the unmanned equipment, and the maximum allowable torque of the unmanned equipment is dynamically adjusted according to the current battery voltage.

[0070] If the absolute value of the feedback control torque is greater than the maximum allowable torque, the calculation method of the feedback control torque is adjusted: the accumulation of the integral term is frozen, and the damping enhancement function of the derivative term is activated to prevent overshoot or oscillation of the control system caused by overload of the actuator.

[0071] Step S4: Control the attitude and heading of the unmanned equipment through the final control torque.

[0072] While controlling the attitude and heading of the unmanned equipment through the final control torque, the current battery voltage of the unmanned equipment is also detected in real time.

[0073] If the current battery voltage of the unmanned equipment is less than the second threshold, the maximum permissible torque of the unmanned equipment is automatically reduced to adjust the limiting threshold of the final control torque control.

[0074] This effectively solves the problem of inconsistent actuator response characteristics caused by battery voltage fluctuations. This design not only prevents actuator saturation failure under low voltage conditions but also ensures maximum control efficiency under high voltage conditions, enabling unmanned equipment to obtain stable and reliable control performance across the entire operating range. This significantly improves the adaptability and safety of the unmanned equipment's control system in complex power supply environments.

[0075] Implementing the embodiments of this application has the following beneficial effects: This application's embodiments collected flight data from three types of unmanned equipment with different attributes, obtaining multi-source heterogeneous data. This multi-source heterogeneous data was fused together to achieve mathematical coupling between attitude and heading, resolving the control incoordination problem caused by separate design of attitude and heading. When constructing the optimal heading control strategy for the unmanned equipment, the impact of external environmental factors such as strong winds and waves on the unmanned equipment was considered, along with the errors in the dynamic model itself. Real-time perception and adaptive adjustment of environmental changes were achieved, significantly improving the stability and robustness of the unmanned equipment's attitude and heading control under complex conditions. Then, the specific current wind disturbance coefficient was converted into a corresponding control torque, and the heading deviation and rollover risk of the unmanned equipment were converted into corresponding correction torques. These two torques were used to generate the final control torque of the unmanned equipment, pre-balancing the wind load before a substantial deviation in the unmanned equipment's attitude occurred. This significantly reduced the impact of strong winds on the unmanned equipment's attitude and avoided severe rollover caused by emergency control, preventing the equipment from rolling laterally when adjusting its heading.

[0076] Second Embodiment Furthermore, in order to demonstrate the pose and heading control method of the unmanned equipment provided in the above method embodiments, this application embodiment takes an unmanned surface vessel as the object for pose and heading control, and the specific control process is as follows.

[0077] 1. Acquisition and fusion of multi-source heterogeneous data.

[0078] First, the inertial measurement unit collects the three-axis angular velocities (with a focus on yaw and roll angular velocities) and three-axis accelerations of the hull at a frequency of 200 Hz; the electronic compass collects the geomagnetic field intensity; and the satellite navigation system provides absolute position and ground speed information at a frequency of 10 Hz.

[0079] The multi-source heterogeneous data collected from unmanned equipment are preprocessed. For example, considering the hard iron interference generated by the motor current of the unmanned vessel, the magnetometer data is first calibrated by ellipsoid fitting. At the same time, the number of satellites and accuracy factor of the GPS signal are monitored. If the accuracy factor is too large due to bridge obstruction or multipath effect, the data frame is automatically marked as invalid and only inertial calculation is used to prevent the heading divergence caused by positioning drift.

[0080] The preprocessed multi-source heterogeneous data is input into an improved unscented Kalman filter, which outputs two sets of key data: The pose estimation data of unmanned equipment represents the "true value" of the current three-dimensional attitude (roll / pitch) and heading of the unmanned surface vessel, solving the measurement noise problem caused by water surface wave undulation.

[0081] The confidence matrix of the pose estimation data represents the reliability of the pose estimation data.

[0082] 2. Identify disturbances caused by the external environment to the unmanned surface vessel and output the optimal heading control strategy for the unmanned equipment.

[0083] While acquiring the pose estimation data, the hydrological and meteorological environment outside the unmanned surface vessel is assessed in parallel.

[0084] First, a dynamic model based on autoregressive moving average and discrete time is established. By comparing the deviation between the theoretically predicted heading angular velocity of the dynamic model and the measured angular velocity provided by the inertial measurement unit, the current wind disturbance coefficient of the unmanned equipment, which is equivalent to the combined force of wind and water flow, is output.

[0085] For example, suppose an unmanned surface vessel suddenly encounters strong crosswinds or crashing waves (a sudden environmental change) during navigation. The recursive least squares algorithm will immediately detect the amplified deviation and the deviation will converge rapidly within 5 to 10 control cycles, calculating the current wind disturbance coefficient.

[0086] Then, combining the current wind disturbance coefficient, the preset model reliability, and the preset target heading error, a comprehensive evaluation value is calculated. If the comprehensive evaluation value is small, it indicates that the sea surface is calm, and the optimal heading control strategy for the unmanned equipment is output while prioritizing the smoothness of track tracking and energy saving. If the comprehensive evaluation value is large, it indicates that the sea state is bad and the wind and waves are large, and the optimal heading control strategy for the unmanned equipment is output while preventing the hull from deviating significantly or losing control.

[0087] 3. Perform coupled calculations on the pose and heading of the unmanned equipment to output the final control torque of the unmanned equipment.

[0088] First, the output heading command is softened. Assuming a shore-based emergency obstacle avoidance command of "immediately turn 90 degrees right" is sent, the large angular deviation is not directly fed into the controller. Instead, an S-shaped reference curve is generated through third-order trajectory planning. This ensures that the error signal input to the controller is continuous in angular velocity and angular acceleration, avoiding severe heeling (risk of capsizing) caused by sharp rudder maneuvers.

[0089] When an unmanned surface vessel (USV) makes a high-speed turn, yaw motion often causes roll motion (dynamic coupling). In this embodiment, the heading deviation and roll attitude are calculated in a unified manner by using the imaginary part vector of the pose error. When the controller outputs the steering command, it will take into account the current roll angle to prevent the hull attitude from becoming unstable due to over-steering.

[0090] The current wind disturbance coefficient is converted into a reverse compensation torque, which can increase the reverse thrust or rudder angle in advance before the wave impact causes a substantial deviation in course.

[0091] By combining the calculated feedback control torque and the reverse compensation torque, the final control torque of the unmanned equipment is obtained.

[0092] 4. Execute the final control torque.

[0093] The system reads the current battery voltage of the unmanned equipment. If the current battery voltage is in a low power or high load voltage drop state, the system will automatically reduce the maximum allowable torque of the unmanned equipment and freeze the integral term to prevent the servo motor or propulsion motor from experiencing response lag or oscillation due to insufficient voltage.

[0094] The modified control signal is converted into a PWM wave to drive the servo motor to adjust the rudder angle or control the differential rotation of the dual propulsion motors.

[0095] Through the above attitude control and heading control process, the unmanned surface vessel (USV) smoothly and accurately completed the right turn under the interference of lateral wind and waves, with minimal heading overshoot and the hull roll amplitude effectively suppressed within a safe range.

[0096] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of this application. It should be understood that the above descriptions are merely specific embodiments of this application and are not intended to limit the scope of protection of this application. In particular, it should be noted that any modifications, equivalent substitutions, or improvements made by those skilled in the art within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. A method for attitude and heading control of unmanned equipment, characterized in that, include: Data fusion is performed on the multi-source heterogeneous data collected from unmanned equipment to output pose estimation data of the unmanned equipment; wherein, the multi-source heterogeneous data includes inertial data, geomagnetic data and satellite navigation data; By using a pre-set dynamic model, the influence of the external environment on the unmanned equipment is predicted based on the pre-set model confidence level and the current wind disturbance coefficient of the unmanned equipment, and the optimal heading control strategy of the unmanned equipment is obtained; wherein, the heading angular velocity, control torque and damping coefficient of the unmanned equipment control system are used to construct the dynamic model. Based on the optimal heading control strategy and the attitude estimation data, with the goal of counteracting wind disturbance, the heading deviation and roll attitude of the unmanned equipment at future moments are calculated, and the final control torque of the unmanned equipment is output. The attitude and heading of the unmanned equipment are controlled by the final control torque.

2. The pose and heading control method for unmanned equipment according to claim 1, characterized in that, The process of fusing multi-source heterogeneous data collected from unmanned equipment to output pose estimation data for the unmanned equipment specifically involves: Acquire the pose and heading information of the unmanned equipment and construct the system state vector of the unmanned equipment; Based on the satellite navigation data, the motion acceleration and gravity components of the unmanned equipment are calculated, and the attitude components of the system state vector are corrected using the motion acceleration and gravity components. The heading component of the system state vector is corrected using geomagnetic data; Based on the inertial data, the pose estimation data and the confidence matrix of the pose estimation data are obtained through the corrected system state vector.

3. The pose and heading control method for unmanned equipment according to claim 1, characterized in that, The inertial data is obtained by collecting the three-axis angular velocity and three-axis acceleration of the unmanned equipment through an inertial measurement unit. The geomagnetic data were obtained by collecting triaxial geomagnetic field strength using a magnetometer. The absolute position and ground velocity vector of the unmanned equipment are collected through a satellite navigation system to obtain the satellite navigation data. The inertial data, geomagnetic data, and satellite navigation data are subjected to coordinate system transformation, magnetometer calibration, and outlier removal.

4. The pose and heading control method for unmanned equipment according to claim 1, characterized in that, The process involves using a pre-defined dynamic model, based on the pre-defined model reliability and the current wind disturbance coefficient of the unmanned equipment, to predict the impact of the external environment on the unmanned equipment, thereby obtaining the optimal heading control strategy for the unmanned equipment. Specifically: The control torque and the heading angular velocity of the previous moment are input into the preset wind disturbance prediction model to calculate the deviation between the model prediction value and the sensor measurement value. The preset initial wind disturbance coefficient is updated by the deviation change to obtain the current wind disturbance coefficient. The performance of the dynamic model is evaluated to obtain the model's reliability. Based on the current wind disturbance coefficient, the model reliability, and the preset target heading error, the influence of the external environment on the trajectory of the unmanned equipment is analyzed, and a comprehensive evaluation value is calculated. The optimal heading control strategy is obtained by adjusting the weights of the heading control strategy based on the magnitude of the comprehensive evaluation value.

5. The pose and heading control method for unmanned equipment according to claim 4, characterized in that, The process involves analyzing the impact of the external environment on the trajectory of the unmanned equipment based on the current wind disturbance coefficient, the model reliability, and the preset target heading error, and calculating a comprehensive evaluation value, specifically as follows: Calculate the ratio of the current wind disturbance coefficient to the preset maximum wind resistance capacity to obtain the first ratio; Calculate the ratio of the model's credibility to a preset credibility threshold to obtain a second ratio; The first ratio, the second ratio, and the target heading error are weighted and calculated to obtain the comprehensive evaluation value.

6. The pose and heading control method for unmanned equipment according to claim 4, characterized in that, The process of adjusting the heading control strategy weights based on the comprehensive evaluation value to obtain the optimal heading control strategy is as follows: If the comprehensive evaluation value is less than the first threshold, the heading control strategy weight is increased with the goal of reducing the energy consumption of the unmanned equipment and ensuring the smoothness of the trajectory, so as to obtain the optimal heading control strategy. If the comprehensive evaluation value is greater than or equal to the first threshold, then with the goal of resisting the impact of wind speed from the external environment, the weight of the heading control strategy is reduced to obtain the optimal heading control strategy.

7. The pose and heading control method for unmanned equipment according to claim 1, characterized in that, Based on the optimal heading control strategy and the attitude estimation data, with the aim of counteracting wind disturbance, the heading deviation and roll attitude of the unmanned equipment at future moments are calculated, and the final control torque of the unmanned equipment is output, specifically as follows: The pose estimation data is compared with the expected pose data to obtain the pose error; wherein, the expected pose data comes from the navigation system of the unmanned equipment. Based on the pose error and the optimal heading control strategy, and considering the heading deviation and roll attitude of the unmanned equipment, a feedback control torque is generated. Based on the current wind disturbance coefficient, a counter-compensation torque of equal magnitude and opposite direction to the wind disturbance is generated; the counter-compensation torque is used to counteract the wind disturbance. The final control torque is obtained by combining the feedback control torque and the reverse compensation torque.

8. The pose and heading control method for unmanned equipment according to claim 7, characterized in that, The step of generating feedback control torque based on pose error and optimal heading control strategy, taking into account heading deviation and roll attitude of unmanned equipment, is as follows: If the unmanned equipment is in a stable operating condition, then based on the imaginary part vector of the pose error and the optimal heading control strategy, a feedback control torque is generated with the goal of minimizing the energy consumption of the unmanned equipment. If the unmanned equipment is in a strong wind disturbance condition, a feedback control torque is generated based on the sliding surface parameters and torque switching gain of the unmanned equipment to resist the wind disturbance.

9. The pose and heading control method for unmanned equipment according to claim 7, characterized in that, Adjust the maximum permissible torque of the unmanned equipment based on its current battery voltage; if the feedback control torque is greater than the maximum permissible torque, adjust the calculation method for the feedback control torque.

10. The pose and heading control method for unmanned equipment according to claim 1, characterized in that, The attitude and heading of the unmanned equipment are controlled by the final control torque, specifically as follows: If the current battery voltage of the unmanned equipment is less than the second threshold, the maximum permissible torque of the unmanned equipment is reduced. The limiting threshold of the final control torque control is adjusted based on the reduced maximum permissible torque.

Images