Research on kinematic modeling and trajectory planning of six-axis robot

Kang-jia HE, Ling HE, Lei FENG， Guang-xing ZHANG

(Key Laboratory of Advanced Manufacturing Technology Ministry of Education， Guizhou University， Guiyang 550003， China)

Abstract: Taking PUMA560 robot as the research object, kinematics analysis of this model robot is carried out. Firstly, a kinematics model is established to solve the forward and inverse kinematics equations of the robot. Secondly, three methods (cubic polynomial interpolation, quintic polynomial interpolation and five-segment s-curve interpolation) were used to plan the robot’s trajectory. The three methods are analyzed and compared, and finally it is concluded that the five-segment position S curve interpolation trajectory planning method can make the robot run more stable and have better motion characteristics, so that it can reach the desired pose quickly and smoothly, and finally complete the assigned task.

Key words: PUMA560, Trajectory planning, Cubic polynomial interpolation, Quintic polynomial interpolation, Five-segment S curve interpolation

1 Introduction

In the 21st century, as technology in Artificial Intelligence has developed, industrial robots are widely used in many fields of social life. When industrial robots perform tasks, the optimization of the trajectory of each joint of the manipulator will directly affect the efficiency, stability and energy consumption of the robot [1]. It is not only beneficial to improve the working efficiency of the robot, but also to prolong the life of the robot by optimizing the operation trajectory before the robot performs the task.

Many scholars at home and abroad have done a lot of scientific research on trajectory planning of robots, which has effectively promoted the rapid development of trajectory planning technology. For example, Zhou Sheng [2] improved the ABC algorithm to optimize the series manipulator. Su Tingting[3]proposed a path planning method based on Pythagoras’ fast end curve to solve the problem of Descartes’ spatial trajectory unsmooth when delta parallel robot is working at high speed. Dongchen et al [4] proposed an algorithm for multi-objective trajectory planning to improve immune clonal selection. He Wang[5] et alproposed to use the motion constraint of higher-order polynomial curve in the point-to-point trajectory planning of industrial robots.

The research object of this paper is to select PUMA560 industrial robot, through the robot kinematics model and motion equation established in D-H coordinate system[6], to solve the forward and backward solutions of the robot kinematics equation, and then through the use of cubic polynomial interpolation, quintic polynomial interpolation and five segment position s curve interpolation to carry out the trajectory planning of the robot. In order to make the trajectory planning more intuitive, the software MATLAB was used for simulate the kinematics and get the position,velocity and acceleration curves.

Fig.1 PUMA560 robot model

Among:a2=432.8 mm,a3=20.32 mm,d2=149.09 mm,d4=433.07 mm. The values in brackets are the actual values of the joint variables in the above figure.

2 Kinematic model

The basis of robot control is robot kinematics. In order to get the accurate position and pose of robot, the kinematics must be analyzed[7].Forward kinematics and inverse kinematics are two methods of kinematic analysis. For the manipulator, it can be simplified as an open chain linkage system connected by rotating and moving joints. Every degree of freedom of the robot is determined by every independently driven joint. To conveniently describe the relationship between these joints conveniently, a coordinate system is set up on each joint. The attitude and position between the coordinate systems can be easily described by homogeneous transformation.

Fig.2 D-H coordinate system

For PUMA560, it is a six DOF arm robot with six joints to control its motion. The first three joints are used to determine the position of the end tool of the manipulator, the last three joints are used to determine the direction of the end tool, and the axes of the later three joints converge at a point, which will coincide with the origin of the coordinate system on the three joints.

Fig.3 Puma560 model

2.1 Forward kinematics

For the forward kinematics of the robot, it is based on the parameters and coordinate system between the links of the robot to solve the attitude and position of the end effector. According to the D-H method, the transformation matrix can be derived.

After calculation, the forward kinematics equation of PUMA560 is obtained as follows:

Among:

nx=c1[c23(c4c5c6-s4s6)-s23s5c6]+s1(s4c5c6+c4s6)

ny=s1[c23(c4c5c6-s4s6)-s23s5c6]-

c1(s4c5c6+c4s6)

nz=-s23(c4c5c6-s4s6)-c23s5s6

ox=-c1[c23(c4c5c6+s4s6)+s23s5c6]+

s1(c4s6-s4c5c6)

oy=-s1[c23(c4c5c6+s4s6)+s23s5c6]-

c1(c4s6-s4c5c6)

oz=s23(c4c5c6+s4s6)+c23s5s6

ax=-c1(c23s4s5+s23c5)-s1s4s5

ay=-s1(c23s4s5+s23c5)+c1s4s5

az=s23c4s5-c23c5

px=c1(a2c2+a3c23-d4s23)-d2s1

py=s1(a2c2+a3c23-d4s23)+d2c1

pz=-a3s23-a2s2-d4c23

Mark:

sθ1=sinθ1；cθ1=cosθ1

s23=c2s3+s2c3=sin(θ2+θ3)

c23=c2c3-s2s3=cos(θ2+θ3)

2.2 Backward kinematics

In order for the position and attitude of the end effector of the robot toreach a given goal or to compel a specified task by moving along a continuous trajectory, the inverse kinematics of the robot must be calculated.

The equation of motion of PUMA560 is：

Solved:

θ23=θ2+θ3=atan2[-(a3+a2c3)pz+

(c1px+s1py)(a2s3-d4),(-d4+a2s3)pz+(c1pz+s1py)(a2c3+a3)]

(According to the four possible combinations ofθ1andθ3solutions, four possibleθ23values can be obtained, thus four possibleθ2solutions can be obtained.)

θ4=atan2(-axs1+ayc1,-axc1c23-

ays1c23+azs23)

θ5=atan2(s5,c5)

θ6=atan2(s6,c6)

3 Trajectory planning

The trajectory is the set of all paths that the end of the robot has traveled in space. Trajectory planning refers to the trajectory obtained by completing the requirements of corresponding tasks. Planning, which starts from the initial state of a specific problem, uses the corresponding operation steps or algorithms to achieve the purpose of solving the problem.

3.1 Cubic polynomial interpolation

In the process of manipulator motion, we can use an interpolation functionθ(t) about the joint angle of the start point and the end point to describe the motion track. Since the joint angle of the starting pointθ0is known, however, the joint angle of the ending pointθfcan usually be solved by kinematics. At this point, there will be multiple smoothing functions that can be used as joint interpolation functions, as shown in the following figure.

The cubic polynomial interpolation is applicable to the condition that the velocity of the starting point and the ending point are both 0, and the angular value of the constraint joint at the starting point and the ending point and the position angular velocity of the two ends of the specified trajectory are also fixed.

Fig.4 Different trajectory curves of single joint

Suppose the joint angle meets conditions in the following formulas:

θ(t)=a0+a1t+a2t2+a3t3

Two adjacent points are regarded as the starting point and the ending point of a small section of track, which are expressed byθ0and θfrespectively. The starting speed isv0and the ending speed is vf, then

θ(t0)=θ0,θ(tf)=θf

The constraints are as follows. To facilitate the calculation, we assume thatt0=0, then

a0=θ0,a1=v0

Therefore, the trajectory of cubic polynomial programming is as follows:

When the speed is 0, it is applicable to the starting and ending points of a track. If it is not 0, it is applicable to the passing points of a track.

Using MATLAB to simulate these two situations:

Table 2 v=0

Fig.5 Characteristic curve of robot with v=0

Table 3 v≠0

Fig.6 Characteristic curve of robot with v≠0

Given the position, velocity and time of discrete points, this cubic polynomial interpolation can interpolate a continuous smooth curve. But the angular velocity is not continuous.

3.2 Quintic polynomial interpolation

Indeed, we need to face more constraints. We need to use higher order polynomials to interpolate the path segments of the trajectory. It is assumed that the joint position, velocity and acceleration are specified at the beginning and end of a certain path. In this case, we need to use a quintic polynomial for interpolation.

Because there are six undetermined coefficients in quintic polynomials, six conditions are needed to determine these six coefficients accurately. Nevertheless, these six conditions can be given by the angle of starting point, target point, angular velocity and angular acceleration respectively [8]。

The function expressions of angular displacement, angular velocity and angular acceleration in quintic polynomial interpolation are as follows:

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

The constraint conditions are as follows: (compared with the cubic polynomial interpolation, the constraint on the angular velocity of the start and stop points is added, which is also assumed for the convenience of calculationt0=0)

θ(t0)=θ0=a0

Solved:

Table 4 constraints

Fig.7 Characteristic curve of quintic interpolation robot

Compared with the cubic polynomial interpolation, the acceleration is a smooth curve without jump.

3.3 Five segment position s curve interpolation

It is more suitable for five segment position s- curve interpolation in the case of higher stability requirements. The interpolation of S-curve of five sections is divided into five sections: acceleration, acceleration and deceleration, uniform speed, deceleration and deceleration.

Suppose that the time of the other four segments is equal toTa, the total time isT, the speed of the uniform segment isVs, the slope of the four segments isA, the total displacement of the whole trajectory isL, the acceleration segment displacement isL1, and the acceleration and deceleration segment displacement isL2.

Then the acceleration subsection function is:

The piecewise function of velocity can be obtained by integrating acceleration:

And then, by integrating the velocity, the piecewise function of displacement s can be obtained:

Fig.8 Characteristic curve of robot interpolating five segment position s curve

4 Conclusion

In this paper, the PUMA 560 robot is taken as the research object. Through the D-H model, the forward/backward kinematics of the robot is deduced and analyzed. The trajectory and motion parameters of the robot are analyzed by cubic polynomial and quintic polynomial respectively and five segment position S-curve interpolation. MATLAB software is used for simulation. The results show that: (1) As long as the position, velocity and time of discrete points are given, continuous and smooth curve can be interpolated. (2) In reality, there may be more constraints. At this moment, higher order polynomial interpolation is needed. Compared with cubic polynomial interpolation, it is found that quintic polynomial acceleration is also a smooth curve, and there is no jump. (3) If there is a high demand for stationarity in practice, it is necessary to introduce a five-segment position S-curve interpolation method.It provides reference for further research on robot kinematics.