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

基于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}\\ {\phi }_{k+1}\end{array}\right)=\left(\begin{array}{c}{x}_k+v_k{T}_s\cos {\phi }_k\\ y_k+v_k{T}_s\sin {\phi }_k\\ {\phi }_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)$$
$$A(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)B(k)\triangleq \left(\begin{array}{cc}\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)\\ ⋮\\ \tilde{x}(k+N\mid k)\end{array}\right)\bar{u}(k)\triangleq \left(\begin{array}{c}\tilde{u}(k\mid k)\\ \tilde{u}(k+1\mid k)\\ ⋮\\ \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)\\ ⋮\\ \\ 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\\ ⋮& & \ddots & ⋮\\ & & & \\ 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。该值之后会用在滚动优化上。

滚动优化

待优化的二次型多项式为：
$$\varphi (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内置函数直接求解，也可以手动求解。