百度Apollo5.0控制模块代码学习（三）纵向控制

一 纵向控制模块疑问

之前分析了纵向控制算法的代码，对其中部分内容仍有疑问，虽然是简单的PID控制，但是位置误差和速度误差到底如何设置的在之前并没有分析明白，看了很多文章对此也没有具体说明，甚至有的文章分析是有问题的。在此我试着自己做一下解答。
通过看之前的文章，有人将纵向控制的工作原理整理如下图所示：

二纵向控制模块分析

通过对以下代码分析可知：先对纵向偏差计算模块得到的速度误差和位置误差进行限制，然后再采用PID控制算法进行相应的加速度和速度补偿值，其中位置误差采用的值为：debug->station_error()，速度误差采用的值为speed_controller_input。

 station_error_limited = common::math::Clamp(
        debug->station_error(), -station_error_limit, station_error_limit);

 double speed_offset =
      station_pid_controller_.Control(station_error_limited, ts);

 speed_controller_input = speed_offset + debug->speed_error();

  speed_controller_input_limited =
      common::math::Clamp(speed_controller_input, -speed_controller_input_limit,
                          speed_controller_input_limit);

 acceleration_cmd_closeloop =
      speed_pid_controller_.Control(speed_controller_input_limited, ts);

  double acceleration_cmd =
      acceleration_cmd_closeloop + debug->preview_acceleration_reference() +
      FLAGS_enable_slope_offset * debug->slope_offset_compensation();

其中，位置误差debug->station_error()为以下方式求得：

debug->set_station_error(reference_point.path_point().s() - s_matched);

reference_point：为距离当前车辆时间最近的规划点，即下一时刻的规划点；
s_matched：longitudinal accumulated distance along reference trajectory，为距离当前车辆距离最近的规划点和车辆目前位置的纵向距离（当规划点不是最后一个点时，该点为距离车辆最近的规划点和下一个规划点之间的插值法得到的点）。下面用图来表示：

绿色为车辆轨迹点。
1：车辆当前位置；
2：距离车辆最近的点，物理和时间均为最近，同时也为reference_point，车辆跟踪点；
3：距离车辆最近的点2和下一点4之间插值法得到的点，为matched_point;
由于每个规划点带有相应的参考速度和时间戳，因此我们的目的是使车辆到达reference_point的同时，车速也为相应的参考车速。

reference_point.path_point().s()：为车辆从目前位置到reference_point走过的实时路程。因此debug->station_error()为车辆走过的路程和距离match_point之差。假设车辆走到点2，则debug->station_error()为点2和点3之间的距离，如果车辆走到最后一点，此时matched_point和reference重合，debug->station_error()就为0，此时的速度补偿为0。
这样做的目的可能是防止控制响应延迟，因此车辆跟踪位置要往后挪一点距离。

速度PID误差采用的值为speed_controller_input，该值为

speed_controller_input = speed_offset + debug->speed_error()

其中speed_offset 是由位置误差求出的速度补偿量， debug->speed_error()计算方法如下：

debug->set_speed_error(reference_point.v() - s_dot_matched);

其中：
reference_point.v()是位置2的参考车速，即车辆跟踪点的车速;
s_dot_matched：位置1车速，车辆当前的纵向车速；
因此，速度PID的误差=速度补偿+跟踪点车速-车辆当前纵向车速。

求出的即为加速度补偿量，加速度求取如下：

  double acceleration_cmd =
      acceleration_cmd_closeloop + debug->preview_acceleration_reference() +
      FLAGS_enable_slope_offset * debug->slope_offset_compensation();

根据代码可知：
加速度=加速度补偿量+预览点参考加速度+坡度加速度补偿；
在该项目中，速度补偿量的计算未参考预览点，但是加速度计算参考了该点。

三纵向控制模块原理

因此最开始的纵向控制图可以更改如下：

为何要这么做，是否有相关论文支持，我还不清楚，如果有人了解，请留言评论。感谢支持。