百度Apollo5.0控制模块代码学习（六）横纵向控制总结

本文禁止转载！！

1 横纵向控制原理分析

之前分析了百度Apollo的横纵向控制原理，并分别画出了原理图。由于实际车辆控制当中是横向控制和纵向控制共同起作用，因此将横纵向控制原理总结如下图。
输入：规划模块确定的轨迹，车辆当前的状态
输出：油门，刹车，方向盘转角

注意：横向控制中输入偏差是横向位置偏差，纵向控制中输入偏差是纵向位置偏差。
横纵向控制有共同的目标点，但是横向控制中目标点是距离最近的点，而纵向控制中目标点是时间最近的点。一般情况下这两个点是同一个点，特殊情况下（如车辆急转弯，车辆跑圈）可能不是一个点。特殊轨迹情况下控制结果如何，需要仿真观察。这个点的不同，不知是故意为之还是一个bug，有知道的可以留言。

2 百度Apollo坐标系分析

关于Apollo中涉及的坐标系，可以参考以下文章：
https://blog.csdn.net/davidhopper/article/details/79162385

其次，根据Apollo的帮助文档可以知道，坐标系共有以下几种：
1，全球地理坐标系

X坐标表示经度，Y坐标表示纬度，Z表示海拔；

2，局部坐标系-东北上（ENU）

X：指向东面
Y：指向北面
Z：指向上方

ENU依赖于地球表面建立的3D笛卡尔坐标系，在Apollo中，UTM坐标系在定位，Planning等模块中作为局部坐标系使用。

3，车辆坐标系-右前上（RFU）

坐标系的转换公式如下：

即坐标轴绕元殿向左旋转后坐标值满足的公式，具体可参考下列文章：
https://blog.csdn.net/u013914471/article/details/83748571

2.1 纵向控制中偏差计算

在控制模块中主要是涉及车身坐标系和SL（弧长-横向偏移）坐标系的转换。
首先计算出在笛卡尔坐标系中的横纵向偏差；

  double dx = x - ref_point.x();
  double dy = y - ref_point.y();

然后根据坐标转换公式转换偏差到SL坐标系下：

  double cos_ref_theta = std::cos(ref_point.theta());
  double sin_ref_theta = std::sin(ref_point.theta());

  double cross_rd_nd = cos_ref_theta * dy - sin_ref_theta * dx;
  double dot_rd_nd = dx * cos_ref_theta + dy * sin_ref_theta;

计算航向角偏差，从而计算出横向车速的变化；

  double delta_theta = theta - ref_point.theta();
  double cos_delta_theta = std::cos(delta_theta);
  double sin_delta_theta = std::sin(delta_theta);
  *ptr_d_dot = v * sin_delta_theta;

纵向车速的变化如下（考虑了曲率）：

  double one_minus_kappa_r_d = 1 - ref_point.kappa() * (*ptr_d);
  *ptr_s_dot = v * cos_delta_theta / one_minus_kappa_r_d;

2.2 横向控制中偏差计算

首先计算出在笛卡尔坐标系中的横纵向偏差；

  const double dx = x - target_point.path_point().x();
  const double dy = y - target_point.path_point().y();

然后根据坐标转换公式转换偏差到SL坐标系下：

  const double cos_target_heading = std::cos(target_point.path_point().theta());
  const double sin_target_heading = std::sin(target_point.path_point().theta());

  double lateral_error = cos_target_heading * dy - sin_target_heading * dx;

计算航向角偏差，从而计算出横向车速和横向加速度的变化；

double heading_error =
      common::math::NormalizeAngle(theta - debug->ref_heading());
  auto lateral_error_dot = linear_v * std::sin(heading_error);
  auto lateral_error_dot_dot = linear_a * std::sin(heading_error);

通过以上的比较，可以知道横纵向控制中坐标系转换方法是一致的，追踪点相同，其中计算出的偏差也相同。