无人船 | 图解基于MPC控制的路径跟踪算法(以全驱动无人艇WAMV为例)

1 模型预测控制原理

模型预测控制(Model Predictive Control，MPC)是一种先进的控制策略，广泛应用于工程领域。它通过建立动态系统的数学模型，并基于当前状态进行预测，优化未来一段时间内的控制动作，以达到最优的控制效果。模型预测控制通过预测和优化方法，能够在系统动态变化和约束条件下，实现更高效、更灵活的控制，因此在工业应用中具有重要的实际价值。

形式化地，以下图阐述模型预测控制原理。不计较模型形式，以状态空间模型为例

$$\{\begin{array}{l}\mathbf{x}\left(k+1\right)=\mathbf{f}\left(\mathbf{x}\left(k\right),\mathbf{u}\left(k\right)\right),\mathbf{x}\left(0\right)={\mathbf{x}}_0\\ \mathbf{y}\left(k\right)=\mathbf{h}\left(\mathbf{x}\left(k\right),\mathbf{u}\left(k\right)\right)\end{array}$$

其中$\mathbf{x}\left(k\right)\in {\mathbb{R}}^n$、$\mathbf{u}\left(k\right)\in {\mathbb{R}}^l$、$\mathbf{y}\left(k\right)\in {\mathbb{R}}^q$分别表示时刻 系统的状态、控制输入和输出向量。基于预测模型可以计算预测方程，根据预测方程可以求解系统起始于$\mathbf{y}\left(k\right)$的未来一段时间内的输出序列

$$\left\{{\mathbf{y}}_p\left(k+1|k\right),{\mathbf{y}}_p\left(k+2|k\right),\cdots ,\mathbf{y}\left(k+p|k\right)\right\}$$

其中$p$称为预测时域

控制输入序列

U k = d e f { u ( k ∣ k ) , u ( k + 1 ∣ k ) , ⋯ , u ( k + p − 1 ∣ k ) } U_k\xlongequal{\mathrm{def}}\left\{ \boldsymbol{u}\left( k|k \right) , \boldsymbol{u}\left( k+1|k \right) , \cdots , \boldsymbol{u}\left( k+p-1|k \right) \right\} Uk​def {u(k∣k),u(k+1∣k),⋯,u(k+p−1∣k)}

是MPC需要求解的优化问题的独立变量。最常见的，我们希望系统实际输出 跟踪期望的参考输入

{ r ( k + 1 ) , r ( k + 2 ) , ⋯ , r ( k + p ) } \left\{ \boldsymbol{r}\left( k+1 \right) , \boldsymbol{r}\left( k+2 \right) , \cdots , \boldsymbol{r}\left( k+p \right) \right\} {r(k+1),r(k+2),⋯,r(k+p)}

同时满足实际问题的约束条件，例如控制约束和输出约束

$$\{\begin{array}{l}{\mathbf{u}}_{\min }⩽\mathbf{u}\left(k+i\right)⩽{\mathbf{u}}_{\max },i⩾0\\ {\mathbf{y}}_{\min }⩽\mathbf{y}\left(k+i\right)⩽{\mathbf{y}}_{\max },i⩾0\end{array}$$

根据这些指标设计优化目标函数$J=J\left(y\left(k\right),U_k\right)$，其最优解可记为

$$U_k^{\ast }=\arg \underset{U_k}{\min }J\left(y\left(k\right),U_k\right)$$

将$k$时刻优化解$U_k^{\ast }$的第一个分量实际作用于系统，并在$k+1$时刻以新得到的测量值$y(k+1)$为初始条件重新预测系统未来输出并求解优化问题。随着当前时间向前推移，预测时域也向前滚动

2 离散全驱动无人船动力学推导

针对WAMV非线性动力学模型，首先将非线性问题线性化，得到系统线性误差状态方程。具体而言，选择状态量$\mathbf{x}={\left[\begin{array}{cccccc}lx& y& \psi & u& v& r\end{array}\right]}^T$和控制量$\mathbf{u}={\left[\begin{array}{ccc}{\tau }_l& {\tau }_m& {\tau }_r\end{array}\right]}^T$，系统状态方程为$\dot{\mathbf{x}}=\mathbf{f}\left(\mathbf{x},\mathbf{u}\right)$，则令$\mathbf{f}$在参考点${\mathbf{x}}_r$、${\mathbf{u}}_r$泰勒级数展开，忽略非线性项可得

$$\dot{\mathbf{x}}=\mathbf{f}\left({\mathbf{x}}_r,{\mathbf{u}}_r\right)+\frac{\partial \mathbf{f}\left(\mathbf{x},\mathbf{u}\right)}{\partial \mathbf{x}}{\mid }_{{\mathbf{x}}_r,{\mathbf{u}}_r}^{}\left(\mathbf{x}-{\mathbf{x}}_r\right)+\frac{\partial \mathbf{f}\left(\mathbf{x},\mathbf{u}\right)}{\partial \mathbf{u}}{\mid }_{{\mathbf{x}}_r,{\mathbf{u}}_r}^{}\left(\mathbf{u}-{\mathbf{u}}_r\right)$$

其中雅克比矩阵

$$\{\begin{array}{l}\mathbf{A}=\frac{\partial \mathbf{f}\left(\mathbf{x},\mathbf{u}\right)}{\partial \mathbf{x}}{\mid }_{{\mathbf{x}}_r,{\mathbf{u}}_r}^{}=\left[\begin{array}{cccccc}0& 0& -u_r\sin {\psi }_r-v_r\cos {\psi }_r& \cos {\psi }_r& -\sin {\psi }_r& 0\\ 0& 0& u_r\cos {\psi }_r-v_r\sin {\psi }_r& \sin {\psi }_r& \cos {\psi }_r& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& \frac{X_u+2X_{|u|u}|u_r|}{m}& r_r& v_r\\ 0& 0& 0& -r_r& Y_v/m& -u_r\\ 0& 0& 0& 0& 0& N_r/I_z\end{array}\right]\\ \mathbf{B}=\frac{\partial \mathbf{f}\left(\mathbf{x},\mathbf{u}\right)}{\partial \mathbf{u}}{\mid }_{{\mathbf{x}}_r,{\mathbf{u}}_r}^{}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 1/m& 0& 1/m\\ 0& 1/m& 0\\ -D/I_z& 0& D/I_z\end{array}\right]\end{array}$$

选择状态误差量$\tilde{\mathbf{x}}={\left[\begin{array}{cccccc}lx-x_r& y-y_r& \psi -{\psi }_r& u-u_r& v-v_r& r-r_r\end{array}\right]}^T$得到线性误差状态方程$\dot{\tilde{\mathbf{x}}}=\mathbf{A}\tilde{\mathbf{x}}+\mathbf{Bu}+\mathbf{C}$，其中$\mathbf{C}={\mathbf{Bu}}_r$是常数矩阵。为了应用标准LQR过程，暂时忽略常数$\mathbf{C}$，采用采样步长为$T$的前向欧拉法离散化上述状态方程可得

$$\tilde{\mathbf{x}}\left(k+1\right)=\left(T\mathbf{A}+\mathbf{I}\right)\tilde{\mathbf{x}}\left(k\right)+T\mathbf{Bu}\left(k\right)$$

3 全驱动无人艇MPC控制推导

基于模型进行系统未来动态的预测

$$\{\begin{array}{l}\tilde{\mathbf{x}}\left(k+m\right)={\tilde{\mathbf{A}}}^m\tilde{\mathbf{x}}\left(k\right)+{\sum }_{i=0}^{m-1}{\tilde{\mathbf{A}}}^i\tilde{\mathbf{B}}\Delta \mathbf{u}\left(k+m-1-i\right)\\ \tilde{\mathbf{x}}\left(k+p\right)={\tilde{\mathbf{A}}}^p\tilde{\mathbf{x}}\left(k\right)+{\sum }_{i=p-m}^{p-1}{\tilde{\mathbf{A}}}^i\tilde{\mathbf{B}}\Delta \mathbf{u}\left(k+p-1-i\right)\end{array}$$

同理，可迭代计算系统输出

$$\{\begin{array}{l}\tilde{\mathbf{y}}\left(k+m\right)={\tilde{\mathbf{C}}\tilde{\mathbf{A}}}^i\tilde{\mathbf{x}}\left(k\right)+{\sum }_{i=0}^{m-1}{\tilde{\mathbf{C}}\tilde{\mathbf{A}}}^i\tilde{\mathbf{B}}\Delta \mathbf{u}\left(k+m-1-i\right)\\ \tilde{\mathbf{y}}\left(k+p\right)={\tilde{\mathbf{C}}\mathbf{A}}^i\tilde{\mathbf{x}}\left(k\right)+{\sum }_{i=p-m}^{p-1}{\tilde{\mathbf{C}}\tilde{\mathbf{A}}}^i\tilde{\mathbf{B}}\Delta \mathbf{u}\left(k+p-1-i\right)\end{array}$$

定义预测输出向量和控制输入向量为

$${\mathbf{Y}}_p\left(k\right)\stackrel{\mathrm{def}}{=}{\left[\begin{array}{c}\tilde{\mathbf{y}}\left(k+1\right)\\ \tilde{\mathbf{y}}\left(k+2\right)\\ ⋮\\ \tilde{\mathbf{y}}\left(k+p\right)\end{array}\right]}_{p\times 1},\Delta {\mathbf{U}}_m\left(k\right)\stackrel{\mathrm{def}}{=}{\left[\begin{array}{c}\Delta \mathbf{u}\left(k\right)\\ \Delta \mathbf{u}\left(k+1\right)\\ ⋮\\ \Delta \mathbf{u}\left(k+m-1\right)\end{array}\right]}_{m\times 1}$$

则系统输出方程可表达为矩阵形式

$${\mathbf{Y}}_p\left(k\right)=S_x\tilde{\mathbf{x}}\left(k\right)+S_u\Delta {\mathbf{U}}_m\left(k\right)$$

基于预测方程定义开环优化目标函数

$$J={\left({\mathbf{Y}}_p-{\mathbf{Y}}_r\right)}^T\tilde{\mathbf{Q}}\left({\mathbf{Y}}_p-{\mathbf{Y}}_r\right)+\Delta {\mathbf{U}}_m^T\tilde{\mathbf{R}}\Delta {\mathbf{U}}_m$$

对于优化问题$\Delta {\mathbf{U}}_m=\arg {\min }_{\Delta {\mathbf{U}}_m}J\left(\Delta {\mathbf{U}}_m\right)$有

$$\frac{\partial J}{\partial \Delta {\mathbf{U}}_m}=\frac{\partial }{\partial \Delta {\mathbf{U}}_m}\left[\frac{1}{2}\Delta {\mathbf{U}}_m^T\left(S_u^T\tilde{\mathbf{Q}}{S}_u+\tilde{\mathbf{R}}\right)\Delta {\mathbf{U}}_m+{\left(S_x\tilde{\mathbf{x}}-{\mathbf{Y}}_r\right)}^T\tilde{\mathbf{Q}}{S}_u\Delta {\mathbf{U}}_m\right]$$

对应转化为标准的二次规划问题

$$\Delta {\mathbf{U}}_m=\arg \underset{\Delta {\mathbf{U}}_m}{\min }\left(\frac{1}{2}\Delta {{\mathbf{U}}_m}^T\mathbf{H}\Delta {\mathbf{U}}_m+{\mathbf{g}}^T\Delta {\mathbf{U}}_m\right)$$

进行求解

4 跟踪效果分析

定性测试结果：

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇