基于 MATLAB 的 PUMA560 机器人运动仿真与轨迹规划 5

The movement simulation and trajectory planning of

PUMA560 robot

Shibo zhao

Abstract:In this essay, we adopt modeling method to study PUMA560 robot in the use of Robotics Toolbox based on MATLAB. We mainly focus on three problems include: the forward kinematics, inverse kinematics and trajectory planning. At the same time, we simulate each problem above, observe the movement of each joint and explain the reason for the selection of some parameters. Finally, we verify the feasibility of the modeling method.

Key words:PUMA560 robot; kinematics; Robotics Toolbox; The simulation;

I.Introduction

As automation becomes more prevalent in people’s life, robot begins more further to change people’s world. Therefore, we are obliged to study the mechanism of robot. How to move, how to determine the position of target and the robot itself, and how to determine the angles of each point needed to obtain the position. In order to study robot more validly, we adopt robot simulation and object-oriented method to simulate the robot kinematic characteristics. We help researchers understand the configuration and limit of the robot’s working space and reveal the mechanism of reasonable movement and control algorithm. We can let the user to see the effect of the design, and timely find out the shortcomings and the insufficiency, which help us avoid the accident and unnecessary losses on operating entity. This paper establishes a model for Robot PUMA560 by using Robotics Toolbox,and study the forward kinematics and inverse kinematics of the robot and trajectory planning problem.

II.The introduction of the parameters for the PUMA560 robot PUMA560 robot is produced by Unimation Company and is defined as 6 degrees of freedom robot. It consists 6 degrees of freedom rotary joints (The structure diagram is shown in figure 1). Referring to the human body structure, the first joint（J1）called waist joints. The second joint（J2）called shoulder joint. The third joint （J3）called elbow joints. The joints J4 J5, J6, are called wrist joints. Where, the first three joints determine the position of wrist reference point. The latter three joints determine the orientation of the wrist. The axis of the joint J1 located vertical direction. The axis direction of joint J2, J3 is horizontal and parallel, a3 meters apart. Joint J1, J2 axis are vertical intersection and joint J3, J4 axis are vertical crisscross, distance of a4. The latter three joints’ axes have an intersection point which is also origin point for {4}, {5}, {6} coordinate. (Each link coordinate system is shown in figure 2)

Fig1

】

【4 the structure of puma560

Fig2】

【4 the links coordinate of puma 560

When PUMA560 Robot is in the initial state, the corresponding link parameters are showed in table 1. m d m d m a m a 4331.0,1491.0,0203.0,4381.04232==== The expression of parameters:

Let length of the bar 1-i α represent the distance between 1-i z and i z along 1-i x . Torsion angle 1-i α denote the angle revolving 1-i x from 1-i z to i z . The measuring distance between 1-i x and i x along i z is i d . Joint angle i θ is the angle revolving from 1-i x to i x along i z .

Table 1

】

【4 the parameters of puma560

link )/(1 -i α

)/(1 -i a

)/( i θ

)/(m d i

Range

1 0 0 90 0 -160~160

2 -90 0 0 0.1491 -225~45

3 0 0.4318 -90 0 -45~225

4 -90 -0.0213 0 0.4331 -110~170

5 90 0 0 0 -100~100 6

-90

-266~266

III.The movement analysis of Puma560 robot

3.1 Forward kinematic

Definition: Forward kinematics problem is to solve the pose of end-effecter coordinate relative to the base coordinate when given the geometric parameters of link and the translation of joint. Let make things clearly :

What you are given: the length of each link and the angle of each joint

What you can find: the position of any point (i.e. it’s ),,,,,(γβαz y x coordinate)

3.2 The solution of forward kinematics

Method: Algebraic solution

Principal: )(q k x = The kinematic model of a robot can be written like this, where q denotes the vector of joint variable, x denotes the vector of task variable,()k is the direct kinematic function that can be derived for any robot structure .

The origin of )(q k

Each joint is assigned a coordinate frame. Using the Denavit-Hartenberg notation, you need 4 parameters (d a ,,,θα) to describe how a frame (i ) relates to a previous frame

(1-i )T i

i 1-. For two frames positioned in space, the first can be moved into coincidence with the second by a sequence of 4 operations:

1. Rotate around the 1-i x axis by an angle 1-i α.

2. Translate along the 1-i x axis by a distance 1-i α.

3. Rotate around the new z axis by an angle i θ.

4. Translate along the new z axis by a distance i d .

),(),(),(),(111

i i i i i i

d z Transl z Rotz x Transl x Rotx T θαα---= (1.1)

?

????

????

???--?-=----------100001111111111i i i i i i i i i i i i i i i i i

i i s d c c c c s s d s c c c s s c T αααθαθαααθαθθθ (1.2) Therefore, according to the theory above the final homogeneous transform

corresponding to the last link of the manipulator:

?????

???????==100065544332211006z z z z y y y y x x x x p a s n p a s n p a s n T T T T T T T (1.3)

3.3Inverse kinematic

Definition : Robot inverse kinematics problem is that resolve each joint variables of the robot based on given the position and direction of the end-effecter or of the link (It can show as position matrix T). As for PUMA560 Robot, variable 61θθ need to be resolved.

Let make things clearly :

What you are given: The length of each link and the position of some point on the robot.

What you can find: The angles of each joint needed to obtain that position.

3.4 The solution of inverse kinematics

Method: Algebraic solution

Principal: ?

?

=q q J x )(

Where q k J ??=/ is the robot Jacobian. Jacobian can be seen as a mapping from Joint velocity space to Operational velocity space.

3.5 The trajectory planning of robot kinematics

The trajectory planning of robot kinematics mainly studies the movement of robot. Our goal is to let robot moves along given path. We can divide the trajectory of robots into two kinds. One is point to point while the other is trajectory tracking. The former is only focus on specific location point. The latter cares the whole path.

Trajectory tracking is based on point to point, but the route is not determined. So, trajectory tracking only can ensure the robots arrives the desired pose in the end position, but can not ensure in the whole trajectory. In order to let the end-effecter arriving desired path, we try to let the distance between two paths as small as possible when we plan Cartesian space path. In addition, in order to eliminate pose and position’s uncertainty between two path points, we usually do motivation plan among every joints under gang control. In a word, let each joint has same run duration when we do trajectory planning in joint space.

At same time, in order to make the trajectory planning more smoothly, we need to apply the interpolating method.

Method: polynomial interpolating [1]

Given: boundary condition

???==f f θθθθ）（）

（t 00 (1.3)

?????==?

?0t 0

0）（）（f θ

θ (1.4) Output : joint space trajectory ()t θ between two points

()t θ=332210t a t a t a a +++ （1.5）

Polynomial coefficient can be computed as follows:

???

?

??

???--=-===)(2)

(30023

02

2100θθθθθf f f f t a t a a a (1.6)

IV. Kinematic simulation based on MATLAB

?How to use link

In Robotics Toolbox, function ’ link ’ is used to create a bar. There are two methods. One is to adopt standard D-H parameters and the other is to adopt modified D-H parameters, which correspond to two coordinate systems. We adopt modified D-H parameters in our paper. The first 4 elements in Function ‘link ’ are α, a, θ, d. The last element is 0 (represent Rotational joint) or 1 (represent translation joint). The final parameter of link is ’mod’, which means standard or modified. The default is standard. Therefore, if you want to build your own robot, you may use function ‘link ’. You can call it like this:’ L1=link([0 0 pi 0 0],'modified'); ?The step of simulation is:

Step1: First of all, according to the data from Table 1, we build simulation program of the robot (shown in Appendix rob1.m).

Step2: Present 3D figure of the robot (shown in Fig4). This is a three-dimensional figure when the robot located the initial position (0i =θ). We can adjust the position of the slider in control panel to make the joint rotation (in Fig 5), just like controlling real robot.

Step3:Point A located at initial position. It can de described as ]0,0,0,0,0,0[=A q . The target point is Point B. The joint rotation angle can be expressed as ]0,392.0,0,7854.0,7854.0,0[--=B q . We can achieve the solution of forward

kinematics and obtain the end-effecter pose relative to the base coordinate system is (0.737, 0.149, 0.326) , relative to the three axes of rotation angle is the (0, 0, -1). The ro bot’s three -dimensional pose in B q is shown in Fig 6.

Step4: According to the homogeneous transformation matrix, we can obtain each joint variable from the initial position to the specified location

Step5:Simulate trajectory from point A to point B. The simulation time is 10s. Time interval is 0.1s. Then, we can picture location image, the angular velocity and angular acceleration image (shown as Fig 8) which describe each joint transforms over time from Point A to Point B. In this paper, we only present the picture of joint 3. By using the function ‘T=fkine(r,q)’, we obtain ‘T ’ a three-dimensional matrix. The first two dimensional matrix represent the coordinate change while the last dimension is time ‘t’.

-1

-0.8

-0.6

-0.4

-0.2

0.2

0.4

0.6

0.8

1

-1

-0.8

-0.6

-0.4

-0.2

0.2

0.4

0.6

0.8

X zhaoshibo

x y z

Fig 4

Fig 5

-1

-0.5

0.5

1

-1

-0.50

0.5

1

-1-0.50

0.51X

Y

Z

zhaoshibo

x y

z

Fig 6

Fig 7

01234

5678910

-1

-0.50

time t/s

a n g l e /r a d

01234

5678910

-0.2

-0.10time t/s

v e l o c i t y /（r a d /s )

01234

5678910

-0.05

00.05time t/s

a c c e l e r a t i o n /（r a d /s 2)

Fig8

V The problem during the simulation

?The reason for selection of some parameter

The parameter of link: From kinematic simulation and program, you can see that I set certain value not arbitrary when I call ‘link ’. That is because I want the simulation can be more close to the real situation .So; I adopt the parameter of puma560 (you can see it from the program) and there is no difference between my robot and puma560 radically.

The parameter of B q : When I choose the parameter of B q , I just want to test something.

For example, when you denote the parameter of ‘B q ’ like this

‘]0,392.0,0,7854.0,7854

.0,0[--=B q ’, you want to use the function ‘fkine(p560, B q )’ to obtain the homogenous function ‘T ’, then, you want to use ‘ikine(p560,T)’ to test whether the ‘B q ’ is what you have settled before. The result is as follows:

B q =[0 -pi/4 -pi/4 0 pi/8 0]; T=fkine(p560, B q );

?

????

????

???---=100032563.038268.009238.01500.00107317.09238

.003826.0T B q =ikine(p560,T)

B q =[0 -pi/4 -pi/4 0 pi/8 0]

Actually, not all of the parameter B q can do like this. For example, when you try B q =[pi/2,pi/2,pi/2,pi/2,pi/2,pi/2] , the answer is not B q itself.

VI. References

[1]https://www.360docs.net/doc/b712224502.html,/wiki/Robot_ [online], 7-ferbury-2015

[2]https://www.360docs.net/doc/b712224502.html,/p-947411515.html [online], 7-ferbury-2015

[3]https://www.360docs.net/doc/b712224502.html,/wiki/Jacobian_algorithm [access ed 8-February-2015]

[4] Youlun Xiong, Han Ding, Encang Liu, Robot[M], Tinghua university press ,1993 VIIAppendix

clc; clear;

%modified 改进的D-H法

L1=link([0 0 pi 0 0],'modified');

L2=link([-pi/2 0 0 0.1491 0],'modified');

L3=link([0 0.4318 -pi/2 0 0],'modified');

L4=link([-pi/2 0.0203 0 0.4318 0],'modified');

L5=link([pi/2 0 0 0 0],'modified');

L6=link([-pi/2 0 0 0 0],'modified');

r=robot({L1 L2 L3 L4 L5 L6});

https://www.360docs.net/doc/b712224502.html,='zhaoshibo';%模型的名称

drivebot(r)

track.m

%前3 个关节对机械手位置的影响

qA=[0,0,0,0,0,0]; %起始点关节空间矢量

qB= [0 -pi/4 -pi/4 0 pi/8 0]

; %终止点关节空间矢量

t=[0:0.1:10]; %仿真时间

[q,qd,qdd]=jtraj(qA,qB,t); %关节空间规划

plot(r,q)

%关节3 的角速度、角速度和角加速度曲线

figure

subplot(1,3,1)

plot(t,q(:,3))%关节3 的位移曲线

xlabel('时间t/s');ylabel('关节的角位移/rad');grid on

subplot(1,3,2)

plot(t,qd(:,3))%关节3 的位移曲线

xlabel('时间t/s');ylabel('关节的角速度/（rad/s)');grid on

subplot(1,3,3)

plot(t,qdd(:,3))%关节3 的位移曲线

xlabel('时间t/s');ylabel('关节的角加速度/（rad/s^2)');grid on