ENGG 542 Course Project: Simulation of PUMA 56 Manipulator ZHENG Fan, 115551778 mrzhengfan@gmail.com April 5, 215. Preface This project is to derive programs for simulation of inverse dynamics and control based on the model of PUMA 56 manipulator, with the last 3 joints neglected. PUMA 56 is widely used in academic and industrial application, and has been modeled with several D-H schemes with slightly different measured parameters [1]. Here we adopt the scheme and parameters of B. Armstrong [2], with the configuration and parameters shown below. Figure 1 D-H model
Table 1 D-H parameters [mm] Table 2 Link mass [kg] Table 3 Link center of gravity [mm] Table 4 Moment of inertia about COG [kg m 2 ] 1. Inverse Dynamics with Newton-Euler Formulation In this part we make a program to calculate its inverse dynamics using the Newton-Euler formulation and plot the input torques at joints when the first three joints move in trajectories q i = h i sin ω i t. Here we select h 1 = h 2 = h 3 = 1, ω 1 = ω 2 = ω 3 = 1 rad/s. Derive kinematic equations (all expressed in the base frame {x y z}): Write s i = sin q i, c i = cos q i. Rotation matrix: q 1 = q 2 = q 3 = sin t = q q 1 = q 2 = q 3 = cos t = q q 1 = q 2 = q 3 = sin t = q
c 1 s 1 c 1 c 2 c 1 s 2 s 1 c 1 c 2+3 c 1 s 2+3 s 1 1R = [ s c 1 ], 2 R = [ s 1 c 2 s 1 s 2 c 1 ], 3 R = [ s 1 c 2+3 s 1 s 2+3 c 1 ] 1 s 2 c 2 s 2+3 c 2+3 Corresponding z axis: s 1 z 1 = [ ], z 2 = z 3 = [ c 1 ] 1 Relative position of frame origin and center of mass: s 2 = Angular velocity and differentiation: Velocity of frame origin: Acceleration of frame origin: Acceleration of center of mass: 2 [ s 1 s 1 =.243 [ c 1 ].432 R ], r 2 = R.93 r 3 = R 3 [ 2 [.7].14 ω 1 = [ ], ω 1 = [ ] q q.68.6 ].16 s 1 s 1 c 1 ω 2 = ω 1 + z 2 q = q [ c 11 ], ω 2 = q [ c 11 ] + q 2 [ s 1 ] 2s 1 2s 1 2c 1 ω 3 = ω 2 + z 3 q = q [ 2c 1 ], ω 3 = q [ 2c 1 ] + q 2 [ 2s 1 ] 1 1 v 1 = v 2 = v 1 + ω 1 s 1 v 3 = v 2 + ω 2 s 2 a 1 = a 2 = a 1 + ω 1 s 1 + ω 1 (ω 1 s 1 ) a 3 = a 2 + ω 2 s 2 + ω 2 (ω 2 s 2 ) a c2 = a 2 + ω 2 r 2 + ω 2 (ω 2 r 2 ) a c3 = a 3 + ω 3 r 3 + ω 3 (ω 3 r 3 )
Then derive the backward dynamic equations: Inertia tensor matrix: I xx2 I xx3 I 1 = [ ], I 2 = 2 R [ I yy2 ] 2R T, I 3 = 3 R [ I yy3 I zz1 I zz2 Force and torque at joint: f 3 = m 3 a c3 + [ ] m 3 g n 3 = I 3 ω 3 + ω 3 I 3 ω 3 + r 3 f 3 τ 3 = z 3 T n 3 f 2 = m 2 a c2 + f 3 + [ ] m 2 g n 2 = I 2 ω 2 + ω 2 I 2 ω 2 + n 3 + r 2 f 2 + (s 2 r 2 ) f 3 τ 2 = z 2 T n 2 n 1 = I 1 ω 1 + n 2 + s 1 f 2 τ 1 = z 1 T n 1 3 I zz3 ] R T Finally τ 1, τ 2, τ 3 are the input torques at joints. Write program in MATLAB (see Appix-1) and plot them as below: Figure 2 Inverse dynamic simulation result
2. Controller simulation In this part we make programs to simulate the performance of the robot under the control of the PD controller, the PID controller, and the PD plus gravity compensator controller, respectively. First we derive the Lagrange formulation of robot dynamics. Take {x y z} as base frame and plane x-y as zero potential energy plane. Generalized coordinates q = [q 1 q 2 q 3 ] T. Calculating velocities: ω 1 = [ ], ω 1 = [ ] q 1 q 1 q 2s 1 s 1 c 1 ω 2 = ω 1 + z 2 q 2 = [ q 2c 1 ], ω 2 = q [ c 11 ] + q 2 [ s 1 ] q 1 (q 2 + q 3)s 1 2s 1 2c 1 ω 3 = ω 2 + z 3 q = [ (q 2 + q 3)c 1 ], ω 3 = q [ 2c 1 ] + q 2 [ 2s 1 ] q 1 1.984c 1.68s 1 c 2 +.6s 1 s 2.68c 1 s 2.6c 1 c 2 v c2 = v 2 + ω 2 r 2 = q 1 [.984s 1 +.68c 1 c 2.6c 1 s 2 ] + q 2 [.68s 1 s 2.6s 1 c 2 ].68c 2 +.6s 2 Kinetic energy:.921c 1.432s 1 c 2.7s 1 s 2+3 v c3 = v 3 + ω 3 r 3 = q 1 [.921s 1 +.432c 1 c 2 +.7c 1 s 2+3 ].432c 1 s 2 +.7c 1 c 2+3.7c 1 c 2+3 +q 2 [.432s 1 s 2 +.7s 1 c 2+3 ] + q 3 [.7s 1 c 2+3 ].432c 2.7s 2+3.7s 2+3 K = 1 2 (I zz1ω 2 1 + I zz2 ω 2 2 + I zz3 ω 2 3 + m 2 v 2 c2 + m 3 v 2 c3 ) (11 +.45c 2 2 )q 1 2 +.83q 2 2 +.55q 3 2 + (1.17 + 1.92s 2 )q 1q 2.3c 2+3 q 1q 3.29q 3q 2 Potential energy: U =.68m 2 gc 2 + m 3 g[.432c 2 +.7c(q 2 + q 3 )] = 31.9c 2 + 3.3c 3 c 2 3.3s 2 s 3 Lagrangian L = K U. L q = [ 91.9s 2 + 3.3s 2 c 3 + 3.3c 2 s 3.9c 2 s 2 q 1 2 + 1.92c 2 q 1q 2 +.3s 2+3 q 1q 3 ] 3.3s 3 c 2 + 3.3s 2 c 3 +.3s 2+3 q 1q 3
L (22 +.9c 2 2 )q 1 + (1.17 + 1.92s 2 )q 2.3c 2+3 q 3 = [ 1.66q 2 + (1.17 + 1.92s q 2 )q 1.29q 3 ].11q 3.3c 2+3 q 1.29q 2 d L dt q (22 +.9c 2 2 )q 1 + (1.17 + 1.92s 2 )q 2.3c 2+3 q 3 1.8c 2 s 2 q 1q 2 + 1.92c 2 q 2 2 +.3s 2+3 q 2q 3 +.3s 2+3 q 3 2 = [ 1.66q 2 + (1.17 + 1.92s 2 )q 1.29q 3 + 1.92c 2 q 1q 2.11q 3.3c 2+3 q 1.29q 2 +.3s 2+3 q 1q 2 +.3s 2+3 q 1q 3 ] Dynamic equation of the manipulator: τ 1 d L L dt q q = [ τ 2 ] τ 3 Based on this dynamic equation, we develop several controller to simulate the robot s performance. Here we consider only position control problem, and select the desired position as q d = [ π 2 π 2 π 2 ] T 1) PD controller: PD controller input is τ = A(q q d ) Bq. Choose proper matrix A and B, construct state space expression and use MATLAB program with ode method (see Appix-2) to simulate the performance. The result is shown below. Figure 3 PD control simulation result
It can be seen from the figure that all the 3 joints is stable. However, only joint 1 converge to the desired position, while there exit offset error between desired and actual position in both joint 2 and 3. The offset error is caused by the existence of gravity. We can kill this error by PID control or PD feedback plus gravity compensation. 2) PID controller PID controller input is τ = A(q q d ) Bq C (q q d )dt. Choose proper matrix A, C and B, construct state space expression and use MATLAB program with ode method (see Appix-3) to simulate the performance. The result is shown below. t Figure 4 PID control simulation result As the figure shows, all 3 joints converge to desired values and no offset error exists. 3) PD plus gravity compensator controller PD plus gravity compensator controller input is τ = A(q q d ) Bq + G(q). Choose proper matrix A and B, construct state space expression and use MATLAB program with ode method (see Appix-4) to simulate the performance. The result is shown below.
Figure 5 PD control plus gravity compensation simulation result The 3 joints also converge to desired values with no offset error, and with satisfactory transition process. 3. Computed torque control In this part we make a program to simulate the performance of the robot under the control of the computed torque control method. We select desired trajectory as q d = The closed loop system is then π 2 π 2 π + sin t + sin t. The control input is [ + sin t 2 ] τ = C(q, q ) + G(q) + H(q)(q d A(q q d) B(q q d )) q = q d A(q q d) B(q q d ) Choose proper matrix A and B, construct state space expression and use MATLAB program with ode method (see Appix-5) to simulate the performance. The result is shown below.
Figure 6 Computed torque control simulation result: joint position Figure 7 Computed torque control simulation result: joint velocity
Preferences [1] Corke P I, Armstrong-Helouvry B. A search for consensus among model parameters reported for the PUMA 56 robot[c]//robotics and Automation, 1994. Proceedings., 1994 IEEE International Conference on. IEEE, 1994: 168-1613. [2] Armstrong B, Khatib O, Burdick J. The explicit dynamic model and inertial parameters of the PUMA 56 arm[c]//robotics and Automation. Proceedings. 1986 IEEE International Conference on. IEEE, 1986, 3: 51-518.
Appix All MATLAB code and this report are available in http://izhengfan.github.io/puma.html. 1. Inverse dynamics simulation code. function main_puma1 tau1 = zeros(1,21); tau2 = zeros(1,21); tau3 = zeros(1,21); for i = :1:2; t = i*.1; q = sin(t); qdot = cos(t); qddot = -sin(t); cq = cos(q); sq = sin(q); c2q = cos(q+q); s2q = sin(q+q); R1 = [cq,-sq,;sq,cq,;,,1]; R2 = [cq*cq,-cq*sq,-sq;sq*cq,-sq*sq,cq;-sq,-cq,]; R3 = [cq*c2q,-cq*s2q,-sq;sq*c2q,-sq*s2q,cq;-s2q,-c2q,]; z1 = [;;1]; z2 = [-sq;cq;]; z3 = z2; s1 =.243*z2; s2 = R2*[.432;;-.93]; r2 = R2*[.68;.6;-.16]; r3 = R3*[;-.7;.14]; w1 = [;;qdot]; w1dot = [;;qddot]; w2 = qdot*(z1+z2); w2dot = qddot*(z1+z2)+qdot*qdot*[-cq;-sq;]; w3 = qdot*(z1+2*z2); w3dot = qddot*(z1+2*z2)+qdot*qdot*[-2*cq;-2*sq;]; v2 = cross(w1,s1); v3 = v2+cross(w2,s2); a2 = cross(w1dot,s1)+cross(w1,cross(w1,s1)); a3 = a2+cross(w2dot,s2)+cross(w2,cross(w2,s2)); ac2 = a2+cross(w2dot,r2)+cross(w2,cross(w2,r2)); ac3 = a3+cross(w3dot,r3)+cross(w3,cross(w3,r3)); m2 = 17.4; m3 = 4.8; I1 =.35; I2 = R2*diag([.13,.542,.539])*R2'; I3 = R3*diag([.66,.125,.86])*R3'; g = 9.8;
f3 = m3*ac3+[;;m3*g]; n3 = I3*w3dot+cross(w3,I3*w3)+cross(r3,f3); t3 = z3'*n3; f2 = m2*ac2+f3+[;;m2*g]; n2 = I2*w2dot+cross(w2,I2*w2)+cross(r2,f2)+n3+cross((s2-r2),f3); t2 = z2'*n2; n1 = I1*w1+n2+cross(s1,f2); t1 = z1'*n1; tau1(i+1) = t1; tau2(i+1) = t2; tau3(i+1) = t3; t = :.1:2; plot(t,tau1,'-',t,tau2,'--',t,tau3,'-.'); xlabel('t/s'),ylabel('\tau /Nm'); leg('\tau 1','\tau 2','\tau 3'); 2. PD control simulation code function main_puma_pd [t,x] = ode45(@puma_pd,:.1:2,[ ]); plot(t,x(:,1),'-',t,x(:,2),'--',t,x(:,3),'-.','linewidth',2); leg('q1','q2','q3'); hold on plot(t,pi*ones(1,length(t))/2,'--'); xlabel('t/s'),ylabel('q/rad'); hold off function xdot = puma_pd(~,q) xdot = zeros(6,1); qd = [pi/2; pi/2; pi/2]; A = [5 ; 18 ; 5]; B = [35 ; 5 ; 2]; tau = -A*([q(1);q(2);q(3)]-[qd(1);qd(2);qd(3)])-B*[q(4);q(5);q(6)]; xdot(1) = q(4); xdot(2) = q(5); xdot(3) = q(6); H = [22+.9*cos(q(2))*cos(q(2)), 1.17+1.92*sin(q(2)), -.3*cos(q(2)+q(3)); 1.17+1.92*sin(q(2)), 1.66, -.29; -.3*cos(q(2)+q(3)), -.29,.11]; xdot(4:6) = H\(-... [- 1.8*cos(q(2))*sin(q(2))*q(4)*q(5)+1.92*cos(q(2))*q(5)*q(5)+.3*sin(q(2)+q(3))*(q(5)+q(6))* q(6); 91.9*sin(q(2))-3.3*sin(q(2)+q(3))+.9*sin(q(2))*cos(q(2))*q(4)*q(4)-.3*sin(q(2)+q(3))*q(4)*q(6);.3*sin(q(2)+q(3))*q(4)*q(5)-3.3*sin(q(2)+q(3))]+tau);
3. PID control simulation code function main_puma_pid [t,x] = ode45(@puma_pid,:.1:2,[ ]); plot(t,x(:,1),'-',t,x(:,2),'--',t,x(:,3),'-.','linewidth',2); leg('q1','q2','q3'); hold on plot(t,pi*ones(1,length(t))/2,'--'); xlabel('t/s'),ylabel('q/rad'); hold off function xdot = puma_pid(~,q) xdot = zeros(9,1); qd = [pi/2; pi/2; pi/2]; A = [12 ; 18 ; 5]; B = [3 ; 5 ; 1]; C = [3 ; 12 ; 1]; tau = -A*([q(1);q(2);q(3)]-[qd(1);qd(2);qd(3)])-B*[q(4);q(5);q(6)]-C*[q(7);q(8);q(9)]; xdot(1) = q(4); xdot(2) = q(5); xdot(3) = q(6); H = [22+.9*cos(q(2))*cos(q(2)), 1.17+1.92*sin(q(2)), -.3*cos(q(2)+q(3)); 1.17+1.92*sin(q(2)), 1.66, -.29; -.3*cos(q(2)+q(3)), -.29,.11]; xdot(4:6) = H\(-... [- 1.8*cos(q(2))*sin(q(2))*q(4)*q(5)+1.92*cos(q(2))*q(5)*q(5)+.3*sin(q(2)+q(3))*(q(5)+q(6))* q(6); 91.9*sin(q(2))-3.3*sin(q(2)+q(3))+.9*sin(q(2))*cos(q(2))*q(4)*q(4)-.3*sin(q(2)+q(3))*q(4)*q(6);.3*sin(q(2)+q(3))*q(4)*q(5)-3.3*sin(q(2)+q(3))]+tau); xdot(7:9) = q(1:3)-qd; 4. PD plus gravity compensation control simulation code function main_puma_pdg [t,x] = ode45(@puma_pdg,:.1:2,[ ]); plot(t,x(:,1),'-',t,x(:,2),'--',t,x(:,3),'-.','linewidth',2); leg('q1','q2','q3'); hold on plot(t,pi*ones(1,length(t))/2,'--'); xlabel('t/s'),ylabel('q/rad'); hold off function xdot = puma_pdg(~,q) xdot = zeros(6,1); qd = [pi/2; pi/2; pi/2]; A = [5 ; 18 ; 5];
B = [35 ; 5 ; 2]; tau = -A*([q(1);q(2);q(3)]-[qd(1);qd(2);qd(3)])-B*[q(4);q(5);q(6)]; xdot(1) = q(4); xdot(2) = q(5); xdot(3) = q(6); H = [22+.9*cos(q(2))*cos(q(2)), 1.17+1.92*sin(q(2)), -.3*cos(q(2)+q(3)); 1.17+1.92*sin(q(2)), 1.66, -.29; -.3*cos(q(2)+q(3)), -.29,.11]; xdot(4:6) = H\(-... [- 1.8*cos(q(2))*sin(q(2))*q(4)*q(5)+1.92*cos(q(2))*q(5)*q(5)+.3*sin(q(2)+q(3))*(q(5)+q(6))* q(6);.9*sin(q(2))*cos(q(2))*q(4)*q(4)-.3*sin(q(2)+q(3))*q(4)*q(6);.3*sin(q(2)+q(3))*q(4)*q(5)]+tau); 5. Computed torque control simulation code function main_puma_ct [t,x] = ode45(@puma_ct,:.1:2,[ ]); figure(1), plot(t,x(:,1),'-',t,x(:,2),'--',t,x(:,3),'-.','linewidth',2.5); leg('q1','q2','q3'); hold on plot(t,pi/2+sin(t),'--'); xlabel('t/s'),ylabel('q/rad'); hold off figure(2), plot(t,x(:,4),'-',t,x(:,5),'--',t,x(:,6),'-.','linewidth',2.5); leg('w1','w2','w3'); hold on plot(t,cos(t),'--'); xlabel('t/s'),ylabel('w/rad/s'); hold off function xdot = puma_ct(t,q) xdot = zeros(6,1); qd = [pi/2+sin(t); pi/2+sin(t); pi/2+sin(t)]; qddot = [cos(t); cos(t); cos(t)]; qd2dot = -[sin(t); sin(t); sin(t)]; A = [5 ; 18 ; 5]; B = [35 ; 5 ; 2]; xdot(1) = q(4); xdot(2) = q(5); xdot(3) = q(6); xdot(4:6) = qd2dot - A*(q(4:6)-qddot) -B*(q(1:3)-qd);