基于目标运动轨迹预测的自动紧急制动避撞策略

童 燮，万茂松，赵奉奎

（南京林业大学汽车与交通工程学院，江苏南京 210037）

0 引言

我国汽车拥有量持续增加，各种交通事故频发，给家庭和社会带来巨大损失。传统的被动安全措施原理是减小事故发生后传递到驾驶员及乘客身体的冲击，从而减小伤害程度，但却难以避免事故发生。随着交通系统的发展，行车速度得到了很大提高，单独依靠被动安全措施难以保证高速行车事故下的人身安全。自动紧急制动系统（AEB）是汽车高级驾驶辅助系统（Advanced Driving Assistance System，ADAS）的重要组成部分，是一种主动安全措施。通过信息与传感技术感知周围环境信息，自动判断汽车当前的安全状态，提醒驾驶员制动或采取强制制动措施，避免碰撞事故发生，AEB 系统已经逐步成为汽车的标准配置。

避撞策略是汽车AEB 系统核心，精确合理的避撞策略对AEB 系统实现更好的控制效果起着至关重要的作用，而避撞策略的制定则依据车辆安全状态判断。目前有两种方式判断汽车安全状态：①计算出即碰时间，与系统计算的阈值比较，称为安全时间模型；②通过距离判断汽车的安全状态，称为安全距离模型［1］。

安全时间模型中应用较多的为基于碰撞时间（TTC）的比较模型，碰撞时间参数最早由Hayward 于1971 年提出。徐杰等［2］提出一种考虑本车与目标车相对加速度的二阶TTC 公式，弥补了经典TTC 公式在两车车速非常接近时的不足。安全距离模型主要有Honda 模型、Mazda 模型、Berkeley 模型等［3］。此外，有研究提出其它因素的安全距离模型，如驾驶员预估安全距离模型［4-5］、基于车间时距的安全距离模型［6-8］、基于制动过程的安全距离模型［9-11］、改进的驾驶员预估安全模型［12］等；杨为等［13］搭建了对行人保护的分层控制避撞策略，但其制动减速度和风险评估模型是基于经验值判断的；唐阳山等［14］通过采集驾驶员的反应时间确定不同驾驶员的反应时间对安全距离的影响；姜顺明等［15］基于驾驶员状态识别建立自动紧急制动控制策略。

上述模型均只考虑了车与目标的安全时间或者安全距离，没有对目标的运动状态进行深入研究，而且判断目标多为同车道或邻近车道车辆，对于复杂的交通环境，比如交叉路口下的行人、车辆以及其它有可能威胁车辆安全的目标不能准确判断。本文针对上述模型对目标判断的局限性，以驾驶员的自车为原点建立坐标系，预测目标下一段时间的运动轨迹，并将目标的运动轨迹加入汽车安全状态的判断条件，提出一种基于目标运动轨迹的自动紧急制动避撞策略。

1 基于卡尔曼滤波的目标运动轨迹预测算法

1.1 卡尔曼滤波算法

卡尔曼滤波能从一系列包含噪声的测量中估计动态系统状态，是一种高效的递归滤波器。卡尔曼滤波会根据各种测量在不同时间下的值，考虑各时间下的联合分布对未知变数进行估计，比单一测量为基础的估计方式要准。

对于运动状态频繁变化的系统，卡尔曼滤波能通过输入输出观测数据对系统进行最优状态估计，从而实时预测目标的运动状态变化［16］。

卡尔曼滤波状态方程一般为：

观测方程一般为：

其中，Xk为系统当前时刻的状态向量，Ak-1为系统的状态转移矩阵，Xk-1为系统上一时刻的状态向量，Bk-1为系统的控制矩阵，uk-1为系统上一时刻的控制向量，wk-1为系统的过程噪声，Zk为系统当前时刻的观测量，Hk-1为系统的观测矩阵，sk-1为系统的测量噪声。

本文预测过程没有对目标的控制，因此忽略控制输入，即：假设系统的过程噪声wk-1和测量噪声sk-1是相互独立的，且为高斯白噪声，其协方差分别为Q 和R。卡尔曼滤波计算分为两步：时间更新和状态更新，利用递归算法获得系统的最优状态估计。

时间更新方程为：

状态更新方程为：

因此，将目标上一时刻的状态代入时间更新方程便可得到当前状态的先验估计，通过计算卡尔曼增益得到当前状态的后验估计，即最优估计。然后将当前时刻的最优估计代入时间更新方程便可得到下一时刻的先验估计，完成对目标运动轨迹的预测。

1.2 目标运动轨迹建模

汽车检测到的目标，无论是行人还是车辆均可看作是移动对象。以自车为原点建立直角坐标系，通过毫米波雷达和视觉等车载传感器得到目标相对于自车的位置、速度和加速度等信息，将这些信息存储到数据集Tri中，即：

其中，pij为第j时刻的位置矢量，vij为第i个目标在第j个时刻的速度，aij为第i个目标在第j个时刻的加速度。

根据运动学公式：

加速度实时测量模型中只需要使用该值，不需要预测。将公式（10）和（11）写成矩阵形式即为状态方程：

将公式（12）和公式（13）代入卡尔曼滤波的时间更新方程和状态更新方程中，即可完成对目标当前状态的最优估计以及对下一时刻的轨迹预测，通过调整不同的Δt即可实现对未来一段时间的轨迹预测。

2 风险评估模型设计

2.1 报警及制动阈值设定

目前比较常见的有两种AEB 系统风险评估模型：①通过分析本车与目标车之间的制动过程设定相应的安全预碰撞距离阈值，当本车与目标车之间的距离小于设定的安全阈值时开始紧急制动，称为安全距离模型；②计算本车和目标车之间的碰撞时间（TTC），设置相应的碰撞时间阈值，当两车的碰撞时间低于设定的碰撞阈值时开始制动，称为安全时间模型。

但上述两种模型都无法对运动轨迹复杂的目标进行有效判断，汽车仍然存在安全隐患。本文提出一种基于目标运动轨迹的自动紧急制动避撞策略，对自车前方的目标进行轨迹预测，判断自车当前的安全状态，实施过程如下：

在自车行驶过程中，以自车为原点建立平面直角坐标系，如图1 所示。假设自车前方某位置存在一障碍物G，自车安装有毫米波雷达和机器视觉等传感器，可以检测目标相对于自车的距离、角度、相对速度、相对加速度。

Fig.1 Self-driving coordinate system图1 自车坐标系

已知条件为物体G 相对于本车的速度、加速度、距离、角度等信息，使用卡尔曼滤波算法对目标接下来的Tg 秒时间的轨迹进行预测，分为图1 所示4 种情况。可以看出，目标的轨迹与自车坐标系y 轴有不同的交点，将目标轨迹与y 轴交点称为YL。

定义线段S1 为紧急制动区，表示接下来Tg 秒内经过此区域的目标有可能严重威胁自车安全。

定义线段S2 为预警区，表示接下来Tg 秒内经过此区域的目标有可能危及车辆安全，但驾驶员有足够的时间采取措施。

定义线段S1 与S2 之外的区域为安全区，表示接下来Tg 秒内经过此区域的目标不会对自车安全造成影响。

对于轨迹L1 与L4，可以看出物体A 在接下来的Tg秒内的运动轨迹完全在汽车的警报范围之外，这种情况的运动对本车的安全不造成任何影响，此时AEB 不采取任何措施。

对于轨迹L2，可以看出物体A 的运动轨迹与S2 有交点，说明物体G 可能在一个比较危险的时间段内经过汽车前方，此时需要警报提示驾驶员有潜在的安全隐患。

对于轨迹L3，可以看出物体A 的运动轨迹与S1 有交点，说明汽车此时已经处于一种比较危险的状态，应当立即采取制动措施，避免碰撞。

2.2 分级报警制动系统

本文所设计的风险评估模型根据目标的运动轨迹与自车坐标系y 轴的交点纵坐标大小，对各个工况下的行车安全程度进行3 个安全等级划分。

一级为正常行驶级别，如图1 中的轨迹L1 和L4。目标轨迹经过安全区处于正常行驶级别，此时系统仍然对目标实时检测，但无预警无制动。

二级为预警级别，如图1 中的轨迹L2。目标轨迹经过预警区处于碰撞预警级别，此时若驾驶员未意识到危险而采取加速措施极有可能发生碰撞，故此时应发出报警信号，提醒驾驶员减速行驶。

三级为紧急制动级别，如图1 中的轨迹L3。目标轨迹经过紧急制动区处于紧急制动级别，此时盲目加速或者不采取措施都有极大可能发生碰撞事故，AEB 系统应实施自动紧急制动，保障车内外人员安全。

3 自动紧急制动控制系统设计

3.1 AEB 系统工作流程

本文采用上层模糊下层PID 控制的方式对AEB 系统硬件进行控制。图2 为AEB 系统工作流程，车载传感器实时检测前方目标信息，通过卡尔曼滤波预测目标轨迹并将结果输入风险评估模型中，得到当前的报警及制动阈值，将此结果输入到模糊控制器得到当前期望的减速度，通过底层PID 控制器控制制动管路油压，实现对AEB 系统的硬件控制。

Fig.2 System workflow图2 系统工作流程

3.2 AEB 上层模糊控制器设计

为了在制动过程中尽可能保证乘车人员舒适性，本文通过模糊控制器获取期望减速度。本文所述的AEB 模糊控制器是以目标运动轨迹与自车坐标系y 轴交点的纵坐标YL（m）、自车车速Vz（km/h）以及剩余碰撞时间T（s）为输入，以制动时的期望减速度a（m/s2）和报警信号Signal 为输出的三输入双输出模糊控制。

交点纵坐标YL 的隶属度函数如图3 所示，取值为［-1，11］。当YL 为小于0 的任何值时不采取任何措施，认为此时YL 为-1 即可；当YL 大于10 时，认为此时汽车处于安全状态，令YL 为11 即可；当YL 值较小时应采取紧急制动模式；当YL 值较大时，对汽车安全无影响，可正常行驶。隶属度函数使用高斯型函数，模糊语言变量为负、低、中等、高，YL 的模糊集设置为｛NA，LO，MI，HI｝。

Fig.3 YL membership function图3 YL 隶属度函数

自车车速Vz 的取值为［0，80］，Vz 较小时可采取强度较低的制动模式保证车辆行驶安全；当Vz 较高时，为了避免碰撞应加大制动强度，甚至采取紧急制动模式。自车车速Vz 的隶属度函数取高斯型，如图4 所示，其模糊语言变量为停止、非常低、低、中等、高、非常高，Vz 的模糊集设置为｛ST，VL，LO，MI，HI，VH｝。

Fig.4 Vz membership function图4 Vz 隶属度函数

剩余碰撞时间T 的隶属度函数如图5 所示，取值为［-1，6］，表示目标将在T 秒后经过自车坐标系y 轴。当T小于0 时，认为此目标已经过了汽车前方，后面不会再影响汽车安全，故此时令T 为-1 即可；T 越小，表示潜在风险可能更快发生，故此时需要较大的减速度令汽车减速。

Fig.5 T membership function图5 T 隶属度函数

期望减速度a 的取值为［0，8］，其隶属度函数使用高斯型函数，如图6 所示。模糊语言变量为零、非常低、低、中等、高、非常高，将模糊集设置为｛LI，VL，LO，MI，HI，VH｝。

Fig.6 a membership function图6 a 隶属度函数

报警信号Signal 取值为0 和1，其隶属度函数如图7所示。模糊语言变量为0、1，将模糊集设置为｛LI，YI｝。

Fig.7 Signal membership function图7 Signal 隶属度函数

根据隶属度函数分析和仿真研究，设计部分模糊控制器规则如表1 所示。

Table 1 Fuzzy control rules表1 模糊控制规则

3.3 AEB 下层PID 制动控制系统设计

下层PID 控制系统主要功能是接收模糊控制器的期望减速度，通过计算得到车辆制动管路压力值从而对车辆进行减速控制。PID 制动控制系统计算公式如下：

其中，Δa(k)为减速度变化量，kp为比例系数，ki为积分系数，kd为微分系数，e(k)为第k 时刻期望减速度与实际减速度之差。

调整比例系数、积分系数以及微分系数大小，使得实际减速度能快速收敛到期望减速度。选取收敛速度最快的一组PID 参数为最优参数。通过实验，本文PID 的3 个参数最终确定为：kp=3,ki=3.7,kd=0。

4 系统仿真及结果分析

4.1 目标运动轨迹预测仿真

本文选取网络上的一组数据集对卡尔曼滤波轨迹预测算法进行验证，数据集包括目标的横向位置、纵向位置以及加速度等信息，实验软件为MATLAB，通过编写MATLAB 程序对算法进行验证。实验结果如图8 所示（彩图扫描OSID 码可见）。

Fig.8 Track prediction results图8 轨迹预测结果

从图8 可以看出，本文基于卡尔曼滤波的轨迹预测算法能够实现对目标运动轨迹的预测，预测结果与实际运动趋势基本一致，可以作为自动紧急制动系统控制的依据。

4.2 系统避撞策略仿真

系统运行时，通过车载雷达等传感器对目标进行检测，然后通过轨迹预测程序对目标的运动轨迹进行预测，结合当前车速和目标轨迹与自车坐标系纵轴的交点纵坐标，利用模糊控制器判断自车的安全状态，确定危险等级，得到期望减速度；然后通过PID 控制系统控制自车的制动压力，进而实现系统的避撞控制。本文根据系统工作流程建立CarSim 和Simulink 联合仿真模型，如图9 所示。

Fig.9 Co-simulation model图9 联合仿真模型

联合仿真模型包含轨迹预测模型、模糊控制模型、整车模型以及PID 控制模型。其中整车模型根据某车辆的真实数据，通过CarSim 软件建立制动系统动力学模型等等。轨迹预测模型主要根据车辆的距离、加速度等信息预测目标的运动轨迹，得到轨迹与自车坐标系纵轴的交点纵坐标YL。模糊控制器根据YL、当前车速以及剩余碰撞时间，得到期望减速度和报警信号，通过PID 控制完成车身制动。

4.3 仿真结果分析

本文取车速为60km/h 对系统进行仿真分析，通过Car-Sim 软件建立汽车模型，AEB 系统收集本车及前车的信息（速度、加速度），由轨迹预测模型预测目标轨迹，通过模糊控制器进行风险评估并输出期望减速度与报警信号，从而通过PID 进行制动控制。仿真结果如图10 所示。

Fig.10 Simulation results图10 仿真结果

从图10 可以看出，汽车初始速度为60km/h，此时系统检测到某目标会在6s 后经过汽车前方约1m 的位置，即YL 约为1。考虑到当前车速较快，若不采取措施极有可能发生事故，故此时系统采取紧急制动措施。初始时减速度较小，2s 后系统检测到YL 约为3，汽车仍处于危险状态，故系统采取进一步制动措施。3.5s 左右时可以看出YL 为8，此时的距离足够驾驶员发现目标并采取相应的措施，故此时仅报警并不采取制动措施。6s 后，剩余碰撞时间为-1，说明此时目标已经通过了汽车前方，危险状态解除，报警信号归0，汽车匀速行驶。

从仿真结果可以看出，系统的报警信号以及制动措施均与设想相符合，报警信号与汽车安全状态保持同步，充分考虑了目标运动轨迹对汽车安全状态的影响，制动效果良好。

5 结语

本文结合卡尔曼滤波提出一种基于目标轨迹的自动紧急制动避撞策略。通过CarSim/Simulink 联合仿真，结果表明在复杂交通环境下，系统能准确预测目标的运动轨迹并给出合理的减速度，实现自动紧急制动，解决了现有模型无法对目标进行有效判断的局限性。本文所设计的自动紧急制动避撞策略可以满足行车安全需求，对AEB 系统开发设计有一定的参考意义。