自动驾驶--车辆动力学模型

车辆动力学模型

第一章：Kinematic Bicycle Model + Longitudinal Dynamics（运动学模型 + 纵向动力学）

---

1.1 模型简介与使用场景

Kinematic Bicycle Model 是一种简化的车辆模型，仅考虑车辆的几何结构与转向约束，忽略轮胎侧偏、惯性力、质量分布等因素。

• 优势：模型简单、计算效率高、适合低速场景；
• 典型应用：路径规划、低速轨迹跟踪、MPC 控制建模；
• 配合纵向动力学模型：可以在路径跟踪同时考虑加速、刹车行为。

---

1.2 变量定义与建模假设

📌 状态变量：

符号	含义	单位
$x$	车体质心在全局坐标系下的 X 位置	m
$y$	车体质心在全局坐标系下的 Y 位置	m
$\theta$	航向角（车辆朝向与 X 轴夹角）	rad
$v$	车辆质心处的速度	m/s
$\delta$	前轮转角（控制输入）	rad

📌 参数定义：

符号	含义
$L$	车辆轴距（前轮轴心到后轮轴心的距离）
$L_f,L_r$	质心到前/后轮的距离，满足$L=L_f+L_r$
$a,b$	一般用于表示车辆前后半轴距

---

1.3 横向运动建模（几何约束）

从几何角度建模车辆的运动。假设车辆速度 $v$ 作用在质心上，方向与车身一致（即忽略横向打滑、侧偏角等现象）。

🔸 基本几何关系：

车辆质心沿着车头方向 $\theta$ 运动，同时受前轮转角 δ\deltaδ 控制车辆的转向。

$\{\begin{array}{l}\dot{x}=v\cos (\theta )\\ \dot{y}=v\sin (\theta )\end{array}$

🔸 航向角变化（角速度）：

车辆转弯时将产生偏航角速度 θ˙\dot{\theta}θ˙，其近似表达式为：

$\dot{\theta }=\frac{v}{L}\tan (\delta )$

这是由于前轮转角控制了车辆的转向半径 $R=\frac{L}{\tan (\delta )}$，而角速度 $\omega =\frac{v}{R}$。

✅ 综合横向运动学模型为：

$\boxed{\{\begin{array}{l}\dot{x}=v\cos (\theta )\\ \dot{y}=v\sin (\theta )\\ \dot{\theta }=\frac{v}{L}\tan (\delta )\end{array}}$

---

1.4 纵向动力学建模（牛顿第二定律）

考虑车辆的加速度来自于纵向驱动力（发动机提供）和阻力（如滚动阻力、空气阻力、坡度等）。

🔸 纵向力平衡公式：

$m\dot{v}=F_{\text{drive}}-F_{\text{drag}}-F_{\text{roll}}-F_{\text{brake}}$

可进一步展开如下：

$\dot{v}=\frac{1}{m}\left(F_{\text{drive}}-\frac{1}{2}\rho C_{d}A{v}^2-mg{f}_r-F_{\text{brake}}\right)$

其中：

• $\rho$：空气密度
• $C_d$：空气阻力系数
• $A$：迎风面积
• $f_r$：滚阻系数

✅ 简化版纵向动力学（无坡道时）：

当只考虑输入加速度 $a$，或用简单模型时可写作：

$\dot{v}=a\text{或}\dot{v}=\frac{F_{\text{long}}}{m}$

其中 $a$ 由油门/刹车控制，$F_{\text{long}}$ 是总纵向力。

---

1.5 模型组合与完整系统形式

🚘 状态空间向量：

$\mathbf{x}=[x,y,\theta ,v{]}^T,\mathbf{u}=[\delta ,a{]}^T$

✅ 联合动力学模型表达式：

$\boxed{\{\begin{array}{l}\dot{x}=v\cos (\theta )\\ \dot{y}=v\sin (\theta )\\ \dot{\theta }=\frac{v}{L}\tan (\delta )\\ \dot{v}=a\end{array}}$

1.6 状态空间形式

为了方便后续控制器设计（如 LQR、MPC），我们明确给出状态与输入向量结构：

• 状态向量：

  $\mathbf{x}=\left[\begin{array}{c}x\\ y\\ \theta \\ v\end{array}\right]$

• 控制向量：

  $\mathbf{u}=\left[\begin{array}{c}\delta \\ a\end{array}\right]$

• 连续时间系统：

  $\dot{\mathbf{x}}=\left[\begin{array}{c}v\cos (\theta )\\ v\sin (\theta )\\ \frac{v}{L}\tan (\delta )\\ a\end{array}\right]$

• 离散时间系统（用于 MPC）：

  ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+T\cdot f({\mathbf{x}}_k,{\mathbf{u}}_k)$

---

1.8 控制约束与示例参数设定

在实际控制器设计中，我们常常需要引入物理约束条件，典型参数如下：

📌 几何与限制参数：

参数名称	符号	示例值	说明
车辆轴距	$L$	2.5 m	前后轮间距离
最大前轮转角	${\delta }_{\max }$	$\pm$30°	控制输入上限
最大加速度	$a_{\max }$	3.0 m/s²	最大油门控制
最小加速度	$a_{\min }$	–5.0 m/s²	最大制动能力
最大速度	$v_{\max }$	35 m/s	大约 126 km/h
最小速度	$v_{\min }$	0 m/s	静止状态

📌 控制约束（可直接用于 MPC）：

$\{\begin{array}{l}\delta \in [-0.52,+0.52]\text{rad}\\ a\in [-5.0,+3.0]{\text{m/s}}^2\\ v\in [0,35]\text{m/s}\end{array}$

第二章：Dynamic Bicycle Model + Longitudinal Dynamics

---

2.1 模型简介与使用场景

Dynamic Bicycle Model 是将车辆建模为一个“动力学双轮车模型”，考虑了轮胎的侧向力、车身的惯性和质心偏移等因素，相较于运动学模型更贴近真实车辆运动，尤其适用于：

• 中高速车辆建模
• 稳定性控制（如电子稳定系统 ESP）
• 轨迹跟踪时的打滑补偿控制
• 横向控制器（LQR、MPC）建模

---

2.2 状态变量与车辆假设

📌 状态变量

符号	含义	单位
$v_x$	车体坐标系下的纵向速度	m/s
$v_y$	车体坐标系下的横向速度	m/s
$r$	偏航角速度（yaw rate）	rad/s
$\delta$	前轮转角（控制输入）	rad
$a_x$	加速度/驱动力（控制输入）	m/s²

坐标系统说明

我们以车辆质心为原点建立车体坐标系：

• $x$ 轴沿车辆前进方向
• $y$ 轴指向车辆左侧（右手坐标系）
• 偏航角速度 $r=\dot{\theta }$ 为车体绕垂直轴的旋转速度

---

2.3 动力学建模

2.3.1 质心动力学（横纵向）

对质心进行牛顿第二定律建模，在车体坐标系中：

$\{\begin{array}{l}m({\dot{v}}_x-r{v}_y)=F_x\\ m({\dot{v}}_y+r{v}_x)=F_{yf}\cos \delta +F_{yr}\end{array}$

其中：

• $F_x$：车辆前进方向的总驱动力（发动机牵引或制动）
• $F_{yf},F_{yr}$：前/后轮产生的侧向力
• $m$：车辆质量
• $r{v}_y$, $r{v}_x$：哥氏力项（非惯性坐标下的修正）

---

2.3.2 偏航角动力学（绕 z 轴）

以车辆质心为参考点，进行刚体转动分析：

$\dot{r}=l_f{F}_{yf}\cos \delta -l_r{F}_{yr}$

• $I_z$：车辆绕垂直轴的转动惯量
• $l_f$、$l_r$：质心到前轴、后轴的距离

---

2.3.3 轮胎侧向力建模（线性轮胎模型）

侧向力来源于轮胎与地面间的侧偏角 $\alpha$，在小角度近似下（线性区）：

$F_{yf}=-C_{\alpha f}{\alpha }_f,F_{yr}=-C_{\alpha r}{\alpha }_r$

其中：

• $C_{\alpha f},C_{\alpha r}$：轮胎侧偏刚度（单位侧偏角产生的侧向力）

🔸 侧偏角建模（假设小角度）：

$\{\begin{array}{l}{\alpha }_f=\delta -\frac{v_y+l_{f}r}{v_x}\\ {\alpha }_r=-\frac{v_y-l_{r}r}{v_x}\end{array}$

2.3.4 纵向动力学补充

在车体坐标系下对纵向方向列牛顿方程：

$m({\dot{v}}_x-r{v}_y)=F_{\text{drive}}-F_{\text{drag}}-F_{\text{roll}}-F_{\text{brake}}$

• 驱动力 / 制动力

  $F_{\text{drive}}={\sum }_i{T}_i/R_w,F_{\text{brake}}={\sum }_i{F}_{b,i}$

  $T_i$：各驱动轮轮端扭矩；$R_w$：有效轮胎半径。

• 空气阻力

  $F_{\text{drag}}=\frac{1}{2}\rho C_d{A}_f{v}_x^2$

• 滚动阻力

  $F_{\text{roll}}=mg{f}_r$

若在坡道上，还应加上坡度阻力 $F_{\text{slope}}=mg\sin {\theta }_{\text{slope}}$。

整理得到纵向加速度：

KaTeX parse error: \tag works only in display equations

---

🌟 方案 A —— 与横向 3 状态并列（常用于耦合 MPC）

新的 7 维状态向量

$\mathbf{x}={\left[\begin{array}{ccccccc}{v}_x& v_y& r& e_y& e_{\psi }& s& \delta \end{array}\right]}^T$

• 轨迹进度 $s$由 $\dot{s}=v_x\cos e_{\psi }-v_y\sin e_{\psi }$ 给出。
• 控制向量
  $\mathbf{u}=[T_{\text{drive}},F_{\text{brake}},\delta {]}^T$

插入式写法（与原横向方程并排）即可得到完整 非线性 7-DOF 模型，用于能量–稳定性联合 MPC。

---

🌟 方案 B —— 线性近似 + 速度误差通道（常用于层次 LQR）

• 设定参考巡航速度 $V_{\text{ref}}$。
• 定义速度误差 $e_v=v_x-V_{\text{ref}}$。
• 在 小角度、$v_x\approx V_{\text{ref}}$ 前提下线性化式 (8)：

KaTeX parse error: \tag works only in display equations

其中$K_{\text{drag}}=\frac{1}{2}\rho C_d{A}_{f}2V_{\text{ref}}$.

这样就得到 横向 4 维 + 纵向 1 维 的 5 维 LTI 系统
$\dot{\tilde{\mathbf{x}}}=A\tilde{\mathbf{x}}+B\tilde{\mathbf{u}}$，
其中
$\tilde{\mathbf{x}}=[v_y,r,e_y,e_{\psi },e_v{]}^T$,
$\tilde{\mathbf{u}}=[\delta ,F_{\text{drive}},F_{\text{brake}}{]}^T$.

---

2.4 完整系统模型（非线性）

将以上所有项代入后，得到非线性动力学模型：

$\boxed{\{\begin{array}{l}{\dot{v}}_x=\frac{1}{m}\left(F_x\right)+r{v}_y\\ {\dot{v}}_y=\frac{1}{m}\left(-C_{\alpha f}(\delta -\frac{v_y+l_{f}r}{v_x})-C_{\alpha r}(-\frac{v_y-l_{r}r}{v_x})\right)-r{v}_x\\ \dot{r}=\frac{1}{I_z}\left(l_f(-C_{\alpha f})(\delta -\frac{v_y+l_{f}r}{v_x})-l_r(-C_{\alpha r})(\frac{v_y-l_{r}r}{v_x})\right)\end{array}}$

---

• 2.5 简化线性模型（含纵向误差）

  线性化前提

  • 车辆以恒定参考速度 $V_{\text{ref}}$ 行驶， $v_x\approx V_{\text{ref}}$。
  • 轮胎小侧偏角、$\sin \theta \approx \theta ,\cos \theta \approx 1$。
  • 纵向、横向耦合项中的高阶积被忽略。

  2.5.1 定义误差变量

  $e_v=v_x-V_{\text{ref}},e_y,e_{\psi }\text{同前节定义}$

  2.5.2 线性化动力学

  $\begin{array}{rl}{\dot{v}}_y& =\frac{-C_{\alpha f}-C_{\alpha r}}{m{V}_{\text{ref}}}{v}_y+[\frac{-C_{\alpha f}{l}_f+C_{\alpha r}{l}_r}{m{V}_{\text{ref}}}-V_{\text{ref}}]r+\frac{C_{\alpha f}}{m}\delta \\ \dot{r}& =\frac{-C_{\alpha f}{l}_f+C_{\alpha r}{l}_r}{I_z{V}_{\text{ref}}}{v}_y-\frac{C_{\alpha f}{l}_f^2+C_{\alpha r}{l}_r^2}{I_z{V}_{\text{ref}}}r+\frac{C_{\alpha f}{l}_f}{I_z}\delta \\ {\dot{e}}_y& =v_y+V_{\text{ref}}{e}_{\psi }\\ {\dot{e}}_{\psi }& =r\\ {\dot{e}}_v& =-\frac{1}{m}(\frac{1}{2}\rho C_d{A}_f{V}_{\text{ref}}^2+mg{f}_r)+\frac{1}{m}(F_x-F_{\text{brake}})\end{array}$

  最后一行把空气阻力、滚阻归并为常数项；若需严格 LTI，可将常数项视为外部扰动并在控制环外补偿。

  ---

  2.6 线性状态空间形式

  2.6.1 状态 & 输入向量

  $\tilde{\mathbf{x}}=\left[\begin{array}{c}{v}_y\\ r\\ e_y\\ e_{\psi }\\ e_v\end{array}\right],\tilde{\mathbf{u}}=\left[\begin{array}{c}\delta \\ F_x\\ F_{\text{brake}}\end{array}\right]$

  2.6.2 系统矩阵

  令

  $\begin{array}{rl}{A}_1& =-\frac{C_{\alpha f}+C_{\alpha r}}{m{V}_{\text{ref}}},A_2=-\frac{C_{\alpha f}{l}_f-C_{\alpha r}{l}_r}{m{V}_{\text{ref}}}-V_{\text{ref}},\\ A_3& =-\frac{C_{\alpha f}{l}_f-C_{\alpha r}{l}_r}{I_z{V}_{\text{ref}}},A_4=-\frac{C_{\alpha f}{l}_f^2+C_{\alpha r}{l}_r^2}{I_z{V}_{\text{ref}}},\\ K_{\text{drag}}& =\frac{1}{2}\rho C_d{A}_{f}2V_{\text{ref}}.\end{array}$.$A=\left[\begin{array}{ccccc}{A}_1& A_2& 0& 0& 0\\ A_3& A_4& 0& 0& 0\\ 1& 0& 0& -V_{\text{ref}}& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& -\frac{K_{\text{drag}}}{m}\end{array}\right],B=\left[\begin{array}{ccc}\frac{C_{\alpha f}}{m}& 0& 0\\ \frac{C_{\alpha f}{l}_f}{I_z}& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& \frac{1}{m}& -\frac{1}{m}\end{array}\right].$

  • $A$ 五行五列对应 $\tilde{\mathbf{x}}$；
  • $B$ 五行三列对应 $\tilde{\mathbf{u}}$。
    若只用一个合成纵向输入 $F_{\text{long}}$，可将 $B$ 最后一行改为 $\frac{1}{m}$ 并删除第 3 列。

  2.6.3 离散化（零阶保持）

  采样周期 TTT 下：

  $A_d=e^{AT},B_d={\int }_0^T{e}^{A\tau }d\tau B.$

  在 Python/Matlab 中可用 scipy.linalg.expm 或 c2d 获得 $A_d,B_d$。

---

2.7 典型参数设定表（仿真或工程应用）

以下是典型中型车辆的模型参数（参考文献或工程系统常用）：

参数名称	符号	示例值	说明
车辆质量	$m$	1500 kg	车辆总质量
偏航转动惯量	$I_z$	2500 kg·m²	绕质心的惯量
前轴距	$l_f$	1.2 m	质心到前轴距离
后轴距	$l_r$	1.3 m	质心到后轴距离
轴距总长	$L=l_f+l_r$	2.5 m	车辆轴距
前轮侧偏刚度	$C_{\alpha f}$	80000 N/rad	单位角度产生的侧向力
后轮侧偏刚度	$C_{\alpha r}$	80000 N/rad	同上
控制周期	$T$	0.1 s	离散时间步长（用于仿真/MPC）

---

📌 说明：

• 上述参数适用于中型轿车（如 Prius、Tesla Model 3 等）；

• 若进行离散化建模（如用于 MPC），你还需要对矩阵 $A,B$ 进行离散化（例如零阶保持 ZOH 方法）：

  $A_d=e^{AT},B_d={\int }_0^T{e}^{A\tau }d\tau \cdot B$

第三章：曲线坐标系车辆模型（Curvilinear Coordinates Vehicle Model）

---

3.1 建模背景与坐标系定义

为了更准确地描述车辆在复杂路径上的跟踪误差，我们引入与参考轨迹绑定的Frenet-Serret 曲线坐标系。在该坐标系下，车辆位置、方向与速度等信息被投影到路径坐标系中，便于定义“与轨迹相关”的误差动态。

📌 坐标系定义：

• $s$：参考路径的弧长进度；
• $n$：车辆质心到参考曲线的横向偏移（正值代表车辆在右侧）；
• ${\varphi }_s$：参考路径在 $s$ 处的切线方向；
• $\phi$：车辆实际航向角；
• $u=\phi -{\varphi }_s$：航向误差；
• $k(s)$：参考曲线在 $s$ 处的曲率（$R=1/k$）。

---

3.2 状态变量与控制输入定义

我们构建的状态空间模型如下：

✅ 状态向量：

$\mathbf{x}=\left[\begin{array}{c}s\\ n\\ \mu \\ v\\ a\\ \delta \\ \dot{\delta }\end{array}\right]$

符号	含义
$s$	跟踪进度（沿轨迹前进的弧长）
$n$	横向误差（车辆质心与轨迹的垂直距离）
$\mu$	航向误差（车辆与轨迹切线夹角）
$v$	车辆质心速度
$a$	车辆纵向加速度
$\delta$	前轮转角
$\dot{\delta }$	转角变化率

✅ 控制输入：

$\mathbf{u}=\left[\begin{array}{c}{u}_{\text{jerk}}\\ u_{\dot{\delta }}\end{array}\right]$

控制量	含义
$u_{\text{jerk}}$	加加速度，控制纵向 jerk
$u_{\dot{\delta }}$	前轮角加速度

---

3.3 偏移角 $\beta$ 定义与推导

由于车辆质心不在转向轮上，前轮转角 $\delta$ 与车辆质心的运动方向之间存在夹角 $\beta$。其计算公式为：

$\beta ={\tan }^{-1}\left(\frac{l_r}{l_f+l_r}\tan \delta \right)$

其中：

• $l_f$：前轴到质心距离；
• $l_r$：后轴到质心距离；
• $L=l_f+l_r$：车辆轴距。

---

3.4 曲线坐标系下的非线性运动学模型

🔷 基于几何关系推导动态方程：

1. 纵向进度变化率（弧长变化）：

$\dot{s}=\frac{v\cos (\mu +\beta )}{1-nk(s)}$

1. 横向误差变化率：

$\dot{n}=v\sin (\mu +\beta )$

1. 航向误差变化率：

$\dot{\mu }=\frac{v}{l_r}\sin \beta -\frac{k(s)v\cos (\mu +\beta )}{1-nk(s)}$

1. 纵向速度与转向动态：

$\dot{v}=a,\dot{a}=u_{\text{jerk}},\dot{\delta }=\dot{\delta },\ddot{\delta }=u_{\dot{\delta }}$

---

3.5 状态空间模型汇总表达

$\boxed{\{\begin{array}{l}\dot{s}=\frac{v\cos (\mu +\beta )}{1-nk(s)}\\ \dot{n}=v\sin (\mu +\beta )\\ \dot{\mu }=\frac{v\sin \beta }{l_r}-\frac{k(s)v\cos (\mu +\beta )}{1-nk(s)}\\ \dot{v}=a\\ \dot{a}=u_{\text{jerk}}\\ \dot{\delta }=\dot{\delta }\\ \ddot{\delta }=u_{\dot{\delta }}\end{array}}$

---

3.6 模型特性与应用说明

• ✅ 优势：
  • 便于设计误差反馈控制器（如轨迹跟踪 LQR、MPC）；
  • 状态变量紧贴实际控制目标（误差最小化）；
  • 结构清晰，适合路径规划与控制统一建模。
• ⚠️ 局限：
  • 曲率 k(s)k(s)k(s) 需已知或可实时计算；
  • 非线性较强，需配合线性化或数值方法求解。

---

3.7 模型结构总结

状态变量	控制变量	描述能力
$e_y,e_{\psi },s$	$\delta$	表示车辆相对于轨迹的几何误差
+ $v$（可选）	$a$（纵向控制）	可加入纵向加速控制

第四章：三种车辆模型的对比分析与选择建议

---

本章将系统地从结构组成、建模精度、控制友好性、计算复杂度、适用场景等角度出发，比较以下三种常见车辆模型：

1. 运动学模型（Kinematic Bicycle Model + Longitudinal Dynamics）
2. 动力学模型（Dynamic Bicycle Model + Longitudinal Dynamics）
3. 曲线坐标系模型（Curvilinear Coordinates Vehicle Model）

---

4.1 建模结构对比

模型	描述方式	是否建模轮胎力	是否考虑质心	是否建模偏航
Kinematic	几何关系	❌	✅	✅
Dynamic	牛顿力学	✅	✅	✅
Curvilinear	误差反馈	❌（隐式）	✅	✅（与参考轨迹比较）

---

4.2 控制友好性与离散化性能

模型	控制器设计适配性	离散化是否简单	是否适合线性化
Kinematic	✅ 适合 MPC、LQR	✅（一阶 Euler 足够）	✅ 小角度可线性化
Dynamic	⚠️ 控制器需处理非线性	⚠️ 需处理侧偏角和轮胎力关系	⚠️ 复杂，适合高保真建模
Curvilinear	✅ 误差驱动控制最优	✅（直接预测误差）	✅（LQR/MPC 核心模型）

---

4.3 适用速度范围与典型场景

模型	适用速度范围	典型应用
Kinematic	低速（0–30 km/h）	路径规划，泊车控制
Dynamic	中高速（30–120 km/h）	高速稳定控制，打滑建模
Curvilinear	全速度段（依赖参考轨迹）	轨迹跟踪、LQR、MPC 控制

---

4.4 控制器适配建议

控制器类型	推荐模型	理由
PID	Kinematic	状态简单，反馈误差直接可用
LQR	Curvilinear	线性状态空间友好，误差形式直观
MPC	Curvilinear / Kinematic	离散化明确，预测窗口容易设置
RL 强化学习	Dynamic	模拟更真实，适合端到端训练

---

4.5 总结对比表

维度	Kinematic Model	Dynamic Model	Curvilinear Model
模型复杂度	★☆☆（低）	★★★（高）	★★☆（中）
控制器适配性	★★☆	★☆☆	★★★
精度与真实度	★☆☆	★★★	★★☆
计算效率	★★★	★☆☆	★★☆
易线性化性	★★★	★☆☆	★★★
轨迹跟踪能力	★★☆	★★★	★★★

---

4.6 总结建议

• 若你的控制器或系统工作在低速、规整环境（如停车、狭窄场景），推荐使用：

  ✅ Kinematic Bicycle Model

• 若希望追求真实响应、考虑车体惯性、轮胎侧偏影响等高保真模型，则推荐：

  ✅ Dynamic Bicycle Model

• 若任务为轨迹跟踪、需要误差驱动反馈、结合最优控制（LQR、MPC），则优选：

  ✅ Curvilinear Model