[发明专利]车辆行驶轨迹预测和轨迹偏离危险度评估的系统与方法

权利要求书

1.一种车辆行驶轨迹预测和轨迹偏离危险度评估的系统，其特征在于，包括传感器滤波融合估计模块、预测用目标轨迹跟踪控制模块、模型干扰预测模块、卡尔曼预报器及修正模块、角点与车道线距离计算模块、车辆轨迹偏离危险度评估模块；

所述传感器滤波融合估计模块：输入为车载传感器获得的实时数据、坐标系、目标车辆的物理参数和车辆的动力学模型；输出为车辆实时的运动信息的估计值、实时的位姿信息的估计值、估计误差的协方差矩阵和模型干扰的实时估计值；

所述预测用目标轨迹跟踪控制模块从车用实际目标轨迹跟踪控制器移植或学习控制律，并进行一致性校验：输入为卡尔曼预报器及修正模块输出的车辆在预测过程中的运动信息、位姿信息的预测值，和目标轨迹及车道线信息；输出为车辆在预测过程中的控制动作的预测值；

所述模型干扰预测模块拟合模型干扰在开始预测时刻前后一段时间内的变化规律：输入为传感器滤波融合估计模块输出的在开始预测时刻之前一段时间内模型干扰的估计值序列；输出为预测过程中每一步的模型干扰的预测值；

所述卡尔曼预报器及修正模块使用预测过程中上一步的预测结果、控制动作的预测值和模型干扰预测值，计算下一步的预测结果：输入为传感器滤波融合估计模块输出的开始预测时刻车辆运动信息和位姿信息的实时估计值以及估计误差的协方差矩阵、预测用目标轨迹跟踪控制模块输出的预测过程中控制动作的预测值、模型干扰预测模块输出的预测过程中每一步的模型干扰的预测值和车辆的动力学模型；输出为预测过程中每一步车辆运动信息和位姿信息的预测值以及预测误差的协方差矩阵；

所述角点与车道线距离计算模块：输入为卡尔曼预报器及修正模块输出的预测过程中每一步得到的车辆位姿信息的预测值以及预测误差的协方差矩阵和目标轨迹及车道线信息；输出为车辆在每一步预测过程外形四个角点距目标轨迹两侧车道线的距离的预测值以及预测误差的方差，包括左侧角点距左侧车道线和右侧角点距右侧车道线的距离；

所述车辆轨迹偏离危险度评估模块：输入为角点与车道线距离计算模块输出的预测过程每一步车辆外形角点与左右车道线距离的预测值以及预测误差的方差；输出为对应预测过程每一步车辆是否在目标轨迹两侧车道线内的决策结果。

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