倒立摆模型推导与LQR位置跟踪控制详解
倒立摆模型推导与LQR位置跟踪控制详解
倒立摆控制问题是一个经典的控制理论问题,广泛应用于机器人、航天器姿态控制等领域。本文将详细介绍倒立摆模型的推导过程,并使用LQR(线性二次调节器)方法实现位置跟踪控制。通过MATLAB/Simulink仿真,验证控制算法的有效性。
一 参数设定
倒立摆示意图如下:
其中各个符号如下:
符号 | 含义 | 符号 | 含义 |
---|---|---|---|
$M$ | 小车质量 | $m$ | 摆杆质量 |
$b_1$ | 小车移动阻尼 | $b_2$ | 摆杆转动阻尼 |
$x$ | 小车位置(水平向右为正) | $\theta$ | 摆杆摆动的角度(顺时针转动为正) |
- | - | $l$ | 转动关节到摆杆质心的长度 |
$F$ | 作用到小车的外力(水平向右为正) | $I$ | 摆杆绕质心的转动惯量 |
$N_{车}$ | 摆杆对小车的力水平分量 | $P_{杆}$ | 小车对摆杆的力竖直分量 |
$N_{杆}$ | 小车对摆杆的力水平分量 | $P_{车}$ | 摆杆对小车的力竖直分量 |
二 受力分析
1. 小车水平方向:
$$
F - N_{车} - b_1 \dot{x} = M \ddot{x}
$$
2. 摆杆水平方向:
$$
N_{杆} = m \frac{d^2(x + l \sin \theta)}{dt^2} = m (\ddot{x} + \ddot{\theta} l \cos \theta - \dot{\theta}^2 l \sin \theta)
$$
3. 摆杆竖直方向:
$$
P_{杆} - mg = m \frac{d^2(l \cos \theta)}{dt^2} = -m (\ddot{\theta} l \sin \theta + \dot{\theta}^2 l \cos \theta)
$$
4. 摆杆转动方向(对质心求矩):
$$
P_{杆} l \sin \theta - N_{杆} l \cos \theta - b_2 \dot{\theta} = I \ddot{\theta}
$$
5. 牛顿第三定律定律:
$$
N_{车} = N_{杆}, \quad P_{车} = P_{杆}
$$
三 方程求解
1. 联立方程(1)、(2)、(5),消去$N_{杆}$、$N_{车}$:
$$
(M + m) \ddot{x} + b_1 \dot{x} + m (\ddot{\theta} l \cos \theta - \dot{\theta}^2 l \sin \theta) = F
$$
2. 联立方程(2)、(3)、(4),消去$P_{杆}$、$N_{杆}$:
$$
(I + ml^2) \ddot{\theta} + b_2 \dot{\theta} + ml \ddot{x} \cos \theta = mgl \sin \theta
$$
3. 联立方程(7)、(8),整理得:
$$
\begin{align}
\ddot{\theta} &= \frac{(M+m)b_2 \dot{\theta} + ml [(F + ml \dot{\theta}^2 \sin \theta - b_1 \dot{x}) \cos \theta - (M+m)g \sin \theta]}{m^2 l^2 \cos^2 \theta - (I + ml^2)(M+m)} \
\ddot{x} &= \frac{(I + ml^2)(F - b_1 \dot{x}) + ml [ml (l \dot{\theta}^2 \sin \theta - g \cos \theta \sin \theta) + I \dot{\theta} \sin \theta + b_2 \dot{\theta} \cos \theta]}{(I + ml^2)(M+m) - m^2 l^2 \cos^2 \theta}
\end{align}
$$
注意:上述方程均采用MATLAB进行推导,正确与否还有待验证!!!
四 模型线性化
由三 方程求解的方程结果可知,倒立摆系统是一个非线性系统。
为了便于实现后续的控制,可在工作点($\theta = 0$)附近,进行线性化,具体过程如下:
当$\theta \approx 0$处,有:
$$
\begin{align}
\cos \theta &= 0 \
\sin \theta &= \theta \
\dot{\theta}^2 &= 0
\end{align}
$$
带入(10)到三 方程求解中的非线性模型(9)中,简单整理可得:
$$
\begin{align}
\ddot{\theta} &= \frac{3(M+m)g}{(4M+m)l} \theta - \frac{3(M+m)b_2}{(4M+m)ml^2} \dot{\theta} + \frac{3b_1}{(4M+m)l} \dot{x} - \frac{3}{(4M+m)l} F \
\ddot{x} &= -\frac{3mg}{4M+m} \theta + \frac{3b_2}{(4M+m)l} \dot{\theta} - \frac{4b_1}{4M+m} \dot{x} + \frac{4}{4M+m} F
\end{align}
$$
其中$I = \frac{ml^2}{3}$
取$X = [\theta \quad \dot{\theta} \quad x \quad \dot{x}]^T$为状态变量,倒立摆状态空间模型如下:
$$
\begin{align}
\dot{X} &= AX + Bu \
Y &= CX
\end{align}
$$
其中:
$$
A = \begin{bmatrix}
0 & 1 & 0 & 0 \
\frac{3(M+m)g}{(4M+m)l} & -\frac{3(M+m)b_2}{(4M+m)ml^2} & 0 & \frac{3b_1}{(4M+m)l} \
0 & 0 & 0 & 1 \
-\frac{3mg}{4M+m} & \frac{3b_2}{(4M+m)l} & 0 & -\frac{4b_1}{4M+m}
\end{bmatrix}, \quad
B = \begin{bmatrix}
0 \
-\frac{3}{(4M+m)l} \
0 \
\frac{4}{4M+m}
\end{bmatrix}, \quad
C = \begin{bmatrix}
1 & 0 & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 1 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}
$$
五 采用LQR方法进行全状态反馈控制
1. LQR(Linear Quadratic Regulator)方法
针对线性系统,定义状态误差加权和控制输入加权为代价函数$J$:
$$
J = \int_{0}^{\infty} (E^T Q E + u^T R u) dt = \int_{0}^{\infty} [(X - Ref)^T Q (X - Ref) + u^T R u] dt
$$
其中$Q$,$R$为正定或者半正定矩阵,$Ref$为参考的状态
通过系列方法,最终求解黎卡提$Riccati$方程:
$$
A^T P + PA - P B R^{-1} B^T P + Q = 0
$$
得出$K = R^{-1} B^T P$
最终得出使得代价$J$最小的控制律为:
$$
u = -K (X - Ref)
$$
在MATLAB中可使用函数lqr(lqr函数介绍)直接得出$K$:
调用$lqr(A,B,Q,R)$即可求解$K$
关于lqr方法的推导过程可参考lqr推导过程
2. 倒立摆全状态反馈控制模型仿真
取模型中固有参数如下:
$$
\begin{align}
M &= 1.0\ kg \
m &= 0.5\ kg \
l &= 0.5\ m \
b_1 &= 0.3 \
b_2 &= 0.0
\end{align}
$$
按照四 模型线性化中的模型得:
$$
A = \begin{bmatrix}
0 & 1 & 0 & 0 \
19.6133 & 0 & 0 & 0.4 \
0 & 0 & 0 & 1 \
-3.2689 & 0 & 0 & -0.2667
\end{bmatrix}, \quad
B = \begin{bmatrix}
0 \
-1.3333 \
0 \
0.8889
\end{bmatrix}, \quad
C = \begin{bmatrix}
1 & 0 & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 1 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}
$$
设置$Q$,$R$如下:
$$
Q = \begin{bmatrix}
20000 & 0 & 0 & 0 \
0 & 1 & 0 & 0 \
0 & 0 & 10000 & 0 \
0 & 0 & 0 & 1
\end{bmatrix}, \quad
R = \begin{bmatrix}
0.001
\end{bmatrix}
$$
通过MATLAB中lqr函数计算出反馈增益$K$:
$$
K = [-7470.90\ -1559.47\ -3162.27\ -2193.39]
$$
在simulink中搭建控制仿真模型如下:
输入计算出的反馈增益$K$仿真结果如下:
MATLAB代码和simulink模型点击这里
欢迎大家一起交流!!!
Reference:
- LQR控制算法推导-连续与离散形式:https://blog.csdn.net/zjh2883/article/details/136167154
- MATLAB LQR函数:https://ww2.mathworks.cn/help/control/ref/lti.lqr.html