Apollo代码学习(二)—车辆运动学模型

Apollo(阿波罗)是一个开放的、完整的、安全的自动驾驶平台，以灵活和高性能的技术架构，为全自动驾驶提供支持。

前言

  要实现对车辆的控制，研究其运动学模型和动力学模型必不可少。上文对横纵向控制的构成及实现做了概述，本文将从车辆的运动学和动力学模型入手，研究如何控制车辆，以及为什么这样控制车辆。

  本文的观点及图表多基于下述书籍及链接，并结合Apollo代码整理而来，由于本身并没有学过实车相关的知识，对力学的认知也不够深，如有理解偏颇或侵权之处，请指正删改。

【1】Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006.

【2】龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014.

【3】无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型

提倡大家支持正版资源，本人提供文档仅限交流学习使用，侵删：

【1】Rajamani R. Vehicle Dynamics and Control[M]. Springer Science, 2006. | CSDN资源

【2】龚建伟, 姜岩, 徐威. 无人驾驶车辆模型预测控制[M]. 北京理工大学出版社, 2014. | CSDN资源

车辆模型

  车辆运动学模型与动力学模型的建立是出于车辆运动的规划与控制考虑的。自动驾驶场景下，车辆大多按照规划轨迹行驶，控制模块的作用就是控制车辆尽可能精准的按照规划轨迹行驶。这就要求规划轨迹尽可能贴近实际情况，也就是说，轨迹规划过程中应尽可能考虑车辆运动学及动力学约束，使得运动跟踪控制的性能更好。

  搭建车辆模型主要是为了更好的规划和控制，因此，在分析模型时尽量以应用时所需的输入、输出对对象进行建模分析。Apollo中，计算控制命令需要车辆的定位信息、底盘信息以及规划信息等，

Status xxxController::ComputeControlCommand(
    const localization::LocalizationEstimate *localization, //定位信息
    const canbus::Chassis *chassis,  //底盘信息
    const planning::ADCTrajectory *planning_published_trajectory, //规划信息
    ControlCommand *cmd //控制命令)

其中，定位信息(localization.proto)包含

message LocalizationEstimate {
  optional apollo.common.Header header = 1;
  optional apollo.localization.Pose pose = 2;
  optional Uncertainty uncertainty = 3;

  // The time of pose measurement, seconds since the GPS epoch (Jan 6, 1980).
  optional double measurement_time = 4;  // In seconds.

  // Future trajectory acturally driven by the drivers
  repeated apollo.common.TrajectoryPoint trajectory_point = 5;
}

底盘信息(chassis.proto)包含

message Chassis {
  enum DrivingMode {
    COMPLETE_MANUAL = 0;  // human drive
    COMPLETE_AUTO_DRIVE = 1;
    AUTO_STEER_ONLY = 2;  // only steer
    AUTO_SPEED_ONLY = 3;  // include throttle and brake

    // security mode when manual intervention happens, only response status
    EMERGENCY_MODE = 4;
  }
  
  ...

  // Only available for Lincoln now
  optional ChassisGPS chassis_gps = 28;

  optional apollo.common.EngageAdvice engage_advice = 29;
}

规划信息(planning.proto)包含

message ADCTrajectoryPoint {
  optional double x = 1;  // in meters.
  optional double y = 2;  // in meters.
  optional double z = 3;  // height in meters.

  optional double speed = 6;  // speed, in meters / second
  optional double acceleration_s = 7;  // acceleration in s direction
  optional double curvature = 8;  // curvature (k = 1/r), unit: (1/meters)
  // change of curvature in unit s (dk/ds)
  optional double curvature_change_rate = 9;
  // in seconds (relative_time = time_of_this_state - timestamp_in_header)
  optional double relative_time = 10;
  optional double theta = 11;  // relative to absolute coordinate system
  // calculated from the first point in this trajectory
  optional double accumulated_s = 12;

  // in meters, reference to route SL-coordinate
  optional double s = 4 [deprecated = true];
  // in meters, reference to route SL-coordinate
  optional double l = 5 [deprecated = true];
}

对上述元素进行精简，我们需要关注的信息主要有：

1. 时间 （$t$）
2. 位置（$x,y$）
3. 速度（$v$）
4. 加速度 （$a$）
5. 角速度 （$\omega$）
6. 位移 （$s$）
7. 航向角 （$\psi$）
8. 曲率（$k$）
9. 前轮转角 （$wheel$）
10. 方向盘转角 （$steer$）
11. 刹车 （$brake$）
12. 油门 （$throttle$）
13. 档位 （$gear$）

其中

• 方向盘转角 （$steer$）
• 刹车 （$brake$）
• 油门 （$throttle$）
• 档位 （$gear$）

为主要被控量。

单车模型(Bicycle Model)

  建立模型时，应尽可能使模型简单易用，且能真实反映车辆特性，搭建车辆模型多基于单车模型(Bicycle Model)，使用单车模型需做如下假设[^1]：

1. 不考虑车辆在Z轴方向的运动，只考虑XY水平面的运动，如图1所示；
2. 左右侧车轮转角一致，这样可将左右侧轮胎合并为一个轮胎，以便于搭建单车模型，如图2所示；
3. 车辆行驶速度变化缓慢，忽略前后轴载荷的转移；
4. 车身及悬架系统是刚性的。

图1 车辆模型

单车模型（Bicycle Model)将左/右前轮合并为一个点，位于A点；将左/右后轮合并为一个点，位于B点；点C为车辆质心点。

图2 单车模型

其中，$O$ 为OA、OB的交点，是车辆的瞬时滚动中心，线段OA、OB分别垂直于两个滚动轮的方向； $\beta$ 为滑移角（Tire Slip Angle），指车辆速度方向和车身朝向两者间所成的角度， $\psi$为航向角（Heading Angle），指车身与X轴的夹角。

关于滑移角和航向角，此处有较大争议，由于此处是翻译而来的，而我不能给大家准确的信息，因此附上原文，望大家自己甄别：
原文出处：Vehicle Dynamics and Control. p20
( X , Y ) are inertial coordinates of the location of the vehicle while ψ \psiψ describes the orientation of the vehicle. The velocity of the vehicle is denoted by V VV and makes an angle β \betaβ with the longitudinal axis of the vehicle. The angle β \betaβ is called the slip angle of the vehicle.
如若有确切出处可以证明我所书写的有错，感谢指正并附上出处，感激不尽。

符号	定义	符号	定义
A	前轮中心	B	后轮中心
C	车辆质心	O	转向圆心
V	质心车速	R	转向半径
${\ell }_r$	后悬长度	${\ell }_f$	前悬长度
$\beta$	滑移角	$\psi$	航向角
${\delta }_r$	后轮偏角	${\delta }_f$	前轮偏角

当车辆为前轮驱动（front-wheel-only)时，可假设${\delta }_r$恒为0。

车辆运动学模型

  运动学是从几何学的角度研究物体的运动规律，包括物体在空间的位置、速度等随时间而产生的变化，因此，车辆运动学模型应该能反映车辆位置、速度、加速度等与时间的关系。在车辆轨迹规划过程中应用运动学模型，可以使规划出的轨迹更切合实际，满足行驶过程中的运动学几何约束，且基于运动学模型设计出的控制器也能具有更可靠的控制性能。

  基于单车模型，如图2所示，搭建车辆运动学模型。由正弦法则：

$$\frac{\sin ({\delta }_f-\beta )}{{\ell }_f}=\frac{\sin (\frac{\pi }{2}-{\delta }_f)}{R}\text{\hspace{1em}(1.1)}$$
$$\frac{\sin (\beta -{\delta }_r)}{{\ell }_r}=\frac{\sin (\frac{\pi }{2}+{\delta }_r)}{R}\text{\hspace{1em}(1.2)}$$

展开公式(1.1) (1.2)可得：

$$\frac{\sin {\delta }_f\cos \beta -\sin \beta \cos {\delta }_f}{{\ell }_f}=\frac{\cos {\delta }_f}{R}\text{\hspace{1em}(1.3)}$$
$$\frac{\cos {\delta }_r\sin \beta -\cos \beta \sin {\delta }_r}{{\ell }_r}=\frac{\cos {\delta }_r}{R}\text{\hspace{1em}(1.4)}$$

联立公式(1.3)(1.4)可得：

$$(\tan {\delta }_f-\tan {\delta }_r)\cos \beta =\frac{{\ell }_f+{\ell }_r}{R}\text{\hspace{1em}(1.5)}$$

低速环境下，车辆行驶路径的转弯半径变化缓慢，此时我们可以假设车辆的方向变化率等于车辆的角速度。则车辆的角速度为

$$\dot{\psi }=\frac{V}{R}\text{\hspace{1em}(1.6)}$$

联立公式(1.5)(1.6)可得：

$$\dot{\psi }=\frac{V\cos \beta }{{\ell }_f+{\ell }_r}(\tan {\delta }_f-\tan {\delta }_r)\text{\hspace{1em}(1.7)}$$

则在惯性坐标系XY下，可得车辆运动学模型：

$$\{\begin{array}{l}\dot{X}=V\cos (\psi +\beta )\\ \dot{Y}=V\sin (\psi +\beta )\\ \dot{\psi }=\frac{Vcos\beta }{{\ell }_f+{\ell }_r}(tan{\delta }_f-tan{\delta }_r)\end{array}\text{\hspace{1em}(1.8)}$$

此模型中有三个输入：${\delta }_f$、${\delta }_r$、$V$。

滑移角$\beta$可由公式(1.3)(1.4)求得：

$$\beta ={\tan }^{-1}(\frac{{\ell }_f\tan {\delta }_r+{\ell }_r\tan {\delta }_f}{{\ell }_f+{\ell }_r})\text{\hspace{1em}(1.9)}$$

阿克曼转向几何(Ackerman turning geometry)

阿克曼转向几何(Ackerman Turning Geometry)是一种为了解决交通工具转弯时，内外转向轮路径指向的圆心不同的几何学。

在单车模型中，将转向时左/右前轮偏角假设为同一角度，虽然通常两个角度大致相等，但实际并不是，通常情况下，内侧轮胎转角更大。如下图所示，${\delta }_o$ 和 ${\delta }_i$ 分别为外侧前轮和内侧前轮偏角，当车辆右转时，右前轮胎为内侧轮胎，其转角${\delta }_i$ 较左前轮胎转角 ${\delta }_o$ 更大。${\ell }_w$ 为轮距，$L$为轴距，后轮两轮胎转角始终为 0°。

当以后轴中心为参考点时，转向半径R为下图中红线。

图3 阿克曼转向几何

当滑移角$\beta$很小时，且后轮偏角为0时，公式(1.7)可表述为：
$$\frac{\dot{\psi }}{V}\approx \frac{1}{R}=\frac{\delta }{L}\text{\hspace{1em}(1.10)}$$

由于内外侧轮胎的转向半径不同，因此有：
$${\delta }_o=\frac{L}{R+\frac{{\ell }_w}{2}}\text{\hspace{1em}(1.11)}$$
$${\delta }_i=\frac{L}{R-\frac{{\ell }_w}{2}}\text{\hspace{1em}(1.12)}$$

则前轮平均转角
$$\delta =\frac{{\delta }_o+{\delta }_i}{2}\cong \frac{L}{R}\text{\hspace{1em}(1.13)}$$

内外转角之差
$$\Delta \delta ={\delta }_i-{\delta }_o=\frac{L}{R^2}{\ell }_w={\delta }^2\frac{{\ell }_w}{L}\text{\hspace{1em}(1.14)}$$

因此，两个前轮的转向角的差异$\Delta \delta$ 与平均转向角$\delta$ 的平方成正比。

依据阿克曼转向几何设计的车辆，沿着弯道转弯时，利用四连杆的相等曲柄使内侧轮的转向角比外侧轮大大约2~4度，使四个轮子路径的圆心大致上交会于后轴的延长线上瞬时转向中心，让车辆可以顺畅的转弯。

图4 梯形拉杆装置差动转向机构

小结

以后轴中心为参考点，以单车模型为基础建立车辆的运动学模型，$(X_r,Y_r)$为后轴中心坐标，$\psi$为航向角，$v_r$为车速，${\delta }_f$为前轮转角，后轮转角${\delta }_r$恒为0°，$\omega$为横摆角速度，滑移角$\beta$极小，假设为0。

当状态量为$\xi =[X_r,Y_r,\psi ]$，被控量为$u=[v_r,{\delta }_f{]}^T$时，公式(1.8)可转换为如下形式：
$$\left[\begin{array}{c}\dot{X_r}\\ \dot{Y_r}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}cos\psi \\ sin\psi \\ tan{\delta }_f/l\end{array}\right]v_r\text{\hspace{1em}(1.15)}$$
但在无人车控制过程中，一般控制对象$u=[v_r,\omega {]}^T$，则式1.15可写为：

$$\left[\begin{array}{c}\dot{X_r}\\ \dot{Y_r}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}cos\psi \\ sin\psi \\ 0\end{array}\right]v_r+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\omega \text{\hspace{1em}(1.16)}$$

速度$v_r$的控制主要通过刹车（brake）、油门（throttle）、档位（gear）等来控制，横摆角速度主$\omega$要通过转动方向盘（steer）来控制。

Apollo中的运动学约束(control_gflags.cc)主要有：

DEFINE_int32(max_localization_miss_num, 20,
             "Max missing number of localization before entering estop mode");
DEFINE_int32(max_chassis_miss_num, 20,
             "Max missing number of chassis before entering estop mode");
DEFINE_int32(max_planning_miss_num, 20,
             "Max missing number of planning before entering estop mode");

DEFINE_double(max_acceleration_when_stopped, 0.01,
              "max acceleration can be observed when vehicle is stopped");
DEFINE_double(max_abs_speed_when_stopped, 0.01,
              "max absolute speed can be observed when vehicle is stopped");
DEFINE_double(steer_angle_rate, 100.0,
              "Steer angle change rate in percentage.");