代码解析之自行车模型在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 *