自动驾驶控制系统建模之车辆运动学模型

Part One Kinematic Model Setup 系统建模之车辆运动学模型

Assumption:

车辆前后轮均可以转动，左右轮具有相同转角，即前后轮可以分别简化为单车模型 Bicycle Model (注：对于front-wheel-only 系统，后轮转角可设置为 zero)

车辆运动严格限制在 X-Y 二维平面，即忽略车辆上下坡运动

车辆低速运动，前后轮转动产生的轮胎侧滑角为 zero ，即前后轮速度矢量方向与转角一致

车辆低加速度运动，忽略车辆加速、减速或转向时产生的前后轴负载转移

车身以及悬架系统是刚体 rigid body

Parameters Definition:

Parameter	Description	Parameter	Description
O	ICR 旋转中心	C	车辆质心
A	前轮中心	B	后轮中心
V	车辆质心速度	R	车辆转向半径
l_f	前悬长度	l_r	后悬长度
β	车辆侧偏角	ψ	车辆航向角
δ_f	前轮转角	δ_r	后轮转角

注：车辆方向角ψ表示车体坐标系x轴正方向与惯性坐标系x轴正方向之间的夹角；车辆偏移角β即为车辆速度矢量方向与车体坐标系x轴正方向之间的夹角；详细定义如下：

Quotes from Vehicle Dynamics and Control

(X, Y) are inertial coordinates of the location of the c.g. of the vehicle, while ψ describes the orientation of the vehicle. The angle ψ is called the heading angle of the vehicle.

The velocity at the c.g. of the vehicle is denoted by V and makes an angle β with the longitudinal axis of the vehicle. The angle β is called the slip angle of the vehicle.

The course angle for the vehicle is γ = ψ + β .

State Space Equation:

建立模型时，请务必不要过于关注公式的推导结果而忽视公式的推导过程本身，请务必不要将精力集中在公式推导上而忽视模型的状态量与输入量。

以车辆质心为参考点的运动学模型

Step1. 确认模型状态量与输入量

状态量为质心位置与车辆航向角，输入量为车辆速度与前后轮转角。

Step2. 推导

对 TriangleOCA 与 TriangleOCB 应用正弦法则，得：

以下是经过同义改写的文本

基于运动学模型建立的前提条件，在车辆以较低速度运行时（即转向半径的变化速率较低），假设车辆方向在惯性坐标系下的变化率等于车辆自身的角速度ω(t)，即：

\dot{\theta}(t) = \omega(t)

ψ˙=ω=VR联立(5)与(7)ψ˙=Vcos(β)lf+lr[tan(δf)−tan(δr)]

Step3. The overall equations of motion

根据模型状态量与输入量，确认运动学方程如下：

该系统中各变量遵循以下运动学关系：

\begin{cases} \dot{X} & \text{遵循 } V\cos(\psi + \beta) \\ \dot{Y} & \text{遵循 } V\sin(\psi + \beta) \end{cases}

其中，

\dot{\psi} = \frac{[V\cos(\beta)]}{l_f + l_r} [\tan(\delta_f) - \tan(\delta_r)]

[X˙Y˙ψ˙]=[cos(ψ+β)sin(ψ+β)cos(β)[tan(δf)−tan(δr)]lf+lr]∗V(10) = *V \tag{10}

对于 front-wheel-only 系统， δ_r 恒等于 zero

[X˙Y˙ψ˙]=[cos(ψ+β)sin(ψ+β)cos(β)tan(δf)lf+lr]∗V(11) = *V \tag{11}

以后轴中心为参考点的运动学模型 (front-wheel-only)

Step1. 确认模型状态量与输入量

状态量为后轮中心位置与车辆航向角，输入量为车辆速度与前轮转角。

Step2. The overall equations of motion

根据模型状态量与输入量，确认运动学方程如下：

\dot{X}_r = V_{r} \cdot \cos(\psi)
\dot{Y}_r = V_{r} \cdot \sin(\psi) \dot{\psi} = \frac{V_{r} \cdot \tan(\delta_{f})}{l_{f} + l_{r}}

[X˙rY˙rψ˙]=[cos(ψ)sin(ψ)tan(δf)lf+lr]∗Vr(12) = *V_r \tag{12}

以前轴中心为参考点的运动学模型 (front-wheel-only)

Step1. 确认模型状态量与输入量

状态量为前轮中心位置与车辆航向角，输入量为车辆速度与前轮转角

Step2. The overall equations of motion

根据模型状态量与输入量，确认运动学方程如下：

\dot{X}_{\text{ft}} = V_{\text{ft}} \cos(\psi + \delta_{\text{ft}}) \
\dot{Y}_{\text{ft}} = V_{\text{ft}} \sin(\psi + \delta_{\text{ft}}) \
\dot{\phi}_{\text{lateral}} = \\frac{{V}_{\text{lateral,f}}{{\\cdot}}}{{l}_{\text{lateral,fore}}} + \\frac{{-}{\\cdot}}{{l}_{\\text{lateral,appendix}}}

[X˙fY˙fψ˙]=[cos(ψ+δf)sin(ψ+δf)sin(δf)lf+lr]∗Vf = *V_f

Linearization and Discretization:

基于车辆运动学建模所得的非线性系统，在需经过线性化及离散化处理之前，在特定工作区域内不丢失原有精度的关键点上，则会使得系统控制器的设计变得更加简便。

基于参考系统的线性化算法

对于以后轴中心为参考点的运动学模型 (front-wheel-only) ：

Step1. 确认模型状态量与输入量

状态量为后轮中心位置与车辆航向角，输入量为车辆速度与前轮转角。

Step2. 运动学方程线性化处理

以上游 routing 模块生成的轨迹为基准，在此基础上作为参考系统，并通过处理参考系统与当前系统的偏差来实现运动学模型按照给定路径运行的目的。对于该参考系统而言，在任意时刻的状态量与其所受控制量之间均满足：

[\dot{X_r} \quad \dot{Y_r} \quad \dot{\psi_r}] = [\cos(\psi)\sin(\psi)\tan(\delta_f)\frac{l_f + l_r}{V_r}] \cdot V_r = [f_1\ f_2\ f_3] \tag{1}
其中，
\dot{X}_{\text{ref}} = F(X_{\text{ref}}, u_{\text{ref}})
其导数表示为：
\dot{X}_{\text{ref}} = [\dot{x}_{\text{ref}}, \dot{y}_{\text{ref}}, \dot{\psi}_{\text{ref}}]^T
其中：
u_{\text{ref}} = [V_{\text{ref}}, \delta_{\text{ref}}]^T
状态向量定义为：
F = [f_1,\ f_2,\ f_3]^T

在参考点处进行一阶 Taylor 展开，得：

该方法基于泰勒展开式对非线性运动学模型进行线性化处理，并通过状态误差与控制输入误差的传播机制实现了系统的动态特性描述（2）

经线性化处理后，得线性连续状态方程：

dX_{l}/dt=A_{l}X_{l}+B_{l}u_{l}\tag{3}

X_{l}=\\Delta X=u_{l}=\\Delta u=A_{l}=\\frac{\\partial F}{\\partial X}=B_{l}=\\frac{\\partial F}{\\partial u} 经过线性化处理的运动学模型中所涉及的偏导数值均取自该点处(参考点)的真值，在此过程中模型输入量的形式发生了转变：不再以单一时刻的状态量形式出现，而是变为误差量（即当前系统的偏差量），相应地控制器设计目的也随之转变：当系统达到稳态时，在零点处收敛于零值。 ##### 线性连续系统的离散化 经过对状态误差量（真实系统与参考系统之间的偏差）的相应处理后得到了线性连续的状态空间；然而这些基于时间连续模型的状态方程无法直接用于设计控制器从而导致必须对其进行离散化处理以适应控制器的设计需求。 基于积分中值定理中的ξ取不同值的情况, 可依据具体应用需求将数值积分方法划分为前向欧拉法、后向欧拉法以及改进型的中点欧拉法三类. 这些不同的数值求解策略经过对原始方程式的化简处理, 分别对应于下文中列出的（2）、（3）、（4）等相应表达式. 均值积分定理表明： ∫TT+dtX˙dt=∫TT+dt(AX+Bu)dt \int _{T}^{T+dt}{\dot X}dt = \int_ {T}^{T+dt}{(AX+Bu)}dt 在应用积分中值定理后得到： X(T+dt)−X(T)=AX(ξ)dt + Bu(ξ)dt, 其中ξ∈[T,T+dt] (1) X(T+dt)-X(T) = AX(\xi)dt + Bu(\xi)dt \quad \xi\in[T,T+dt] \tag{1} 其中： X(ξ)=X(T), X(T+dt)=(I + Adt)X(T) + (B dt)u(T) (2) X(\xi)=X(T) \\ \quad X(T+dt) = (I + Adt)X(T) + (B dt u)(T)) \tag{2} 进一步推导可得： X(ξ)=X(T + dt), X(T + dt)=(I - Adt)^{-1} X(T)) \\ \quad +(I - Adt)^{-1}(B dt u)(T)) (3) X(\xi)=X(T+dt)) \\ \quad X(T+dt)=(I - Adt)^{-1} X_{\text{新}}(T)) \\ +(I - Adt)^{-1}(B dt u)(T)) \tag{3} 通过平均处理得到： [X_{\text{旧}}(T + dt)+[X_{\text{旧}}](T)]/2, [X_{\text{新}}}(T + dt))=[X_{\text{旧}}}(T)+[新输入]]/2 \\ 即：[公式4] (4) > > > 以前向欧拉法为例，得到系统的线性离散状态方程： > > Xl(k+1)=(I+Aldt)Xl(k)+(Bldt)ul(k) X_l(k+1)=(I+A_ldt)X_l(k) + (B_ldt)u_l(k) =[10−Vrefsin(ψref)dt01Vrefcos(ψref)dt001]Xl(k)+[cos(ψref)dt0sin(ψref)dt0tan(δref)dtlf+lrVrefdt(lf+lr)cos2(δref)]ul(k)(5)=X_l(k)+ u_l(k) \tag{5} 在实际的控制器设计中**_** pipeline 常常被划分为横向控制器和纵向控制器两部分，在实现对车辆进行解耦控制的过程中，在运动学模型中定义的状态量包括后轮中心位置以及车辆航向角两个要素；这些状态下系统接受的输入量包括车辆行驶速度以及前轮转向角度；其中横向控制器（基于LQR或MPC算法）主要负责控制前轮转向角度以实现路径跟踪；而纵向控制器（基于PID算法）则用于调节纵向速度，在具体的运动学模型应用场景下，则要求横向速度趋于零状态以保证车辆平稳行驶。**_