A composite layered anti-interference method for a rotor flight manipulator

By designing a composite layered anti-interference method, using differential flatness, synovial term, adaptive control and H∞ controller technologies, the problem of control coupling between drones and robotic arms in rotor flight robotic arms is solved, and the robotic arms are improved Control accuracy and system stability of the end effector.

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

Patent Information

Application Number
CN202211164762.6
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-09-23
Publication Date
2024-11-15
Estimated Expiration
2042-09-23

AI Technical Summary

Technical Problem

The existing rotor flight robot arm control method only focuses on the control accuracy of the drone, ignoring the control accuracy of the end effector of the robot arm, resulting in the system being unable to achieve high-precision control, especially in the grabbing task, the interference of the floating base cannot be ignored.

Method used

The differential flat method is adopted, synovial term, adaptive control term, linear H∞ controller and kinematic controller based on potential energy function, to handle dynamic coupling interference between the drone and the robotic arm respectively, and design composite layered anti-interference Method: decouple the control of the drone and the robot arm to improve the control accuracy of the end effector of the robot arm.

Benefits of technology

It effectively reduces the impact of drone base floating on the control accuracy of the end of the robot arm, improves the control accuracy of the end of the robot arm, and maintains the stability of the entire system.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115366109B_ABST
    Figure CN115366109B_ABST
Patent Text Reader

Abstract

The present invention relates to a composite layered anti-interference method for a rotorcraft flight manipulator. When the rotorcraft flight manipulator performs aerial operation tasks, it has strong internal dynamic coupling and center of mass offset, as well as its own model uncertainty and base floating. In view of the influence of multi-source interference, the present invention proposes three sub-controllers. In terms of UAV control, it includes a robust H∞ position controller for processing the dynamic coupling force applied to the UAV position loop. A composite adaptive anti-interference controller is used to ensure the control accuracy of the UAV attitude loop. In terms of manipulator control, a potential field-based kinematic controller is used to suppress base floating. The present invention can ensure that the UAV can accurately grasp the target object in a hovering state.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the field of aircraft attitude control, and in particular relates to a composite layered anti-interference method for a rotor flight mechanical arm. Background Art

[0002] In recent years, rotorcraft as a special type of UAV has attracted wide attention from academia and industry. Driven by technological development and many application possibilities, the development of rotorcraft has experienced exponential growth. Ordinary multi-rotor UAVs are widely used in environmental monitoring, traffic monitoring, agricultural plant protection and aerial photography. As a specific scientific research topic, rotorcraft has also become one of the main research areas of most robotics conferences.

[0003] The Chinese invention patent "A flying manipulator based on sliding mode PID control" (publication number: CN108279562 A) discloses a flying manipulator based on sliding film PID control. This control method improves the poor anti-interference and robustness in traditional PID control, as well as the jitter problem in sliding film control. However, its control method only focuses on the control accuracy of the UAV and ignores the control accuracy of the end effector of the manipulator. Since the rotor flight manipulator is a highly coupled system, focusing only on the control of the UAV cannot achieve high-precision control of the entire system.

[0004] The Chinese invention patent "Adaptive RBFNNs Noise Measurement Self-Disturbance Rejection Control Method for Rotary-wing Flight Manipulator" (Publication No.: CN108398885A) discloses an adaptive RBFNNs noise measurement self-disturbance rejection control method for a rotary-wing flight manipulator. This method uses adaptive RBFNNs to estimate and compensate for the internal and external interference of each channel of the flight manipulator in real time. On this basis, an attitude controller based on RBFNNs\ADRC is designed, which can effectively compensate for the interference of the manipulator movement on the flight platform. However, it only considers the interference caused by the manipulator movement to the drone, but in actual grasping tasks, the control accuracy of the manipulator end effector should be considered more, so the floating base interference caused by the drone to the manipulator cannot be ignored. Summary of the invention

[0005] In view of the above problems existing in the prior art, the present invention provides a composite layered anti-interference method for a rotorcraft flight robotic arm, which decouples the control of the UAV and the robotic arm and regards them as two independent individuals. Therefore, the dynamic coupling between them is regarded as the interference imposed on the UAV and the robotic arm respectively. On this basis, three sub-controllers are designed, which can effectively solve the interference such as floating base and center of mass offset existing in the system, and realize high-precision control of the rotorcraft flight robotic arm system.

[0006] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0007] A composite layered anti-interference method for a rotorcraft flying robot arm is proposed. The UAV and the robot arm are decoupled and controlled. The UAV and the robot arm are regarded as two independent entities, and the dynamic coupling between them is regarded as the interference imposed on the UAV and the robot arm respectively. The differential flat method is adopted, and the desired lift F provided by the motor is d and the desired rotation matrix R d Calculate the Euler angle expected by inorganic people Setting up the Disturbance Observer Quickly compensate for disturbances imposed on the drone's rotational motion; set attitude speed Based on this, a sliding film term s is set to deal with the interference suppression problem on the rotational motion, and an adaptive control term λ is set to deal with bounded interference, and the overall control input τ of the UAV attitude loop is set; in the position loop of the UAV, due to the boundedness of the error, a linear H is set ∞ The controller is used to deal with the force disturbance acting on the UAV and the interaction effect between the position loop and the attitude loop. In the control of the robotic arm, a kinematic controller based on the potential energy function is set up to ensure that during the grasping task, the end effector of the aerial manipulator accurately tracks the given position or trajectory instructions, while dealing with the base floating problem.

[0008] Furthermore, a virtual control input v is set for the linear H∞ controller of the drone position loop:

[0009]

[0010] In the formula, the gain matrix K = WX -1 , where X = P -1 , W is obtained by solving the following linear matrix inequality according to Schur's complement lemma:

[0011]

[0012] In the formula Where 0 represents the zero matrix, I n Represents the nxn identity matrix. C=I 6 , X=P -1 , where P is a selected positive definite symmetric matrix, χ is a positive constant, and W is the matrix to be solved.

[0013] In the attitude loop of the UAV, set an overall control input τ:

[0014]

[0015] K s , K a represents the gain, represents the inertia matrix, represents the centripetal moment and the Coriolis moment, represents the Euler angle of the body, represents the desired attitude angular velocity, for The first derivative of .

[0016] The formula includes a sliding film term s, an adaptive control term λ and a disturbance observer

[0017]

[0018] Where α is the gain, represents the attitude angle tracking error, for e a The derivative of

[0019]

[0020] In the formula, ||·|| represents the bi-norm, ρ is a positive constant, Represents the torque interference boundary b 2 Estimates.

[0021]

[0022] In the formula, represents the auxiliary vector, L represents the positive definite gain matrix to be designed, The auxiliary equations for the design satisfy

[0023] In the control of the robotic arm, a kinematic controller based on potential energy function is set to suppress the floating base and provide the reference angular velocity of each joint of the robotic arm:

[0024]

[0025] T 2 represents the joint Jacobian matrix of the design, Indicates T 2 The pseudo-reversal, Indicates T 2 is the transpose of . Δε is the gradient of the designed potential energy function, b is the interference boundary of the UAV to the robotic arm 4 The estimate of sat(aΔx e ) is the designed saturation function.

[0026] Furthermore, a Lyapunov function is constructed to prove the stability of the control error convergence of the end effector of the designed controller:

[0027]

[0028] Among them, P(Δx e ) is the designed potential energy function, κ b For gain, for b 4 The estimation error, b s represents a positive constant value, is the angular velocity tracking error of the robot joint.

[0029] Beneficial effects:

[0030] The present invention can effectively reduce the influence of the floating of the UAV base on the control accuracy of the end of the mechanical arm, improve the control accuracy of the end of the mechanical arm, and maintain the stability of the entire system. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 It is a schematic diagram of the appearance model of the rotor flight manipulator involved in the present invention. Among them: B = [XB YB ZB] is the body coordinate system, E = [XE YE ZE] is the manipulator end effector coordinate system, and W = [XW YW ZW] is the world coordinate system.

[0032] Figure 2 It is a flow chart of the composite layered anti-interference method of the rotor flight mechanical arm of the present invention. DETAILED DESCRIPTION

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

[0034] Figure 1 The coordinate system of the rotor flight manipulator is shown. The whole system establishes three coordinate systems: the world coordinate system∑ W , body coordinate system∑ B , robot end effector coordinate system∑ E . represents the Euler angle expected by the drone, F d represents the expected lift, R d Represents the desired rotation matrix from the world coordinate system to the body coordinate system.

[0035] In order to generate a smooth trajectory that can be achieved by an underactuated quadrotor, the present invention adopts a differential flattening method, and the desired lift F provided by the motor is d and the desired rotation matrix R d (From the world coordinate system to the body coordinate system) Calculate the Euler angle expected by the inorganic person Designing a disturbance observer The interference imposed on the rotational motion of the UAV is quickly compensated. In order to deal with the interference suppression problem on the rotational motion, the present invention proposes a reference attitude velocity Based on this, a sliding film term s is designed. Furthermore, in order to deal with bounded disturbances, an adaptive control term λ is proposed. Therefore, the overall control input τ of the UAV attitude loop is designed. In the position loop of the UAV, due to the boundedness of the error, a linear H is proposed ∞ The controller handles the force disturbances acting on the drone and the interaction effects between the position loop and the attitude loop.

[0036] In terms of the control of the manipulator, in order to ensure that the end effector of the aerial manipulator can accurately track the given position or trajectory command during the grasping task, and considering the floating base interference of the rotor flight manipulator system, a kinematic controller based on potential energy function is designed to deal with the base floating problem. The designed controllers have proved the stability of the system by constructing the Lyapunov method.

[0037] Figure 2 The flowchart of the whole control method is shown. Specifically, a composite layered anti-interference method of a rotor flight mechanical arm of the present invention comprises the following steps:

[0038] Step 1: Design the expected Euler angle The z-axis of the expected body coordinate system can be expressed as:

[0039] Among them, ||·|| represents the two-norm;

[0040] By using a Euler angle with the drone's desired The associated intermediate unit vector Get the representation of the other two axes:

[0041]

[0042] Therefore, the Euler angle expected by the drone can be obtained:

[0043] φ d =arctan(r 32 / r 33 ),θ d = -arcsin(r 31 );here It is a containment relationship.

[0044] Among them, r ij Represents R d (i,j) th elements. Among them, i and j are constants, representing the i row and j column of the matrix.

[0045] Step 2: Design disturbance observer:

[0046]

[0047] In the formula, Indicates the torque disturbance d m The estimate, represents an auxiliary variable, L represents a positive definite observer constant gain matrix, Represents an auxiliary function, described as follows:

[0048]

[0049] in, represents the Euler angle of the body, is the Euler angular velocity, is the Euler angular acceleration, represents the inertia matrix;

[0050] Step 3: Design synovial items:

[0051]

[0052] In the formula, is the reference attitude velocity, described as follows:

[0053]

[0054] In the formula, is the Euler angle expected by the drone The derivative of is the attitude tracking error, and α is a positive gain.

[0055] Step 4: Design the adaptive control term λ:

[0056]

[0057] In the formula, ρ represents a positive gain, represents a bounded estimate of the disturbance, which is updated in real time in the controller:

[0058]

[0059] Where κ represents a positive gain.

[0060] Step 5: Design the overall control input τ of the UAV attitude loop:

[0061]

[0062] In the formula, K s and K a represents a positive definite matrix.

[0063] Bring the overall control input of the system into the Euler-Lagrangian description of the attitude loop of the UAV system:

[0064]

[0065] Among them, d a The torque disturbance caused by the robot arm, d m The interference caused by model uncertainty.

[0066] You can get:

[0067]

[0068] in is the derivative of the synovial term s;

[0069] In order to prove the stability of the attitude loop, the Lyapunov function can be designed as follows:

[0070]

[0071] Where Γ represents a positive definite matrix, defined as Γ = L -1 , represents the estimation error with a weight of 1.

[0072] Step 6: Design Linear H ∞ Controller: The purpose of the controller is to obtain a gain matrix K, and then obtain is the acceleration of the drone body, and then the expected torque F of the drone is obtained d =mv-mge 3 .

[0073] Among them, p d represents the desired position of the UAV, v d represents the expected speed of the drone, and the position error e p =p b -p d . Define a virtual control input v:

[0074]

[0075] Among them, F d represents the desired torque of the drone, m represents the mass of the system, g is the acceleration due to gravity, and e3 Represents the world coordinate system ∑ W The z-axis.

[0076] The translational dynamics of the UAV can be described as follows:

[0077]

[0078] In the formula represents the expected acceleration; Where 0 represents the zero matrix, I n represents the nxn identity matrix; C=I 6 , u is the control input, and δ represents the disturbance term in the system. p , They are position error, first-order derivative of position error, and second-order derivative of position error respectively.

[0079] represents the interference term, F represents the actual lift, C=I 6 , FF d Represents the interaction force between the position loop and the attitude loop. n Represents the nxn identity matrix.

[0080] For the interference term δ, it has the following properties:

[0081]

[0082] Because e a is bounded, so is also bounded, and ||d p || is bounded, so ||δ|| is bounded. A linear H∞ controller with u=Kz is designed. The control input u=Kz and a symmetric positive definite matrix P satisfy the following inequality:

[0083]

[0084] Among them, P is a selected positive definite symmetric matrix, χ is a positive constant, and K is a gain matrix.

[0085] The virtual control input v is designed as:

[0086]

[0087] In the formula, the gain matrix K = WX -1 , where X = P -1 , W is obtained by solving the following linear matrix inequality (LMI) according to Schur's complement lemma:

[0088]

[0089] Final expected lift F d It can be obtained by the following formula:

[0090] F d =mv-mge 3

[0091] Among them, e 3 Represents the world coordinate system ∑ W The z-axis.

[0092] Step 7: Design of kinematic controller based on potential energy function:

[0093] The overall kinematics of the rotorcraft manipulator is described as:

[0094]

[0095] In the formula, the vector x e For example, is its derivative.

[0096] In the formula, represents the position and posture of the drone in the world coordinate system, q represents the joint vector of the robotic arm, Represents the angular velocity from w to The transformation matrix of the time derivative of in Represents the Jacobian matrix of the robot arm.

[0097] In order to solve the floating base interference problem in the rotor flight manipulator system, the reference angular velocity of the manipulator is proposed at the kinematic level. First, the potential energy function P(Δx e ) design:

[0098]

[0099] Among them, N,k p represents a positive constant, η represents the set error boundary, and max() represents the maximum value between the two.

[0100] In the formula, Δx e =x e -x d represents the tracking error of the end effector of the manipulator to track the desired trajectory, and η represents a positive constraint, so the reference angular velocity control input is as follows:

[0101]

[0102] in, represents the desired speed of the drone, and a represents the gain.

[0103] In the formula, is an estimate of the interference, k is a positive gain, Represents the matrix T 2 The pseudo-inverse of the column vector Δε represents the potential energy function P(Δx e ) is defined as follows:

[0104]

[0105] sat(aΔx e ) represents a saturation function, defined as follows:

[0106]

[0107] Where μ, a is a positive constant used to describe the saturated linear range, Δx ei Represents the vector Δx e elements.

[0108] b 4 The update is:

[0109]

[0110] κ b is a positive gain.

[0111] In order to prove the stability of the control error convergence of the proposed robot end effector, the designed Lyapunov function is as follows:

[0112]

[0113] Among them, P(Δx e ) is the designed potential energy function, κ b For gain, for b 4 The estimation error, b s represents a positive constant value, is the angular velocity tracking error of the robot joint.

[0114] Regarding the control part of the UAV, a related controller model was built in Simulink, and simulation and experimental verification were carried out. The control part of the robotic arm was completed on the onboard computer and experimental verification was completed, proving the performance and practicality of the proposed control algorithm.

[0115] It will be easily understood by those skilled in the art that 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 composite layered anti-interference method for a rotor flight manipulator, characterized in that: The UAV and the robotic arm are decoupled and controlled. The UAV and the robotic arm are regarded as two independent entities, and the dynamic coupling between them is regarded as the interference imposed on the UAV and the robotic arm respectively. The differential flatness method is adopted, and the desired lift provided by the motor and the desired rotation matrix Calculate the desired Euler angle of the drone ; Set up the disturbance observer , quickly compensate for the disturbance imposed on the rotational motion of the drone; set the attitude speed , and based on this set a synovial item , which is used to deal with the disturbance rejection problem on the rotational motion and set the adaptive control terms Used to handle bounded interference and set the overall control input of the drone attitude loop ; In the position loop of the UAV, due to the boundedness of the error, a linear The controller is used to deal with the force disturbance acting on the UAV and the interaction effect between the position loop and the attitude loop. In the control of the robotic arm, a kinematic controller based on the potential energy function is set up to ensure that during the grasping task, the end effector of the aerial manipulator accurately tracks the given position or trajectory instructions, while dealing with the base floating problem.

2. The composite layered anti-interference method for a rotor flight manipulator according to claim 1, characterized in that: is the linearity of the drone position loop The controller sets a virtual control input : ; In the formula, , They are position error and first-order derivative of position error respectively; represents the desired acceleration; the gain matrix ,in , It is obtained by solving the following linear matrix inequality according to Schur's complement lemma: ; In the formula ,in represents the zero matrix, represents the nxn identity matrix; , , , ,in is a selected positive definite symmetric matrix, is a positive constant, is the matrix to be solved; In the attitude loop of the drone, set an overall control input : ; in, , represents the gain, represents the inertia matrix, represents the centripetal moment and the Coriolis moment, represents the Euler angle of the body, represents the desired attitude angular velocity, for The first derivative of ; The formula includes a synovial term , an adaptive control and disturbance observer : ; in, For positive gain, Represents the attitude angle tracking error; for The derivative of ; In the formula, represents the two-norm, is a positive constant value, Represents the torque interference boundary estimates; ; In the formula, represents the auxiliary vector, represents the positive definite gain matrix to be designed, The auxiliary equations for the design satisfy ; In the control of the robotic arm, a kinematic controller based on potential energy function is set to suppress the floating base and provide the reference angular velocity of each joint of the robotic arm: ; in, represents the joint Jacobian matrix of the design, express The pseudo-reversal, express The transpose of is the gradient of the designed potential energy function, Interference boundary for the UAV to the robotic arm The estimate, is the designed saturation function, Indicates the desired speed of the drone.

3. The composite layered anti-interference method for a rotor flight manipulator according to claim 2 is characterized in that: The Lyapunov function is constructed to prove the stability of the control error convergence of the designed controller robot end effector: ; in, is the designed potential energy function, For gain, for The estimation error of represents a positive constant value, is the angular velocity tracking error of the robot joint.