An anti-interference control method for a flying manipulator based on full-state coupled dynamics
Through the anti-interference control method based on full-state coupling dynamics, the dynamic strong coupling interference problem caused by rapid movement of the flight robot arm is solved, and high-precision active operation tasks are achieved, and control accuracy and safety are improved.
Patent Information
- Application Number
- CN202411104478.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-13
- Publication Date
- 2024-11-05
- Estimated Expiration
- 2044-08-13
AI Technical Summary
The prior art cannot effectively solve the problem of dynamic strong coupling interference caused by the flight robot arm during rapid movement, resulting in reduced control accuracy and safety threats, especially in high-precision active operation tasks, which have problems such as small work space and limited operating capabilities.
Using an anti-interference control method based on full-state coupling dynamics, by constructing a full-state coupling dynamics model of the flight robot arm, designing a differential device to obtain full-state information, establishing a residual observer that couples interference forces and torques, and combining cascade Control principle designs the anti-interference controller of the position ring and attitude ring to achieve high-precision motion control.
It significantly improves the control accuracy of the flight robot under dynamically strong coupling interference, and can complete high-precision active operation tasks, such as high-voltage tower maintenance and high-altitude sculpture inspection, improving the safety and operation capabilities of the system.
Smart Images

Figure CN118664604B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of flying robot control, and in particular relates to a flying robot arm anti-interference control method based on full-state coupled dynamics, which is suitable for a flying robot arm control system that needs to perform high-precision active operation tasks. Background Art
[0002] In recent years, small drones with multi-rotors as driving mechanisms have been widely used in disaster warning, geological survey, emergency rescue, defense, electricity, aerial photography and other fields, but most of them are limited to the field of non-contact information perception. At present, with the continuous development of mechanical design, high-precision sensors and advanced control theory, a flying robot composed of a multi-rotor drone and a robotic arm that can physically interact with the environment has become a very popular field. With the increasing complexity of tasks and scenes, higher requirements are placed on the control accuracy of the flying robot arm. However, compared with the traditional bare multi-rotor, the flying robot arm carries a robotic arm that can perform complex interactive tasks. The rapid movement of the robotic arm makes the coupling dynamics between the drone and the robotic arm very complicated, resulting in changes in the center of mass and inertia of the entire flying robot arm system, which seriously affects the control accuracy of the flying robot arm system and even threatens the safety of the system itself. Therefore, in order to ensure the high-precision control performance of the flying robot arm, improve the success rate of active operation tasks, and enable the flying robot arm to complete precise interactive tasks such as high-voltage tower maintenance, the anti-interference control method of the flying robot arm based on full-state coupling dynamics must solve the dynamic strong coupling interference problem caused by the rapid movement of the robot arm mentioned above during the design process.
[0003] Chinese invention patent CN201610389286.6 designed a control algorithm that combines differential geometry with LQR to deal with the interference of a space manipulator, but there are two problems: (1) the established dynamic model does not take gravity into account, and the controller is only applicable in a weightless environment; (2) the manipulator has fewer degrees of freedom, which greatly limits its operational capabilities; Chinese invention patent CN201810010602.3 proposed an arm-carrying drone equipped with a parallel manipulator, and a parallel manipulator is provided under the rotorcraft, but the proposed device has a small working space and can only work under the rotorcraft; the rotor flight grasping manipulator proposed in Chinese invention patent CN202123017099.0 also has a small working space and cannot complete active operation tasks.
[0004] Therefore, none of the above methods considers realizing anti-interference control of the flight manipulator in the presence of dynamic strong coupling interference to complete high-precision active operation tasks. Summary of the invention
[0005] In order to overcome the defects of the existing methods, for a flying robot system composed of a multi-rotor UAV and a multi-degree-of-freedom robotic arm, the present invention provides an anti-interference control method for a flying robotic arm based on full-state coupled dynamics, which can ensure high-precision control of the flying robotic arm under dynamic strong coupling interference to complete various active operation tasks.
[0006] In order to achieve the above object, the technical solution adopted by the present invention is:
[0007] A method for anti-interference control of a flight manipulator based on full-state coupled dynamics comprises the following steps:
[0008] The first step is to construct the kinematic equation of the end position of the flight robot arm actuator in the inertial coordinate system based on the principle of homogeneous transformation;
[0009] The second step is to use the principle of conservation of momentum to construct a coupled dynamics model of the full state of the flying manipulator;
[0010] The third step is to design a differentiator to obtain the full state information of the flight manipulator;
[0011] The fourth step is to design the corresponding coupling disturbance force and coupling disturbance torque residual observers for the estimation errors of the coupling disturbance of the position loop and attitude loop of the UAV platform respectively;
[0012] Step 5: Based on the cascade control principle and the constructed coupling disturbance force residual observer, the position loop anti-interference controller of the UAV platform is established;
[0013] Step 6. Combine the constructed coupling disturbance torque residual observer to establish the attitude loop anti-interference controller to complete the high-precision motion control of the flight manipulator system under strong dynamic coupling interference.
[0014] Furthermore, the first step specifically includes the following steps:
[0015] According to the motion characteristics of the flying robot arm, the kinematic model of the end position of the flying robot arm actuator is established according to the principle of homogeneous transformation, which is expressed as follows: In the formula, Indicates the position of the end of the flight robot arm actuator in the world system. is the rotation matrix of the end of the flight robot arm actuator in the world system; Indicates the position of the center of mass of the drone base in the world system, is the rotation matrix of the drone's center of mass in the world system; Indicates the position of the end of the robotic arm actuator in the drone coordinate system, Represents the rotation matrix of the end of the robotic arm actuator in the drone coordinate system.
[0016] Furthermore, the second step includes the following specific steps:
[0017] According to Newton's second law, the dynamics of the aerial platform considering the coupling effect can be expressed as: In the formula, Represents the velocity of the center of mass of the UAV platform in the world system; is the angular velocity of the UAV platform; is the mass of the flight manipulator; is the local gravitational acceleration; is a unit vector; is the projection vector of the lift of the UAV platform in the inertial system; The lift for the UAV platform; The coupling interference force caused by the movement of the robot arm; is the moment of inertia of the UAV platform; is the moment of the UAV platform; It is the coupling interference torque caused by the movement of the robot arm; is an antisymmetric matrix operator that performs a cross product.
[0018] The dynamic coupling effect caused by the rapid movement of the manipulator consists of two parts: the movement of the manipulator relative to the UAV platform and the movement of the UAV platform itself, which can be expressed as follows: In the formula, , and They represent the position, velocity and acceleration of the center of mass of the manipulator in the coordinate system of the drone body respectively; and They represent the inertia tensor and its differential of the robotic arm relative to the coordinate system of the drone body; and They represent the angular momentum and its differential of the manipulator relative to the coordinate system of the drone body; Represents the angular acceleration of the UAV platform; Represents the mass of the robot arm.
[0019] Furthermore, the specific steps of the third step are as follows:
[0020] A differentiator is designed to obtain the full state information of the flight manipulator, and corresponding residual observers are designed for the estimated errors of the coupled dynamics of the position loop and attitude loop of the UAV platform:
[0021] Construct the following differentiator to obtain and , which is expressed as follows: In the formula, represents the sampling time, Represents the current control moment. Other state quantities required for calculating dynamic coupling interference can be directly measured by sensors, so the differentiator can be used to quantitatively calculate the dynamic coupling interference force and torque caused by the movement of the robot arm.
[0022] Furthermore, the specific steps of the fourth step are as follows:
[0023] Considering the estimation error caused by the uncertainty of inertial parameters, the coupled disturbance can be rewritten as: In the formula, and They represent the disturbance force and disturbance torque directly calculated according to the full-state coupling model. and They represent the residual estimates of the coupled disturbance force and torque, respectively.
[0024] A residual observer of coupled disturbance force and torque is constructed to further improve the control accuracy of the flight robotic arm.
[0025] The residual observer of the coupled disturbance force is expressed as follows: In the formula, Residual The estimated value of is an auxiliary variable, is the estimated gain matrix of the coupling force residual observer.
[0026] The residual observer of the coupled disturbance torque is expressed as follows: In the formula, Residual The estimated value of is an auxiliary variable, is the estimated gain matrix of the coupled torque residual observer.
[0027] Furthermore, the specific steps of the fifth step are as follows:
[0028] To establish the anti-interference controller of the position loop of the UAV platform, we first define the position tracking error , Indicates the expected position of the UAV platform in the inertial system. Define auxiliary variables ,in is the designed stability function, which is expressed as follows: In the formula, represents the expected speed of the UAV platform in the inertial system, is the gain matrix to be designed.
[0029] Combination , , and , the designed UAV platform position loop controller is expressed as follows: In the formula, is the gain matrix to be designed.
[0030] Furthermore, the specific steps of the sixth step are as follows:
[0031] To establish the attitude loop anti-interference controller of the UAV platform, first define the attitude tracking error and angular velocity tracking error , which is expressed as follows: In the formula, represents the desired angular velocity of the UAV platform, represents the desired rotation matrix of the UAV platform, Represents the inverse of the antisymmetric matrix operator.
[0032] Combination and ,The designed UAV platform attitude loop controller is expressed as follows: In the formula, represents the desired angular acceleration of the UAV platform. and Represents the attitude loop control parameters to be designed.
[0033] The advantages of the present invention compared with the prior art are:
[0034] This method aims at the dynamic strong coupling interference caused by the rapid movement of the manipulator. First, the kinematic equation of the end position of the flying manipulator actuator in the inertial coordinate system is constructed according to the principle of homogeneous transformation. Then, the coupled dynamic model of the flying manipulator in the whole state is constructed using the principle of conservation of momentum. Secondly, a robust differentiator is designed to obtain the full state information of the flying manipulator, and the corresponding residual observers are designed for the estimated errors of the coupled dynamics of the position loop and attitude loop of the UAV platform. Finally, the anti-interference controller of the position loop and the anti-interference controller of the attitude loop of the UAV platform are established according to the cascade control principle to complete the high-precision motion control of the flying manipulator system under the strong coupling interference of dynamics. This method can significantly improve the control accuracy of the flying manipulator under the strong coupling interference of dynamics, and can be used for high-precision special operations such as cleaning foreign objects in high-voltage lines, detecting and repairing high-altitude sculptures. BRIEF DESCRIPTION OF THE DRAWINGS
[0035] Figure 1 The present invention discloses an anti-interference control method for a flying manipulator arm based on full-state coupled dynamics. DETAILED DESCRIPTION
[0036] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below 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 invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.
[0037] The present invention takes a general flying robot arm system consisting of a multi-rotor UAV and a multi-degree-of-freedom robot arm as an example to illustrate the specific implementation of the system and method. When the flying robot arm performs high-precision active operation tasks, it has very high requirements on the accuracy of the system and its own safety.
[0038] like Figure 1 As shown, the specific implementation steps of the anti-interference control method of the flight manipulator based on full-state coupled dynamics of the present invention are as follows:
[0039] The first step is to establish the kinematic model of the end position of the flight robot arm actuator according to the principle of homogeneous transformation based on the motion characteristics of the flight robot arm, which is expressed as follows: In the formula, Indicates the position of the end of the flight robot arm actuator in the world system. is the rotation matrix of the end of the flight robot arm actuator in the world system; Indicates the position of the center of mass of the drone base in the world system, is the rotation matrix of the drone's center of mass in the world system; Indicates the position of the end of the robotic arm actuator in the drone coordinate system, Represents the rotation matrix of the end of the robotic arm actuator in the drone coordinate system.
[0040] In the second step, according to Newton's second law, the dynamics of the aerial platform considering the coupling effect can be expressed as: In the formula, Represents the velocity of the center of mass of the UAV platform in the world system; is the angular velocity of the UAV platform; is the mass of the flight manipulator; is the local gravitational acceleration; is a unit vector; is the projection vector of the lift of the UAV platform in the inertial system; The lift for the UAV platform; The coupling interference force caused by the movement of the robot arm; is the moment of inertia of the UAV platform; is the moment of the UAV platform; It is the coupling interference torque caused by the movement of the robot arm; is an antisymmetric matrix operator that performs a cross product.
[0041] The dynamic coupling effect caused by the rapid movement of the manipulator consists of two parts: the movement of the manipulator relative to the UAV platform and the movement of the UAV platform itself, which can be expressed as follows: In the formula, , and They represent the position, velocity and acceleration of the center of mass of the manipulator in the coordinate system of the drone body respectively; and They represent the inertia tensor and its differential of the robotic arm relative to the coordinate system of the drone body; and They represent the angular momentum and its differential of the manipulator relative to the coordinate system of the drone body; Represents the angular acceleration of the UAV platform; Represents the mass of the robot arm.
[0042] The third step is to design a differentiator to obtain the full state information of the flight robot arm. Construct the following differentiator to obtain and It is expressed as follows: In the formula, represents the sampling time, Represents the current control moment. Other state quantities required for calculating dynamic coupling interference can be directly measured by sensors, so the differentiator can be used to quantitatively calculate the dynamic coupling interference force and torque caused by the movement of the robot arm.
[0043] In the fourth step, considering the estimation error caused by the uncertainty of inertial parameters, the coupled disturbance can be rewritten as: In the formula, and They represent the disturbance force and disturbance torque directly calculated according to the full-state coupling model. and They represent the residual estimates of the coupled disturbance force and torque, respectively.
[0044] A residual observer of coupled disturbance force and torque is constructed to further improve the control accuracy of the flight robotic arm.
[0045] The residual observer of the coupled disturbance force is expressed as follows: In the formula, Residual The estimated value of is an auxiliary variable, is the estimated gain matrix of the coupling force residual observer.
[0046] The residual observer of the coupled disturbance torque is expressed as follows: In the formula, Residual The estimated value of is an auxiliary variable, is the estimated gain matrix of the coupled torque residual observer.
[0047] Step 5: Establish the position loop anti-interference controller of the UAV platform. First, define the position tracking error , Indicates the expected position of the UAV platform in the inertial system. Define auxiliary variables ,in is the designed stability function, which is expressed as follows: In the formula, represents the expected speed of the UAV platform in the inertial system, is the gain matrix to be designed.
[0048] Combination , , and , the designed UAV platform position loop controller is expressed as follows: In the formula, is the gain matrix to be designed.
[0049] The sixth step is to establish the attitude loop anti-interference controller of the UAV platform. First, define the attitude tracking error and angular velocity tracking error , which is expressed as follows: In the formula, represents the desired angular velocity of the UAV platform, represents the desired rotation matrix of the UAV platform, Represents the inverse of the antisymmetric matrix operator.
[0050] Combination and ,The designed UAV platform attitude loop controller is expressed as follows: In the formula, represents the desired angular acceleration of the UAV platform. and Represents the attitude loop control parameters to be designed.
[0051] The contents not described in detail in the specification of the present invention belong to the prior art known to the professional and technical personnel in the field and are easily understood by the technical personnel in the field. The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.
Claims
1. A method for anti-interference control of a flight manipulator based on full-state coupled dynamics, characterized in that: The following steps are involved: The first step is to construct the kinematic equation of the end position of the flight robot arm actuator in the inertial coordinate system based on the principle of homogeneous transformation; The second step is to use the principle of conservation of momentum to construct a coupled dynamics model of the full state of the flying manipulator; The third step is to design a differentiator to obtain the full state information of the flight manipulator; The fourth step is to design the corresponding coupling disturbance force and coupling disturbance torque residual observers for the estimation errors of the coupling disturbance of the position loop and attitude loop of the UAV platform respectively; Step 5: Based on the cascade control principle and the constructed coupling disturbance force residual observer, the position loop anti-interference controller of the UAV platform is established; Step 6: Combine the constructed coupling disturbance torque residual observer to establish the attitude loop anti-interference controller to complete the high-precision motion control of the flight manipulator system under the strong coupling disturbance of dynamics; The first step is as follows: According to the motion characteristics of the flight robot arm, the kinematic model of the end position of the flight robot arm actuator is established according to the principle of homogeneous transformation, which is expressed as follows: In the formula, Indicates the position of the end of the flight robot arm actuator in the world system. is the rotation matrix of the end of the flight robot arm actuator in the world system; Indicates the position of the center of mass of the drone base in the world system, is the rotation matrix of the drone's center of mass in the world system; Indicates the position of the end of the robotic arm actuator in the drone coordinate system, Represents the rotation matrix of the end of the manipulator actuator in the drone coordinate system; The second step is as follows: According to Newton's second law, the dynamics of the aerial platform considering the coupling effect can be expressed as: In the formula, Represents the velocity of the center of mass of the UAV platform in the world system; is the angular velocity of the UAV platform; is the mass of the flight manipulator; is the local gravitational acceleration; is a unit vector; is the projection vector of the lift of the UAV platform in the inertial system; The lift for the UAV platform; The coupling interference force caused by the movement of the robot arm; is the moment of inertia of the UAV platform; is the moment of the UAV platform; It is the coupling interference torque caused by the movement of the robot arm; is an antisymmetric matrix operator that performs a cross product operation; The dynamic coupling effect caused by the rapid movement of the manipulator consists of two parts: the movement of the manipulator relative to the UAV platform and the movement of the UAV platform itself, which can be expressed as follows: In the formula, , and They represent the position, velocity and acceleration of the center of mass of the manipulator in the coordinate system of the drone body respectively; and They represent the inertia tensor and its differential of the robotic arm relative to the coordinate system of the drone body; and They represent the angular momentum and its differential of the manipulator relative to the coordinate system of the drone body; Represents the angular acceleration of the UAV platform; Indicates the mass of the robot arm.
2. The anti-interference control method of a flight manipulator based on full-state coupled dynamics according to claim 1, characterized in that: The third step is as follows: A differentiator is designed to obtain the full state information of the flight manipulator, and corresponding residual observers are designed for the estimated errors of the coupled dynamics of the position loop and attitude loop of the UAV platform: Construct the following differentiator to obtain and , which is expressed as follows: In the formula, represents the sampling time, Represents the current control moment.
3. The anti-interference control method of a flight manipulator based on full-state coupled dynamics according to claim 2, characterized in that: The fourth step is as follows: Considering the estimation error caused by the uncertainty of inertial parameters, the coupled disturbance is rewritten as: In the formula, and denote the disturbance force and disturbance torque directly calculated according to the full-state coupling model, and denote the residual estimates of the coupling disturbance force and torque, respectively; The residual observer of the coupled disturbance force is expressed as follows: In the formula, Residual The estimated value of is an auxiliary variable, is the estimated gain matrix of the coupling force residual observer; The residual observer of the coupled disturbance torque is expressed as follows: In the formula, Residual The estimated value of is an auxiliary variable, is the estimated gain matrix of the coupled torque residual observer.
4. The anti-interference control method of a flight manipulator based on full-state coupled dynamics according to claim 3, characterized in that: The fifth step is as follows: To establish the anti-interference controller of the position loop of the UAV platform, we first define the position tracking error , Indicates the expected position of the UAV platform in the inertial system and defines auxiliary variables ,in is the designed stability function, which is expressed as follows: In the formula, represents the expected speed of the UAV platform in the inertial system, is the gain matrix to be designed; Combination , , and , the designed UAV platform position loop controller is expressed as follows: In the formula, is the gain matrix to be designed.
5. The anti-interference control method of a flight manipulator based on full-state coupled dynamics according to claim 4, characterized in that: The sixth step is as follows: To establish the attitude loop anti-interference controller of the UAV platform, first define the attitude tracking error and angular velocity tracking error , which is expressed as follows: In the formula, represents the desired angular velocity of the UAV platform, represents the desired rotation matrix of the UAV platform, represents the inverse operation of the antisymmetric matrix operator; Combination and ,The designed UAV platform attitude loop controller is expressed as follows: In the formula, represents the desired angular acceleration of the UAV platform, and Represents the attitude loop control parameters to be designed.
Citation Information
Patent Citations
Differential geometry nonlinear control method of spatial mechanical arm anti-interference attitude stabilization
CN105912007A
Unmanned aerial vehicle having arm and capable of automatically fetching objects
CN108298084A
Rotor wing flying grabbing mechanical arm
CN216328358U
Angular momentum control method of spacecraft in inertial system
CN104290925A
Aerial work robot
CN107891975A
