A force-position hybrid control method for constrained multi-manipulators

Abstract

The present application relates to a method for force-position hybrid control of constrained multi-manipulators based on an adaptive reduced-order sliding mode algorithm. The method comprises: when a control instruction carrying expected position information is received, the position information of the object controlled by the manipulator is obtained; the position information of the controlled object and the expected position information are input into an internal and external loop force-position hybrid control system composed of a pre-constructed reduced-order constrained dual-two-degree-of-freedom multi-manipulator system dynamics model and an adaptive reduced-order sliding mode controller, so that the internal and external loop force-position hybrid control system outputs a control signal to control the manipulator to move along the expected motion trajectory according to the position information and the expected position information; the use of this control method not only improves the control accuracy of the controller but also improves the response speed of the system.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial automation control, and in particular to a constrained multi-manipulator force-position hybrid control method based on an adaptive reduced-order sliding mode algorithm. Background Art

[0002] In recent years, with the rapid development of my country's industry, the application of robots has been greatly improved, and has gradually been applied to manufacturing, agriculture and other fields. At the same time, domestic research on robots has also deepened year by year, but these applications and research are mostly concentrated in the field of single robotic arms, and there is still a lack of research and application of multi-robotic arms. In practical applications, single robotic arms have many limitations such as low efficiency, susceptibility to interference, inability to adapt to complex working environments, and inability to handle large and heavy objects. Therefore, compared with single robotic arm systems, multi-robotic arm systems have great advantages.

[0003] Although compared with a single robot system, multi-robots have many advantages in practical applications, this also puts forward higher control requirements for them. When a multi-robot controls an object to move in a constrained environment, the multi-robot and the object form a whole. Therefore, it is necessary not only to consider controlling the object to move according to the desired position instruction and controlling the constraint force between the object and the constrained environment, but also to consider the internal force of the robot arm holding the object to ensure that the robot arm can stably control the object without causing damage to the object. Therefore, domestic and foreign experts have proposed many control methods for the force-position hybrid control problem of multi-robot systems. Yu Jinpeng and others combined command filtering with backstepping, and eliminated filtering errors based on an error compensation system. Gueaieb and others designed a sliding mode control algorithm to solve the uncertainty problem of coordinated robot arms. Li Shurong and others designed a neural network adaptive algorithm to overcome the influence of model modeling and time lag on the system.

[0004] However, these methods are relatively complicated in the design of controllers, and only consider the position and internal force issues separately, ignoring the influence of constraint forces. Summary of the invention

[0005] In view of the above technical problems, a multi-manipulator force-position hybrid control method based on an adaptive reduced-order sliding mode algorithm is provided, which can integrate position control, constraint force control, and internal force control in the same controller, and can improve the system response speed and reduce the controller tracking error;

[0006] A multi-manipulator force-position hybrid control method based on an adaptive reduced-order sliding mode algorithm, the method comprising:

[0007] When receiving a control instruction carrying expected position information, obtaining position information of the object controlled by the robot arm;

[0008] Inputting the position information of the controlled object and the desired position information into an internal-external loop force-position hybrid control system composed of a pre-constructed reduced-order constrained dual-two-degree-of-freedom multi-manipulator system dynamics model and an adaptive reduced-order sliding mode controller, so that the internal-external loop force-position hybrid control system outputs a control signal to control the manipulator to move along a desired motion trajectory according to the position information and the desired position information;

[0009] The pre-built reduced-order dynamics model of the multi-manipulator system with dual two-degree-of-freedom constraints is:

[0010]

[0011] Where x = [x 1 T ,x 2 T ] T is the current position information of the controlled object, For x 1 The first derivative of For x 1 The second derivative of L (x 1 )∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G L (x 1 )∈R n is the gravity vector, n is the degree of freedom of the robot, R (n*n) represents the column vector of n*n dimensional real space, τ L To control the input torque,

[0012] The pre-built adaptive reduced-order sliding mode controller is:

[0013] τ=J e T (J o T ) + τ a +J e T (F Id -K I e FI )

[0014] in, Y a is the regression matrix, p a is the unknown constant vector of the system, D 1 , C 1 , G1 The estimate of K d >0, w=diag(w 1 ,...,w n ), K λ >0, s is the sliding mode function, J o , J e , J c is the Jacobian matrix, used to control the force term λ r =λ d -k λ e λ ,λ d is the expected contact force, and the force error e λ =λ-λ d , λ is the contact force, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

[0015] The method of constructing a reduced-order constrained dual-two-degree-of-freedom multi-manipulator system model includes:

[0016] A multi-manipulator system consisting of k n-DOF manipulators and a controlled object is established. The dynamic model of the i-th manipulator is:

[0017]

[0018] Where: q i ∈R n is the robot arm angle vector, for q i The first derivative of for q i The second derivative of i (q i )∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G i (q i )∈R n is the gravity term vector, J ei (q i )∈R (n*n) For i The Jacobian matrix of the position vector to the end of the robot arm, F ei ∈R n is the force exerted by the end of the robot arm on the controlled object, and τ is the control input torque.

[0019] The multi-manipulator dynamics model consisting of k manipulators can be obtained:

[0020]

[0021] The dynamic equation of the controlled object is established as:

[0022]

[0023] Where x = [x 1 T ,x 2 T ,...,x n T ] T ∈R n is the position vector of the controlled object, D o (x)∈R (n*n) is the inertia matrix of the controlled object, is the centrifugal force and Coriolis force matrix of the controlled object, G o (x)∈R n is the gravity vector of the controlled object, F o ∈R n It is the external force exerted on the controlled object.

[0024] Establish a dynamic model of a multi-manipulator system:

[0025] Take x ei ∈R n is the position vector of the end of the i-th robot arm, with x e =[x e1 T ,x e2 T ,...,x ek T ] T ∈R kn ,set up: in, is the vector from q to position x e The transformation matrix of .

[0026]

[0027] in:

[0028] After transformation, the above equation can be obtained:

[0029] in, τ a =J o T J e -T τ. J o , Je is the Jacobian matrix.

[0030] Finally, the dynamic model of the constrained dual two-degree-of-freedom multi-manipulator system is established:

[0031] Assume that the multi-manipulator system operates in an m-dimensional constraint environment. c is the contact point between the object and the constraint surface, then for the object's center of gravity position vector x, we have x c =h(x), so the constraint environment can be expressed as:

[0032] ψ(x)=Φ(h(x))=Φ(x c )=0

[0033] Taking the derivative of both sides with respect to time t, we get:

[0034] in, is the vector from x to the contact point x c The Jacobian matrix of .

[0035] The dynamic equation of the constrained object is:

[0036]

[0037] Among them, F c =J c T λ∈R n is the constraint force on the object, λ∈R m is the contact force vector.

[0038] but:

[0039] In order to facilitate the control of the multi-manipulator system, let x = [x 1 T ,x 2 T ] T , since the object operates in an m-dimensional constraint environment, the system's degree of freedom is reduced to nm, then x 1 ∈R n-m , x 2 ∈R m , take x 1 Describe the constrained motion of the robot arm, which can be expressed as x 1 To represent x 2 , which is expressed as: x 2 =σ(x 1 ).but:

[0040]

[0041] in,

[0042] Right now:

[0043] The dynamic equation of the multi-manipulator system with two degrees of freedom constrained is expressed as:

[0044]

[0045] Among them, D 1 =D a L, G 1 =G a .

[0046] Multiply both sides by L T (x 1 ), we can derive:

[0047]

[0048] Among them, D L =L T D 1 , C L =L T C 1 , G L =L T G 1 , τ L =L T τ a ,and is a skew-symmetric matrix.

[0049] The methods of constructing an adaptive reduced-order sliding mode controller include:

[0050] Let x d For the desired object center of gravity position vector, define the following variables:

[0051]

[0052] Among them, x 1e is the position error, x 1d For x 1 The expected position of x 1r is the system error vector, Λ>0.

[0053] The sliding mode function is: Force error: e λ =λ-λ d , where λ d is the expected contact force.

[0054] Design the controller as:

[0055]

[0056] in, D 1 , C 1 , G 1 The estimate of K d >0, w=diag(w 1 ,...,w n ), K λ > 0. The term used to control the force: λ r =λ d -k λ e λ .

[0057] Then the position and force controllers are:

[0058]

[0059] In order to enable the robot to effectively control the object, internal force control is added to the controller, and we get:

[0060]

[0061] in: Assume that the term used to control the internal force is: F Ir =F Id -K I e FI , where K I >0,e FI =F I -F Id , F Id To expect inner strength.

[0062] By τ a =J o T J e -T τ and J o T F I =0, we get:

[0063] τ a =J o T J e -T τ+J o T F Ir

[0064] The final adaptive reduced-order sliding mode controller is:

[0065] τ=J e T (J oT ) + τ a +J e T (F Id -K I e FI )

[0066] in, Y a is the regression matrix, p a is the unknown constant vector of the system, D 1 , C 1 , G 1 The estimate of K d >0, w=diag(w 1 ,...,w n ), K λ >0, s is the sliding mode function, J o , J e , J c is the Jacobian matrix, used to control the force term λ r =λ d -k λ e λ ,λ d is the expected contact force, and the force error e λ =λ-λ d , λ is the contact force, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

[0067] The above-mentioned constrained multi-manipulator force-position hybrid control method based on the adaptive reduced-order sliding mode algorithm obtains the current position information of the controlled object when receiving a control instruction carrying the expected position information; the position information and the expected position information are input into the inner and outer loop force-position hybrid control system composed of a pre-constructed reduced-order constrained dual two-degree-of-freedom manipulator dynamics model and an adaptive reduced-order sliding mode controller, so that the inner and outer loop force-position hybrid control system outputs a control signal to control the manipulator to move along the expected motion trajectory according to the position information and the expected position information; this control method not only improves the control accuracy of the controller but also improves the response speed of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 It is a step diagram of the force-position hybrid control method of constrained multi-manipulators based on the adaptive reduced-order sliding mode algorithm;

[0069] Figure 2 It is a specific flow chart of the force-position hybrid control method of constrained multi-manipulators based on the adaptive reduced-order sliding mode algorithm;

[0070] Figure 3 It is a diagram of the constrained dual two-degree-of-freedom robot model; DETAILED DESCRIPTION

[0071] like Figure 1 As shown, a method for hybrid force-position control of a robotic arm based on an adaptive reduced-order sliding mode algorithm is provided, comprising the following steps:

[0072] A mechanical arm force-position hybrid control method based on an adaptive reduced-order sliding mode algorithm, the method comprising:

[0073] When receiving a control instruction carrying expected position information, obtaining position information of the object controlled by the robot arm;

[0074] Inputting the position information of the controlled object and the desired position information into an internal-external loop force-position hybrid control system composed of a pre-constructed reduced-order constrained dual-two-degree-of-freedom multi-manipulator system dynamics model and an adaptive reduced-order sliding mode controller, so that the internal-external loop force-position hybrid control system outputs a control signal to control the manipulator to move along a desired motion trajectory according to the position information and the desired position information;

[0075] The pre-built reduced-order dynamics model of the multi-manipulator system with dual two-degree-of-freedom constraints is:

[0076]

[0077] Where x = [x 1 T ,x 2 T ] T is the current position information of the controlled object, For x 1 The first derivative of For x 1 The second derivative of L (x 1 )∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G L (x 1 )∈R n is the gravity vector, n is the degree of freedom of the robot, R (n*n) represents the column vector of n*n dimensional real space, τ L To control the input torque,

[0078] The pre-built adaptive reduced-order sliding mode controller is:

[0079] τ=J e T (J o T ) + τ a +J e T (F Id -K I e FI )

[0080] in, Y a is the regression matrix, p a is the unknown constant vector of the system, D 1 , C 1 , G 1 The estimate of K d >0, w=diag(w 1 ,...,w n ), K λ >0, s is the sliding mode function, J o , J e , J c is the Jacobian matrix, used to control the force term λ r =λ d -k λ e λ ,λ d is the expected contact force, and the force error e λ =λ-λ d , λ is the contact force, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

[0081] The method of constructing a reduced-order constrained dual-two-degree-of-freedom multi-manipulator system model includes:

[0082] A multi-manipulator system consisting of k n-DOF manipulators and a controlled object is established. The dynamic model of the i-th manipulator is:

[0083]

[0084] Where: q i ∈R n is the robot arm angle vector, for q i The first derivative of for q i The second derivative of i (q i )∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G i (q i )∈R n is the gravity term vector, J ei (q i )∈R (n*n) For i The Jacobian matrix of the position vector to the end of the robot arm, F ei ∈R n is the force exerted by the end of the robot arm on the controlled object, and τ is the control input torque.

[0085] The multi-manipulator dynamics model consisting of k manipulators can be obtained:

[0086]

[0087] The dynamic equation of the controlled object is established as:

[0088]

[0089] Where x = [x 1 T ,x 2 T ,...,x n T ] T ∈R n is the position vector of the controlled object, D o (x)∈R (n*n) is the inertia matrix of the controlled object, is the centrifugal force and Coriolis force matrix of the controlled object, G o (x)∈R n is the gravity vector of the controlled object, F o ∈R n It is the external force exerted on the controlled object.

[0090] Establish a dynamic model of a multi-manipulator system:

[0091] Take x ei ∈R n is the position vector of the end of the i-th robot arm, with x e =[x e1 T ,x e2 T ,...,x ek T ] T ∈Rkn ,set up: in, is the vector from q to position x e The transformation matrix of .

[0092]

[0093] in:

[0094] After transformation, the above equation can be obtained:

[0095] in, τ a =J o T J e -T τ. J o , J e is the Jacobian matrix.

[0096] Finally, the dynamic model of the constrained dual two-degree-of-freedom multi-manipulator system is established:

[0097] Assume that the multi-manipulator system operates in an m-dimensional constraint environment. c is the contact point between the object and the constraint surface, then for the object's center of gravity position vector x, we have x c =h(x), so the constraint environment can be expressed as:

[0098] ψ(x)=Φ(h(x))=Φ(x c )=0

[0099] Taking the derivative of both sides with respect to time t, we get:

[0100] in, is the vector from x to the contact point x c The Jacobian matrix of .

[0101] The dynamic equation of the constrained object is:

[0102]

[0103] Among them, F c =J c T λ∈R n is the constraint force on the object, λ∈R m is the contact force vector.

[0104] but:

[0105] In order to facilitate the control of the multi-manipulator system, let x = [x 1 T ,x 2 T ] T , since the object operates in an m-dimensional constraint environment, the system's degree of freedom is reduced to nm, then x 1 ∈R n-m , x 2 ∈R m , take x 1 Describe the constrained motion of the robot arm, which can be expressed as x 1 To represent x 2 , which is expressed as: x 2 =σ(x 1 ).but:

[0106]

[0107] in,

[0108] Right now:

[0109] The dynamic equation of the multi-manipulator system with two degrees of freedom constrained is expressed as:

[0110]

[0111] Among them, D 1 =D a L, G 1 =G a .

[0112] Multiply both sides by L T (x 1 ), we can derive:

[0113]

[0114] Among them, D L =L T D 1 , C L =L T C 1 , G L =L T G 1 , τ L =L T τ a ,and is a skew-symmetric matrix.

[0115] The methods of constructing an adaptive reduced-order sliding mode controller include:

[0116] Let xd For the desired object center of gravity position vector, define the following variables:

[0117]

[0118] Among them, x 1e is the position error, x 1d For x 1 The expected position of x 1r is the system error vector, Λ>0.

[0119] The sliding mode function is: Force error: e λ =λ-λ d , where λ d is the expected contact force.

[0120] Design the controller as:

[0121]

[0122] in, D 1 , C 1 , G 1 The estimate of K d >0, w=diag(w 1 ,...,w n ), K λ > 0. The term used to control the force: λ r =λ d -k λ e λ .

[0123] Then the position and force controllers are:

[0124]

[0125] In order to enable the robot to effectively control the object, internal force control is added to the controller, and we get:

[0126]

[0127] in: Assume that the term used to control the internal force is: F Ir =F Id -K I e FI , where K I >0,e FI =F I -F Id , F Id To expect inner strength.

[0128] By τ a=J o T J e -T τ and J o T F I =0, we get:

[0129] τ a =J o T J e -T τ+J o T F Ir

[0130] The final adaptive reduced-order sliding mode controller is:

[0131] τ=J e T (J o T ) + τ a +J e T (F Id -K I e FI )

[0132] in, Y a is the regression matrix, p a is the unknown constant vector of the system, D 1 , C 1 , G 1 The estimate of K d >0, w=diag(w 1 ,...,w n ), K λ >0, s is the sliding mode function, J o , J e , J c is the Jacobian matrix, used to control the force term λ r =λ d -k λ e λ ,λ d is the expected contact force, and the force error e λ =λ-λ d , λ is the contact force, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

[0133] The above-mentioned constrained multi-manipulator force-position hybrid control method based on the adaptive reduced-order sliding mode algorithm obtains the current position information of the controlled object when receiving a control instruction carrying the expected position information; the position information and the expected position information are input into the inner and outer loop force-position hybrid control system composed of a pre-constructed reduced-order constrained dual two-degree-of-freedom manipulator dynamics model and an adaptive reduced-order sliding mode controller, so that the inner and outer loop force-position hybrid control system outputs a control signal to control the manipulator to move along the expected motion trajectory according to the position information and the expected position information; this control method not only improves the control accuracy of the controller but also improves the response speed of the system.

[0134] The specific implementation process of the entire invention is shown in Figure 2 The specific implementation methods are as follows:

[0135] 1. Build a multi-robot dynamics model

[0136] The dynamic model of the i-th robotic arm is:

[0137]

[0138] Where: q i ∈R n is the robot arm angle vector, D i (q i )∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G i (q i )∈R n is the gravity term vector, J ei (q i )∈R (n*n) For i Jacobian matrix of the position vector of the end of the robot, F ei ∈R n is the force exerted by the end of the robot arm on the controlled object, and τ is the control input torque.

[0139] The multi-manipulator dynamics model consisting of k manipulators can be obtained:

[0140]

[0141] Where: q = [q 1 T ,q 2 T ,...,q k T ] T ∈R kn ;

[0142] D(q)=blockdiag[D 1 (q 1 ),D 2 (q 2 ),...,D k (q k )]∈R kn*kn ;

[0143] G=[G 1 T ,G 2 T ,...,G k T ] T ∈R kn ;

[0144] J e =blockdiag[J e1 ,J e2 ,...,J ek ]∈R kn*kn ;

[0145] F e =[F e1 T ,F e2 T ,...,F ek T ] T ∈R kn ;

[0146] τ=[τ 1 T ,τ 2 T ,...,τ k T ] T ∈R kn .

[0147] The multi-manipulator dynamics model composed of k manipulators has the following properties:

[0148] Property 1: There exists a positive number k 1 ,k 2 , satisfying the following formula:

[0149] k 1 ||s|| 2 ≤s T D(q)s≤k 2 ||s|| 2

[0150] Property 2: is a skew-symmetric matrix, then:

[0151]

[0152] Property 3: There is a parameter linearization relationship:

[0153]

[0154] in, For the known variable q, The regression matrix, p∈R n is the unknown constant vector of the robot arm.

[0155] 2. Dynamic model of the controlled object

[0156] The dynamic equation of the controlled object is:

[0157]

[0158] Where x = [x 1 T ,x 2 T ,...,x n T ] T ∈R n is the position vector of the controlled object, D o (x)∈R (n*n) is the inertia matrix of the controlled object, is the centrifugal force and Coriolis force matrix of the controlled object, G o (x)∈R n is the gravity vector of the controlled object, F o ∈R n is the external force on the controlled object, and

[0159]

[0160] Among them, J oi (x)∈R (n*n) is the Jacobian matrix from the robot end position vector to x, F ei ∈R n is the force exerted by the end of the robot arm on the object. e It can be divided into internal force F I ∈R k*n and external force F o ∈R K*n Two parts.

[0161] Since the total internal force of the object is zero in the coordinate system with the center of gravity of the object as the origin, that is, J o T F I=0.

[0162] We can get: F e =(J o T ) + F o +F I

[0163] Among them, (J o T ) + ∈R (kn*n) Indicates J o T The pseudo-inverse matrix of .

[0164] 3. Dynamics model of multi-manipulator system

[0165] Take x ei ∈R n is the position vector of the end of the i-th robot arm, with x e =[x e1 T ,x e2 T ,...,x ek T ] T ∈R kn ,set up

[0166] in, is the vector from q to position x e The transformation matrix of .

[0167] Taking the derivatives of both sides with respect to time, we get:

[0168]

[0169] in,

[0170] There are also:

[0171]

[0172] Among them, J o (x)∈R (n*n) .

[0173] It can be deduced that:

[0174] From Assumption 4, we know that J e Reversible, then:

[0175]

[0176] Taking the derivatives of both sides with respect to time, we get:

[0177]

[0178] We can get:

[0179]

[0180] in:

[0181] Because J o T F I =0, then we can deduce that:

[0182]

[0183] in, τ a =J o T J e -T τ.

[0184] 4. Dynamic model of constrained multi-manipulator system

[0185] Assume that the multi-manipulator system operates in an m-dimensional constraint environment. c is the contact point between the object and the constraint surface, then for the object's center of gravity position vector x, we have x c =h(x), so the constraint environment can be expressed as:

[0186] ψ(x)=Φ(h(x))=Φ(x c )=0

[0187] Taking the derivative of both sides with respect to time t, we get:

[0188]

[0189] in, is the vector from x to the contact point x c The Jacobian matrix of .

[0190] The dynamic equation of the constrained object is:

[0191]

[0192] Among them, F c =J c T λ∈R n is the constraint force on the object, λ∈R m is the contact force vector.

[0193] have to:

[0194]

[0195] like Figure 3 As shown in the figure, the end effector of the robot arm is subject to force constraints. In order to facilitate the control of the multi-robot system, let x = [x 1 T ,x 2 T ] T , since the object operates in an m-dimensional constraint environment, the system's degree of freedom is reduced to nm, then x 1 ∈R n-m , x 2 ∈R m , take x 1 Describe the constrained motion of the robot arm, which can be expressed as x 1 To represent x 2 , which is expressed as: x 2 =σ(x 1 ).but:

[0196]

[0197]

[0198] in,

[0199] have to:

[0200] The dynamic equation of the constrained multi-manipulator system is expressed as:

[0201]

[0202] Among them, D 1 =D a L, G 1 =G a .

[0203] The dynamic equations of the constrained multi-manipulator system have the following properties:

[0204] Nature 1: J c L=L T J c T =0

[0205] Property 2: It has a property similar to Property 3 of the multi-manipulator dynamics model composed of k manipulators, that is, it satisfies the parameter linearization relationship.

[0206] The final constrained multi-manipulator system dynamics model can be derived:

[0207]

[0208] Among them, D L =L T D 1 , C L =L T C 1 , G L =L T G 1 , τ L =L T τ a ,and is a skew-symmetric matrix.

[0209] 5. Controller design

[0210] Let x d For the desired object center of gravity position vector, define the following variables:

[0211]

[0212] Among them, x 1e is the position error, x 1d For x 1 The expected position of x 1r is the system error vector, Λ>0.

[0213] The sliding mode function is:

[0214] Force error: e λ =λ-λ d

[0215] Among them, λ d is the expected contact force.

[0216] Design the controller as:

[0217]

[0218] in, D 1 , C 1 , G 1 The estimate of K d >0, w=diag(w 1 ,...,w n ), K λ >0.

[0219] Its parameter linearization relationship is:

[0220]

[0221] Among them, Y ais the regression matrix, p a is the unknown constant vector of the system.

[0222] The term used to control the force: λ r =λ d -k λ e λ

[0223] Then the position and force controllers are:

[0224]

[0225] Adaptive Law for:

[0226] because, is the parameter estimation error, then Γ=Γ T >0.

[0227] In order to make the robot effectively control the object, internal force control is added to the constrained multi-robot system controller, and the result is:

[0228]

[0229] in:

[0230]

[0231] Assume that the term used to control the internal forces is:

[0232] F Ir =F Id -K I e FI

[0233] Among them, K I >0,e FI =F I -F Id , F Id To expect inner strength.

[0234] By τ a =J o T J e -T τ and J o T F I =0, we get:

[0235] τ a =J o T J e -T τ+J oT F Ir

[0236] The final adaptive reduced-order sliding mode controller is:

[0237] τ=J e T (J o T ) + τ a +J e T (F Id -K I e FI )

[0238] The above description is only a specific implementation of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by a technician familiar with the technical field within the technical scope disclosed by the present invention should be covered by the protection scope of the present invention.

Claims

1. A mechanical arm force-position hybrid control method based on an adaptive reduced-order sliding mode algorithm, the method comprising: When receiving a control instruction carrying expected position information, obtaining position information of the object controlled by the robot arm; Inputting the position information of the controlled object and the desired position information into an internal-external loop force-position hybrid control system composed of a pre-constructed reduced-order constrained dual-two-degree-of-freedom multi-manipulator system dynamics model and an adaptive reduced-order sliding mode controller, so that the internal-external loop force-position hybrid control system outputs a control signal to control the manipulator to move along a desired motion trajectory according to the position information and the desired position information; The pre-built reduced-order dynamics model of the multi-manipulator system with dual two-degree-of-freedom constraints is: Where x = [x1 T ,x2 T ] T is the current position information of the controlled object, is the first-order derivative of x1, is the second-order derivative of x1, D L (x1)∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G L (x1)∈R n is the gravity vector, n is the degree of freedom of the robot, R (n*n) represents the column vector of n*n dimensional real space, τ L To control the input torque, The pre-built adaptive reduced-order sliding mode controller is: τ=J e T (J o T ) + t a +J e T (F Id -K I e FI ) in, Y a is the regression matrix, p a is the unknown constant vector of the system, are the estimates of D1, C1, G1, and K d >0, w=diag(w1,...,w n ), K λ >0, s is the sliding mode function, J o , J e , J c is the Jacobian matrix, used to control the force term λ r =λ d -k λ e λ ,λ d is the expected contact force, and the force error e λ =λ-λ d , λ is the contact force, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

2. The method according to claim 1, characterized in that The way to build a multi-manipulator system model includes: A multi-manipulator system consisting of k n-DOF manipulators and a controlled object is established, and the dynamic model of the i-th manipulator is: Where: q i ∈R n is the robot arm angle vector, Q i The first derivative of Q i The second derivative of i (q i )∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G i (q i )∈R n is the gravity term vector, J ei (q i )∈R (n *n) For i The Jacobian matrix of the position vector to the end of the robot arm, F ei ∈R n is the force exerted by the end of the robot arm on the controlled object, τ i To control the input torque; The multi-manipulator dynamics model consisting of k manipulators can be obtained: The dynamic equation of the controlled object is established as: Where x = [x1 T ,x2 T ,...,x n T ] T ∈R n is the position vector of the controlled object, D o (x)∈R (n*n) is the inertia matrix of the controlled object, is the centrifugal force and Coriolis force matrix of the controlled object, G o (x)∈R n is the gravity vector of the controlled object, F o ∈R n It is the external force on the controlled object; Establish a dynamic model of a multi-manipulator system: Take x ei ∈R n is the position vector of the end of the i-th robot arm, with x e =[x e1 T ,x e2 T ,...,x ek T ] T ∈R kn ,set up: in, is the vector from q to position x e The transformation matrix of in: After transformation, the above equation can be obtained: in, τ a =J o T J e -T τ, J o , J e is the Jacobian matrix.

3. The method according to claim 2, characterized in that Finally, the dynamic model of the constrained dual two-degree-of-freedom multi-manipulator system is established: Assume that the multi-manipulator system operates in an m-dimensional constraint environment, and take x c is the contact point between the object and the constraint surface, then for the object's center of gravity position vector x, we have x c =h(x), so the constraint environment can be expressed as: ψ(x)=Φ(h(x))=Φ(x c )=0 Taking the derivative of both sides with respect to time t, we get: in, is the vector from x to the contact point x c The Jacobian matrix of The dynamic equation of the constrained object is: Among them, F c =J c T λ∈R n is the constraint force on the object, λ∈R m is the contact force vector, but: In order to facilitate the control of the multi-manipulator system, let x = [x1 T ,x2 T ] T , since the object operates in an m-dimensional constraint environment, the system's degree of freedom is reduced to nm, then x1∈R n-m , x2∈R m , take x1 to describe the constrained motion of the robot arm, and x1 can be used to represent x2, which is expressed as: x2 = σ (x1), then: in, Right now: The dynamic equation of the multi-manipulator system with two degrees of freedom constrained is expressed as: Where D1 = D a L, G1=G a， Multiply both sides by L T (x1), we can derive: Among them, D L =L T D1, C L =L T C1, G L =L T G1, τ L =L T τ a ,and is a skew-symmetric matrix.

4. The method according to claim 3, characterized in that The methods of constructing an adaptive reduced-order sliding mode controller include: Let x d For the desired object center of gravity position vector, define the following variables: x 1e =x1-x 1d , Among them, x 1e is the position error, x 1d is the expected position of x1, x 1r is the system error vector, Λ>0, The sliding mode function is: Force error: e λ =λ-λ d , where λ d is the expected contact force, Design the controller as: in, are the estimates of D1, C1, G1, and K d >0, w=diag(w1,...,w n ), K λ > 0, term used to control force: λ r =λ d -k λ e λ， Then the position and force controllers are: In order to enable the robot to effectively control the object, internal force control is added to the controller, and we get: in: Assume that the term used to control the internal force is: F Ir =F Id -K I e FI , where K I >0,e FI =F I -F Id , F Id To expect inner strength, By τ a =J o T J e -T τ and J o T F I =0, we get: t a =J o T J e -T t+J o T F Ir The final adaptive reduced-order sliding mode controller is: τ=J e T (J o T ) + t a +J e T (F Id -K I e FI ) in, Y a is the regression matrix, p a is the unknown constant vector of the system, are the estimates of D1, C1, G1, and K d >0, w=diag(w1,...,w n ), K λ >0, s is the sliding mode function, J o , J e , J c is the Jacobian matrix, used to control the force term λ r =λ d -k λ e λ ,λ d is the expected contact force, and the force error e λ =λ-λ d , λ is the contact force, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

Images