A rigid-soft integrated aerial contact operation robot and control method

Abstract

The present invention discloses a rigid-soft integrated aerial contact operation robot and control method, wherein the robot comprises an all-wheel-drive UAV platform, a single-degree-of-freedom omnidirectional rotation rigid mechanism and a single-section soft arm; the control method comprises constructing a coordinate system and establishing a forward kinematics model of the aerial contact operation robot system; constructing an inverse kinematics model of the single-section soft arm; designing an adaptive inverse kinematics control algorithm based on reinforcement learning based on the inverse kinematics model of the single-section soft arm; establishing a dynamics model of the all-wheel-drive UAV platform, and designing a nonlinear model predictive control method based on an extended Kalman filter estimator, which can constrain the lateral force input of the all-wheel-drive UAV platform and ensure accurate tracking of the all-wheel-drive UAV platform under additional disturbance. Thus, the motion control method of the all-wheel-drive UAV platform and the inverse kinematics control algorithm of the soft arm are used to realize flexible aerial manipulation in a restricted environment.

Description

Technical Field

[0001] The present invention relates to the technical field of aerial working robot control, and in particular to a rigid-flexible integrated aerial contact working robot and a control method thereof. Background Art

[0002] With the advancement of drone and automation technology, aerial robots equipped with robotic arms have expanded the possibilities of aerial operations, such as aerial transportation, assembly, polar scientific research, environmental sampling, and regular inspection and maintenance of infrastructure. However, aerial robots equipped with rigid robotic arms have problems such as difficulty in extension and compliant movement, which restricts their ability to operate in confined environments and greatly limits the scope of application of active aerial operations. In particular, the leverage effect usually occurs during continuous physical interaction with the environment and has a significant impact on aerial robots. Most existing solutions rely on compliant algorithms to solve this problem, and there is little exploration from the perspective of the principles of aerial robots themselves. However, compliant algorithms have potential safety issues in complex dynamic environments and cannot ensure the safety of aerial robots. Summary of the invention

[0003] In order to solve the technical problem that the aerial working robot equipped with a rigid mechanical arm in the prior art has difficulty in stretching and compliant movement, and is difficult to perform flexible work in the air under a restricted environment, the present invention provides a rigid-flexible integrated aerial contact working robot and a control method.

[0004] In order to achieve the above technical purpose, the technical solution of the present invention is:

[0005] A rigid-soft integrated aerial contact working robot comprises a full-drive multi-rotor drone with tilted rotors, a rotating mechanism and a working arm; the central part of the rotating mechanism is fixed to the central fuselage of the multi-rotor drone, the outer edge part of the rotating mechanism is rotatably assembled on the central part, the working arm is connected to the outer edge part to rotate around the central fuselage of the multi-rotor drone along with the outer edge part, and the plane formed by the working arm when rotating along with the rotating mechanism is perpendicular to the plane jointly formed by the rotor shafts of the multi-rotor drone, and the working arm is staggered with the rotors and the rotor shafts when rotating;

[0006] The working arm comprises a proximal rigid arm 1 connected to a rotating mechanism, a distal rigid arm 13 with an end effector 2 at the tail end, and a soft arm 3 connected between the proximal rigid arm 1 and the distal rigid arm 13. The soft arm 3 is a single-section bending soft body.

[0007] The described is a rigid-soft integrated aerial contact working robot, wherein the described soft arm 3 includes two connecting disks and three soft actuators of the same shape and size and connected in parallel, wherein the two ends of the three soft actuators are respectively fixed on the inner sides of the two connecting disks, and the outer sides of the two connecting disks are respectively connected to the ends of the proximal rigid arm 1 and the distal rigid arm 13, and the three soft actuators are arranged in a herringbone shape around the center of the connecting disk; the described soft actuator includes a hose with a hollow cavity, a vacuum pump and a control unit, wherein the air pressure generated by the vacuum pump acts on the hose to realize the movement of the hose, and the described control unit is connected to the vacuum pump to control the start and stop of the vacuum pump.

[0008] A control method for a rigid-flexible integrated aerial contact working robot is based on the aforementioned rigid-flexible integrated aerial contact working robot, comprising the following steps:

[0009] Step 1, construct a coordinate system group including the world coordinate system, the multi-rotor UAV body coordinate system, the rotating mechanism coordinate system, the soft arm start coordinate system, the soft arm end coordinate system and the end effector coordinate system, and based on the coordinate system group, establish a positive kinematic model of the change relationship between the end effector motion and the motion of other moving parts;

[0010] Step 2: Based on the end position of the soft arm, the parameters are converted into arc parameters through geometric calculation, and then further converted into the cavity length of each bending soft body, so as to establish the inverse kinematics model of the soft arm;

[0011] Step 3, based on the inverse kinematics model of the single-section soft arm, the static pressure-length hysteresis model is used as a hysteresis model to describe the hysteresis characteristics of the soft arm to predict the hysteresis characteristics, and the control algorithm based on reinforcement learning is set according to the hysteresis model to achieve the end tracking of the soft arm;

[0012] Step 4, establish a dynamic model of the full-wheel drive UAV platform with six-degree-of-freedom input; then based on model predictive control, consider nonlinear system dynamics and external disturbances to implement nonlinear model predictive control with an extended Kalman filter estimator, thereby constraining the state and input to achieve lateral force constraints on the full-wheel drive UAV platform with tilt rotors and complete the entire control process.

[0013] The method, in step 1, the forward kinematics model expression is:

[0014]

[0015] in, and They represent the transformation matrix and rotation matrix from the coordinate system "*" to the coordinate system "*", with the symbol *={B,M,E} and the symbol *={I,B,M}, respectively. represents a real number set, and B represents the coordinate system of the UAV platform M represents the rotating mechanism coordinate system E represents the end effector coordinate system I represents the world coordinate system The world coordinate system The end effector position in The body coordinate system The joint positions of the rotating mechanism in is the rotation mechanism joint coordinate system The end effector position in the matrix, the superscript T represents the matrix transpose, p b Represents the world coordinate system The location of the UAV platform.

[0016] The method, the transformation matrix in is the rotation mechanism joint coordinate system and the software arm start coordinate system The transformation matrix between Indicates the coordinate system of the start end of the soft arm Coordinate system of the end of the soft robot arm The transformation matrix between is the end effector coordinate system Coordinate system of the end of the soft robot arm The transformation matrix between , and:

[0017]

[0018] in,

[0019] c φ 、s φ 、c θ and θ They are expressed as cosφ, sinφ, cosθ and sinθ respectively, φ is the curvature angle of the soft arm, and θ is the bending angle of the soft arm; and Represented as the software arm start coordinate system The translation and rotation in R y (θ) represents the coordinate system of the software arm around the base The y-axis rotation angle θ, R z (φ) represents the coordinate system of the starting point of the soft arm The z-axis rotation angle φ, xs ,y s ,z s They are respectively the x-axis, y-axis, and z-axis coordinates of the task space of the soft arm, i.e., the end position that needs to be reached.

[0020] The method, step 2 comprises:

[0021] Step 201, calculate the end position {x s ,y s ,z s} is converted to arc parameters {ρ, φ, θ}:

[0022]

[0023] Where r is the radius of curvature, ρ is the curvature of the soft arm;

[0024] Step 202, calculating the cavity length of each bending soft body by the following calculation formula:

[0025]

[0026]

[0027] The constraints are:

[0028]

[0029] Among them l i represents the cavity length of the i-th bending soft body, h represents the cross-sectional radius, and π represents the circumference of a circle.

[0030] In the method, in step 3, the hysteresis model is expressed by the following formula:

[0031]

[0032] Among them l (k) is the cavity length, is the pressure in the cavity in the hysteresis model, where the pressure is expressed by the symmetric part Γ CPI (l (k) ), asymmetric part Γ UPI (l (k) ) and the polynomial part W(l (k) ) to express it in order to fit the formal curve; is the operator output of the hysteresis model, the superscript (k) represents the kth value; a0 is the linear weight gain, b i ,δ ij and w i are the weight gains of the symmetric part, asymmetric part and polynomial part respectively; γ jis the jth dead zone, i.e., the input signal range corresponding to zero output; c j and d j are the jth tilt angles of the cavity during pressurization and decompression, respectively; w0 is the offset related to the working angle of the hysteresis curve; N c and N w are the number of symmetric parts and polynomial parts respectively; N u is the total number of dead zones in the asymmetric part;

[0033] Then the proportional-integral-derivative control compensation term is introduced As a feedback term to modify the feedforward term in the hysteresis model in real time have:

[0034]

[0035] in represents the cavity length error, is the expected cavity length, l (k) is the actual cavity length, which is obtained by calculating the arc parameters {ρ, φ, θ} according to the calculation formula in step 201 based on the expected position or actual position of the end of the given soft arm, and then using the constraint conditions of calculating the cavity length of each bending soft body in step 202; for The first derivative of k p , k i and k d are proportional, integral and differential coefficients respectively; the proportional coefficient k p Set to an exponential function, expressed as where k p0 is a constant term, λ1, λ2 and λ3 are terms that determine the exponential value, and they are all positive numbers. k is determined online through a control algorithm based on reinforcement learning. p .

[0036] The method, in step 3, setting the control algorithm based on reinforcement learning according to the hysteresis model includes:

[0037] Based on the ∈-greedy strategy, the soft arm selects the best action through learning, where the ∈-greedy strategy is:

[0038]

[0039] Where rand() represents the initial random assignment for judgment, parameter ∈∈(0,1); is the state action value, expressed as:

[0040]

[0041] Where β is the learning rate and σ is the discount factor; is the state space, which includes 7 continuous and symmetrical intervals S1-S7:

[0042]

[0043] is an action space containing four actions A1-A4, and

[0044]

[0045] In the method, in step 4, the dynamic model of the all-wheel-drive UAV platform with six-degree-of-freedom input is expressed as:

[0046]

[0047] Where m s is the total mass of the all-wheel-drive UAV platform, J b is the inertia matrix of the all-wheel drive UAV platform, g is the gravity constant; vector is the vector v b The first derivative of b is the linear velocity of the all-wheel drive UAV platform in the world coordinate system; ω b is the angular velocity of the all-wheel drive UAV platform in the body coordinate system, and the vector ω b The first derivative of c and τ c are the control input force and torque of the all-wheel drive UAV platform; F e and τ e They are the disturbance force and moment on the all-wheel drive UAV platform respectively;

[0048] The position and attitude dynamics of the all-wheel drive UAV platform are expressed as:

[0049]

[0050] Among them, the vector is the vector p b The first derivative of b is the position of the all-wheel drive UAV platform in the world coordinate system; vector is the vector q b The first derivative of b The attitude of the all-wheel drive UAV platform expressed by quaternion.

[0051] In the method, in step 4, implementing lateral force constraint on the all-wheel drive UAV platform includes:

[0052] The state vector x and input vector u are expressed as:

[0053]

[0054] The system dynamics is described by the all-wheel drive UAV platform dynamics model and the platform position and attitude dynamics expressions, and the nonlinear optimization control is expressed as:

[0055]

[0056] in, where x k,d and u k,d are the expected state vector and the expected input vector respectively; Q x ≥0 and R u ≥0 represents the penalty matrix of state and input respectively, Q N Represents the penalty matrix of the terminal state; N represents the prediction step size; and They are state and input constraints respectively; and are the estimated disturbance forces and moments, respectively;

[0057] The state vector of the extended Kalman filter estimator is and the input vector u EKF It is expressed as:

[0058]

[0059] in, Represent the variables based on the extended Kalman filter The estimated value of the all-wheel drive UAV platform position, attitude, linear velocity and angular velocity are used as the measurement vector z of the extended Kalman filter. EKF ,Right now:

[0060]

[0061] Then the system dynamics and the constraints are given by the discrete time series t0,…,t N The system dynamics are simulated forward along the interval using a 4th-order implicit Runge-Kutta integrator, and the boundary values ​​are solved for each sampling interval to obtain the estimated force and estimated moments Thus the nonlinear optimization control expression is solved.

[0062] The technical effect of the present invention is that the soft arm of the present invention is installed on the UAV platform, and the soft arm performs functions similar to those of the traditional rigid-link mechanical arm. The trade-off between the arm weight and the degree of freedom of the traditional rigid-link mechanical arm limits their flexibility and operability, while the inherent compliance of the soft arm can ensure safe operation and robust interaction with the environment. Therefore, by utilizing the agility and flexibility of the UAV, the integrated soft arm can improve the required compliance and inherent safety, thereby enabling flexible aerial operations in a restricted environment. The present invention solves the strong leverage effect caused by the traditional aerial contact operation robot with integrated rigid operating mechanism on the UAV platform during continuous interactive manipulation in the air, can effectively alleviate the impact on the stability of the all-wheel drive UAV platform during continuous manipulation in the air, and can use the soft arm to achieve flexible manipulation tasks in a restricted environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 is a schematic structural diagram of an aerial contact working robot according to an embodiment of the present invention;

[0064] Figure 2 It is a flowchart of manufacturing the soft arm of the aerial contact working robot in an embodiment of the present invention;

[0065] Figure 3 A general flowchart of the method in the embodiment of the present invention;

[0066] Figure 4 It is a flowchart of the calculation process of the method in the embodiment of the present invention.

[0067] Among them, 1 is the proximal rigid arm, 2 is the end effector, 3 is the soft arm, 4 is the propeller, 5 is the onboard computer, 6 is the flight controller, 7 is the electronic speed regulator, 8 is the motor, 9 is the rigid mechanism, 10 is the transmission mechanism, 11 is the power module, 12 is the drive servo, and 13 is the distal rigid arm. DETAILED DESCRIPTION

[0068] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0069] See also Figure 1The all-wheel-drive UAV platform provided in this embodiment is developed from a traditional six-rotor aircraft. The propeller 4 of the UAV platform of this embodiment is tilted at a fixed angle of 30° and points in different directions. This configuration can independently control the position and attitude of the UAV platform. Specifically, the UAV has the ability to move in six degrees of freedom, but the control input of the traditional under-actuated UAV is only four, and the position movement in the x and y directions needs to be indirectly controlled by controlling the roll and pitch angles; while the all-wheel-drive type has six control inputs, which can independently control the six degrees of freedom of position and attitude. An onboard computer 5 is installed on the base of the all-wheel-drive UAV platform of this embodiment for processing high-level control signals, and a flight controller is also installed for processing low-level flight signals. The command rotor speed signal generated by the flight controller 6 is transmitted to the electronic speed regulator 7 to drive the motor 8 and the propeller 4.

[0070] The drone of this embodiment is equipped with a single-degree-of-freedom rotating mechanism, which is a rigid mechanism 9 capable of 360° omnidirectional rotation, including a main frame, a transmission mechanism 10, a working arm, a power module 11 and a driving servo 12, wherein the main frame is used as an outer edge portion, and is rigidly connected to the drone platform through a central portion composed of gears as a transmission mechanism 10. A proximal rigid arm 1 serving as an intermediate connection is installed at one end of the main frame of the rotating mechanism, and two battery mounting plates are designed at the other end, so that two batteries of the power module 11 used to drive the entire working robot are installed at the other end of the rotating mechanism, so as to serve as a counterweight to balance the torque generated by the weight of the working arm including the end effector 2, so that the center of gravity of the rotating mechanism is always maintained at the center of the body, thereby alleviating the problem of center of gravity offset caused by the rotating mechanism.

[0071] The main frame and the flying platform are driven by a transmission mechanism, which can realize 360° omnidirectional rotation of the working arm and the end effector 2. The plane formed by the working arm when rotating is perpendicular to the plane formed by the rotor shafts of the multi-rotor drone, and the working arm is staggered with each rotor and the rotor shaft when rotating. The rotating mechanism connector simultaneously connects the main frame and the transmission gear of the transmission mechanism, wherein a 360° gear slide is embedded inside the transmission gear, and the gear slide is meshed with the transmission pinion on the driving servo 12, thereby realizing the rotation of the operating mechanism at any angle; the servo connector simultaneously connects the driving servo 12 and the flying platform for rigid fixed installation.

[0072] See also Figure 2, the soft arm 3 of this embodiment is installed between the proximal rigid arm 1 and the distal rigid arm 13 of the working arm. The soft arm 3 includes two connecting plates and three soft actuators of the same shape and size and connected in parallel. The proximal rigid arm 1 and the distal rigid arm 13 are connected to the soft arm 3 through the connecting plates. The two ends of the three soft actuators are respectively fixed on the inner sides of the two connecting plates, and the outer sides of the two connecting plates are respectively connected to the ends of the proximal rigid arm 1 and the distal rigid arm 13. The three soft actuators are arranged in a herringbone shape around the center of the connecting plate. This embodiment selects three bellows of the same size as soft actuators, and reduces the pressure on each bellows to shorten it. At the same time, in order to ensure that the axes of the soft actuator remain relatively parallel during the shortening process, this embodiment also sets a plurality of Y-shaped structural retainers in parallel between the three bellows for support and limitation. The soft actuator is also provided with a vacuum pump and a control unit, wherein the air pressure generated by the vacuum pump acts on the hose to realize the movement of the hose, and the control unit is connected to the vacuum pump to control the start and stop of the vacuum pump. The control unit of this embodiment includes a micro relay for intermittently measuring the pressure in the cavity and a micro controller for signal processing.

[0073] This embodiment also uses an octahedron-ring-octahedron connection structure outside the bellows to construct a reinforcement layer to enhance the structural strength. This octahedron-ring-octahedron connection structure has greater flexibility than the octahedron-octahedron connection structure. Specifically, the octahedron is a hollow frame structure. The octahedron is connected to another octahedron at the bottom through two chains connected to each other by multiple ring structures, thereby forming an octahedron-ring-octahedron connection structure. Multiple identical octahedron-ring-octahedron connection structures are connected to each other, and at the same time, two parallel octahedra are connected to each other through an octahedron, thereby forming a reinforcement layer. Finally, the outside of the reinforcement layer is sealed with a soft film, and a single-section lightweight reinforced soft arm is obtained. In this embodiment, a pneumatic hose structure is used as the soft arm. In actual use, soft structures including concentric tubes, rope drive and magnetic drive can also be considered to achieve drive.

[0074] See also Figure 3 and Figure 4 This embodiment also provides a control method for a rigid-flexible integrated aerial contact working robot to control the aforementioned rigid-flexible integrated aerial contact working robot, comprising the following steps:

[0075] Step 1: Construct a coordinate system group including the world coordinate system, the multi-rotor drone body coordinate system, the rotating mechanism coordinate system, the soft arm start coordinate system, the soft arm end coordinate system and the end effector coordinate system, and establish a forward kinematic model of the change relationship between the end effector motion and the motion of other moving parts based on the coordinate system group. The position and linear velocity signals are obtained by external sensor measurement, and the attitude and angular velocity signals are obtained from the airborne inertial measurement unit.

[0076] Specifically, in step 1, six coordinate systems are first defined to describe the kinematics of the rigid-flexible aerial contact robot system: the world coordinate system UAV platform body coordinate system Rotating mechanism coordinate system Software arm start coordinate system Coordinate system of the end of the soft arm and the end effector coordinate system After establishing the robot system coordinate system, the forward kinematics is derived to solve the transformation between the end effector motion and the aircraft / joint displacement. The forward kinematics is described as follows:

[0077]

[0078] in, and They represent the transformation matrix and rotation matrix from the coordinate system "*" to the coordinate system "*", with the symbol *={B,M,E} and the symbol *={I,B,M}, respectively. represents a real number set; B represents the coordinate system of the UAV platform M represents the rotating mechanism coordinate system E represents the end effector coordinate system I represents the world coordinate system The world coordinate system The end effector position in The body coordinate system The position of the rotating mechanism in is the rotating mechanism coordinate system The end effector position in the matrix; the superscript T indicates the matrix transpose, p b Represents the world coordinate system The location of the UAV platform.

[0079] It is worth noting that the matrix Includes software arm The transformation between the beginning and the end, where is the rotation mechanism joint coordinate system and the software arm start coordinate system The transformation matrix between is the end effector coordinate system Coordinate system of the end of the soft robot arm Since the three cavities of the soft arm are assembled into a parallel structure, it is assumed that the soft arm has a constant curvature. In order to obtain the forward kinematics of the soft arm, it is necessary to determine the transformation matrix from the actuator space {u1,u2,u3,u s} to the soft robot task space {x s ,y s ,z s}, where the reinforcement layer pressure u s It is isolated from the cavity pressure {u1,u2,u3} and is only used to enable the reinforcement layer. The transformation from the soft arm joint space {l1,l2,l3} to the soft arm configuration space {ρ,φ,θ} is expressed as

[0080]

[0081]

[0082] Among them, l i represents the cavity length of the i-th bending soft body, ρ, φ and θ are the curvature, curvature angle and bending angle of the soft body arm respectively, h represents the cross-sectional radius, u1, u2 and u3 are the first, second and third cavity pressures respectively, x s ,y s and z s are the xyz positions of the end of the soft arm. s ,y s ,z s Transformation of To model, first, the soft arm revolves around the base coordinate system The y-axis rotation angle θ, that is, R y (θ); then, the soft arm moves around the base coordinate system The z-axis is rotated by an angle φ, that is, R z (φ); then translate the soft arm out of the xz plane and translate p θ = r[1-c θ ,0,s θ ] T Where r = 1 / ρ is the radius of curvature; Finally, the posture is adjusted by right multiplying the rotation matrix R(-φ). Therefore, the transformation matrix It is expressed as follows:

[0083]

[0084] Among them, c φ 、s φ 、c θ and θ They are expressed as cosφ, sinφ, cosθ and sinθ respectively; and Represented as the software arm start coordinate system Translational and rotational motion in .

[0085] Step 2: Based on the end position of the soft arm, it is converted into arc parameters through geometric calculation, and then further converted into the cavity length of each bending soft body, so as to establish the inverse kinematics model of the soft arm.

[0086] Specifically, in order to determine the end motion of the soft arm, based on the given end position {x s ,y s ,z s}Build an inverse kinematics model. The modeling steps are as follows: First, according to geometric calculation, the given end position {x s ,y s ,z s} is converted into arc parameters {ρ, φ, θ}. That is, according to the geometric calculation of formula (7), given the position of the end point, the arc parameters {ρ, φ, θ} can be obtained as follows:

[0087]

[0088] Second, the arc parameters {ρ, φ, θ} are converted into cavity lengths {l1, l2, l3}. At the same time, considering that the simultaneous driving of the three cavities of the soft arm will result in the situation of l1=l2=l3, this indicates that the soft arm will perform extension or contraction movements and may cause singular problems. In order to avoid potential singularities, a constraint is given: at most two cavities are driven simultaneously, and at least one cavity maintains its initial length. Therefore, according to geometric calculations, the initial length of the cavity {l1, l2, l3} can be calculated based on the arc parameters {ρ, φ, θ} as follows:

[0089]

[0090] Step 3, based on the inverse kinematics model of the single-section soft arm, the static pressure-length hysteresis model is used as the hysteresis model to describe the hysteresis characteristics of the soft arm to predict the hysteresis characteristics, and the control algorithm based on reinforcement learning is set according to the hysteresis model to achieve the end tracking of the soft arm under the influence of gravity and hysteresis.

[0091] Specifically, based on the inverse kinematics of the soft arm based on formula (8) and formula (9), given the end position {x s ,y s ,z s}, the corresponding cavity length {l1,l2,l3} can be calculated. In order to convert the cavity length {l1,l2,l3} into cavity pressure {u1,u2,u3}, while considering the effects of gravity, hysteresis and nonlinearity, the hysteresis model of the soft arm is first established, and then the control method is designed to compensate for the effects of gravity and external interference.

[0092] Due to the elasticity of the material, the actuator cavity has an asymmetric hysteresis characteristic. Therefore, the static pressure-length hysteresis model is used to describe this hysteresis phenomenon, which is expressed as follows:

[0093]

[0094] Among them, l (k) is the cavity length, is the actuator pressure in the hysteresis model, where the pressure is given by the symmetric part Γ CPI (l (k) ), asymmetric part Γ UPI (l (k) ) and the polynomial part W(l (k) ) to fit the formal curve; is the operator output of the hysteresis model; a0 is the amplification l (k) The linear weight gain, b i ,δ ij and w i is the weight gain; j is the jth dead zone, that is, the input signal range corresponding to the zero output. j and d j are the jth inclination angles of the pressurization process and the decompression process respectively; w0 is the offset related to the working angle of the hysteresis curve; N c and N w are the number of symmetric parts and polynomial parts respectively; N u is the total number of asymmetric dead zones, the superscript (k) represents the kth value, and the superscript (k-1) represents the k-1th value.

[0095] The cavity pressure predicted by the model in formula (10) is is regarded as a feedforward term. However, the prediction performance is easily affected by external disturbances. To solve this problem, a proportional-integral-derivative control compensation term is introduced. Acts as feedback to modify actual chamber pressure It is expressed as follows:

[0096]

[0097] in represents the cavity length error, Expected cavity length and the actual cavity length l (k) According to formula (8) and formula (9), the expected position and actual location Calculated; k p , k i and k dare proportional, integral and differential coefficients respectively; in order to improve the feedback control performance, the proportional coefficient k p is adjusted and set to an exponential function, expressed as follows:

[0098]

[0099] Among them, k p0 , λ1, λ2 and λ3 are positive constants, representing the constant term and the term that determines the exponential value respectively; and in order to smoothly adjust the coefficient k p , the Sarsa learning algorithm is used to determine this parameter online. In order to evaluate the performance of the end effector, the state space is defined And divide it into several continuous and symmetrical intervals S1-S7 as follows:

[0100]

[0101] Then set an action space containing four actions A1-A4 Adjust the parameter k separately p0 , λ1, λ2 and λ3 are expressed as follows:

[0102]

[0103] This shows that in the current state Next, we can look at the action space Select an action A (k) , determine the proportionality coefficient k by formula (15) p . Further, in order to evaluate the selected action, based on the state space and action space Designed a reward table Next, in order to let the soft arm learn to choose the best action, an improved ∈-greedy strategy is used to reduce the diversity of action selection and improve the convergence speed. The improved ∈-greedy strategy is defined as follows:

[0104]

[0105] Among them, the parameter ∈∈(0,1), is the state action value, which is expressed as follows:

[0106]

[0107] Where β is the learning rate and σ is the discount factor. According to formula (11)-formula (17), the adaptive inverse kinematics control of the soft arm based on reinforcement learning can be realized.

[0108] Step 4: Establish a dynamic model of the all-wheel-drive UAV platform with six degrees of freedom input. Then, based on model predictive control, consider nonlinear system dynamics and external disturbances to implement nonlinear model predictive control with an extended Kalman filter estimator, thereby constraining the state and input to achieve lateral force constraints on the all-wheel-drive UAV platform with tilt rotors, ensure accurate tracking of the all-wheel-drive UAV platform under additional disturbances, and complete the entire control process.

[0109] Specifically, the Newton-Euler method is first used to model the dynamics of the all-wheel drive UAV platform as follows:

[0110]

[0111] Among them, m s is the total mass of the all-wheel-drive UAV platform, J b is the inertia matrix of the all-wheel drive UAV platform, g is the gravity constant; vector is the vector v b The first derivative of b is the linear velocity of the all-wheel drive UAV platform in the world coordinate system; ω b is the angular velocity of the all-wheel drive UAV platform in the body coordinate system, and the vector ω b The first derivative of c and τ c are the control input force and torque of the all-wheel drive UAV platform; F e and τ e They are the disturbance force and moment acting on the all-wheel drive UAV platform respectively.

[0112] Then, the position and attitude dynamics of the all-wheel drive UAV platform are expressed as follows:

[0113]

[0114] Among them, the vector is the vector p b The first derivative of b is the position of the all-wheel drive UAV platform in the world coordinate system; vector is the vector q b The first derivative of b The attitude of the all-wheel drive UAV platform expressed by quaternion.

[0115] Since the lateral force is usually limited for the tilt-rotor configuration with a fixed angle, it is necessary to consider the actuator saturation problem in the lateral direction. Therefore, the introduction of model predictive control technology can realize the state and input constraints to ensure the stable flight of the all-wheel drive UAV platform, where the state vector x and the input vector u are expressed as follows:

[0116]

[0117] System Dynamics It can be described by formula (18)-formula (20), and then the nonlinear optimization control problem is defined as follows:

[0118]

[0119] in, where x k,d and u k,d are the expected state vector and the expected input vector respectively; Q x ≥0 and R u ≥0 represents the penalty matrix of state and input respectively, Q N Represents the penalty matrix of the terminal state; N represents the prediction step size; and They are state and input constraints respectively; and are the estimated disturbance force and moment, respectively.

[0120] In order to estimate the unmodeled system dynamics and external disturbances, a disturbance observer based on extended Kalman filter is incorporated into the design of nonlinear model predictive controller to achieve offset-free tracking control behavior. In the nonlinear model predictive controller, the dynamic model equations in formula (18)-formula (20) are the state vector of the extended Kalman filter method. and the input vector u EKF The definition is as follows:

[0121]

[0122] in, Represent the variables based on the extended Kalman filter Then, the measured position, attitude, linear velocity and angular velocity of the AWD platform are used as the measurement vector z of the extended Kalman filter. EKF , as shown below:

[0123]

[0124] Next, we use the multi-targeting technique and the ACADO toolkit to solve the optimization control problem in formula (23); the system dynamics and constraints are discrete time series in the sampling interval t0,…,t N The system dynamics are simulated forward along the intervals using a 4th-order implicit Runge–Kutta integrator. Then, a boundary value problem is solved for each sampling interval.

[0125] By using the motion control method of the all-wheel drive UAV platform, i.e., formula (23), and the inverse kinematics control algorithm of the soft arm, i.e., formula (11), the rigid-flexible integrated aerial contact operation robot system can be flexibly manipulated in the air in a restricted environment.

[0126] The above description is only a preferred embodiment of the technology of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and substitutions can be made without departing from the technical principles of the present invention. These improvements and substitutions should also be regarded as the scope of protection of the present invention.

Claims

1. A control method for a rigid-flexible integrated aerial contact operation robot, characterized in that: An aerial contact working robot based on rigid-soft integration comprises a full-drive multi-rotor drone with tilted rotors, a rotating mechanism and a working arm; the central part of the rotating mechanism is fixed to the central fuselage of the multi-rotor drone, the outer edge part of the rotating mechanism is rotatably assembled on the central part, the working arm is connected to the outer edge part to realize rotation around the central fuselage of the multi-rotor drone along with the outer edge part, and the plane formed by the working arm when rotating along with the rotating mechanism is perpendicular to the plane jointly formed by the rotor shafts of the multi-rotor drone, and the working arm is staggered with the rotors and the rotor shafts when rotating; The working arm comprises a proximal rigid arm (1) connected to a rotating mechanism, a distal rigid arm (13) with an end effector (2) at its tail end, and a soft arm (3) connected between the proximal rigid arm (1) and the distal rigid arm (13), wherein the soft arm (3) is a single-section structured bending soft body; The following steps are involved: Step 1, construct a coordinate system group including the world coordinate system, the multi-rotor UAV body coordinate system, the rotating mechanism coordinate system, the soft arm start coordinate system, the soft arm end coordinate system and the end effector coordinate system, and based on the coordinate system group, establish a positive kinematic model of the change relationship between the end effector motion and the motion of other moving parts; Step 2: Based on the end position of the soft arm, the parameters are converted into arc parameters through geometric calculation, and then further converted into the cavity length of each bending soft body, so as to establish the inverse kinematics model of the soft arm; Step 3, based on the inverse kinematics model of the single-section soft arm, the static pressure-length hysteresis model is used as a hysteresis model to describe the hysteresis characteristics of the soft arm to predict the hysteresis characteristics, and the control algorithm based on reinforcement learning is set according to the hysteresis model to achieve the end tracking of the soft arm; Step 4, establish a dynamic model of the full-wheel drive UAV platform with six-degree-of-freedom input; then based on model predictive control, consider nonlinear system dynamics and external disturbances to implement nonlinear model predictive control with an extended Kalman filter estimator, thereby constraining the state and input to achieve lateral force constraints on the full-wheel drive UAV platform with tilt rotors and complete the entire control process.

2. The method according to claim 1, characterized in that: In step 1, the forward kinematics model expression is: in, and They represent the transformation matrix and rotation matrix from the coordinate system "*" to the coordinate system "★", the symbol *={B,M,E} and the symbol ★={I,B,M}, respectively. represents a real number set, and B represents the coordinate system of the UAV platform M represents the rotating mechanism coordinate system E represents the end effector coordinate system I represents the world coordinate system The world coordinate system The end effector position in The body coordinate system The joint positions of the rotating mechanism in is the rotation mechanism joint coordinate system The end effector position in represents the matrix transpose, p b Represents the world coordinate system The location of the UAV platform.

3. The method according to claim 2, characterized in that Transformation Matrix in is the rotation mechanism joint coordinate system and the software arm start coordinate system The transformation matrix between Indicates the coordinate system of the start end of the soft arm Coordinate system of the end of the soft robot arm The transformation matrix between is the end effector coordinate system Coordinate system of the end of the soft robot arm The transformation matrix between , and: in, c φ 、s φ 、c θ and θ They are expressed as cosφ, sinφ, cosθ and sinθ respectively, φ is the curvature angle of the soft arm, θ is the bending angle of the soft arm; and Represented as the software arm start coordinate system The translation and rotation in R y (θ) represents the coordinate system of the software arm around the base The y-axis rotation angle θ, R z (φ) represents the starting coordinate system of the soft arm The z-axis rotation angle φ, x s ,y s ,z s They are respectively the x-axis, y-axis, and z-axis coordinates of the task space of the soft arm, i.e., the end position that needs to be reached.

4. The method according to claim 3, characterized in that The step 2 comprises: Step 201, calculate the end position {x s ,y s ,z s } is converted to arc parameters {ρ, φ, θ}: Where r is the radius of curvature, ρ is the curvature of the soft arm; Step 202, calculating the cavity length of each bending soft body by the following calculation formula: The constraints are: Among them l i represents the cavity length of the i-th bending soft body, h represents the cross-sectional radius, and π represents the circumference of a circle.

5. The method according to claim 4, characterized in that In step 3, the hysteresis model is expressed by the following formula: Among them l (k) is the cavity length, is the pressure in the cavity in the hysteresis model, where the pressure is expressed by the symmetric part Γ CPI (l (k) ), asymmetric part Γ UPI (l (k) ) and the polynomial part W(l (k) ) to express it in order to fit the formal curve; is the operator output of the hysteresis model, the superscript (k) represents the kth value, and the superscript (k-1) represents the k-1th value; a0 is the linear weight gain, b i , δ ij and w i are the weight gains of the symmetric part, asymmetric part and polynomial part respectively; γ j is the jth dead zone, i.e., the input signal range corresponding to zero output; c j and d j are the jth tilt angles of the cavity during pressurization and decompression, respectively; w0 is the offset related to the working angle of the hysteresis curve; N c and N w are the number of symmetric parts and polynomial parts respectively; N u is the total number of dead zones in the asymmetric part; Then the proportional-integral-derivative control compensation term is introduced As a feedback term to modify the feedforward term in the hysteresis model in real time have: in represents the cavity length error, is the expected cavity length, l (k) is the actual cavity length, which is obtained by calculating the arc parameters {ρ, φ, θ} according to the calculation formula in step 201 based on the expected position or actual position of the end of the given soft arm, and then using the constraint conditions of calculating the cavity length of each bending soft body in step 202; for The first derivative of k p , k i and k d are proportional, integral and differential coefficients respectively; the proportional coefficient k p Set to an exponential function, expressed as where k p0 is a constant term, λ1, λ2 and λ3 are terms that determine the exponential value, and they are all positive numbers. k is determined online through a control algorithm based on reinforcement learning. p .

6. The method according to claim 5, characterized in that In step 3, setting a control algorithm based on reinforcement learning according to the hysteresis model includes: Based on the ∈-greedy strategy, the soft arm selects the best action through learning, where the ∈-greedy strategy is: Where rand() represents the initial random assignment for judgment, parameter ∈∈(0,1); is the state action value, expressed as: Where β is the learning rate and σ is the discount factor; is the state space, which includes 7 continuous and symmetrical intervals S1-S7: is an action space containing four actions A1-A4, and 7. The method according to claim 2, characterized in that: In step 4, the dynamic model of the all-wheel-drive UAV platform with six-degree-of-freedom input is expressed as: Where m s is the total mass of the all-wheel-drive UAV platform, J b is the inertia matrix of the all-wheel drive UAV platform, g is the gravity constant; vector is the vector v b The first derivative of b is the linear velocity of the all-wheel drive UAV platform in the world coordinate system; ω b is the angular velocity of the all-wheel drive UAV platform in the body coordinate system, and the vector ω b The first derivative of c and τ c are the control input force and torque of the all-wheel drive UAV platform; F e and τ e They are the disturbance force and moment on the all-wheel drive UAV platform respectively; The position and attitude dynamics of the all-wheel drive UAV platform are expressed as: Among them, the vector is the vector p b The first derivative of b is the position of the all-wheel drive UAV platform in the world coordinate system; vector is the vector q b The first derivative of b The attitude of the all-wheel drive UAV platform expressed by quaternion.

8. The method according to claim 7, characterized in that In step 4, implementing lateral force constraint on the all-wheel drive UAV platform includes: The state vector x and input vector u are expressed as: The system dynamics is described by the all-wheel drive UAV platform dynamics model and the platform position and attitude dynamics expressions, and the nonlinear optimization control is expressed as: in, where x k,d and u k,d are the expected state vector and the expected input vector respectively; Q x ≥0 and R u ≥0 represents the penalty matrix of state and input respectively, Q N Represents the penalty matrix of the terminal state; N represents the prediction step size; and They are state and input constraints respectively; and are the estimated disturbance forces and moments, respectively; The state vector of the extended Kalman filter estimator is and the input vector u EKF It is expressed as: in, Represent the variables based on the extended Kalman filter The estimated value of the all-wheel drive UAV platform position, attitude, linear velocity and angular velocity are used as the measurement vector z of the extended Kalman filter. EKF ,Right now: Then the system dynamics and the constraints are given by the discrete time series t0,…,t N The system dynamics are simulated forward along the interval using a 4th-order implicit Runge-Kutta integrator, and the boundary values ​​are solved for each sampling interval to obtain the estimated force and estimated moments Thus the nonlinear optimization control expression is solved.

Images