基于MPC的移动机器人轨迹跟踪控制matlab例程

github地址

https://github.com/zzy5510/MPC_control_robot

移动机器人建模

因为所有机器人均可转化为独轮车模型，因此本项目采用独轮车模型。
$\left(\begin{array}{c}\dot{x}_{\mathrm{c}} \\ \dot{y}_{\mathrm{c}} \\ \dot{\theta}_{\mathrm{c}}\end{array}\right)=\left(\begin{array}{cc}\cos \theta_{\mathrm{c}} & 0 \\ \sin \theta_{\mathrm{c}} & 0 \\ 0 & 1\end{array}\right)\left(\begin{array}{c}v_{\mathrm{c}} \\ \omega_{\mathrm{c}}\end{array}\right)$
vc为输入速度，wc为输入角速度。积分后离散，得到离散化的状态方程：
$\left(\begin{array}{c}x_{k+1} \\ y_{k+1} \\ \varphi_{k+1}\end{array}\right)=\left(\begin{array}{c}x_{k}+v_{k} T_{s} \cos \varphi_{k} \\ y_{k}+v_{k} T_{s} \sin \varphi_{k} \\ \varphi_{k}+w_{k} T_{s}\end{array}\right)$
可以看出，该状态方程组是非线性的，角度和位置之间存在耦合。然而基于状态方程的MPC要求方程为线性，因此需要对该方程进行处理。线性化方法参考了文献《基于模型预测控制的自主移动机器人跟踪控制研究_周靖期》。
将方程绕 $\left(x_{\mathrm{r}}, u_{\mathrm{r}}\right)$ 展开，得：
$\dot{x}=f\left(x_{r}, u_{r}\right)+f_{x, r}\left(x-x_{r}\right)+f_{u, r}\left(u-u_{r}\right)$
其中， $x_{r}$ , $y_{r}$ 为待跟踪的轨迹位置。 $f_{x, r}$ , $f_{y,r}$ 为f对x和y的偏导，即：
$\dot{\tilde{x}}=f_{x, \mathrm{r}} \tilde{x}+f_{u, \mathrm{r}} \tilde{u}$
离散化后，得：
$\tilde{x}(k+1)=A(k) \tilde{x}(k)+B(k) \tilde{u}(k)$
$\triangleq\left(\begin{array}{ccc} 1 & 0 & -v_{r}(k) \sin \theta_{r}(k) T \\ 0 & 1 & v_{r}(k) \cos \theta_{r}(k) T \\ 0 & 0 & 1 \end{array}\right) \quad B(k) \triangleq\left(\begin{array}{ccc} \cos \theta_{r}(k) T & 0 \\ \sin \theta_{r}(k) \mathrm{T} & 0 \\ 0 & \mathrm{~T} \end{array}\right)$
离散化的方法参考https://wenku.baidu.com/view/ab37b916fbb069dc5022aaea998fcc22bdd14314.html

预测模型

预测模型的过程如下：
将预测过程中的所有量结合成向量：
$\bar{x}(k+1) \triangleq\left(\begin{array}{c} \tilde{x}(k+1 \mid k) \\ \tilde{x}(k+2 \mid k) \\ \vdots \\ \tilde{x}(k+N \mid k) \end{array}\right) \quad \bar{u}(k) \triangleq\left(\begin{array}{c} \tilde{u}(k \mid k) \\ \tilde{u}(k+1 \mid k) \\ \vdots \\ \tilde{u}(k+N-1 \mid k) \end{array}\right)$
$\bar{x}(k+1)=\bar{A}(k) \tilde{x}(k \mid k)+\bar{B}(k) \bar{u}(k)$
其中，
$\bar{A}(k) \triangleq\left(\begin{array}{c} A(k \mid k) \\ A(k \mid k) A(k+1 \mid k) \\ \vdots \\ \\A(k \mid k) A(k+1 \mid k)...A(k+P-1\mid k) \end{array}\right)$
$\bar{B}(k) \triangleq\left(\begin{array}{cccc} B(k \mid k) & 0 & \cdots & 0 \\ A(k+1 \mid k) B(k \mid k) & B(k+1 \mid k) & \cdots & 0 \\ \vdots & & \ddots & \vdots \\ \\A(k+1 \mid k)...A(k+P-1\mid k) B(k \mid k) & A(k+2 \mid k)...A(k+P-1\mid k) B(k+1 \mid k) & \cdots & B(k+N-1 \mid k) \end{array}\right)$
这里将预测时域和控制时域设为同一个值P。

反馈校正

将模型预测的结果与车辆的实际位置做对比，得到误差值e。该值之后会用在滚动优化上。

滚动优化

待优化的二次型多项式为：
$\phi(k)=\frac{1}{2} \bar{u}^{\mathrm{T}}(k) H(k) \bar{u}(k)+f^{\mathrm{T}}(k) \bar{u}(k)+a(k)$
其中，
$\begin{array}{l} H(k) \triangleq 2\left(\bar{B}^{\mathrm{T}}(k) \bar{Q} \bar{B}(k)+\bar{R}\right) \\ f(k) \triangleq 2 \bar{B}^{\mathrm{T}}(k) \bar{Q} \bar{A}(k) \tilde{x}(k \mid k) \\ a(k) \triangleq \tilde{x}^{\mathrm{T}}(k \mid k) \bar{A}^{\mathrm{T}}(k) \bar{Q} \bar{A}(k) \tilde{x}(k \mid k) \end{array}$

a与待求解的u无关。得到一个标准的QP问题，可以用matlab内置函数直接求解，也可以手动求解。