A multi-rotor UAV manipulator control method considering center of mass offset constraints

Abstract

The present application relates to the technical field of attitude control of drones, and specifically to a method for controlling the robotic arm of a multi-rotor drone taking into account center of mass offset constraints. The method comprises: collecting multiple pitch angles, roll angles, and yaw angles of the multi-rotor drone at each moment in each flight cycle to form an attitude angle vector, and obtaining the position coordinates of the drone at each moment; calculating the trajectory complexity, attitude coordination, error tolerance weight, and error evaluation value of each flight cycle, determining the adaptive factor of each flight cycle, updating the measurement noise covariance matrix in the unscented Kalman filter algorithm, and performing attitude solution on the multi-rotor drone using the updated measurement noise covariance matrix to dynamically control the robotic arm of the multi-rotor drone. The present application effectively reduces the error of attitude solution, improves the adaptability of the multi-rotor drone in different flight environments, and accurately controls the attitude of the robotic arm of the multi-rotor drone.

Description

Technical Field

[0001] The present application relates to the technical field of UAV attitude control, and in particular to a multi-rotor UAV robotic arm control method considering center of mass offset constraints. Background Art

[0002] Multirotor drones (UAVs) have gained widespread adoption in recent years due to their high maneuverability and vertical takeoff and landing capabilities. When equipped with a robotic arm, they utilize visual intelligence to detect targets and can perform complex tasks such as tracking and capturing aerial or ground objects, removing foreign objects and bird nests from power lines, and performing rescue operations in diverse terrains. However, during the movement of the robotic arm, changes in its mass distribution and the dynamic adjustment of its load can cause the drone's overall center of mass to shift, affecting the multirotor's control accuracy.

[0003] Because the three-axis inertial measurement sensor is affected by the interference of its own characteristics, the measured data contains noise. The traditional method, the quaternion unscented Kalman filter algorithm, can fuse the measurement data of multiple sensors to solve the attitude of the drone. However, the measurement noise covariance in the unscented Kalman filter algorithm is selected based on experience and cannot adapt to the dynamic changes in different flight environments. This reduces the accuracy of the drone's attitude solution and cannot accurately estimate the drone's attitude, affecting the accuracy of the drone's center of mass offset compensation, resulting in low accuracy of the drone's attitude control. Summary of the Invention

[0004] In order to solve the above technical problems, a multi-rotor UAV robotic arm control method considering center of mass offset constraints is provided to solve the existing problems.

[0005] The solution to the technical problem of this application is to provide a multi-rotor UAV robotic arm control method considering the center of mass offset constraint, including the following steps:

[0006] Multiple instruments are used to collect multiple pitch angles, roll angles, and yaw angles of the multi-rotor drone at each moment in each flight cycle, forming the attitude angle vector at each moment. The drone is then positioned in real time to obtain its position coordinates at each moment.

[0007] Based on the changes in the position coordinates of the UAV in a local period of each flight cycle, a trajectory curve is obtained; and the trajectory complexity of each flight cycle is calculated based on the complexity of the trajectory curve and the flight speed of the UAV along the trajectory curve;

[0008] The attitude coordination degree of each flight cycle is determined by the differences in the elements of the same dimension in the attitude angle vector at different times in each flight cycle. Combined with the trajectory complexity, the error tolerance weight of each flight cycle is determined.

[0009] Analyzing fluctuations in differences between pitch angles, roll angles, and yaw angles collected by the multiple instruments at all times during each flight cycle to determine an error assessment value for each flight cycle;

[0010] Based on the error evaluation value and error tolerance weight, the adaptive factor of each flight cycle is determined, and the measurement noise covariance matrix in the unscented Kalman filter algorithm is updated. The attitude of the multi-rotor UAV is solved through the updated measurement noise covariance matrix, and the attitude of the multi-rotor UAV robotic arm is controlled in real time.

[0011] Preferably, the process of acquiring the attitude angle vector is:

[0012] The pitch angle, roll angle and yaw angle of the multi-rotor drone at each moment are calculated by the gyroscope, and the pitch angle, roll angle and yaw angle of the multi-rotor drone at each moment are calculated by the accelerometer and magnetometer. The two pitch angles, two roll angles and two yaw angles measured at each moment are combined to form an attitude angle vector.

[0013] Preferably, the trajectory curve is further obtained by:

[0014] The duration of multiple flight cycles before each flight cycle is recorded as a local period;

[0015] The trajectory curve of the UAV is obtained through the position coordinates of all moments in the local period.

[0016] Preferably, the calculation of the trajectory complexity of each flight cycle includes:

[0017] Calculating the fractal dimension of the trajectory curve; calculating the average speed of the UAV flying along the trajectory curve;

[0018] The trajectory complexity is the product of the fractal dimension and the average speed.

[0019] Preferably, determining the attitude coordination degree of each flight cycle includes:

[0020] The elements of the same dimension in the attitude angle vector at all moments in each flight cycle are arranged in chronological order to form attitude angle sequences; the difference between the elements at each moment and the previous moment in each attitude angle sequence is recorded as a relative difference;

[0021] Using a natural breakpoint algorithm, obtain two segmentation points of the relative difference at all moments in each posture angle sequence, record the largest segmentation point as the upper threshold, and record the smallest segmentation point as the lower threshold;

[0022] Calculating the attitude change at each moment in each attitude angle sequence by comparing the relative difference at each moment in each attitude angle sequence with the upper threshold and the lower threshold;

[0023] The cumulative sum of the attitude changes at all moments of each attitude angle sequence is calculated, and the sum of the cumulative sums of all attitude angle sequences in each flight cycle is used as the attitude coordination degree of each flight cycle.

[0024] Preferably, the calculation formula of the posture change is: ,in, For the The first position in the attitude angle sequence The amount of posture change at any moment, For the The first position in the attitude angle sequence The relative difference in time, is the lower threshold, is the upper threshold, To preset the first value, The preset second value is greater than the preset first value.

[0025] Preferably, the error tolerance weight is the inverse of the product of the trajectory complexity and the posture coordination degree.

[0026] Preferably, determining the error evaluation value of each flight cycle includes:

[0027] The differences in the pitch angle and roll angle between the gyroscope and the accelerometer at each moment are recorded as the measurement deviations of the pitch angle and roll angle at each moment, respectively;

[0028] The difference between the yaw angle corresponding to the gyroscope and the yaw angle corresponding to the magnetometer at each moment is recorded as the measurement deviation corresponding to the yaw angle at each moment;

[0029] Calculate the product of the mean and standard deviation of the measured deviations corresponding to the pitch angle, roll angle, and yaw angle at all times in each flight cycle, and use them as the deviation fluctuations corresponding to the pitch angle, roll angle, and yaw angle in each flight cycle.

[0030] The error evaluation value is the sum of the deviation fluctuations corresponding to the pitch angle, roll angle, and yaw angle in each flight cycle.

[0031] Preferably, the adaptive factor is a normalized result of the ratio of the error evaluation value to the error tolerance weight.

[0032] Preferably, The updated measurement noise covariance matrix under flight cycles The calculation method is: ,in, For the Adaptive factor for each flight cycle, For the The measurement noise covariance matrix before updating under the flight cycle.

[0033] This application has at least the following beneficial effects:

[0034] This application calculates the trajectory complexity of each flight cycle by analyzing the complexity of the flight trajectory of the multi-rotor UAV in a local time period. Its beneficial effect is that it takes into account the complexity of the overall flight process of the UAV when performing the flight mission during this period, so as to reflect the possibility that the UAV is performing complex flight and robotic arm control tasks; it determines the attitude coordination degree of each flight cycle. Its beneficial effect is that it takes into account the significant changes of a single specific attitude angle at adjacent moments to illustrate the situation where the UAV's attitude changes drastically. Secondly, through the significant changes of all attitude angles, it illustrates the attitude angle synchronization and coordination ability when the multiple attitude angles of the UAV jointly adjust the flight trajectory, so as to reflect the multi-rotor UAV's The coupling complexity between multiple attitude angles of humans and machines further reflects the possibility that the UAV is performing complex flight and robotic arm control tasks in this flight cycle, promotes the control system of the UAV to pay more attention to the degree of coordination between multiple attitude angles, improves the accuracy of coordination evaluation of multi-rotor UAVs, and enhances the adaptability of the control system to complex tasks and complex environments; determines the error tolerance weight of each flight cycle, which has the beneficial effect of giving the UAV error tolerance weights for different control conditions according to the complexity of the UAV's flight mission. When the UAV flight and the robotic arm are in simple control, it can improve the convergence speed of the subsequent UAV attitude solution in simple control. When the arm is complexly controlled, it can reduce the response to noise, thereby improving the stability of the UAV attitude solution and robotic arm control, and enhancing the safety of the UAV robotic arm control; determining the error evaluation value of each flight cycle, its beneficial effect is that it takes into account the deviation of the same data measured by the gyroscope, accelerometer and magnetometer to reflect the interference of random noise components and the influence of gyroscope zero drift instability, and then explains the error degree caused by the subsequent attitude solution of the multi-rotor UAV; determining the adaptive factor of each flight cycle, its beneficial effect is that it takes into account the evaluation of the UAV attitude measurement error and the complex situation of UAV flight and robotic arm control, and selects the appropriate Adaptive factor improves the stability of UAV attitude solution and robotic arm control; then the measurement noise covariance matrix in the unscented Kalman filter algorithm is updated, and the attitude of the multi-rotor UAV is solved through the updated measurement noise covariance matrix to dynamically control the robotic arm of the multi-rotor UAV. Its beneficial effect is that by fusing multiple sensor data, the noise interference in the data measured by a single sensor is reduced, the error of attitude solution is effectively reduced, the adaptability of the multi-rotor UAV in different flight environments is improved, the attitude of the UAV is estimated more accurately, and the accuracy of the subsequent compensation for the center of mass offset of the UAV is improved, thereby precisely controlling the attitude of the multi-rotor UAV robotic arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] The following is a detailed description of a multi-rotor UAV robotic arm control method considering center of mass offset constraints in accordance with the present application, with reference to the accompanying drawings.

[0036] Figure 1 A flowchart of the steps of a multi-rotor UAV robotic arm control method considering center of mass offset constraints provided in an embodiment of the present application;

[0037] Figure 2 A flowchart of the steps of the method for obtaining the adaptive factor provided in an embodiment of the present application. DETAILED DESCRIPTION

[0038] In order to make the purpose, technical solutions and advantages of this application more clearly understood, the following, in conjunction with the accompanying drawings and implementation examples, further describes in detail a multi-rotor drone robotic arm control method considering center of mass offset constraints proposed in this application. It should be understood that the specific embodiments described herein are only used to explain this application and are not intended to limit this application.

[0039] Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0040] See also Figure 1 , which shows a flowchart of a method for controlling a multi-rotor UAV manipulator arm considering a center of mass offset constraint provided by one embodiment of the present application, the method comprising the following steps:

[0041] Step 1: Use multiple instruments to collect multiple pitch angles, roll angles, and yaw angles of the multi-rotor drone at each moment in each flight cycle to form the attitude angle vector at each moment. By real-time positioning of the drone, the position coordinates of the drone at each moment are obtained.

[0042] With the rapid development of microelectromechanical systems (MEMS) technology, low-cost, low-power, and compact MEMS inertial sensors are widely used for attitude estimation in small multi-rotor drones. Attitude estimation is fundamental to drone flight stability, and the accuracy of attitude estimation is directly related to the stability of attitude control. In small aircraft, gyroscopes, accelerometers, and magnetometers are commonly used as attitude measurement devices. Gyroscopes measure the angular velocity of the moving vehicle to calculate the yaw, pitch, and roll angles. Accelerometers measure the vehicle's specific force to calculate the pitch and roll angles. Magnetometers measure the magnetic field's magnetic induction intensity to calculate the yaw angle of the moving vehicle. However, gyroscopes exhibit constant drift, resulting in cumulative errors in the attitude estimation process. Accelerometers have zero bias, while magnetometers are susceptible to interference from external magnetic sources in the surrounding environment. Using a single sensor for attitude estimation results in low accuracy, impacting the drone's stable flight.

[0043] Taking a quad-rotor drone as an example, in order to meet the needs of describing its position and posture, an inertial coordinate system, a body coordinate system, and an end-effector coordinate system of the drone's robotic arm are established. , firmly connected to the Earth, origin At the Earth's center of mass; body coordinate system , its origin At the center of mass of the multi-rotor drone, The axis is vertical and the flight platform plane is vertically upward; the coordinate system of the end effector of the UAV robotic arm , where the center point of the end effector of the UAV robotic arm is the origin , vertically downward axis.

[0044] Multirotor drones move in various ways, including up and down, left and right, forward and backward, roll, pitch, yaw, and rotation of the robotic arm. To better describe the multirotor's spatial position, a three-axis inertial measurement unit (IMU) is deployed on the drone. This unit contains an accelerometer, magnetometer, and gyroscope. The gyroscope acquires pitch, roll, and yaw angles in real time, while the accelerometer calculates pitch and roll angles, and the magnetometer calculates yaw angles in real time. Using a rotation matrix, the attitude angles are converted from the drone's body coordinate system to the inertial coordinate system. This yields the multirotor's attitude angles at different times, which include pitch, roll, and yaw.

[0045] In this embodiment, the acquisition frequency of the three-axis inertial measurement sensor is 400 Hz. As other implementation methods, the implementer can set it according to actual conditions. Secondly, the process of transforming the translational motion of the center of mass of the drone between the body coordinate system and the inertial coordinate system through the rotation matrix is a well-known technology and will not be repeated here.

[0046] Therefore, the three-axis inertial measurement sensor will have two pitch angles, two roll angles, and two yaw angles at the same time. All the pitch angles, roll angles, and yaw angles obtained will form an attitude angle vector.

[0047] Secondly, by real-time positioning of the multi-rotor UAV, the position coordinates of the UAV at each moment are obtained;

[0048] A flight cycle of a preset duration is set; thus, the attitude angle vector of the multi-rotor UAV at different times in each flight cycle is obtained.

[0049] In this embodiment, the preset duration of the flight cycle is 10ms, that is, the attitude of the drone is solved once every 10ms. As other implementation methods, the implementer can set it according to actual conditions.

[0050] At this point, the attitude angle vector and position coordinates of the multi-rotor UAV at each moment in each flight cycle are obtained.

[0051] Step 2: Based on the changes in the position coordinates of the UAV in the local time period of each flight cycle, a trajectory curve is obtained; the trajectory complexity of each flight cycle is calculated based on the complexity of the trajectory curve and the flight speed of the UAV along the trajectory curve; the attitude coordination degree of each flight cycle is determined by the differences in the elements of the same dimension in the attitude angle vector at different times in each flight cycle.

[0052] Multi-rotor drones equipped with robotic arms can replace workers in high-risk occupations such as high-altitude rescue, construction, and equipment maintenance. They can perform both inspections and various maintenance operations, greatly improving the flexibility and efficiency of inspections and protecting workers' lives. Due to the varying requirements of multi-rotor drones for flight missions, environmental conditions, and flight control systems, they sometimes perform simple maneuvers such as hovering and direct flight during flight, while other times, complex tasks require multi-rotor drones to operate at different positions and angles, requiring coordinated posture coordination.

[0053] First, by analyzing the complexity of the UAV's flight path in each flight cycle, the trajectory complexity is calculated, specifically:

[0054] The duration of multiple flight cycles before each flight cycle is recorded as a local period;

[0055] In this embodiment, the cumulative duration of the five flight cycles before each flight cycle is selected and recorded as the local time period. As for other implementation methods, the implementer can set it according to actual conditions.

[0056] Obtaining the trajectory curve of the UAV through the position coordinates of all moments in the local period;

[0057] Calculating the fractal dimension of the trajectory curve;

[0058] In this embodiment, the fractal dimension is calculated using the Higuchi algorithm, which is a well-known technology and will not be described in detail here. As other implementation methods, the implementer may adopt other methods of the existing technology, such as the Hurst index method, etc. This embodiment does not impose any special restrictions on this.

[0059] Calculate the average speed of the UAV flying along the trajectory curve;

[0060] In this embodiment, the average speed is calculated by the ratio of the flight distance of the trajectory curve to the time length of each flight cycle.

[0061] The product of the fractal dimension and the average speed is used as the trajectory complexity of each flight cycle;

[0062] It should be noted that the fractal dimension reflects the degree of tortuosity of the flight trajectory of a multi-rotor UAV when performing a flight mission. The larger the fractal dimension, the more complex the flight trajectory of the UAV. The greater the average speed, the more complex the working state of the engine in the UAV and the more frequent the thrust adjustment. The greater the trajectory complexity, the more tortuosity the UAV's trajectory at high flight speeds, reflecting that the overall flight system of the UAV during the flight cycle is more complex, and the more likely it is to be performing complex flight and robotic arm control tasks.

[0063] Furthermore, the attitude change is calculated based on the differences in attitude angles at different times in each flight cycle, specifically:

[0064] The elements of the same dimension in the attitude angle vector at all moments in each flight cycle are arranged in time order to form attitude angle sequences;

[0065] It should be noted that, since the dimension of the attitude angle vector is 6, the number of attitude angle sequences is 6.

[0066] The difference between the elements at each moment and the previous moment in each attitude angle sequence is recorded as relative difference;

[0067] In this embodiment, the absolute value of the difference between the elements at each moment and the previous moment in each attitude angle sequence is recorded as the relative difference.

[0068] Using a natural breakpoint algorithm, obtain two segmentation points of the relative difference at all moments in each posture angle sequence, record the largest segmentation point as the upper threshold, and record the smallest segmentation point as the lower threshold;

[0069] It should be noted that the natural breakpoint algorithm is a well-known technology and will not be described in detail here.

[0070] The calculation formula for the attitude change at each moment in each attitude angle sequence is:

[0071]

[0072] in, For the The first position in the attitude angle sequence The amount of posture change at any moment, For the The first position in the attitude angle sequence The relative difference in time, is the lower threshold, is the upper threshold, To preset the first value, To preset the second value, in this embodiment, the first value is preset The value is 0, the second value is preset The value is 1. As other implementation methods, the implementer can set it according to actual conditions.

[0073] It should be noted that the larger the attitude change, that is, the closer it is to 1, it means that during the flight of the multi-rotor drone, the attitude of the drone has changed dramatically at the corresponding moment, reflecting that the drone is more likely to perform a complex flight mission at this time; conversely, the smaller the attitude change, that is, the closer it is to 0, it means that the attitude angle does not change much at the corresponding moment, and the drone attitude is relatively stable.

[0074] Furthermore, based on the posture change, the posture coordination degree is calculated, specifically:

[0075] Calculate the cumulative sum of the attitude changes at all moments of each attitude angle sequence, and use the sum of the cumulative sums of all attitude angle sequences in each flight cycle as the attitude coordination degree of the multi-rotor UAV in each flight cycle;

[0076] It should be noted that the greater the attitude coordination degree, the more the multiple attitude angles of the UAV jointly adjust the flight trajectory, the stronger the attitude angle synchronization cooperation ability, the higher the coupling complexity between the multiple attitude angles of the multi-rotor UAV, and the more likely it is that complex flight and robotic arm control tasks are being performed during this flight cycle; conversely, the smaller the attitude coordination degree, the more independent or smaller the attitude angle changes are, and a simple flight mission may be being performed.

[0077] At this point, the trajectory complexity and attitude coordination of the multi-rotor UAV in each flight cycle are obtained.

[0078] Step 3: Determine the error tolerance weight for each flight cycle based on attitude coordination and trajectory complexity; analyze the fluctuations in the differences between the pitch angles, roll angles, and yaw angles collected by multiple instruments at all times in each flight cycle to determine the error evaluation value for each flight cycle.

[0079] Since the attitude control requirements for drones performing simple flight missions are relatively low, even if there are certain errors in the attitude measurement data during the attitude solution process, it will not significantly affect the completion of the mission. Therefore, the error tolerance of the drone's attitude measurement data is relatively high. Multi-rotor drones performing complex flight and robotic arm control missions have higher attitude control requirements for drones and higher accuracy and precision requirements for attitude solution. A small error in the attitude measurement data may cause the mission to fail or the drone to fly unstable, and the error tolerance for the drone's attitude measurement data is lower. Therefore, the error tolerance weight is obtained through the trajectory complexity and the attitude coordination degree, which is specifically:

[0080] The reciprocal of the product of the trajectory complexity and the attitude coordination is used as the error tolerance weight of the multi-rotor UAV in each flight cycle;

[0081] It should be noted that when calculating the reciprocal, in order to avoid the denominator being 0, a preset value greater than 0 is added to the denominator. In this embodiment, the preset value greater than 0 is 0.1. As other implementation methods, the implementer can set it according to actual conditions.

[0082] It should be noted that the smaller the error tolerance weight is, the more accurate the attitude calculation of the drone is.

[0083] Secondly, the attitude calculation of drones typically relies on measurement data from multiple sensors. Among them, the attitude angle data of the gyroscope has high short-term stability. Attitude calculation relies on gyroscope attitude measurement data, which is more suitable for multi-rotor drones that perform complex flight and robotic arm control tasks. However, gyroscopes are prone to offset errors, which affect the credibility of the gyroscope's attitude measurement data. Secondly, due to the characteristics of the sensor itself and the influence of environmental factors, the initial zero point reading of the gyroscope will drift over time. The greater the zero drift, the greater the error in the attitude measurement. At the same time, this error will accumulate as the gyroscope rotation or angle estimation drifts. Zero drift instability can further reduce the credibility of the attitude measurement data.

[0084] Therefore, by analyzing the difference between the attitude angle measured by the gyroscope and the attitude angle measured by the accelerometer and magnetometer, the error evaluation value is calculated, specifically:

[0085] The difference between the pitch angle corresponding to the gyroscope and the pitch angle corresponding to the accelerometer at each moment is recorded as the measurement deviation corresponding to the pitch angle at each moment. The product of the mean and standard deviation of the measurement deviation corresponding to the pitch angle at all moments in each flight cycle is calculated as the deviation fluctuation corresponding to the pitch angle for each flight cycle.

[0086] The difference between the roll angle corresponding to the gyroscope and the roll angle corresponding to the accelerometer at each moment is recorded as the measurement deviation corresponding to the roll angle at each moment. The product of the mean and standard deviation of the measurement deviation corresponding to the roll angle at all moments in each flight cycle is calculated as the deviation fluctuation corresponding to the roll angle for each flight cycle.

[0087] The difference between the yaw angle corresponding to the gyroscope and the yaw angle corresponding to the magnetometer at each moment is recorded as the measurement deviation corresponding to the yaw angle at each moment. The product of the mean and standard deviation of the measurement deviation corresponding to the yaw angle at all moments in each flight cycle is calculated as the deviation fluctuation corresponding to the yaw angle for each flight cycle.

[0088] In this embodiment, the absolute value of the difference between the pitch angle corresponding to the gyroscope and the pitch angle corresponding to the accelerometer at each moment is recorded as the measurement deviation corresponding to the pitch angle at each moment; the absolute value of the difference between the roll angle corresponding to the gyroscope and the roll angle corresponding to the accelerometer at each moment is recorded as the measurement deviation corresponding to the roll angle at each moment; and the absolute value of the difference between the yaw angle corresponding to the gyroscope and the yaw angle corresponding to the magnetometer at each moment is recorded as the measurement deviation corresponding to the yaw angle at each moment.

[0089] The sum of the deviation fluctuations corresponding to the pitch angle, roll angle, and yaw angle in each flight cycle is used as the error evaluation value of the multi-rotor drone in each flight cycle;

[0090] It should be noted that the larger the mean value, the greater the deviation in the attitude angle data measured by the gyroscope, accelerometer, and magnetometer corresponding to different sensors, the more random noise components in the corresponding attitude angle, or the higher the gyroscope zero drift intensity. The larger the standard deviation, the stronger the instability of the attitude angle data measured by different sensors, and the more likely it is caused by the zero drift instability of the gyroscope. The greater the deviation fluctuation, the greater the obtained error evaluation value, indicating that the higher the error in the attitude measurement data for attitude solution of the multi-rotor drone, and the lower the credibility of the subsequent attitude solution results.

[0091] At this point, the error evaluation value of the multi-rotor UAV in each flight cycle is obtained.

[0092] In step 4, based on the error evaluation value and the error tolerance weight, the adaptive factor of each flight cycle is determined, and the measurement noise covariance matrix in the unscented Kalman filter algorithm is updated. The attitude of the multi-rotor UAV is solved through the updated measurement noise covariance matrix, and the attitude of the multi-rotor UAV robotic arm is controlled in real time.

[0093] Furthermore, based on the error tolerance weight and the error evaluation value, an adaptive factor is determined, specifically:

[0094] Normalizing the ratio of the error evaluation value to the error tolerance weight as an adaptive factor for each flight cycle;

[0095] In this embodiment, the sigmoid function is used for normalization processing, wherein the sigmoid function is a well-known technology and will not be described in detail here. As other implementation methods, the implementer can adopt other methods of the existing technology, such as the tanh function, etc. This embodiment does not impose any special restrictions on this.

[0096] It should be noted that by evaluating the attitude measurement error of the drone, a lower adaptive factor is given when the drone is in simple flight and the robotic arm is controlled, thereby improving the convergence speed of the attitude solution process of the drone during subsequent simple control. When the drone is in complex flight and the robotic arm is controlled, a higher adaptive factor is given, thereby reducing the response to noise and improving the stability of the drone attitude solution and robotic arm control. Secondly, the step flow chart of the method for obtaining the adaptive factor provided in the embodiment of the present application is as follows: Figure 2 shown.

[0097] Secondly, because gyroscopes have cumulative errors and accelerometers and magnetometers are subject to significant external interference, effective data fusion is required to ensure a more accurate attitude angle. Because attitude calculations contain random drift errors and are susceptible to the influence of non-gravitational acceleration, data fusion is achieved by adaptively adjusting the measurement noise covariance matrix in the unscented Kalman filter algorithm and performing attitude calculations, thereby reducing the influence of non-gravitational acceleration. This suppresses the random drift errors of the gyroscopes and adaptively compensates for the influence of non-gravitational acceleration on attitude estimation.

[0098] Before the attitude calculation is performed using the unscented Kalman filter algorithm, the measurement data of the gyroscope, accelerometer and magnetometer are converted into quaternion form using the quaternion method;

[0099] It should be noted that the quaternion method is a well-known technology and will not be described in detail here.

[0100] After performing quaternion transformation on the angular velocity data output by the gyroscope, the state equation of the unscented Kalman filter algorithm is constructed;

[0101] The measurement equation of the unscented Kalman filter algorithm is constructed by performing quaternion transformation on the acceleration data and geomagnetic field data output by the accelerometer and magnetometer;

[0102] It should be noted that the process of constructing the state equation and the measurement equation is a well-known technology and will not be described in detail here.

[0103] Through the state equation and measurement equation, the state noise covariance matrix and measurement noise covariance matrix are obtained;

[0104] Based on the adaptive factor, the measurement noise covariance matrix in the unscented Kalman filter algorithm is updated, and the updated measurement noise covariance matrix is:

[0105]

[0106] in, For the The updated measurement noise covariance matrix under flight cycles is: For the Adaptive factor for each flight cycle, For the The measurement noise covariance matrix before updating under each flight cycle;

[0107] It should be noted that the quaternion-based unscented Kalman filter algorithm is a well-known technology and will not be described in detail here.

[0108] The gain coefficient is obtained through the updated measurement noise covariance matrix and state noise covariance matrix, and combined with the unscented Kalman filter algorithm, the attitude of the multi-rotor UAV is solved, thereby more accurately reflecting the flight attitude of the multi-rotor UAV, thereby evaluating the center of mass offset of the UAV, and performing center of mass compensation by considering the influence of the center of mass offset to offset the force and torque changes caused by the center of mass offset, thereby dynamically controlling the multi-rotor unmanned robotic arm; the measurement noise covariance matrix is updated through the adaptive factor to improve the adaptability of the multi-rotor UAV in different flight environments and avoid safety accidents.

[0109] It should be noted that the specific process of dynamic control of the multi-rotor unmanned robotic arm is: based on the results of attitude solution of the multi-rotor UAV, the UAV dynamics model and the robotic arm dynamics model are established, according to the deviation between the attitude solution result and the desired attitude, combined with the dynamic characteristics of the UAV, the approximate direction and size of the center of mass offset are calculated, the center of mass offset vector is estimated in real time, and the compensation control law is used to adjust the attitude of the quadcopter UAV and the movement of the robotic arm to offset the influence of the center of mass offset. Among them, the design of the compensation control law is specifically as follows: according to the deviation between the attitude solution result and the desired attitude, the required center of mass offset compensation torque is calculated through the PID controller, the center of mass offset compensation torque is distributed to each robotic arm, the rotor speed is adjusted, the actual attitude of the UAV is brought closer to the desired attitude, and the influence of the center of mass offset is offset. Furthermore, according to the task planning of the robotic arm, the expected trajectory of its joint angle is calculated. On the basis of considering the compensation of the center of mass offset, the driving torque required for each joint of the robotic arm is obtained through inverse kinematics and inverse dynamics calculations, and the actual joint angle of the robotic arm and the posture information of the drone are fed back in real time to adjust the movement of the robotic arm. Among them, the method of constructing the drone dynamics model and the robotic arm dynamics model, as well as the process of calculating the driving torque required for each joint of the robotic arm by inverse kinematics and inverse dynamics are well-known technologies and will not be repeated here.

[0110] It should be noted that the process of performing attitude calculation using the unscented Kalman filter algorithm and the method of compensating for center of mass offset are both well-known technologies and will not be described in detail here.

[0111] It should be understood that although Figure 1The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. In addition, Figure 1 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.

[0112] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0113] The above-described embodiments merely represent several implementation methods of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the present application. It should be noted that a person skilled in the art can make various modifications and improvements without departing from the spirit of the present application. Therefore, any simple modifications, equivalent variations, and modifications to the above embodiments made in accordance with the technical essence of the present application without departing from the content of the present application's technical solution fall within the scope of protection of the present application's technical solution.

Claims

1. A multi-rotor UAV manipulator control method considering center of mass offset constraints, characterized in that: The method comprises the following steps: Multiple instruments are used to collect multiple pitch angles, roll angles, and yaw angles of the multi-rotor drone at each moment in each flight cycle, forming the attitude angle vector at each moment. The drone is then positioned in real time to obtain its position coordinates at each moment. Based on the changes in the position coordinates of the UAV in a local period of each flight cycle, a trajectory curve is obtained; and the trajectory complexity of each flight cycle is calculated based on the complexity of the trajectory curve and the flight speed of the UAV along the trajectory curve; The attitude coordination degree of each flight cycle is determined by the differences in the elements of the same dimension in the attitude angle vector at different times in each flight cycle. Combined with the trajectory complexity, the error tolerance weight of each flight cycle is determined. Analyzing fluctuations in differences between pitch angles, roll angles, and yaw angles collected by the multiple instruments at all times during each flight cycle to determine an error assessment value for each flight cycle; Based on the error evaluation value and error tolerance weight, the adaptive factor of each flight cycle is determined, and the measurement noise covariance matrix in the unscented Kalman filter algorithm is updated. The attitude of the multi-rotor UAV is solved through the updated measurement noise covariance matrix, and the attitude of the multi-rotor UAV robotic arm is controlled in real time.

2. A multi-rotor UAV manipulator control method considering center of mass offset constraints as claimed in claim 1, characterized in that: The process of obtaining the attitude angle vector is as follows: The pitch angle, roll angle and yaw angle of the multi-rotor drone at each moment are calculated by the gyroscope, and the pitch angle, roll angle and yaw angle of the multi-rotor drone at each moment are calculated by the accelerometer and magnetometer. The two pitch angles, two roll angles and two yaw angles measured at each moment are combined to form an attitude angle vector.

3. The multi-rotor UAV manipulator control method considering center of mass offset constraint according to claim 1, characterized in that: The further acquisition method of the trajectory curve is: The duration of multiple flight cycles before each flight cycle is recorded as a local period; The trajectory curve of the UAV is obtained through the position coordinates of all moments in the local period.

4. The multi-rotor UAV manipulator control method considering center of mass offset constraint according to claim 1, characterized in that: The calculation of the trajectory complexity of each flight cycle includes: Calculating the fractal dimension of the trajectory curve; calculating the average speed of the UAV flying along the trajectory curve; The trajectory complexity is the product of the fractal dimension and the average speed.

5. The multi-rotor UAV manipulator control method considering center of mass offset constraint according to claim 1, characterized in that: Determining the attitude coordination degree of each flight cycle includes: The elements of the same dimension in the attitude angle vector at all moments in each flight cycle are arranged in chronological order to form attitude angle sequences; the difference between the elements at each moment and the previous moment in each attitude angle sequence is recorded as a relative difference; Using a natural breakpoint algorithm, obtain two segmentation points of the relative difference at all moments in each posture angle sequence, record the largest segmentation point as the upper threshold, and record the smallest segmentation point as the lower threshold; Calculating the attitude change at each moment in each attitude angle sequence by comparing the relative difference at each moment in each attitude angle sequence with the upper threshold and the lower threshold; The cumulative sum of the attitude changes at all moments of each attitude angle sequence is calculated, and the sum of the cumulative sums of all attitude angle sequences in each flight cycle is used as the attitude coordination degree of each flight cycle.

6. A multi-rotor UAV manipulator control method considering center of mass offset constraints as claimed in claim 5, characterized in that: The calculation formula of the posture change is: ,in, For the The first position in the attitude angle sequence The amount of posture change at any moment, For the The first position in the attitude angle sequence The relative difference in time, is the lower threshold, is the upper threshold, To preset the first value, The preset second value is greater than the preset first value.

7. The multi-rotor UAV manipulator control method considering center of mass offset constraint according to claim 1, characterized in that: The error tolerance weight is the inverse of the product of the trajectory complexity and the posture coordination degree.

8. The multi-rotor UAV manipulator control method considering center of mass offset constraint according to claim 2, characterized in that: Determining the error evaluation value of each flight cycle includes: The differences in the pitch angle and roll angle between the gyroscope and the accelerometer at each moment are recorded as the measurement deviations of the pitch angle and roll angle at each moment, respectively; The difference between the yaw angle corresponding to the gyroscope and the yaw angle corresponding to the magnetometer at each moment is recorded as the measurement deviation corresponding to the yaw angle at each moment; Calculate the product of the mean and standard deviation of the measured deviations corresponding to the pitch angle, roll angle, and yaw angle at all times in each flight cycle, and use them as the deviation fluctuations corresponding to the pitch angle, roll angle, and yaw angle in each flight cycle. The error evaluation value is the sum of the deviation fluctuations corresponding to the pitch angle, roll angle, and yaw angle in each flight cycle.

9. The multi-rotor UAV manipulator control method considering center of mass offset constraint according to claim 1, characterized in that: The adaptive factor is a normalized result of the ratio of the error evaluation value to the error tolerance weight.

10. The multi-rotor UAV manipulator control method considering center of mass offset constraint according to claim 1, characterized in that: No. The updated measurement noise covariance matrix under flight cycles The calculation method is: ,in, For the Adaptive factor for each flight cycle, For the The measurement noise covariance matrix before updating under the flight cycle.

Images