勘误 Update 02/23/2021

之前的文章中有不严谨的地方,这里做一个勘误.错误就在下面描述坐标系的图中.<更正后的图已覆盖到坐标系小节下>
在这个图中,我指出ENU坐标下,车自身的朝向角 $\psi$ 近似等于理想路径上匹配点(最近的一个点)的朝向角 ${\psi }_{match}$. 其实这是不成立的.考虑以下这个场景,理想轨迹平行于ENU坐标x轴,即朝东.此时车身朝向东偏北,显然车身朝向角$\psi$ 与理想路径上匹配点(距车辆最近的一个点)的朝向角 ${\psi }_{match}$不相同,详见下图.

那么我一开始是怎么得出错误的结论呢?这就要回到百度apollo的MPC相关代码中. 其中计算横向误差的代码为

  const double dx = x - matched_point.path_point().x();
  const double dy = y - matched_point.path_point().y();

  const double cos_matched_theta = std::cos(matched_point.path_point().theta());
  const double sin_matched_theta = std::sin(matched_point.path_point().theta());
  // d_error = cos_matched_theta * dy - sin_matched_theta * dx;
  debug->set_lateral_error(cos_matched_theta * dy - sin_matched_theta * dx);

可以看到,计算横向误差 $e_1$ 时, Apollo 的开发者使用的是理想轨迹上匹配点的朝向角 ${\psi }_{match}$,即代码中的matched_point.path_point().theta()
这个操作,就是把$ENU$世界坐标系下的位置误差dx,dy,转到了$Frenet$坐标系下表述.需要注意的是,所有的受力分析均是在$FLU$车身坐标系下完成的,也就是说所有的状态量,包括横向误差$e_1$也应是在$FLU$坐标系下表述.所以,计算横向误差 $e_1$时,应该采用车身的朝向角$\psi$.

代入到我刚才举得例子中,如果采用理想轨迹上匹配点的朝向角计算,会得到
$$\begin{array}{rl}已知:& \\ & dx=0,dy=l_1\\ & {\psi }_{match}=0,\psi \ne 0\\ 因此:& \\ & e_1=cos{\psi }_{match}dy-sin{\psi }_{match}dx=dy=l_1\end{array}$$
若采用车身的朝向角计算,会得到
$$\begin{array}{rl}& e_1=cos\psi dy-sin\psi dx=l_2\end{array}$$
显然,$FLU$坐标系下的横向误差,即$\beta$方向的横向误差应是$l_2$.

为了比较两种计算方法对MPC控制性能的影响,我跑了几次仿真,结果影响并不是很大.但是我始终觉得这样做不严谨,我并不清楚 Apollo 开发者这么写的理由是什么?不知道是否有工程上的考量?如果有大神了解或者有任何想法,可以在评论区一起讨论,非常感谢.

Hi All

新年挖新坑,今日开启船新连载.内容是无人车的横向控制,整体涵盖从0-1为车辆横向控制设计MPC控制器设计与MPC+MRAC耦合控制.大家有问题,有兴趣可以在评论区多多交流.原图如下

车辆横向动力学模型

引言

首先我们要问:针对车辆横向控制的问题,我们为什么需要建立动力学模型?
简单来说,当车辆在较高速度下行驶时,运动学模型(自行车模型)中提出"汽车轮胎速度方向与车辆朝向相同"的假设不再成立.车辆受到的横向力将不可忽视,如向心力将随着速度的增大而平方倍地增大.因此引入动力学模型,旨在建立更高阶量之间的联系,以更好地描述车辆转弯的非线性特性.

那么,让我们开始吧.为了在不失一般性的前提下尽可能简化模型,动力学模型将建立在以下几个假设上
$$\begin{array}{rl}& \text{1.轮胎速度方向与车辆纵向方向(x)的夹角θv(后统称轮胎速度方向角)较小且满足小角度假设:}\\ & a){\theta }_v\approx tan{\theta }_v\\ & \text{2.轮胎转角(δ)与轮胎速度方向角(θv)的夹角α(后统称轮胎侧滑角)较小}\\ & \text{3.车辆纵向速度维持不变:}\\ & a)V_x:=Constant\\ & \text{4.忽略路堤角(ϕ)对横向控制的影响}\end{array}$$

坐标系

本模型在$FLU\text{(Front-Left-Universe)}$ 惯性坐标系下建立.坐标系原点固定在车辆质心位置,$x$轴方向为车辆纵向方向,指向车头前方.$y$轴方向与x轴垂直且指向车辆左侧,$z$轴方向垂直于$x$,$y$轴且指向天空.值得注意的是,全局(地图)坐标系为ENU(East-North-Universe),其xyz指向规则于FLU相似,分别指向东北天.还有一个局部坐标系为Frenet坐标系,其固定在理想轨迹上,这里就不展开讲了,详见下图.

受力分析

车辆受力分析图如下

根据牛顿第二定律,对车辆y方向(横向)进行受力分析
$$F_{yf}+F_{yr}=m{a}_y$$
其中 $F_{yf}$ 与 $F_{yr}$ 分别是车辆前轮和后轮在$y$方向受到的力,$m$为车辆质量,$a_y$为车辆在$y$方向上的加速度.

车辆在$y$方向上的加速度由两部分构成:
1.因车辆在$y$方向上运动产生的加速度,定义为 $\ddot{y}$.
2.车辆的向心加速度,记为 $a_{yc}$.
$$a_y=\ddot{y}+a_{yc}=\ddot{y}+{\omega }^{2}R=\ddot{y}+{\dot{\psi }}^{2}R=\ddot{y}+V_x\dot{\psi }$$

因此
$$F_{yf}+Fyr=m(\ddot{y}+V_x\dot{\psi })$$

对z轴进行偏航动力学分析,由力矩平衡可得
$$I_z\ddot{\psi }=l_f{F}_{yf}-l_r{F}_{yr}$$
其中$l_f$ 与 $l_r$ 分别是车辆前轮和后轮距离车辆重心的距离.

下一步,我们要对横向力 $F_{yf}$ 与 $F_{yr}$ 进行分析.实验表明,当轮胎侧滑角 $\alpha$ 较小时,轮胎受到的横向力的大小与轮胎侧滑角成正比.其中轮胎侧滑角被定义为轮胎转角与轮胎速度方向角的夹角.

因此,前轮(方向轮)侧滑角 ${\alpha }_f$ 为
$${\alpha }_f=\delta -{\theta }_{vf}$$
其中 $\delta$为前轮转角,${\theta }_{vf}$ 为前轮速度方向角.

同理,后轮(假定后轮无法转向)侧滑角为
$${\alpha }_r=0-{\theta }_{vr}=-{\theta }_{vr}$$
其中 ${\theta }_{vr}$ 为后轮速度方向角.

基于上述两点推断, 轮胎横向力可被改写为以下形式
$$\begin{array}{rl}& F_{yf}=2C_{{\alpha }_f}(\delta -{\theta }_{vf})\\ & F_{yr}=-2C_{{\alpha }_r}{\theta }_{vr}\end{array}$$
其中 $C_{{\alpha }_f}$ 与 $C_{{\alpha }_r}$ 分别为前轮与后轮的侧滑刚度系数.

由小角度假设与牵连运动公式可得
$$\begin{array}{rl}& {\theta }_{vf}\approx tan({\theta }_{vf})=\frac{\dot{y}+l_f\dot{\psi }}{V_x}\\ & {\theta }_{vr}\approx tan({\theta }_{vr})=\frac{\dot{y}-l_r\dot{\psi }}{V_x}\end{array}$$

综上可得