Đ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ẽ

Tay máy phẳng hai bậc tự do

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

[x1y1]=[l1sinθ1l1cosθ1]      (1)[x2y2]=[l1sinθ1+l2sin(θ1+θ2)l1cosθ1+l2cos(θ1+θ2)]      (2)\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ệ:

KE=12m1(x˙12+y˙12)+12m2(x˙22+y˙22)      (3)\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:

KE=12(m1+m2)l12θ˙12+12m2l22θ˙12+m2l22θ˙1θ˙2+12m2l22θ˙22+m2l1l2(θ˙1θ˙2θ˙22)cosθ2      (4)\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ệ:

PE=m1gy1+m2gy2=m1gl1cosθ1+m2g[l1cosθ1+l2cos(θ1+θ2)]      (5)\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 L=KEPE\mathcal{L}=KE-PE

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

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

Trong đó: q=[θ1θ2]q=\begin{bmatrix}\theta_{1}\\\theta_{2}\end{bmatrix}f=[τ1τ2]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

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

Với:

B(q)q¨=[(m1+m2)l12+m2l22+2m2l1l2cosθ2m2l2+m2l1l2cosθ2m2l22+m2l1l2cosθ2m2l22]C(q˙,q)=[m2l1l2sinθ2(2θ˙1θ˙2+θ˙22)m2l1l2sinθ2θ˙1θ˙2]G(q)=[(m1+m2)gl1sinθ1m2gl2sin(θ1+θ2)m2gl2sin(θ1+θ2)]\begin{alignedat} BB\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{alignedat}

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

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

q¨=B1(q)[C(q˙,q)G(q)]+F^      (8)\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 đó:F^=B1(q)FF=B(q)F^\hat{F}=B^{-1}\left(q\right)F\Leftrightarrow F=B\left(q\right)\hat{F}.

Đặt u=F^=[u1u2]u=\hat{F}=\begin{bmatrix} u_{1}\\u_{2} \end{bmatrix} thì uu 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

[τ1τ2]=B(q)[u1u2]      (9)\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

e=qdq[e1e2]=[θ1dθ2d][θ1θ2]      (10)\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: qd=[θ1dθ2d]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=[θ1θ2]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

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

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

u=Kp(qdq)+Kd(q˙dq˙)+Ki(qdq)dt      (12)\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 qd=[θ1dθ2d]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 q˙d=[00]\dot{q}^{d}=\begin{bmatrix} 0 \\ 0 \end{bmatrix}

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

u=Kp(qdq)Kdq˙+Ki(qdq)dt\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

[u1u2]=[K1p00K2p][θ1dθ1θ2dθ2][K1d00K2d][θ˙1θ˙2]+[K1i00K2i][(θ1dθ1)dt(θ2dθ2)dt]      (13)\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ụ

x=[x1x2]=[(θ1dθ1)dt(θ2dθ2)dt]x˙=[x˙1x˙2]=[θ1dθ1θ2dθ2]\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

[u1u2]=[K1p00K2p][θ1dθ1θ2dθ2][K1d00K2d][θ˙1θ˙2]+[K1i00K2i][x1x2]      (14)\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

{x˙1=θ1dθ1x˙2=θ2dθ2[θ¨1θ¨2]=B1(θ)[C(θ˙,θ)G(θ)]+[u1u2]      (15)\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

[τ1τ2]=B(θ)[u1u2]      (16)\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

Sai số góc khớp 1

Sai số góc khớp 1

Sai số góc khớp 2 Sai số góc khớp 2

Moment động cơ khớp 1 Moment động cơ khớp 1

Moment động cơ khớp 2 Moment động cơ khớp 2

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);

Đăng bởi@nvtienanh
Không gì là không thể.

GitHubTwitterFacebookLinkedIn