推导倒立摆模型并使用LQR进行位置跟踪控制

推导倒立摆模型采用LQR进行位置跟踪控制及其Simulink实现

一 参数设定

倒立摆示意图如下：

其中各个符号如下:

符号	含义	符号	含义
$M$	小车质量	$m$	摆杆质量
$b_1$	小车移动阻尼	$b_2$	摆杆转动阻尼
$x$	小车位置(水平向右为正)	$\theta$	摆杆摆动的角度(顺时针转动为正)
-	-	$l$	转动关节到摆杆质心的长度
$F$	作用到小车的外力(水平向右为正)	$I$	摆杆绕质心的转动惯量
$N_{车}$	摆杆对小车的力水平分量	$P_{杆}$	小车对摆杆的力竖直分量
$N_{杆}$	小车对摆杆的力水平分量	$P_{车}$	摆杆对小车的力竖直分量

二 受力分析

1. 小车水平方向:

$$\begin{array}{r}F-N_{车}-b_1\dot{x}=M\ddot{x}\end{array}$$

2. 摆杆水平方向：

$$\begin{array}{r}{N}_{杆}=m\frac{d^2(x+l\sin \theta )}{d{t}^2}=m(\ddot{x}+\ddot{\theta }l\cos \theta -{\dot{\theta }}^{2}l\sin \theta )\end{array}$$

3. 摆杆竖直方向：

$$\begin{array}{r}{P}_{杆}-mg=m\frac{d^2(l\cos \theta )}{d{t}^2}=-m(\ddot{\theta }l\sin \theta +{\dot{\theta }}^{2}l\cos \theta )\end{array}$$

4. 摆杆转动方向(对质心求矩)：

$$\begin{array}{r}{P}_{杆}l\sin \theta -N_{杆}l\cos \theta -b_2\dot{\theta }=I\ddot{\theta }\end{array}$$

5. 牛顿第三定律定律:

$$\begin{array}{rl}& N_{车}=N_{杆}\\ & P_{车}=P_{杆}\end{array}$$

三 方程求解

1. 联立方程(1)、(2)、(5)，消去$N_{杆}、N_{车}$:

$$\begin{array}{r}(M+m)\ddot{x}+b_1\dot{x}+m(\ddot{\theta }l\cos \theta -{\dot{\theta }}^{2}l\sin \theta )=F\end{array}$$

2. 联立方程(2)、(3)、(4)，消去$P_{杆}、N_{杆}$:

$$\begin{array}{r}(I+m{l}^2)\ddot{\theta }+b_2\dot{\theta }+ml\ddot{x}\cos \theta =mgl\sin \theta \end{array}$$

3. 联立方程(7)、(8)，整理得:

$$\begin{array}{r}\{\begin{array}{rl}& \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+m{l}^2)(M+m)}\\ & \ddot{x}=\frac{(I+m{l}^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+m{l}^2)(M+m)-m^2{l}^2{\cos }^2\theta }\end{array}\end{array}$$
$$注意：上述方程均采用\mathbf{MATLAB}进行推导，正确与否还有待验证！！！$$

四 模型线性化

由三 方程求解 的方程结果可知，倒立摆系统是一个的非线性系统。

为了便于实现后续的控制，可在工作点($\theta =0$)附近，进行线性化,具体过程如下：

当$\theta \approx 0$处，有：
$$\begin{array}{r}\{\begin{array}{rl}\cos \theta & =0\\ \sin \theta & =\theta \\ {\dot{\theta }}^2& =0\end{array}\end{array}$$
带入(10)到三 方程求解 中的非线性模型(9)中，简单整理可得：
$$\begin{array}{r}\{\begin{array}{rl}& \ddot{\theta }=\frac{3(M+m)g}{(4M+m)l}\theta -\frac{3(M+m)b_2}{(4M+m)m{l}^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{array}\end{array}$$
其中$I=\frac{m{l}^2}{3}$

取$X=[\theta \dot{\theta }x\dot{x}{]}^T$为状态变量，倒立摆状态空间模型如下:

$$\begin{array}{r}\{\begin{array}{rl}& \dot{X}=AX+Bu\\ & Y=CX\end{array}\end{array}$$
其中:
$$\begin{gathered} A=\left[\begin{array}{cccc}0& 1& 0& 0\\ \frac{3(M+m)g}{(4M+m)l}& -\frac{3(M+m)b_2}{(4M+m)m{l}^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{array}\right],B=\left[\begin{array}{c}0\\ -\frac{3}{(4M+m)l}\\ 0\\ \frac{4}{4M+m}\end{array}\right] \\ C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right] \end{gathered}$$

五 采用LQR方法进行全状态反馈控制

1. LQR(Linear Quadratic Regulator)方法

针对线性系统，定义状态误差加权和控制输入加权为代价函数$J$：
$$J={\int }_0^{\infty }(E^{T}QE+u^{T}Ru)dt={\int }_0^{\infty }[(X-Ref{)}^{T}Q(X-Ref)+u^{T}Ru]dt$$
其中$Q$，$R$为正定或者半正定矩阵,$Ref$为参考的状态

通过系列方法，最终求解黎卡提$Riccati$方程：
$$A^{T}P+PA-PB{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{array}{rl}M& =1.0kg\\ m& =0.5kg\\ l& =0.5m\\ b_1& =0.3\\ b_2& =0.0\end{array}$$

按照四 模型线性化中的模型得：
$$A=\left[\begin{array}{cccc}0& 1& 0& 0\\ 19.6133& 0& 0& 0.4\\ 0& 0& 0& 1\\ -3.2689& 0& 0& -0.2667\end{array}\right],B=\left[\begin{array}{c}0\\ -1.3333\\ 0\\ 0.8889\end{array}\right],C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$
设置Q，R如下:
$$Q=\left[\begin{array}{cccc}20000& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 10000& 0\\ 0& 0& 0& 1\end{array}\right],R=\left[\begin{array}{c}0.001\end{array}\right]$$
通过MATLAB中lqr函数计算出反馈增益$K$:
$$K=[-7470.90-1559.47-3162.27-2193.39]$$
在simulink中搭建控制仿真模型如下：

输入计算出的反馈增益$K$仿真结果如下：

MATLAB代码和simulink模型点击这里

欢迎大家一起交流！！！

Reference：

1. LQR控制算法推导-连续与离散形式：https://blog.csdn.net/zjh2883/article/details/136167154
2. MATLAB LQR函数：https://ww2.mathworks.cn/help/control/ref/lti.lqr.html