Apollo油门刹车标定方案-机器学习

Apollo油门刹车标定方案-机器学习

Introduction

我们考虑整个纵向控制算法作为一个端到端的问题，可以分两步解决：

1. 基于人类驾驶数据，首先生成标定表。
2. 然后用更复杂的在线算法更新标定表，使其更符合车辆动力学。

我们的研究结果表明自动校准系统确实节省了相当多的费用并提高了控制精度。标定是也是自动化和智能化的，因此适用于大规模自动驾驶汽车部署。

Method

apollo控制模块

阿波罗控制模块设计为了跟踪阿波罗规划模块生成的轨迹，并使跟踪误差最小化。它需要轨迹，车辆位置和车辆状态作为输入，以操纵车辆。

纵向控制算法使用公式(1)计算油门刹车值，其中速度(v)和加速度(accA)时纵向控制得到的已知的变量。

$$T:acc,v\to cmd\text{\hspace{1em}(1)}$$

其中$T$表示标定表，$cmd$表示油门刹车控制命令。

1. 离线模型：离线模型根据人工驾驶数据生成一个初始标定表。这个离线算法的工作流程包括：

   1. 采集人的驾驶数据
   2. 预处理数据并选择输入特征
   3. 通过机器学习模型生成校准表

2. 在线学习：在线算法基于自动驾驶中的实时反馈数据调整标定表。目的是让离线模型更加match车辆动力学模型。在线算法的工作流程包括：

   1. 实时收集车辆状态和反馈
   2. 预处理和过滤数据
   3. 调整校准表

数据预处理

1. 用于训练离线模型的数据预处理

人类驾驶数据作为输入，纵向速度、加速度和油门、刹车作为特征。注意，油门模型和刹车模型分开训练。

由于人类驾驶数据天生具有噪声和不均匀性（即速度/加速度不均匀地分布在驾驶过程中），阿波罗控制模块会采取以下步骤以清洗数据。i. 采用公式(2)中的均值滤波器平滑数据；ii. 公式(3)用来消除数据横向摆动；iii. 公式(4)用来消除异常值。

$$y=\frac{\left(x_{t-1}+x_{t-2}+\cdots +x_{t-N}\right)}{N}\text{\hspace{1em}(2)}$$

表示前N个时刻数据的平均值。

$$|\theta |<{\delta }_{\text{steer }}\text{\hspace{1em}(3)}$$

其中$\theta$表示方向盘转角，${\delta }_{steer}$表示方向盘转角阈值。

$$\frac{\left|x-x_{\text{mean }}\right|}{x_{std}}>1\to \text{ 异常值 }\text{\hspace{1em}(4)}$$

其中$x$是待处理数据，$x_{mean}$是数据平均值，$x_{std}$表示数据标准差。

2. 用于在线学习的数据预处理

在线学习会基于自动驾驶模式下的数据反馈，频繁的调节标定表。这个算法的特征选取为：i. 油门刹车命令；ii. 速度；iii. 期望加速度；iiii. 实际加速度。

仅选择符合公式(3)的数据。使用公式(5)来保证标定表对于控制量$cmd$单调。

$$\frac{T^{\prime }\left[cm{d}_i\right]\left[v_x\right]-T^{\prime }\left[cm{d}_j\right]\left[v_x\right]}{cm{d}_i-cm{d}_j}<0,\forall cm{d}_i,cm{d}_j,v_x\text{\hspace{1em}(5)}$$

其中$T^{\prime }$表示标定表的索引。

由于实时数据采集中的响应延迟（即发送油门刹车命令，到命令被执行后得到加速度）会影响数据质量，我们延时一段时间从传感器采集数据。在油门/刹车指令发出200ms后，我们从IMU中采集加速度。采用截止频率为2Hz的3阶巴特沃夫低通滤波器来消除数据的高频扰动。由于200ms的延迟并不总是合适，采用公式(6)进一步保证数据的一致性。

$$\left|cm{d}_{ref}-cm{d}_{ref+{\Delta }_t}\right|<{\delta }_{cmd\_gap},{\Delta }_t=-100\mathrm{ms},\cdots ,100\mathrm{ms}\text{\hspace{1em}(6)}$$

其中${\delta }_{cmd\_gap}$表示允许的最大控制量波动。仅当控制命令$cm{d}_{ref}$的周围$\left[{\mathrm{cmd}}_{ref-100\mathrm{ms}},{\mathrm{cmd}}_{ref+100\mathrm{ms}}\right]$都满足公式(6)时，该数据才能被使用。公式(7)用来消除传感器数据$\left(v_k,a_k\right)$的噪声。

$$\left(v_{ref}-v_k\right)\times \left(a_{ref}-a_k\right)>0\text{\hspace{1em}(7)}$$

3. 离线建模算法

伪码1展现了数据处理过程，得到了对应关系后使用神经网络进行训练。采用Tensorflow实现标准三层神经网络，激活函数采用sigmoid，损失函数采用均方误差，使用内置的Adam优化器。

4. 在线学习算法

每个控制周期在线学习算法都会更新标定表，一旦调整后在下个控制周期生效，伪码2给出了更新细节。

1-5行采集数据，如果速度已经收敛（如公式(8)），6-7行停止标定程序。

$$e_v=\left|v_{\text{ref }}-v_{\text{actual }}\right|⩽{\gamma }_v\text{\hspace{1em}(8)}$$

其中$v_{ref}$表示期望速度，$v_{actual}$表示当前实际速度，${\gamma }_v$表示阈值。

8-11行中的${\Delta }_k$由$gai{n}_k$得到，${\Delta }_k$等于期望加速度$ac{c}_{ref}$减延迟后的实际加速度$ac{c}_k$。

$$gai{n}_k=ac{c}_{ref}-ac{c}_k\text{\hspace{1em}(9)}$$

为了更新标定表，定义了损失函数（公式(10)），该损失函数用来更新对应的标定表$T\left[{\mathrm{cmd}}_i\right]\left[v_j\right]$。

$${\mathrm{cost}}_{{\mathrm{cmd}}_i,v_j}={\text{ distance }}_{cm{d}_i,v_j}\times {\mathrm{cost}}_{\text{similarity }}\text{\hspace{1em}(10)}$$

其中${\text{ distance }}_{cm{d}_i,v_j}$，是一个二维的距离函数，具有距离衰减特性(距离越近值越小，距离越远值越大)。

$$distanc{e}_{cm{d}_i,v_j}=(1-\mu )\times \left[\alpha {\left(cm{d}_{ref}-cm{d}_i\right)}^{m_{cmd}}+\beta {\left(v_{ref}-v_j\right)}^{m_v}\right)+\xi ]\text{\hspace{1em}(11)}$$

其中$\alpha$和$\beta$是衰减系数， $m_{cmd}$和$m_v$是距离衰减因子，$\xi$设置为 $1e^{-8}$，$\mu$表示为

$$\mu =\{\begin{array}{ll}1,& \text{ if }\left|cm{d}_{\text{ref }}-cm{d}_i\right|⩽{\delta }_{\text{cmd }}\text{ or }\left|v_{ref}-v_j\right|⩽{\delta }_v\\ 0,& \text{ otherwise }\end{array}\text{\hspace{1em}(12)}$$

相似性损失定义为公式(13)，实际数据与初始表越相似，损失越大。

$${\mathrm{cost}}_{\text{similarity }}=\epsilon \times e^{-ı\times \mid \text{ init }\_\text{table }\left[{\mathrm{cmd}}_i\right]\left[v_j\right]-ac{c}_k\mid }\text{\hspace{1em}(13)}$$

其中$\epsilon$表示相似性衰减系数 $ı$ 表示指数衰减因子，最终的更新量${\Delta }_t$ 由公式(14)表示

$${\Delta }_t=\frac{{\mathrm{gain}}_k\times \sigma }{1+{\mathrm{cost}}_{cm{d}_i,v_j}}\text{\hspace{1em}(14)}$$

其中$\sigma$表示学习率。

最终，原始表$T$更新为$T^{\prime }$

$$T^{\prime }\left[cm{d}_i\right]\left[v_j\right]=a_t+{\Delta }_t\text{\hspace{1em}(15)}$$

约束条件：

$$\begin{array}{l}\frac{T^{\prime }\left[cm{d}_i\right]\left[v_x\right]-T^{\prime }\left[cm{d}_j\right]\left[v_x\right]}{cm{d}_i-cm{d}_j}<0,j,x=1,\cdots ,n\\ \left|cm{d}_{ref}-cm{d}_m\right|<{\delta }_{cm{d}_{-gap}},m=k-t,\cdots ,k+t\\ \left(v_{ref}-v_k\right)\times \left(a_{ref}-a_k\right)>0\end{array}$$

最终更新效果就是对于每一个标定表$T\left[cm{d}_i\right]\left[v_j\right]$，实际加速度与标定表的值差距越大更新量越大，$\left(cm{d}_i,v_j\right)$距离$\left(cm{d}_{ref},v_{ref}\right)$越近更新量越大。

References

[1] Zhu F, Xu X, Ma L, et al. Autonomous driving vehicle control auto-calibration system: An industry-level, data-driven and learning-based vehicle longitudinal dynamic calibrating algorithm[C]//2020 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2020: 391-397.