【MPC】模型预测控制 | 在车辆控制中的应用（一）曲线坐标车辆模型

写在前面：
🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落。📝
个人主页：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。

👍 点赞、评论、收藏，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货，还有思维的火花！

📚 系列专栏：【运动控制】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

一、车辆运动控制架构

1、整体架构

  本文深入研究如何应用 MPC 进行车辆控制，先看如下架构图：  运动规划模块生成参考路径，同时给出速度参考并构建约束，约束中包含了上层对车辆安全性能的考量，包括安全的驾驶范围。接下来把参考路径和约束作为输入给到 MPC 里， MPC 根据输入的约束和参考路径进行优化。
  MPC 可以根据当前反馈的测量值和模型预测在一定时域内的车辆 预测轨迹(predicted trajectory)，同时生成一系列 最优解的控制序列(Optimal control horzion)，把其中第一个控制量发给车量的 底层控制器(Low level control)，最终产生油门、刹车来控制车辆运动。

2、底层控制器的选取

部分情况下底层控制器不是必需的：

• 若 MPC 运算足够快，则不需要使用底层控制器也可以直接产生油门、刹车和方向盘的指令。
• 若 MPC 问题构架复杂，运算量较大，长时间下很难保证系统的实时性，这种情况下需要加入底层控制器避免延时的问题，常用底层控制器：PID，可运行在高频架构下，保证抗干扰性能。

2、控制策略总结

• MPC 的作用是在有限时域内，对车辆轨迹不断迭代优化，并产生相应的轨迹追踪控制信号，每当收到新的反馈，就需要基于MPC进行新的优化，保证优化的实时性；
• 好的模型会让 MPC 生成高质量的预测状态从而保证准确性和有效性；
• 构建的约束会对提高优化解的安全性；
• 目标函数可以更好选取需要优化的量，从而最大限度保证车辆达到预期行驶的方向和轨迹追踪。

---

二、车辆运动控制输出

1、控制量的选取策略

  下面考虑MPC输出给底层控制器的变量选择什么，在不同的应用下有不同的选择：

  比如可以选择加速度和转角速度，那么就是控制加速度和转向角变化率
  一般来讲，在把 MPC 运用到耦合的横向和纵向运动的，必须都存在这两方面的控制量。

2、一阶与二阶微分组合的应用场景分析

控制量的选取有如下组合：

• 速度 $+$ 转向角度
• 加速度 $+$ 转角速度
• 加加速度 $+$ 转角加速度

具体使用一阶微分的组合还是二阶微分的组合，要根据不同的应用场景。

  比如，使用二阶微分的组合（加加速度 $+$ 转角加速度），可以保证速度和加速度具有连续性和平滑性，并且可以有针对性优化加加速度（跃度）和转角加速度。

3、MPC车辆控制关键要素

  总的来说，用 MPC 进行车辆控制，需要注意以下几个方面：

a）模型选择

  选择合理的模型构建系统，预测未来信息，针对不同应用场景选择不同的颗粒度。

b）目标函数选择

  保证轨迹追踪的实时性、车辆控制效率、舒适度。

c）约束选择

  保证安全性和模型准确性（满足模型、驱动力限制）。

d）底层控制器选择

  基于底层控制器加入引擎关系、动力模型、执行器模型、轮胎力模型等。

---

三、经典车辆模型

1、运动学自行车模型 (kinematics bicycle model)

  一个准确的模型可以让我们求解最优控制量时，使预测误差和实际误差比较小。但同时也会导致问题变复杂，所以 MPC 设计很重要的就是选择合适的模型，根据具体问题权衡模型的简化和准确程度。
  运动学自行车模型如下：

  只需要四个参数就可以描述车辆的运动轨迹：$$\{\begin{array}{l}\dot{p_x}=v\cos (\psi )\\ \dot{p_y}=v\sin (\psi )\\ \dot{\psi }=\frac{v}{L}\tan (\delta )\\ \dot{v}=a\end{array}$$

2、动力学自行车模型 (Dynamic bicycle model)：

  需要更多、更复杂的变量来精准描述车辆模型：
$$\begin{array}{rl}\dot{x}& =v_x\cos (\psi )-v_y\sin (\psi )\\ \dot{\phi }& =r\\ {\dot{v}}_x& =\frac{1}{m}(F_{r,x}-F_{f,y}\sin (\delta )+m{v}_{y}r)\\ {\dot{v}}_y& =\frac{1}{m}(F_{r,y}+F_{f,y}\cos (\delta )-m{v}_{x}r)\\ \dot{r}& =\frac{1}{I_z}(F_{f,y}{l}_f\cos (\delta )-F_{r,y}{l}_r)\end{array}$$
包括车辆侧边角等参数。

实际使用要根据不同应用场景选择模型：

• 在低速情况下，使用运动学模型就足够了。
• 在高速情况下，比如赛车场景，需要快速且能过急转弯，就必须考虑轮胎侧滑和摩擦的影响，需要使用动力学模型。

总而言之，要结合具体问题选择适合的模型。

---

四、曲线坐标车辆模型 (Curvilinear Coordinates vehicle model)

  下面来介绍一个能够有效描述车辆与参考轨迹之间关系的车辆模型：曲线坐标车辆模型(Curvilinear Coordinates Vehicle Model)。
  该模型描述车辆相对轨迹的速度与车辆自身速度之间的关系，与轨迹之间的横向误差、轨迹曲率之间的关系。

下面来看一下这个模型的这个状态量和输入量都有哪些？

1、模型状态量

  模型的状态量是由两部分组成：①参考轨迹；②车辆自身。

a）参考轨迹

  参考轨迹通过 s 描述，也就是有走了多远的距离，轨迹的行进速度用 $\dot{s}=\frac{ds}{dt}$ 描述。在任意一点用 $\varphi$ 或 ${\varphi }_s$ 表示当前点的轨迹朝向。把 $\dot{s}$ 的方向平移到车辆当前点的位置，可以得到 $v_s$ 和车辆自身 $v_x$ 之间的夹角，就是车辆当前朝向相对参考轨迹匹配点的朝向 $\mu$。车辆的参考轨迹是绕着转动中心以 ${\dot{\varphi }}_s$ 的速度转动，旋转半径为 $r$，车辆与匹配点之间沿着垂直于轨迹切线方向的距离为 $n$ 。

b）车辆自身

  车辆速度 $v$ 是横向 $v_x$ 和纵向 $v_y$ 的合成速度，加速度 $a$ 是车辆在行进方向的速度变化，转向角为 $\delta$ ，由此构成了 $7$ 个状态变量的模型：
$$x=\left[\begin{array}{c}s\\ n\\ \mu \\ v\\ a\\ \delta \\ \dot{\delta }\end{array}\right]$$其中，$s$ 为车辆沿参考轨迹的行驶距离，n车辆与参考点的横向误差，$\mu$ 为车辆当前的航向，$v$ 为车辆的速度，$a$ 为车辆行驶方向上的加速度，$\delta$ 为转向角，$\dot{\delta }$ 为转向速率。
  其中有些变量是为了描述约束而建立，并不是要把所有变量都给底层控制器。

2、模型输入量

  下面来研究模型的输入量：
$$u=\left[\begin{array}{c}{u}_{jerk}\\ u_{\ddot{\delta }}\end{array}\right]$$
  选用加加速度(jerk) $\ddot{v}$ 和转角加速度 $\ddot{\delta }$ 来作为输入量，这就是 加加速度 $+$ 转角加速度的控制量组合。

为什么要来选择这两个参数作为输入量？

a）积分比微分准确

  如果求出了速度，再把它二次微分求加加速度的话，这个加加速度会包含很多噪声，而其反向操作——积分会准确得多。反之如果拿到了加加速度，可以反向积分求加速度以及速度， 同样对于转向角也一样。

b）输入项和舒适度相关

  可以有针对的去优化 加加速度 和 转角加速度，因为这两项和舒适度息息相关。

---

五、车辆模型推导

下面来介绍整个模型是如何推导的。

1、车辆运动和参考轨迹之间的关系

  曲线坐标车辆模型的核心是建立车辆相对于参考轨迹的关系，可以通过用弧速度 $\dot{s}=\frac{ds}{dt}$ 来描述车辆沿着参考轨迹的运动。

  车辆重心沿着 s 的速度为 $v_s$，方向和 $\dot{s}$ 的切线方向一致，大小等于车辆自身坐标系下纵向速度 $v_x$ 和横向速度 $v_y$在 $v_s$ 方向上的分量之和：
$$v_s=v_{x}cos\mu -v_y\sin \mu =(R-n){\dot{\varphi }}_s$$
  车辆沿参考轨迹的运动速度可以通过参考点旋转半径 $R$ 减去车辆与参考点之间的距离 $n$，再乘以旋转速度 ${\dot{\varphi }}_s$ ， 参考轨迹点的行进速度 $\dot{s}$等于$R$ 乘以旋转速度 ${\dot{\varphi }}_s$ ，联立起来，可以写成如下形式：
$$\begin{array}{c}{v}_s=(R-n){\dot{\varphi }}_s\\ \dot{s}=R{\dot{\varphi }}_s\end{array}\}\Rightarrow \dot{s}=\frac{R}{R-n}(v_{x}cos\mu -v_y\sin \mu )$$这样就可以建立起参考轨迹 $\dot{s}$ 和车辆速度 $v_x、v_y$ 之间的关系。

  一般来说，对于路径或轨迹，用曲率来描述旋转角度的大小。所以用曲率 $\kappa$ 来代替旋转半径的倒数 $\frac{1}{R}$，可以得到用曲率 $\kappa$ 来表示参考轨迹 $\dot{s}$ 和车辆速度 $v_x、v_y$ 之间的关系：
$$\begin{array}{c}\dot{s}=\frac{R}{R-n}\left(v_{x}cos\mu -v_{y}sin\mu \right)\\ \kappa =\frac{1}{R}\end{array}\}\Rightarrow \dot{s}=\frac{v_{x}cos\mu -v_y\sin \mu }{1-n\kappa }$$这样就建立起了车辆运动和参考轨迹之间的关系。

2、与自行车模型的结合

  下面将车辆运动和参考轨迹之间的关系与 自行车模型(Bicycle model) 结合：

  车辆速度v和车辆轴线之间的夹角是侧偏角 $\beta$，可以得到其与 $v_x、v_y$ 关系 ，把这两个式子带进去：
$$\begin{array}{c}\begin{array}{c}\begin{array}{rl}\dot{s}& =\frac{v_{x}cos\mu -v_{y}sin\mu }{1-n\kappa }\end{array}\\ v_x=vcos\beta \end{array}\\ v_y=vsin\beta \end{array}\}\Rightarrow \dot{s}=\frac{vcos\beta cos\mu -vsin\beta sin\mu }{1-n\kappa }=\frac{vcos\left(\beta +\mu \right)}{1-n\kappa }$$其中，$\beta +\mu$ 就是车辆速度 $v$ 和轨迹参考点速度 $\dot{s}$ 之间的夹角。

  $\dot{n}$ 就是 $v_x、v_y$ 沿着横向的分量，可以写成如下形式：
$$\dot{n}=v_{x}sin\mu +v_{y}cos\mu =vcos\beta sin\mu +vsin\beta cos\mu =vsin(\beta +\mu )$$车头朝向的旋转速度 $\mu$ 就是车本身的旋转速度 $r$ 和参考系旋转速度 ${\dot{\varphi }}_s$ 之差。用 r 来代表车自身的旋转速度，可以写作 $r=\frac{v}{R_v}$， $R_v$ 是车辆的旋转半径，由于$\dot{s}=R{\varphi }_s$，把 $R$ 换成 $\kappa$，那么就是等于 $\frac{{\dot{\varphi }}_s}{\kappa }$，这样就可以写成 ${\dot{\varphi }}_s=\kappa \dot{s}$，那么可以推导出如下关系：
$$\dot{\mu }=r-{\dot{\varphi }}_s=\frac{v}{R_v}-\kappa \frac{vcos(\beta +\mu )}{1-n\kappa }=\frac{vsin\beta }{l_r}-\kappa \frac{vcos(\beta +\mu )}{1-n\kappa }$$把刚才的推导整理一下，可以得到以下车辆模型：
$$\dot{x}=\left[\begin{array}{c}\dot{s}\\ \dot{n}\\ \dot{\mu }\\ \dot{v}\\ \dot{a}\\ \dot{\delta }\\ \ddot{\delta }\end{array}\right]=\left[\begin{array}{c}\frac{vcos(\mu +\beta )}{1-n\kappa }\\ vsin(\mu +\beta )\\ \frac{v}{l_r}\sin (\beta )-\kappa \frac{vcos(\mu +\beta )}{1-n\kappa }\\ a\\ u_{jerk}\\ \dot{\delta }\\ u_{\ddot{\delta }}\end{array}\right]$$

---

六、模型特点

1、非线性模型

  可以看到曲线坐标车辆模型有7个状态变量，有很多 $cos、sin$ 在里面，所以是非线性模型。

2、横纵向耦合

  其最大特点是把纵向和横向运动耦合起来，也就是在 $\dot{s}$ 里和 $n$ 建立关系。

3、描述车辆运动与参考轨迹之间的关系

  该模型给很好地描述了车辆运动和参考轨迹之间的关系。

---

七、车辆侧偏角的计算

  下面看一下车辆模型的变量，$v$ 直接可以测得，当前航向 $\mu$ 可以通过两个 朝向相减得到，$n$ 可以通过相对距离计算。 $\kappa$ 是个轨迹里自带的属性，其中唯一需要计算的就是 $\beta$，根据自行车模型的几何关系得到：

  根据互余可知 $\angle AOG=\beta$，所以 $\sin \beta =\frac{l_r}{R}$，可以得到
$$R=\frac{l_r}{\sin \beta }$$同时还知道$\angle ABO=\frac{\pi }{2}-{\delta }_f$，根据三角关系可得 $\angle GOB={\delta }_f-\beta$。在 $△OGB$ 中应用正弦定理：
$$\frac{l_f}{\sin ({\delta }_f-\beta )}=\frac{R}{\sin (\frac{\pi }{2}-{\delta }_f)}$$把这个式子展开，分母同除以$\cos {\delta }_f\cos \beta$，得到以下关系:
$$\begin{gathered} \frac{l_f}{\sin {\delta }_{f}cos\beta -cos{\delta }_{f}sin\beta }=\frac{l_r}{cos{\delta }_{f}sin\beta } \\ \Rightarrow \frac{l_f}{\tan {\delta }_f-tan\beta }=\frac{l_r}{tan\beta } \end{gathered}$$可计算出
$$\tan \beta =\frac{l_r}{l_f+l_r}\tan {\delta }_f$$因此可得 $\beta$：
$$\beta =\arctan (\frac{l_r}{l_r+l_f}\tan {\delta }_f)$$其他量都可以实时计算出来，那么就可以建立起曲线坐标车辆模型。

---

八、MPC的模型选取

  用什么模型构建MPC，要根据需求或具体情况来进行分析：

1、要求精确度

• 非线性(Nonlinearity)
• 动力学模型(Dynamics)
• 横纵向耦合(Coupled)

2、要求快速性

• 线性(Linear)
  使用二次规划QP求解。
• 运动学模型(Kinematics)
  适用于低速情况，忽略轮胎特性和轮胎刚度，牺牲部分准确度。
• 解耦合(Decoupled)
  降低模型复杂度，求解起来更快速。但这种方法存在缺点：缺少横纵向之间的联系，需要在后续增加把横纵向联合起来的过程，或者是在前期的运动规划模块中解决。

  综合来看，模型选取需要取舍，并不是越准确或越快越好，要有效选择适合的模型。

---

后记：
🌟 感谢您耐心阅读这篇关于 曲线坐标车辆模型 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀