车辆行驶轨迹预测和轨迹偏离危险度评估的系统与方法技术方案

本发明专利技术公开了一种车辆行驶轨迹预测和轨迹偏离危险度评估的系统与方法，基于车辆动力学模型，设计传感器滤波融合估计算法和卡尔曼预报器，估计、预测车辆的运动和位姿；结合目标轨迹，根据实际轨迹控制器设计预测用的轨迹控制算法、预测下一步控制动作，输入卡尔曼预报器，获得下一步预测结果，然后估计和预测车辆模型干扰，进一步修正轨迹预测结果，并重复直至完成剩余步数的轨迹预测；根据车辆位姿预测和目标轨迹，计算车辆角点与目标轨迹车道线距离的预测值与预测误差的方差，评估得到车辆轨迹偏离危险度。本发明专利技术可克服传统算法限定条件严格、未考虑闭环控制引起的准确度不足，提高车辆轨迹预测精度，得到的轨迹偏离危险度可用于自动驾驶紧急干预的依据。

【技术实现步骤摘要】
车辆行驶轨迹预测和轨迹偏离危险度评估的系统与方法
本专利技术属于智能网联车辆
，尤其涉及一种车辆行驶轨迹预测和轨迹偏离危险度评估的系统与方法。
技术介绍
车辆以安全、舒适的状态行驶在预期车道位置，是驾驶中的核心任务，是目前车道偏离预警系统(LDW，LaneDepartureWarning)、车道保持辅助系统(LKA，LaneKeepingAssist)、车道中心保持辅助系统(LCA，LaneCenteringAssist)等先进驾驶辅助系统(ADAS，AdvancedDrivingAssistanceSystem)的目标，也是自动驾驶(ADS，AutomatedDrivingsystem)中的核心目标之一。在这些ADAS、ADS系统中，车辆行驶轨迹的估计或预测方法是系统能按照预期计划实现偏离预警、纠偏干预、自动循迹驾驶的关键。目前在车辆的车道偏离预警系统或者车道保持辅助系统的研究内容中，对于车辆是否会在未来一段时间内有驶离车道风险的决策手段主要有：基于车辆轮胎跨越车道线的时间TLC(TimetoLineCrossing)的方法、基于未来偏移量差异FOD(FutureOffsetDifference)的方法，以及基于车道线夹角大小关系的方法。其中，基于TLC的方法假设车辆保持现有的航向角不变或保持现有的前轮转角不变，使用直线轨迹或者圆轨迹来近似车辆未来的行驶轨迹，并计算车辆当前位置距离轨迹与车道线的交点的距离，用这个距离除以车辆的当前车速得到TLC，当TLC小于阈值时发出报警。基于FOD的方法考虑了驾驶人的个人驾车习惯，在绘制虚拟的车道边界时添加了驾驶人自然转向时习惯的偏移量。基于车道线夹角大小关系的方法主要基于对图像的处理，首先将摄像头拍摄的图片经过一定的后处理获得左右车道线在图像中的位姿信息，然后比较左右车道线与水平线的夹角，若左车道线与水平线的夹角持续增大且右车道线与水平线的夹角持续减小，则说明车辆正在向左偏离车道中心，当左车道线与水平线的夹角增大到某一阈值时，则判定车辆将要驶离车道，同理对右车道线亦然。比较而言，基于TLC的方法的假设条件过于理想，若车辆的航向角或者车速在预测时域内发生改变，则该方法决策的结果就会出现误差；对于基于FOD的方法，虽然该方法考虑了驾驶人的驾驶习惯，实验也证明了这个方法能有效地降低预警地误报率，但是该方法与基于TLC方法相似，基于车速保持不变的假设，与实际可能不符；对于基于车道线夹角大小关系的方法，该方法的有效性依赖于车辆的位姿、车道线形状等因素，通用性差，而且无法对未来的情况进行预测。总结来看，已有的轨迹预测方法存在上述诸多不足，此外均未考虑包括车道保持控制、车道循迹控制等车辆自有ADAS/ADS系统所搭载的闭环控制算法，因此在实际精度方面存在严重不足，无法作为ADAS/ADS高精度轨迹预测所用，更无法用于监督ADAS/ADS系统是否安全运行。另一方面，车载传感器的发展趋势表明，未来车辆将会搭载除了摄像头之外的高精地图、组合惯性导航、高精度定位等先进传感器，这为更精确、及时的车辆轨迹预测算法开发提供了新的可能。利用自动驾驶车辆搭载的高精地图、摄像头与GNSS/IMU(组合惯性导航，GlobalNavigationSatelliteSystem/InertialMeasurementUnit))共同对车道线、本车位置进行感知，并进行一定的数据融合，就可以获得较为准确的车道线位姿信息与车辆位置的一个置信区域。
技术实现思路
本专利技术的目标在于针对现有技术的不足，提出了一种车辆行驶轨迹预测和轨迹偏离危险度评估的系统与方法。本专利技术考虑了感知模块的感知误差对决策的影响，基于车辆的动力学模型对轨迹偏离情况进行预测，并且预测了自动驾驶车辆在预测过程中对车辆施加的控制输入，并且对可能存在的侧向干扰也进行了预测，使整个预测结果更加符合实际。本专利技术的目标是通过以下技术方案来实现的：一种车辆行驶轨迹预测和轨迹偏离危险度评估的系统，包括传感器滤波融合估计模块、预测用目标轨迹跟踪控制模块、模型干扰预测模块、卡尔曼预报器及修正模块、角点与车道线距离计算模块、车辆轨迹偏离危险度评估模块。所述传感器滤波融合估计模块：输入为车载传感器获得的实时数据、坐标系、目标车辆的物理参数和车辆的动力学模型。输出为车辆实时的运动信息的估计值、实时的位姿信息的估计值、估计误差的协方差矩阵和模型干扰的实时估计值。所述预测用目标轨迹跟踪控制模块从车用实际目标轨迹跟踪控制器移植或学习控制律，并进行一致性校验：输入为卡尔曼预报器及修正模块输出的车辆在预测过程中的运动信息、位姿信息的预测值，和目标轨迹及车道线信息。输出为车辆在预测过程中的控制动作的预测值。所述模型干扰预测模块拟合模型干扰在开始预测时刻前后一段时间内的变化规律：输入为传感器滤波融合估计模块输出的在开始预测时刻之前一段时间内模型干扰的估计值序列。输出为预测过程中每一步的模型干扰的预测值。所述卡尔曼预报器及修正模块使用预测过程中上一步的预测结果、控制动作的预测值和模型干扰预测值，计算下一步的预测结果：输入为传感器滤波融合估计模块输出的开始预测时刻车辆运动信息和位姿信息的实时估计值以及估计误差的协方差矩阵、预测用目标轨迹跟踪控制模块输出的预测过程中控制动作的预测值、模型干扰预测模块输出的预测过程中每一步的模型干扰的预测值和车辆的动力学模型。输出为预测过程中每一步车辆运动信息和位姿信息的预测值以及预测误差的协方差矩阵。所述角点与车道线距离计算模块：输入为卡尔曼预报器及修正模块输出的预测过程中每一步得到的车辆位姿信息的预测值以及预测误差的协方差矩阵和目标轨迹及车道线信息。输出为车辆在每一步预测过程外形四个角点距目标轨迹两侧车道线的距离的预测值以及预测误差的方差，包括左侧角点距左侧车道线和右侧角点距右侧车道线的距离。所述车辆轨迹偏离危险度评估模块：输入为角点与车道线距离计算模块输出的预测过程每一步车辆外形角点与左右车道线距离的预测值以及预测误差的方差。输出为对应预测过程每一步车辆是否在目标轨迹两侧车道线内的决策结果。进一步地，所述车载传感器获得的实时数据包括车辆质心的东向速度和北向速度、车辆横摆角速度、车辆质心的经度和维度、车辆的航向角、轮速以及方向盘转角。所述运动信息包括车辆的质心速度和横摆角速度。所述位姿信息包括车辆质心的位置和车辆航向。所述控制动作为前轮转角。一种车辆行驶轨迹预测和轨迹偏离危险度评估的方法，包括以下步骤：1)确定车辆行驶轨迹预测的预测时域、步长、步数；确定所需的车辆参数、传感器参数；确定大地坐标系OXYZ、车身坐标系oxyz；2)基于车辆动力学模型，设计传感器滤波融合估计模块，实时获得车辆的运动信息的估计值、位姿信息的估计值、估计误差的协方差矩阵、模型干扰信息的估计值；3)根据传感器滤波融合估计算法获得的车辆位姿信息与目标轨迹及车道线信息，基于车辆动力学模型本文档来自技高网...

【技术保护点】
1.一种车辆行驶轨迹预测和轨迹偏离危险度评估的系统，其特征在于，包括传感器滤波融合估计模块、预测用目标轨迹跟踪控制模块、模型干扰预测模块、卡尔曼预报器及修正模块、角点与车道线距离计算模块、车辆轨迹偏离危险度评估模块。/n所述传感器滤波融合估计模块：/n输入为车载传感器获得的实时数据、坐标系、目标车辆的物理参数和车辆的动力学模型。/n输出为车辆实时的运动信息的估计值、实时的位姿信息的估计值、估计误差的协方差矩阵和模型干扰的实时估计值。/n所述预测用目标轨迹跟踪控制模块从车用实际目标轨迹跟踪控制器移植或学习控制律，并进行一致性校验：/n输入为卡尔曼预报器及修正模块输出的车辆在预测过程中的运动信息、位姿信息的预测值，和目标轨迹及车道线信息。/n输出为车辆在预测过程中的控制动作的预测值。/n所述模型干扰预测模块拟合模型干扰在开始预测时刻前后一段时间内的变化规律：/n输入为传感器滤波融合估计模块输出的在开始预测时刻之前一段时间内模型干扰的估计值序列。/n输出为预测过程中每一步的模型干扰的预测值。/n所述卡尔曼预报器及修正模块使用预测过程中上一步的预测结果、控制动作的预测值和模型干扰预测值，计算下一步的预测结果：/n输入为传感器滤波融合估计模块输出的开始预测时刻车辆运动信息和位姿信息的实时估计值以及估计误差的协方差矩阵、预测用目标轨迹跟踪控制模块输出的预测过程中控制动作的预测值、模型干扰预测模块输出的预测过程中每一步的模型干扰的预测值和车辆的动力学模型。/n输出为预测过程中每一步车辆运动信息和位姿信息的预测值以及预测误差的协方差矩阵。/n所述角点与车道线距离计算模块：/n输入为卡尔曼预报器及修正模块输出的预测过程中每一步得到的车辆位姿信息的预测值以及预测误差的协方差矩阵和目标轨迹及车道线信息。/n输出为车辆在每一步预测过程外形四个角点距目标轨迹两侧车道线的距离的预测值以及预测误差的方差，包括左侧角点距左侧车道线和右侧角点距右侧车道线的距离。/n所述车辆轨迹偏离危险度评估模块：/n输入为角点与车道线距离计算模块输出的预测过程每一步车辆外形角点与左右车道线距离的预测值以及预测误差的方差。/n输出为对应预测过程每一步车辆是否在目标轨迹两侧车道线内的决策结果。/n...

【技术特征摘要】
1.一种车辆行驶轨迹预测和轨迹偏离危险度评估的系统，其特征在于，包括传感器滤波融合估计模块、预测用目标轨迹跟踪控制模块、模型干扰预测模块、卡尔曼预报器及修正模块、角点与车道线距离计算模块、车辆轨迹偏离危险度评估模块。
所述传感器滤波融合估计模块：
输入为车载传感器获得的实时数据、坐标系、目标车辆的物理参数和车辆的动力学模型。
输出为车辆实时的运动信息的估计值、实时的位姿信息的估计值、估计误差的协方差矩阵和模型干扰的实时估计值。
所述预测用目标轨迹跟踪控制模块从车用实际目标轨迹跟踪控制器移植或学习控制律，并进行一致性校验：
输入为卡尔曼预报器及修正模块输出的车辆在预测过程中的运动信息、位姿信息的预测值，和目标轨迹及车道线信息。
输出为车辆在预测过程中的控制动作的预测值。
所述模型干扰预测模块拟合模型干扰在开始预测时刻前后一段时间内的变化规律：
输入为传感器滤波融合估计模块输出的在开始预测时刻之前一段时间内模型干扰的估计值序列。
输出为预测过程中每一步的模型干扰的预测值。
所述卡尔曼预报器及修正模块使用预测过程中上一步的预测结果、控制动作的预测值和模型干扰预测值，计算下一步的预测结果：
输入为传感器滤波融合估计模块输出的开始预测时刻车辆运动信息和位姿信息的实时估计值以及估计误差的协方差矩阵、预测用目标轨迹跟踪控制模块输出的预测过程中控制动作的预测值、模型干扰预测模块输出的预测过程中每一步的模型干扰的预测值和车辆的动力学模型。
输出为预测过程中每一步车辆运动信息和位姿信息的预测值以及预测误差的协方差矩阵。
所述角点与车道线距离计算模块：
输入为卡尔曼预报器及修正模块输出的预测过程中每一步得到的车辆位姿信息的预测值以及预测误差的协方差矩阵和目标轨迹及车道线信息。
输出为车辆在每一步预测过程外形四个角点距目标轨迹两侧车道线的距离的预测值以及预测误差的方差，包括左侧角点距左侧车道线和右侧角点距右侧车道线的距离。
所述车辆轨迹偏离危险度评估模块：
输入为角点与车道线距离计算模块输出的预测过程每一步车辆外形角点与左右车道线距离的预测值以及预测误差的方差。
输出为对应预测过程每一步车辆是否在目标轨迹两侧车道线内的决策结果。


2.如权利要求1所述车辆行驶轨迹预测和轨迹偏离危险度评估的系统，其特征在于，所述车载传感器获得的实时数据包括车辆质心的东向速度和北向速度、车辆横摆角速度、车辆质心的经度和维度、车辆的航向角、轮速以及方向盘转角。所述运动信息包括车辆的质心速度和横摆角速度。所述位姿信息包括车辆质心的位置和车辆航向。所述控制动作为前轮转角。


3.一种车辆行驶轨迹预测和轨迹偏离危险度评估的方法，其特征在于，包括以下步骤：
1)确定车辆行驶轨迹预测的预测时域、步长、步数；确定所需的车辆参数、传感器参数；确定大地坐标系OXYZ、车身坐标系oxyz；
2)基于车辆动力学模型，设计传感器滤波融合估计模块，实时获得车辆的运动信息的估计值、位姿信息的估计值、估计误差的协方差矩阵、模型干扰信息的估计值；
3)根据传感器滤波融合估计算法获得的车辆位姿信息与目标轨迹及车道线信息，基于车辆动力学模型，将车用实际目标轨迹跟踪控制器中的控制律移植到预测用目标轨迹跟踪控制模块；
4)令当前时刻k为预测过程的第0步，将滤波融合估计模块输出的车辆运动信息的估计值、位姿信息的估计值、估计误差协方差矩阵作为第0步的车辆运动信息的预测值、位姿信息的预测值、预测误差协方差矩阵；
5)根据当前时刻k之前一段时间内模型干扰的估计值序列，利用模型干扰预测算法，得到第1步至第N步(N为预测的总步数)预测过程中的模型干扰预测值；
6)根据第i-1步预测过程中的车辆运动信息的预测值、位姿信息的预测值，结合预测过程的第0步时由高精地图/导航定位获得的预测时域内的目标轨迹及车道线信息，代入预测用目标轨迹跟踪控制模块，计算得到第i步预测过程的控制动作信息的预测值；
7)第i步(i≥1)预测过程对应未来时刻k+i，采用第i-1步的车辆运动信息的预测值、位姿信息的预测值、预测误差协方差矩阵，以及目标轨迹跟踪控制模块输出的第i步预测过程中控制动作信息的预测值，将四类信息代入卡尔曼预报器，并利用模型干扰预测模块输出的第i步预测过程中模型干扰信息的预测值，加以修正，得到未来时刻k+i的车辆运动信息的预测值、位姿信息的预测值以及预测误差协方差矩阵；
8)根据第i步预测的车辆位姿信息的预测值与预测误差协方差矩阵，结合预测过程的第0步时由高精地图/导航定位获得的预测时域内的目标轨迹及车道线信息，分别计算车辆外部边缘四个角点与左右车道线距离的预测值与预测误差方差，并作为车辆轨迹偏离危险度的评估依据，进行危险度的评估；
9)在预测时域内，重复过程步骤6)～8)，进行多步车辆状态预测和轨迹偏离危险度评估，直至完成整个过程。


4.根据权利要求3所述车辆行驶轨迹预测和轨迹偏离危险度评估的方法，其特征在于，所述步骤5)中，模型干扰的预测，具体指：
用线性模型来近似模型干扰在时刻k前后一段时段内近似的变化规律；式中，k0和k1表示模型拟合的系数。
用于拟合的数据来自k-1时刻、k-2时刻、...k-M时刻的模型干扰的估计值，使用最小二乘拟合，得到系数k0和k1。


5.根据权利要求4所述车辆行驶轨迹预测和轨迹偏离危险度评估的方法，其特征在于，所述步...

【专利技术属性】
技术研发人员：李道飞，林思远，刘关明，肖斌，潘豪，胡建侃，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江;33