Rigidity and flexibility integrated air contact type operation robot and control method

Abstract

The invention discloses a rigid-flexible integrated aerial contact type operation robot and a control method. The robot comprises a full-drive type unmanned aerial vehicle platform, a single-degree-of-freedom omnidirectional rotation rigid mechanism and a single-section soft arm. The control method comprises the steps that a coordinate system is constructed, and a forward kinematics model of the air contact type operation robot system is established; constructing an inverse kinematics model of the single-section soft arm; designing a self-adaptive inverse kinematics control algorithm based on reinforcement learning on the basis of the inverse kinematics model of the single-section soft arm; according to the method, an all-drive unmanned aerial vehicle platform dynamic model is established, a nonlinear model prediction control method based on an extended Kalman filter estimator is designed, lateral force input of the all-drive unmanned aerial vehicle platform can be restrained, and accurate tracking of the all-drive unmanned aerial vehicle platform under the condition of additional disturbance is guaranteed. Therefore, flexible control in the air in a limited environment is realized by using a motion control method and a soft arm inverse kinematics control algorithm of the all-wheel-driven unmanned aerial vehicle platform.

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 Respectively represent the coordinate system " "To coordinate system" "The transformation matrix and rotation matrix, symbol and symbol , 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, 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] , , , and Respectively expressed as , , and , is the curvature angle of the soft arm, is the bending angle of the soft arm; and Respectively represented as the software arm start coordinate system The translational and rotational motions in Represents the coordinate system of the soft arm around the base The y-axis rotation angle , Indicates the coordinate system of the start end of the soft arm The z-axis rotation angle , 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 by the following formula Arc parameters converted to :

[0022] ;

[0023] in 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] ;

[0028] The constraints are:

[0029] ;

[0030] in Indicates The cavity length of the root bending soft body, represents the radius of the cross section and π represents the circumference of a circle.

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

[0032] ;

[0033] in is the cavity length, is the pressure in the cavity of the hysteresis model, where the pressure is expressed by the symmetric part , asymmetric part and the polynomial part To express to fit the formal curve; is the operator output of the hysteresis model, and the superscript (k) indicates the kth value; is the linear weight gain, , and are the weight gains of the symmetric part, asymmetric part, and polynomial part respectively; For the A dead zone, which is the input signal range corresponding to zero output; and They are the first Angle of inclination; is the offset related to the working angle of the hysteresis curve; and are the number of symmetric parts and polynomial parts, respectively; is the total number of dead zones in the asymmetric part;

[0034] 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:

[0035] ;

[0036] ;

[0037] in represents the cavity length error, , is the expected cavity length, is the actual cavity length, which is calculated by the expected position or actual position of the end of the given soft arm according to the calculation formula in step 201. Then, the constraint conditions for calculating the cavity length of each bending soft body in step 202 are used to obtain; for The first derivative of ; , and are proportional, integral and differential coefficients respectively; the proportional coefficient Set to an exponential function, expressed as ;in is a constant term, , and are terms that determine the exponential value, and are positive numbers, and are determined online by a control algorithm based on reinforcement learning .

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

[0039] based on - Greedy strategy to make the soft arm choose the best action through learning, where -The greedy strategy is:

[0040] ;

[0041] Among them, rand() represents the initial random assignment for judgment, and the parameter ; is the state action value, expressed as:

[0042] ;

[0043] in is the learning rate, is the discount factor; is the state space, which includes 7 continuous and symmetrical intervals S1-S7:

[0044] ;

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

[0046] .

[0047] 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:

[0048] ;

[0049] in is the total mass of the all-wheel drive UAV platform, It is the inertial matrix of the all-wheel drive UAV platform. is the gravitational constant; the vector For vector The first-order derivative of is the linear velocity of the all-wheel drive UAV platform in the world coordinate system; is the angular velocity of the all-wheel drive UAV platform in the body coordinate system, and the vector for The first derivative of ; and They are the control input force and torque for the all-wheel drive UAV platform; and They are the disturbance force and moment on the all-wheel drive UAV platform respectively;

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

[0051] ;

[0052] ;

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

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

[0055] The state vector and the input vector It is expressed as:

[0056] ;

[0057] ;

[0058] 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:

[0059] ;

[0060] in, , ,in and are the expected state vector and the expected input vector respectively; and denote the penalty matrices of state and input respectively, represents the penalty matrix for terminal states; represents the prediction step length; and They are state and input constraints respectively; and are the estimated disturbance forces and moments, respectively;

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

[0062] ,

[0063] ;

[0064] 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 of the extended Kalman filter. ,Right now:

[0065] ;

[0066] Then the system dynamics and the constraints are discrete time series within the sampling interval 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 , thereby solving the nonlinear optimization control expression.

[0067] 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

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

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

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

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

[0072] 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

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

[0074] See also Figure 1 The 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.

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

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

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

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

[0079] 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:

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

[0081] 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 , Rotational mechanism coordinate system , software arm start coordinate system , software arm end coordinate system 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:

[0082] ; (1)

[0083] in, , ; and Respectively represent the coordinate system " "To coordinate system" "The transformation matrix and rotation matrix, symbol and symbol , 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 represents the matrix transpose, Represents the world coordinate system The location of the UAV platform.

[0084] 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 curvature from the actuator space To the soft robot task space The mapping of the reinforcement layer pressure and cavity pressure Isolate, only used to enable reinforcement layer. From the soft arm joint space Go to the software arm configuration space The transformation is expressed as

[0085] ; (2)

[0086] ; (3)

[0087] ; (4)

[0088] in, Indicates The cavity length of the root bending soft body, , and are the curvature, curvature angle, and bending angle of the soft arm, represents the cross-sectional radius, , and are the air pressures of the first, second and third cavities respectively, , and are the xyz positions of the end of the soft arm. To the task space Transformation To model, first, the soft arm revolves around the base coordinate system The y-axis rotation angle ,Right now ; Then, the soft arm moves around the base coordinate system The z-axis rotation Angle, that is ; Then move the software arm out of the xz plane and translate in is the radius of curvature; finally, by right-multiplying the rotation matrix Adjust the pose. Therefore, the transformation matrix It is expressed as follows:

[0089] ; (5)

[0090] ; (6)

[0091] ; (7)

[0092] in, , , and Respectively expressed as , , and ; and Respectively represented as the software arm start coordinate system Translational and rotational motion in .

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

[0094] Specifically, in order to determine the end motion of the soft arm, based on the given end position Establish an inverse kinematics model. The modeling steps are as follows: First, according to geometric calculation, the given end position Convert to 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 shown below:

[0095] ; (8)

[0096] Second, the arc parameters Convert to cavity length At the same time, considering that the three cavities of the soft arm are driven simultaneously, This indicates that the soft arm will perform extension or contraction motion and may cause singularity problems. 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 the geometric calculation, the cavity The initial length of the arc can be determined based on the arc parameters The calculation is as follows:

[0097] ; (9)

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

[0099] Specifically, based on the inverse kinematics of the soft arm based on formulas (8) and (9), given the end position , the corresponding cavity length can be calculated In order to reduce the cavity length Convert to cavity pressure ,Considering the effects of gravity, hysteresis and nonlinearity, a hysteresis model of the soft arm is first established, and then a control method is designed to compensate for the effects of gravity and external disturbances.

[0100] 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:

[0101] ; (10)

[0102] in, is the cavity length, is the actuator pressure in the hysteresis model, where the pressure is given by the symmetric part , asymmetric part and the polynomial part composition to fit the formal curve; is the operator output of the hysteresis model; To enlarge The linear weight gain, , and is the weight gain; For the A dead zone is the input signal range where the output is zero. and The first two are the pressure increase process and the pressure reduction process. Angle of inclination; is the offset related to the working angle of the hysteresis curve; and are the number of symmetric parts and polynomial parts, respectively; 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.

[0103] 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 cavity pressure , which is expressed as follows:

[0104] ; (11)

[0105] ; (12)

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

[0107] ; (13)

[0108] in, , , and are positive numbers, representing the constant term and the term that determines the exponential value; and in order to smoothly adjust the coefficient , 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:

[0109] ; (14)

[0110] Then set an action space containing four actions A1-A4 , adjust the parameters respectively , , and , which is expressed as follows:

[0111] ; (15)

[0112] This shows that in the current state Next, we can look at the action space Select an action , the proportionality coefficient is determined by formula (15) . 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, we use the improved - Greedy strategy to reduce the diversity of action selection and improve convergence speed. -The greedy strategy is defined as follows:

[0113] ; (16)

[0114] Among them, the parameters , is the state action value, which is expressed as follows:

[0115] ; (17)

[0116] in, is the learning rate, 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.

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

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

[0119] ; (18)

[0120] in, is the total mass of the all-wheel drive UAV platform, It is the inertial matrix of the all-wheel drive UAV platform. is the gravitational constant; the vector For vector The first-order derivative of is the linear velocity of the all-wheel drive UAV platform in the world coordinate system; is the angular velocity of the all-wheel drive UAV platform in the body coordinate system, and the vector for The first derivative of ; and They are the control input force and torque for the all-wheel drive UAV platform; and They are the disturbance force and moment acting on the all-wheel drive UAV platform respectively.

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

[0122] ; (19)

[0123] ; (20)

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

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

[0126] ;(twenty one)

[0127] ;(twenty two)

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

[0129] ;(twenty three)

[0130] in, , ,in and are the expected state vector and the expected input vector respectively; and denote the penalty matrices of state and input respectively, represents the penalty matrix for terminal states; represents the prediction step length; and They are state and input constraints respectively; and are the estimated disturbance force and moment, respectively.

[0131] 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) and the state vector of the extended Kalman filter method are and the input vector The definition is as follows:

[0132] ;(twenty four)

[0133] ; (25)

[0134] 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 of the extended Kalman filter. , as shown below:

[0135] ; (26)

[0136] Next, we use the multi-targeting technique and the ACADO toolkit to solve the optimization control problem in Equation (23); the system dynamics and the constraints are discrete time series within the sampling interval 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.

[0137] 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), a rigid-flexible integrated aerial contact operation robot system can be flexibly manipulated in the air in a restricted environment.

[0138] 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 rigid-soft integrated aerial contact working robot, comprising 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 with the outer edge part, and the plane formed by the working arm when rotating 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; It is characterized in that The working arm comprises a proximal rigid arm (1) connected to a rotating mechanism, a distal rigid arm (13) having 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); the soft arm (3) is a bending soft body with a single-section structure.

2. The rigid-flexible aerial contact working robot according to claim 1, characterized in that: The soft arm (3) comprises two connection disks and three soft actuators of the same shape and size and connected in parallel, the two ends of the three soft actuators are respectively fixed on the inner sides of the two connection disks, the outer sides of the two connection 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 connection disk; the soft actuator comprises a hose with a hollow cavity, a vacuum pump and a control unit, 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.

3. A control method for a rigid-flexible aerial contact operation robot, characterized in that: Based on the rigid-flexible integrated aerial contact working robot as claimed in claim 1 or 2, the invention comprises the following steps: 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.

4. The method according to claim 3, characterized in that In step 1, the forward kinematics model expression is: ； in, , ; and Respectively represent the coordinate system" "To Coordinate System" "The transformation matrix and rotation matrix, symbol and Symbol , 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, Represents the world coordinate system The location of the UAV platform.

5. The method according to claim 4, 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, , , , , and Respectively expressed as , , and , is the curvature angle of the soft arm, is the bending angle of the soft arm; and Respectively represented as the software arm start coordinate system The translational and rotational motions in Represents the coordinate system of the soft arm around the base The y-axis rotation angle , Indicates the coordinate system of the start end of the soft arm The z-axis rotation angle , 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.

6. The method according to claim 5, characterized in that The step 2 comprises: Step 201, calculate the end position by the following formula Arc parameters converted to : ； in 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: ； in Indicates The cavity length of the root bending soft body, represents the radius of the cross section and π represents the circumference of a circle.

7. The method according to claim 4, characterized in that In step 3, the hysteresis model is expressed by the following formula: ； in is the cavity length, is the pressure in the cavity of the hysteresis model, where the pressure is expressed by the symmetric part , asymmetric part and the polynomial part To express 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; is the linear weight gain, , and are the weight gains of the symmetric part, asymmetric part, and polynomial part respectively; For the A dead zone, which is the input signal range corresponding to zero output; and They are the first Angle of inclination; is the offset related to the working angle of the hysteresis curve; and are the number of symmetric parts and polynomial parts, respectively; 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, is the actual cavity length, which is calculated by the expected position or actual position of the end of the given soft arm according to the calculation formula in step 201. Then, the constraint conditions for calculating the cavity length of each bending soft body in step 202 are used to obtain; for The first derivative of ; , and are proportional, integral and differential coefficients respectively; the proportional coefficient Set to an exponential function, expressed as ;in is a constant term, , and are terms that determine the exponential value, and are positive numbers, and are determined online by a control algorithm based on reinforcement learning .

8. The method according to claim 7, characterized in that In step 3, setting a control algorithm based on reinforcement learning according to the hysteresis model includes: based on - Greedy strategy to make the soft arm choose the best action through learning, where -The greedy strategy is: ； Among them, rand() represents the initial random assignment for judgment, and the parameter ; is the state action value, expressed as: ； in is the learning rate, 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 。 9. The method according to claim 4, 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: ； in is the total mass of the all-wheel drive UAV platform, It is the inertial matrix of the all-wheel drive UAV platform. is the gravitational constant; the vector For vector The first-order derivative of is the linear velocity of the all-wheel drive UAV platform in the world coordinate system; is the angular velocity of the all-wheel drive UAV platform in the body coordinate system, and the vector for The first derivative of ; and They are the control input force and torque for the all-wheel drive UAV platform; and 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 For vector The first-order derivative of is the position of the all-wheel drive UAV platform in the world coordinate system; vector For vector The first-order derivative of The attitude of the all-wheel drive UAV platform expressed by quaternion.

10. The method according to claim 9, characterized in that In step 4, implementing lateral force constraint on the all-wheel drive UAV platform includes: The state vector and the input vector It is 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, , ,in and are the expected state vector and the expected input vector respectively; and denote the penalty matrices of state and input respectively, represents the penalty matrix for terminal states; represents the prediction step length; 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 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 of the extended Kalman filter. ,Right now: ； Then the system dynamics and the constraints are discrete time series within the sampling interval 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 , thereby solving the nonlinear optimization control expression.

Images