J2945/1 中Path Prediction模块介绍

2.Path Prediction模块

2.1 简介

路径预测（Path Prediction：PP）是使用车辆的动态信息来估算驾驶员未来的路径的一种算法。整个预测过程不依赖于来自外部信息源所给出的未来道路的几何信息。路径预测主要是获得预测路径的曲线半径以及对应的置信度。

PP模块的主要涉及以下基本步骤：

1. 采集车辆动态信息。

(2)通过车辆动态信息来计算路径半径，从而表征驾驶员的未来轨迹意图。半径等于曲率的倒数。

(3)通过车辆动态数据的变化率来计算预测路径半径的置信度，从而推断出瞬态情况（例如，非稳定状态情况）。

个人疑问：计算出路径的半径如何应用到车辆安全相关的场景中？

2.2 PP模块创建方法

PP模块的目的采用车辆的动态信息来计算未来车辆行驶路径的连续曲线半径。简单来说，就是使用该车辆的速度和方向角的变化率（横摆角速度），通过物理方程来计算曲率。这个曲率可以用来推断车辆未来的路径。

图 2-1车辆未来路径

PP模块需要以下输入信号：

•    车辆速度（m/s）

•    横摆角速度（deg/s）

注：注意分母为0的情况，并且采用适当的方法防止数据溢出。这在车辆速度接近于0的时候尤为重要。

2.3 半径计算

为了更有效地获得PP的半径，计算其倒数来得到曲率（1/r）。这将可以避免当半径在正负无穷大震荡时产生的滤波器输入信号的较大的不连贯性。一旦曲率被计算出来，信号就将被过滤掉不需要的高频噪声。这个滤波器被设计用来大大减小以下的效应：

•     道路噪声

•     传感器噪声

•     驾驶员的噪声（在车道内左右晃动）

PP模块采用二阶低通滤波器来消除横摆角速度信号中的噪声。滤波器是标准离散二阶滤波器，如下图所示：

图 2-2 离散二阶低通滤波器

在图2-2中，ω0 = 2πf0, f0是截止频率, ζ是阻尼系数, Ts采样时间。注: ζ = 1 (默认) 为高阻尼系统

以下方程为基本的车辆半径计算

• radius (m) = vehicle speed (m/s) / yaw rate (rad/s)

在预处理过程中，横摆角速度被转化成rad/s。为了消除分母为0（比如当车辆为静止状态）滤波器输入信号的较大不连续，使用曲率（半径的倒数）来作为输入。

• curvature (1/m) = yaw rate (rad/s) / vehicle speed (m/s)

在计算曲率的时候，信号通过离散二阶低通滤波器，并且滤波器已经被校准到合适的截止频率，阻尼参数以及采样频率。一旦曲率计算被过滤以后，它将被做倒数计算而转换回半径。当曲率为0或者接近于0的时候，须防止计算半径溢出。半径的正负计算根据SAE的旋转方向来定义，以车辆正常朝向的中轴线的顺时针旋转为正，逆时针旋转为负。

最后需要对两个情况做判断来决定是否道路为直线：

•     车辆速度小于设定阈值

•     半径计算结果大于设定阈值

图 2-3 道路半径计算框图

2.4 置信度计算

当车辆处于稳态的时候，PP半径计算的方法是十分有效的；但是在车辆处于动态过程时就会遇到问题。所以当路径预估中出现不准确的情况时，必须有一种方法区分出来并将该情况告知其他车辆。通过在差分并过滤的横摆角速度信号之间插入置信度信息来分别稳定状态。当在短时间内车辆横摆角速度发生很大变化时则为低置信度。这些状态可能包含以下一种或更多的情况：

•     变道

•     进出弯道

•     处于弯道过渡区

•     避障以及其它高动态驾驶情况

在PP模块中，使用离散二阶低通滤波器通过转向输入来识别车辆是否在稳定模式下。离散二阶低通滤波器如图2-4所示。

图 2-4 离散二阶低通滤波器

其中, ω0 = 2πf0, f0 为截止频率, ζ 为阻尼系数, Ts 为采样时间. 注: ζ = 1 (默认值) 为高阻尼系统。

为了使PP模块能够提供高精度的预测路径，车辆必须处在稳定状态情况下。通过对横摆角速度信号进行计算来确定自车是否处于稳定状态。PP模块通过获取车辆横摆角速度变化率来确定车辆是否在稳定状态。置信度的计算具体通过图2-5中离散二阶低通滤波器来实现的。为了能够在动态驾驶的情况下获得路径半径计算，置信度滤波器的截止频率较高。这保证了置信度指示器能够在半径变化输出之前报告置信度变化。

横摆角速度经过滤波器后，再通过查询置信度表就可以获得从0%到100%区间范围的置信度。

图 2-5 车辆预测路径置信度计算

2.5 配置参数

表2-1包含了一张PP模块配置参数表，同时也包含了其他默认，最小，最大配置参数等。

表2-1 PP配置参数表

校准参数	描述	缺省值	最小值	最大值
曲率截止频率	曲率低通滤波器的截止频率	0.33 Hz	0.32 Hz	0.34 Hz
曲率阻尼参数	曲率滤波器阻尼参数	1	0	2
曲率采样周期	离散曲率滤波器采样时间	100 ms	100 ms	400 ms
最小车辆速度	曲率计算所用的车速下限	1 m/s	0 m/s (straight path only)	2 m/s
最大半径	超过这个半径值，路径将会被视为直线	2500 m	2000 m	5000 m
直线路径	当半径比最大半径大时，半径将被设置为这个值并输出来表明为直线	32,767	32,767	32,767
置信度截止频率	置信度滤波器低通截止频率	1 Hz	0.33 Hz	1 Hz
置信度阻尼参数	置信度滤波器阻尼参数	1	0	2
置信度滤波器采样周期	离散置信度滤波器采样时间	100 ms	100 ms	400 ms
置信度	用滤波和差分之后的横摆角速度来查找的二维查找表	见表2-2的值

表 2-2置信度表

Input: Filtered/Differentiated Yaw Rate (deg/s2)	25	20	15	10	5	2.5	2	1.5	1	0.5	0
Output: Confidence (%)	0	10	20	30	40	50	60	70	80	90	100