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

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

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
Tay máy phẳng hai bậc tự do

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{align}
\label{eq:1}
\begin{bmatrix}x_1\\y_1\end{bmatrix}&=\begin{bmatrix}l_1\sin\theta_1\\l_1\cos\theta_1\end{bmatrix}\\
\label{eq:2}\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}
\end{align}
Động năng của cơ hệ
\begin{align}\label{eq:3}
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)
\end{align}
Thế phương trình \eqref{eq:1} và \eqref{eq:2} vào \eqref{eq:3} ta được:
\begin{align}\label{eq:4}
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}&&\\\nonumber
&+m_{2}l_{1}l_{2}\left(\dot{\theta}_{1}\dot{\theta}_{2} \dot{\theta}_{2}^{2}\right)\cos{\theta_{2}}
\end{align}
Thế năng của cơ hệ
\begin{align}\label{eq:5}
PE &= m_{1}gy_{1}+m_{2}gy_{2}&&\\\nonumber
& = 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]
\end{align}
Phương trình Lagrang $\mathcal{L}=KE-PE$
Phương trình động lực học tay máy

\begin{align}\label{eq:6}
\dfrac{d}{dt}\dfrac{\partial\mathcal{L}}{\partial\dot{q}}-\dfrac{\partial\mathcal{L}}{\partial q}=f
\end{align}
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 \eqref{eq:6} về dạng
\begin{align}\label{eq:7}
B\left(q\right)\ddot{q}+C\left(\dot{q},q\right)+G\left(q\right)=F
\end{align}
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 \eqref{eq:7} chúng ta có
\begin{align}\label{eq:8}
\ddot{q}&=B^{-1}\left(q\right)\left[-C\left(\dot{q},q\right)-G\left(q\right)\right]+\hat{F}
\end{align}
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{align}\label{eq:9}
\begin{bmatrix} \tau_{1} \\ \tau_{2} \end{bmatrix}&=B\left(q\right)\begin{bmatrix} u_{1} \\ u_{2} \end{bmatrix}
\end{align}
Định nghĩa sai số điều khiển
\begin{align}\label{eq:10}
e&=q^{d}-q&&\\\nonumber
\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}
\end{align}
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{align}\label{eq:11}
u&=K_{p}e+K_{d}\dot{e}+K_{i}\int edt
\end{align}
Thế \eqref{eq:10} vào \eqref{eq:11} ta được
\begin{align}\label{eq:12}
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
\end{align}
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 (\ref{eq:12}) trở thành
\begin{align*}
u&=K_{p}\left(q^{d}-q\right)-K_{d}\dot{q}+K_{i} \int \left(q^{d}-q\right)dt
\end{align*}
Viết lại dưới dạng ma trận
\begin{align}\label{eq:13}
\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}\\\nonumber
&\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}
\end{align}
Tiếp theo chúng ta đặt biến phụ
\begin{align*}
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{align*}
Phương trình \eqref{eq:13} trở thành
\begin{align}\label{eq:14}
\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}\\\nonumber
&\quad+\begin{bmatrix} K_{1}^{i} & 0 \\ 0 & K_{2}^{i} \end{bmatrix} \begin{bmatrix} x_{1} \\ x_{2} \end{bmatrix}
\end{align}
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{align}\label{eq:15}
\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.
\end{align}
Moment của động cơ tại mỗi khớp
\begin{align}\label{eq:16}
\begin{bmatrix} \tau_{1} \\ \tau_{2} \end{bmatrix}&=B\left(\theta\right)\begin{bmatrix} u_{1} \\ u_{2} \end{bmatrix}
\end{align}

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

Code main

Code animation

1 Bình luận

Trackbacks and Pingbacks

  1. […] Bài viết này sẽ hướng dẫn tính toán đơn giản các phương trình chứa biến symbolic trong MATLAB. Ví dụ được sử dụng là xác định phương trình động lực học của cơ hệ thông qua phương trình Lagrang trong bài viết […]

Viết bình luận

Địa chỉ email của bạn sẽ được giữ bí mật. Đánh dấu * là trường bắt buộc

Trang web này sử dụng Akismet để giảm spam. Tìm hiểu cách xử lý dữ liệu nhận xét của bạn .