Apollo纵向控制位置环的bug

Question

最近在调试自动驾驶的纵向控制，参考Apollo的双环PID，对外环即位置环的设计产生了疑问：

double current_control_time = ::apollo::cyber::Clock::NowInSeconds();
double preview_control_time = current_control_time + preview_time;

TrajectoryPoint reference_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTime(current_control_time);
TrajectoryPoint preview_point =
      trajectory_analyzer->QueryNearestPointByAbsoluteTime(preview_control_time);

1. 如果用参考点做误差，没用到预瞄，执行器有延迟，控制效果会不好；
2. 如果用预瞄点做误差，那么问题来了，预瞄点始终在自车前方，station error会始终是正值，经过位置环PID（一般是P控制）后的输出也始终是正值，这显然不对。

所以，是我哪里理解错了？还是Apollo作为一个demo刻意留的bug？
目前我自己想到的一个思路是，仍然用预瞄方法，但不与自车当前位置做差，而是对自车做匀加速模型预测，预测预瞄时间后的s_predict，用这个值跟期望值（预瞄点的s）做差，这样station error就会有正有负了，而且和横向预瞄的思路一致了。

Answer 1

引自免费微信小程序：皆我百晓生

在理解您的问题后，我认为您对于 Apollo 纵向控制中的预瞄点使用确实有一些误解。预瞄点的主要目的是为了考虑到车辆的动态响应和延迟，确保控制器可以提前调整车辆的速度，以更好地跟踪期望轨迹。在您的描述中，您提出了两个主要的问题，并提供了一个潜在的解决方案。让我为您解释一下，以及如何改进这个方案。

首先，我们来分析一下预瞄点的作用和当前问题：

1. 如果只使用参考点作为误差来源，确实没有充分利用预瞄点的优势，因为车辆可能在到达参考点时才发现速度需要调整，导致反应滞后。
2. 使用预瞄点作为误差源会导致station error始终为正，这是因为预瞄点总是位于车辆前方，而实际车辆位置会逐渐接近并可能超过预瞄点。

您的解决方案是基于车辆的匀加速模型进行预测，然后将预测的位置与预瞄点比较计算误差。这是一个合理的方法，但在实现上可以更简洁一些。我们可以采用一个加权平均的方法来融合当前点和预瞄点的信息，使得station error既可以是正也可以是负，同时也能考虑车辆的动态特性。具体来说，可以使用如下公式计算 station error：

[ station_error = w \cdot (preview_point.s - current_position) + (1 - w) \cdot (reference_point.s - current_position) ]

其中，(w) 是一个权重因子，通常 (0 < w < 1)，它决定了当前点和预瞄点对站位误差的影响程度。当 (w) 接近 1 时，系统更依赖于预瞄点，反之则更多地依赖于当前参考点。

这里是一个 Python 版本的示例代码，用于计算 station error：

import math

def calculate_station_error(current_position, current_control_time, preview_time, trajectory_analyzer):
    current_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(current_control_time)
    preview_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(current_control_time + preview_time)

    # 假设 w 的取值范围是 0.5 到 0.9，可以根据实际情况调整
    w = 0.7
    
    # 计算站位误差
    station_error = w * (preview_point.s - current_position) + (1 - w) * (current_point.s - current_position)
    
    return station_error

# 调用函数计算 station error
station_error = calculate_station_error(current_position, current_control_time, preview_time, trajectory_analyzer)

通过这种方法，您可以得到一个动态平衡当前位置和预瞄点的 station error，进而驱动 PID 控制器产生适当的控制输出。当然，根据实际情况，您可能需要进一步优化 PID 参数，比如 P、I 和 D 值，以及 (w) 的选择，以达到最佳的控制性能。此外，您还可以考虑在控制策略中加入滑模控制或其他先进的控制算法，以提高鲁棒性。