A multi-manipulator force position control method based on fuzzy adaptive sliding mode algorithm

Abstract

The present application relates to a multi-manipulator force and position control method based on a fuzzy adaptive sliding mode algorithm, the method comprising: when a control instruction carrying expected position information is received, obtaining the position information of the object controlled by the manipulator; inputting the position information of the controlled object and the expected position information into a force and position control system composed of a pre-built multi-manipulator system dynamics model affected by uncertain factors such as friction and a fuzzy adaptive sliding mode controller, so that the force and position 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 multi-mechanical arm force position control method based on a fuzzy adaptive 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. Multi-arm systems are playing an important role in many fields such as industrial production and finished product assembly. However, there are still many problems. First, multi-arm systems are easily affected by uncertain factors such as friction in the actual working process, which leads to major problems in the control accuracy and stability of the multi-arm systems; second, when multiple arm end effectors control the same object at the same time, clamping internal force will be generated. If the internal force is too small, the robot arm will not be able to stably control the object. If the internal force is too large, it will cause damage to the controlled object.

[0003] Therefore, first of all, we need to overcome the influence of uncertain factors such as friction on the system, and secondly, we need to ensure that the robot arm can stably control the object without causing damage to the object, and these are the two major problems we need to solve. In response to the above problems, domestic and foreign experts have proposed many control methods. Such as sliding mode control, adaptive control, neural network control, etc. Kawasaki et al. proposed a distributed adaptive coordination control method for multi-robot systems and introduced a new reference model. Sun et al. improved the sliding mode control strategy to promote the uniform convergence of the control strategy to zero within a finite time. Hamamci et al. proposed an adaptive controller for parameter uncertainty to achieve asymptotic synchronization of the network. Y. Yang et al. designed a new non-singular integral terminal sliding mode control, thereby solving the fixed-time synchronization control problem of bilateral remote control operation systems with external disturbances and system uncertainties.

[0004] However, most of these control strategies only solve a single problem, and the designed control schemes are relatively complex. Summary of the invention

[0005] In view of the above technical problems, a multi-manipulator force and position control method based on fuzzy adaptive sliding mode algorithm is provided, which can overcome the influence of uncertain factors and perform internal force control and position control, and ultimately improve the system response speed and controller control accuracy;

[0006] A multi-manipulator force position control method based on fuzzy adaptive 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] The position information of the controlled object and the expected position information are input into a force-position control system composed of a pre-built multi-manipulator system dynamics model affected by uncertain factors such as friction and a fuzzy adaptive sliding mode controller, so that the force-position control system outputs a control signal to control the manipulator to move along a desired motion trajectory according to the position information and the expected position information;

[0009] The pre-built dynamic model of the multi-manipulator system affected by uncertain factors such as friction is:

[0010]

[0011] Where x = [x1 T ,x2 T ,...,x n T ] T ∈R n is the current position information of the controlled object, is the first-order derivative of x, is the second-order derivative of x, D a (x)∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G(x)∈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, J o , J e are all Jacobian transformation matrices, is a term composed of uncertain factors such as friction, τ a Input torque for control.

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

[0013]

[0014] in, x e is the position error, x d is the expected position of x, x r is the system error vector, Λ>0, For x r The first derivative of For x r The second-order derivative of for The estimate of K d >0, w=diag(w1,...,w n ), s is the sliding mode function, J o , Je is the Jacobian matrix, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

[0015] The methods for constructing the dynamic model of a multi-manipulator system affected by uncertain factors such as friction include:

[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, Q i The first derivative of Q i The second derivative of i (q i )∈R (n*n) is the mass matrix, is the centrifugal force and Coriolis force matrix, G i (q i )∈R n is the gravity matrix, J ei (q i )∈R (n*n) For i The Jacobian matrix of the position vector of the end of the robot arm, F ei ∈R n is the force acting at the end of the robot arm, is a term composed of uncertain factors such as friction, and τ is the control 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 = [x1 T ,x2 T ,...,x n T ] T ∈R n is the position vector of the controlled object, Do (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 affected by uncertain factors such as friction:

[0025] Take x li ∈R n is the position vector of the end of the i-th robot arm, with x l =[x l1 T ,x l2 T ,...,x lk T ] T ∈R kn ,set up: in, is the vector from q to position x l The transformation matrix of .

[0026]

[0027] in:

[0028] After transformation, the above equations can be obtained into the dynamic model of multi-manipulator system affected by uncertain factors such as friction:

[0029]

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

[0031] The methods of constructing the fuzzy adaptive sliding mode controller include:

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

[0033]

[0034] Among them, x e is the position error, x d is the expected position of x, xr is the system error vector, Λ>0.

[0035] The sliding mode function is:

[0036] Design the controller as:

[0037]

[0038] in, for The estimate of K d >0, w=diag(w1,...,w n ).

[0039] By transforming the above controller, the final fuzzy adaptive sliding mode controller is:

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

[0041] in, x e is the position error, x d is the expected position of x, x r is the system error vector, Λ>0, For x r The first derivative of For x r The second-order derivative of for The estimate of K d >0, w=diag(w1,...,w n ), s is the sliding mode function, J o , J e is the Jacobian matrix, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

[0042] The above-mentioned multi-manipulator force and position control method based on fuzzy adaptive 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 a force and position control system composed of a pre-built multi-manipulator system dynamics model affected by uncertain factors such as friction and a fuzzy adaptive sliding mode controller, so that the force and position 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

[0043] Figure 1 It is a step diagram of the multi-manipulator force position control method based on fuzzy adaptive sliding mode algorithm;

[0044] Figure 2 This is a specific flow chart of the multi-manipulator force position control method based on the fuzzy adaptive sliding mode algorithm;

[0045] Figure 3 This is a model diagram of a dual two-degree-of-freedom robotic arm; DETAILED DESCRIPTION

[0046] like Figure 1 As shown, a multi-manipulator force position control method based on a fuzzy adaptive sliding mode algorithm is provided, comprising the following steps:

[0047] A multi-manipulator force position control method based on fuzzy adaptive sliding mode algorithm, the method comprising:

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

[0049] The position information of the controlled object and the expected position information are input into a force-position control system composed of a pre-built multi-manipulator system dynamics model affected by uncertain factors such as friction and a fuzzy adaptive sliding mode controller, so that the force-position control system outputs a control signal to control the manipulator to move along a desired motion trajectory according to the position information and the expected position information;

[0050] The pre-built dynamic model of the multi-manipulator system affected by uncertain factors such as friction is:

[0051]

[0052] Where x = [x1 T ,x2 T ,...,x n T ] T ∈R nis the current position information of the controlled object, is the first-order derivative of x, is the second-order derivative of x, D a (x)∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G(x)∈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, J o , J e are all Jacobian transformation matrices, is a term composed of uncertain factors such as friction, τ a Input torque for control.

[0053] The pre-built fuzzy adaptive sliding mode controller is:

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

[0055] in, x e is the position error, x d is the expected position of x, x r is the system error vector, Λ>0, For x r The first derivative of For x r The second-order derivative of for The estimate of K d >0, w=diag(w1,...,w n ), s is the sliding mode function, J o , J e is the Jacobian matrix, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

[0056] The methods for constructing the dynamic model of a multi-manipulator system affected by uncertain factors such as friction include:

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

[0058]

[0059] 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 mass matrix, is the centrifugal force and Coriolis force matrix, G i (q i )∈R n is the gravity matrix, J ei (q i )∈R (n*n) For i The Jacobian matrix of the position vector of the end of the robot arm, F ei ∈R n is the force acting at the end of the robot arm, is a term composed of uncertain factors such as friction, and τ is the control torque.

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

[0061]

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

[0063]

[0064] 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 exerted on the controlled object.

[0065] Establish a dynamic model of a multi-manipulator system affected by uncertain factors such as friction:

[0066] Take x li ∈R n is the position vector of the end of the i-th robot arm, with x l =[x l1 T ,x l2 T ,...,x lk T ] T ∈R kn ,set up: in, is the vector from q to position x l The transformation matrix of .

[0067]

[0068] in:

[0069] After transformation, the above equations can be obtained into the dynamic model of multi-manipulator system affected by uncertain factors such as friction:

[0070]

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

[0072] The methods of constructing the fuzzy adaptive sliding mode controller include:

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

[0074]

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

[0076] The sliding mode function is:

[0077] Design the controller as:

[0078]

[0079] in, for The estimate of K d >0, w=diag(w1,...,w n ).

[0080] By transforming the above controller, the final fuzzy adaptive sliding mode controller is:

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

[0082] in, x e is the position error, x d is the expected position of x, x r is the system error vector, Λ>0, For x r The first derivative of For x r The second-order derivative of for The estimate of K d >0, w=diag(w1,...,w n ), s is the sliding mode function, J o , J e is the Jacobian matrix, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

[0083] The above-mentioned multi-manipulator force and position control method based on fuzzy adaptive 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 a force and position control system composed of a pre-built multi-manipulator system dynamics model affected by uncertain factors such as friction and a fuzzy adaptive sliding mode controller, so that the force and position 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.

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

[0085] 1. Establish a multi-manipulator dynamics model

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

[0087]

[0088] 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 mass matrix, is the centrifugal force and Coriolis force matrix, G i (q i )∈R n is the gravity matrix, J ei (q i )∈R (n*n) For i The Jacobian matrix of the position vector of the end of the robot arm, F ei ∈R n is the force acting at the end of the robot arm, is a term composed of uncertain factors such as friction, and τ is the control torque.

[0089] The dynamic model of a multi-manipulator consisting of k manipulators is:

[0090]

[0091] Where:

[0092] q=[q1 T ,q2 T ,...,q k T ] T ∈R kn ;D(q)=blockdiag[D1(q1),D2(q2),...,D k (q k )]∈R kn *kn ;

[0093]

[0094] G=[G1 T ,G2 T ,...,G k T ]T ∈R kn ; J e =blockdiag[J e1 ,J e2 ,...,J ek ]∈R kn*kn ;

[0095] F e =[F e1 T ,F e2 T ,...,F ek T ] T ∈R kn ; F(q,q,q)=[F1 T ,F2 T ,...,F k T ] T ∈R kn ;

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

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

[0098] Property 1: There exist positive numbers k1, k2 that satisfy the following equation:

[0099] k1||s|| 2 ≤s T D(q)s≤k2||s|| 2

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

[0101]

[0102] 2. Establish the dynamic model of the controlled object

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

[0104]

[0105] 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 mass 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 matrix of the controlled object, F o ∈R n is the external force on the controlled object, and

[0106]

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

[0108] Since the sum of the internal forces acting on the object is zero, that is, J o T F I =0.

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

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

[0111] 3. The multi-manipulator system dynamics model is integrated by the multi-manipulator dynamics model and the controlled object dynamics model.

[0112] Take x li ∈R n is the position vector of the end of the i-th robot arm, such as Figure 3 , there are x l =[x l1 T ,x l2 T ,...,x lk T ]T ∈R kn ,set up:

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

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

[0115]

[0116] in,

[0117] There are also: Among them, J o (x)∈R (n*n) .

[0118] It can be deduced that:

[0119] have:

[0120] Taking the derivative of the left and right sides with respect to time, we get:

[0121]

[0122] It can be deduced that:

[0123]

[0124] in:

[0125] Because J o T F I =0, let the left and right sides of the derived equation be multiplied by J o T J e -T , the dynamic model of the multi-manipulator system can be obtained as follows:

[0126]

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

[0128] 3. Design a force position controller based on fuzzy adaptive sliding mode algorithm

[0129] Let x dFor the desired object center of gravity position vector, define the following variables:

[0130]

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

[0132] The sliding mode function is:

[0133] Design the controller as:

[0134]

[0135] in: for The estimate of K d >0, w=diag(w1,...,w n ).

[0136] Designing adaptive laws for:

[0137]

[0138] Where: Θ i is the ideal parameter, is the parameter estimation error, then Γ=Γ T >0.

[0139] For variable x i (i=1,2,...n), define m i A fuzzy set i bi (b i =1,2,...,m i ), available fuzzy rules to construct the fuzzy system, then

[0140]

[0141] in, for dimensional vector, is the membership function.

[0142] The fuzzy system is designed as:

[0143]

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

[0145] FIr =F Id -K I e FI

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

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

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

[0149] Then the final multi-manipulator system force position controller is:

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

[0151] 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 multi-manipulator force position control method based on fuzzy adaptive 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; The position information of the controlled object and the expected position information are input into a force-position control system composed of a pre-built multi-manipulator system dynamics model affected by uncertain factors such as friction and a fuzzy adaptive sliding mode controller, so that the force-position control system outputs a control signal to control the manipulator to move along a desired motion trajectory according to the position information and the expected position information; The pre-built dynamic model of the multi-manipulator system affected by uncertain factors such as friction is: Where x = [x1 T ,x2 T ,...,x n T ] T ∈R n is the current position information of the controlled object, is the first-order derivative of x, is the second-order derivative of x, D a (x)∈R (n*n) is the inertia matrix, is the centrifugal force and Coriolis force matrix, G a (x)∈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, J o , J e are all Jacobian transformation matrices, is a term composed of uncertain factors such as friction, τ a To control the input torque; The pre-built fuzzy adaptive sliding mode controller is: τ=J e T (J o T ) + t a +J e T (F Id -K I e FI ) in, x e is the position error, x d is the expected position of x, x r is the system error vector, Λ>0, For x r The first derivative of For x r The second-order derivative of for The estimate of K d >0, w=diag(w1,...,w n ), s is the sliding mode function, J o , J e is the Jacobian matrix, 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 methods for constructing the dynamic model of a multi-manipulator system affected by uncertain factors such as friction include: 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 mass matrix, is the centrifugal force and Coriolis force matrix, G i (q i )∈R n is the gravity matrix, J ei (q i )∈R (n*n) For i The Jacobian matrix of the position vector of the end of the robot arm, F ei ∈R n is the force acting at the end of the robot arm, is a term composed of uncertain factors such as friction, τ i To control the 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 affected by uncertain factors such as friction: Take x li ∈R n is the position vector of the end of the i-th robot arm, with x l =[x l1 T ,x l2 T ,...,x lk T ] T ∈R kn ,set up: in, is the vector from q to position x l The transformation matrix, in: After transformation, the above equations can be obtained into the dynamic model of multi-manipulator system affected by uncertain factors such as friction: Among them, t a =J o T J e -T t.

3. The method according to claim 2, characterized in that The methods of constructing the fuzzy adaptive sliding mode controller include: Let x d For the desired object center of gravity position vector, define the following variables: Among them, x e is the position error, x d is the expected position of x, x r is the system error vector, Λ>0, The sliding mode function is: Design the controller as: in, for The estimate of K d >0, w=diag(w1,...,w n ), By transforming the above controller, the final fuzzy adaptive sliding mode controller is: τ=J e T (J o T ) + t a +J e T (F Id -K I e FI ) in, x e is the position error, x d is the expected position of x, x r is the system error vector, Λ>0, For x r The first derivative of For x r The second-order derivative of for The estimate of K d >0, w=diag(w1,...,w n ), s is the sliding mode function, J o , J e is the Jacobian matrix, K I >0,e FI =F I -F Id , F Id is the expected internal force, F I For internal force.

Images