平衡小车的LQR控制器与Rhomberg观测器

动力学分析

动力学分析.jpg

控制器设计

状态空间方程

令$\xi = [\theta,\dot{\theta},x,\dot{x}]^T$；且令$\theta\to 0; \dot{\theta} \to 0$，相当于在0处进行泰勒展开

化简得到

$\dot{\xi} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ \frac{M+m}{Ml}g & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ -\frac{mg}{M} & 0 & 0 & 0 \end{bmatrix} \xi + \begin{bmatrix} 0 \\ -\frac{1}{Ml} \\ 0 \\ \frac{1}{M} \end{bmatrix} u$

取输出水平力 $u = -K_1X_1-K_2X_2-K_3X_3-K_4X_4$

Simulink仿真

取m=1kg, M=1kg, g=10m/s^2, l=1m

$Q=\begin{bmatrix} 10&0&0&0 \\ 0&1&0&0 \\ 0&0&1&0 \\ 0&0&0&1 \end{bmatrix} ,\quad R=0.1$

取代价函数最小值，得到

$K = \begin{bmatrix} -70.7312 -18.9427 -3.1623 -6.4873 \end{bmatrix}$

使用Simulink进行仿真，设置初始Pithch角为5°

$\dot{x_1} = x_2 \\ \dot{x_2} = 20x_1 - u \\ \dot{x_3} = x_4 \\ \dot{x_4} = -10x_1 + u \\ u = -k_1x_1-k_2x_2-k_3x_3-k_4x_4$

simulink-只有控制器.png

运行得到结果

曲线-controller only.png

曲线.png

观测器设计

观测器设计的初步学习可以参考下面这篇笔记

不考虑机械中值

观测器方程

方便起见，状态变量暂时只取 $X_1,X_2$，即为 $\theta$ 和 $\dot{\theta}$ ；

$\begin{bmatrix}\dot{X_1}\\\dot{X_2}\end{bmatrix} = \begin{bmatrix}0 & 1 \\ \frac{M+m}{Ml}g & 0\end{bmatrix} \begin{bmatrix}X_1 \\ X_2\end{bmatrix} + \begin{bmatrix}0 \\ -\frac{1}{Ml}\end{bmatrix} u$

取输出$y=\begin{bmatrix}1&0\end{bmatrix}\begin{bmatrix}X_1\\X_2\end{bmatrix}$, 则$C=\begin{bmatrix}1&0\end{bmatrix}$

可观测性检测

$O=\begin{bmatrix}C\\CA\end{bmatrix}=\begin{bmatrix}1&0 \\ 0&1\end{bmatrix}$

满秩，可观测

观测方程

$\begin{bmatrix}\dot{\hat{X_1}} \\ \hat{\dot{X_2}} \end{bmatrix} = \begin{bmatrix}0 & 1 \\ \frac{M+m}{Ml}g & 0\end{bmatrix} \begin{bmatrix}\hat{X_1} \\ \hat{X_2}\end{bmatrix} + \begin{bmatrix}0 \\ -\frac{1}{Ml}\end{bmatrix} u + L (y - \hat{y})$

由观测器和控制器分离原理

令A-LC和A-BK的特征值实部分别小于0计算出L矩阵和K矩阵即可

Simulink仿真

直接令$(\lambda+1)^2=0$得到L矩阵和K矩阵

observer-without alpha.png

左半部分为观测器系统，右半部分为理想情况下的现实物理系统。观测器能获得的数据只有角度值θ即$X_1$，由此估计出 $\hat{X_1},\hat{X_2}$，并依据这两个估计值给出控制量$u$，输入到物理系统中。

将物理系统中的角度初始值设为5°，将观测器中的角度初始值设置为0°，仿真运行结果如下。

曲线3.png

曲线4.png

最后观测器收敛至真实值，但是由于初始信息差的存在，导致控制效果受到了一定的影响。

机械中值的收敛

实际上的机械中值是没法精确测得的，并且也有可能随时间变化，所以需要设计观测器使系统可以得到真实的机械中值。

不寻找机械中值的效果

不寻找alpha.png

曲线5.png

在之前的观测器系统中，直接添加 $\alpha$ 后控制系统失控，而且观测器也不再收敛。

如果是之前的没有观测器的模型的话，大致是这样的一个过程。平衡点在3°，当车子为0°时车子会向后倾，角度值减小；此时为了使车身回到0°，轮子也向后移动，最后角度不变，趋于匀加速移动。

观测器设计

将真实的机械中值设为$\alpha$，则$\theta$应当变为$\theta - \alpha$，得到新的状态空间方程

$\begin{bmatrix}\dot{X_1}\\\dot{X_2}\end{bmatrix} = \begin{bmatrix}0 & 1 \\ \frac{M+m}{Ml}g & 0\end{bmatrix} \begin{bmatrix}X_1 \\ X_2\end{bmatrix} + \begin{bmatrix}0 \\ -\frac{1}{Ml}\end{bmatrix} u + \begin{bmatrix}0 \\ -\frac{M+m}{Ml}g\end{bmatrix}\alpha$

状态空间取$\hat{X} = \begin{bmatrix}\hat{\theta} & \dot{\hat{\theta}} & \hat{\alpha}\end{bmatrix}^T$，输出 $y = \begin{bmatrix}1&0&0\end{bmatrix}\begin{bmatrix}\theta \\ \dot{\theta} \\ \alpha \end{bmatrix}$

可观测性检测得可观测

观测器方程

$\begin{bmatrix}\dot{\hat{X_1}} \\ \dot{\hat{X_2}} \\ \dot{\hat{X_3}}\end{bmatrix} = \begin{bmatrix}0 & 1 & 0 \\ \frac{M+m}{Ml}g & 0 & -\frac{M+m}{Ml}g \\ 0 & 0 & 0\end{bmatrix} \begin{bmatrix}\hat{X_1} \\ \hat{X_2} \\ \hat{X_3}\end{bmatrix} + \begin{bmatrix}0 \\ -\frac{1}{Ml} \\ 0\end{bmatrix} u + \begin{bmatrix}l_1 \\ l_2 \\ l_3\end{bmatrix} (y - \hat{y})$

同样可令A-LC和A-BK的特征值实部分别小于0计算出L矩阵和K矩阵即

输出

$u = -K_1(X_1-X_3)-K_2X_2$

Simulink仿真

simulink observer.png

曲线6.png

曲线7.png

曲线8.png

在观测器添加了 $\alpha$ 后，估计值 $\hat{\alpha}$ 确实可以收敛到 $\alpha$。但是在$\alpha$的估计值与真实值相差较大时，X1与X2的观测效果都不太好，导致控制效果也不太好。

调整k

上面的控制中，为了加快收敛，我直接将输出u*5后给到系统中。u的峰值达到了580左右。

但是这样做显然是不合理的，这直接改变了系统的状态方程，虽然确实有一些用。可是更改了Q,R矩阵后依然没有太大改善。

调整L矩阵

DR_CAN在课程中提到过，我们希望观测器的收敛速度大于控制器，所以尝试使A-LC矩阵的特征值更小一些。令$\lambda_1=\lambda_2=\lambda_3=-2$，得到新的$L=[6,32,-0.4]^T$

曲线9.png

曲线11.png

曲线10.png

继续令$\lambda_1=\lambda_2=\lambda_3=-3$，得到的效果似乎更好，角度偏差峰值只有4°多一点

那$\lambda$是不是越小越好呢？令$\lambda=-100$后仿真出来的结果也不错，也没有很夸张。不过我也不太清楚，仿真并不能完全的模拟实际情况，也不能太相信仿真结果。

添加初始误差

令$X_1=5°,\hat{X_1}=0°$，得到的效果也不错。

曲线12.png

曲线13.png

STM32控制实现

极性约定

根据上面的模型，小车朝向右方：

向右倾斜时倾斜角θ为正；
使小车向右运动，即轮子顺时针转为正

参数确定

m=0.55kg
M=0.4kg
l=0.05m

得到

$A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 465.5 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ -13.475 & 0 & 0 & 0 \\ \end{bmatrix} , \quad B = \begin{bmatrix} 0 \\ -50 \\ 0 \\ 2.5 \end{bmatrix}$

仍然取

$Q=\begin{bmatrix} 10&0&0&0 \\ 0&1&0&0 \\ 0&0&1&0 \\ 0&0&0&1 \end{bmatrix} ,\quad R=0.1$

使用MATLAB计算