Điều khiển LQR mô hình con lắc ngược 1

Bài viết này sẽ hướng dẫn các bạn điều khiển mô hình con lắc gắn trên xe bằng bộ điều khiển LQR trên Matlab. Đầu tiên mình sẽ phân tích để tìm ra phương trình trạng thái của hệ thống sau đó viêt code mô phỏng trong matlab.

Mô hình hệ thống

invert-pendulum
Trong đó:

  • $u$ là lực tác dụng lên xe
  • $x$ là vị trí của xe
  • $\theta$ là góc nghiêng của con lắc
  • $m$ là khối lượng của con lắc
  • $M$ là khối lượng của xe
  • $I$ là moment quán tính của con lắc.
  • $L=2l$ là chiều dài của con lắc ngược

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

invert-pendulum-2

Phương pháp 1

Áp dụng các định luật Newton
Phân tích lực tác động lên từng thành phần của hệ thống là xe và con lắc. Tại khớp nối sẽ xuất hiện các cặp phản lực tương ứng triệt tiêu nhau là $H$ và $V$. Giả sử con lắc có dạng thanh thẳng đồng chất, trọng tậm $G$ nằm ở chính giữa thanh với tọa độ:$\left(x_{G},y_{G}\right)$:
\begin{equation}\label{ref1}
\begin{cases}x_{G}=x+lsin\theta\\y_{G}=lcos\theta\end{cases}\qquad
\end{equation}
Con lắc chuyển động xoay quanh trọng tâm $G$ và chuyển động theo xe:
Chuyển động xoay:
\begin{equation}\label{ref2}
I\ddot{\theta}=Vl\sin\theta-Hl\cos\theta\qquad
\end{equation}
Chuyển động theo phương ngang:
\begin{equation}\label{ref3}
\ddot{x}_{G}=\frac{d^{2}\left(x_{G}\right)}{dt^{2}}=H\qquad
\end{equation}
\begin{equation}
\Leftrightarrow \frac{d^{2}\left(x+l\sin\theta\right)}{dt^{2}}=H\nonumber
\end{equation}
Chuyển động theo phương đứng:
\begin{equation}\label{ref4}
\ddot{y}_{G}=\frac{d^{2}\left(y_{G}\right)}{dt^{2}}=HV-mg\qquad
\end{equation}
\begin{equation}
\Leftrightarrow\frac{d^{2}\left(l\cos\theta\right)}{dt^{2}}=V-mg\nonumber
\end{equation}
Phương trình chuyển động của xe:
\begin{equation}\label{ref5} M\ddot{x}=u-H\qquad\end{equation}
Mục tiêu điều khiển là duy trì con lắc nằm ở phương thẳng đứng, chúng ta giả sử rằng $\theta\left(t\right)$ và $\dot{\theta}\left(t\right)$ nhỏ nên có thể xem $\sin\theta\approx\theta,\cos\theta\approx1,\theta\dot{\theta}\approx0$. Phương trình động lực học của hệ thống:
\begin{equation}\label{ref6}
\left(2\right)\Leftrightarrow I\ddot{\theta}=Vl\theta-Hl\qquad
\end{equation}
\begin{equation}\label{ref7}
\left(3\right)\Leftrightarrow m\left(\ddot{x}+l\ddot{\theta}\right)=H\qquad
\end{equation}
\begin{equation}\label{ref8}
\left(4\right)\Leftrightarrow 0=V-mg\qquad
\end{equation}
Kết hợp \eqref{ref5} và \eqref{ref7} ta được:
\begin{equation}\label{ref9}
\left(M+m\right)\ddot{x}+ml\ddot{\theta}=u\qquad
\end{equation}
Kết hợp \eqref{ref6}, \eqref{ref7} và \eqref{ref8}:
\begin{equation}\label{ref10}
\left(I+ml^{2}\right)\ddot{\theta}+ml\ddot{x}=mgl\theta\qquad
\end{equation}

Phương pháp 2

Áp dụng phương trình Euler-Lagrange (đang cập nhật)

Laplace 2 vế của \eqref{ref9} và \eqref{ref10}:
\begin{equation}\label{ref11}
\begin{bmatrix}\dot{\theta} \\ \ddot{\theta} \\\dot{x} \\ \ddot{x} \end{bmatrix}=\begin{bmatrix}0&1&0&0 \\ \frac{\left(M+m\right)mgl}{I\left(M+m\right)+Mml^{2}}&0&0&0 \\0&0&0&1\\ -\frac{m^{2}gl^{2}}{I\left(M+m\right)+Mml^{2}}&0&0&0 \end{bmatrix}\begin{bmatrix} \theta \\ \dot{\theta} \\x \\ \dot{x} \end{bmatrix}+\begin{bmatrix}0 \\ -\frac{ml}{I\left(M+m\right)+Mml^{2}} \\0 \\ \frac{I+ml^{2}}{I\left(M+m\right)+Mml^{2}} \end{bmatrix}u\qquad
\end{equation}

Bộ điều khiển LQR

Đang cập nhật

Code Matlab

Inverted_Pendulum.m

pendulum.m

1 Bình luận

Trackbacks and Pingbacks

  1. […] Ở bài viết này sẽ hướng dẫn các bạn viết chương trình mô phỏng con lắc ngược trong Matlab. Mô hình sử dụng bộ điều khiển PID. Phần tính toán động lực học các bạn xem ở bài viết Điều khiển LQR mô hình con lắc ngược […]

Bình luận

Website này sử dụng Akismet để hạn chế spam. Tìm hiểu bình luận của bạn được duyệt như thế nào.