Multi-rotor unmanned aerial vehicle mechanical arm control method considering mass center offset constraint

Abstract

The invention relates to the technical field of unmanned aerial vehicle attitude control, in particular to a multi-rotor unmanned aerial vehicle mechanical arm control method considering centroid offset constraint, and the method comprises the steps: collecting a plurality of pitch angles, roll angles and yaw angles of a multi-rotor unmanned aerial vehicle at each moment in each flight period, and forming an attitude angle vector; position coordinates of the unmanned aerial vehicle at all moments are obtained; the method comprises the following steps: calculating the trajectory complexity, attitude collaboration degree, error tolerance weight and error evaluation value of each flight period, determining the adaptive factor of each flight period, updating a measurement noise covariance matrix in an unscented Kalman filtering algorithm, performing attitude calculation on the multi-rotor unmanned aerial vehicle through the updated measurement noise covariance matrix, and calculating the attitude of the multi-rotor unmanned aerial vehicle according to the attitude of the multi-rotor unmanned aerial vehicle. Therefore, the mechanical arm of the multi-rotor unmanned aerial vehicle is dynamically controlled. According to the method, the error of attitude calculation is effectively reduced, the adaptability of the multi-rotor unmanned aerial vehicle in different flight environments is improved, and the attitude of the mechanical arm of the multi-rotor unmanned aerial vehicle is accurately controlled.

Description

Technical Field

[0001] This application relates to the technical field of UAV attitude control, and particularly to a control method for a multi-rotor UAV manipulator considering the centroid offset constraint. Background Art

[0002] Multi-rotor UAVs have been widely used in recent years due to their strong mobility and vertical takeoff and landing. When equipped with a manipulator, it can detect work targets through visual intelligence and perform complex tasks such as tracking and capturing aerial or ground objects, removing foreign objects and bird nests from transmission lines, and carrying out rescue operations in diverse terrains. During the movement of the manipulator, due to the change in its mass distribution and the dynamic adjustment of the load, the overall centroid of the UAV will shift, thereby affecting the working control accuracy of the multi-rotor UAV.

[0003] Due to the interference of the triaxial inertial measurement sensor by its own characteristics, the measured data contains noise. In traditional methods, the unscented Kalman filter algorithm based on quaternions can fuse the measurement data of multiple sensors for attitude solution of the UAV. 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, reducing the accuracy of the UAV's attitude solution, unable to accurately estimate the UAV's attitude, affecting the accuracy of the centroid offset compensation of the UAV, and resulting in low attitude control accuracy of the UAV. Summary of the Invention

[0004] To solve the above technical problems, a control method for a multi-rotor UAV manipulator considering the centroid offset constraint is provided to solve the existing problems.

[0005] The solution of this application to solve the technical problem is to provide a control method for a multi-rotor UAV manipulator considering the centroid offset constraint, including the following steps: Collect multiple pitch angles, roll angles, and yaw angles of the multi-rotor UAV at each moment in each flight cycle through multiple instruments to form an attitude angle vector at each moment, and obtain the position coordinates of the UAV at each moment by real-time positioning of the UAV; Based on the change situation of the position coordinates of the UAV in the local time period of each flight cycle, obtain the trajectory curve; calculate the trajectory complexity of each flight cycle through the complexity of the trajectory curve and the flight speed of the UAV on the trajectory curve; Determine the attitude coordination degree of each flight cycle through the difference situation of the elements in the same dimension of the attitude angle vector at different moments in each flight cycle, and combine the trajectory complexity to determine the error tolerance weight of each flight cycle; Analyze the fluctuation conditions of the differences between the pitch angles, roll angles, and yaw angles collected by the multiple instruments at all times in each flight cycle, and determine the error evaluation value for each flight cycle; Based on the error evaluation value and the error tolerance weight, determine the adaptive factor for each flight cycle, update the measurement noise covariance matrix in the unscented Kalman filter algorithm, and perform attitude resolution on the multi-rotor UAV through the updated measurement noise covariance matrix to perform real-time control on the attitude of the manipulator of the multi-rotor UAV.

[0006] Preferably, the process of obtaining the attitude angle vector is as follows: Calculate the pitch angle, roll angle, and yaw angle of the multi-rotor UAV at each moment through the gyroscope, calculate the pitch angle, roll angle, and yaw angle of the multi-rotor UAV at each moment through the accelerometer and magnetometer, and form an attitude angle vector with the two pitch angles, two roll angles, and two yaw angles measured at each moment.

[0007] Preferably, the further method for obtaining the trajectory curve is as follows: Record the duration of multiple flight cycles before each flight cycle as the local time period; Obtain the trajectory curve of the UAV through the position coordinates at all times within the local time period.

[0008] Preferably, calculating the trajectory complexity of each flight cycle includes: Calculate the fractal dimension of the trajectory curve; calculate the average speed of the UAV flying on the trajectory curve; The trajectory complexity is the product of the fractal dimension and the average speed.

[0009] Preferably, determining the attitude coordination degree of each flight cycle includes: Arrange the elements of the same dimension within the attitude angle vector at all times in each flight cycle in chronological order to form each attitude angle sequence; record the difference between the elements at each moment and the previous moment within each attitude angle sequence as the relative difference; Adopt the natural breakpoint algorithm to obtain two segmentation points of the relative differences at all times within each attitude angle sequence, record the largest segmentation point as the upper threshold, and record the smallest segmentation point as the lower threshold; Calculate the attitude change amount at each moment within each attitude angle sequence by comparing the relative difference at each moment within each attitude angle sequence with the upper threshold and the lower threshold; Calculate the cumulative sum of the attitude change amounts at all times within each attitude angle sequence, and use the sum value of the cumulative sums of all attitude angle sequences within each flight cycle as the attitude coordination degree of each flight cycle.

[0010] Preferably, the calculation formula for the attitude change amount is: , where is the attitude change amount at the th moment in the th attitude angle sequence, is the relative difference at the th moment in the th attitude angle sequence, is the lower threshold, is the upper threshold, is a preset first value, is a preset second value, where the preset second value is greater than the preset first value.

[0011] Preferably, the error tolerance weight is the reciprocal of the product of the trajectory complexity and the attitude coordination degree.

[0012] Preferably, determining the error evaluation value for each flight cycle includes: Respectively record the differences between the corresponding pitch angles and roll angles between the gyroscope and the accelerometer at each moment as the measurement deviations corresponding to the pitch angles and roll angles at each moment; Record the difference between the yaw angle corresponding to the gyroscope and the yaw angle corresponding to the magnetometer at each moment as the measurement deviation corresponding to the yaw angle at each moment; Respectively calculate the product of the mean and standard deviation of the measurement deviations corresponding to the pitch angles, roll angles, and yaw angles at all moments within each flight cycle, and use them as the deviation fluctuation degrees corresponding to the pitch angles, roll angles, and yaw angles in each flight cycle respectively; The error evaluation value is the sum of the deviation fluctuation degrees corresponding to the pitch angle, roll angle, and yaw angle in each flight cycle.

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

[0014] Preferably, the calculation method for the updated measurement noise covariance matrix in the th flight cycle is: , where is the adaptive factor of the th flight cycle, is the measurement noise covariance matrix before update in the th flight cycle.

[0015] This application has at least the following beneficial effects: This application analyzes the complexity of the flight trajectory of a multi-rotor UAV within a local time period, calculates the trajectory complexity of each flight cycle. The beneficial effect is that it considers the complexity of the overall flight process of the UAV when performing a flight mission during this time period, so as to reflect the possibility that the UAV is performing complex flight and manipulator control tasks; determines the attitude coordination degree of each flight cycle. The beneficial effect is that it considers the significant change situation of a single specific attitude angle at adjacent moments to illustrate the situation where the attitude of the UAV changes drastically. Secondly, through the significant change situations of all attitude angles, it illustrates the attitude angle synchronization and cooperation ability when the UAV adjusts the flight trajectory with multiple attitude angles, so as to reflect the coupling complexity between multiple attitude angles of the multi-rotor UAV, and further reflect the possibility that the UAV is performing complex flight and manipulator control tasks during this flight cycle, promoting the control system of the UAV to pay more attention to the coordination degree between multiple attitude angles, improving the accuracy of the cooperative evaluation of the multi-rotor UAV, and enhancing the adaptability of the control system to complex tasks and complex environments; determines the error tolerance weight of each flight cycle. The beneficial effect is that according to the complexity of the flight mission of the UAV, different error tolerance weights for different control situations of the UAV are given. When the UAV flight and the manipulator are in simple control, it can improve the convergence speed of the subsequent attitude solution of the UAV during simple control. When the UAV flight and the manipulator are in complex control, it can reduce the response to noise, thereby improving the stability of the UAV attitude solution and the manipulator control, and enhancing the safety of the UAV manipulator control; determines the error evaluation value of each flight cycle. The beneficial effect is that it considers the deviation situation of the same kind of data measured by the gyroscope, accelerometer, and magnetometer to reflect the interference of the random noise component and the influence of the gyroscope zero-drift instability, and further illustrates the error degree caused by the multi-rotor UAV during subsequent attitude solution; determines the adaptive factor of each flight cycle. The beneficial effect is that it considers the attitude measurement error situation of the UAV and the complexity of the UAV flight and manipulator control, selects a suitable adaptive factor, and improves the stability of the UAV attitude solution and the manipulator control; and then updates the measurement noise covariance matrix in the unscented Kalman filter algorithm. Through the updated measurement noise covariance matrix, attitude calculation is performed on the multi-rotor UAV to dynamically control the manipulator of the multi-rotor UAV. The beneficial effect is that by fusing data from multiple sensors, the noise interference in the data measured by a single sensor is reduced, the error of the attitude calculation 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, the accuracy of the subsequent centroid offset compensation of the UAV is improved, and then the attitude of the multi-rotor UAV manipulator is accurately controlled. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] The following further elaborates in detail a control method for a multi-rotor UAV manipulator considering centroid offset constraints in conjunction with the accompanying drawings.

[0017] Figure 1 It is a flowchart of the steps of a control method for a multi-rotor UAV manipulator considering centroid offset constraints provided by an embodiment of the present application; Figure 2 It is a flowchart of the steps of a method for obtaining an adaptive factor provided by an embodiment of the present application. Specific embodiments

[0018] In order to make the objectives, technical solutions, and advantages of the present application more clearly understood, the following further elaborates in detail a control method for a multi-rotor UAV manipulator considering centroid offset constraints in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0019] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the technical field to which the present application belongs.

[0020] Please refer to Figure 1 , which shows a flowchart of the steps of a control method for a multi-rotor UAV manipulator considering centroid offset constraints provided by an embodiment of the present application. The method includes the following steps: Step 1, collect multiple pitch angles, roll angles, and yaw angles of the multi-rotor UAV at each moment in each flight cycle through multiple instruments, form the attitude angle vector at each moment, and obtain the position coordinates of the UAV at each moment through real-time positioning of the UAV.

[0021] With the rapid development of microelectromechanical system (MEMS) technology, low-cost, low-power, and small-sized MEMS inertial sensors are widely used in the attitude solution of multi-rotor small UAVs. The attitude estimation of UAVs is the basis for their stable flight, and the accuracy of attitude solution is directly related to the stability of attitude control. In small aircraft, gyroscopes, accelerometers, and magnetometers are often used as attitude measurement devices. The gyroscope calculates the yaw angle, pitch angle, and roll angle by measuring the angular velocity of the carrier's movement. The accelerometer calculates the pitch angle and roll angle by measuring the specific force of the carrier; the magnetometer calculates the yaw angle of the moving carrier by measuring the magnetic induction intensity of the magnetic field. However, the gyroscope has a constant drift and accumulative errors in the attitude estimation process. The accelerometer has a zero bias situation, and the magnetometer is easily affected by external disturbance magnetic sources in the surrounding environment. The accuracy of using a single sensor for attitude solution is relatively low, affecting the stable flight of the UAV.

[0022] Taking a quadrotor UAV as an example, to meet the requirements for describing its position and attitude, an inertial coordinate system, a body coordinate system, and an end effector coordinate system of the UAV manipulator are established. Among them, the inertial coordinate system , is fixedly connected to the Earth, and the origin is at the Earth's centroid; the body coordinate system , its origin is at the centroid position of the multi-rotor UAV, the axis is vertically upward perpendicular to the flight platform plane; the end effector coordinate system of the UAV manipulator, where the center point of the end effector of the UAV manipulator is taken as the origin , and the direction perpendicular to the downward direction is taken as the axis.

[0023] During the movement of the multi-rotor UAV, there are up and down, left and right, front and back, roll, pitch, yaw, and rotation of the manipulator. To better describe the spatial pose of the multi-rotor UAV, a three-axis inertial measurement sensor (IMU) is deployed on the multi-rotor UAV. The three-axis inertial measurement sensor contains an accelerometer, a magnetometer, and a gyroscope. The pitch angle, roll angle, and yaw angle are obtained in real time through the gyroscope, the pitch angle and roll angle are calculated in real time through the accelerometer, the yaw angle is calculated in real time through the magnetometer, and the attitude angles are converted from the body coordinate system to the inertial coordinate system through the rotation matrix. Therefore, the attitude angles of the multi-rotor UAV at different times are obtained, where the attitude angles include the pitch angle, roll angle, and yaw angle.

[0024] In this embodiment, the acquisition frequency of the three-axis inertial measurement sensor is 400Hz. As other implementation manners, the implementer can set it according to the actual situation; secondly, the process of transforming the translational motion of the UAV centroid between the body coordinate system and the inertial coordinate system through the rotation matrix is a well-known technology and will not be elaborated here.

[0025] Thus, there will be two pitch angles, two roll angles, and two yaw angles at the same moment for the three-axis inertial measurement sensor. All the obtained pitch angles, roll angles, and yaw angles are combined to form an attitude angle vector; Secondly, by performing real-time positioning on the multi-rotor UAV, the position coordinates of the UAV at each moment are obtained; A flight period with a preset duration is set; therefore, the attitude angle vectors of the multi-rotor UAV at different times within each flight period are obtained.

[0026] In this embodiment, the preset duration of the flight period is set to 10ms, that is, the attitude of the UAV is solved once every 10ms. As other implementation manners, the implementer can set it according to the actual situation.

[0027] So far, the attitude angle vectors and position coordinates of the multi-rotor UAV at each moment within each flight period are obtained.

[0028] Step 2: Based on the change of the position coordinates of the drone within the local time period of each flight cycle, obtain the trajectory curve; calculate the trajectory complexity of each flight cycle through the complexity of the trajectory curve and the flight speed of the drone on the trajectory curve; determine the attitude coordination degree of each flight cycle through the difference of the elements in the same dimension of the attitude angle vectors at different moments in each flight cycle.

[0029] The multi-rotor drone equipped with an assembly robotic arm can replace workers in high-risk occupations such as high-altitude rescue, construction, and equipment maintenance. It can not only fly for inspection but also complete various maintenance operations, greatly improving the flexibility and efficiency of inspection and ensuring the safety of workers' lives. Due to the different requirements of multi-rotor drones in flight missions, environmental conditions, and flight control systems, multi-rotor drones sometimes perform simple behaviors such as hovering and straight flight during flight, and sometimes when performing complex tasks, they need to operate at different positions and angles, requiring the attitude coordination and cooperation of the drones.

[0030] First, calculate the trajectory complexity by analyzing the complexity of the flight route of the drone in each flight cycle, specifically: Record the duration of multiple flight cycles before each flight cycle as the local time period; In this embodiment, the cumulative duration of the 5 flight cycles before each flight cycle is selected and recorded as the local time period. As other implementation manners, the implementer can set it by himself according to the actual situation.

[0031] Obtain the trajectory curve of the drone through the position coordinates at all moments within the local time period; Calculate the fractal dimension of the trajectory curve; In this embodiment, the Higuchi algorithm is used to calculate the fractal dimension. Among them, the Higuchi algorithm is a well-known technology and will not be elaborated here. As other implementation manners, the implementer can adopt other methods of existing technologies, such as the Hurst exponent method, etc. This embodiment does not make special restrictions on this.

[0032] Calculate the average speed of the drone flying on the trajectory curve; 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.

[0033] Take the product of the fractal dimension and the average speed as the trajectory complexity of each flight cycle; It should be noted that the fractal dimension reflects the tortuosity of the flight trajectory of the 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 higher the tortuosity of the trajectory of the UAV at a high flight speed, reflecting that the overall flight system of the UAV in this flight cycle is more complex and is more likely to be performing complex flight and robotic arm control tasks.

[0034] Furthermore, the attitude change amount is calculated by the difference in the attitude angles at different times within each flight cycle for each attitude angle, specifically as follows: The elements of the same dimension within the attitude angle vectors at all times within each flight cycle are arranged in chronological order to form each attitude angle sequence; It should be noted that since the dimension of the attitude angle vector is 6, the number of attitude angle sequences is 6.

[0035] The difference between the elements at each moment within each attitude angle sequence and the previous moment is denoted as the relative difference; In this embodiment, the absolute value of the difference between the elements at each moment within each attitude angle sequence and the previous moment is denoted as the relative difference.

[0036] Using the natural breakpoint algorithm, two breakpoints of the relative differences at all times within each attitude angle sequence are obtained. The largest breakpoint is denoted as the upper threshold, and the smallest breakpoint is denoted as the lower threshold; It should be noted that the natural breakpoint algorithm is a well-known technology and will not be elaborated here.

[0037] The calculation formula for the attitude change amount at each moment within each attitude angle sequence is:

[0038] Where, is the attitude change amount at the -th moment within the -th attitude angle sequence, is the relative difference at the -th moment within the -th attitude angle sequence, is the lower threshold, is the upper threshold, is a preset first value, is a preset second value. In this embodiment, the preset first value takes a value of 0, and the preset second value takes a value of 1. As other implementation manners, the implementer can set them according to the actual situation.

[0039] It should be noted that the larger the attitude change amount, that is, the closer it is to 1, it indicates that during the flight of the multi-rotor UAV, the attitude of the UAV has changed drastically at the corresponding moment, reflecting that the UAV is more likely to be performing complex flight tasks at this time; on the contrary, the smaller the attitude change amount, that is, the closer it is to 0, it indicates that the attitude angle has not changed much at the corresponding moment, and the attitude of the UAV is relatively stable.

[0040] Furthermore, based on the attitude change amount, calculate the attitude coordination degree, specifically: Calculate the sum of the attitude change amounts at all moments of each attitude angle sequence, and take the sum of the sums of all attitude angle sequences within each flight cycle as the attitude coordination degree of the multi-rotor UAV in each flight cycle; It should be noted that the larger the attitude coordination degree, it indicates that the multi-attitude angles of the UAV jointly adjust the flight trajectory, the stronger the synchronous cooperation ability of the attitude angles, the higher the coupling complexity between the multi-attitude angles of the multi-rotor UAV, and it is more likely to be performing complex flight and manipulator control tasks in this flight cycle; on the contrary, the smaller the attitude coordination degree, it indicates that the attitude angle changes are relatively independent or small, and it may be performing simple flight tasks.

[0041] So far, the trajectory complexity and attitude coordination degree of the multi-rotor UAV in each flight cycle are obtained.

[0042] Step 3, based on the attitude coordination degree and trajectory complexity, determine the error tolerance weight for each flight cycle; analyze the fluctuation conditions of the differences between the pitch angles, roll angles, and yaw angles collected by multiple instruments at all moments in each flight cycle, and determine the error evaluation value for each flight cycle.

[0043] Since the UAV performing simple flight tasks has relatively low requirements for the attitude control of the UAV, therefore, during the process of attitude solution, even if there are certain errors in the attitude measurement data, it will not significantly affect the completion of the task, so the error tolerance for the attitude measurement data of the UAV is relatively high. For the multi-rotor UAV performing complex flight and manipulator control tasks, the requirements for the attitude control of the UAV are relatively high, and the requirements for the accuracy and precision of the attitude solution are also higher. A small error in the attitude measurement data may lead to task failure or unstable flight of the UAV, and the error tolerance for the attitude measurement data of the UAV is lower. Thus, through the trajectory complexity and the attitude coordination degree, obtain the error tolerance weight, specifically: Take the reciprocal of the product of the trajectory complexity and the attitude coordination degree as the error tolerance weight of the multi-rotor UAV in each flight cycle; It should be noted that when calculating the reciprocal, to avoid a denominator of 0, a preset value greater than 0 is added to the denominator. In this embodiment, the preset value greater than 0 is taken as 0.1. As other implementation manners, the implementer can set it according to the actual situation.

[0044] It should be noted that the smaller the error tolerance weight, the higher the accuracy of the attitude solution for the drone.

[0045] Secondly, the attitude solution of the drone usually depends on the measurement data of multiple sensors. Among them, the attitude angle data of the gyroscope has high short-term stability, and the attitude solution depends on the gyroscope attitude measurement data, which is more suitable for multi-rotor drones performing complex flight and robotic arm control tasks. However, the gyroscope is 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 reading of the gyroscope will drift over time. The greater the zero drift intensity, the greater the measurement error of the attitude. At the same time, this error will accumulate with the rotation of the gyroscope or the drift of the angle estimation, and the zero drift instability can further reduce the credibility of the attitude measurement data.

[0046] Therefore, by analyzing the differences between the attitude angles measured by the gyroscope and the attitude angles measured by the accelerometer and magnetometer, an error evaluation value is calculated, specifically: 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 deviations corresponding to the pitch angles at all moments within each flight cycle is calculated as the deviation fluctuation corresponding to the pitch angle in each flight cycle; 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 deviations corresponding to the roll angles at all moments within each flight cycle is calculated as the deviation fluctuation corresponding to the roll angle in each flight cycle; 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 deviations corresponding to the yaw angles at all moments within each flight cycle is calculated as the deviation fluctuation corresponding to the yaw angle in each flight cycle; 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; 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.

[0047] Sum the deviation fluctuation degrees corresponding to the pitch angle, roll angle, and yaw angle in each flight cycle, and use it as the error evaluation value of the multi-rotor UAV in each flight cycle; It should be noted that the larger the mean value, the greater the deviation of the attitude angle data measured by the gyroscope from that measured by the accelerometer and magnetometer for different sensors, the more random noise components in the corresponding attitude angle, or the higher the zero drift intensity of the gyroscope. The larger the standard deviation, the stronger the instability of the data deviation of the attitude angle measured by different sensors, and the more likely it is caused by the zero drift instability of the gyroscope. Then, the larger the deviation fluctuation degree, the larger the obtained error evaluation value, indicating that the error in the attitude measurement data for attitude solution of the multi-rotor UAV is higher, and the lower the credibility of the subsequent attitude calculation result.

[0048] Thus, the error evaluation value of the multi-rotor UAV in each flight cycle is obtained.

[0049] Step 4: Based on the error evaluation value and the error tolerance weight, determine the adaptive factor for each flight cycle, update the measurement noise covariance matrix in the unscented Kalman filter algorithm, and perform attitude solution for the multi-rotor UAV through the updated measurement noise covariance matrix to perform real-time control on the attitude of the manipulator of the multi-rotor UAV.

[0050] Furthermore, based on the error tolerance weight and the error evaluation value, determine the adaptive factor, specifically: Use the normalized result of the ratio of the error evaluation value to the error tolerance weight as the adaptive factor for each flight cycle; In this embodiment, the sigmoid function is used for normalization. The sigmoid function is a well-known technology and will not be elaborated here. As other implementation manners, implementers can use other methods in the prior art, such as the tanh function, etc. This embodiment does not make special restrictions on this.

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

[0052] Secondly, due to the cumulative error of the gyroscope and the large external interference on the accelerometer and magnetometer, it is necessary to perform effective data fusion to ensure obtaining relatively accurate attitude angles. Due to the problems of random drift error and susceptibility to non-gravitational acceleration in attitude calculation, by adaptively adjusting the measurement noise covariance matrix in the unscented Kalman filter algorithm and performing attitude calculation, data fusion is achieved, thereby reducing the influence of non-gravitational acceleration, suppressing the random drift error of the gyroscope, and adaptively compensating for the influence of non-gravitational acceleration on attitude estimation.

[0053] Before the process of attitude calculation through the unscented Kalman filter algorithm, the measurement data of the gyroscope, accelerometer, and magnetometer are converted into quaternion form for representation using the quaternion method; It should be noted that the quaternion method is a well-known technology and will not be elaborated here.

[0054] After performing quaternion transformation on the angular velocity data output by the gyroscope, the state equation of the unscented Kalman filter algorithm is constructed; After performing quaternion transformation on the acceleration data and geomagnetic field data output by the accelerometer and magnetometer, the measurement equation of the unscented Kalman filter algorithm is constructed; It should be noted that the construction processes of the state equation and the measurement equation are well-known technologies and will not be elaborated here.

[0055] Through the state equation and the measurement equation, the state noise covariance matrix and the measurement noise covariance matrix are obtained; 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:

[0056] Wherein, is the updated measurement noise covariance matrix at the th flight cycle, is the adaptive factor at the th flight cycle, is the measurement noise covariance matrix before update at the th flight cycle; It should be noted that the unscented Kalman filter algorithm based on quaternion is a well-known technology and will not be elaborated here.

[0057] Based on the updated measurement noise covariance matrix and state noise covariance matrix, the gain coefficient is obtained. Combining with the unscented Kalman filter algorithm, attitude resolution is performed on the multi-rotor UAV, thereby more accurately reflecting the flight attitude of the multi-rotor UAV, and then evaluating the centroid offset of the UAV. By considering the influence of the centroid offset, centroid compensation is carried out to offset the changes in force and moment caused by the centroid offset, and then dynamic control of the multi-rotor unmanned manipulator is performed; the measurement noise covariance matrix is updated by the adaptive factor to improve the adaptability of the multi-rotor UAV in different flight environments and avoid safety accidents.

[0058] It should be noted that the specific process of dynamically controlling the multi-rotor unmanned manipulator is as follows: Based on the result of attitude resolution of the multi-rotor UAV, a UAV dynamics model and a manipulator dynamics model are established. According to the deviation between the attitude resolution result and the desired attitude, combined with the dynamic characteristics of the UAV, the approximate direction and magnitude of the centroid offset are deduced, the centroid offset vector is estimated in real time, and a compensation control law is used to adjust the attitude of the quad-rotor UAV and the movement of the manipulator to offset the influence of the centroid offset. Among them, the design of the compensation control law is specifically as follows: According to the deviation between the attitude resolution result and the desired attitude, the required centroid offset compensation torque is calculated by a PID controller, the centroid offset compensation torque is distributed to each manipulator, and the rotational speed of the rotor is adjusted to make the actual attitude of the UAV approach the desired attitude and offset the influence caused by the centroid offset. Furthermore, according to the task planning of the manipulator, the desired trajectory of its joint angle is calculated. On the basis of considering centroid offset compensation, through inverse kinematics and inverse dynamics calculations, the required driving torque of each joint of the manipulator is obtained, and the actual joint angle of the manipulator and the attitude information of the UAV are fed back in real time to adjust the movement of the manipulator. Among them, the construction methods of the UAV dynamics model and the manipulator dynamics model, and the process of calculating the required driving torque of each joint of the manipulator by inverse kinematics and inverse dynamics are well-known technologies and will not be elaborated here.

[0059] It should be noted that the process of attitude resolution by the unscented Kalman filter algorithm and the method of centroid offset compensation are both well-known technologies and will not be elaborated here.

[0060] It should be understood that although Figure 1 the steps in the flowchart of Figure 1At least some of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages do not necessarily need to be completed 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 alternately or in turns with at least some of the other steps or sub-steps or stages of the other steps.

[0061] The technical features of the above embodiments can be combined arbitrarily. For the sake of brevity of description, 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, it should be considered as falling within the scope described in this specification.

[0062] The above-described embodiments only represent several implementation manners of the present application. The description is relatively specific and detailed, but it should not be construed as a limitation to the present application. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present application, several modifications and improvements can still be made. Therefore, any simple modifications, equivalent changes and decorations made to the above embodiments based on the technical essence of the present application without departing from the content of the technical solution of the present application all belong to the protection scope of the technical solution of the present application.

Claims

1. A control method for the manipulator of a multi-rotor UAV considering the centroid offset constraint, characterized in that, The method includes the following steps: Collect multiple pitch angles, roll angles, and yaw angles of the multi-rotor UAV at each moment in each flight cycle through multiple instruments, form the attitude angle vector at each moment, and obtain the position coordinates of the UAV at each moment by real-time positioning of the UAV; Based on the change of the position coordinates of the UAV in the local time period of each flight cycle, obtain the trajectory curve; calculate the trajectory complexity of each flight cycle through the complexity of the trajectory curve and the flight speed of the UAV on the trajectory curve; Determine the attitude coordination degree of each flight cycle through the difference of the elements in the same dimension of the attitude angle vector at different moments in each flight cycle, and combine the trajectory complexity to determine the error tolerance weight of each flight cycle; Analyze the fluctuation of the difference between the pitch angles, the fluctuation of the difference between the roll angles, and the fluctuation of the difference between the yaw angles collected by the multiple instruments at all moments in each flight cycle to determine the error evaluation value of each flight cycle; Based on the error evaluation value and the error tolerance weight, determine the adaptive factor of each flight cycle, update the measurement noise covariance matrix in the unscented Kalman filter algorithm, and perform attitude solution on the multi-rotor UAV through the updated measurement noise covariance matrix to perform real-time control on the attitude of the manipulator of the multi-rotor UAV.

2. The multi-rotor UAV manipulator control method considering the centroid offset constraint according to claim 1, characterized in that The process of obtaining the attitude angle vector is as follows: Calculate the pitch angle, roll angle, and yaw angle of the multi-rotor UAV at each moment through the gyroscope, calculate the pitch angle, roll angle, and yaw angle of the multi-rotor UAV at each moment through the accelerometer and magnetometer, and form the attitude angle vector with the two pitch angles, two roll angles, and two yaw angles measured at each moment.

3. A control method for a multi-rotor UAV manipulator considering the centroid offset constraint as claimed in claim 1, characterized in that, The further method for obtaining the trajectory curve is as follows: Record the duration of multiple flight cycles before each flight cycle as the local time period; Obtain the trajectory curve of the UAV through the position coordinates at all moments within the local time period.

4. The method for controlling the robotic arm of a multi-rotor UAV considering the centroid offset constraint according to claim 1, wherein, The calculation of the trajectory complexity of each flight cycle includes: Calculate the fractal dimension of the trajectory curve; calculate the average speed of the UAV flying on the trajectory curve; The trajectory complexity is the product of the fractal dimension and the average speed.

5. A control method for the manipulator of a multi-rotor UAV considering the centroid offset constraint as claimed in claim 1, characterized in that, The determination of the attitude coordination degree of each flight cycle includes: Arrange the elements in the same dimension of the attitude angle vector at all moments within each flight cycle in chronological order to form each attitude angle sequence; record the difference between the elements at each moment and the previous moment within each attitude angle sequence as the relative difference; Adopt the natural breakpoint algorithm to obtain two segmentation points of all the relative differences within each attitude angle sequence, record the largest segmentation point as the upper threshold, and record the smallest segmentation point as the lower threshold; By comparing the relative differences at each moment within each attitude angle sequence with the upper threshold and the lower threshold, calculate the attitude change amount at each moment within each attitude angle sequence; Calculate the sum of the accumulated attitude change amounts at all moments of each attitude angle sequence, and use the sum value of all attitude angle sequences within each flight cycle as the attitude coordination degree of each flight cycle.

6. The manipulator control method of a multi-rotor UAV considering the centroid offset constraint according to claim 5, characterized in that The calculation formula for the attitude change amount is as follows: , where is the attitude change amount at the -th moment in the -th attitude angle sequence, is the relative difference at the -th moment in the -th attitude angle sequence, is the lower threshold, is the upper threshold, is a preset first value, is a preset second value, where the preset second value is greater than the preset first value.

7. A control method for the manipulator of a multi-rotor UAV considering the centroid offset constraint as described in claim 1, characterized in that The error tolerance weight is the reciprocal of the product of the trajectory complexity and the attitude coordination degree.

8. The method for controlling a robotic arm of a multi-rotor UAV considering the centroid offset constraint according to claim 2, wherein The determination of the error evaluation value for each flight cycle includes: Respectively record the differences between the corresponding pitch angles and roll angles between the gyroscope and the accelerometer at each moment as the measurement deviations corresponding to the pitch angles and roll angles at each moment; Record the difference between the yaw angle corresponding to the gyroscope and the yaw angle corresponding to the magnetometer at each moment as the measurement deviation corresponding to the yaw angle at each moment; Respectively calculate the product of the mean and standard deviation of the measurement deviations corresponding to the pitch angles, roll angles, and yaw angles at all moments within each flight cycle, and use them as the deviation fluctuation degrees corresponding to the pitch angles, roll angles, and yaw angles in each flight cycle; The error evaluation value is the sum of the deviation fluctuation degrees corresponding to the pitch angles, roll angles, and yaw angles in each flight cycle.

9. The method for controlling a robotic arm of a multi-rotor UAV considering the centroid offset constraint according to claim 1, wherein The adaptive factor is the normalized result of the ratio of the error evaluation value to the error tolerance weight.

10. The method for controlling the manipulator of a multi-rotor UAV considering the centroid offset constraint as described in claim 1, wherein, The calculation method of the updated measurement noise covariance matrix at the th flight cycle is as follows: , where is the adaptive factor at the th flight cycle, is the measurement noise covariance matrix before update at the th flight cycle.

Images