Điều khiển tay máy phẳng hai bậc tự do

Giới thiệu

Bài viết này sẽ trình bày việc tính toán động lực học tay máy phẳng 2 bậc tự do. Quá trình tính toán phương trình động lực học sử dụng bằng Matlab symbolics để tính toán với các biến số. Sau khi tìm được phương trình động lực học thì bài viết sẽ hướng dẫn sử dụng bộ điều khiển PID để điều khiển tay máy di chuyển từ vị trí ban đầu đến vị trí mong muốn. Kết quả mô phỏng cho thấy sai số hội tụ về 0 và cho thấy giá trị torque mà động cơ tại mỗi khớp cần cung cấp. Cuối cùng bài viết sẽ cung cấp toàn bộ code Matlab dùng để mô phỏng.

Phương trình động lực học

Giả sử tay máy có thông số như hình vẽ

Vị trí của khối lượng m_1 và m_2 lần lượt là (x_1,y_1) và (x_2,y_2). Chúng ta dễ dàng tính được:

\begin{aligned}
\begin{bmatrix}x_1\\y_1\end{bmatrix}&=\begin{bmatrix}l_1\sin\theta_1\\l_1\cos\theta_1\end{bmatrix}\;\;\;(1)\\ 
\begin{bmatrix}x_2\\y_2\end{bmatrix}&=\begin{bmatrix}l_1\sin\theta_1+l_2\sin\left(\theta_1+\theta_2\right)\\l_1\cos\theta_1+l_2\cos\left(\theta_1+\theta_2\right)\end{bmatrix}\;\;\;(2)
\end{aligned}

Động năng của cơ hệ:

\begin{aligned}
KE &= \dfrac{1}{2}m_1\left(\dot{x}_{1}^{2}+\dot{y}_{1}^{2}\right)+\dfrac{1}{2}m_2\left(\dot{x}_{2}^{2}+\dot{y}_{2}^{2}\right) \;\;\;(3)
\end{aligned}

Thế phương trình (1) và (2) vào (3) ta được:

\begin{aligned}

KE &= \frac{1}{2}\left(m_1+m_2\right)l_{1}^{2}\dot{\theta}_{1}^{2}+\frac{1}{2}m_2l_{2}^{2}\dot{\theta}_{1}^{2}+m_{2}l_{2}^{2}\dot{\theta}_{1}\dot{\theta}_{2}+\frac{1}{2}m_{2}l_{2}^{2}\dot{\theta}_{2}^{2}&&\\
&+m_{2}l_{1}l_{2}\left(\dot{\theta}_{1}\dot{\theta}_{2} \dot{\theta}_{2}^{2}\right)\cos{\theta_{2}} \;\;\;(4)

\end{aligned}

Thế năng của cơ hệ:

\begin{aligned}
PE &= m_{1}gy_{1}+m_{2}gy_{2}&&\\
& = m_{1}gl_{1}\cos{\theta_{1}}+m_{2}g\left[l_{1}\cos{\theta_{1}}+l_{2}\cos{\left(\theta_{1}+\theta_{2}\right)}\right] \;\;\;(5)
\end{aligned}

Phương trình Lagrang \mathcal{L}=KE-PE

Phương trình động lực học tay máy

\begin{aligned}
\dfrac{d}{dt}\dfrac{\partial\mathcal{L}}{\partial\dot{q}}-\dfrac{\partial\mathcal{L}}{\partial q}=f \;\;\;(6)
\end{aligned}

Trong đó: q=\begin{bmatrix}\theta_{1}\\\theta_{2}\end{bmatrix} và f=\begin{bmatrix}\tau_{1}\\\tau_{2}\end{bmatrix}

Sau khi tính toán (có thể dùng Matlab Symbolic để tính toán với các biến số) ta đưa (6) về dạng

\begin{aligned}
B\left(q\right)\ddot{q}+C\left(\dot{q},q\right)+G\left(q\right)=F \;\;\;(7)

\end{aligned}

Với:

\begin{align*}

B\left(q\right)\ddot{q}&=\begin{bmatrix}

\left(m_{1}+m_{2}\right)l_{1}^{2}+m_{2}l_{2}^{2}+2m_{2}l_{1}l_{2}\cos{\theta_{2}}&m_{2}l_{2}+m_{2}l_{1}l_{2}\cos{\theta_{2}}

\\

m_{2}l_{2}^{2}+m_{2}l_{1}l_{2}\cos{\theta_{2}}& m_{2}l_{2}^{2}

\end{bmatrix}\\

C\left(\dot{q},q\right)&=\begin{bmatrix}-m_{2}l_{1}l_{2}\sin{\theta_{2}}\left(2\dot{\theta}_{1}\dot{\theta}_{2}+\dot{\theta}_{2}^{2}\right)

\\ -m_{2}l_{1}l_{2}\sin{\theta_{2}}\dot{\theta}_1\dot{\theta}_{2}

\end{bmatrix}\\

G\left(q\right)&=\begin{bmatrix}-\left(m_{1}+m_{2}\right)gl_{1}\sin{\theta_{1}}-m_{2}gl_{2}\sin{\left(\theta_{1}+\theta_{2}\right)}	\\ -m_{2}gl_{2}\sin{\left(\theta_{1}+\theta_{2}\right)}
\end{bmatrix}

\end{align*}

Thiết kế bộ điều khiển

Từ phương trình (7) chúng ta có:

\begin{aligned} \ddot{q}&=B^{-1}\left(q\right)\left[-C\left(\dot{q},q\right)-G\left(q\right)\right]+\hat{F} \end{aligned} \;\;\;(8)

Trong đó:\hat{F}=B^{-1}\left(q\right)F\Leftrightarrow F=B\left(q\right)\hat{F}.

Đặt:

u=\hat{F}=\begin{bmatrix}
u_1\\ 
u_2
\end{bmatrix}

thì u chính là tín hiệu điều khiển và mối liên hệ với torque của động cơ tại mỗi khớp

\begin{aligned}
\begin{bmatrix}
 \tau_{1} \\ \tau_{2} \end{bmatrix}&=B\left(q\right)\begin{bmatrix} u_{1} \\ u_{2} 
 \end{bmatrix} \;\;\;(9)
\end{aligned} 

Định nghĩa sai số điều khiển

\begin{aligned} 
e&=q^{d}-q&&\\ \Leftrightarrow 
\begin{bmatrix} e_{1} \\ e_{2} \end{bmatrix}&=\begin{bmatrix} \theta_{1}^{d} \\ \theta_{2}^{d} \end{bmatrix}-\begin{bmatrix} \theta_{1} \\ \theta_{2} \end{bmatrix} \;\;\;(10)
\end{aligned}

Với: q^{d}=\begin{bmatrix} \theta_{1}^{d} \ \theta_{2}^{d} \end{bmatrix} là giá trị góc mong muốn của từng khớp và q=\begin{bmatrix} \theta_{1} \ \theta_{2} \end{bmatrix} là giá trị góc hiện tại của hai khớp.

Bộ điều khiển PID

Cấu trúc của bộ điều khiển PID

\begin{aligned} u&=K_{p}e+K_{d}\dot{e}+K_{i}\int edt \end{aligned} \;\;\;(11)

Thế (10) vào (11) ta được

\begin{aligned}
u&=K_{p}\left(q^{d}-q\right)+K_{d}\left(\dot{q}^{d}-\dot{q}\right)+K_{i} \int \left(q^{d}-q\right)dt \;\;\;(12)
\end{aligned} 

Lưu ý rằng: q^{d}=\begin{bmatrix} \theta_{1}^{d} \ \theta_{2}^{d} \end{bmatrix} là vị trí góc mong muốn và giá trị này là một hằng số nên \dot{q}^{d}=\begin{bmatrix} 0 \ 0 \end{bmatrix}

Vì vậy (12) trở thành:

\begin{aligned} u&=K_{p}\left(q^{d}-q\right)-K_{d}\dot{q}+K_{i} \int \left(q^{d}-q\right)dt \end{aligned} 

Viết lại dưới dạng ma trận:

\begin{aligned}
\begin{bmatrix} u_{1} \\ u_{2} \end{bmatrix}&=\begin{bmatrix} K_{1}^{p} & 0 \\ 0 & K_{2}^{p} \end{bmatrix} \begin{bmatrix} \theta_{1}^{d}-\theta_{1} \\ \theta_{2}^{d}-\theta_{2} \end{bmatrix} - \begin{bmatrix} K_{1}^{d} & 0 \\ 0 & K_{2}^{d} \end{bmatrix} \begin{bmatrix} \dot{\theta}_{1} \\ \dot{\theta}_{2}\end{bmatrix}\\ &\quad+\begin{bmatrix} K_{1}^{i} & 0 \\ 0 & K_{2}^{i} \end{bmatrix} \begin{bmatrix}\int \left(\theta_{1}^{d}-\theta_{1}\right)dt \\ \int\left(\theta_{2}^{d}-\theta_{2}\right)dt
\end{bmatrix} \;\;\;(13)
\end{aligned} 

Tiếp theo chúng ta đặt biến phụ

\begin{aligned} x&=\begin{bmatrix} x_{1} \\ x_{2} \end{bmatrix} = \begin{bmatrix}\int \left(\theta_{1}^{d}-\theta_{1}\right)dt \\ \int\left(\theta_{2}^{d}-\theta_{2}\right)dt\end{bmatrix}\\ \Rightarrow \dot{x}&=\begin{bmatrix} \dot{x}_{1} \\ \dot{x}_{2} \end{bmatrix}=\begin{bmatrix}\theta_{1}^{d}-\theta_{1} \\ \theta_{2}^{d}-\theta_{2}\end{bmatrix} 
\end{aligned} 

Phương trình (13) trở thành

\begin{aligned} \begin{bmatrix} u_{1} \\ u_{2} \end{bmatrix}&=\begin{bmatrix} K_{1}^{p} & 0 \\ 0 & K_{2}^{p} \end{bmatrix} \begin{bmatrix} \theta_{1}^{d}-\theta_{1} \\ \theta_{2}^{d}-\theta_{2} \end{bmatrix} - \begin{bmatrix} K_{1}^{d} & 0 \\ 0 & K_{2}^{d} \end{bmatrix} \begin{bmatrix} \dot{\theta}_{1} \\ \dot{\theta}_{2}\end{bmatrix}\\ &\quad+\begin{bmatrix} K_{1}^{i} & 0 \\ 0 & K_{2}^{i} \end{bmatrix} \begin{bmatrix} x_{1} \\ x_{2} 
\end{bmatrix} \;\;\; (14)
\end{aligned} 

Cuối cùng phương trình động lực học của tay máy dùng để mô phỏng trong Matlab

\begin{aligned}

\left\{\begin{matrix}

\dot{x}_{1}&=\theta_{1}^{d}-\theta_{1}\\

\dot{x}_{2}&=\theta_{2}^{d}-\theta_{2}\\

\begin{bmatrix} \ddot{\theta}_{1} \\ \ddot{\theta}_{2} \end{bmatrix}&=B^{-1}\left(\theta\right)\left[-C\left(\dot{\theta},\theta\right)-G\left(\theta\right)\right]+\begin{bmatrix} u_{1} \\ u_{2} \end{bmatrix}

\end{matrix}\right. \;\;\;(15)

\end{aligned}

Moment của động cơ tại mỗi khớp

\begin{aligned} 
\begin{bmatrix} \tau_{1} \\ \tau_{2} \end{bmatrix}&=B\left(\theta\right)\begin{bmatrix} u_{1} \\ u_{2} \end{bmatrix} \;\;\; (16)
\end{aligned}

Bộ điều khiển Computed torque

Đang cập nhật

Kết quả mô phỏng

Code Matlab

Code ODE45

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];
F=Bq*Fhat; % actual input to the system
%% 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=Bq\(-Cq-Gq+F);
xdot(5)=q2dot(1); %theta1-2dot
xdot(6)=q2dot(2); %theta1-2dot
%control input function output to outside computer program
xdot(7)=F(1);
xdot(8)=F(2);

Code main

close all
clear all
clc
%% Initilization
th_int=[-pi/2 pi/2]; %initial positions
ths=[pi/2 -pi/2]; %set-points
x0=[0 0 th_int 0 0 0 0]; %states initial values
Ts=[0 20]; %time span
%% 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;
Kpid=[Kp1 Kd1 Ki1 Kp2 Kd2 Ki2];
%% 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
%torque inputs computation from the 7th,8th states inside ODE
F1=diff(X(:,7))./diff(T);
F2=diff(X(:,8))./diff(T);
tt=0:(T(end)/(length(F1)-1)):T(end);
%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
%theta1 error plot
plot(T,ths(1)-th1)
grid
title('Theta-1 error')
ylabel('theta1 error (rad)')
xlabel('time (sec)')
%theta2 error plot
figure
plot(T,ths(2)-th2)
grid
title('Theta-2 error')
ylabel('theta2 error (rad)')
xlabel('time (sec)')
%torque1 plot
figure
plot(tt,F1)
grid
title('Torque of theta 1')
ylabel('theta1 torque')
xlabel('time (sec)')
%torque2 plot
figure
plot(tt,F2)
grid
title('Torque of theta 2')
ylabel('theta2 torque')
xlabel('time (sec)')

Code animation

%% setting frames speed
d=2;
j=1:d:length(T);
%% generating images in 2D
figure
vd = VideoWriter('2DOF_rob.avi');
open(vd);
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
drawnow;
frame = getframe;
writeVideo(vd,frame);
end
close(vd);