一种基于车路作用机理的越野环境中车辆行驶风险预测方法

摘要

本发明公开了一种基于车路作用机理的越野环境中车辆行驶风险预测方法，其方法为：第一步、数据预处理；第二步、对车辆运动状态进行估计；第三步、对环境状态与环境参数进行估计；第四步、对行驶风险进行预测。有益效果：结合了车辆动力学、地面力学、计算机视觉等学科的技术，通过结合前方道路的几何和力学特征进行风险的静、动态分层预测的方式，得以用基于车‑路作用的最根本的原理解决车辆行驶风险预测问题，并且能够结合多学科的先进技术手段，符合自动驾驶技术发展趋势，具有广泛的应用前景和多场景、多工况下的可行性。

权利要求

1.一种基于车路作用机理的越野环境中车辆行驶风险预测方法，其特征在于：其方法如下所述：

第一步、数据预处理；具体过程如下：

步骤一、进行数据采集：采集智能车辆上搭载的车载CAN总线、卫星‑惯导组合定位系统、相机、激光雷达和毫米波雷达的传感器信号，按照固定采集周期对所有传感器数据进行采集，所采集的传感器数据形式包括：彩色相机产生的以帧为单位的彩色图像，激光雷达产生的以帧为单位的空间三维点云，毫米波雷达产生的以帧为单位的障碍物状态列表，车载CAN总线上产生的以时间序列表示的车辆操纵与运动状态数据，卫星‑惯导组合定位系统产生的以时间序列表示的定位与姿态数据；

步骤二、对数据进行校验和对齐，具体内容包括：传感器数据的时间对齐；传感器数据的空间对齐；传感器数据的有效性校验；生成带有有效性标记、经过时间和空间对齐的车载总线对齐信号，车辆状态测量对齐信号和多模态环境传感器对齐信号，供后续步骤使用；

第二步、对车辆运动状态进行估计；具体过程如下：步骤一、进行卫星‑惯导组合定位系统估计：利用车辆状态测量对齐信号，根据车辆动力学，对包括车辆的纵侧向速度、车辆的纵侧向加速度、垂向加速度和车轮载荷的车辆动力学状态进行估计；利用车辆状态测量对齐信号，对包括俯仰角和侧倾角在内的车辆姿态进行估计；利用车辆状态测量对齐信号，对车辆的位置进行估计；

步骤二、进行动力学状态估计：通过读取车载总线对齐信号并进行滤波处理得到包括车辆的操纵状态，包括踏板开度、方向盘转角和车辆是否处于静止状态的车辆的操纵状态；

通过读取车载总线对齐信号结合车辆动力学，估计包括车辆纵向速度、侧向速度、车辆纵向加速度、侧向加速度、垂向加速度和车轮载荷在内的车辆动力学状态；通过读取车载总线对齐信号，通过滤波估计包括制动压力、挡位和ABS触发状态在内的车辆部件状态；读取车载总线对齐信号和车辆状态测量对齐信号，根据车辆动力学计算车轮滑转率这一车辆动力学信号；

步骤三、进行同时建图与定位：通过同时建图与定位方法根据多模态环境传感器对齐信号，估计包括车辆纵向速度、侧向速度、车辆纵向加速度、侧向加速度、垂向加速度和车轮载荷在内的车辆动力学信号；通过同时建图与定位方法根据多模态环境传感器对齐信号，估计包括车辆俯仰角和侧倾角在内的车辆姿态；通过同时建图与定位方法根据多模态环境传感器对齐信号，对车辆的位置进行估计；

步骤四、状态估计融合：评估卫星‑惯导组合定位系统估计、动力学状态估计和同时建图与定位各自的可靠程度，将由不同步骤，即不同估计原理得到的相同物理意义的估计结果进行融合；对分别由卫星‑惯导组合定位系统估计、动力学状态估计和同时建图与定位得到的车辆动力学状态进行融合；对分别由卫星‑惯导组合定位系统估计和同时建图与定位得到的车辆姿态进行融合；对分别由卫星‑惯导组合定位系统估计和同时建图与定位得到的车辆位置进行融合；融合方法包括但不限于：卡尔曼滤波法或线性加权法；

第三步、对环境状态与环境参数进行估计；

第四步、对行驶风险进行预测；具体过程如下：步骤一、进行运动障碍物风险评估：利用车辆运动状态估计信号来预测本车运动轨迹并根据运动障碍物信息列表信号来预测动态障碍物的运动轨迹，通过计算本车与动态障碍物在预测运动轨迹上的碰撞时间来表征主车与动态障碍物碰撞风险的风险定量信号；

步骤二、进行地面风险评估，步骤二由四个环节完成，具体如下：环节一、进行静态行驶风险评估，根据地面几何参数信号、地面力学参数信号进行静态行驶风险评估，判断主车行驶路径是否存在无法跨越障碍物并计算车辆静态可行驶概率；

环节二、运算触发计算，判断静态可行驶概率是否达到触发条件，根据静态可行驶概率和触发条件的关系得到计算触发标志；

环节三、进行动态行驶风险评估：若静态可行使概率达到触发标志，则根据地面类别信号采取软、硬路面计算模式对车辆运动状态估计信号、地面几何参数信号和地面力学参数信号进行动态行驶风险评估；定义由于车辆动力不足引起的爬坡失效、因地面力学特性引起的爬坡失效以及车辆倾覆的量化危险判别指标，并根据评价指标量化动态可行驶概率；

环节四、进行概率整合：对静态可行驶概率和动态可行驶概率进行概率整合来得到唯一的、用于表征主车在地面上行驶失效风险的风险概率信号。

2.根据权利要求1所述的一种基于车路作用机理的越野环境中车辆行驶风险预测方法，其特征在于：所述的第三步中对环境状态与环境参数进行估计的过程如下：步骤一、进行运动障碍物检测：将多模态环境传感器对齐信号中的前方障碍物运动信息利用车辆运动状态估计结果进行坐标系转换；提取其中的运动障碍物并进行运动跟踪，输出包括障碍物的位置、障碍物的速度、障碍物的加速度、障碍物的三维尺寸、障碍物的动/静状态的运动障碍物信息列表信号传至第四步；

步骤二、进行地面提取：将多模态环境传感器对齐信号中的彩色图像和三维点云进行地面数据提取，生成多模态环境传感地面信号；

步骤三、进行地面几何特征估计：对地面三维点云数据进行直角坐标栅格化，将车辆前方视野一相同大小的矩形栅格描述，之后以最小二乘法进行平面拟合计算栅格平面法向量，利用法向量估计栅格内的地面纵向和侧向坡角的地面几何特征并用0和1之间的数值表示地面纵向和侧向坡角计算结果的可信程度即置信度，利用车辆运动状态估计结果将坡角估计值转换到大地坐标系下，生成以栅格为单位的地面几何参数信号并传至第四步；

步骤四、进行地面类别识别：利用地面的彩色图像数据和车辆运动状态估计信号对地面类别进行识别和识别置信度的评价，对地面类别识别结果进行融合，融合手段包括利用基于规则和基于概率推理的方式，对识别结果进行决策融合；融合手段还包括使用基于车辆动力学的识别结果对基于图像的识别结果进行自监督学习，即不断利用动力学识别结果生成新采集的图像数据的标签，并用新采集的图像及标签重新训练基于图像识别的算法，利用重新训练的算法进行地面类别识别，地面类别识别结果最终使用与地面几何参数信号一致的栅格格式表示为地面类别信号并传至第四步；

步骤五、进行地面力学特征估计：利用车辆状态估计信号和地面类别信号进行地面力学参数的识别，识别的地面力学参数包括硬路面的路面附着系数，以及软路面的地面剪切特性参数土壤粘聚力、土壤摩擦角和剪切力‑变形曲线模量，接着对连续识别的地面力学参数进行高斯混合模型描述，最终生成地面力学参数信号并传至第四步。

说明书

一种基于车路作用机理的越野环境中车辆行驶风险预测方法

技术领域

[0001] 本发明涉及一种车辆行驶风险预测方法，具体涉及一种基于车路作用机理的越野环境中车辆行驶风险预测方法。

背景技术

[0002] 目前，智能汽车已经成为了主流的汽车技术发展方向之一，汽车智能化技术也在各种不同的汽车应用领域得到发展。车辆行驶环境的行驶风险预测，可以为车辆的自动驾
驶决策与规划提供依据，也可以为人类驾驶员提供丰富的驾驶建议。在越野行驶环境中，除
了包含动静态障碍物以外，地面本身也具有复杂多变的特性，如果对越野路面参数和车路
作用机理欠缺足够的预判即进行车辆自动行驶或人类驾驶，其过程中产生的不恰当的行驶
方式可能导致车辆通行失败甚至引发车辆碰撞、倾覆等安全问题。因此针对越野场景的可
行驶性预测技术对于实现越野环境下的智能汽车技术十分关键。

[0003] 越野条件下的地面可行驶性预测技术具备以下几个特点。首先，路面可行驶性需基于丰富的道路参数。越野路面往往欠缺规则的道路边界，路面几何形状不规则，地面材
质、力学特征复杂多变，这使车路作用过程变得复杂。目前已有多种传感信息可用于环境状
态的估计，它们各有利弊。车辆状态是地面力学条件在车辆上的真实响应，但是车辆状态不
能反映未来以及未驶过路面的特性。彩色图像能够采集周围环境的材质和纹理变化，但是
不直接反映路面的几何特征。激光雷达或双目相机生成的三维点云可以体现环境的几何特
征，但是其中却包含有限的材质信息。路面可行驶性预测需要综合道路的材质、几何和力学
特征，因此需要基于不同行驶的传感信息进行综合道路参数估计与分析，也需要采用搭载
多种传感设备的系统来实现功能。其次，路面特征的多变性和传感器精度决定了道路参数
估计结果具有一定的随机性，这使得对路面可行驶性进行概率性建模变得必要。最后，在实
际车辆行驶时，车辆的操作会影响路段的可行驶性，如车辆在高速行驶时可以冲过低速条
件下本无法通过的坡路，所以路面可行驶性评价也应考虑车辆的操作和行驶状态。

[0004] 目前，国内外的团队已经探索了行驶风险预测相关的方法。但是现有的技术仍存在一定不足。首先，针对越野行驶环境的技术较少，大部分技术仍然面向具有车道线的结构
化道路。其次，车路作用机理在行驶风险预测方面的挖掘仍不够深入，采用的多源传感信息
仍然以激光雷达和相机为主，对车辆对地面的响应探索不足。最后，将车辆的操作和运行动
态作为影响因素的技术较少，大部分行驶风险的定义是基于环境参数的静态属性，并没有
考虑车辆这一多体系统的动态属性。

[0005] 中国专利CN201811290887.7公开了一种基于视觉与激光雷达传感器的车辆可通过性检测系统，建立了环境与车辆的三维几何模型，通过模拟车辆未来行驶轨迹，根据通过
性几何参数计算车辆的可通过性。中国专利CN201611101906.2公开了一种根据周围环境数
据、地势高度地图和前方车辆轨迹加权得到概率性的路段可行驶性评价信号。中国专利
CN201780026201.0公开了一种根据数字地图识别周围环境中可下穿对象的可行驶性的方
法。上述三项专利只能实现基于周围环境障碍物信息和几何信息的行驶风险预测，并不针
对越野行驶环境，无法基于车路作用机理进行预测，对车辆行驶风险的定义也局限于静态
的特征，无法从本质上解决越野条件下的环境可驶性预测与评价问题。

发明内容

[0006] 本发明的目的是为了确保智能车辆进行风险预测时，能够针对越野场景的特点，克服现有方法中对考虑车‑路作用机理考虑不足以及对车辆操纵与车辆状态考虑不足的问
题，进而提供一种基于车路作用机理的越野环境中车辆行驶风险预测方法。

[0007] 本发明提供的基于车路作用机理的越野环境中车辆行驶风险预测方法，其方法如下所述：

[0008] 第一步、数据预处理；

[0009] 第二步、对车辆运动状态进行估计；

[0010] 第三步、对环境状态与环境参数进行估计；

[0011] 第四步、对行驶风险进行预测。

[0012] 第一步中数据预处理的过程如下:

[0013] 步骤一、进行数据采集：采集智能车辆上搭载的车载CAN总线、卫星‑惯导组合定位系统、相机、激光雷达和毫米波雷达的传感器信号，按照固定采集周期对所有传感器数据进
行采集，所采集的传感器数据形式包括：彩色相机产生的以帧为单位的彩色图像，激光雷达
产生的以帧为单位的空间三维点云，毫米波雷达产生的以帧为单位的障碍物状态列表，车
载CAN总线上产生的以时间序列表示的车辆操纵与运动状态数据，卫星‑惯导组合定位系统
产生的以时间序列表示的定位与姿态数据；

[0014] 步骤二、对数据进行校验和对齐，具体内容包括：传感器数据的时间对齐；传感器数据的空间对齐；传感器数据的有效性校验；生成带有有效性标记、经过时间和空间对齐的
车载总线对齐信号，车辆状态测量对齐信号和多模态环境传感器对齐信号，供后续步骤使
用。

[0015] 第二步中对车辆运动状态进行估计的过程如下：

[0016] 步骤一、进行卫星‑惯导组合定位系统估计。利用车辆状态测量对齐信号，根据车辆动力学，对包括车辆的纵侧向速度、车辆的纵侧向加速度、垂向加速度和车轮载荷的车辆
动力学状态进行估计；利用车辆状态测量对齐信号，对包括俯仰角和侧倾角在内的车辆姿
态进行估计；利用车辆状态测量对齐信号，对车辆的位置进行估计。

[0017] 步骤二、进行动力学状态估计。通过读取车载总线对齐信号并进行滤波处理得到包括车辆的操纵状态，包括踏板开度、方向盘转角和车辆是否处于静止状态的车辆的操纵
状态；通过读取车载总线对齐信号结合车辆动力学，估计包括车辆纵向速度、侧向速度、车
辆纵向加速度、侧向加速度、垂向加速度和车轮载荷在内的车辆动力学状态；通过读取车载
总线对齐信号，通过滤波估计包括制动压力、挡位和ABS触发状态在内的车辆部件状态；读
取车载总线对齐信号和车辆状态测量对齐信号，根据车辆动力学计算车轮滑转率这一车辆
动力学信号。

[0018] 步骤三、进行同时建图与定位。通过同时建图与定位方法根据多模态环境传感器对齐信号，估计包括车辆纵向速度、侧向速度、车辆纵向加速度、侧向加速度、垂向加速度和
车轮载荷在内的车辆动力学信号；通过同时建图与定位方法根据多模态环境传感器对齐信
号，估计包括车辆俯仰角和侧倾角在内的车辆姿态；通过同时建图与定位方法根据多模态
环境传感器对齐信号，对车辆的位置进行估计。

[0019] 步骤四、状态估计融合。评估卫星‑惯导组合定位系统估计、动力学状态估计和同时建图与定位各自的可靠程度，将由不同步骤，即不同估计原理得到的相同物理意义的估
计结果进行融合。对分别由卫星‑惯导组合定位系统估计、动力学状态估计和同时建图与定
位得到的车辆动力学状态进行融合；对分别由卫星‑惯导组合定位系统估计和同时建图与
定位得到的车辆姿态进行融合；对分别由卫星‑惯导组合定位系统估计和同时建图与定位
得到的车辆位置进行融合。融合方法包括但不限于：卡尔曼滤波法或线性加权法。

[0020] 第三步中对环境状态与环境参数进行估计的过程如下：

[0021] 步骤一、进行运动障碍物检测，将多模态环境传感器对齐信号中的前方障碍物运动信息利用车辆运动状态估计结果进行坐标系转换；提取其中的运动障碍物并进行运动跟
踪，输出包括障碍物的位置、障碍物的速度、障碍物的加速度、障碍物的三维尺寸、障碍物的
动/静状态的运动障碍物信息列表信号传至第四步；

[0022] 步骤二、进行地面提取，将多模态环境传感器对齐信号中的彩色图像和三维点云进行地面数据提取，生成多模态环境传感地面信号；

[0023] 步骤三、进行地面几何特征估计，对地面三维点云数据进行直角坐标栅格化，将车辆前方视野一相同大小的矩形栅格描述，之后以最小二乘法进行平面拟合计算栅格平面法
向量，利用法向量估计栅格内的地面纵向和侧向坡角的地面几何特征并用0和1之间的数值
表示地面纵向和侧向坡角计算结果的可信程度即置信度，利用车辆运动状态估计结果将坡
角估计值转换到大地坐标系下，生成以栅格为单位的地面几何参数信号并传至第四步；

[0024] 步骤四、进行地面类别识别，利用地面的彩色图像数据和车辆运动状态估计信号对地面类别进行识别和识别置信度的评价，对地面类别识别结果进行融合，融合手段包括
利用基于规则和基于概率推理的方式，对识别结果进行决策融合；融合手段还包括使用基
于车辆动力学的识别结果对基于图像的识别结果进行自监督学习，即不断利用动力学识别
结果生成新采集的图像数据的标签，并用新采集的图像及标签重新训练基于图像识别的算
法，利用重新训练的算法进行地面类别识别，地面类别识别结果最终使用与地面几何参数
信号一致的栅格格式表示为地面类别信号并传至第四步；

[0025] 步骤五、进行地面力学特征估计：利用车辆状态估计信号和地面类别信号进行地面力学参数的识别，识别的地面力学参数包括硬路面的路面附着系数，以及软路面的地面
剪切特性参数土壤粘聚力、土壤摩擦角和剪切力‑变形曲线模量，接着对连续识别的地面力
学参数进行高斯混合模型描述，最终生成地面力学参数信号并传至第四步。

[0026] 第四步中对行驶风险进行预测的过程如下：

[0027] 步骤一、进行运动障碍物风险评估：利用车辆运动状态估计信号来预测本车运动轨迹并根据运动障碍物信息列表信号来预测动态障碍物的运动轨迹，通过计算本车与动态
障碍物在预测运动轨迹上的碰撞时间来表征主车与动态障碍物碰撞风险的风险定量信号；

[0028] 步骤二、进行地面风险评估，步骤二由四个环节完成，具体如下：

[0029] 环节一、进行静态行驶风险评估，根据地面几何参数信号、地面力学参数信号进行静态行驶风险评估，判断主车行驶路径是否存在无法跨越障碍物并计算车辆静态可行驶概
率；

[0030] 环节二、运算触发计算，判断静态可行驶概率是否达到触发条件，根据静态可行驶概率和触发条件的关系得到计算触发标志。

[0031] 环节三、进行动态行驶风险评估。若静态可行使概率达到触发标志，则根据地面类别信号采取软、硬路面计算模式对车辆运动状态估计信号、地面几何参数信号和地面力学
参数信号进行动态行驶风险评估；定义由于车辆动力不足引起的爬坡失效、因地面力学特
性引起的爬坡失效以及车辆倾覆的量化危险判别指标，并根据评价指标量化动态可行驶概
率；

[0032] 环节四、进行概率整合。对静态可行驶概率和动态可行驶概率进行概率整合来得到唯一的、用于表征主车在地面上行驶失效风险的风险概率信号。

[0033] 本发明的有益效果：

[0034] 本发明提供了一种评估和预测越野环境中车辆行驶的风险的方法。本发明中涉及的越野环境包含水泥路、土路、沙地、雪地、碎石路和冰路等不同材质道路，涵盖平路、纵坡
和横坡等路面几何结构，地面上存在静止障碍物和运动障碍物。本发明涵盖多种形式的车
辆行驶风险，包括但不局限于车辆与碍物之间的碰撞、因车辆动力不足引起的爬坡失效、因
地面力学特性引起的爬坡失效以及车辆的倾覆。本发明结合了车辆动力学、地面力学、计算
机视觉等学科的技术，通过结合前方道路的几何和力学特征进行风险的静、动态分层预测
的方式，得以用基于车‑路作用的最根本的原理解决车辆行驶风险预测问题，并且能够结合
多学科的先进技术手段，符合自动驾驶技术发展趋势，具有广泛的应用前景和多场景、多工
况下的可行性。

附图说明

[0035] 图1为本发明所述的在越野环境中预测车辆行驶风险的方法的整体步骤说明。

[0036] 图2为本发明所述的在越野环境中预测车辆行驶风险的方法架构框图。

[0037] 图3为本发明所述的第一步的示例性架构框图。

[0038] 图4为本发明所述的第一步涉及的数据形式示意图。

[0039] 图5为本发明所述的第二步的示例性架构框图。

[0040] 图6为本发明所述的第三步的示例性架构框图。

[0041] 图7为本发明所述的第四步的示例性架构框图。

[0042] 图8为本发明所述的地面风险评估的算法流程图。

[0043] 图9为本发明所述的地面风险评估的示例性架构框图。

[0044] 图10为本发明所述的风险定量信号运算结果示例。

[0045] 图11为本发明所述的风险概率信号结果示例。

具体实施方式

[0046] 请参阅图1至图11所示：

[0047] 本发明提供的越野环境中预测车辆行驶风险的方法，其方法如下所述：

[0048] 第一步、数据预处理；

[0049] 第二步、对车辆运动状态进行估计；

[0050] 第三步、对环境状态与环境参数进行估计；

[0051] 第四步、对行驶风险进行预测。

[0052] 第一步中数据预处理的过程如下:

[0053] 步骤一、进行数据采集。采集智能车辆上搭载的车载CAN总线、卫星‑惯导组合定位系统、相机、激光雷达和毫米波雷达的传感器信号，按照固定采集周期对所有传感器数据进
行采集，所采集的传感器数据形式包括：彩色相机产生的以帧为单位的彩色图像，激光雷达
产生的以帧为单位的空间三维点云，毫米波雷达产生的以帧为单位的障碍物状态列表，车
载CAN总线上产生的以时间序列表示的车辆操纵与运动状态数据，卫星‑惯导组合定位系统
产生的以时间序列表示的定位与姿态数据；

[0054] 步骤二、对数据进行校验和对齐。具体内容包括：传感器数据的时间对齐；传感器数据的空间对齐；传感器数据的有效性校验；生成带有有效性标记、经过时间和空间对齐的
车载总线对齐信号，车辆状态测量对齐信号和多模态环境传感器对齐信号，供后续步骤使
用。在图3中示出第一步的一示例性实施方式。第一步对输入的传感器信号车载总线信号、
车辆状态测量信号和多模态环境传感信号实现初步的处理，处理方式包括但不限于对信号
质量的评价和信号的对齐。多模态数据指的是多种信号源模式及信号传递状态的数据，在
本发明中具体包括由激光雷达通过以太网线传输的三维点云、彩色相机通过USB传输的彩
色图像和毫米波雷达经过CAN总线传输的障碍物状态列表。三维点云指采集的三维空间数
据，以三维空间点集合形成的点云形式采集得到。多模态数据的基本形式和传输方式如附
图4所述。为实现第一步的功能，依次通过有效性校验、时间对齐和空间对齐，对车载总线信
号、车辆状态测量信号和多模态环境传感信号进行处理。

[0055] 第一步的步骤一通过读取传感器发出的信号所得到的信号包括：用于反映车辆行驶状态和操纵状态的车载总线信号，由卫星定位系统、惯性导航仪和惯性测量单元产生的
包含车辆位置、运动与姿态的车辆状态测量信号，以及由激光雷达、彩色相机和毫米波雷达
共同产生的多模态环境传感信号。

[0056] 多模态环境传感信号以激光雷达、彩色相机和毫米波雷达按数据帧采集生成。本发明中所述的方法要求最终产生的多模态环境传感信号包含的信息为：彩色相机生成的环
境彩色图像，激光雷达生成的环境三维点云和毫米波雷达生成的前方障碍物运动信息列
表。

[0057] 步骤二中，首先通过有效性校验实现信号的有效性检查，分别读取车载总线信号、车辆状态测量信号和多模态环境传感信号的数值并评估它们是否是在传感器正常工作时
采集生成，用一个整数标记体现，正值表示传感器工作正常，负值表示异常；若判断出输入
信号异常，则在第一步的输出信号车载总线对齐信号、车辆状态测量对齐信号和多模态环
境传感对齐信号中将整数标记设置为负数。

[0058] 其次，通过时间对齐协调输入信号的采集周期，使得输出信号实现时间对齐。对齐策略包括但不限于：提取数据缓存区的最新数据，或者对输入信号加入内部计时、计数标
记，或对输入信号加入GPS时间等全局时间戳。

[0059] 最后，通过空间对齐对输入信号在空间上进行同步，具体方法包括但不限于：将车辆状态测量信号转换到车辆坐标系下，消除多模态传感信号中三维点云由于旋转式测量的
机理和车辆运动带来的点云缩放畸变。

[0060] 第一步最终生成具备有效性标记、经过时间和空间对齐的车载总线对齐信号，车辆状态测量对齐信号和多模态环境传感器对齐信号。

[0061] 第二步中对车辆运动状态进行估计的过程如下：

[0062] 步骤一、进行卫星‑惯导组合定位系统估计。利用车辆状态测量对齐信号，根据车辆动力学，对包括车辆的纵侧向速度、车辆的纵侧向加速度、垂向加速度和车轮载荷的车辆
动力学状态进行估计；利用车辆状态测量对齐信号，对包括俯仰角和侧倾角在内的车辆姿
态进行估计；利用车辆状态测量对齐信号，对车辆的位置进行估计。

[0063] 步骤二、进行动力学状态估计。通过读取车载总线对齐信号并进行滤波处理得到包括车辆的操纵状态，包括踏板开度、方向盘转角和车辆是否处于静止状态的车辆的操纵
状态；通过读取车载总线对齐信号结合车辆动力学，估计包括车辆纵向速度、侧向速度、车
辆纵向加速度、侧向加速度、垂向加速度和车轮载荷在内的车辆动力学状态；通过读取车载
总线对齐信号，通过滤波估计包括制动压力、挡位和ABS触发状态在内的车辆部件状态；读
取车载总线对齐信号和车辆状态测量对齐信号，根据车辆动力学计算车轮滑转率这一车辆
动力学信号。

[0064] 步骤三、进行同时建图与定位。通过同时建图与定位方法根据多模态环境传感器对齐信号，估计包括车辆纵向速度、侧向速度、车辆纵向加速度、侧向加速度、垂向加速度和
车轮载荷在内的车辆动力学信号；通过同时建图与定位方法根据多模态环境传感器对齐信
号，估计包括车辆俯仰角和侧倾角在内的车辆姿态；通过同时建图与定位方法根据多模态
环境传感器对齐信号，对车辆的位置进行估计。

[0065] 步骤四、状态估计融合。评估卫星‑惯导组合定位系统估计、动力学状态估计和同时建图与定位各自的可靠程度，将由不同步骤，即不同估计原理得到的相同物理意义的估
计结果进行融合。对分别由卫星‑惯导组合定位系统估计、动力学状态估计和同时建图与定
位得到的车辆动力学状态进行融合；对分别由卫星‑惯导组合定位系统估计和同时建图与
定位得到的车辆姿态进行融合；对分别由卫星‑惯导组合定位系统估计和同时建图与定位
得到的车辆位置进行融合。融合方法包括但不限于：卡尔曼滤波法或线性加权法。

[0066] 在附图5中示出第二步的一示例性实施方式。

[0067] 第三步中对环境状态与环境参数进行估计的过程如下：

[0068] 步骤一、进行运动障碍物检测。将多模态环境传感器对齐信号中的前方障碍物运动信息利用车辆运动状态估计结果进行坐标系转换；提取其中的运动障碍物并进行运动跟
踪，输出包括障碍物的位置、障碍物的速度、障碍物的加速度、障碍物的三维尺寸、障碍物的
动/静状态的运动障碍物信息列表信号传至第四步；

[0069] 步骤二、进行地面提取。将多模态环境传感器对齐信号中的彩色图像和三维点云进行地面数据提取，生成多模态环境传感地面信号；

[0070] 步骤三、进行地面几何特征估计。对地面三维点云数据进行直角坐标栅格化，将车辆前方视野一相同大小的矩形栅格描述，之后以最小二乘法进行平面拟合计算栅格平面法
向量，利用法向量估计栅格内的地面纵向和侧向坡角的地面几何特征并用0和1之间的数值
表示地面纵向和侧向坡角计算结果的可信程度即置信度，利用车辆运动状态估计结果将坡
角估计值转换到大地坐标系下，生成以栅格为单位的地面几何参数信号并传至第四步；

[0071] 步骤四、进行地面类别识别。利用地面的彩色图像数据和车辆运动状态估计信号对地面类别进行识别和识别置信度的评价，对地面类别识别结果进行融合，融合手段包括
利用基于规则和基于概率推理的方式，对识别结果进行决策融合；融合手段还包括使用基
于车辆动力学的识别结果对基于图像的识别结果进行自监督学习，即不断利用动力学识别
结果生成新采集的图像数据的标签，并用新采集的图像及标签重新训练基于图像识别的算
法，利用重新训练的算法进行地面类别识别，地面类别识别结果最终使用与地面几何参数
信号一致的栅格格式表示为地面类别信号并传至第四步；

[0072] 步骤五、进行地面力学特征估计。利用车辆状态估计信号和地面类别信号进行地面力学参数的识别，识别的地面力学参数包括硬路面的路面附着系数，以及软路面的地面
剪切特性参数土壤粘聚力、土壤摩擦角和剪切力‑变形曲线模量，接着对连续识别的地面力
学参数进行高斯混合模型描述，最终生成地面力学参数信号并传至第四步。

[0073] 在附图6中示出第三步的一示例性实施方式。第三步根据多模态环境传感对齐信号和车辆运动状态估计信号实现环境状态与参数的估计。通过地面提取、障碍物运动检测、
地面几何特征估计和地面类别识别得到第三步的输出信号，包括运动障碍物信息列表信
号，地面几何参数信号，地面类别信号和地面力学参数信号。

[0074] 障碍物运动检测采用多模态环境传感对齐信号中的前方障碍物运动信息列表，利用车辆运动状态估计信号中的车辆姿态和车辆速度将障碍物状态信息转换到车辆和大地
坐标系下，提取其中的处于绝对运动状态的障碍物，将这些障碍物的信息构造障碍物运动
检测的输出。障碍物运动检测生成的信息最终存在于运动障碍物信息列表信号，其内容包
括障碍物的位置、障碍物的速度、障碍物的加速度、障碍物的三维尺寸、障碍物的动/静状
态。

[0075] 地面提取将多模态环境传感对齐信号中的彩色图像和三维点云进行越野路地面区域数据的分割，分割方法分别为针对图像的深度学习分割模型和针对点云的深度学习分
割模型。最终提取出包含地面彩色图像和地面三维点云的多模态环境传感地面信号。

[0076] 地面几何特征估计、地面类别识别和地面力学特征估计基于多模态环境传感地面信号实现。地面几何特征估计基于多模态环境传感地面信号中的地面点云信息，对驾驶环
境中前方40m*10m范围内地面纵向、侧向坡角以栅格地图为单位进行估计。单位栅格尺寸为
0.5m*0.5m，在激光雷达坐标系下，通过基于最小二乘的平面拟合的方法对单位栅格内点云
的法向量进行求解，将法向量分别沿栅格地图纵向和侧向投影得到激光雷达坐标系下的道
路纵向和侧向坡角。统计栅格内点云与法向量对应的平面之间的距离信息，利用经验值将
统计出来的距离信息和参考值计算平面拟合过程中的估计置信度。

[0077] 基于车辆运动状态估计信号中的车姿信息，将激光雷达坐标系下的坡角修正到地面坐标系下，修正后的地面纵向、侧向坡角和估计置信度，构成了地面几何参数信号。地面
几何特征估计能够涵盖平路、纵坡和横坡的地面几何结构。

[0078] 求解地面纵侧向坡角的方式如下所述。针对每个栅格n，在激光雷达坐标系内对点云利用最小二乘法进行平面拟合，得到满足cn>0的法向量nn＝(an,bn,cn)。

[0079] 激光雷达坐标系L下的纵向坡角 由下列公式求解：

[0080] n′n＝nn‑nxoznn·nxoz

[0081]

[0082] 式中，nxoz为激光雷达坐标系下xL ozL平面的法向量，n′n是nn向xLozL平面的投影向量，nxoy为激光雷达坐标系下xL oyL平面的法向量。

[0083] 激光雷达坐标系L下的纵向坡角 由下列公式求解：

[0084] n″n＝nn‑nyoznn·nyoz

[0085]

[0086] 式中，nyoz为激光雷达坐标系下yL ozL平面的法向量，n″n是nn向yL ozL平面的投影向量，nxoz为激光雷达坐标系下xL ozL平面的法向量。

[0087] 车辆行驶过程中影响车辆受力的主要是大地坐标系G下的坡角，根据车辆对地面的俯仰角 和横滚角 对激光雷达坐标系下求解的坡角进行补偿得到大地坐标系下的栅
格纵向坡角 和侧向坡角

[0088]

[0089]

[0090] 地面类别识别利用基于多模态环境传感地面信号中的地面彩色图像信息和车辆运动状态估计信号实现地面类别的识别，输出信号存在于地面类别信号，输出信号以与地
面几何参数信号格式相同的栅格地图形式进行表示。地面类别信号中用整数值表征地面的
类别，同时用[0,1]之间的小数描述地面类别的识别置信度。地面类别信号能够表征的地面
类别包括水泥路、土路、沙地、雪地、碎石路和冰路。

[0091] 本发明专利中涉及地面类别识别的两种实现方案。

[0092] 第一种实现地面类别识别的方案是，利用有监督机器学习方法，即利用带有数据真值进行训练而得到的统计模型，分别实现基于多模态环境传感对齐信号的地面类别识别
和基于车辆运动状态估计信号中车辆加速度信息的地面类别识别，接着对上述两种识别结
果进行融合，生成统一的地面类别识别结果，并推算出相应的置信度。融合方案包括：基于
规则的融合方法和基于概率推理的融合方法。基于概率推理的融合方法包括隐马尔可夫模
型和贝叶斯网络推理。

[0093] 第二种实现地面类别识别的方案是通过自监督学习的方法。在离线条件下粗略训练一种基于图像的地面类别分类算法，在车辆行驶过程中始终记录多模态环境传感地面信
号中的地面彩色图像和车辆驶过相应路段时基于车辆运动状态估计信号的地面类别识别
结果作为补充数据集，利用补充数据集对粗略训练的分类算法在线进行增量的训练，最终
生成唯一的地面类别信息，体现为地面类别信号。

[0094] 地面力学特征估计利用车辆运动状态估计信号和地面类别信号实现地面力学参数的识别，输出信号存在于地面力学参数信号。地面力学参数信号包括用高斯混合模型描
述路面附着系数和地面剪切特性参数。地面力学特征估计首先读取地面类别信号，利用地
面类别信号初始化高斯混合模型，接着利用后续读取10次地面类别信号的数值在线更新高
斯混合模型，作为附着系数的识别结果。地面力学特征估计读取车辆运动状态估计信号，利
用地面力学和车辆动力学方法估计地面剪切特性参数，并利用该估计结果初始化高斯混合
模型，接着利用后续读取10次估计结果在线更新高斯混合模型，作为地面剪切特性参数的
识别结果。

[0095] 用高斯混合模型描述地面力学特征参数的方法为：

[0096] 将读取的10次估计结果记作样本z,设定高斯混合数N，即将数据抽象为Y＝{1,2,…,N}的所有可能的高斯混合类别，并将类别标记记为y∈Y。假设样本由高斯混合模型生
成，且每个类别对应一个高斯混合成分。最终，地面力学参数是基于如下概率密度生成：

[0097]

[0098] 其中，混合系数 p(z|μi,Σi)是样本z属于第i个高斯混合成分的概率；μi和∑i为该高斯混合成分的参数。在线估计高斯混合模型参数的方法为需要进
行E步和M步计算。

[0099] 首先利用地面类别初始化得到力学参数的经验分布，根据该分布随机采样，采样数为l,作为有标记样本集Dl＝{(z1,y1),(z2,y2),…,(zl,yl)}，地面力学参数估计结果作为
样本数量为u的未标记样本集Du＝{zl+1,zl+2,…,zl+u}。将l取为5，将u取为10。

[0100] 接着展开E步，根据当前模型参数计算未标记样本zj属于各高斯混合成分的概率：

[0101]

[0102] 然后展开M步，基于γji更新模型参数，其中li表示第i类的有标记样本数目，由以下公式实现：

[0103]

[0104]

[0105]

[0106] 最后循环迭代E步和M步实现高斯混合模型的更新。

[0107] 第四步中对行驶风险进行预测的过程如下：

[0108] 步骤一、进行运动障碍物风险评估。利用车辆运动状态估计信号来预测本车运动轨迹并根据运动障碍物信息列表信号来预测动态障碍物的运动轨迹，通过计算本车与动态
障碍物在预测运动轨迹上的碰撞时间来表征主车与动态障碍物碰撞风险的风险定量信号；

[0109] 步骤二、进行地面风险评估。步骤二由四个环节完成。

[0110] 环节一、进行静态行驶风险评估，根据地面几何参数信号、地面力学参数信号进行静态行驶风险评估，判断主车行驶路径是否存在无法跨越障碍物并计算车辆静态可行驶概
率；

[0111] 环节二、运算触发计算，判断静态可行驶概率是否达到触发条件，根据静态可行驶概率和触发条件的关系得到计算触发标志。

[0112] 环节三、进行动态行驶风险评估。若静态可行使概率达到触发标志，则根据地面类别信号采取软、硬路面计算模式对车辆运动状态估计信号、地面几何参数信号和地面力学
参数信号进行动态行驶风险评估；定义由于车辆动力不足引起的爬坡失效、因地面力学特
性引起的爬坡失效以及车辆倾覆的量化危险判别指标，并根据评价指标量化动态可行驶概
率；

[0113] 环节四、进行概率整合。对静态可行驶概率和动态可行驶概率进行概率整合来得到唯一的、用于表征主车在地面上行驶失效风险的风险概率信号。

[0114] 在附图7中示出第四步的一示例性实施方式。第四步中进行行驶风险预测，最终得到表征主车与动态障碍物碰撞风险的风险定量信号和表征主车在地面上行驶失效风险的
风险概率信号，由运动障碍物风险评估和地面风险评估共同完成。本发明中方法所产生的
风险定量信号采用离散分级的形式量化行驶风险的程度。风险概率信号采用0到1之间的数
值描述未来车辆出现符合失效条件定义的失效形态的可能性大小。风险定量信号和风险概
率信号可用于对人类驾驶员的驾驶警示和自动驾驶系统的轨迹规划。

[0115] 一方面，由运动障碍物风险评估根据运动障碍物信息列表信号和车辆运动状态估计信号，计算得到风险定量信号。运动障碍物风险评估利用信号中运动障碍物的运动状态，
根据动力学模型以100毫秒为预测步长预测障碍物在未来5秒时间范围内的运动轨迹；利用
车辆运动状态估计信号中本车的运动状态，根据动力学模型以100毫秒为预测步长预测本
车在未来10秒时间范围内的运动轨迹。预测时在车辆纵向和侧向采用常加速度模型：

[0116]

[0117] 式中T代表预测步长，wa代表过程噪声系数，根据实际工况选取。xt、vt和at代表当前预测步长纵向和侧向的位移、速度和加速度，xt+1、vt+1和at+1代表下一预测步长的纵向和
侧向的位移、速度和加速度。

[0118] 在每一个预测步长上，为障碍物和本车建立二维轮廓模型，判断两个轮廓模型是否相交，相交的时刻定义为二者的碰撞时间。将碰撞时间在10秒范围内划分为5个区间，0至
1s定义为重度危险区间，在风险定量信号中用数值5表示；1至2秒定义为严重危险区间，在
风险定量信号中用数值4表示；2至4秒定义为中度危险区间，在风险定量信号中用数值3表
示；4至6秒定义为轻度危险区间，在风险定量信号中用数值2表示；6秒以上或者预测范围内
没有碰撞发生的情形，定义为轻微危险，在风险定量信号中用数值1表示。

[0119] 另一方面，由地面风险评估步骤根据车辆运动状态估计信号、地面几何参数信号、地面类别信号和地面力学参数信号计算得到风险概率信号。

[0120] 地面风险评估的一示例性实施方案如附图8和附图9所示。地面风险评估由静态行驶风险评估、动态行驶风险评估、运算触发计算和概率整合四个环节实现。

[0121] 首先，静态行驶风险评估根据地面几何参数信号中坡角的数值判断主车前方行驶路径上的栅格是否存在无法跨越的障碍物，若存在，则将静态行驶风险评估的输出静态可
行驶概率设置为0；若不存在不可跨越的障碍物，则通过地面几何参数和地面力学信号通过
解析行驶的车辆倾覆模型和车辆驱动力模型计算静态可行驶概率。

[0122] 接着，运算触发计算读取静态可行驶概率，若静态可行驶概率数值小于切换阈值0.6，则运算触发计算的输出计算触发标志设置为1，否则设置为0。

[0123] 然后，动态行驶风险评估实时读取计算触发标志，当计算触发标志数值为0时，则启动运算，动态行驶风险评估输出的动态可行驶概率取值为1；否则，动态可行驶概率的数
值以动态行驶风险评估的动态可行驶概率计算结果为准。运算过程为：首先，根据地面类别
信号定义运算模式，若地面类别信号中表明主车前方行驶地面为水泥路、土路、碎石路或冰
路，则实施硬路面计算模式；若地面类别信号中表明主车前方行驶路面为沙地或雪地，则实
施软路面计算模式。在硬路面计算模式下，核心运算步骤为：利用地面几何参数信号和地面
力学参数信号作为参数，车辆运动状态估计信号作为输入，根据车辆动力学模型以100毫秒
为预测步长迭代预测主车在未来5秒时间范围内的加速度变化，接着通过加速度变化推算
主车在未来5s内的运动状态和轨迹变化。在软路面计算模式下，核心运算步骤为：利用地面
几何参数信号和地面力学参数信号作为参数，车辆运动状态估计信号作为输入，根据地面
力学和车辆动力学模型以100毫秒为预测步长迭代预测主车在未来5秒时间范围内的挂钩
牵引力变化，进而通过挂钩牵引力变化推算主车在未来5秒时间范围内的加速度变化，最后
根据加速度变化推算主车在未来5s内的运动状态和轨迹变化。

[0124] 定义由于车辆动力不足引起的爬坡失效、因地面力学特性引起的爬坡失效以及车辆倾覆的量化危险判别指标。

[0125] 为了实现动态可行驶概率的运算，动态行驶风险评估结合上述核心运算步骤使用以下两种方案进行运算：

[0126] 方案一：利用蒙特卡洛法，将地面几何参数信号和地面力学参数信号中的概率参数进行1000次组合采样，利用每一次采样到的参数组合采样值作为参数进行核心运算。统
计1000次核心运算得到的车辆轨迹中，没有达到量化危险判别指标定义的危险状态的运算
次数占总的核心运算次数的百分比，作为动态可行驶概率的数值。

[0127] 方案二：根据不同的地面类别下地面力学参数信号的典型值，以多项式混沌模型构建考虑地面力学参数信号分布前提下的车辆动力学多项式展开模型。根据车辆运动状态
估计信号、地面几何参数信号和地面力学参数信号的数值，计算多项式展开模型中车辆动
力学状态随时间的变化规律，并根据概率分布计算其满足行驶安全，即没有达到量化危险
叛变指标定义的危险状态的概率值，作为动态可行驶概率的数值。

[0128] 最后，在地面风险评估中，通过概率整合步骤读取静态可行驶概率和动态可行驶概率，取静态可行驶概率和动态可行驶概率之间的较小值作为输出风险概率信号的取值。

[0129] 附图10和附图11分别为风险定量信号和风险概率信号的计算结果示例。