Flight manipulator end control method resistant to multi-source interference and coordinated motion planning

Abstract

The application relates to a flight mechanical arm end control method resisting multi-source interference and coordinating motion planning, aiming at the problem of environmental external force, mechanical arm swing and the like influencing flight mechanical arm control precision, first, flight mechanical arm coupling dynamics equations containing environmental external force and mechanical arm swing are constructed; second, corresponding estimators are designed aiming at the coupling action between the mechanical arm and the unmanned plane; third, double disturbance observers are designed aiming at the multi-source interference; then, aiming at the angle between the direction of the disturbance and the expected motion track, a fine anti-interference controller is designed to compensate or utilize; finally, through an integrated coordinated motion planner, the motion of the unmanned plane and the mechanical arm is planned, and high-precision motion control of the flight mechanical arm under multi-source interference is completed; the method can significantly improve the pose control precision of a multi-rotor flight mechanical arm adopting a direct current motor actuator, and can be used for high-precision coordinated motion control of multi-rotor unmanned systems such as flight mechanical arms.

Description

Technical Field

[0001] This invention belongs to the field of special operation unmanned aerial vehicle (UAV) control, specifically relating to a flight manipulator end control method that resists multi-source interference and coordinates motion planning. It is applicable to multi-rotor flight unmanned systems equipped with multi-degree-of-freedom manipulators and requiring high-precision attitude control. Background Technology

[0002] In recent years, with the increasing application of multi-rotor drones and the growing complexity of their missions, there is a growing demand for multi-rotor drones to be equipped with more functional multi-degree-of-freedom robotic arms as special mission payloads. Furthermore, as the missions and scenarios of multi-rotor drone systems become increasingly complex, higher demands are placed on the attitude control of the robotic arm's end effector, requiring lighter and more flexible robotic arms to achieve high-precision object grasping. From the perspective of endurance and drone stability requirements, the mass and volume of the robotic arm and end effector are strictly limited. Therefore, the aforementioned robotic arms and end grippers typically employ lightweight, high-strength multi-degree-of-freedom mechanical structures. Simultaneously, the increased degrees of freedom of the robotic arm complicates the coupling between the drone and the robotic arm, causing changes in the center of mass and inertia of the entire flight robotic arm system, severely affecting the attitude control accuracy of the flight robotic arm system.

[0003] Furthermore, high-precision anti-interference control of flying robotic arms under multi-source interference conditions has always been a key technology in the development of special-purpose UAVs. Brushless DC motors are one of the most important actuators in the attitude control system of UAVs, and the accuracy of the UAV's position control determines the control accuracy of the robotic arm's end effector. Conventional coaxial multi-rotor UAVs are typical underactuated systems, and their position control accuracy is easily affected by environmental interference. For special-purpose UAVs represented by flying robotic arms, on the one hand, there are external environmental interferences in the real environment, such as gusts of wind, obstacle collisions, and interactions during object grasping. These external interferences can be treated as lumped interferences during modeling. On the other hand, the coupling effect generated by the robotic arm's movement affects the UAV through its rigid connection with the UAV. This coupling torque easily transmits back and forth between the robotic arm and the UAV body, causing deviations in the UAV's end effector control. Simultaneously, considering the underactuated characteristics of the UAV system, attitude errors further affect the UAV's position control and lead to a decrease in the grasping accuracy of the flying robotic arm's end effector, ultimately resulting in mission failure. Therefore, to achieve high-precision attitude control of flying robotic arms, the influence of the aforementioned multi-source interferences must be overcome during the design of this type of special-purpose UAV. Finally, the external forces exerted on the UAV by the coupling effect of environmental forces and robotic arms are considered as disturbance forces on the UAV. When the direction of the disturbance force is different from the desired motion, the anti-interference strategy of "intelligent classification and compensation utilization" can achieve the effect of rapid response to changes in external forces and saving control energy.

[0004] Chinese patent application CN201721229966.1 proposes a rotary-wing flying robotic arm system for grasping operations, but it has two problems: (1) the paper does not consider the external force interference caused by the coupling effect of the robotic arm movement on the position control of the UAV, which will affect the attitude and position control accuracy of the UAV; (2) it does not consider the relationship between the lumped external force interference and the desired motion trajectory, which will be limited in actual use. Chinese patent application CN201811366685.6 proposes a rotary-wing flying dual robotic arm target positioning and grasping system and method, which also has similar problems: (1) it does not consider the coupling effect of the robotic arm and the influence of the external environment on the UAV; (2) it does not fully consider the relationship between the lumped external force interference of the UAV and the desired trajectory involved in the method, so the accuracy and stability are not good enough. Chinese patent application CN202110994016.9 proposes an anti-interference pose control method for multi-rotor robotic arms, but it has two problems: (1) it does not provide a specific calculation method for multi-source interference; (2) it does not consider coordinating motion planning for the end effector of the robotic arm gripper. Therefore, when the robotic arm swings, it has a greater impact on the control of the UAV and reduces the control accuracy of the end effector gripper. Summary of the Invention

[0005] To overcome the shortcomings of existing technologies, this invention provides a flight robotic arm end-effector control method that resists multi-source interference and coordinates motion planning. This method is used to improve the stability and environmental adaptability of aerial robotic arm systems based on rotary-wing UAVs, and enables high-precision end-effector control of the flight robotic arm system.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: A method for pose control of a flight robotic arm that resists multi-source interference and coordinates motion planning includes the following steps: Step 1: Establish the coupled dynamic equations of the flying robotic arm containing environmental external forces and robotic arm swing, and design a corresponding estimator for the coupling effect between the robotic arm and the UAV; Step 2: For the multi-source disturbances in the continuous interaction between the flying robotic arm and the external environment, the multi-source disturbances are divided into model-based disturbances and model-free disturbances according to the different characteristics of internal disturbances, external disturbances, and model uncertainties, and a dual-disturbance observer is designed accordingly. Step 3: In response to the different angles between the direction of the interference and the desired motion trajectory, an intelligent matching and compensation strategy is adopted. When the direction of the interference is the same as the desired motion direction, the designed fine anti-interference controller utilizes this interference to reduce energy consumption; otherwise, the controller compensates for the impact of the interference. Step 4: Design an integrated motion planner for the UAV and robotic arm based on a fine anti-interference controller and nonlinear model prediction method. By planning the integrated coordinated motion of the end gripper of the robotic arm, the internal coupling effect between the robotic arm and the UAV is reduced, and high-precision attitude coordinated motion control of the flying robotic arm under the influence of multi-source interference is achieved.

[0007] Further, step one includes: To address the saturation characteristics of brushless DC motors and considering both the coupling effect of the robotic arm and external force disturbances in the dynamic equations, a flying robotic arm model is established, as follows: ; In the formula, The inertia matrix representing the flying robotic arm; Represents the Coriolis matrix; It is a gravity term; The generalized state variables representing multi-rotor drones include the drone's position and the joint angles of the robotic arm; The above represents the generalized velocity variable of a multi-rotor UAV. This represents the first derivative, including the translational velocity of the drone and the joint angular velocity of the robotic arm; The generalized acceleration variables representing multi-rotor drones include the translational acceleration of the drone and the joint acceleration of the robotic arm; For the control input variables of multi-rotor drones, including drone propeller commands and robotic arm joint torques; The interference received by the multi-rotor UAV includes interference from the center of mass shift on the base, contact force interference from environmental interaction, and interference from model uncertainty. Further results were obtained: ; In the formula, This is the transformation matrix from the body coordinate system to the inertial coordinate system of the multi-rotor UAV; The angular velocity of the multi-rotor drone; This represents the coupling force generated by the swinging of the robotic arm, which is considered as an interfering external force applied to the drone. This indicates the external force interference generated by the interaction between the flying robotic arm and the external environment; This represents the center of mass of the system with the base as the reference origin. The second derivative is represented in the formula; The calculation is as follows: In the formula Indicates the first The quality of each joint This indicates the center of mass of the robotic arm's joints. This indicates the total mass of the multi-rotor drone.

[0008] Furthermore, in step two, the model interference is characterized by the following model: ; In the formula, and These represent the rotation angle and angular velocity of the multi-rotor UAV about its center of mass, respectively. Represents model-free lumped perturbation; the above Denotes the first derivative; This represents the equivalent moment of inertia of a multi-rotor unmanned aerial vehicle (UAV) about its center of mass. denoted by , representing the equivalent Coriolis term of the multi-rotor UAV around its center of mass; h represents the derivative of the lumped perturbation; There is model interference; This indicates no model interference; The specific implementation of the interference observer with known model information is as follows: ; in, It is an auxiliary state variable of the base floating observer. It is the rate of change of the auxiliary state variable. It is the effect of external interference on the system state. The base floating is for lumped interference The estimated value, The gain of the interference observer to be determined is... It is related to the control quantity The relevant control allocation matrix, , It is the characteristic matrix of the system's state equations. Indicates control input, and It is the parameter matrix of the interference model. This represents the equivalent moment of inertia of the flying robotic arm when its joints are initially configured. This indicates the attitude measurement output of the UAV; , , for , The predicted value; The extended observer is designed to handle model-free disturbances as follows: ; In the formula, This represents the true state value of a multi-rotor drone. This represents the sensor measurement output of a multi-rotor drone. , , These represent the gain matrices of the observer, Indicates and The relevant control quantities of drones.

[0009] Furthermore, step three includes: Based on the multi-source interference received by the multi-rotor drone Desired interaction force with the flying robotic arm The included angle Determine compensation or utilize multi-source interference received by multi-rotor drones ,in, The total mass of the multi-rotor drone, This represents the desired acceleration of the flying robotic arm; when the interference is in the same direction as the desired motion, then the multi-source interference received by the multi-rotor drone is utilized. To save energy consumption; when the direction of the disturbance is opposite to the desired motion, then the intelligent system selects a compensation strategy that completely subtracts the disturbance. The strategy for intelligent classification is as follows: ; In the formula, Indicates the interference compensation term; It refers to time, indicating that the item is time-varying.

[0010] Furthermore, step four includes: Coordinated motion planning for UAV and robotic arm based on nonlinear model predictive control method: ; The constraints are satisfied: ; ; ; In the formula, Indicates to The minimum optimization function; This represents the cost function to be optimized. , and These represent the predicted system output state, the expected output, and the increment of the control input, respectively. The first nonlinear model predictive controller represents the... step; Representing vectors The second norm; It predicts the step size; It controls the step size; and These represent the weight matrices for tracking error and control energy, respectively. and These represent the lower and upper limits of the control input, respectively; and Indicates the lower and upper limits of the control input increment; It is a positive constant to ensure closed-loop stability; Indicates the index value.

[0011] The advantages of this invention compared to the prior art are as follows: This invention primarily considers aerial work platforms using multi-rotor UAVs as carriers. It deeply analyzes the multi-source coupled interference and environmental force interference caused by the movement of the robotic arm on the UAV work platform, clarifying the impact of each interference source on the system's position and attitude control error. Multi-source interference is categorized into model-based and model-free interference, thus broadening its applicability. Furthermore, a dual-interference observer is used for the UAV work platform, enabling rapid and accurate tracking of lumped interference. Simultaneously, a compensation relationship between the interference force and the desired trajectory is designed, increasing the rapid response capability of the flight robotic arm's attitude control system and reducing the energy consumption of the UAV in resisting lumped interference. Finally, a nonlinear model predictive control method is integrated to achieve integrated coordinated motion control of the UAV and robotic arm, significantly improving the speed, accuracy, and stability of the flight robotic arm's end-effector control method. Attached Figure Description

[0012] Figure 1 This is a flowchart of a flight robotic arm pose control method for resisting multi-source interference and coordinating motion planning according to the present invention; Figure 2 The simulation results are shown in the figure, which illustrates the use of the basic controller and the present invention. Detailed Implementation

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

[0014] like Figure 1 As shown, the present invention provides a flight robotic arm pose control method for resisting multi-source interference and coordinating motion planning, comprising the following steps: Step 1: Establish the coupled dynamic equations of the flying robotic arm, which include environmental forces and the motion characteristics of the robotic arm, including: Design a corresponding estimator for the coupling effect between the robotic arm and the drone; To address the saturation characteristics of brushless DC motors and considering both the coupling effect of the robotic arm and external force disturbances in the dynamic equations, a flying robotic arm model is established, as follows: ; In the formula, The inertia matrix representing the flying robotic arm; Represents the Coriolis matrix; It is a gravity term; The generalized state variables representing multi-rotor drones include the drone's position and the joint angles of the robotic arm; The above represents the generalized velocity variable of a multi-rotor UAV. This represents the first derivative, including the translational velocity of the drone and the joint angular velocity of the robotic arm; The generalized acceleration variables representing multi-rotor drones include the translational acceleration of the drone and the joint acceleration of the robotic arm; For the control input variables of multi-rotor drones, including drone propeller commands and robotic arm joint torques; The interference received by the multi-rotor UAV includes interference from the center of mass shift on the base, contact force interference from environmental interaction, and interference from model uncertainty. Further results were obtained: ; In the formula, This is the transformation matrix from the body coordinate system to the inertial coordinate system of the multi-rotor UAV; The angular velocity of the multi-rotor drone; This represents the coupling force generated by the swinging of the robotic arm, which is considered as an interfering external force applied to the drone. This indicates the external force interference generated by the interaction between the flying robotic arm and the external environment; This represents the centroid of the system with the base as the reference origin; the dot (.) above it represents the first derivative, and the dot (..) above it represents the second derivative; in the formula... The following can be calculated: In the formula, Indicates the first The quality of each joint This indicates the center of mass of the robotic arm's joints. This indicates the total mass of the multi-rotor drone.

[0015] Step 2: Design a multi-source interference dual-observator, including: For interference caused by interaction with the external environment in the flying robotic arm, the interference is divided into model-based interference (model information is known) and model-free interference (model information is unknown) according to different characteristics such as internal interference, external interference, and model uncertainty. For model-based interference, it can be characterized by the following model: ; In the formula, and These represent the rotation angle and angular velocity of the multi-rotor UAV about its center of mass, respectively. Represents model-free lumped perturbation; the above Denotes the first derivative; This represents the equivalent moment of inertia of a multi-rotor unmanned aerial vehicle (UAV) about its center of mass. denoted by , representing the equivalent Coriolis term of the multi-rotor UAV around its center of mass; h represents the derivative of the lumped perturbation; There is model interference; This indicates no model interference; The interference observer with known model information is implemented as follows: ; in, It is an auxiliary state variable of the base floating observer. It is the rate of change of the auxiliary state variable. It is the effect of external interference on the system state. The base floating is for lumped interference The estimated value, The gain of the interference observer to be determined is... It is related to the control quantity The relevant control allocation matrix, , It is the characteristic matrix of the system's state equations. Indicates control input, and It is the parameter matrix of the interference model. This represents the equivalent moment of inertia of the flying robotic arm when its joints are initially configured. This indicates the attitude measurement output of the UAV; , , for , The predicted value; Then, for model-free lumped perturbations, an extended observer is designed as follows: ; In the formula, This represents the true state value of a multi-rotor drone. This represents the sensor measurement output of a multi-rotor drone. , , These represent the gain matrices of the observer, Indicates and The relevant control quantities of drones.

[0016] Step 3: Determine compensation based on the direction of interference or design a sophisticated anti-interference controller using the interference, including: To address observation errors such as estimation errors in the coupling effect between the robotic arm and the drone, and interference from environmental forces, a sophisticated anti-interference controller is designed. Furthermore, an intelligent classification and compensation strategy is employed for the estimated interference, and a coordinated motion planner is designed using nonlinear model methods to achieve end-effector control of the flying robotic arm. Based on the multi-source interference received by the multi-rotor drone Desired interaction force with the flying robotic arm The included angle To determine whether to compensate for or utilize this interference, The total mass of the multi-rotor drone, This represents the desired acceleration of the flying robotic arm. When the disturbance is in the same direction as the desired motion, it can be utilized to save energy. When the disturbance is in the opposite direction to the desired motion, a conventional disturbance compensation strategy is intelligently selected (the conventional disturbance compensation strategy is one that doesn't have specific classification and simply subtracts the disturbance term). Therefore, the intelligent classification strategy is as follows: ; In the formula, Indicates the interference compensation term; It refers to time, indicating that the item is time-varying.

[0017] Step 4: Integrate nonlinear model predictive control methods to achieve integrated coordinated motion control of the UAV and robotic arm, including: ; The constraints are satisfied: ; ;

[0018] ; In the formula, Indicates to The minimum optimization function; This represents the cost function to be optimized. , and These represent the predicted system output state, the expected output, and the increment of the control input, respectively. The first nonlinear model predictive controller represents the... step; Representing vectors The second norm; It predicts the step size; It controls the step size; and These represent the weight matrices for tracking error and control energy, respectively. and These represent the lower and upper limits of the control input, respectively; and Indicates the lower and upper limits of the control input increment; It is a positive constant to ensure closed-loop stability; i is the index value.

[0019] According to an embodiment of the present invention, a general-purpose special-operation flying robotic arm that uses a multi-rotor UAV as an aerial work platform is used as an example to illustrate the specific implementation of the present invention. When the flying robotic arm performs object manipulation operations in the air, it has high requirements for position and attitude control accuracy and stability.

[0020] Example: This invention uses a robotic arm to verify its method. The parameters of the robotic arm are shown in Table 1, and the simulation comparison results are shown in Table 2. Compared with the basic controller, the anti-interference method proposed in this invention can improve the average position error and variance by 48.6% and 40.8%, respectively. Figure 2 As shown.

[0021] Table 1

[0022] Table 2

[0023] Therefore, the present invention can ensure that the flight robotic arm control system can track quickly and maintain stability, and achieve high-precision attitude control of the flight robotic arm under the condition of lumped interference.

[0024] The contents not described in detail in this specification are existing technologies known to those skilled in the art.

Claims

1. A method for pose control of a flight robotic arm that resists multi-source interference and coordinates motion planning, characterized in that, Includes the following steps: Step 1: Establish the coupled dynamic equations of the flying robotic arm containing environmental external forces and robotic arm swing, and design a corresponding estimator for the coupling effect between the robotic arm and the UAV; Step 2: For the multi-source disturbances in the continuous interaction between the flying robotic arm and the external environment, the multi-source disturbances are divided into model-based disturbances and model-free disturbances according to the different characteristics of internal disturbances, external disturbances, and model uncertainties, and a dual-disturbance observer is designed accordingly. Step 3: Addressing the issue of different angles between the direction of interference and the desired trajectory, an intelligent matching and compensation strategy is employed. When the direction of interference is the same as the desired trajectory, the designed sophisticated anti-interference controller utilizes this interference to reduce energy consumption; otherwise, the controller compensates for the interference's impact, including: Based on the multi-source interference received by the multi-rotor drone Desired interaction force with the flying robotic arm The included angle Determine compensation or utilize multi-source interference received by multi-rotor drones ,in, The total mass of the multi-rotor drone, This represents the desired acceleration of the flying robotic arm; when the interference is in the same direction as the desired motion, then the multi-source interference received by the multi-rotor drone is utilized. To save energy consumption; when the direction of the disturbance is opposite to the desired motion, then the intelligent system selects a compensation strategy that completely subtracts the disturbance. The strategy for intelligent classification is as follows: ； In the formula, Indicates the interference compensation term; It refers to time, indicating that the item is time-varying; This represents the angle between the multi-source interference received by the multi-rotor drone and the expected interaction force of the flying robotic arm; Step 4: Design an integrated motion planner for the UAV and robotic arm based on a fine anti-interference controller and nonlinear model prediction method. By planning the integrated coordinated motion of the end gripper of the robotic arm, the internal coupling effect between the robotic arm and the UAV is reduced, and high-precision attitude coordinated motion control of the flying robotic arm under the influence of multi-source interference is achieved.

2. The flight robotic arm pose control method for resisting multi-source interference and coordinating motion planning according to claim 1, characterized in that, Step one includes: To address the saturation characteristics of brushless DC motors and considering both the coupling effect of the robotic arm and external force disturbances in the dynamic equations, a flying robotic arm model is established, as follows: ； In the formula, The inertia matrix representing the flying robotic arm; Represents the Coriolis matrix; It is a gravity term; The generalized state variables representing multi-rotor drones include the drone's position and the joint angles of the robotic arm; The above represents the generalized velocity variable of a multi-rotor UAV. This represents the first derivative, including the translational velocity of the drone and the joint angular velocity of the robotic arm; The generalized acceleration variables representing multi-rotor drones include the translational acceleration of the drone and the joint acceleration of the robotic arm; For the control input variables of multi-rotor drones, including drone propeller commands and robotic arm joint torques; The interference received by the multi-rotor UAV includes interference from the center of mass shift on the base, contact force interference from environmental interaction, and interference from model uncertainty. Further results were obtained: ； In the formula, This is the transformation matrix from the body coordinate system to the inertial coordinate system of the multi-rotor UAV; The angular velocity of the multi-rotor drone; This represents the coupling force generated by the swinging of the robotic arm, which is considered as an interfering external force applied to the drone; This indicates the external force interference generated by the interaction between the flying robotic arm and the external environment; This represents the center of mass of the system with the base as the reference origin. The second derivative is represented in the formula; The calculation is as follows: In the formula Indicates the first The quality of each joint This indicates the center of mass of the robotic arm's joints. This indicates the total mass of the multi-rotor drone.

3. The flight robotic arm pose control method for resisting multi-source interference and coordinating motion planning according to claim 1, characterized in that, In step two, the model interference is characterized by the following model: ； In the formula, and These represent the rotation angle and angular velocity of the multi-rotor UAV about its center of mass, respectively. Represents model-free lumped perturbation; the above Denotes the first derivative; This represents the equivalent moment of inertia of a multi-rotor unmanned aerial vehicle (UAV) about its center of mass. denoted by , representing the equivalent Coriolis term of the multi-rotor UAV around its center of mass; h represents the derivative of the lumped perturbation; There is model interference; This indicates no model interference; The specific implementation of the interference observer with known model information is as follows: ； in, It is an auxiliary state variable of the base floating observer. It is the rate of change of the auxiliary state variable. It is the effect of external interference on the system state. The base floating is designed to address lumped interference. The estimated value, The gain of the interference observer to be determined is... It is related to the control quantity The relevant control allocation matrix, , It is the characteristic matrix of the system's state equations. Indicates control input, and It is the parameter matrix of the interference model. This represents the equivalent moment of inertia of the flying robotic arm when its joints are initially configured. This indicates the attitude measurement output of the UAV; , , for , The predicted value; The extended observer is designed to handle model-free disturbances as follows: ； In the formula, This represents the true state value of a multi-rotor drone. This represents the sensor measurement output of a multi-rotor drone. , , These represent the gain matrices of the observer, Indicates and The relevant control quantities of drones.

4. The flight robotic arm pose control method for resisting multi-source interference and coordinating motion planning according to claim 1, characterized in that, Step four includes: Coordinated motion planning for UAV and robotic arm based on nonlinear model predictive control method: ； The constraints are satisfied: ； ； ； In the formula, Indicates to The minimum optimization function; This represents the cost function to be optimized. , and These represent the predicted system output state, the expected output, and the increment of the control input, respectively. The first nonlinear model predictive controller represents the... step; Representing vectors The second norm; It predicts the step size; It controls the step size; and These represent the weight matrices for tracking error and control energy, respectively. and These represent the lower and upper limits of the control input, respectively; and Indicates the lower and upper limits of the control input increment; It is a positive number to ensure closed-loop stability; Indicates the index value.

Images