模型预测控制器(MPC)系列: 1.建立车辆横向动力学模型

勘误 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{aligned} 已知:&\\ &dx=0,dy=l_1\\ &\psi_{match}=0,\psi \neq 0 \\ 因此:&\\ &e_1 = cos\psi_{match}dy - sin\psi_{match}dx = dy = l_1 \end{aligned}
若采用车身的朝向角计算,会得到
\begin{aligned} &e_1 = cos\psi dy - sin\psi dx = l_2 \end{aligned}
显然,FLU坐标系下的横向误差,即\beta方向的横向误差应是l_2.

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

Hi All

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

车辆横向动力学模型

引言

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

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

坐标系

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

受力分析

车辆受力分析图如下

根据牛顿第二定律,对车辆y方向(横向)进行受力分析
F_{yf} + F_{yr} = ma_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^2R = \ddot{y}+\dot{\psi}^2R = \ddot{y}+V_x\dot{\psi}

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

对z轴进行偏航动力学分析,由力矩平衡可得
I_z\ddot{\psi}=l_fF_{yf} - l_rF_{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{aligned} &F_{yf} = 2C_{\alpha_f}(\delta - \theta_{vf}) \\ &F_{yr} = -2C_{\alpha_r}\theta_{vr} \end{aligned}
其中 C_{\alpha_f} 与 C_{\alpha_r} 分别为前轮与后轮的侧滑刚度系数.

由小角度假设与牵连运动公式可得
\begin{aligned} &\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{aligned}

综上可得

\begin{aligned} &\ddot{y} = \frac{2}{m}[C_{\alpha_f}\delta - \frac{C_{\alpha_f}+C_{\alpha_r}}{V_x}\dot{y}+ \frac{-C_{\alpha_f}l_f+C_{\alpha_r}l_r}{V_x}\dot{\psi}] -V_x\dot{\psi} \\ &\ddot{\psi}=\frac{2}{I_z}(C_{\alpha_f}l_f\delta-\frac{C_{\alpha_f}l_f- C_{\alpha_r}l_r}{V_x}\dot{y}-\frac{C_{\alpha_f}l_f^2+C_{\alpha_r}l_r^2}{V_x}\dot{\psi}) \end{aligned}

将上式改写为矩阵形式

\begin{bmatrix} \dot{y} \\ \ddot{y} \\ \dot{\psi} \\ \ddot{\psi} \\ \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{2(C_{\alpha_f}+C_{\alpha_r})}{mV_x} & 0 & \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}-V_x \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{2(C_{\alpha_f}l_f- C_{\alpha_r}l_r)}{I_zV_x} & 0 & -\frac{2(C_{\alpha_f}l_f^2+C_{\alpha_r}l_r^2)}{I_zV_x} \\ \end{bmatrix} \begin{bmatrix} y \\ \dot{y} \\ \psi \\ \dot{\psi} \\ \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{2C_{\alpha_f}}{m} \\ 0 \\ \frac{2C_{\alpha_f}l_f}{I_z} \\ \end{bmatrix} \delta

动力学误差模型

将动力学模型中的状态变量设置为误差量,将有利于简化后续控制器的设计.因此,定义误差状态变量
\begin{aligned} &e_1:= y-y_{des} \quad\text{横向误差} \\ &e_2:= \psi-\psi_{des} \quad\text{前轮航向角误差} \\ \end{aligned}

误差动力学分析

理想航向角速率
\dot{\psi}_{des} = \frac{V_x}{R}
理想横向加速度为
a_{y_{des}} = \ddot{y}_{des}+a_{yc_{des}} = 0+\frac{V^2_x}{R} = V_x\dot{\psi}_{des}
因此,横向误差e_1可改写为
\ddot{e}_1 = a_y - a_{y_{des}} = \ddot{y}+V_x\dot{\psi}-V_x\dot{\psi}_{des} = \ddot{y}+V_x(\dot{\psi}-\dot{\psi}_{des}) = \ddot{y}+V_x\dot{e}_2
前面提到V_x为常数,因此对上式积分可得
\dot{e}_1 = \dot{y}+V_x(\psi-\psi_{des}) = \dot{y}+V_xe_2

由此可得
\begin{aligned} & \dot{y} = \dot{e}_1 - V_xe_2\\ & \ddot{y} = \ddot{e}_1 - V_x\dot{e}_2 \\ & \dot{\psi}= \dot{\psi}_{des}+\dot{e}_2 \\ \end{aligned}

将上述式子代入到动力学模型中,替换掉原先的变量可得

\begin{aligned} &(\ddot{e}_1 - V_x\dot{e}_2) = \frac{2}{m}[C_{\alpha_f}\delta - \frac{C_{\alpha_f}+C_{\alpha_r}}{V_x}(\dot{e}_1 - V_xe_2)+ \frac{-C_{\alpha_f}l_f+C_{\alpha_r}l_r}{V_x}(\dot{\psi}_{des}+\dot{e}_2)] -V_x(\dot{\psi}_{des}+\dot{e}_2) \\ &\ddot{\psi}=\frac{2}{I_z}(C_{\alpha_f}l_f\delta-\frac{C_{\alpha_f}l_f- C_{\alpha_r}l_r}{V_x}(\dot{e}_1 - V_xe_2)-\frac{C_{\alpha_f}l_f^2+C_{\alpha_r}l_r^2}{V_x}(\dot{\psi}_{des}+\dot{e}_2)) \end{aligned}

整理可得

\begin{aligned} &\ddot{e}_1 = \frac{2C_{\alpha_f}}{m}\delta+\frac{-2(C_{\alpha_f}+C_{\alpha_r})}{mV_x}\dot{e}_1 +\frac{2(C_{\alpha_f}+C_{\alpha_r})}{m}e_2 + \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}\dot{e}_2 +(\frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}-V_x)\dot{\psi}_{des} \\ &\ddot{\psi} = \frac{2C_{\alpha_f}l_f}{I_z}\delta+\frac{-2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_zV_x}\dot{e}_1 +\frac{2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_z}e_2 +\frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x}\dot{e}_2 +\frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x}\dot{\psi}_{des} \end{aligned}

改写为矩阵形式

\begin{bmatrix} \dot{e}_1 \\ \ddot{e}_1 \\ \dot{e}_2 \\ \ddot{e}_2 \\ \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & \frac{-2(C_{\alpha_f}+C_{\alpha_r})}{mV_x} & \frac{2(C_{\alpha_f}+C_{\alpha_r})}{m} & \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}\\ 0 & 0 & 0 & 1 \\ 0 & \frac{-2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_zV_x} & \frac{2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_z} & \frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x} \\ \end{bmatrix} \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \\ \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{2C_{\alpha_f}}{m} \\ 0 \\ \frac{2C_{\alpha_f}l_f}{I_z} \\ \end{bmatrix} \delta + \begin{bmatrix} 0 \\ \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}-V_x \\ 0 \\ \frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x} \\ \end{bmatrix} \dot{\psi}_{des}

定义

\begin{aligned} &x = \begin{bmatrix} e_1 \\ \dot{e}_1 \\ e_2 \\ \dot{e}_2 \\ \end{bmatrix} \quad A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & \frac{-2(C_{\alpha_f}+C_{\alpha_r})}{mV_x} & \frac{2(C_{\alpha_f}+C_{\alpha_r})}{m} & \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}\\ 0 & 0 & 0 & 1 \\ 0 & \frac{-2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_zV_x} & \frac{2(C_{\alpha_f}l_f-C_{\alpha_r}l_r)}{I_z} & \frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x} \\ \end{bmatrix} \\ &B = \begin{bmatrix} 0 \\ \frac{2C_{\alpha_f}}{m} \\ 0 \\ \frac{2C_{\alpha_f}l_f}{I_z} \\ \end{bmatrix} \quad B_c = \begin{bmatrix} 0 \\ \frac{2(-C_{\alpha_f}l_f+C_{\alpha_r}l_r)}{mV_x}-V_x \\ 0 \\ \frac{-2(C_{\alpha_f}l^2_f+C_{\alpha_r}l^2_r)}{I_zV_x} \\ \end{bmatrix} \end{aligned}

则误差动力学方程为

\dot{x} = Ax + B\delta + B_c\dot{\psi}_{des}

离散化

离散化的方法有很多,例如双线性离散化,前向欧拉差分,后向欧拉差分,AB-2差分法等等.通常的,对于系统矩阵A,为了保障其离散化后的精度与稳定性,我们采用双线性离散化的方法.对于控制矩阵B与矩阵B_c,为了简化运算,我们采用前向欧拉差分法进行离散.离散后的结果如下:
\begin{aligned} &A_d = (I-\frac{T}{2}A)^{-1}(I+\frac{T}{2}A)\\ &B_d = BT\\ &B_{cd} = B_cT \end{aligned}
其中,下标d表示离散域矩阵,T为离散时间

则离散误差动力学模型为
x(k+1) = A_dx(k)+B_d\delta(k)+B_{cd}\dot{\psi}_{des}(k)

至此,我们完成了车辆横向动力学模型的搭建与离散化.下一步,就是将其封装为一个QP问题,并以此设计MPC控制器.