推导倒立摆模型并使用LQR进行位置跟踪控制
推导倒立摆模型并使用LQR进行位置跟踪控制
一 参数设定
倒立摆示意图如下：
其中各个符号如下:
| 符号 | 含义 | 符号 | 含义 |
|---|---|---|---|
| \(M\) | 小车质量 | \(m\) | 摆杆质量 |
| \(b_1\) | 小车移动阻尼 | \(b_2\) | 摆杆转动阻尼 |
| \(x\) | 小车位置(水平向右为正) | \(\theta\) | 摆杆摆动的角度(顺时针转动为正) |
| - | - | \(l\) | 转动关节到摆杆质心的长度 |
| \(F\) | 作用到小车的外力(水平向右为正) | \(I\) | 摆杆绕质心的转动惯量 |
| \(N_{车}\) | 摆杆对小车的力水平分量 | \(P_{杆}\) | 小车对摆杆的力竖直分量 |
| \(N_{杆}\) | 小车对摆杆的力水平分量 | \(P_{车}\) | 摆杆对小车的力竖直分量 |
二 受力分析
1. 小车水平方向:
\[\begin{align}
F-N_{车}-b_1\dot{x}=M\ddot{x}
\end{align}
\]
2. 摆杆水平方向：
\[\begin{align}
N_{杆}=m\frac{d^2(x+l\sin{\theta})}{dt^2}=m(\ddot{x}+\ddot{\theta}l\cos{\theta}-\dot{\theta}^2l\sin{\theta})
\end{align}
\]
3. 摆杆竖直方向：
\[\begin{align}
P_{杆}-mg=m\frac{d^2(l\cos{\theta})}{dt^2}=-m(\ddot{\theta}l\sin{\theta}+\dot{\theta}^2l\cos{\theta})
\end{align}
\]
4. 摆杆转动方向(对质心求矩)：
\[\begin{align}
P_{杆}l\sin{\theta}-N_{杆}l\cos{\theta}-b_2\dot{\theta}=I\ddot{\theta}
\end{align}
\]
5. 牛顿第三定律定律:
\[\begin{align}
& N_{车} = N_{杆}\\
& P_{车} = P_{杆}
\end{align}
\]
三 方程求解
1. 联立方程(1)、(2)、(5)，消去\(N_{杆}、N_{车}\):
\[\begin{align}
(M+m)\ddot{x}+b_1\dot{x}+m(\ddot{\theta}l\cos{\theta}-\dot{\theta}^2l\sin{\theta})=F
\end{align}
\]
2. 联立方程(2)、(3)、(4)，消去\(P_{杆}、N_{杆}\):
\[\begin{align}
(I+ml^2)\ddot{\theta}+b_2\dot{\theta}+ml\ddot{x}\cos{\theta}=mgl\sin{\theta}
\end{align}
\]
3. 联立方程(7)、(8)，整理得:
\[\begin{align}
\left\{
\begin{aligned}
&\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^2l^2\cos^2\theta-(I+ml^2)(M+m)}\\[5ex]
&\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^2l^2\cos^2\theta}
\end{aligned}
\right.
\end{align}
\]
\[\bf{注意：上述方程均采用MATLAB进行推导，正确与否还有待验证！！！}
\]
四 模型线性化
由三 方程求解 的方程结果可知，倒立摆系统是一个的非线性系统。
为了便于实现后续的控制，可在工作点(\(\theta = 0\))附近，进行线性化,具体过程如下：
当\(\theta \approx 0\)处，有：
\[\begin{align}
\left\{
\begin{aligned}
\cos{\theta}& = 0\\
\sin{\theta}& = \theta\\
\dot{\theta}^2& = 0\\
\end{aligned}
\right.
\end{align}
\]
带入(10)到三 方程求解 中的非线性模型(9)中，简单整理可得：
\[\begin{align}
\left\{
\begin{aligned}
&\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{aligned}
\right.
\end{align}
\]
其中\(I = \frac{ml^2}{3}\)
取\(X=[\theta \quad \dot{\theta} \quad x \quad \dot{x}]^T\)为状态变量，倒立摆状态空间模型如下:
\[
\begin{align}
\left\{
\begin{aligned}
&\dot{X} = AX+Bu\\
&Y = CX
\end{aligned}
\right.
\end{align}
\]
其中:
\[
A =
\begin{bmatrix}
0 &1 &0 &0\\[1ex]
\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}\\[1ex]
0 &0 &0 &1\\[1ex]
-\frac{3mg}{4M+m} &\frac{3b_2}{(4M+m)l} &0 &-\frac{4b_1}{4M+m}\\[1ex]
\end{bmatrix}
,
B =
\begin{bmatrix}
0\\[1ex]
-\frac{3}{(4M+m)l}\\[1ex]
0\\[1ex]
\frac{4}{4M+m}\\[1ex]
\end{bmatrix}
,
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^TQE+u^TRu) dt = \int_{0}^{\infty} [(X-Ref)^TQ(X-Ref)+u^TRu] 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. 倒立摆全状态反馈控制模型仿真
取模型中固有参数如下:
\[\left\{
\begin{aligned}
M &= 1.0\hspace{0.5em}kg \\
m &= 0.5\hspace{0.5em}kg \\
l &= 0.5\hspace{0.5em}m \\
b_1 &= 0.3\hspace{0.5em} \\
b_2 &= 0.0\hspace{0.5em} \\
\end{aligned}
\right.
\]
按照四 模型线性化中的模型得：
\[A =
\begin{bmatrix}
0 &1 &0 &0\\[1ex]
19.6133 &0 &0 &0.4\\[1ex]
0 &0 &0 &1\\[1ex]
-3.2689 &0 &0 &-0.2667\\[1ex]
\end{bmatrix}
,
B =
\begin{bmatrix}
0\\[1ex]
-1.3333\\[1ex]
0\\[1ex]
0.8889\\[1ex]
\end{bmatrix}
,
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\\[1ex]
0 &1 &0 &0\\[1ex]
0 &0 &10000 &0\\[1ex]
0 &0 &0 &1\\[1ex]
\end{bmatrix}
,
R =
\begin{bmatrix}
0.001 \\
\end{bmatrix}
\]
通过MATLAB中lqr函数计算出反馈增益\(K\):
\[K=[-7470.90\quad-1559.47\quad-3162.27\quad-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
浙公网安备 33010602011771号