CN113467501B - Dynamic gliding grabbing and force position hybrid control method for operation flying robot - Google Patents
Dynamic gliding grabbing and force position hybrid control method for operation flying robot Download PDFInfo
- Publication number
- CN113467501B CN113467501B CN202110819205.2A CN202110819205A CN113467501B CN 113467501 B CN113467501 B CN 113467501B CN 202110819205 A CN202110819205 A CN 202110819205A CN 113467501 B CN113467501 B CN 113467501B
- Authority
- CN
- China
- Prior art keywords
- force
- moment
- grabbing
- error
- equation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 230000008569 process Effects 0.000 claims abstract description 19
- 238000013528 artificial neural network Methods 0.000 claims abstract description 17
- 238000005096 rolling process Methods 0.000 claims abstract description 9
- 230000005484 gravity Effects 0.000 claims abstract description 5
- 238000006073 displacement reaction Methods 0.000 claims description 15
- 238000004422 calculation algorithm Methods 0.000 claims description 12
- 230000003044 adaptive effect Effects 0.000 claims description 7
- 238000013461 design Methods 0.000 claims description 7
- 239000012636 effector Substances 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000009795 derivation Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000013441 quality evaluation Methods 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 2
- 230000000694 effects Effects 0.000 description 9
- 238000010586 diagram Methods 0.000 description 8
- 238000005259 measurement Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000011664 nicotinic acid Substances 0.000 description 1
- 239000000575 pesticide Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000005507 spraying Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
The invention relates to a dynamic gliding grabbing and force position hybrid control method for an operation flying robot, which comprises the following steps of: step S1: considering gravity center shift and stress and moment in the grabbing process, a four-rotor unmanned aerial vehicle system model carrying a mechanical arm and a two-degree-of-freedom manipulator model are constructed; step S2: calculating the instantaneous contact force and the grabbing force applied to the tail end of the manipulator; and step S3: constructing a parameter estimator and carrying out parameter estimation; and step S4: according to the estimated parameters, the position of the flight platform is controlled, the neural network sliding mode self-adaptive control is carried out under the condition that modeling errors exist, and the passing lift force, the rolling moment, the pitching moment and the yawing moment are solved; step S5: performing two-degree-of-freedom mechanical arm force control aiming at the captured force analysis, and solving out a control moment; step S6: the rotating speeds of the four rotor wings are calculated through the lift force, the rolling moment, the pitching moment and the yawing moment; step S7: and controlling the unmanned aerial vehicle through the resolved data.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicles, in particular to a dynamic gliding grabbing and force position hybrid control method for an operation flying robot.
Background
The unmanned aerial vehicle realizes the unmanned mode from remote control driving to the onboard computer automatic control. Unmanned aerial vehicles are mature flight platforms, and can carry different components on the flight platforms to expand the application of the flight platforms in different fields. For example, the fields of aerial survey, pesticide spraying, target tracking and the like have the potential of unmanned aerial vehicle application. Wherein, these applications need not carry on the arm on the unmanned aerial vehicle platform, combine the two just operation type flying robot, and the equipment of so high-end can make industry obtain very big facility. With the deepening of researchers to the field, students have realized the application of unmanned aerial vehicles to carry mechanical arms in practice. The operation type flying robot with the 7-degree-of-freedom mechanical arm can flexibly complete grabbing and assembling operations; visual servo control is added on the operation type flying robot system, and an autonomous grabbing task can be completed; the tail end of a mechanical arm of the operation type flying robot is contacted with an object to replace a force sensor to finish contact force measurement work; and a parallel operation type flying robot system is adopted, so that better bionic work can be realized.
These applications all have a flight grabbing action. And when the user wants to grab the object by means of command flight, some technical difficulties need to be overcome. The grabbing mode is the problem to be solved firstly by the control engineering. Simulating an organism to glide and grab an object is one of the hot points of current research without doubt, and because gliding and grabbing the object can produce great impact force, great influence can be produced to the flight platform, if the flight speed is too fast or the object that snatchs is overweight, can make the flight platform deviate from the position that plans well even out of control.
For the problem that the operation type flying robot grabs objects, many scholars have made control methods. For example, the tail end of a mechanical arm of a working type flying robot is contacted with an object to replace a force sensor to complete contact force measurement work; separately establishing flight platform and mechanical arm dynamics models, using H ∞ The control method controls the gripped object. Most of the control methods can only grab objects with small mass, and large errors and even out of control can be generated when the objects grab large objects.
Disclosure of Invention
In view of this, the invention aims to provide a hybrid control method for dynamic gliding grabbing and force position of an operation flying robot, which can effectively improve the grabbing control precision of an unmanned aerial vehicle.
In order to achieve the purpose, the invention adopts the following technical scheme:
a dynamic gliding grabbing and force position hybrid control method for a working flying robot comprises the following steps:
step S1: considering gravity center shift and stress and moment in the grabbing process, a four-rotor unmanned aerial vehicle system model carrying a mechanical arm and a two-degree-of-freedom manipulator model are constructed;
step S2: calculating the instantaneous contact force f borne by the tail end of the manipulator by analyzing the instantaneous contact force and the grabbing force of the manipulator and the object m And a grasping force f 1 ,f 2 ；
And step S3: constructing a parameter estimator and applying the mass m and the inertia tensor I of the flight platform x ,I y ,I z Carrying out estimation;
and step S4: according to the estimated parameters obtained in the step S3, the position of the flight platform is controlled, the neural network sliding mode self-adaptive control is carried out under the condition that the modeling error exists, and the passing lift force u is solved 1 Rolling moment u 2 Pitching moment u 3 Yaw moment u 4 ；
Step S5: two-degree-of-freedom mechanical arm force control is carried out according to the captured force analysis, and control moment tau is solved 1 ,τ 2 ；
Step S6: by a lifting force u 1 Rolling moment u 2 Pitching moment u 3 Yaw moment u 4 Solve the rotational speed omega of four rotors i ，i＝1,2,3,4；
Step S7: control moment tau obtained by calculation 1 ,τ 2 And the rotational speeds ω of the four rotors i And controlling the unmanned aerial vehicle.
Further, it specifically is to establish the four rotor unmanned aerial vehicle system models who carries on the arm:
modeling a four-rotor unmanned aerial vehicle system carrying the mechanical arm by using a Newton-Euler equation method, and obtaining the dynamics of the flight platform according to force balance and moment balance:
wherein x is the displacement of the flight platform in the x-axis direction, y is the displacement of the flight platform in the y-axis direction, z is the displacement of the flight platform in the z-axis direction, phi is the roll angle of the flight platform, theta is the pitch angle of the flight platform, psi is the yaw angle of the parting platform,
flying robot position and attitude [ x, y, z, phi, theta, psi ] for quad-rotor operation] T Second derivative of (i.e. acceleration, u) 1 For the lift of the flight platform, [ u ] 2 ,u 3 ,u 4 ]For the moment of the flight platform, m is the total mass of the operation type flying robot, I x ,I y ,I z Respectively, the inertia tensor F generated by the X-axis, the Y-axis and the Z-axis of the flight platform B ＝[ B f x , B f y , B f z , B m x , B m y , B m z ]For the induced and disturbance torques generated during the grabbing process, s and c represent sin (. Cndot.) and cos (. Cndot.), a, respectively 1 ,a 2 ,a 3 ,b 1 ,b 2 ,b 3 ,d 1 ,d 2 ,d 3 Disturbance terms caused by center-of-gravity shift;
the established two-degree-of-freedom mechanical arm dynamics are as follows:
wherein M is f (q)、
And G f (q) are system variables relating to moment of inertia, mass, and rotational speed of the robot, respectively, q = [ q ] 1 ,q 2 ]The rotational speed, tau, of a two-degree-of-freedom robot arm d ＝[τ 1 ,τ 2 ]Input torque, tau, for a two-degree-of-freedom manipulator f The force and moment applied in the grabbing process.
Further, the step S2 specifically includes:
s21, constructing a dynamic equation of the operation type flying robot:
wherein:
M u ＝diag(m,m,m,I x ,I y ,I z )，ξ＝[x,y,z,φ,θ,ψ,q 1 ,q 2 ] T ,τ＝[v 1 ,v 2 ,v 3 ,u 2 ,u 3 ,u 4 ,τ 1 ,τ 2 ] T ，f m ＝[f x ,f y ,f z ] T 。 I R E ＝[ I R Eu , I R Ef ] T ，
v 1 ,v 2 ,v 3 is a virtual control quantity;
step S22: at the moment when the operation type flying robot grabs the object, the following can be obtained by the momentum theorem:
wherein: t is t 0 The time is the grabbing start time, and delta t is the grabbing time;
simultaneously, the momentum of the grabbed object can be calculated:
ξ m ＝[x m ,y m ,z m ] T is the displacement of the gripped object;
step S23: calculating impulse generated in the motion process by using impulse theorem;
wherein P is m The impulse generated in the collision process;
step S24: the speed of the object after collision is the same as that of the operation type flying robot end effector:
wherein: I R B ＝[I 3×3 ,RP x ,RP q ],
step S26: obtaining instantaneous contact force f by combining vertical type (6) - (9) m ：
Step S27: the gripping force in the gripping process is obtained by Newton motion mechanics as follows:
wherein theta is r For the end effector finger speed, K r ,
k r2 And (ζ) is a grip variable parameter. />
Further, the step S3 specifically includes:
and S31, converting the kinetic model designed in the step S1 into:
wherein: f = f m +f r1 +f r2
Step S32: design parameter estimator
And &>
The following were used:
where c and k are user-defined positive definite gain matrices, and
is the estimated state, the state estimation error is defined as
Step S33: using equation (12) and equation (13), the error dynamics is written as:
step S34: m u Expressed as:
M u ＝mβ 1 +I x β 2 +I y β 3 +I z β 4 , (15)
wherein:
β 1 ＝diag(1,1,1,0,0,0),β 2 ＝diag(0,0,0,1,0,0),β 3 ＝diag(0,0,0,0,1,0),β 4 ＝diag(0,0,0,0,0,1)
for the error dynamic equation (14), the adaptive control law is designed as follows:
the goal of the state error model is asymptotically stable
The state of (1);
step S35: it is demonstrated for equation (16) that to account for the convergence of the error dynamics equation (16), the following Lyapunov candidate function is defined:
step S36: to V 1 And (3) carrying out derivation:
substituting equation (16) into equation (18), V 1 The time derivative of (c) can be found in:
further, the step S4 specifically includes the following steps:
step S41: defining an expected vector χ d ＝[x d ,y d ,z d ,φ d ,θ d ,ψ d ] T The error is defined as follows:
e＝χ-χ d (20)
step S42: the sliding surface variable s is defined as:
wherein
Λ is a diagonal gain matrix;
step S43: using RBF neural network to carry out quality evaluation, the network algorithm is defined as follows:
wherein W T And
weights and errors of the network algorithm, respectively;
step S44: the estimated mass is defined as:
step S45: from equations (22) to (23), the following mass estimation error is obtained:
wherein
Is a bounded variable and defines>
Step S46: symbol f * The definition is as follows:
step S47: modeling error exists in the dynamics of the grabbing force, and is defined as delta u Let a Δ m ＝f-m s f * And Δ = Δ u +Δ m Then the error estimation is designed to be
Aiming at the dynamics of an unmanned aerial vehicle, the design of the sliding mode neural network controller is as follows:
wherein:
further, the step S5 specifically includes:
step S51: definition of
The instantaneous contact force is written as:
wherein J = - [ Delta t: (A) ( I R B -1 m s -1 +M -1I R E )]，
Step S52: the manipulator dynamics are rewritten as:
step S53: the expected displacement of the manipulator is defined as
And define the systematic error as follows: />
Step S54: order to
Equation (28) can be rewritten as:
step S55: the RBF neural network is used to estimate E, and the network algorithm can be expressed as:
wherein
And &>
Respectively, a weight and an error of the network algorithm, and &>
Step S56: for the dynamic characteristics of the drone (28), the robust adaptive neural network controller is designed as follows:
wherein
Is an estimate of E, and->
v f Is a robust compensator term defined as:
wherein
Wherein->
Further, step S6 specifically includes:
step S61: the system control force and the control torque u are obtained from the formula (26), u 1 、u 2 、u 3 And u 4 The relationship of (1) is:
u 1 ＝C 1 (ω 1 2 +ω 2 2 +ω 3 2 +ω 4 2 )
u 2 ＝C 1 (-ω 2 2 +ω 4 2 ),u 3 ＝C 1 (-ω 1 2 +ω 3 2 )
u 4 ＝C 2 (ω 1 2 -ω 2 2 +ω 3 2 -ω 4 2 )
(34)
step S62: solve the rotational speed omega of four rotors i ，i＝1,2,3,4。
Compared with the prior art, the invention has the following beneficial effects:
the invention constructs a dynamic model based on instantaneous contact force and grabbing, adopts a force position hybrid control strategy and can complete the task of gliding grabbing; the instantaneous contact force during gliding gripping is analyzed by impulse and momentum theorems, taking into account some factors that affect gripping performance, including irregular shape of the object, object rolling, and geometrically asymmetric gripping. Meanwhile, the mass and inertia tensor of the held object are treated as unknown bounded terms. Advantageously, a more accurate dynamic model of the UAM gliding grip is ensured. A hybrid/position controller based on a robust adaptive neural network estimator is then used for the UAM to overcome internal and external disturbances.
Drawings
Fig. 1 is a schematic view of a flight platform gliding capture according to an embodiment of the invention.
Fig. 2 is a schematic flow structure diagram according to an embodiment of the present invention.
Fig. 3 is a schematic diagram illustrating the effect of controlling the X-axis component in the position controller according to the embodiment of the present invention.
Fig. 4 is a schematic diagram illustrating the control effect of the Y-axis component in the position controller according to the embodiment of the present invention.
Fig. 5 is a schematic diagram illustrating the control effect of the Z-axis component in the position controller according to the embodiment of the present invention.
FIG. 6 shows the roll angle in the attitude controller according to the embodiment of the present invention
The control effect of (1) is shown schematically.
Fig. 7 is a schematic diagram illustrating the effect of controlling the pitch angle θ in the attitude controller according to the embodiment of the present invention.
Fig. 8 is a schematic diagram illustrating the effect of controlling the roll angle ψ in the attitude controller according to the embodiment of the present invention.
Fig. 9 is a schematic diagram illustrating the control effect of the robot arm 1 in the robot controller according to the embodiment of the present invention.
Fig. 10 is a schematic diagram illustrating the effect of controlling the robot arm 2 in the robot controller according to the embodiment of the present invention.
Detailed Description
The invention is further explained by the following embodiments in conjunction with the drawings.
Referring to fig. 2, the invention provides a modeling and force position hybrid control method based on dynamic gliding capture of a working flying robot, comprising the following steps:
step S1: considering the gravity center shift, constructing a four-rotor unmanned aerial vehicle system model carrying a mechanical arm and a two-degree-of-freedom manipulator model;
the construction of the four-rotor unmanned aerial vehicle system model carrying the mechanical arm specifically comprises the following steps: modeling a four-rotor unmanned aerial vehicle system carrying the mechanical arm by using a Newton-Euler equation method, and obtaining the dynamics of the flight platform according to force balance and moment balance:
wherein x is the displacement of the flight platform in the x-axis direction, y is the displacement of the flight platform in the y-axis direction, z is the displacement of the flight platform in the z-axis direction, phi is the roll angle of the flight platform, theta is the pitch angle of the flight platform, psi is the yaw angle of the parting platform,
flying robot position and attitude [ x, y, z, phi, theta, psi ] for quad-rotor operation] T Second derivative of (i.e. acceleration, u) 1 For the lift of the flying platform, [ u ] 2 ,u 3 ,u 4 ]For the moment of the flying platform, m is the total mass of the operation type flying robot, I x ,I y ,I z Respectively, the inertia tensor F generated by the X-axis, the Y-axis and the Z-axis of the flight platform B ＝[ B f x , B f y , B f z , B m x , B m y , B m z ]For the induced and disturbance torques generated during the grabbing process, s and c represent sin (. Cndot.) and cos (. Cndot.), a, respectively 1 ,a 2 ,a 3 ,b 1 ,b 2 ,b 3 ,d 1 ,d 2 ,d 3 The disturbance term caused by the center-of-gravity shift is defined as follows: />
Wherein r is G ＝[x G ,y G ,z G ]Is the center of gravity offset.
The established two-degree-of-freedom mechanical arm dynamics are as follows:
wherein M is f (q)、
And G f (q) are system variables relating to the moment of inertia, mass, and rotational speed of the manipulator, respectively, q = [ q ] 1 ,q 2 ]The rotational speed, tau, of a two-degree-of-freedom robot arm d ＝[τ 1 ,τ 2 ]Input torque, tau, for a two-degree-of-freedom manipulator f The force and moment applied in the grabbing process.
Step S2: calculating the instantaneous contact force f borne by the tail end of the manipulator by analyzing the instantaneous contact force and the grabbing force of the manipulator and the object m And a gripping force f 1 ,f 2 ；
And S21, obtaining a dynamic equation of the operation type flying robot by simultaneous formulas (1) and (3):
wherein:
M u ＝diag(m,m,m,I x ,I y ,I z )，ξ＝[x,y,z,φ,θ,ψ,q 1 ,q 2 ] T ,τ＝[v 1 ,v 2 ,v 3 ,u 2 ,u 3 ,u 4 ,τ 1 ,τ 2 ] T ，f m ＝[f x ,f y ,f z ] T 。 I R E ＝[ I R Eu , I R Ef ] T ，
v 1 ,v 2 ,v 3 for the virtual control quantity:
step S22: at the moment when the operation type flying robot grabs the object, the following can be obtained by the momentum theorem:
wherein: t is t 0 To capture start time, Δ t is capture time.
The momentum of the gripped object can be calculated at the same time:
ξ m ＝[x m ,y m ,z m ] T is the displacement of the gripped object.
Step S23: calculating impulse generated in the motion process by using impulse theorem;
wherein P is m The impulse generated in the collision process;
step S24: the speed of the object after collision is the same as that of the end effector of the operation type flying robot:
wherein: I R B ＝[I 3×3 ,RP x ,RP q ],
step S26: obtaining instantaneous contact force f by combined vertical type (6) - (9) m ：
Step S27: the gripping force in the gripping process is obtained by Newton motion mechanics as follows:
wherein theta is r For the end effector finger speed, K r ,
k r2 And (ζ) is a grip variable parameter.
And step S3: design parameter estimator for mass m and inertia tensor I of flying platform x ,I y ,I z Carrying out estimation;
and S31, converting the kinetic model designed in the step S1 into the following form:
wherein: f = f m +f r1 +f r2 .
Step S32: design parameter estimator
And &>
The following were used:
where c and k are user-defined positive definite gain matrices, and
is an estimated state, state
The estimation error is defined as
Step S33: using equation (12) and equation (13), the error dynamics can be written as:
step S34: m u Can be expressed as:
M u ＝mβ 1 +I x β 2 +I y β 3 +I z β 4 , (15)
wherein:
β 1 ＝diag(1,1,1,0,0,0),β 2 ＝diag(0,0,0,1,0,0),β 3 ＝diag(0,0,0,0,1,0),β 4 ＝diag(0,0,0,0,0,1)
for the error dynamic equation (14), the adaptive control law is designed as follows:
the goal of the state error model is asymptotically stable
The state of (c).
Step S35: to demonstrate the convergence of the error dynamics equation (16), we define the following Lyapunov candidate function, demonstrating equation (16):
step S36: to V 1 Make a derivation：
Substituting equation (16) into equation (18), V 1 The time derivative of (a) can yield:
and step S4: the position of a flight platform is controlled based on the related parameter estimator, the neural network sliding mode self-adaptive control is carried out under the condition that a modeling error exists, and the passing lift force u is solved 1 Rolling moment u 2 Pitching moment u 3 Yaw moment u 4 ；
Step S41: defining an expected vector χ d ＝[x d ,y d ,z d ,φ d ,θ d ,ψ d ] T The error is defined as follows:
e＝χ-χ d . (20)
step S42: let us define the sliding surface variable s as:
wherein
Λ is the diagonal gain matrix.
Step S43: using RBF neural network to carry out quality evaluation, the network algorithm is defined as follows:
wherein W T And
respectively the weights and the errors of the network algorithm.
Step S44: the estimated mass is defined as:
step S45: from equations (22) to (23), the following mass estimation error can be obtained: :
wherein
Is a bounded variable and defines>
Step S46: symbol f * The following may be defined:
step S47: modeling error exists in the dynamics of the grabbing force, and is defined as delta u Let a m ＝f-m s f * And Δ = Δ u +Δ m Then the error estimation is designed to
Aiming at the dynamics (4) of the unmanned aerial vehicle, the design of the sliding mode neural network controller is as follows:
wherein:
then, the dynamics of equation (26) gradually stabilize at equilibrium state e = 0.
Step S5: two-degree-of-freedom mechanical arm force control is carried out according to the captured force analysis, and control moment tau is solved 1 ,τ 2 ；
Step S51: definition of
Equation (10) can be written as:
wherein J = - [ Delta t: (A) ( I R B -1 m s -1 +M -1I R E )]，
Step S52: using equations (3) and (27), the manipulator dynamics can be rewritten as:
step S53: the expected displacement of the manipulator is defined as
And defines the systematic error as follows:
step S54: order to
Equation (28) can be rewritten as:
step S55: the RBF neural network is used to estimate E, and the network algorithm can be expressed as:
wherein
And &>
Respectively, the weight and the error of the network algorithm, and>
step S56: for the dynamic characteristics of the drone (28), the robust adaptive neural network controller is designed as follows:
wherein
Is an estimate of E, and->
v f Is a robust compensator term defined as:
wherein
Wherein->
The kinetics of equation (28) are in equilibrium e f Gradually stabilize at = 0.
Step S6: by a lifting force u 1 Rolling moment u 2 Pitching moment u 3 Yaw moment u 4 Solve the rotational speed omega of four rotors i ，i＝1,2,3,4；
Step S61: from the equation (26), the system control force and the control torque u can be determined, u 1 、u 2 、u 3 And u 4 The relationship of (1) is:
u 1 ＝C 1 (ω 1 2 +ω 2 2 +ω 3 2 +ω 4 2 )
u 2 ＝C 1 (-ω 2 2 +ω 4 2 ),u 3 ＝C 1 (-ω 1 2 +ω 3 2 )
u 4 ＝C 2 (ω 1 2 -ω 2 2 +ω 3 2 -ω 4 2 )
(34)
step S7: and controlling the unmanned aerial vehicle through the resolved data.
In this embodiment, referring to fig. 3 to fig. 10, the operation of the present invention will be described in detail with a specific application example, and the controller according to the control method of the present invention is further designed to mainly study the control and tracking effect when gliding and grasping the object under the influence of the friction force and the contact force. The specific settings are as follows:
1) Set to grab 0.5kg of object in the presence of friction and contact forces, and set the contact impact time short, 0.02s:
2) In the simulation process, the moment of inertia is considered to be constantly changed along with time, and the influence of external disturbance on the flight platform is considered:
3) Hardware parameters are shown in table 1:
TABLE 1 hardware parameters
As shown in fig. 3 to 10, the controller further designed according to the control method of the present embodiment can make the respective components of the position and attitude of the working type aircraft robot and the rotational speed of the robot follow the target trajectory with small fluctuations. And then the operation type flying robot moves under a small steady-state error. As can be seen in fig. 7, the pitch angle is clearly buffeting within 0.5 s. But the overshoot is small and the response time is short. The controller is still considered to be effective. Figures 3-10 demonstrate the effectiveness and advantages of the present invention.
The above description is only a preferred embodiment of the present invention, and all the equivalent changes and modifications made according to the claims of the present invention should be covered by the present invention.
Claims (4)
1. A dynamic gliding grabbing and force position hybrid control method for a working flying robot is characterized by comprising the following steps:
step S1: considering gravity center shift and stress and moment in a grabbing process, a four-rotor unmanned aerial vehicle system model carrying a mechanical arm and a two-degree-of-freedom manipulator model are constructed;
step S2: calculating the instantaneous contact force f borne by the tail end of the mechanical hand by analyzing the instantaneous contact force and the grabbing force of the mechanical hand and the object m And a gripping force f 1 ,f 2 ；
And step S3: constructing a parameter estimator and calculating the mass m and the inertia tensor I of the flight platform x ,I y ,I z Carrying out estimation;
and step S4: according to the estimated parameters obtained in the step S3, the position of the flight platform is controlled, the neural network sliding mode self-adaptive control is carried out under the condition that the modeling error exists, and the lift force u is solved 1 Rolling moment u 2 Pitching moment u 3 Yaw moment u 4 ；
Step S5: two-degree-of-freedom mechanical arm force control is carried out aiming at the captured force analysis, and the control moment tau is solved 1 ,τ 2 ；
Step S6: by a lifting force u 1 Rolling moment u 2 Pitching moment u 3 Yaw moment u 4 Solve the rotational speed omega of four rotors i ，i＝1,2,3,4；
Step S7: control moment tau obtained by calculation 1 ,τ 2 And the rotation speed omega of four rotors i Controlling the unmanned aerial vehicle;
the step S2 specifically comprises the following steps:
s21, constructing a dynamic equation of the operation type flying robot:
wherein:
M u ＝diag(m,m,m,I x ,I y ,I z )，ξ＝[x,y,z,φ,θ,ψ,q 1 ,q 2 ] T ,τ＝[v 1 ,v 2 ,v 3 ,u 2 ,u 3 ,u 4 ,τ 1 ,τ 2 ] T ，f m ＝[f x ,f y ,f z ] T ， I R E ＝[ I R Eu , I R Ef ] T ，
v 1 ,v 2 ,v 3 is a virtual control quantity;
step S22: at the moment when the operation type flying robot grabs the object, the momentum theorem is as follows:
wherein: t is t 0 The time is the starting time of grabbing, and delta t is the grabbing time;
and simultaneously calculating the momentum of the grabbed object:
ξ m ＝[x m ,y m ,z m ] T displacement of the gripped object;
step S23: calculating impulse generated in the motion process by using impulse theorem;
wherein P is m The impulse generated in the collision process;
step S24: the speed of the object after collision is the same as that of the operation type flying robot end effector:
wherein: I R B ＝[I 3×3 ,RP x ,RP q ],
step S25: obtaining instantaneous contact force f by combined vertical type (6) - (9) m ：
Step S26: the gripping force in the gripping process is obtained by Newton dynamics:
wherein theta is r For the end effector finger speed, K r ,
k r2 (ζ) is a grasping variable parameter;
the step S4 specifically includes the following steps:
step S41: defining an expected vector χ d ＝[x d ,y d ,z d ,φ d ,θ d ,ψ d ] T The error is defined as follows:
e＝χ-χ d (20)
step S42: the sliding surface variable s is defined as:
wherein
Λ is a diagonal gain matrix;
step S43: using RBF neural network to carry out quality evaluation, the network algorithm is defined as follows:
wherein W T And
weights and errors of the network algorithm, respectively;
step S44: the estimated mass is defined as:
step S45: from equations (22) to (23), the following mass estimation error is obtained:
wherein
Is a bounded variable and defines>
Step S46: symbol f * The definition is as follows:
step S47: modeling error exists in the dynamics of the grabbing force, and is defined as delta u Let a m ＝f-m s f * And Δ = Δ u +Δ m Then the error estimation is designed to
Aiming at the dynamics of an unmanned aerial vehicle, the design of the sliding mode neural network controller is as follows:
wherein:
the step S5 specifically comprises the following steps:
step S51: definition of
The instantaneous contact force is written as:
wherein J = - [ Delta t: ( I R B -1 m s -1 +M -1I R E )]，
Step S52: the manipulator dynamics are rewritten as:
step S53: the expected displacement of the manipulator is defined as
And define the systematic error as follows:
step S54: order to
Equation (28) is rewritten as:
step S55: the RBF neural network is used to estimate E, and the network algorithm is expressed as:
wherein
And &>
Respectively, the weight and the error of the network algorithm, and>
step S56: for the dynamic characteristics of the unmanned aerial vehicle (28), the robust adaptive neural network controller is designed as follows:
wherein
Is an estimate of E, and->
v f Is a robust compensator term defined as:
wherein
Where θ (0) > 0.
2. The dynamic gliding grabbing and force-position hybrid control method for the working flying robot according to claim 1, wherein the building of the robot arm-mounted quadrotor unmanned aerial vehicle system model is specifically as follows:
modeling the four-rotor unmanned aerial vehicle system carrying the mechanical arm by using a Newton-Euler equation method, and obtaining dynamics of the flight platform according to force balance and moment balance:
wherein x is the displacement of the flight platform in the x-axis direction, y is the displacement of the flight platform in the y-axis direction, z is the displacement of the flight platform in the z-axis direction, phi is the roll angle of the flight platform, theta is the pitch angle of the flight platform, psi is the yaw angle of the parting platform,
flying robot position and attitude [ x, y, z, phi, theta, psi ] for four-rotor operation] T Second derivative of (i.e. acceleration, u) 1 For the lift of the flying platform, [ u ] 2 ,u 3 ,u 4 ]For the moment of the flight platform, m is the total mass of the operation type flying robot, I x ,I y ,I z Respectively, the inertia tensor F generated by the X-axis, the Y-axis and the Z-axis of the flight platform B ＝[ B f x , B f y , B f z , B m x , B m y , B m z ]For the induced and disturbance torques generated during the grabbing process, s and c represent sin (. Cndot.) and cos (. Cndot.), a, respectively 1 ,a 2 ,a 3 ,b 1 ,b 2 ,b 3 ,d 1 ,d 2 ,d 3 Disturbance terms caused by center-of-gravity shift;
the established two-degree-of-freedom mechanical arm dynamics are as follows:
wherein M is f (q)、
And G f (q) are system variables relating to moment of inertia, mass, and rotational speed of the robot, respectively, q = [ q ] 1 ,q 2 ]The rotational speed, tau, of a two-degree-of-freedom robot arm d ＝[τ 1 ,τ 2 ]Input torque, tau, for a two-degree-of-freedom manipulator f The force and moment applied in the grabbing process.
3. The method for dynamic gliding grabbing and force location hybrid control of a working flying robot as claimed in claim 1, wherein said step S3 is specifically:
and S31, converting the kinetic model designed in the step S1 into:
wherein: f = f m +f r1 +f r2
Step S32: design parameter estimator
And &>
The following were used:
where c and k are user-defined positive definite gain matrices, and
is the estimated state, the state estimation error is defined as
Step S33: using equation (12) and equation (13), the error dynamics is written as:
step S34: m is a group of u Expressed as:
M u ＝mβ 1 +I x β 2 +I y β 3 +I z β 4 , (15)
wherein:
β 1 ＝diag(1,1,1,0,0,0),β 2 ＝diag(0,0,0,1,0,0),β 3 ＝diag(0,0,0,0,1,0),β 4 ＝diag(0,0,0,0,0,1)
for the error dynamic equation (14), the adaptive control law is designed as follows:
the goal of the state error model is asymptotically stable
The state of (1);
step S35: it is demonstrated for equation (16) that to account for the convergence of the error dynamics equation (16), the following Lyapunov candidate function is defined:
step S36: to V 1 And (3) carrying out derivation:
substituting equation (16) into equation (18), V 1 The time derivative of (a) yields:
4. the method for dynamically gliding and grabbing and force level hybrid control of a working flying robot as claimed in claim 1, wherein said step S6 is specifically:
step S61: the system control force and the control torque u are obtained from the formula (26), u 1 、u 2 、u 3 And u 4 The relationship of (c) is:
step S62: solve the rotational speed omega of four rotors i ，i＝1,2,3,4。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110819205.2A CN113467501B (en) | 2021-07-20 | 2021-07-20 | Dynamic gliding grabbing and force position hybrid control method for operation flying robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110819205.2A CN113467501B (en) | 2021-07-20 | 2021-07-20 | Dynamic gliding grabbing and force position hybrid control method for operation flying robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113467501A CN113467501A (en) | 2021-10-01 |
| CN113467501B true CN113467501B (en) | 2023-03-28 |
Family
ID=77881284
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110819205.2A Active CN113467501B (en) | 2021-07-20 | 2021-07-20 | Dynamic gliding grabbing and force position hybrid control method for operation flying robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113467501B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115480583B (en) * | 2022-09-22 | 2024-06-07 | 福州大学 | Visual servo tracking and impedance control method for flying operation robot |
| CN115364255B (en) * | 2022-09-27 | 2023-09-01 | 江苏理工学院 | Virus disinfection device with wide application range and stable disinfection and control method |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110427043A (en) * | 2019-09-04 | 2019-11-08 | 福州大学 | Pose Control device design method based on operation flying robot's centre-of gravity shift |
| CN111650836A (en) * | 2020-06-18 | 2020-09-11 | 福州大学 | Control method for dynamically gliding and grabbing object based on operation flying robot |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106737659B (en) * | 2016-11-15 | 2017-11-21 | 哈尔滨工程大学 | A kind of hand ship control method for coordinating of underwater unmanned vehicle and arm-and-hand system |
| US10509415B2 (en) * | 2017-07-27 | 2019-12-17 | Aurora Flight Sciences Corporation | Aircrew automation system and method with integrated imaging and force sensing modalities |
| CN109426147B (en) * | 2017-08-23 | 2022-02-08 | 中国空气动力研究与发展中心计算空气动力研究所 | Adaptive gain adjustment control method for combined spacecraft after satellite acquisition |
| CN110275436B (en) * | 2019-05-31 | 2021-04-06 | 广东工业大学 | RBF neural network self-adaptive control method of multi-single-arm manipulator |
| CN111844095B (en) * | 2020-07-24 | 2021-08-03 | 江南大学 | Method and structure for judging holding state of flexible manipulator by obtaining angular displacement through distance measurement |
| CN111993417B (en) * | 2020-08-04 | 2021-07-20 | 北京航空航天大学 | Mechanical arm self-adaptive impedance control method based on RBF neural network |
| CN112809666B (en) * | 2020-12-17 | 2023-04-28 | 安徽工业大学 | 5-DOF mechanical arm strength position tracking algorithm based on neural network |
-
2021
- 2021-07-20 CN CN202110819205.2A patent/CN113467501B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110427043A (en) * | 2019-09-04 | 2019-11-08 | 福州大学 | Pose Control device design method based on operation flying robot's centre-of gravity shift |
| CN111650836A (en) * | 2020-06-18 | 2020-09-11 | 福州大学 | Control method for dynamically gliding and grabbing object based on operation flying robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113467501A (en) | 2021-10-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112904728B (en) | Mechanical arm sliding mode control track tracking method based on improved approach law | |
| CN111185907B (en) | Pose stability control method for operation type flying robot after grabbing | |
| CN113467501B (en) | Dynamic gliding grabbing and force position hybrid control method for operation flying robot | |
| CN110427043B (en) | Pose controller design method based on gravity center offset of operation flying robot | |
| Yüksel et al. | A nonlinear force observer for quadrotors and application to physical interactive tasks | |
| CN107160398B (en) | The safe and reliable control method of Rigid Robot Manipulator is limited based on the total state for determining study | |
| CN109606753A (en) | A kind of control method of Dual-arm space robot collaboration capture target | |
| CN112346470A (en) | Four-rotor attitude control method based on improved active disturbance rejection control | |
| Fang et al. | Skill learning for human-robot interaction using wearable device | |
| CN111240344B (en) | Autonomous underwater robot model-free control method based on reinforcement learning technology | |
| CN112091976B (en) | Task space control method for underwater mechanical arm | |
| CN112894821B (en) | Current method based collaborative robot dragging teaching control method, device and equipment | |
| CN111650836B (en) | Control method for dynamically gliding and grabbing object based on operation flying robot | |
| CN112578805A (en) | Attitude control method of rotor craft | |
| CN113703319A (en) | Joint module inequality constraint optimal robust control method based on reinforcement learning | |
| CN110641738B (en) | Trajectory tracking control method of space five-degree-of-freedom free flying mechanical arm | |
| CN115556111A (en) | Variable inertial parameter modeling-based coupling disturbance control method for aircraft mechanical arm | |
| CN109947099B (en) | Robot control method and device based on event trigger mechanism | |
| Zhang et al. | Gliding grasping analysis and hybrid force/position control for unmanned aerial manipulator system | |
| CN108227493A (en) | A kind of robot trace tracking method | |
| CN115826597A (en) | Adaptive neural network-based anti-interference control method and device for rotorcraft | |
| CN113580127B (en) | Multi-rotor aircraft humanoid double-mechanical-arm system and dynamic self-balancing control design method thereof | |
| CN112560262B (en) | Three-finger smart manual mechanical modeling method, system, equipment and storage medium | |
| Liu et al. | Bio-inspired inverted landing strategy in a small aerial robot using policy gradient | |
| CN114355771A (en) | Cooperative robot force and position hybrid control method and system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |