【MPC练习】差速底盘使用MPC跟踪路径1/2——模型建立

---

前言

本文记录了自己用于学习MPC控制移动底盘的代码，目的在于学习移动底盘运动模型的建立和使用CppAD库求解控制量以使得底盘可以跟踪预先给定的路径。

为了简单起见，选取了最简单的差速底盘作为对象，而不是阿克曼底盘。

实现效果：

mpc-1

整体代码运行在ROS中

---

零、涉及到的坐标系

在路径跟踪中主要涉及到3个坐标系：

• 全局坐标系 World
• 机器人底盘坐标系 Robot
• 参考轨迹中的frenet坐标 Frenet

一、控制量

差速底盘的控制量非常简单：
$$\mathbf{U}=[v_x,\omega ]$$ 其中$v_x$是在车坐标系下沿x轴方向的速度，$\omega$为旋转角速度。

二、状态变量

为了跟踪路径，设置状态变量如下：
$$\mathbf{X}=[x,y,\delta ,cte,pesi]$$这里面最主要的问题是确定状态变量是处于哪个坐标系

为了更好表示车辆和轨迹之间的误差，选择将状态在frenet坐标系下表示。

补充有很多路径跟踪MPC的实现选用了在机器人底盘坐标系下进行实现，可以理解为“线找车”，将底盘坐标系作为参考，这样就需要重新定义误差（其实就是取相反值），两种状态变量所在坐标系的选择方式在工程中的表现还有待测试。

$x$: frenet坐标系下x坐标

$y$: frenet坐标系下y坐标

$\delta$: frenet坐标系下 车辆的航向角

$cte$: frenet坐标系下 车辆对于轨迹的横向偏离误差

$pesi$: frenet坐标系下航向角的偏离误差

这里我在一开始有一个绕进去的点，从图中看，$cte$的值就是$y$的值，$pesi$的值就是$\delta$的值。所以似乎不需要这两个值，但是实际上在这一时刻进行预测控制的时候，会生成多个预测的状态，而这些预测状态中的$x$,$y$,$\delta$都是在这一时刻的frenet坐标系下，如果以目标是使$y$ ,$\delta$为0，则会导致预测以这一时刻的$X_F$轴为参考曲线。

与$x$,$y$,$\delta$更新方式不同，在进行这一时刻的$cte$和$pesi$的值的预测的时候，会把轨迹的变化情况作为参考值进行更新加入更新的方程中。其中轨迹的变化情况可以使用3次或者5次多项式曲线的方式对离散的坐标点进行描述。

$cte$和$pesi$的表示与参考轨迹的变化有关(误差 = 测量值 - 参考值)：
$$\begin{gathered} ct{e}_k=y_k-y_{ref} \\ eps{i}_k={\delta }_k-{\delta }_{ref} \end{gathered}$$当使用多项式曲线对轨迹进行拟合参数化之后，参考轨迹在$x_k$处的横向距离$y_{ref}$的值为$f(x_k)$

而在$x_k$处的航向角参考值为$arctan(f^{\prime }(x_k))$，换言之，曲线上各处的参考值的变化是由$x$的函数。

三、微分约束

$$\begin{gathered} \dot{x}=v_{x}cos\delta \\ \dot{y}=v_{x}sin\delta \\ \dot{\delta }=\omega \end{gathered}$$控制量$v_x$在frenet坐标系下对机器人在x轴，y轴变化的贡献量，以及角速度$\omega$对于$\delta$贡献量显而易见。

但是控制是离散的，所以需要将运动模型的更新和误差的更新离散化（省略速度控制量$v_x$的下标x）：
$$\begin{gathered} x_{k+1}-x_k=v_k\cdot cos\delta \cdot dt \\ {y}_{k+1}-y_k=v_k\cdot sin\delta \cdot dt \\ {\delta }_{k+1}-{\delta }_k={\omega }_k\cdot dt \\ ct{e}_{k+1}-ct{e}_k=v_k\cdot sin\delta \cdot dt \\ eps{i}_{k+1}-eps{i}_k={\omega }_k\cdot dt \end{gathered}$$离散化后的模型建立了时间上相邻的两个状态之间的关系。实际底盘的移动需要满足这一组模型。

对于误差的更新，不妨将参考值的表示加入进来：
$$\begin{gathered} ct{e}_{k+1}=y_k-y_{ref}+v_k\cdot sin\delta \cdot dt \\ eps{i}_{k+1}={\delta }_k-{\delta }_{ref}+{\omega }_k\cdot dt \end{gathered}$$我对于这两个式子的理解是，在$k+1$时刻的误差，是上一时刻$k$的误差加上两个时刻之间相应状态或者控制量的对误差的影响。

上边建立的微分关系将会作为优化问题的等式约束加入进去。

四、目标函数

• 控制量尽可能小
  $$J_1=\sum _k^{N_p-1}(w_v\cdot (v_k-v_{ref}{)}^2+w_{\omega }\cdot ({\omega }_k{)}^2)$$
• 跟踪误差尽可能小
  $$J_2=\sum _k^{N_p}(w_{cte}\cdot (ct{e}_k{)}^2+w_{epsi}\cdot (eps{i}_k{)}^2)$$其中$N_p$是预测的步长，控制量减一是因为最后一个时间不长可以不预测。

最基础可以使用$J_1+J_2$作为最小化的目标函数，如果想要更平滑一些，也可以限制相邻控制量之间的变化等等。

参考

参考文章

参考代码