Six-degree-of-freedom robot (manipulator) kinematics modeling and motion planning series (4) - trajectory planning

1. Overview of trajectory planning

The main task of trajectory planning for the robot is to obtain a series of time series of robot terminal pose transformations according to the working environment and task requirements of the robot, so that the robot terminal can correctly move from the initial posture along the desired trajectory to the end. Posture, complete work tasks,. For a robot with six degrees of freedom, the key problem to be solved in trajectory planning is to transform the transformation of the end pose into the transformation of six joint variables, which is also the difference between it and the trajectory planning of mobile robots.
There are two methods for trajectory planning: one is planning in joint space, and the other is planning in Cartesian space, that is, Cartesian coordinate space.
(1) Joint space planning is to plan the spatial trajectory of the end of the manipulator by determining the sequence of rotation angle changes of each joint. Its advantage is that it only needs to plan the six joint variables separately, and the calculation is simple, but it cannot accurately control the pose change of the end of the mechanical arm.
(2) Cartesian space planning is to directly plan the pose change of the end of the manipulator. After obtaining the pose of the end of the manipulator at a certain moment, use the inverse kinematics equation to directly calculate the value of each joint rotation angle variable at that moment. The advantage is that the trajectory of the end of the manipulator is easy to observe, and the trajectory of the end can be controlled with more precise control, while avoiding collisions between the end effector and spatial obstacles. But the disadvantage is that a large number of inverse kinematics calculations are required, and a large number of intermediate interpolation points need to be selected to obtain an accurate end trajectory, and the control process is complicated.

2. Joint space planning

Robot joint space planning is generally divided into point-to-point planning and multi-node planning.

1. Point-to-point planning

When the constraint conditions of the starting point and the ending point are given, and the trajectory connecting the two points needs to be found, such a problem is called point-to-point trajectory planning. Commonly used point-to-point trajectory planning methods include cubic polynomial programming, quintic polynomial programming, and so on. Here, a quintic polynomial is used for planning, and its general form is:

six unknowns need to be given six constraints, which are the angular displacement, angular velocity and angular acceleration of the starting point and the ending point, respectively. The equations of the constraints are:

Solve the system of equations to obtain six unknowns, where T=(te-ts).

Substitute the parameters into the quintic polynomial to obtain the desired trajectory.

2. Multi-node planning

When given the constraints of several intermediate points except the starting point and the end point, when it is necessary to find the trajectory passing through all points, such a problem is called multi-node trajectory planning. When planning multi-nodes, multi-nodes can be divided into segments, each segment has two nodes, and each segment is planned point-to-point. That is, multi-node planning can be regarded as a combination of multi-segment point-to-point planning.
In order to ensure the smooth operation of the robot, an effective method in multi-node planning is to make the angular displacement, angular velocity and angular acceleration at the intermediate connection points the same. In this case, quintic polynomial programming can still be used. It is only necessary to give the angular displacement, angular velocity and angular acceleration conditions at each node, according to the method in the previous section, find the six parameters of each trajectory, and substitute them into the trajectory equation to obtain the desired trajectory.

sample program

Here, multi-node planning is carried out for six joint angles, and a total of four node angles, angular velocities and angular acceleration conditions are given, and the planning procedure for one section is as follows:

for i=1:3  %每一段规划的时间
     T=t_array(i+1)-t_array(i);
     a10=q1_array(i);
     a11=v_array(i);
     a21=a_array(i)/2;
     a31=(20*q1_array(i+1)-20*q1_array(i)-(8*v_array(i+1)+12*v_array(i))*T-(3*a_array(i)-a_array(i+1))*T^2)/(2*T^3);
     a41=(30*q1_array(i)-30*q1_array(i+1)+(14*v_array(i+1)+16*v_array(i))*T+(3*a_array(i)-2*a_array(i+1))*T^2)/(2*T^4);
     a51=(12*q1_array(i+1)-12*q1_array(i)-(6*v_array(i+1)+6*v_array(i))*T-(a_array(i)-a_array(i+1))*T^2)/(2*T^5);%计算五次多项式系数 
     ti=t_array(i):0.01:t_array(i+1);
     qi=a10+a11*(ti-t_array(i))+a21*(ti-t_array(i)).^2+a31*(ti-t_array(i)).^3+a41*(ti-t_array(i)).^4+a51*(ti-t_array(i)).^5;
     vi=a11+2*a21*(ti-t_array(i))+3*a31*(ti-t_array(i)).^2+4*a41*(ti-t_array(i)).^3+5*a51*(ti-t_array(i)).^4;
     ai=2*a21+6*a31*(ti-t_array(i))+12*a41*(ti-t_array(i)).^2+20*a51*(ti-t_array(i)).^3;
     t=[t,ti(2:end)];q=[q,qi(2:end)];v1=[v1,vi(2:end)];a1=[a1,ai(2:end)];
end

The result of angle programming is shown in the figure.

The complete sample code is as follows:
Four-node quintic polynomial programming completes the code.

3. Cartesian Space Planning

When planning the robot's Cartesian space trajectory, the general method is:
(1) Determine a path from the starting point to the ending point according to the trajectory of the starting point and the ending point at the end of the robot arm.
(2) Determine the number of interpolation points, and then determine several intermediate points on the path.
(3) Carry out inverse kinematics calculations based on a series of end coordinates of the manipulator, and solve the changes of the six joint rotation angles.
This method is called numerical solution .
It mainly involves three aspects of planning:

1. Speed ​​planning

It can be known from the principle of mechanics that if there is a sudden change in the acceleration of the robot during the movement, it will produce mechanical shock and cause the wear of the robot joints. Therefore, when planning the trajectory, it is first necessary to plan the speed of each joint to ensure that the acceleration changes continuously during the operation.
The S curve is a speed curve suitable for this situation. The entire curve is composed of a parabola and a straight line, and the corresponding acceleration curve is continuous. Generally, there are three types of S-curves: three-segment, five-segment and seven-segment S-curves. The seven-segment S-curve includes seven stages. On the basis of satisfying the continuous acceleration, there is also a process of constant acceleration at both ends, and the movement is more stable. The calculation method of the seven-segment S-curve adopts the classification discussion method, and the schematic curve is:

in addition to the S-curve, polynomial curves and the like can also be used.

2. Location planning

Position planning refers to the planning of the trajectory of the end of the manipulator, and generally uses linear interpolation, circular interpolation, and a combination of the two for planning.
(1) The purpose of linear interpolation
is to make the trajectory of the end of the robot arm a straight line. The specific steps are very simple: just connect the starting point and the end point in a straight line, then determine the number of interpolation points according to the required accuracy, and finally determine each The coordinates of the interpolation points are sufficient.
(2) Circular interpolation
Since the circular interpolation in space is more complicated, the following steps can be adopted:
1) First determine a new plane according to the starting point and end point and the center of the circle;
2) Then take the established plane as the reference, Establish a new coordinate system;
3) Determine the conversion relationship between the new coordinate system and the end coordinate system;
4) Complete the plane circular interpolation in the new coordinate system, and determine the coordinates of each interpolation point;
5) According to the coordinates Transform the relationship to obtain the coordinates of the 3D interpolation point under the end coordinates.
The motion diagram of linear interpolation and circular interpolation is as follows:

3. Attitude interpolation

During the operation of the robot, the terminal posture of some scenes does not change, such as grinding, etc., but when performing more complex work, the terminal posture will also change during the movement, such as complex welding process and so on. In the task of attitude planning, according to the attitude of the starting point and the ending point of the end point of the robot arm, a reasonable attitude change process is planned to meet specific constraints (such as obstacle avoidance, work task constraints, etc.).
When planning, one method is to use quaternions for planning. I did not study the specific method at the time, and friends in need can explore it by themselves.

4. A simplified location planning method based on geometric solution

The principle of the position interpolation planning method introduced above is simple, but the disadvantage is that a large number of inverse kinematics calculations are required. Here, according to the particularity of the robot's motion process, a geometric solution method is proposed, which simplifies the solution of the problem.
(1) First discuss the vertical motion process in the plane:

According to the forward kinematics analysis, the joint rotation angle θ1 is only related to the horizontal and vertical coordinates of the end of the manipulator, so θ1 remains unchanged during the vertical motion process. In addition, during the movement process, the posture of the terminal remains unchanged. According to the structure of the robot joints, the joint rotation angles θ4 and θ6 are fixed values. After the above analysis, the movement in the vertical direction only needs to study the changes of the remaining three joint rotation angles. The relative relationship of these three rotation angles is shown in the figure above.
In the figure, two angles α1, α2 and a length l are defined. When the starting point and the ending point are given, l is a fixed value. The relationship between them and the joint rotation angles θ2, θ3, θ5 is shown in the following formula:


After obtaining the above relationship, when planning, only need to perform two inverse kinematics calculations on the end coordinates of the starting point and the ending point to obtain the starting point and The coordinates of the end point are then interpolated to obtain the angle of the intermediate interpolation point, and finally a series of angles of other joint angles are obtained according to the relationship between the joint angles.
The finally obtained bar simulation diagram is as follows:

(2) Horizontal linear motion in the plane

A height difference ∆h is defined in the diagram. When the starting point and the ending point of the trajectory are given, ∆h is a fixed value in the formula. Similarly, during the movement process, the relationship of the joint angle is:


the simulation diagram of the rod obtained through calculation is as follows:

combining these two kinds of movements with the movement of joint 1, different end trajectories can be formed to achieve simple movements. location planning.
The specific code is as follows:
Position planning based on simplified geometric solution, including horizontal linear motion and vertical linear motion

5. Two Matlab programs for planning

Matlab provides two functions, jtraj and ctraj, which are used for trajectory planning in Cartesian space and joint space respectively. The usage formats of the two are:
(1) Joint space planning

[q ,qd, qdd]=jtraj(q1,q2,50)

Given that q1 and q2 are the joint angles of the start point and end point respectively, 50 is the number of interpolation points. The obtained q, qd, qdd are joint angle, angular velocity and angular acceleration series respectively.
(2) Cartesian space planning

Tc=ctraj(p1,p2,step)

Given p1, p2 is the four times four pose matrix of the starting point and the ending point, step is the number of interpolation points, and the obtained Tc is a series of intermediate pose matrices.
Here is a simple planning example using two functions:

clear;
clc;
%建立机器人模型
L1=Link([0       0.4      0.025    pi/2      0     ]); 
L2=Link([pi/2    0        0.56     0         0     ]);
L3=Link([0       0        0.035    pi/2      0     ]);
L4=Link([0       0.515    0        pi/2      0     ]);
L5=Link([pi      0        0        pi/2      0     ]);
L6=Link([0       0.08     0        0         0     ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','kaw');
T1=transl(0.5,0,0);%起始点位姿
T2=transl(0,-0.5,0);%终止点位姿
q1=robot.ikine(T1);%起始点关节角
q2=robot.ikine(T2);%终止点关节角

%关节空间顾规划
[q ,qd, qdd]=jtraj(q1,q2,50); 
T=robot.fkine(q);%根据插值，得到末端执行器位姿
figure(1)
robot.plot(q);%动画演示

%笛卡尔空间顾规划
Tc=ctraj(T1,T2,20);
for i=1:20
    qc(i,:)=robot.ikine(Tc(:,:,i));
end
figure(2)
robot.plot(qc);

Four. Summary

This article introduces a simple method for six-axis robot joint space and Cartesian space trajectory planning, and gives a geometric solution to simplify the planning process, and provides a simple introduction to Matlab trajectory planning functions.