Hovering stability control method of unmanned aerial vehicle with arms based on model-free incremental reinforcement learning

Abstract

The present invention provides a hovering stabilization control method for an arm-carrying unmanned aerial vehicle based on model-free incremental reinforcement learning, including: establishing an unmanned aerial vehicle and a loaded mechanical arm model, establishing a deep reinforcement learning algorithm framework, initializing a neural network strategy, designing a hierarchical reward function, collecting state information of the unmanned aerial vehicle and the loaded mechanical arm, and the strategy neural network outputs a stabilization control instruction according to the collected state information, and performs a weighted superposition with parameters on the control instruction output by the PID controller to obtain a fusion control law, and then inputs the fusion control law into the unmanned aerial vehicle and the loaded mechanical arm model, solves the next state information, calculates the reward value of the state using the next state information, and feeds back to the deep reinforcement learning algorithm framework in combination with the corresponding action, runs a distributed proximal optimization strategy to output a policy gradient, calculates the gradient descent of the policy, and updates the neural network using the gradient descent to complete the update of the policy. The present invention can provide guidance information in real time and improve training efficiency.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle control, and in particular relates to a hovering stability control method for an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning. Background Art

[0002] With the rapid development of automation technology and embedded technology, drone technology has made continuous progress and matured, and its influence at home and abroad is also expanding rapidly. However, the research on aerial drone grasping on moving platforms is still in its infancy. This research direction can be understood as an aerial application, that is, using air-based equipment to complete specific tasks. These air-based equipment include drones and aerial work robots. According to the degree of interaction, current aerial applications can be divided into four levels: no interaction, weak interaction, medium interaction, and strong interaction.

[0003] Strong interactive applications refer to applications that aim to change the state of the environment and have strong interactions of motion and force with the environment. Strong interactive applications include contact detection, installation, and cleaning of high-altitude buildings, as well as aerial drone grabbing of moving platforms. Modern aerial robot technology has become an important measure to enhance the mobility and strike capabilities of aviation. This technology is an enhanced supplement to the new combat mode, which can significantly improve the endurance of fighter jets and plays an extremely important supporting role in strategic or tactical aviation units. Among them, drone technology is becoming an important part of intelligent unmanned warfare with its low-cost and cost-effective combat mode.

[0004] Based on this, a hovering stabilization control method for an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning was proposed. Summary of the invention

[0005] In view of the above-mentioned deficiencies in the prior art, the present invention provides a hovering stability control method for an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning to solve the problems raised in the above-mentioned background technology.

[0006] In order to solve the above technical problems, the technical solution specifically adopted by the present invention is: a hovering stability control method of an armed drone based on model-free incremental reinforcement learning, comprising the following steps:

[0007] S1. Establish the dynamics model of the UAV and the on-board robotic arm, and define the initial state of the UAV and the on-board robotic arm as the current state;

[0008] S2. Establish a deep reinforcement learning algorithm framework, initialize the neural network strategy, design a hierarchical reward function corresponding to the stable hovering of the drone with arms, and start training;

[0009] S3, collect the state information of the UAV and the on-board manipulator, the strategy neural network outputs the stabilization control command according to the collected state information, and performs the weighted superposition with parameters with the control command output by the PID controller to obtain the fusion control law, and then input the fusion control law into the UAV and the on-board manipulator dynamics model, solve the next state information, use the next state information to calculate the reward value of the state, and feed it back to the deep reinforcement learning algorithm framework in combination with the corresponding action, run the distributed proximal optimization strategy to output the policy gradient, calculate the gradient descent of the policy, use the gradient descent to update the neural network, and complete the policy update;

[0010] S4. If the reward value is greater than the set reward threshold, the training is completed and the policy network parameters are saved;

[0011] If not, continue to collect state information, strategy information, and reward information and store them in the experience pool, and repeat step S3 until the reward function meets the standard and the model is stable, and the training is completed.

[0012] As a further illustration of the present invention, in step S1, the process of establishing the UAV dynamics model specifically includes:

[0013] The center of mass translation equation of the UAV is established as follows:

[0014]

[0015] in: is the center of mass translation acceleration of the drone in the three coordinate axes of X, Y, and Z, U1 is the total lift provided by the propeller, m is the mass of the drone, θ, ψ are the roll angle, pitch angle, and yaw angle of the drone respectively; k x , k y , k z They are the components obtained by decomposing the air resistance coefficient under the three coordinate axes of the quadrotor, They are the components of the UAV's flight speed in the X, Y, and Z coordinate axes respectively;

[0016] The rotation equation of the quadcopter of the drone around the center of mass is established as follows:

[0017]

[0018] in: They are the roll angular acceleration, pitch angular acceleration, and yaw angular acceleration of the drone. are the roll angular velocity, pitch angular velocity, and yaw angular velocity of the drone, respectively. u2, u3, and u4 are the virtual control quantities of the roll, pitch, and yaw channels, respectively. x ,I y ,I zare the rotational inertia of the drone in the X, Y, and Z coordinate axes, J RP is the moment of inertia of the propeller, Ω is the algebraic sum of the motor speed, k θ , k ψ are the components of the air resistance torque coefficient under the three coordinate axes of X, Y, and Z, p is the angular velocity around the X axis under the machine system, q is the angular velocity around the Y axis under the machine system, and r is the angular velocity around the Z axis under the machine system;

[0019] Based on the above-mentioned UAV center of mass translation equation and the quadrotor rotation equation around the center of mass, plus external disturbance, the UAV dynamic model is obtained as follows:

[0020]

[0021] Where: x , y , z is the uncertainty disturbance of the three-axis acceleration, ξ θ , ψ is the uncertainty disturbance of the three-axis acceleration, is the three-axis angular acceleration disturbance of the robot arm.

[0022] As a further illustration of the present invention, the process of establishing the center of mass translation equation of the drone is as follows:

[0023] The center of mass translation equation of the UAV is as follows:

[0024]

[0025] Among them: F sum is the total force of the UAV, F n is the rotor lift, F g is the weight of the drone itself, F f is the air resistance, m is the mass of the drone; v is the linear velocity of the drone; t is the time;

[0026] The lift and calculation of the quadrotor are shown in the following formula:

[0027]

[0028] Among them: F b The lift and F of the quadrotor i is the lift provided by a single propeller; K is the lift coefficient, ω i is the propeller speed;

[0029] The virtual control amount of the height channel is shown in the following formula:

[0030]

[0031] F g =[0 0 mg] T

[0032] Where: U1 is the total lift provided by the propeller, is the rotation matrix of the ground-to-aircraft system; the drag force exerted by the air on the drone is proportional to its flight speed, which can be obtained

[0033]

[0034] Where: k x , k y , k z They are the components obtained by decomposing the air resistance coefficient under the three coordinate axes of the quadrotor, They are the components of the UAV's center of mass translation velocity in the X, Y, and Z coordinate axes respectively;

[0035] Combining the above formulas, we get:

[0036]

[0037] Divide both sides of the above equation by m and enter them into the coordinate transformation matrix The center of mass translation equation of the UAV is as follows:

[0038]

[0039] As a further illustration of the present invention, the process of establishing the rotation equation of the quad-rotor of the drone around the center of mass is as follows:

[0040] According to Euler's theorem, the rotation equation about the center of mass is:

[0041]

[0042] Among them, M is the total external torque, I is the moment of inertia, and it only has a value on the diagonal line. The formula is as follows:

[0043]

[0044] The torque sources of the quadrotor drone are: rotor torque M1, gyro torque M2, air resistance torque M3. The expression of the total external torque M is:

[0045] M=M1+M2+M3

[0046]

[0047] M2=ω×J RP

[001] T =J RP[-qp 1] T Ω

[0048]

[0049] in: is the rolling moment, M θ is the pitching moment, M ψ is the yaw moment, k d is the anti-torque coefficient, l is the distance from the center of each rotor of the quadrotor to its center of mass, J RP is the moment of inertia of the propeller, Ω is the algebraic sum of the motor speed, ω x ,ω y ,ω z is the three-axis body angular velocity, p is the roll angular rate, q is the pitch angular rate, and r is the yaw angular rate;

[0050] The expression of air resistance torque is:

[0051]

[0052] in, k θ , k ψ They are the components of the air resistance torque coefficient under the three coordinate axes of X, Y and Z respectively;

[0053] The obtained rotation equation of the quadrotor around the center of mass is:

[0054]

[0055] As a further illustration of the present invention, the process of establishing the dynamic model of the on-board robotic arm specifically includes:

[0056] Based on the inter-joint transformation matrix of the six-axis robot, the forward kinematics equation of the six-axis robot is established as follows:

[0057]

[0058] Among them, T is the homogeneous coordinate transformation matrix from the geographic coordinate system to the end joint coordinate system of the manipulator, R and p are the rotation matrix and actual position from the geographic coordinate system to the end joint coordinate system of the manipulator, respectively, and T wu is the homogeneous transformation matrix from the world coordinate system to the drone coordinate system, T ur is the homogeneous transformation matrix from the drone coordinate system to the robotic arm base coordinate system, T1 T2 T3T4T5 T6 is the homogeneous transformation matrix between the joints of the connecting rod robotic arm, θ i is the rotation angle of each joint;

[0059] Based on the analytical Jacobian matrix and the geometric Jacobian matrix, the inverse kinematics equation of the robot arm is calculated using the Newton iteration method. The specific inverse kinematics equation is as follows:

[0060]

[0061] Its recursive formula is:

[0062]

[0063] in, is the angular velocity of the robot joint, J A (q) is the analytical Jacobian matrix of the joint angle q, v e is the derivative of the terminal state vector, q t is the joint angle of the robot arm at time t, is the differential motion of the robot arm;

[0064] From the above forward and inverse kinematics formulas of the six-axis robot, the dynamic model of the six-axis robot can be obtained as follows:

[0065]

[0066] Among them, J A (q) is the analytical Jacobian matrix of the joint angle q, J(q) is the geometric Jacobian matrix of the joint angle q, and v e is the derivative of the terminal state vector, T A is the analytical homogeneous matrix at the joint angle q, J A (q t ) is the analytical Jacobian matrix of the joint angle q at time t.

[0067] As a further illustration of the present invention, the process of establishing the forward kinematics equation of the six-axis robot arm is specifically as follows:

[0068] The inter-joint transformation matrix of the six-axis robot is:

[0069]

[0070] Among them, θ i is the rotation angle of each joint, a i is the x of the front link coordinate system i Axis and the x of the next coordinate system i+1 The distance between the axes, d i is the x of the front link coordinate system i Axis and the x of the next coordinate system i+1 The vertical distance between the axes, α i is the z coordinate system of the front link i Axis and the z of the next coordinate system i+1 The torsion angle between the axes;

[0071] The forward kinematics equation of the six-axis robot is:

[0072]

[0073] As a further illustration of the present invention, the inverse kinematics equation establishment process of the six-axis robot arm is specifically as follows:

[0074] The geometric Jacobian matrix J(q) of the robot arm is as follows:

[0075]

[0076] Among them, v e is the derivative of the terminal state vector, v p is the linear velocity of the end of the robot arm, w is the angular velocity of the end of the robot arm, q, are the joint angle and angular velocity of the robot arm respectively;

[0077] Analytical Jacobian matrix J of the robot arm A (q) as follows:

[0078]

[0079] Among them, v E is the derivative of the Euler angle with respect to time;

[0080] Euler angles at the end of the robotic arm is a function of the joint angle q,

[0081] in, They are the three-axis Euler angles of the end joint of the robot arm, namely the roll angle, pitch angle and yaw angle. According to the chain derivation rule, the Euler angle is directly differentiated with respect to time to obtain the analytical Jacobian matrix;

[0082] The relationship between the analytical Jacobian matrix and the geometric Jacobian matrix is:

[0083] J(q)=T A J A (q)

[0084]

[0085] Where J(q) is the geometric Jacobian matrix of the joint angle q, J A (q) is the analytical Jacobian matrix of the joint angle q, T A is the analytical homogeneous matrix under the joint angle q state, I is the unit matrix, T(E) is the conversion matrix from the terminal posture parameter derivative to the angular velocity, s1, c1, s2, c2 are

[0086]

[0087] Using Newton's iteration method to calculate the inverse kinematics of the robot arm, we first establish the objective function F(q) that needs to be solved. The objective function is:

[0088]

[0089] Among them, P g is the desired position and Euler angle vector, P n is the actual position and Euler angle vector at angle q, are the expected components of the end of the robot arm on the X, Y, and Z axes in the world coordinate system. are the desired roll, pitch and yaw angles of the end of the robot arm in the world coordinate system, The actual components of the end of the robot arm on the X, Y, and Z axes in the world coordinate system. It is the actual roll angle, pitch angle and yaw angle of the end of the robot arm in the world coordinate system;

[0090] The Newton-Euler iteration formula at time t is:

[0091] F(q t )+F′(q t )(q t+1 -q t )=0

[0092] F′(q t )=J A (q t )

[0093] After transformation, the iterative formula is expressed as:

[0094] J A (q t )(q t+1 -q t )=-F(q t )

[0095] When the degree of freedom of the manipulator is equal to the number of rows of the Jacobian matrix, and the Jacobian matrix is ​​full rank, q t+1 The calculation formula is as follows:

[0096] q t+1 =q t -J A (q t ) -1 F(q t )

[0097] When the degree of freedom of the manipulator is greater than the number of rows in the Jacobian matrix, q t+1 The calculation formula is as follows:

[0098] J A(q t ) + =(J A (q t ) T J A (q t )) -1 J A (q t ) T

[0099] In the above formula, J A (q t ) + represents the pseudo-inverse of the Jacobian matrix;

[0100] When the degree of freedom of the robot is less than the number of rows of the Jacobian matrix, the above linear equations have an infinite number of solutions. t+1 The calculation formula is as follows:

[0101] q t+1 =q t -J A (q t ) + F(q t )+(IJ A (q t ) + J A (q t ))k

[0102] J A (q t ) + =J A (q t ) T (J A (q t )J A (q t ) T ) -1

[0103] Where I is the identity matrix, k is any n-dimensional vector; (IJ A (q t ) + J A (q t ))k represents all vectors in the null space of the Jacobian matrix;

[0104] The process of solving inverse kinematics based on the Jacobian matrix is ​​as follows:

[0105] formula Multiply both ends on the left by J(q) -1 After:

[0106]

[0107] The above formula is the alternative formula of the alternative inverse solution method, where is the differential motion of the robot arm.

[0108] As a further illustration of the present invention, the hierarchical reward function corresponding to the stable hovering of the drone with arms in S2 is as follows:

[0109] The drone position reward and penalty function is as follows:

[0110]

[0111] Among them, p ue , v ue are the expected position and expected velocity, respectively, u , v are the actual position and actual speed of the drone, r p , r v They are the drone position reward and speed reward respectively;

[0112] The drone attitude reward and penalty function is as follows:

[0113]

[0114] φ, θ, ψ are the actual roll angle, pitch angle and yaw angle of the UAV respectively, r φ , r θ , r ψ They are the drone roll angle reward, pitch angle reward and yaw angle reward;

[0115] The reward and penalty function of the robot arm end position is as follows:

[0116]

[0117] Among them, T i is the homogeneous coordinate transformation matrix of each joint of the serial manipulator, T wu , T ur are the homogeneous coordinate transformation matrices from the geographic coordinate system to the UAV coordinate system and from the UAV coordinate system to the robotic arm coordinate system, T is the homogeneous coordinate transformation matrix from the geographic coordinate system to the robotic arm end joint coordinate system, R, p are the rotation matrix and actual position from the geographic coordinate system to the robotic arm end joint coordinate system, r arm , p e_arm They are the robot arm reward and the expected position of the robot arm end.

[0118] As a further explanation of the present invention, in S3, the state information of the drone and the onboard mechanical arm is collected, and the strategy neural network outputs a stabilization control instruction according to the collected state information, and the control instruction output by the PID controller is weighted and superimposed with parameters to obtain the fusion control law. Specifically, the process is as follows:

[0119] The arm-carrying drone interacts with the environment, samples and collects training state quantity samples, inputs them into the strategy neural network and calculates the stabilization control instructions of the control strategy, performs weighted superposition with parameters on the control instructions output by the PID controller, and selects the highest probability weighted control instruction from the multiple instructions obtained by cross-fusion as the final instruction for suppressing electromagnetic interference;

[0120] The specific fusion control law is as follows:

[0121] Introducing the deviation weight parameter ε, in the control command fusion process, the following variable weight process is followed, where p ue is the expected position, and p is the actual position:

[0122]

[0123] x=||p ue -p||

[0124] The final control law is expressed as:

[0125]

[0126] Among them, A t is the action set output by the neural network strategy, U t is the set of control laws output by the artificially designed PID controller, C t is the set of fusion control laws, c t It is the control law with the highest probability selected from the fusion control law set at time t.

[0127] As a further illustration of the present invention, in step 3, the updating process of the neural network is as follows, based on importance sampling;

[0128]

[0129] Transform the loss function gradient of the previous strategy:

[0130]

[0131] Among them, π θ (a t |s t ), are the new and old strategies of the neural network, For strategy π θThe expected cumulative return gradient under For strategy The expected cumulative return under t ,a t ) is the advantage function;

[0132] By formula Infer the objective function from the gradient:

[0133]

[0134] We can get:

[0135]

[0136] Among them, s t and a t are the state quantity and action of the strategy output of the Markov process at time t, θ is the current neural network strategy parameter, θ old is the neural network strategy parameter of the previous round, J(θ) and They are the current loss function and the loss function under the old strategy respectively;

[0137] A regular term is added to the loss function, and an adaptive KL divergence method is introduced to control the change of β, which is expressed as:

[0138]

[0139] Set the upper and lower limits of the KL divergence value. If the KL divergence term of the loss function during training is greater than the upper limit, it means that the penalty term βKL(θ,θ old ) has no effect, increasing β;

[0140] During the training process, β changes to:

[0141] if KL(θ,θ old )>KL max ,β↑

[0142] if KL(θ,θ old )<KL min ,β↓

[0143] Among them, KL(θ,θ old ) is the KL divergence value between the new and old strategies, and β is the hyperparameter of the KL divergence penalty term in the loss function;

[0144] After all samples have completed the gradient propagation, the global gradient is used to update the model parameters.

[0145] Compared with the prior art, the present invention has the following advantages:

[0146] When used, the control instruction fusion processing method based on neural network in the present invention is combined with traditional PID control and intelligent control methods, and the defects of artificial inaccurate modeling and non-performance optimal control design are utilized, as well as the precise understanding of the model by deep reinforcement learning. The control laws can be combined to avoid the strategy network from falling into local optimality and being unable to train effectively in the early stage of training; the state of the mechanical arm is changed under different flight states, and multiple groups of data are collected to comprehensively update the action strategy of the UAV. The reward and punishment mechanism set is more in line with the thinking of human beings to explore and solve problems, and guidance information can be given in real time during the learning process of the intelligent agent. This method can improve the training efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0147] Figure 1 is a flow chart of incremental reinforcement learning control instruction fusion in an embodiment of the present invention;

[0148] Figure 2 It is a flow chart of stable control training based on model-free incremental reinforcement learning in an embodiment of the present invention;

[0149] Figure 3 It is a simulation diagram of position PID and instruction fusion control in an embodiment of the present invention;

[0150] Figure 4 It is a simulation diagram of speed PID and instruction fusion control in an embodiment of the present invention;

[0151] Figure 5 It is a simulation diagram of attitude PID and instruction fusion control in an embodiment of the present invention;

[0152] Figure 6 It is a simulation diagram of angular velocity PID and instruction fusion control in an embodiment of the present invention. DETAILED DESCRIPTION

[0153] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0154] like Figure 2 As shown, the present invention provides a hovering stability control method for an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning, comprising the following steps:

[0155] S1. Establish the dynamics model of the UAV and the on-board robotic arm, and define the initial state of the UAV and the on-board robotic arm as the current state;

[0156] Specifically, in step S1, the process of establishing the UAV dynamics model specifically includes:

[0157] S11. Establish the center of mass translation equation of the UAV:

[0158] The center of mass translation equation of the UAV is as follows:

[0159]

[0160] Among them: F sum is the total force of the UAV, F n is the rotor lift, F g is the weight of the drone itself, F f is the air resistance, m is the mass of the drone; v is the linear velocity of the drone; t is the time;

[0161] Since the lift of the quadrotor is generated by the motor, the magnitude of the force is proportional to the square of the motor speed, as shown below:

[0162]

[0163] Therefore, the lift and calculation of the quadrotor are shown as follows:

[0164]

[0165] Among them: F b The lift and F of the quadrotor i is the lift provided by a single propeller; K is the lift coefficient, ω i is the propeller speed;

[0166] Since the height is controlled by the total lift of the quadrotor, the virtual control amount of the height channel can be obtained from the above formula, which can be defined as U1 as follows:

[0167]

[0168] Then transform U1 into the ground coordinate system to obtain the lift and F of the quadrotor n :

[0169]

[0170] Since gravity is always downward in the ground coordinate system, the drone’s own weight F is obtained g The formula is as follows:

[0171] F g =[00mg] T

[0172] The resistance of the air on the drone is proportional to its flight speed, which can be obtained

[0173]

[0174] Where: k x , k y , k z They are the components obtained by decomposing the air resistance coefficient under the three coordinate axes of the quadrotor, They are the components of the UAV's center of mass translation velocity in the X, Y, and Z coordinate axes respectively;

[0175] Combining the above formulas, we get:

[0176]

[0177] Divide both sides of the above equation by m and enter them into the coordinate transformation matrix The center of mass translation equation of the UAV is as follows:

[0178]

[0179] in: is the center of mass translation acceleration of the drone in the three coordinate axes of X, Y, and Z, U1 is the total lift provided by the propeller, m is the mass of the drone, θ, ψ are the roll angle, pitch angle, and yaw angle of the drone respectively; k x , k y , k z They are the components obtained by decomposing the air resistance coefficient under the three coordinate axes of the quadrotor, They are the components of the UAV's flight speed in the X, Y, and Z coordinate axes respectively;

[0180] S12. Establish the rotation equation of the quadcopter of the drone around the center of mass:

[0181] According to Euler's theorem, the rotation equation about the center of mass is:

[0182]

[0183] Among them, M is the total external torque, I is the moment of inertia, and it only has a value on the diagonal line. The formula is as follows:

[0184]

[0185] The torque sources of the quadrotor drone are: rotor torque M1, gyro torque M2, air resistance torque M3. The expression of the total external torque M is:

[0186] M=M1+M2+M3

[0187] Among the rotor moments, the rolling moment and pitching moment M θThe yaw moment M is caused by the unequal rotation speeds of the two motors on the same shaft, which results in a lift difference on the shaft, and then the lift acts on the rotor. ψ It is generated by excess anti-torque, that is, there is a deviation in the anti-torque of the two rotors on the fuselage on the X-axis and Y-axis. In this way, while ensuring that the total lift of the quadrotor remains unchanged, the total anti-torque is not 0, thereby causing the quadrotor to rotate around the Z-axis.

[0188]

[0189] in, is the rolling moment, M θ is the pitching moment, M ψ is the yaw moment, u2, u3, u4 are the virtual control quantities of the roll, pitch and yaw channels respectively, k d is the anti-torque coefficient, and l is the distance from the center of each rotor of the quadrotor to its center of mass.

[0190] According to the law of angular momentum, when a quadrotor makes a tilted roll or pitch motion, the angular velocity of its axis will change. At this time, the motor will apply a torque to the rotor, called gyroscopic torque, which will force the body to rotate. It should be noted that gyroscopic torque does not exist during the hovering or yaw motion of the quadrotor. The gyroscopic torque formula is as follows:

[0191] M2=ω×J RP

[001] T =J RP [-qp 1] T Ω

[0192] Among them: J RP is the moment of inertia of the propeller, and Ω is the algebraic sum of the motor speeds.

[0193] The air resistance torque is formed by the air resistance acting on the rotor, and its components M in the three coordinate axes are 3x , M 3y , M 3z and the body angular velocity ω x ,ω y ,ω z Since the quadrotor usually flies at a smaller attitude angle, ω x ,ω y ,ω z It is approximately the Euler angular velocity vector, that is:

[0194]

[0195] ω x ,ω y ,ω zis the three-axis body angular velocity, p is the roll angular rate, q is the pitch angular rate, and r is the yaw angular rate.

[0196] The expression of air resistance torque is:

[0197]

[0198] in, k θ , k ψ They are the components of the air resistance torque coefficient on the X, Y and Z coordinate axes respectively.

[0199] In summary, the rotation equation of the quadrotor around the center of mass is:

[0200]

[0201] in: They are the roll angular acceleration, pitch angular acceleration, and yaw angular acceleration of the drone. are the roll angular velocity, pitch angular velocity, and yaw angular velocity of the drone, respectively. u2, u3, and u4 are the virtual control quantities of the roll, pitch, and yaw channels, respectively. x ,I y ,I z are the rotational inertia of the drone in the X, Y, and Z coordinate axes, J RP is the moment of inertia of the propeller, Ω is the algebraic sum of the motor speed, k θ , k ψ are the components of the air resistance torque coefficient under the three coordinate axes of X, Y, and Z, p is the angular velocity around the X axis of the machine system, q is the angular velocity around the Y axis of the machine system, and r is the angular velocity around the Z axis of the machine system.

[0202] S13. Based on the above-mentioned UAV center of mass translation equation and the quadrotor rotation equation around the center of mass, plus external disturbance, the UAV dynamics model is obtained as follows:

[0203]

[0204] Where: x , y , z is the uncertainty disturbance of the three-axis acceleration, ξ θ , ψ is the uncertainty disturbance of the three-axis acceleration, is the three-axis angular acceleration disturbance of the robot arm.

[0205] Specifically, in step S1, the process of establishing the dynamic model of the loaded robot arm specifically includes:

[0206] S14. Based on the inter-joint transformation matrix of the six-axis robot, the forward kinematics equation of the six-axis robot is established:

[0207] The six-axis robot arm modeling is based on the link coordinate system. The DH parameter method is a robot kinematic modeling method. A set of parameters is used to define the position and direction of each link relative to the previous link. Therefore, the inter-joint transformation matrix of the six-axis robot arm is:

[0208]

[0209] Among them, θ i is the rotation angle of each joint, a i is the x of the front link coordinate system i Axis and the x of the next coordinate system i+1 The distance between the axes, d i is the x of the front link coordinate system i Axis and the x of the next coordinate system i+1 The vertical distance between the axes, α i is the z coordinate system of the front link i Axis and the z of the next coordinate system i+1 The torsion angle between the axes;

[0210] The forward kinematics equation of the six-axis robot is:

[0211]

[0212] Among them, T is the homogeneous coordinate transformation matrix from the geographic coordinate system to the end joint coordinate system of the manipulator, R and p are the rotation matrix and actual position from the geographic coordinate system to the end joint coordinate system of the manipulator, respectively, and T wu is the homogeneous transformation matrix from the world coordinate system to the drone coordinate system, T ur is the homogeneous transformation matrix from the drone coordinate system to the robotic arm base coordinate system, T1 T2 T3T4T5 T6 is the homogeneous transformation matrix between the joints of the connecting rod robotic arm, θ i The rotation angle of each joint.

[0213] S15. Based on the analytical Jacobian matrix and the geometric Jacobian matrix, the inverse kinematics equation of the robot arm is calculated using the Newton iteration method:

[0214] The Jacobian matrix of the robot can be divided into two categories: geometric Jacobian matrix and analytical Jacobian matrix. The geometric Jacobian matrix J(q) establishes the relationship between the angular velocity of the robot joint and the terminal velocity and angular velocity, that is:

[0215]

[0216] Among them, v e is the derivative of the terminal state vector, vp is the linear velocity of the end of the robot arm, w is the angular velocity of the end of the robot arm, q, are the joint angles and angular velocities of the robot arm, respectively.

[0217] Since the integral of the angular velocity w of the end of the robot arm has no clear physical meaning, we often need to establish the relationship between the derivatives of the Euler angles, rotation matrices, quaternions, etc. at the end of the robot arm and the angular velocity of the joint in practical applications. This is to analyze the Jacobian matrix J A The role of (q).

[0218] Analytical Jacobian matrix J of the robot arm A (q) as follows:

[0219]

[0220] In the above formula, v E represents the derivative of Euler angles, rotation matrices, quaternions, etc. with respect to time. In the embodiment of the present invention, v E This is explained by taking the derivative of the Euler angle with respect to time as an example.

[0221] Taking the Euler angle as an example, the Euler angle at the end of the robotic arm is a function of the joint angle q. According to the chain rule, it can be directly differentiated with respect to time to obtain the analytical Jacobian matrix. A simpler way to calculate the analytical Jacobian matrix is ​​to establish the relationship between the analytical Jacobian matrix and the geometric Jacobian matrix, and the geometric Jacobian matrix of the robot arm is easy to calculate.

[0222] in, They are the three-axis Euler angles of the end joint of the robotic arm, namely the roll angle, pitch angle and yaw angle.

[0223] Taking the PRY angle as an example, the relationship between the Jacobian matrix and the geometric Jacobian matrix is:

[0224] J(q)=T A J A (q)

[0225]

[0226] Where J(q) is the geometric Jacobian matrix of the joint angle q, J A (q) is the analytical Jacobian matrix of the joint angle q, T A is the analytical homogeneous matrix under the joint angle q state, I is the unit matrix, T(E) is the conversion matrix from the terminal posture parameter derivative to the angular velocity, s1, c1, s2, c2 are

[0227]

[0228] It can be seen that the transformation between the analytical Jacobian matrix and the geometric Jacobian matrix is ​​related to the current end posture of the robot arm.

[0229] Next, the Newton iteration method is used to calculate the inverse kinematics of the robot arm. First, the objective function F(q) that needs to be solved is established. Taking the PRY Euler angle as an example, the objective function is:

[0230]

[0231] Among them, P g is the desired position and Euler angle vector, P n is the actual position and Euler angle vector at angle q, are the expected components of the end of the robot arm on the X, Y, and Z axes in the world coordinate system. are the desired roll, pitch and yaw angles of the end of the robot arm in the world coordinate system, The actual components of the end of the robot arm on the X, Y, and Z axes in the world coordinate system. It is the actual roll angle, pitch angle and yaw angle of the end of the robot arm in the world coordinate system;

[0232] The Newton-Euler iteration formula at time t is:

[0233] F(q t )+F′(q t )(q t+1 -q t )=0

[0234] F′(q t )=J A (q t )

[0235] After transformation, the iterative formula is expressed as:

[0236] J A (q t )(q t+1 -q t )=-F(q t )

[0237] Calculate q t+1 The process is equivalent to solving a linear equation system. When the posture is expressed by Euler angles, J A (q t )∈R 6 ×n , when the rotation matrix is ​​used to represent the posture J A (q t )∈R 12×n , n represents the degree of freedom of the robot arm.

[0238] When the degree of freedom of the manipulator is equal to the number of rows of the Jacobian matrix, and the Jacobian matrix is ​​full rank, q t+1 The calculation formula is as follows:

[0239] q t+1 =q t -J A (q t ) -1 F(q t )

[0240] When the degree of freedom of the manipulator is greater than the number of rows in the Jacobian matrix, q t+1 The calculation formula is as follows:

[0241] J A (q t ) + =(J A (q t ) T J A (q t )) -1 J A (q t ) T

[0242] In the above formula, J A (q t ) + represents the pseudo-inverse of the Jacobian matrix. When the degree of freedom of the robot is less than the number of rows of the Jacobian matrix, the above linear equations have an infinite number of solutions. At this time, q t+1 The calculation formula is as follows:

[0243] q t+1 =q t -J A (q t ) + F(q t )+(IJ A (q t ) + J A (q t ))k

[0244] J A (q t ) + =J A (q t ) T (J A (q t )J A (q t ) T ) -1

[0245] Where I is the identity matrix, k is any n-dimensional vector; (IJ A (q t ) + J A (q t ))k represents all vectors in the null space of the Jacobian matrix. We can optimize the solved joint vectors based on k.

[0246] The process of solving inverse kinematics based on the Jacobian matrix is ​​as follows:

[0247] formula Multiply both ends on the left by J(q) -1 After:

[0248]

[0249] The above formula is the alternative formula of the alternative inverse solution method, where: is the angular velocity of the robot joint, J A (q) is the analytical Jacobian matrix of the joint angle q, v e is the derivative of the terminal state vector, q t is the joint angle of the robot arm at time t, is the differential motion of the robot arm.

[0250] Specifically, the process of solving inverse kinematics based on the Jacobian matrix includes the following steps:

[0251] Step 1: Given the position and posture of the end flange relative to the base coordinate system, convert the position and posture into a homogeneous matrix T new ;

[0252] Step 2: Set the initial iteration number k = 1 and the initial joint angle θ k =[0 0 0 0 90° 0] T ;

[0253] Step 3: Calculate T from 0 positive kinematics cur,k ;

[0254] Step 4: Replace T in formula (3-6-7) with Teur. to obtain Δ k ;

[0255] Step 5: By Δ k Extract the differential motion of the end flange and store θ k , and then calculate θ k+1 ;

[0256] Step 6: By θ k+1 Positive kinematics calculation T cur,k+1 , compared with T cur,k+1 With T newThe difference between them is lower than the set threshold and the output is θ k+1 , otherwise, k=k+1, return to step 3.

[0257] S16. Based on the forward and inverse kinematics formulas of the above six-axis robot arm, the dynamic model of the six-axis robot arm can be obtained as follows:

[0258]

[0259] Among them, J A (q) is the analytical Jacobian matrix of the joint angle q, J(q) is the geometric Jacobian matrix of the joint angle q, and v e is the derivative of the terminal state vector, T A is the analytical homogeneous matrix at the joint angle q, J A (q t ) is the analytical Jacobian matrix of the joint angle q at time t.

[0260] S2. Establish a deep reinforcement learning algorithm framework, initialize the neural network strategy, design a hierarchical reward function corresponding to the stable hovering of the drone with arms, and start training;

[0261] Specifically, the hierarchical reward function corresponding to the stable hovering of the drone with arms in S2 is as follows:

[0262] 1. UAV position reward and penalty function: In the process of achieving spatial positioning, the expected position error and speed error tend to 0. The design is based on the error allowed by the actual task. When the position error is ≤0.1, the positioning requirement can be met. Therefore, when designing the reward function value, a positive reward will be given when the position error is between -0.1m and 0.1m, and the speed error is between -0.2m / s and 0.2m / s. When it is not in this range, a negative reward will be given. Combined with the logarithmic function above, the reward function is designed as follows:

[0263]

[0264] Among them, p ue , v ue are the expected position and expected velocity, respectively, u , v are the actual position and actual speed of the drone, r p , r v They are drone position reward and speed reward respectively.

[0265] 2. UAV attitude reward and penalty function. When designing the space fixed capital, certain rewards and penalties are given to the angle. At the same time, the role of angular acceleration is also taken into account. When the angle does not change, the angular velocity should remain at 0. Therefore, the reward function is designed as follows:

[0266]

[0267] φ, θ, ψ are the actual roll angle, pitch angle and yaw angle of the UAV respectively, r φ , r θ , r ψ They are the drone's roll angle reward, pitch angle reward, and yaw angle reward.

[0268] 3. The reward and penalty function of the end-arm posture. When designing spatial positioning and capital determination, the stability of the drone can be used as its own stability information, and the stability information of the end-arm posture can also be used as the basis for the control strategy to judge stability. Therefore, the reward function is designed as follows:

[0269]

[0270] Among them, T i is the homogeneous coordinate transformation matrix of each joint of the serial manipulator, T wu , T ur are the homogeneous coordinate transformation matrices from the geographic coordinate system to the UAV coordinate system and from the UAV coordinate system to the robotic arm coordinate system, T is the homogeneous coordinate transformation matrix from the geographic coordinate system to the robotic arm end joint coordinate system, R, p are the rotation matrix and actual position from the geographic coordinate system to the robotic arm end joint coordinate system, r arm , p e_arm They are the robot arm reward and the expected position of the robot arm end.

[0271] Furthermore, the relevant reward functions of the UAV and the robotic arm are shown above, including the UAV position reward and penalty function and attitude reward and penalty function, the robotic arm end joint Cartesian coordinate reward and penalty function and the end joint Euler attitude reward and penalty function; in the process of realizing the stability of the UAV in the specified Cartesian space position, the expected position error and velocity error tend to zero for the position reward and penalty function, which is designed according to the error allowed by the actual task; for the attitude reward and penalty function, the three-axis Euler angle and the three-axis angular velocity also tend to zero; the robotic arm end joint Cartesian coordinate reward and penalty function means that the difference between the end joint of the multi-joint chain robotic arm and the specified position in the Cartesian coordinate system always tends to zero; the robotic arm end joint Euler attitude reward and penalty function means that the difference between the end joint of the multi-joint chain robotic arm and the expected attitude in the Cartesian coordinate system always tends to zero.

[0272] S3. Collect the state information of the UAV and the on-board robotic arm. The strategy neural network outputs the stabilization control command according to the collected state information, and performs the weighted superposition with parameters with the control command output by the PID controller to obtain the fusion control law. The fusion control law is then input into the UAV and on-board robotic arm dynamics model, and the next state information is solved. The reward value of the state is calculated using the next state information, and the corresponding action is fed back to the deep reinforcement learning algorithm framework. The distributed proximal optimization strategy is run to output the policy gradient, and the gradient descent of the policy is calculated. The neural network is updated using the gradient descent to complete the policy update.

[0273] Specifically, Figure 1 As shown in Figure 1, in S3, the state information of the UAV and the onboard manipulator is collected, and the strategy neural network outputs the stabilization control command according to the collected state information, and the control command output by the PID controller is weighted and superimposed with parameters to obtain the fusion control law. Specifically, the process is as follows:

[0274] The UAV with arms interacts with the environment, samples and collects training state samples, inputs them into the strategy neural network and calculates the stabilization control instructions of the control strategy, performs weighted superposition with parameters on the control instructions output by the PID controller, and selects the highest probability weighted control instruction from the multiple instructions obtained by cross-fusion as the final instruction for suppressing electromagnetic interference;

[0275] The specific fusion control law is as follows:

[0276] Introducing the deviation weight parameter ε, in the control command fusion process, the following variable weight process is followed, where p ue is the expected position, and p is the actual position:

[0277]

[0278] x=||p ue -p||

[0279] The final control law is expressed as:

[0280]

[0281] Among them, A t is the action set output by the neural network strategy, U t is the set of control laws output by the artificially designed PID controller, C t is the set of fusion control laws, c t It is the control law with the highest probability selected from the fusion control law set at time t.

[0282] Different from the traditional control command controlled by a single controller, which will result in high artificial design cost, complex control law, poor robustness, etc., as well as slow training time for a single strategy, falling into local optimality in the early stage, and being unable to train, the fusion control command provided by the present invention introduces a deviation weight parameter ε, that is, in the process of control command fusion, depending on the state of the drone and the specified posture state, since the neural network is a stabilization process, it has a small effect in the movement of the drone, so the weight is small, anyway, its weight is large at the spatial vertex. Since the Complementary Log Log activation function has the potential to replace the classic Sigmoid activation function. This function saturates faster, and the zero point value is higher than 0.5, so it is suitable for rapidly increasing the stabilization command when approaching the target point.

[0283] In step 3, the update process of the neural network is as follows:

[0284] Based on importance sampling;

[0285]

[0286] Transform the loss function gradient of the previous strategy:

[0287]

[0288] Among them, π θ (a t |s t ), are the new and old strategies of the neural network, For strategy π θ The expected cumulative return gradient under For strategy The expected cumulative return under t ,a t ) is the advantage function;

[0289] By formula Infer the objective function from the gradient:

[0290]

[0291] We can get:

[0292]

[0293] Among them, s t and a t are the state quantity and action of the strategy output of the Markov process at time t, θ is the current neural network strategy parameter, θ old is the neural network strategy parameter of the previous round, J(θ) and are the current loss function and the loss function under the old strategy respectively.

[0294] In order to limit the distribution difference between the learning strategy and the sampling strategy, a regularization term needs to be added to the loss function. There are usually two schemes for proximal strategy optimization algorithms, the KL divergence scheme and the CLIP cutting scheme. The KL divergence method used in the algorithm of the present invention shows good results in the learning process.

[0295] This method introduces an adaptive KL divergence method to control the change of β. It is expressed as:

[0296]

[0297] Set an upper and lower limit for the KL divergence value. If the KL divergence term of the loss function during training is greater than the upper limit, it means that the penalty term βKL(θ,θ old ) has no effect, increasing β; vice versa.

[0298] During the training process, β changes to:

[0299] if KL(θ,θ old )>KL max ,β↑

[0300] if KL(θ,θ old )<KL min ,β↓

[0301] Among them, KL(θ,θ old ) is the KL divergence value between the new and old strategies, and β is the hyperparameter of the KL divergence penalty term in the loss function.

[0302] It should also be noted that after all samples have completed the gradient transmission, the global gradient is used to update the model parameters. This process will continue multiple times to make full use of the currently collected data and use the generalization of the neural network to enable the model to fully understand the data.

[0303] S4. If the reward value is greater than the set reward threshold, the training is completed and the policy network parameters are saved;

[0304] If not, continue to collect state information, strategy information, and reward information and store them in the experience pool, and repeat step S3 until the reward function meets the standard and the model is stable, and the training is completed.

[0305] The simulation results of the above-mentioned method for hovering stability control of an armed drone based on model-free incremental reinforcement learning provided by the present invention are as follows: Figure 3-Figure 6As shown; in the figure, pid and pid_rl respectively represent the control effect curves of the PID controller and the fusion control law provided by the present invention on the hovering of the UAV with arms. As can be seen from the figure, compared with the method based on traditional PID control, the position change of the hovering UAV is significantly smaller in the control method based on reinforcement learning instructions fusion control, the angle of attack change curve of the UAV is smoother, and the speed change is also smoother, indicating that the attitude of the UAV is more stable, indicating that this method can better control the throttle of the UAV for stable hovering. This shows that the fusion control method based on reinforcement learning has obvious advantages in improving the attitude stability and hovering stability of the UAV.

[0306] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0307] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A hovering stabilization control method for an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning, characterized by: The following steps are involved: S1. Establish the dynamics model of the UAV and the on-board robotic arm, and define the initial state of the UAV and the on-board robotic arm as the current state; S2. Establish a deep reinforcement learning algorithm framework, initialize the neural network strategy, design a hierarchical reward function corresponding to the stable hovering of the drone with arms, and start training; S3, collect the state information of the UAV and the on-board manipulator, the strategy neural network outputs the stabilization control command according to the collected state information, and performs the weighted superposition with parameters with the control command output by the PID controller to obtain the fusion control law, and then input the fusion control law into the UAV and the on-board manipulator dynamics model, solve the next state information, use the next state information to calculate the reward value of the state, and feed it back to the deep reinforcement learning algorithm framework in combination with the corresponding action, run the distributed proximal optimization strategy to output the policy gradient, calculate the gradient descent of the policy, use the gradient descent to update the neural network, and complete the policy update; The process of obtaining the fusion control law is as follows: the UAV with arms interacts with the environment, samples and collects training state quantity samples, inputs them into the strategy neural network and calculates the stabilization control instructions of the control strategy, performs weighted superposition with parameters on the control instructions output by the PID controller, and selects the highest probability weighted control instruction from the multiple instructions obtained by cross-fusion as the final instruction for suppressing electromagnetic interference; The specific fusion control law is as follows: Introducing bias weight parameters , in the control command fusion process, the following variable weight process is followed, where is the expected position, For the actual location: ； The final control law is expressed as: ； in, is the action set output by the neural network strategy, The set of control laws output by the artificially designed PID controller, is the set of fused control laws, for The control law with the highest probability is selected from the set of control laws at each moment; S4. If the reward value is greater than the set reward threshold, the training is completed and the policy network parameters are saved; If not, continue to collect state information, strategy information, and reward information and store them in the experience pool, and repeat step S3 until the reward function meets the standard and the model is stable, and the training is completed.

2. The hovering stability control method of an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning according to claim 1 is characterized in that: In step S1, the process of establishing the UAV dynamics model specifically includes: The center of mass translation equation of the UAV is established as follows: ； in: , , is the center of mass translation acceleration of the drone in the three coordinate axes of X, Y, and Z, The total lift provided by the propeller, For drone quality, , , They are the roll angle, pitch angle, and yaw angle of the drone respectively; , , They are the components obtained by decomposing the air resistance coefficient under the three coordinate axes of the quadrotor, , , They are the components of the UAV's flight speed in the X, Y, and Z coordinate axes respectively; The rotation equation of the quadcopter of the drone around the center of mass is established as follows: ； in: , , They are the roll angular acceleration, pitch angular acceleration, and yaw angular acceleration of the drone. , , They are the roll angular velocity, pitch angular velocity, and yaw angular velocity of the drone, , , are the virtual control quantities of the roll, pitch and yaw channels respectively, , , are the rotational inertia of the drone in the X, Y, and Z coordinate axes, is the moment of inertia of the propeller, is the algebraic sum of the motor speed, , , They are the components of the air resistance torque coefficient on the X, Y, and Z axes respectively. is the angular velocity of the machine system around the X axis, is the angular velocity around the Y axis of the machine system, is the angular velocity around the Z axis of the machine system; Based on the above-mentioned UAV's center of mass translation equation and the quadrotor's rotation equation around the center of mass, plus external disturbances, the UAV's dynamic model is obtained as follows: ； in: , , is the uncertainty disturbance of the three-axis acceleration, , , is the uncertainty disturbance of the three-axis acceleration, , , is the three-axis angular acceleration disturbance of the robot arm.

3. The hovering stability control method of an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning according to claim 2 is characterized in that: The process of establishing the center of mass translation equation of the UAV is as follows: The center of mass translation equation of the UAV is as follows: ； in: For the total combined force of the drone, is the rotor lift, is the weight of the drone itself, is the air resistance, For drone quality; is the linear speed of the drone; For time; The lift and calculation of the quadrotor are shown in the following formula: ； in: The lift of the quadrotor and; The lift provided to a single propeller; is the lift coefficient, is the propeller speed; The virtual control amount of the height channel is shown in the following formula: ； ； ； in: The total lift provided by the propeller, is the rotation matrix from the ground system to the aircraft system; The resistance of the air on the drone is proportional to its flight speed, which can be obtained ； in: , , They are the components obtained by decomposing the air resistance coefficient under the three coordinate axes of the quadrotor, , , They are the components of the UAV's center of mass translation velocity in the X, Y, and Z coordinate axes respectively; Combining the above formulas, we get: ； Divide both sides of the above equation by The next generation is the coordinate transformation matrix , the center of mass translation equation of the UAV is as follows: 。 4. The hovering stability control method of an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning according to claim 3 is characterized in that: The process of establishing the rotation equation of the quadrotor of the UAV around the center of mass is as follows: According to Euler's theorem, the rotation equation about the center of mass is: ； in, M The total external torque ， is the moment of inertia, and it only has values ​​on the diagonal. The formula is as follows: ； The torque sources of the quadrotor drone are: rotor torque , gyroscopic torque , air resistance torque M 3 , the total external torque M The expression is: ； ； ; ; in: is the rolling moment, is the pitching moment, is the yaw moment, is the anti-torque coefficient, is the distance from the center of each rotor of the quadrotor to its center of mass, is the moment of inertia of the propeller, is the algebraic sum of the motor speed, , , is the three-axis body angular velocity, is the roll angular rate, is the pitch angular rate, is the yaw angular rate; The expression of air resistance torque is: ; in, , , They are the components of the air resistance torque coefficient under the three coordinate axes of X, Y and Z respectively; The obtained rotation equation of the quadrotor around the center of mass is: 。 5. The hovering stability control method of an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning according to claim 1 is characterized in that: The process of establishing the dynamic model of the on-board robotic arm specifically includes: Based on the inter-joint transformation matrix of the six-axis robot, the forward kinematics equation of the six-axis robot is established as follows: ; in, is the homogeneous coordinate transformation matrix from the geographic coordinate system to the end joint coordinate system of the robotic arm, , are the rotation matrix and actual position from the geographic coordinate system to the end joint coordinate system of the robotic arm, respectively. is the homogeneous transformation matrix from the world coordinate system to the drone coordinate system, is the homogeneous transformation matrix from the drone coordinate system to the robotic arm base coordinate system, is the homogeneous transformation matrix between the joints of the connecting rod robot, is the rotation angle of each joint; Based on the analytical Jacobian matrix and the geometric Jacobian matrix, the inverse kinematics equation of the robot arm is calculated using the Newton iteration method. The specific inverse kinematics equation is as follows: ; Its recursive formula is: ; in, is the angular velocity of the robot joint, is the joint angle The analytical Jacobian matrix of , is the derivative of the terminal state vector, for The joint angle of the robot arm at the moment, is the differential motion of the robot arm; From the above forward and inverse kinematics formulas of the six-axis robot, the dynamic model of the six-axis robot can be obtained as follows: ; in, is the joint angle The analytical Jacobian matrix of , is the joint angle The geometric Jacobian matrix of is the derivative of the terminal state vector, is the joint angle The analytical homogeneous matrix in the state, for Joint angle at time The analytical Jacobian matrix of .

6. The hovering stability control method of an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning according to claim 5 is characterized in that: The process of establishing the forward kinematics equation of the six-axis robot is specifically as follows: The inter-joint transformation matrix of the six-axis robot is: ; in, is the rotation angle of each joint, The coordinate system of the front link Axis and the next coordinate system The distance between the axes, The coordinate system of the front link Axis and the next coordinate system The vertical distance between the axes, The coordinate system of the front link Axis and the next coordinate system The torsion angle between the axes; The forward kinematics equation of the six-axis robot is: 。 7. The hovering stability control method of an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning according to claim 6 is characterized in that: The inverse kinematics equation establishment process of the six-axis robot is specifically as follows: Geometric Jacobian matrix of the robot arm as follows: ; in, is the derivative of the terminal state vector, is the linear velocity of the end of the robot arm, is the angular velocity of the end of the robot arm, are the joint angle and angular velocity of the robot arm respectively; Analytical Jacobian Matrix of the Robotic Arm as follows: ; in, is the derivative of the Euler angle with respect to time; Euler angles at the end of the robotic arm is the joint angle The function of in, They are the three-axis Euler angles of the end joint of the robot arm, namely the roll angle, pitch angle and yaw angle. According to the chain derivation rule, the Euler angle is directly differentiated with respect to time to obtain the analytical Jacobian matrix; The relationship between the analytical Jacobian matrix and the geometric Jacobian matrix is: ; ; ; in, is the joint angle The geometric Jacobian matrix of is the joint angle The analytical Jacobian matrix of , is the joint angle The analytical homogeneous matrix in the state, is the identity matrix, is the conversion matrix from the terminal attitude parameter derivative to the angular velocity, They are ; Using Newton's iteration method to calculate the inverse kinematics of the robot, first establish the objective function to be solved , the objective function is: ; in, is the desired position and Euler angle vector, for The actual position and Euler angle vector at angles, , , are the expected components of the end of the robot arm on the X, Y, and Z axes in the world coordinate system. , , are the desired roll, pitch and yaw angles of the end of the robot arm in the world coordinate system, , , The actual components of the end of the robot arm on the X, Y, and Z axes in the world coordinate system. , , It is the actual roll angle, pitch angle and yaw angle of the end of the robot arm in the world coordinate system; The Newton-Euler iteration formula at the moment is: ; ; After transformation, the iterative formula is expressed as: ; When the degrees of freedom of the manipulator are equal to the number of rows of the Jacobian matrix, and the Jacobian matrix is ​​full rank, The calculation formula is as follows: ; When the degree of freedom of the robot is greater than the number of rows of the Jacobian matrix, The calculation formula is as follows: ; In the above formula represents the pseudo-inverse of the Jacobian matrix; When the degree of freedom of the robot is less than the number of rows of the Jacobian matrix, the above linear equations have an infinite number of solutions. The calculation formula is as follows: ; ; in, is the identity matrix, is any n-dimensional vector; represents all vectors in the Jacobian matrix null space; The process of solving inverse kinematics based on the Jacobian matrix is ​​as follows: formula Multiply both ends on the left After: ; Its recursive formula is: ； The above formula is the alternative formula of the alternative inverse solution method, where is the differential motion of the robot arm.

8. The hovering stability control method of an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning according to claim 1 is characterized in that: The hierarchical reward function corresponding to the stable hovering of the drone with arms in S2 is as follows: The drone position reward and penalty function is as follows: ； in, , are the expected position and expected velocity respectively, , are the actual position and actual speed of the drone, , They are the drone position reward and speed reward respectively; The drone attitude reward and penalty function is as follows: ； , , They are the actual roll angle, pitch angle and yaw angle of the drone respectively. , , They are the drone roll angle reward, pitch angle reward and yaw angle reward; The reward and penalty function of the robot arm end position is as follows: ； in, is the homogeneous coordinate transformation matrix of each joint of the serial manipulator, , They are the homogeneous coordinate transformation matrices from the geographic coordinate system to the drone coordinate system and from the drone coordinate system to the robotic arm coordinate system, is the homogeneous coordinate transformation matrix from the geographic coordinate system to the end joint coordinate system of the robotic arm, , are the rotation matrix and actual position from the geographic coordinate system to the end joint coordinate system of the robotic arm, respectively. , They are the robot arm reward and the expected position of the robot arm end.

9. The hovering stability control method of an unmanned aerial vehicle with arms based on model-free incremental reinforcement learning according to claim 1 is characterized in that: In step 3, the updating process of the neural network is as follows, based on importance sampling; ； Transform the loss function gradient of the previous strategy: ; in, , are the new and old strategies of the neural network, For strategy The expected cumulative return gradient under For strategy The expected cumulative return under is the advantage function; By formula Infer the objective function from the gradient: ; We can get: ; in, and Markov process The state quantity at the moment and the action output by the strategy, is the current neural network strategy parameter, is the neural network strategy parameter of the previous round, and They are the current loss function and the loss function under the old strategy respectively; Add a regular term to the loss function and introduce an adaptive KL divergence method to control The change is expressed as: ； Set the upper and lower limits of the KL divergence value. If the KL divergence term of the loss function during training is greater than the upper limit, it indicates a penalty term. Didn't work, increased ; During training Changes to: ； in, is the KL divergence value between the new and old strategies, is the hyperparameter of the KL divergence penalty term in the loss function; After all samples have completed the gradient propagation, the global gradient is used to update the model parameters.

Images