An integrated coordination planning method for flying manipulators for target capture

By establishing a motion coordinate system and using a model prediction controller to optimize the motion state of the drone and robotic arms, the problem of insufficient consideration of the dynamic characteristics of the flight robotic arms in the prior art is solved, and the high level of the flight robotic arms in the target capture task is achieved Accuracy tracking and system stability.

CN118617424BActive Publication Date: 2024-11-05HANGZHOU INNOVATION RES INST OF BEIJING UNIV OF AERONAUTICS & ASTRONAUTICS
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411104450.5
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

Technical Problem

The prior art is difficult to realize integrated coordinated planning of the flight robot while taking into account the dynamic characteristics of the flight robot arm, and it is impossible to effectively complete complex interactive tasks such as target capture.

Method used

Adopting an integrated coordinated planning method for target capture, the motion state of the drone and the robotic arm is optimized to meet the physics by establishing a motion coordinate system, constructing kinematic equations, designing optimization functions and using model prediction controllers. Constraints for high-precision capture.

Benefits of technology

The high-precision tracking and system stability of the flight robot arm in the target capture task is achieved, ensuring the accurate completion and safety of active operation tasks.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118617424B_ABST
    Figure CN118617424B_ABST
Patent Text Reader

Abstract

The present invention provides an integrated coordination planning method for a flying manipulator for target capture, with the goal of integrated coordination and optimization of the motion trajectory of a UAV and a manipulator under multiple constraints. First, a corresponding motion coordinate system is established according to the actual mechanical structure of the flying manipulator, and the kinematic equation of the terminal position of the flying manipulator arm in the inertial coordinate system is constructed using the principle of homogeneous transformation. Secondly, under the premise of considering the flight attitude of the UAV, a characterization equation of the tracking error of the terminal position of the flying manipulator arm and the motion state of the UAV and the motion state of the terminal of the manipulator arm is constructed; in view of the motion characteristics of the flying manipulator arm and the actual safety constraints when performing the target capture task, an optimization function with the system motion state as a variable is designed, and a corresponding cost function is constructed to constrain each state of the system; finally, a model predictive controller is used to optimize the expected acceleration of the UAV and the terminal of the manipulator arm actuator.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the field of integrated motion planning of flying robots, and specifically relates to an integrated coordination planning method for a flying manipulator for target capture, which is suitable for a flying manipulator system equipped with multi-degree-of-freedom serial manipulators and having complex dynamic interaction capabilities. Background Art

[0002] In recent years, small drones with multi-rotor as driving mechanism 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 new type of flying robot composed of multi-rotor drones and robotic arms that can physically interact with the environment has become a very hot field. The flying robot arm has the ability to interact with the environment, and its application has been extended to various active operation tasks, such as aerial grasping, equipment assembly, and facility inspection and maintenance. However, when the flying robot arm performs complex dynamic interactive operation tasks such as target capture, the system state is often subject to multiple constraints, such as: system acceleration, speed, position constraints, etc. Adding various constraints can effectively prevent the system state from exceeding the physical limit or safety boundary of the system.

[0003] In the prior art inventions, for the coordinated planning of the flying manipulator, the Chinese invention patent CN 202310451834.3 designed a drone controller and a manipulator joint controller respectively, and designed a disturbance observer at each joint of the manipulator to improve the control accuracy. The Chinese invention patent CN201810010602.3 proposed an arm-carrying drone equipped with a parallel manipulator, and a parallel manipulator was provided under the rotorcraft to improve the control accuracy of the actuator end, but the device proposed therein had a small working space, and the actuator could only work under the rotorcraft; the rotor flight grasping manipulator proposed in the Chinese invention patent CN202123017099.0 also had a small working space and could not complete the active operation task.

[0004] None of the above methods can achieve integrated coordinated planning of the flying manipulator while considering the dynamic characteristics of the flying manipulator to complete complex interactive tasks such as target capture. 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 integrated coordination planning method for a flying robotic arm for target capture, which can not only take into account the posture information of the UAV platform but also meet various actual physical constraints from the planning level and maximize the stability of the system to complete various active operation tasks such as target capture.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is:

[0007] A method for integrated coordination planning of a flying manipulator for target capture comprises the following steps:

[0008] The first step is to establish the corresponding motion coordinate system according to the actual mechanical structure of the flight robot arm, and use the principle of kinematic homogeneous transformation to construct the kinematic equation of the end position of the flight robot arm actuator in the inertial coordinate system;

[0009] The second step is to construct the characterization equation of the tracking error of the end position of the flight manipulator arm and the motion state of the UAV and the end motion state of the manipulator arm under the premise of considering the flight attitude of the UAV;

[0010] The third step is to design an optimization function with the motion state of the flight manipulator as a variable according to the motion characteristics of the flight manipulator and the actual safety constraints when performing the target capture task, and construct a corresponding cost function to constrain each state of the flight manipulator;

[0011] The fourth step is to use the model predictive controller to plan the expected acceleration of the UAV and the end of the robotic arm actuator, and obtain the expected speed and position of the UAV and the end of the robotic arm through integration to complete the high-precision capture task of the flying robotic arm.

[0012] Furthermore, the first step specifically includes the following steps:

[0013] according to Figure 2 The structure diagram of the flight manipulator system shown in the figure establishes the corresponding motion coordinate system, which are the inertial coordinate system , UAV body coordinate system , Robot arm base coordinate system , the coordinate system attached to the end of the flight robot arm actuator .exist and There is only a fixed position offset between them.

[0014] Then, the kinematic model of the end position of the flight robot arm actuator in the inertial coordinate system is established by using the principle of kinematic homogeneous transformation, which is expressed as follows:

[0015] In the formula, Indicates the position of the end of the flight robot arm actuator in the inertial system, is the rotation matrix of the end of the flight robot arm actuator in the inertial system; Indicates the position of the center of mass of the drone base in the inertial system, is the rotation matrix of the center of mass of the drone in the inertial system; Indicates that the end of the robot arm actuator is in the robot arm base coordinate system. Represents the rotation matrix of the end of the robot arm actuator in the robot arm base coordinate system; Indicates the constant deviation between the robot base coordinate system and the drone center of mass coordinate system, Represents the rotation matrix of the robot arm base in the drone's center of mass coordinate system. is the identity matrix. The calculation formula is as follows:

[0016] In the formula, They respectively represent the roll angle, pitch angle and attitude angle of the flying robot arm in the inertial system.

[0017] Furthermore, the second step includes the following specific steps:

[0018] According to the kinematic equations established above, under the premise of considering the flight attitude of the UAV, the characterization equation of the tracking error of the end position of the flight manipulator arm and the motion state of the UAV and the end motion state of the manipulator arm is constructed. It is expressed as follows:

[0019] In the formula, Indicates the end of the flight robot arm actuator The desired position in the inertial coordinate system at all times, that is, the position of the captured target; Indicates the end of the flight robot arm actuator The actual position in the inertial coordinate system at all times; Represents the center of mass of the drone The actual position in the inertial coordinate system at all times; Indicates the end of the robot arm actuator The actual position of the robot base coordinate system at all times. The rotation matrix in the inertial system at all times The propagation equation can be expressed as:

[0020] In the formula, is the sampling interval. is an antisymmetric matrix operator that performs a cross product.

[0021] Furthermore, the specific steps of the third step are as follows:

[0022] make As the controlled state variable of the flight manipulator, its kinematic state space modeling is performed as follows:

[0023] in, Indicates that the flight robot arm is controlled by the state variable 12-dimensional output at the moment; Indicates that the flight robot arm is controlled by the state variable 12-dimensional output at the moment; The center of mass of the UAV is represented by the optimized control variable The acceleration in the inertial system and the end of the robotic arm actuator The acceleration in the robot base coordinate system at all times; Indicates that the state space is Output at the moment; Represents the position output of the drone's center of mass, Represents the velocity output of the drone's center of mass, Represents the position output of the end of the robot arm, Represents the velocity output of the end of the robot arm.

[0024] System Matrix , input matrix And the output matrix The values ​​of are as follows:

[0025] In the formula, express The identity matrix of express The zero matrix of ;

[0026] Furthermore, the specific steps of the fourth step are as follows:

[0027] Design the cost function of the model predictive controller, consider multiple constraints such as output oscillation avoidance, control signal smoothness, state constraints and actuator output limiting, and construct the following optimal control problem solution :

[0028] In the formula, represents the total cost function, represents the input vector of model predictive control; They are the tracking error cost function, the output oscillation cost function and the control input cost function; Represents the function The minimum value of For function The independent variable;

[0029] Tracking error cost function of the end effector of a flying robot arm It is expressed as:

[0030] In the formula, The state tracking error The error vector composed of is the designed control domain length; It represents the 2-norm of a vector; is the weight matrix of the tracking error cost function; the tracking error of the end actuator of the flight manipulator can be represented by the state space output in the following form:

[0031] In the formula, is the current control moment; is the step size of forward prediction;

[0032] Output oscillation cost function of the end control of the flight robot arm actuator It is expressed as:

[0033] In the formula, Incremental vector representing the position output of the flight robot arm actuator end. is the weight matrix of the output oscillating cost function.

[0034] Cost function of control input of the end effector of the flight manipulator It is expressed as:

[0035] In the formula, represents the model predictive controller optimization variable The smoothness of the control action is the main factor in protecting the actuator of the flight robot arm. By adding the control input cost function It can smooth the control input of the system and reduce the drastic changes in the acceleration of the flying robot arm; is the weight matrix that controls the input cost function and is an adjustable parameter. The motion state of the drone and the robotic arm can be adjusted.

[0036] In order to ensure the safety of the flying manipulator when performing tasks, the motion state of the flying manipulator must satisfy the dynamic constraints during the entire control cycle; it is expressed as follows:

[0037] In the formula, and Indicates the speed limit of the UAV platform; and Indicates the limit of the end speed of the robot arm actuator.

[0038] In order to ensure the stability of the system when performing the target capture task, the control input planned in the entire control cycle should never exceed the input limit of the flight manipulator actuator; it is expressed as follows:

[0039] In the formula, and Represents the acceleration limit of the UAV platform and the end of the robotic arm.

[0040] Solve the above integrated planning problem to obtain the expected acceleration of the center of mass of the UAV and the expected acceleration of the end of the robotic arm actuator relative to the base coordinate system. By integrating the expected acceleration, the expected speed and expected position of the UAV and the robotic arm are obtained.

[0041] The advantages of the present invention compared with the prior art are:

[0042] The invention relates to an integrated coordination planning method for a flying manipulator for target capture, which is mainly aimed at a flying manipulator system composed of a multi-rotor drone and a multi-degree-of-freedom manipulator. Compared with the traditional fixed-base manipulator, the flying manipulator has the advantages of a wider range of action and stronger air interaction capabilities. However, the mechanical structure of the flying manipulator is complex.

[0043] It is difficult for the prior art inventions to coordinate the planning of the flying manipulator while considering the motion characteristics of the UAV and the manipulator. The present invention fully considers the motion characteristics of the flying manipulator and designs a model predictive controller to coordinate and optimize the acceleration of the UAV and the end of the manipulator actuator in an integrated manner. In the optimization solution process, the state constraints of the system, such as the upper and lower bounds of position, velocity, and acceleration, are fully considered. In addition, the method also constructs an output oscillation cost function and a control input cost function, which significantly improves the stability of the system when performing tasks while achieving accurate tracking of the target by the end of the aircraft actuator, ensuring the accurate completion of active operation tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 A design flow chart of a method for integrated coordination planning of a flying manipulator for target capture according to the present invention;

[0045] Figure 2 This is the structural diagram of the flight robotic arm system. DETAILED DESCRIPTION

[0046] 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.

[0047] 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 complex active interactive tasks, it has very high requirements on the motion trajectory of the flying robot arm.

[0048] like Figure 1 As shown, the specific implementation steps of the integrated coordination planning method of the flight manipulator for target capture of the present invention are as follows:

[0049] The first step is to Figure 2 The structure diagram of the flight manipulator system shown in the figure establishes the corresponding motion coordinate system, which are the inertial coordinate system , UAV body coordinate system , Robot arm base coordinate system , the coordinate system attached to the end of the flight robot arm actuator .exist and There is only a fixed position offset between them.

[0050] Then, the kinematic model of the end position of the flight robot arm actuator in the inertial coordinate system is established by using the principle of kinematic homogeneous transformation, which is expressed as follows:

[0051] In the formula, Indicates the position of the end of the flight robot arm actuator in the inertial system, is the rotation matrix of the end of the flight robot arm actuator in the inertial system; Indicates the position of the center of mass of the drone base in the inertial system, is the rotation matrix of the center of mass of the drone in the inertial system; Indicates that the end of the robot arm actuator is in the robot arm base coordinate system. Indicates the constant deviation between the robot base coordinate system and the drone center of mass coordinate system. Represents the rotation matrix of the end of the robot arm actuator in the robot arm base coordinate system; Indicates the constant deviation between the robot base coordinate system and the drone center of mass coordinate system , Represents the rotation matrix of the robot arm base in the drone's center of mass coordinate system. is the identity matrix. The calculation formula is as follows:

[0052] In the formula, They respectively represent the roll angle, pitch angle and attitude angle of the flying robot arm in the inertial system.

[0053] The second step is to construct the characterization equation of the tracking error of the end position of the flight manipulator and the motion state of the UAV and the end motion state of the manipulator based on the kinematic equation established above and taking into account the flight attitude of the UAV. It is expressed as follows:

[0054] In the formula, Indicates the end of the flight robot arm actuator The desired position in the inertial coordinate system at all times, that is, the position of the captured target; Indicates the end of the flight robot arm actuator The actual position in the inertial coordinate system at all times; Represents the center of mass of the drone The actual position in the inertial coordinate system at all times; Indicates the end of the robot arm actuator The actual position of the robot base coordinate system at all times. The rotation matrix in the inertial system at all times The propagation equation can be expressed as:

[0055] In the formula, is the sampling interval. is an antisymmetric matrix operator that performs a cross product.

[0056] The third step is to As the controlled state variable of the flight manipulator, its kinematic state space modeling is performed as follows:

[0057] in, Indicates that the flight robot arm is controlled by the state variable 12-dimensional output at the moment; Indicates that the flight robot arm is controlled by the state variable 12-dimensional output at the moment; The center of mass of the UAV is represented by the optimized control variable The acceleration in the inertial system and the end of the robotic arm actuator The acceleration in the robot base coordinate system at all times; Indicates that the state space is Output at the moment; Represents the position output of the drone's center of mass, represents the velocity output of the drone's center of mass, Represents the position output of the end of the robot arm, Represents the velocity output of the end of the robot arm.

[0058] System Matrix , input matrix And the output matrix The values ​​of are as follows:

[0059] In the formula, express The identity matrix of express The zero matrix of .

[0060] The fourth step is to design the cost function of the model predictive controller, taking into account various constraints such as output oscillation avoidance, control signal smoothness, state constraints, and actuator output limiting, and construct the following optimal control problem solution: :

[0061] In the formula, represents the total cost function, represents the input vector of model predictive control; They are the tracking error cost function, the output oscillation cost function and the control input cost function; Represents the function The minimum value of For function The independent variable;

[0062] Tracking error cost function of the end effector of a flying robot arm It is expressed as:

[0063] In the formula, The state tracking error The error vector composed of is the designed control domain length; It represents the 2-norm of a vector; is the weight matrix of the tracking error cost function; the tracking error of the end actuator of the flight manipulator can be represented by the state space output in the following form:

[0064] In the formula, is the current control moment; is the step size of forward prediction;

[0065] Output oscillation cost function of the end control of the flight robot arm actuator It is expressed as:

[0066] In the formula, Incremental vector representing the position output of the flight robot arm actuator end. is the weight matrix of the output oscillating cost function.

[0067] Cost function of control input of the end effector of the flight manipulator It is expressed as:

[0068] In the formula, represents the model predictive controller optimization variable The smoothness of the control action is the main factor in protecting the actuator of the flight robot arm. By adding the control input cost function It can smooth the control input of the system and reduce the drastic changes in the acceleration of the flying robot arm; is the weight matrix that controls the input cost function and is an adjustable parameter. The motion state of the drone and the robotic arm can be adjusted.

[0069] In order to ensure the safety of the flying manipulator when performing tasks, the motion state of the flying manipulator must satisfy the dynamic constraints during the entire control cycle; it is expressed as follows:

[0070] In the formula, and Indicates the speed limit of the UAV platform; and Indicates the limit of the end speed of the robot arm actuator.

[0071] In order to ensure the stability of the system when performing the target capture task, the control input planned in the entire control cycle should never exceed the input limit of the flight manipulator actuator; it is expressed as follows:

[0072] In the formula, in the formula, and Represents the acceleration limit of the UAV platform and the end of the robotic arm.

[0073] Solve the above integrated planning problem to obtain the expected acceleration of the center of mass of the UAV and the expected acceleration of the end of the robotic arm actuator relative to the base coordinate system. By integrating the expected acceleration, the expected speed and expected position of the UAV and the robotic arm are obtained.

[0074] 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 integrated coordination planning of a flying manipulator for target capture, characterized in that: The following steps are involved: S1: Establish the corresponding motion coordinate system according to the actual mechanical structure of the flight robot arm, and use the principle of kinematic homogeneous transformation to construct the kinematic equation of the end position of the flight robot arm actuator in the inertial coordinate system; S2: Considering the flight attitude of the UAV, construct the characterization equation of the tracking error of the end position of the flight manipulator arm and the motion state of the UAV and the motion state of the end position of the manipulator arm; S3: According to the motion characteristics of the flight manipulator and the actual safety constraints when performing the target capture task, an optimization function with the motion state of the flight manipulator as a variable is designed, and a corresponding cost function is constructed to constrain each state of the flight manipulator; S4: Use the model predictive controller to plan the expected acceleration of the drone and the end of the robotic arm actuator, and obtain the expected speed and position of the drone and the end of the robotic arm through integration to complete the high-precision capture task of the flying robotic arm; The step S1 is specifically as follows: According to the flight manipulator system structure diagram, the corresponding motion coordinate systems are established, namely, the inertial coordinate system , UAV body coordinate system , Robot arm base coordinate system , the coordinate system attached to the end of the flight robot arm actuator ;exist and There is only a fixed position offset between them; The kinematic model of the end position of the flight robot arm actuator in the inertial coordinate system is established by using the principle of kinematic homogeneous transformation, which is expressed as follows: In the formula, Indicates the position of the end of the flight robot arm actuator in the inertial system, is the rotation matrix of the end of the flight robot arm actuator in the inertial system; Indicates the position of the center of mass of the drone base in the inertial system, is the rotation matrix of the center of mass of the drone in the inertial system; Indicates that the end of the robot arm actuator is in the robot arm base coordinate system; Represents the rotation matrix of the end of the robot arm actuator in the robot arm base coordinate system; Indicates the constant deviation between the robot base coordinate system and the drone center of mass coordinate system, Represents the rotation matrix of the robot arm base in the drone's center of mass coordinate system; is the identity matrix; The calculation formula is as follows: In the formula, They respectively represent the roll angle, pitch angle and attitude angle of the flying manipulator in the inertial system; The step S2 is specifically as follows: According to the kinematic equation established above, under the premise of considering the flight attitude of the UAV, the characterization equation of the tracking error of the end position of the flight manipulator arm and the motion state of the UAV and the end motion state of the manipulator arm is constructed, which is expressed as follows: In the formula, Indicates the end of the flight robot arm actuator The desired position in the inertial coordinate system at all times, that is, the position of the captured target; Indicates the end of the flight robot arm actuator The actual position in the inertial coordinate system at all times; Represents the center of mass of the drone The actual position in the inertial coordinate system at all times; Indicates the end of the robot arm actuator The actual position in the robot base coordinate system at all times; Drone center of mass The rotation matrix in the inertial system at all times The propagation equation can be expressed as: In the formula, is the sampling interval; is an antisymmetric matrix operator that performs a cross product operation; The step S3 is specifically as follows: make As the controlled state variable of the flight manipulator, its kinematic state space modeling is performed as follows: in, Indicates that the flight robot arm is controlled by the state variable 12-dimensional output at the moment; Indicates that the flight robot arm is controlled by the state variable 12-dimensional output at the moment; The center of mass of the UAV is represented by the optimized control variable The acceleration in the inertial system and the end of the robotic arm actuator The acceleration in the robot base coordinate system at all times; Indicates that the state space is Output at the moment; Represents the position output of the drone's center of mass, represents the velocity output of the drone's center of mass, Represents the position output of the end of the robot arm, Represents the speed output at the end of the robot arm; System Matrix , input matrix And the output matrix The values ​​of are as follows: In the formula, express The identity matrix of express The zero matrix of ; The step S4 is specifically as follows: The cost function of the model predictive controller is further designed, and the system output oscillation, control signal smoothness, state constraints and actuator output limiting constraints are considered to construct the following optimal control problem solution : In the formula, represents the total cost function, represents the input vector of model predictive control; , , They are the tracking error cost function, the output oscillation cost function and the control input cost function; Represents the function The minimum value of For function The independent variable; Tracking error cost function of the end effector of a flying robot arm It is expressed as: In the formula, The state tracking error The error vector composed of is the designed control domain length; It represents the 2-norm of a vector; is the weight matrix of the tracking error cost function; the tracking error of the end actuator of the flight manipulator can be represented by the state space output in the following form: In the formula, is the current control moment; is the step size of forward prediction; Output oscillation cost function of the end control of the flight robot arm actuator It is expressed as: In the formula, The incremental vector representing the end position output of the flight robot arm actuator, is the weight matrix of the output oscillation cost function; Cost function of control input of the end effector of the flight manipulator It is expressed as: In the formula, represents the model predictive controller optimization variable The incremental vector of the control action; the smoothness of the control action is the main factor in protecting the flight robot arm actuator. By adding the control input cost function It can smooth the control input of the system and reduce the drastic changes in the acceleration of the flying robot arm; is the weight matrix that controls the input cost function and is an adjustable parameter; by adjusting The motion state of the drone and the robotic arm can be adjusted.

2. The integrated coordination planning method for target capture of a flying manipulator according to claim 1, characterized in that: In order to ensure the safety of the flying manipulator when performing tasks, the motion state of the flying manipulator must satisfy the dynamic constraints during the entire control cycle; it is expressed as follows: In the formula, and Indicates the speed limit of the UAV platform; and Indicates the limit of the end speed of the robot arm actuator.

3. The integrated coordination planning method for target capture of a flying manipulator according to claim 2, characterized in that: In order to ensure the stability of the system when performing the target capture task, the control input planned in the entire control cycle should never exceed the input limit of the flight manipulator actuator; it is expressed as follows: In the formula, and Represents the acceleration limits of the UAV platform and the end of the robotic arm; Solve the above integrated planning problem to obtain the expected acceleration of the center of mass of the UAV and the expected acceleration of the end of the robotic arm actuator relative to the base coordinate system. By integrating the expected acceleration, the expected speed and expected position of the UAV and the robotic arm are obtained.