CN114840947A - A constrained three-degree-of-freedom manipulator dynamic model - Google Patents

Abstract

The invention relates to a three-degree-of-freedom mechanical arm dynamics model with constraint, which is established by utilizing an Euler-Lagrange method, gives a dynamic equation expression form of a state space of the mechanical arm dynamics model, and calculates the motion planning of the mechanical arm dynamics model based on a particle swarm algorithm. The invention has the capability of quick response; accurate position deviation can be obtained; the control method has stronger robustness and adaptivity; the speed regulation range and the torque output range are large enough.

Description

A constrained three-degree-of-freedom manipulator dynamic model

technical field

The invention relates to the technical field of industrial robots, in particular to a constrained three-degree-of-freedom mechanical arm dynamics model.

Background technique

The robotic arm is widely used in production and life. As a mechatronics device, it can efficiently complete various complex and dangerous operations and improve production efficiency. It has been widely used in industry, aerospace, medical and other fields. The motion control of the robotic arm is essentially a multi-axis position follow-up system used for the motion control of the robotic arm joints. The controlled quantity (output quantity) is the angular displacement of the spatial position of the load machine. When the position given quantity (input quantity) does a specific task When changing, the system needs to make the output quickly and accurately track the change of the given amount. Therefore, the motion control system of the robot arm includes two parts: trajectory planning and trajectory tracking. Its task is not only to plan the walking trajectory of the robot arm, but also the angular displacement, angular velocity and angular acceleration of the servo drive mechanism of each axis. Closed-loop control is performed to realize joint control of the overall pose of the manipulator. Robotic arm trajectory planning is a method of calculating the trajectory according to the task requirements and studying the displacement, angular velocity and angular acceleration of the robotic arm joints in motion. These trajectories can be generated either in joint space or in Cartesian space. For trajectory planning in joint space, joint variables are used to describe the motion of the manipulator, which has good real-time performance, but it is difficult to determine the posture of the connecting rod and end effector; for trajectory planning in Cartesian space, the path constraints are mapped into joint space coordinates, and then determine joint trajectories that satisfy the parametric constraints. In comparison, trajectory planning in joint space requires less computation, high real-time performance, and can avoid singularity.

In the prior art, some of them start from the accuracy of the model and the uncertainty of the model parameters to study its influence on the control accuracy, such as "Model Uncertainty Manipulator Motion Control Method Based on Multi-layer Neural Network", "A Flexible control method and system for multi-degree-of-freedom manipulators”; part of the research only studies whether the trajectory design is reasonable, such as “a multi-point motion trajectory planning method for manipulators”, etc., but in the process of trajectory planning, for specific tasks The discussion of the required trajectory function and its constraints needs to be further refined; there are also some patents dedicated to the design of trajectory tracking controllers, such as "A trajectory tracking algorithm for decentralized neural robust control of robotic arms", these patents do not Considering the factors of the rationality of the trajectory, the influence of the inertial force of the manipulator is ignored, which reduces its applicability, and at the same time, it needs to be further improved in terms of tracking accuracy and system stability.

SUMMARY OF THE INVENTION

In order to overcome the deficiencies of the prior art, the present invention designs a reasonable three-degree-of-freedom manipulator dynamic model with constraints. The model has the following four basic characteristics: fast response capability; accurate position deviation can be obtained ; The control method has strong robustness and adaptability; the speed regulation range and torque output range are large enough.

The technical scheme of the present invention is as follows: a constrained three-degree-of-freedom manipulator dynamic model, the Euler-Lagrange method is used to establish the manipulator dynamic model, and the dynamic equation expression form of its state space is given, Based on the particle swarm algorithm, the motion planning of the dynamic model of the manipulator is calculated, and the dynamic equation of the dynamic model of the manipulator is:

是哥氏力和离心力的结合向量；Where: q=[q ₁ , q ₂ , q ₃ ] ^T is the angle vector of the system; τ=[τ ₁ , τ ₂ , τ ₃ ] ^T is the control torque vector; M(q)∈R ^3×3 is the inertia Matrix, with positive definiteness and symmetry;

is the combination vector of Coriolis force and centrifugal force;

Then the state space form of the dynamic model of the manipulator is:

in:

f(x)=[x ₄ ,x ₅ ,x ₆ ,f ₁ ,f ₂ ,f ₃ ]

g(x)=[g _1(x) ,g _2(x) ,g _3(x) ] ^T

则有：x is the state vector variable, f(x) is the state objective function, g(x) is the nonlinear constraint correlation function about x, g _1(x) is the third-order zero matrix, g _ij is about g(x) component function; let

Then there are:

的具体分别为如下形式：The M(q) and

The specific forms are as follows:

The particle swarm optimization algorithm is a stochastic optimization algorithm based on a population, each individual in the population is called a particle, the size of the population is N, and the position vector of each particle in the D-dimensional space is X _i =( x _i1 ,x _i2 ,x _i3 ,x _i4 ,...,x _id ,...,x _iD ); the velocity vector is V _i =(v _i1 ,v _i2 ,v _i3 ,v _i4 ,..., v _id ,...,v _iD ); the optimal position of the individual is P _i =( _pi1 , _pi2 , _pi3 , _pi4 ,..., _pid ,..., _piD ); the The best position of the group is expressed as P _g = (p _g1 ,p _g2 ,p _g3 ,p _g4 ,...,p _gd ,...,p _gD ), and the update model of the individual optimal position is:

The best position of the group is the best position of all individuals, and the update model of its speed and position is as follows:

Among them, ω is the inertia weight, which determines the strength of the algorithm's convergence speed. The larger the ω, the stronger the convergence speed, and vice versa. Experiments show that the particle swarm algorithm has a faster convergence speed when ω is between 0.8 and 1.2. c ₁ and c ₂ are learning factors, also called acceleration constants; r ₁ and r ₂ are uniform random numbers in the range of [0,1].

The steps of time-optimal particle swarm programming under multiple constraints are:

(1) Determine the dimension of the particle, determine the initial population size of the particle swarm, initialize the position and speed of the particle, and set the optimal fitness value;

(2) Set a fitness function, and initially screen particles with better fitness values, and calculate the coefficients of the polynomial;

(3) Calculate the joint velocity and acceleration trajectory from the obtained polynomial coefficients, and check whether the set maximum limit value is exceeded, that is, equations (7) and (8) are satisfied;

(4) Traverse all particles, calculate the fitness value of each particle, and check whether each fitness value is smaller than the updated optimal fitness value, and at the same time check the corresponding polynomial coefficient calculated by the particle. Whether there is a value exceeding the limit of the joint velocity and acceleration trajectory of . If it does not exist, the optimal particle is updated, and the optimal fitness value is updated at the same time; if it exists, the particle is skipped, and the next particle is detected until all particles are detected;

(5) Each particle is compared with the fitness value of the updated optimal particle, and the current overall optimal particle is obtained, and then compared with the global optimal particle experienced by the group. If the fitness value is smaller, then Update the global optimal particle and optimal fitness value;

(6) Iterate until the number of terminations, stop updating, and obtain the corresponding polynomial coefficients according to the global optimal particle, and calculate the corresponding joint angle, angular velocity, and angular acceleration trajectory.

Beneficial effects of the present invention:

1) The model of the present invention has the ability to respond quickly;

2) The model of the present invention can obtain accurate position deviation;

3) The model control method of the present invention has strong robustness and adaptability;

4) The model speed regulation range and torque output range of the present invention are sufficiently large.

Description of drawings:

Fig. 1 is the structural representation of the present invention in the system scheme;

2 is a three-joint angle trajectory diagram of the present invention;

Fig. 3 is the flow chart of the particle swarm algorithm of the present invention in the trajectory planning method;

4 is a three-joint angular velocity trajectory diagram of the present invention;

5 is a three-joint acceleration trajectory diagram of the present invention;

6 is a three-joint moment curve diagram of the present invention;

FIG. 7 is a curve diagram of three-joint trajectory tracking error according to the present invention.

Detailed ways:

Referring to FIGS. 1 to 7 , the present invention relates to a constrained three-degree-of-freedom manipulator dynamic model. The Euler-Lagrange method is used to establish the manipulator dynamic model, and its state space expression is given. Form, based on particle swarm algorithm to calculate the motion planning of the dynamic model of the manipulator, the dynamic equation of the dynamic model of the manipulator is:

是哥氏力和离心力的结合向量。Where: q=[q ₁ , q ₂ , q ₃ ] ^T is the angle vector of the system; τ=[τ ₁ ,0,τ ₃ ] ^T is the control torque vector; M(q)∈R ^{3 ×3} is the inertia matrix , with positive definiteness and symmetry;

is the combined vector of Coriolis force and centrifugal force.

Then the state space form of the system model is:

in:

f(x)=[x ₄ ,x ₅ ,x ₆ ,f ₁ ,f ₂ ,f ₃ ]

g(x)=[g _1(x) ,g _2(x) ,g _3(x) ] ^T

则有：x is the state vector variable, f(x) is the state objective function, g(x) is the nonlinear constraint correlation function about x, g _1(x) is the third-order zero matrix, g _ij is about g(x) component function; let

Then there are:

的具体分别为如下形式：Preferably, the M(q) and

The specific forms are as follows:

The particle swarm optimization algorithm is a group-based random optimization algorithm, each individual in the group is called a particle, the size of the group is N, and the position vector of each particle in the D-dimensional space is X _i. =(x _i1 ,x _i2 ,x _i3 ,x _i4 ,...,x _id ,...,x _iD ); the velocity vector is V _i =(v _i1 ,v _i2 ,v _i3 ,v _i4 ,.. .,v _id ,...,v _iD ); the optimal position of the individual is P _i =( _pi1 , _pi2 , _pi3 , _pi4 ,..., _pid ,..., _piD ); The optimal position of the group is expressed as P _g =(p _g1 ,p _g2 ,p _g3 ,p _g4 ,...,p _gd ,...,p _gD ), and the update model of the individual optimal position is:

The best position of the group is the best position of all individuals, and the update model of its speed and position is as follows:

Among them, ω is the inertia weight, which determines the strength of the algorithm's convergence speed. The larger the ω, the stronger the convergence speed, and vice versa. Experiments show that the particle swarm algorithm has a faster convergence speed when ω is between 0.8 and 1.2. c ₁ and c ₂ are learning factors, also called acceleration constants; r ₁ and r ₂ are uniform random numbers in the range of [0,1].

The invention designs the trajectory tracking controller according to the constraint relationship between the links of the robotic arm system.

The sliding surface is constructed as the formula:

The formula can be obtained by derivation of the sliding mode surface:

β₁，β₂，β₃为常数。in:

β ₁ , β ₂ , and β ₃ are constants.

The sliding mode reaching law is designed as the formula:

k_i＞1，η_i＞0，ζ_i＞0，双幂次趋近律以|S_i|＝1为界，将系统分为两个阶段：in

_ki > 1, η _i > 0, ζ _i > 0, the bi-power reaching law is bounded by |S _i |=1, dividing the system into two stages:

此时

越大，趋近速度越快，但带来的抖振也相应增大，η_i越小系统速度和抖振同时减小。The first stage: when |S _i | < 1, the system function mainly depends on the previous term

at this time

The larger the value is, the faster the approach speed is, but the chattering also increases accordingly. The smaller η _i is, the system speed and chattering decrease at the same time.

k_i越大，趋近速度越快的同时抖振也增大；ζ_i越小，抖振和速度同时变小。因此，采用这种方法需要根据实验选择合适的参数。同时，引入边界层使系统状态控制在一定范围内，并用饱和函数来定义，选取的饱和函数为公式：The second stage: when |S _i |>1, the system function mainly depends on the second term

The larger the k _i is, the faster the approach speed is and the chattering also increases; the smaller the ζ _i is, the smaller the chattering and the speed are at the same time. Therefore, adopting this method requires the selection of appropriate parameters according to the experiment. At the same time, a boundary layer is introduced to control the state of the system within a certain range, and is defined by a saturation function. The selected saturation function is the formula:

Among them, in order to ensure the stability of the system, let 1<γ<2. At this time, the overall approach law of the system is the formula:

The control law of each joint τ=[τ ₁ , τ ₂ , τ ₃ ] ^Τ can be obtained from the above formula.

The following set conditions are given:

|v _i,j |≤1.57rad/s

|a _i,j |≤2.09rad/s ²

The starting points of joints 1, 2, and 3 and the joint vectors of the three fixed points are:

q ₁ =[0.50,1.54,0.32,-0.55]rad,

q ₂ =[-1.20,-1.51,-1.02,0.2798]rad,

q ₃ =[1.70,0.04,-2.20,-1.09]rad;

The number of iterations of the particle swarm algorithm is 200.

According to this, the angle, angular velocity and angular acceleration trajectory of the three joints are obtained as shown in Figure 3-Figure 5. The structural block diagram of the three-five-three combination polynomial interpolation method based on particle swarm optimization adopted in this patent is shown in FIG. 2 .

The invention uses the cubic polynomial function to perform trajectory planning for every two fixed points.

Firstly, the starting point of the manipulator and the spatial positions of the three fixed points are determined in the Cartesian space, and then the corresponding joint positions are obtained through the inverse kinematics model of the manipulator. Since there is one starting point and three waypoints in the trajectory, there are three segments of trajectory interpolation. Set the following parameters: i=1,2,3 represents the number of joints; j=1,2,3 represents the number of interpolation trajectory segments; θ _i,j represents the interpolation angle vector of the ith joint.

Establish the 3-5-3 polynomial function of the i-th joint angle with respect to time, as shown in formula (3):

Among them, l _i,1 (t) represents the first segment of the cubic polynomial interpolation trajectory of the ith joint angle; li _,2 (t) is the second segment of the quintic polynomial interpolation trajectory of the ith joint angle; li _{, 3} (t) represents the third segment of the cubic polynomial interpolation trajectory of the ith joint angle; the coefficient a _i is the correlation coefficient corresponding to the polynomial.

Let the coefficients a _i of the polynomial be a vector a=[a _i,13 ,a _i,12 ,a _i,11 ,a _i,10 ,a _i,25 ,...,a _i,20 ,a _i,33 ,...,a _i,30 ] ^Τ , let the time matrix of the polynomial function be T, then the corresponding matrix solution is T ^14×14 a ^14×1 =θ ^14×1 , where T and θ formula (4) and Equation (5) is shown. When the initial velocity and acceleration of the joint during the second segment trajectory planning are consistent with the velocity and acceleration of the joint at the end of the first segment trajectory, the first segment trajectory needs to be completed before the second segment quintic polynomial interpolation trajectory planning. In the time matrix T, each row corresponds to the element of the column vector a: rows 1 to 3 represent the first segment of cubic polynomial interpolation, and the trajectory of the initial moment corresponding to the second segment of the quintic polynomial corresponding to the initial moment needs to be eliminated. Do the same for the nine lines.

θ=[0,0,0,0,0,0,x ₃ ,0,0,x ₀ ,0,0,x ₂ ,x ₁ ] ^Τ

Among them, x ₃ corresponds to the seventh row of T and the seventh element of a, which represents the displacement of the third stage of cubic polynomial interpolation trajectory planning; x ₀ represents the initial displacement of the first stage of cubic polynomial interpolation planning when time is 0; x ₂ represents When the time is 0, the initial displacement of the third-stage cubic polynomial interpolation plan; x ₁ represents the initial displacement of the second-stage quintic polynomial interpolation plan when the time is 0.

The invention designs a time-optimized joint trajectory planning method under multiple constraints, and performs time-optimal planning on the trajectory between fixed path points. The specific plans are as follows:

For the multi-fixed waypoint trajectory planning task, the optimal time index should be the minimum sum of multiple time periods, then the fitness function should be defined as formula (6):

f(t)=(t ₁ +t ₂ +t ₃ ) _min

Due to various reasons such as the structure of the manipulator system, material structure and motor selection, the manipulator will have the limit of joint speed and acceleration. If such limiting factors are not fully taken into account in trajectory planning, the resulting trajectory usually produces rapid changes in speed and acceleration in a short period of time, and does not conform to the actual working conditions of the manipulator. In the actual execution process, this kind of unstable trajectory will not only cause safety hazards to the surrounding environment, but also irreversibly affect or even damage the system structure of the manipulator. At the same time, the requirements for trajectory tracking control are higher, and the control difficulty is also higher big. In order to ensure the safety and accuracy of the task and meet the actual requirements of the manipulator system, the dual constraints of speed and acceleration are introduced in the trajectory planning task. Set speed constraints:

where v _i,j represents the velocity of the j-th trajectory of the i-th joint, and V _i,j is the maximum speed limit of the j-th trajectory of the i-th joint. Set acceleration constraints:

where a _i,j represents the acceleration of the j-th trajectory of the i-th joint, and aa _i,j is the maximum acceleration limit of the j-th trajectory of the i-th joint.

The steps of time-optimal particle swarm programming under multiple constraints are:

(1) Determine the dimension of the particle, determine the initial population size of the particle swarm, initialize the position and speed of the particle, and set the optimal fitness value;

(2) Set a fitness function, and initially screen particles with better fitness values, and calculate the coefficients of the polynomial;

(3) Calculate the joint velocity and acceleration trajectory from the obtained polynomial coefficients, and check whether the set maximum limit value is exceeded, that is, equations (7) and (8) are satisfied;

(4) Traverse all particles, calculate the fitness value of each particle, and check whether each fitness value is smaller than the updated optimal fitness value, and at the same time check the corresponding polynomial coefficient calculated by the particle. Whether there is a value exceeding the limit of the joint velocity and acceleration trajectory of . If it does not exist, the optimal particle is updated, and the optimal fitness value is updated at the same time; if it exists, the particle is skipped, and the next particle is detected until all particles are detected;

(5) Each particle is compared with the fitness value of the updated optimal particle, and the current overall optimal particle is obtained, and then compared with the global optimal particle experienced by the group. If the fitness value is smaller, then Update the global optimal particle and optimal fitness value;

(6) Iterate until the number of terminations, stop updating, and obtain the corresponding polynomial coefficients according to the global optimal particle, and calculate the corresponding joint angle, angular velocity, and angular acceleration trajectory.

From the experimental results shown in Figures 3-5, it can be seen that the angular velocity and angular acceleration trajectory of each joint does not exceed the maximum absolute value, and the joint trajectory satisfies all constraints. The joint angular velocity trajectory and the joint angular acceleration trajectory are closer to the restricted maximum value in the second segment of the trajectory, indicating that the overall second segment of the joint trajectory is accelerating and converging. In addition, since the cubic polynomial interpolation method and the quintic polynomial interpolation method need to set the time, if the time is not set properly, the joint speed and acceleration will be unstable, the system stability will be reduced, and the task completion efficiency will become low or even impossible. The trajectory planning method based on particle swarm optimization designed in this patent can ensure the smoothness and continuity of each joint angle, angular velocity, and angular acceleration trajectory under the premise of ensuring the shortest time, and improve the efficiency and accuracy of task completion.

For the desired trajectory given in Figure 3, the patent uses an improved synovial controller to track and control it. The obtained control law is shown in Figure 6, and its trajectory tracking error curve is shown in Figure 7.

It can be seen from Figure 6 that the torque obtained by the sliding mode control using the double-power saturation reaching law is relatively stable, and there is no obvious chattering, which can keep the motor output within the rated power and ensure the accuracy of the robot arm to perform tasks.

It can be seen from Figure 7 that the trajectory tracking errors of the three joints are all small, the maximum amplitude is 0.015rad, and it converges to near 0 within a certain period of time, with high trajectory tracking accuracy.

Claims (4)

1. A constrained three-degree-of-freedom manipulator dynamic model, the manipulator dynamic model is established by the Euler-Lagrange method, and the dynamic equation expression form of its state space is given, based on a particle swarm The algorithm calculates the motion planning of the dynamic model of the mechanical arm, and is characterized in that: the dynamic equation of the dynamic model of the mechanical arm is:

Where: q=[q ₁ , q ₂ , q ₃ ] ^T is the angle vector of the system; τ=[τ ₁ , τ ₂ , τ ₃ ] ^T is the control torque vector; M(q)∈R ^3×3 is the inertia Matrix, with positive definiteness and symmetry;

is the combination vector of Coriolis force and centrifugal force; The state space form of the robotic arm dynamics model is:

in: f(x)=[x ₄ ,x ₅ ,x ₆ ,f ₁ ,f ₂ ,f ₃ ] g(x)=[g _1(x) ,g _2(x) ,g _3(x) ] ^T

x is the state vector variable, f(x) is the state objective function, g(x) is the nonlinear constraint correlation function about x, g _1(x) is the third-order zero matrix, g _ij is about g(x) component function; let

Then there are:

2. the three-degree-of-freedom mechanical arm dynamics model of band constraint according to claim 1 is characterized in that: The M(q) and

The specific forms are as follows:

3. The constrained three-degree-of-freedom manipulator dynamics model according to claim 2, wherein the particle swarm optimization algorithm is a group-based random optimization algorithm, and each individual in the group is called is the particle, the size of the group is N, and the position vector of each particle in the D-dimensional space is X _i =(x _i1 ,x _i2 ,x _i3 ,x _i4 ,...,x _id ,..., x _iD ); the velocity vector is V _i =(v _i1 ,v _i2 ,v _i3 ,v _i4 ,...,v _id ,...,v _iD ); the optimal position of the individual is P _i =(pi _i1 ,p _i2 ,p _i3 ,p _i4 ,...,p _id ,...,p _iD ); the optimal position of the population is expressed as P _g =(p _g1 ,p _g2 ,p _g3 ,p _g4 , ...,p _gd ,...,p _gD ), the update model of the individual optimal position is:

The best position of the group is the best position of all individuals, and the update model of its speed and position is as follows:

Among them, ω is the inertia weight, and the particle swarm algorithm has a faster convergence speed when ω is between 0.8 and 1.2; c ₁ and c ₂ are learning factors, also known as acceleration constants; r ₁ and r ₂ are in the range of [0,1] uniform random number within . 4. The three-degree-of-freedom mechanical arm dynamics model with constraints according to claim 3, wherein the step of the particle swarm optimization algorithm is: (1) Determine the dimension of the particle, determine the initial population size of the particle swarm, initialize the position and speed of the particle, and set the optimal fitness value; (2) Set a fitness function, and initially screen particles with better fitness values, and calculate the coefficients of the polynomial; (3) Calculate the joint speed and acceleration trajectory from the obtained polynomial coefficients, and check whether the set maximum limit value is exceeded; (4) Traverse all particles, calculate the fitness value of each particle, and check whether each fitness value is smaller than the updated optimal fitness value, and at the same time check the corresponding polynomial coefficient calculated by the particle. Whether there is any value exceeding the limit of the joint speed and acceleration trajectory of particle; (5) Each particle is compared with the fitness value of the updated optimal particle, and the current overall optimal particle is obtained, and then compared with the global optimal particle experienced by the group. If the fitness value is smaller, then Update the global optimal particle and optimal fitness value; (6) Iterate until the number of terminations, stop updating, and obtain the corresponding polynomial coefficients according to the global optimal particle, and calculate the corresponding joint angle, angular velocity, and angular acceleration trajectory.

Images