代码解析之自行车模型在Apollo规划中的应用
大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。
在进行无人车的轨迹规划时，需要考虑无人车的车辆模型，才可以规划出符合车辆运动特性的、舒适的、容易被跟踪的路径。常用的车辆运动学模型有自行车模型和阿克曼转向几何模型，自行车模型实际上是对阿克曼转向几何的一个简化。
我在之前分析Apollo Hybrid A* 算法时，提到了它使用自行车模型，在当前状态的基础上，计算下一步可行驶的状态，即 Baidu Apollo代码解析之Open Space Planner中的Hybrid A* 这篇博客中的下列代码。对于车辆状态（x, y, phi），x和y的推算很简单，这里讲一下phi的推算的依据。
//扩展节点，扩展一个node就是扩展了一个grid，但是会产生多个在同一grid内的路径点
std::shared_ptr<Node3d> HybridAStar::Next_node_generator(
std::shared_ptr<Node3d> current_node, size_t next_node_index) {
double steering = 0.0;
size_t index = 0;
double traveled_distance = 0.0;
//steering angle为什么这么算？
//首先，根据next_node_index与next_node_num_的对比是可以区分运动方向的
//这里的if-else就是区分运动正反方向讨论的（前进和倒车）
//其次，车辆在当前的姿态下，既可以向左转、又可以向右转，那么steering angle的
//取值范围其实是[-max_steer_angle_, max_steer_angle_]，在正向或反向下，
//能取next_node_num_/2个有效值。
//即，把[-max_steer_angle_, max_steer_angle_]分为（next_node_num_/2-1）份
//所以，steering = 初始偏移量 + 单位间隔 × index
//steering angle的正负取决于车的转向，而非前进的正反
if (next_node_index < static_cast<double>(next_node_num_) / 2) {
steering = -max_steer_angle_ +
(2 * max_steer_angle_ / (static_cast<double>(next_node_num_) / 2 - 1)) *
static_cast<double>(next_node_index);
traveled_distance = step_size_;
} else {
index = next_node_index - next_node_num_ / 2;
steering = -max_steer_angle_ +
(2 * max_steer_angle_ / (static_cast<double>(next_node_num_) / 2 - 1)) *
static_cast<double>(index);
traveled_distance = -step_size_;
}
...
//从当前grid前进到下一个grid，一个grid内可能有多个路径点
for (size_t i = 0; i < arc / step_size_; ++i) {
const double next_x = last_x + traveled_distance * std::cos(last_phi);
const double next_y = last_y + traveled_distance * std::sin(last_phi);
//看车辆运动学模型——自行车模型
const double next_phi = common::math::NormalizeAngle(last_phi +
traveled_distance / vehicle_param_.wheel_base() * std::tan(steering));
...
}
...
}本文主要参考了 Apollo代码学习(二)—车辆运动学模型 和 无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型 2篇博客。同时，推荐一篇论文《基于多传感器多路径规划自动泊车系统仿真及实车验证》便于理解。
1. 代码中的steering是什么含义？
steering的计算思路我在代码注释中分析过了，steering由max_steer_angle_ 推算而来，因此首先要理解max_steer_angle_ 是什么。max_steer_angle_ 在HybridAStar类的构造函数中初始化。
在自行车模型中，假设后轮朝向与车体朝向始终相同，因此，相对于车体的后轮转角恒等于0，由前轮控制、影响车辆的朝向，也就是说，转动方向盘导致的方向盘转角（即通常意义上的steer）变化，都反映到前轮转角（相对于车体）的变化上了，再由前轮转角影响到车体朝向（phi）。那么，方向盘转角和前轮转角的量化关系是怎样的呢？在Apollo中，假设了该量化关系是线性的（或许是在某种条件下的近似），即前轮转角 = 方向盘转角 / 某个比例。对应max_steer_angle_ 的赋值代码来看，vehicle_param_.max_steer_angle() 就是车辆允许的最大方向盘转角，vehicle_param_.steer_ratio() 就是线性关系的比例参数，而max_steer_angle_ 就是车辆允许的最大前轮转角。综上，steering是车辆的前轮转角大小。
HybridAStar::HybridAStar(const PlannerOpenSpaceConfig& open_space_conf) {
...
max_steer_angle_ = vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio();
...
}2. 为什么下一个状态的车体朝向这样计算？
抛开NormalizeAngle()不谈，代码中 新的朝向 = 旧的朝向 + 行驶距离 / 轴距 * tan(前轮转角)。这个式子的依据是什么呢？
const double next_phi = common::math::NormalizeAngle(
last_phi + traveled_distance / vehicle_param_.wheel_base() * std::tan(steering));假设短时间内，车辆的角速度是恒定的，则 。而角速度
。则
行驶距离 / 转弯半径。
如图1，以后轴中心为参考基准，在直角三角形 O-前轮-后轮 中，。那么
，next_phi = last_phi + 行驶距离 / 轴距 * tan(前轮转角)，得证。
3. 参考文章2中的车体朝向计算方式与此不同，怎么理解？
文章 无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型 中，以车辆质心为参考基准，如图2所示。其（x, y, phi）的更新方式如图3所示。
此时， 和 w * dt = v * dt / R = 行驶距离 / 转弯半径 依然成立。不同之处在于如何求R。在图2左侧直角三角形 O-后轮-质心 中，
。那么 next_phi = last_phi + v * dt * sin(β) / Lr = last_phi + 行驶距离 / Lr * sin(β)。
4. 2种不同的计算方式的区别有多大？
对比2个式子，不同之处在于 行驶距离 / 轴距 * tan(前轮转角) 与 行驶距离 / Lr * sin(β)。对比图1和图2，当 L << R 时，前轮转角与β都会很小，tan(前轮转角) ≈ 前轮转角，sin(β) ≈ β，且 前轮转角 ≈ 2*β， 轴距 ≈ 2*Lr。二者几乎相等。
5. 题外话——向心加速度
Apollo在 Lattice Planner 中会对采样构造的横纵向轨迹进行评估，其中一项是计算向心加速度，代码如下。其实向心加速度就是车辆的横向加速度。那么
假设车辆偏离参考线较小，即 较小，则
。那么
因为车辆偏离参考线较小，代码中使用参考线映射点的曲率来近似自车所在位置的曲率，即 ref_point.kappa()。
double TrajectoryEvaluator::CentripetalAccelerationCost(
const PtrTrajectory1d& lon_trajectory) const {
// Assumes the vehicle is not obviously deviate from the reference line.
...
for (...) {
double s = lon_trajectory->Evaluate(0, t);
double v = lon_trajectory->Evaluate(1, t);
PathPoint ref_point = PathMatcher::MatchToPath(*reference_line_, s);
CHECK(ref_point.has_kappa());
double centripetal_acc = v * v * ref_point.kappa();
...
}
...
}