Professional Documents
Culture Documents
KFUPM
EE 656 – Robotics & Control
HW #5
By
Mohammad Shahab
227598
For
Dr. Ahmad Masoud
20 December 2008
Term 081
EE 656 – Robotics & Control
HW #5
𝑥1 = 𝐿1 sin 𝜃1
𝑦1 = 𝐿1 cos 𝜃1
𝑥2 = 𝐿1 sin 𝜃1 + 𝐿2 sin(𝜃1 + 𝜃2 )
𝑦2 = 𝐿1 cos 𝜃1 + 𝐿2 cos(𝜃1 + 𝜃2 )
So, Kinetic Energy could be formed as
1 1 1 1
𝐾𝐸 = 𝑀1 𝑥12 + 𝑀1 𝑦12 + 𝑀2 𝑥22 + 𝑀2 𝑦22
2 2 2 2
By simplification,
⟹
1 1 1
𝐾𝐸 = 𝑀1 + 𝑀2 𝐿21 𝜃12 + 𝑀2 𝐿22 𝜃12 + 𝑀2 𝐿22 𝜃1 𝜃2 + 𝑀2 𝐿22 𝜃22 + 𝑀2 𝐿1 𝐿2 cos 𝜃2 (𝜃1 𝜃2 + 𝜃12 )
2 2 2
And Potential Energy is
ℒ = 𝐾𝐸 − 𝑃𝐸
1 1 1
ℒ= 𝑀1 + 𝑀2 𝐿21 𝜃12 + 𝑀2 𝐿22 𝜃12 + 𝑀2 𝐿22 𝜃1 𝜃2 + 𝑀2 𝐿22 𝜃22 + 𝑀2 𝐿1 𝐿2 cos 𝜃2 𝜃1 𝜃2 + 𝜃12
2 2 2
− 𝑀1 𝑔𝐿1 cos 𝜃1 − 𝑀2 𝑔 𝐿1 cos 𝜃1 + 𝐿2 cos 𝜃1 + 𝜃2
So, forming the dynamics equations to be
𝑑 𝜕ℒ 𝜕ℒ
𝑓𝜃1,2 = −
𝑑𝑡 𝜕𝜃1,2 𝜕𝜃1,2
𝐵 𝑞 𝑞 + 𝐶 𝑞 , 𝑞 + 𝑔(𝑞) = 𝐹
𝜃1
𝑞=
𝜃2
𝑓𝜃1
𝐹=
𝑓𝜃2
Here we have: 𝑀1 = 𝑀2 = 𝐿1 = 𝐿2 = 1
2) Control Design
Having the system equation
𝐵 𝑞 𝑞 + 𝐶 𝑞, 𝑞 + 𝑔 𝑞 = 𝐹
We can have
−1
𝑞=𝐵 𝑞 [−𝐶 𝑞 , 𝑞 − 𝑔 𝑞 ] + 𝐹
With
−1
𝐹=𝐵 𝑞 𝐹⟺𝐹=𝐵 𝑞 𝐹
So, we decoupled the system to have the „new‟ (non-physical) input
𝑓1
𝐹=
𝑓2
However, the physical torque inputs to the system are
𝑓𝜃1 𝑓
=𝐵 𝑞 1
𝑓𝜃2 𝑓2
𝑒 𝜃1 = 𝜃1𝑓 − 𝜃1
𝑒 𝜃2 = 𝜃2𝑓 − 𝜃2
𝑓 = 𝐾𝑝 𝑒 + 𝐾𝐷 𝑒 + 𝐾𝐼 𝑒 𝑑𝑡
𝑓𝜃1 𝑓
=𝐵 𝑞 1
𝑓𝜃2 𝑓2
Solution
In order to apply all controls of Proportional-Derivative-Integral actions, a „dummy‟ state is added for
each angle to resemble the integration inside the computer:
𝑥1 = 𝑒 𝜃1 𝑑𝑡 ⟹ 𝑥1 = 𝜃1𝑓 − 𝜃1
𝑥2 = 𝑒 𝜃2 𝑑𝑡 ⟹ 𝑥2 = 𝜃2𝑓 − 𝜃2
So, the complete system equations are
𝑥1 = 𝜃1𝑓 − 𝜃1
𝑥2 = 𝜃2𝑓 − 𝜃2
𝜃1 −1
𝐾𝑝1 𝜃1𝑓 − 𝜃1 − 𝐾𝐷1 𝜃1 + 𝐾𝐼1 𝑥1
=𝐵 𝑞 −𝐶 𝑞, 𝑞 − 𝑔 𝑞 +
𝜃2 𝐾𝑝2 𝜃2𝑓 − 𝜃2 − 𝐾𝐷2 𝜃2 + 𝐾𝐼2 𝑥2
In MATLAB, “ode45” command was used to solve the ODE. (Full program in Appendix)
By trial & error, the 2 controllers‟ parameters were tuned to have the best performance. The best values
for the parameters was found to be
𝐾𝑝1 = 15
𝐾𝐷1 = 7
𝐾𝐼1 = 10
𝐾𝑝2 = 15
𝐾𝐷2 = 10
𝐾𝐼2 = 10
Actually, by observing the structure of above ODE with control, we can see (roughly):
However, above arguments are rough because of the high nonlinearity of the equation that:
Acceptable overshoot
Acceptable settling time
„Linear‟ behavior
NOTE: The same design was tested on other initial and final positions, the result was very different.
4) Torques Results
Here we analyze the torques resulted. The control inputs here are the torques. However, put in mind that
the actual joints torques are
𝑓𝜃1 𝑓
=𝐵 𝑞 1
𝑓𝜃2 𝑓2
5) Motion Animation
Using MATLAB, the resulting motion is animated in xy-plane to show the actual motion of the robot.
An “mpg” file is generated. The file name is “2DOF_rob.mpg”. (MATLAB program is in Appendix)
Appendix
1) System ODE function file (“r2dof.m”)
function xdot=r2dof(t,x,ths,spec,Kpid)
xdot=zeros(8,1);
%% set-points
th1s=ths(1);
th2s=ths(2);
%% Robot Specifications
M1=spec(3);
M2=spec(4);
L1=spec(1);
L2=spec(2);
g=9.8;
%% Inertia Matrix
b11=(M1+M2)*L1^2+M2*L2^2+2*M2*L1*L2*cos(x(4));
b12=M2*L2^2+M2*L1*L2*cos(x(4));
b21=M2*L2^2+M2*L1*L2*cos(x(4));
b22=M2*L2^2;
Bq=[b11 b12;b21 b22];
%% C Matrix
c1=-M2*L1*L2*sin(x(4))*(2*x(5)*x(6)+x(6)^2);
c2=-M2*L1*L2*sin(x(4))*x(5)*x(6);
Cq=[c1;c2];
%% Gravity Matrix
g1=-(M1+M2)*g*L1*sin(x(3))-M2*g*L2*sin(x(3)+x(4));
g2=-M2*g*L2*sin(x(3)+x(4));
Gq=[g1;g2];
%% PID Control
% PID parameters for theta 1
Kp1=Kpid(1);
Kd1=Kpid(2);
Ki1=Kpid(3);
% PID parameters for theta 2
Kp2=Kpid(4);
Kd2=Kpid(5);
Ki2=Kpid(6);
%decoupled control input
f1=Kp1*(th1s-x(3))-Kd1*x(5)+Ki1*(x(1));
f2=Kp2*(th2s-x(4))-Kd2*x(6)+Ki2*(x(2));
Fhat=[f1;f2];
%% System states
xdot(1)=(th1s-x(3)); %dummy state of theta1 integration
xdot(2)=(th2s-x(4)); %dummy state of theta2 integration
xdot(3)=x(5); %theta1-dot
xdot(4)=x(6); %theta2-dot
q2dot=inv(Bq)*(-Cq-Gq+F);
xdot(5)=q2dot(1); %theta1-2dot
xdot(6)=q2dot(2); %theta1-2dot
%% Robot Specifications
L1=1; %link 1
L2=1; %link 2
M1=1; %mass 1
M2=1; %mass 2
spec=[L1 L2 M1 M2];
%% PID Parameters
% PID parameters for theta 1
Kp1=15;
Kd1=7;
Ki1=10;
% PID parameters for theta 2
Kp2=15;
Kd2=10;
Ki2=10;
%% ODE solving
% opt1=odeset('RelTol',1e-10,'AbsTol',1e-20,'NormControl','off');
[T,X] = ode45(@(t,x) r2dof(t,x,ths,spec,Kpid),Ts,x0);
%% Output
th1=X(:,3); %theta1 wavwform
th2=X(:,4); %theta2 wavwform
%xy
x1=L1.*sin(th1); % X1
y1=L1.*cos(th1); % Y1
x2=L1.*sin(th1)+L2.*sin(th1+th2); % X2
y2=L1.*cos(th1)+L2.*cos(th1+th2); % Y2
%% generating images in 2D
figure
for i=1:length(j)-1
hold off
plot([x1(j(i)) x2(j(i))],[y1(j(i)) y2(j(i))],'o',[0 x1(j(i))],[0
y1(j(i))],'k',[x1(j(i)) x2(j(i))],[y1(j(i)) y2(j(i))],'k')
title('Motion of 2DOF Robotic Arm')
xlabel('x')
ylabel('y')
axis([-3 3 -3 3]);
grid
hold on
MM(i)=getframe(gcf);
end
drawnow;