直线车道保持动力学分析 (Lane Keeping)

自动驾驶车辆总是在对应的高精地图车道里面去行驶的, 因此我们在建立动力学的时候需要增添状态变量考虑车辆相对于车道的位置, 首先我们考虑直线的情况

上图中$s$为车辆沿着路径行驶的距离, 横向误差$e$为车辆质心到车道的距离, 航向角度误差$\Delta \Psi$为车辆纵向中线与车道之间的夹角, 下面的分析都需要基于线性轮胎模型(linear tire model)和自行车模型, 此外我们需要定义下述常量
$$\begin{gathered} c_0=C_f+C_r \\ {c}_1=a{C}_f-b{C}_r \\ {c}_2=a^2{C}_f+b^2{C}_r \end{gathered}$$
$$\begin{gathered} m({\dot{U}}_y+r{U}_x)=F_{yr}+F_{yf} \\ =-C_r{\alpha }_r-C_f{\alpha }_f \\ =-C_r(\frac{U_y-br}{U_x})-C_f(\frac{U_y+ar}{U_x})-mr{U}_x+C_f\delta \end{gathered}$$
使用上面定义的常量, 我们可以得到
$$m{\dot{U}}_y=-\frac{c_0}{U_x}{U}_y-\frac{c_1}{U_x}r-mr{U}_x+C_f\delta$$
$$\begin{gathered} I_z\dot{r}=a{F}_{yf}-b{F}_{yr} \\ =-a{C}_f{a}_f+b{C}_r{a}_r \\ =-a{C}_f{\alpha }_f+b{C}_r{\alpha }_r \\ =-a{C}_f(\frac{U_y+ar}{U_x}-\delta )+b{C}_r(\frac{U_y-br}{U_x}) \end{gathered}$$
同样我们使用常量$c_0$, $c_1$和$c_2$进行化简和优化
$$I_z\dot{r}=-\frac{c_1{U}_y+c_{2}r}{U_x}+a{C}_f\delta$$
纵向行驶距离$s$, 横向误差$e$和$\Delta \Psi$的变化可以用如下微分方程表示
$$\begin{gathered} \dot{s}=U_x\cos \Delta \Psi -U_y\sin \Delta \Psi \approx U_x-U_y\Delta \Psi (1) \\ \dot{e}=U_y\cos \Delta \Psi +U_x\sin \Delta \Psi \approx U_y+U_x\Delta \Psi (2) \\ \Delta \dot{\Psi }=r \end{gathered}$$

传递函数分析

为了分析车辆车道保持动力学, 我们需要分别建立横向误差$e$和航向角度误差$\Delta \Psi$相对于
转向输入$\delta$的传递函数, 我们需要对$(1)$和$(2)$求二阶导数,
$$\begin{gathered} m\ddot{e}=m{\dot{U}}_y+m{U}_x\Delta \dot{\Psi } \\ =-\frac{c_0}{U_x}{U}_y-\frac{c_1}{U_x}r-mr{U}_x+C_f\delta +mr{U}_x \\ =-\frac{c_0}{U_x}(\dot{e}-U_x\Delta \Psi )-\frac{c_1}{U_x}\Delta \dot{\Psi }+C_f\delta \\ =-\frac{c_0}{U_x}\dot{e}+c_0\Delta \Psi -\frac{c_1}{U_x}\Delta \dot{\Psi }+C_f\delta \end{gathered}$$
$$\begin{gathered} I_z\Delta \ddot{\Psi }=I_z\dot{r}=-\frac{c_1{U}_y+c_{2}r}{U_x}+a{C}_f\delta \\ =-\frac{c_1(\dot{e}-U_x\Delta \Psi )+c_{2}r}{U_x}+a{C}_f\delta \\ =-\frac{c_1}{U_x}\dot{e}+c_1\Delta \Psi -\frac{c_2}{U_x}\Delta \dot{\Psi }+a{C}_f\delta \end{gathered}$$

我们可以将上述方程写成矩阵形式
$$\left[\begin{array}{cc}m& 0\\ 0& I_z\end{array}\right]\left[\begin{array}{c}\ddot{e}\\ \Delta \ddot{\Psi }\end{array}\right]=\frac{1}{U_x}\left[\begin{array}{cc}-c_0& c_1\\ -c_1& -c_2\end{array}\right]\left[\begin{array}{c}\dot{e}\\ \Delta \dot{\Psi }\end{array}\right]+\left[\begin{array}{cc}0& c_0\\ 0& c_1\end{array}\right]\left[\begin{array}{c}e\\ \Delta \Psi \end{array}\right]+\left[\begin{array}{c}{C}_f\\ a{C}_f\end{array}\right]\delta$$
使用拉普拉斯变换, 我们可以得到如下几何表达式:
$$m{s}^{2}E=-\frac{c_0}{U_x}sE+c_{0}D-\frac{c_1}{U_x}sD+C_f\Delta$$
$$I_z{s}^{2}D=-\frac{c_1}{U_x}sE+c_{1}D-\frac{c_2}{U_x}sD+a{C}_f\Delta$$
经过一系列数学推导我们可以得到横向误差到方向盘转角的传递函数:
$$\frac{E}{\Delta }(s)=\frac{n_2{s}^2+n_{1}s+n_0}{s^2(s^2+d_{1}s+d_0)}$$
系数如下：
$$\begin{gathered} n_2=m{C}_f{I}_z{U}_x^2 \\ {n}_1=C_f{U}_x{c}_{2}m-C_f{U}_x{I}_{z}a{c}_1 \\ {n}_0=C_f{I}_z{U}_x^{2}a{c}_0-C_{}{U}_x^2{c}_{1}m \\ {d}_1=\frac{c_0{I}_z+c_{2}m}{I_{z}m{U}_x} \\ {d}_0=\frac{C_f{C}_r{L}^2-c_{1}m{U}_x^2}{I_{z}m{U}_x^2} \end{gathered}$$
观察上述表达式可以发现转角到横向误差的系统为四阶系统, 一共有四个极点, 两个极点位于坐标原点， 两个极点对应于自行车模型的极点

转向负反馈(Steering Feedback)

从上面一章我们可以看出车辆从方向盘转角到横向误差的系统是不稳定的, 为了让车辆能够跟随轨迹, 我们需要对方向盘转角进行闭环控制, 我们采取最简单的比例控制， 方向盘转角与重心处的横向误差成比率
$$\delta =-k_{p}e$$
接下来我们需要去求闭环系统的极点
$$\frac{E}{E_{des}}(s)=\frac{k_{p}G(s)}{1+k_{p}G(s)}$$
闭环传递系统的分母为4阶段多项式, $s^4+d_1{s}^3+d_2{s}^2+d_{3}s+d_4$
根据罗斯判据(Routh Criteria). 所有的系数都必须是正数，同时还必须满足下面的不等式
$$\begin{gathered} d_1{d}_2>d_3 \\ {d}_1{d}_2{d}_3>d_3^2+d_1^2{d}_4 \end{gathered}$$
上述两个不等式并不能一直满足，该系统是不稳定的, 尤其是在高速的情况下.
怎么从直观的角度去描述这种不稳定性了?
如上图所示, 假设车辆的横向误差不为0, 它会偏航直到到达目标横向位置，使得横向误差为0, 但是这个时候航向角误差不为0, 车辆会在车道中线附近来回摆动.

为了解决上述问题我们需要采取预瞄的方式, 最简单的方法是通过预瞄将航向角(heading error)误差带入反馈中。
方向盘转角命令扩展为
$$\delta =-k_p{e}_{LA}=-k_p(e+x_{LA}\Delta \Psi )$$
上述转角命令我们可以理解为我们在横向误差比例负反馈的基础上增加航向角误差的比例负反馈。反馈比例系数为$k_p{x}_{LA}$.
我们还可以从另一个角度去理解上述控制率，我们可以认为该控制率为基于横向误差的比例微分控制(PD Control Law), 因为横向误差变化率$\dot{e}=U_y+U_x\Delta \Psi$， 横向误差变化率和航向角度误差呈线性关系