【自动驾驶】动力学横向控制误差模型

参考资料

• 车辆模型-跟踪误差模型
• Vehicle Dynamics and Control

1. 基本概述

车辆横向控制主要通过控制轮胎转角实现，而对于驾驶员来说，可直接操控的是方向盘角度，因此在搭建车辆动力学模型时，可以建立以相对于道路的方向和距离误差为状态变量的动力学模型。

假设——

• $e_y$​:车辆重心距车道中心线的距离；
• $e_{\psi }$​:偏航角误差；
• $V_x$:车辆纵向速度；
• ​$R$：车辆转弯半径，其中转弯半径 $R$足够大，以满足上述章节的小角度近似假设。

定义——

• 车身转过期望角度所需要的偏航角速度为
  $${\dot{\psi }}_{des}=\frac{V_x}{R}\text{\hspace{1em}(1)}$$

• 所需的横向加速度（即期望的向心加速度）为
  $$a_{des}=\frac{V_x^2}{R}=V_x{\dot{\psi }}_{des}\text{\hspace{1em}(2)}$$

• 车辆偏航角误差为

$$e_{\psi }=\psi -{\psi }_{des}\text{\hspace{1em}(3)}$$

• 车辆偏航角速度误差为
  $${\dot{e}}_{\psi }=\dot{\psi }-{\dot{\psi }}_{\text{des }}\text{\hspace{1em}(4)}$$

• 车辆偏航角加速度误差为
  $${\ddot{e}}_{\psi }=\ddot{\psi }-{\ddot{\psi }}_{\text{des }}\text{\hspace{1em}(5)}$$

• 车辆 横向加速度误差为
  $$\begin{array}{rl}{\ddot{e}}_y& =a_y-a_{des}\\ & =\left(\ddot{y}+V_x\dot{\psi }\right)-V_x{\dot{\psi }}_{\text{des }}\\ & =\ddot{y}+V_x\left(\dot{\psi }-{\dot{\psi }}_{des}\right)\end{array}\text{\hspace{1em}(6)}$$
  其中，$a_y$由两 种力共同作用产生 : 车辆延 $y$ 轴产生的惯性加速度 $\ddot{y}$ 和车辆绕旋转中心 $O$ 旋转产生的向心加速度 $a_c=\frac{{V_x}^2}{R}=V_x{\dot{\psi }}_{\circ }$

• 车辆 横向速度误差为

  当车辆纵向速度$V_x$恒定时， $y$ 轴方向的速度误差可以表示为
  $${\dot{e}}_y=\int {\ddot{e}}_y\mathrm{d}t=\dot{y}+V_x\left(\psi -{\psi }_{des}\right)\text{\hspace{1em}(7)}$$
  当纵向速度$V_x$随着时间变化时，对等式(6)积分得
  $${\dot{e}}_y=\int {\ddot{e}}_y\mathrm{d}t=\int \ddot{y}+V_x\left(\dot{\psi }-{\dot{\psi }}_{\text{des }}\right)\mathrm{d}t=\dot{y}+\int V_x\left(\dot{\psi }-{\dot{\psi }}_{\text{des }}\right)\mathrm{d}t\text{\hspace{1em}(8)}$$

  参考资料中的等式(8)为
  $${\dot{e}}_y=\int {\ddot{e}}_y\mathrm{d}t=\dot{y}+\int V_x\left(\psi -{\psi }_{\text{des }}\right)\mathrm{d}t$$
  根据一步一步推导看的话书中的这个等式应该是有问题的，这里先贴在这，有懂的朋友也可以帮忙解惑一下~~

  这就使得模型非线性且时变，不利于控制系统的设计。因此这里假设纵向速度$V_x$恒定， 即当作一个线性时不变(LTI)模型。如果速度变化，就需要使用线性参变模型(LPV) 替代， 其纵向速度是一个随着时间变化的参数。

2. 误差动力学模型

将等式(6)(7)变换如下：

$$\begin{array}{rl}& \ddot{y}={\ddot{e}}_y+V_x{\dot{\psi }}_{des}-V_x\dot{\psi }\\ & \dot{y}={\dot{e}}_y-V_x{e}_{\psi }\end{array}\text{\hspace{1em}(9)}$$
根据车辆动力学模型中的等式(14)
$$\ddot{y}=-\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}\dot{y}-\left(V_x+\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\right)\dot{\psi }+\frac{2C_{\alpha f}}{m}\delta \text{\hspace{1em}(10)}$$

将等式(4)(9)代入等式(10)得

$$\begin{array}{rl}{\ddot{e}}_y+V_x{\dot{\psi }}_{des}-V_x\left({\dot{e}}_{\psi }+{\dot{\psi }}_{\text{des }}\right)& =-\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}\left({\dot{e}}_y-V_x{e}_{\psi }\right)-\\ & \left(V_x+\frac{2C_{\alpha f}{l}_f-2C_{\alpha r}{l}_r}{m{V}_x}\right)\left({\dot{e}}_{\psi }+{\dot{\psi }}_{\text{des }}\right)+\frac{2C_{\alpha f}}{m}\delta \end{array}\text{\hspace{1em}(11)}$$
对等式(11)提取 ${\ddot{e}}_y$​、${\dot{e}}_y$​、$e_y$​、$e_{\psi }$、${\dot{e}}_{\psi }$、${\dot{\psi }}_{des}$​和$\delta$项得

$$\begin{array}{c}{\ddot{e}}_y=\frac{-2C_{\alpha f}-2C_{\alpha r}}{m{V}_x}{\dot{e}}_y+\frac{2C_{\alpha f}+2C_{\alpha r}}{m}{e}_{\psi }+\frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}{\dot{e}}_{\psi }\\ +\left(\frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}-V_x\right){\dot{\psi }}_{\text{des }}+\frac{2C_{\alpha f}}{m}\delta \end{array}\text{\hspace{1em}(12)}$$
整理成矩阵形式为

$$\begin{gathered} \frac{\mathrm{d}}{\mathrm{d}t}{\dot{e}}_y=\left[\begin{array}{llll}0& -\frac{2C_{\alpha f}+2C_{\alpha r}r}{m{V}_x}& \frac{2C_{\alpha f}+2C_{\alpha r}}{m}& \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}\end{array}\right]\left[\begin{array}{c}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]+ \\ \left(\frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}-V_x\right){\dot{\psi }}_{des}+\frac{2C_{\alpha f}}{m}\delta \text{\hspace{1em}(13)} \end{gathered}$$

同理可得，根据车辆动力学模型中的等式(17)
$$\ddot{\psi }=-\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{v}_x}\dot{y}-\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{v}_x}\dot{\psi }+\frac{2l_f{C}_{\alpha f}}{I_z}\delta \text{\hspace{1em}(14)}$$

将等式(4)(5)(9)代入等式(14)，得
$$\begin{array}{r}{\ddot{e}}_{\psi }+{\ddot{\psi }}_{\text{des }}=-\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}\left({\dot{e}}_y-V_x{e}_{\psi }\right)-\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\left({\dot{e}}_{\psi }+{\dot{\psi }}_{\text{des }}\right)+\frac{2l_f{C}_{\alpha f}}{I_z}\delta \end{array}\text{\hspace{1em}(15)}$$

对等式(15)提取 ${\dot{e}}_y$​、$e_y$​、$e_{\psi }$、${\ddot{e}}_{\psi }$​、${\dot{e}}_{\psi }$、${\dot{\psi }}_{des}$​和$\delta$项得
$$\begin{array}{c}{\ddot{e}}_{\psi }=-\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}{\dot{e}}_y+\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}{e}_{\psi }\\ -\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}{\dot{e}}_{\psi }-\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}{\dot{\psi }}_{des}+\frac{2l_f{C}_{\alpha f}}{I_z}\delta -{\ddot{\psi }}_{des}\end{array}\text{\hspace{1em}(16)}$$
由于上述假设为线性时不变系统 $(LT1)\left({\dot{V}}_x=0\right)$ ，故 ${\ddot{\psi }}_{des}=\frac{{\dot{V}}_x}{R}=0$ ，将上述等式整理成矩阵形式得
$$\begin{array}{rl}\frac{\mathrm{d}}{\mathrm{d}t}{\dot{e}}_{\psi }=& \left[\begin{array}{llll}0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}& \frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}& -\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]\left[\begin{array}{c}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]\\ & -\frac{2l_f{}^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}{\dot{\psi }}_{des}+\frac{2l_f{C}_{\alpha f}}{I_z}\delta \end{array}\text{\hspace{1em}(17)}$$
根据等式(13)和(17)，基于跟踪误差变量的状态空间模型表示为
$$\begin{array}{rl}\frac{\mathrm{d}}{\mathrm{d}t}\left[\begin{array}{l}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]=& \left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}& \frac{2C_{\alpha f}+2C_{\alpha r}}{m}& \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}\\ 0& 0& 0& 1\\ 0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}& \frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}& -\frac{2l_f^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]\left[\begin{array}{l}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]\\ & +\left[\begin{array}{c}0\\ \frac{2C_{\alpha f}}{m}\\ 0\\ \frac{2l_f{C}_{\alpha f}}{I_z}\end{array}\right]\delta +\left[\begin{array}{c}0\\ \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}-V_x\\ {\dot{\psi }}_{des}\\ 0\\ -\frac{2l_f^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]{\dot{\psi }}_{des}\end{array}\text{\hspace{1em}(18)}$$

写成一般形式，状态空间表达式如下:
$$tag19\dot{x}=Ax+B_1\delta +B_2{\dot{\psi }}_{\text{des }}$$

如果考虑路面坡度角(road bank angle)$\varphi$（定义可参考前文动力学模型），则等式(18)变为
$$\begin{array}{rl}\frac{\mathrm{d}}{\mathrm{d}t}\left[\begin{array}{l}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]=& \left[\begin{array}{cccc}0& 1& 0& 0\\ 0& -\frac{2C_{\alpha f}+2C_{\alpha r}}{m{V}_x}& \frac{2C_{\alpha f}+2C_{\alpha r}}{m}& \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}\\ 0& 0& 0& 1\\ 0& -\frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z{V}_x}& \frac{2l_f{C}_{\alpha f}-2l_r{C}_{\alpha r}}{I_z}& -\frac{2l_f^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]\left[\begin{array}{l}{e}_y\\ {\dot{e}}_y\\ e_{\psi }\\ {\dot{e}}_{\psi }\end{array}\right]\\ & +\left[\begin{array}{c}0\\ \frac{2C_{\alpha f}}{m}\\ 0\\ \frac{2l_f{C}_{\alpha f}}{I_z}\end{array}\right]\delta +\left[\begin{array}{c}0\\ \frac{-2C_{\alpha f}{l}_f+2C_{\alpha r}{l}_r}{m{V}_x}-V_x\\ {\dot{\psi }}_{des}\\ 0\\ -\frac{2l_f^2{C}_{\alpha f}+2l_r{}^2{C}_{\alpha r}}{I_z{V}_x}\end{array}\right]{\dot{\psi }}_{des}+\left[\begin{array}{c}0\\ g\\ 0\\ 0\end{array}\right]\sin \varphi \end{array}\text{\hspace{1em}(18)}$$