CN114812581A - 一种基于多传感器融合的越野环境导航方法 - Google Patents

Abstract

本发明公开了一种基于多传感器融合的越野环境导航方法，包括基于高分辨率卫星地图建立全局路网，根据任务文件的坐标信息使用Astar算法进行全局路径规划，得到全局参考路径；利用车载设备采集车载多传感器数据进行数据融合，融合后输入SLAM算法，且添加多种约束，使用基于图优化的框架求解获得精准位姿；再拼接多个环境观测，基于滤波融合算法构建越野地形图和全局占据栅格图；根据得到的全局参考路径实时提取车辆当前可行驶区域；当前可行驶区域内使用基于多属性评估的RRT路径规划算法生成局部最优路径进行避障导航。本发明通过利用多传感器融合进行精确定位和构建全局占据栅格图，达到在未知越野环境下进行快速精准避障导航。

Description

一种基于多传感器融合的越野环境导航方法

技术领域

本发明涉及多传感器融合导航技术领域，特别涉及一种基于多传感器融合的越野环境导航方法。

背景技术

SLAM（simultaneous localization and mapping，同时定位和建图）技术可以同时感知周围环境并估计自身位姿，并可实现未知环境下增量式地图构建，使自动驾驶车辆在自主导航时显著减少对卫星定位和先验地图的依赖。由于受越野环境的非结构化道路、不规则的环境特征、不稳定的卫星信号等因素影响，使用单一传感器数据进行车辆自主定位会面临着巨大的挑战；在地图的构建方面，越野环境信息繁杂、道路边界参差不齐，可通行区域难以提取，加大了构建导航地图的难度。

不同于城市道路中的二维导航，在越野环境下的无人车自主导航时，由于越野道路缺少车道线、边界等信息，常见的路径规划算法不能快速有效的生成路径。RRT（rapidlyexploring random tree，快速扩展随机树）是一种能够快速生成安全可行路径的路径规划算法，在机器人和自主驾驶方向得到了广泛的应用，在实现越野道路中的快速准确避障导航有很大的潜力。

现有技术的不足之处在于，目前在越野环境下的无人车自主导航，由于非结构化道路、缺乏导航信息，以及不稳定的卫星信息，会导致无法正常构建地图，完成导航任务，实现快速准确避障的作业。

发明内容

本发明的目的克服现有技术存在的不足，为实现以上目的，采用一种基于多传感器融合的越野环境导航方法，以解决上述背景技术中提出的问题。

一种基于多传感器融合的越野环境导航方法，具体步骤包括：

步骤S1、基于高分辨率卫星地图建立全局路网，根据任务文件的坐标信息使用Astar算法进行全局路径规划，得到全局参考路径；

步骤S2、利用车载设备采集车载多传感器数据进行数据融合，将融合的多模态数据输入SLAM算法中，且添加多种传感器约束，使用基于图优化的框架求解获得精准位姿；

步骤S3、利用获得的精准位姿拼接多个环境观测，基于滤波融合算法构建越野地形图和全局占据栅格图；

步骤S4、根据得到的全局参考路径实时提取车辆当前可行驶区域；

步骤S5、当前可行驶区域内使用基于多属性评估的RRT路径规划算法生成局部最优路径进行避障导航。

作为本发明的进一步的方案：所述步骤S1的具体步骤包括：

首先预设自主无人车辆的任务区域，根据基于高分辨率卫星地图的图片获取对应任务区域的任务地图；

根据获得的任务地图选取基准点GPS坐标，提取任务地图中所有道路信息建立全局路网；

根据导航任务文件中的起点、任务点，以及终点的坐标，使用Astar算法进行全局路径规划，获得全局路网中从起点到终点之间的节点拓扑信息；

再按照拓扑关系拼接初始全局路径，并使用贝塞尔曲线拟合算法对初始全局路径进行平滑处理，得到任务区域内的全局参考路径。

作为本发明的进一步的方案：所述步骤S2中利用车载设备采集车载多传感器数据进行数据融合的具体步骤包括：

通过车载工控机实时采集读取的多模态数据，所述多模态数据包括通过3D激光雷达采集周围环境信息得到的3D点云数据、通过IMU采集车辆的高频线加速度和角速度、通过轮式里程计采集的车辆速度，以及利用GPS采集当前车辆的实时经纬度坐标；

对采集的多模态数据进行预处理，包括:分别计算每个激光点的曲率，根据曲率赋予激光点的线或面的特征信息，并将点云聚类以剔除离散点；对通过IMU采集的加速度和角速度和轮式里程计采集的车辆速度的数据采用批处理构建预积分观测；将实时经纬度坐标转换成空间位置坐标；

将预处理后的多模态数据进行融合处理。

作为本发明的进一步的方案：所述步骤S2中将融合的多模态数据输入SLAM算法中，且添加多种传感器约束，使用基于图优化的框架求解获得精准位姿的具体步骤包括：

根据得到的多模态数据构建激光雷达点云残差、IMU和轮式里程计预积分残差、GPS先验残差，并建立非线性最小二乘问题；

再通过图优化的框架进行求解，获得精准的车辆位姿，最后将点云根据车辆位姿进行拼接，得到环境点云地图。

作为本发明的进一步的方案：所述步骤S3的具体步骤包括：

获取单帧的激光雷达点云数据并根据坡度进行障碍物标记，获取单帧栅格地图中的可通行区域数据，使用高斯过程推断不可观测的点云数据，最后将单帧的点云观测通过二值贝叶斯滤波更新到全局占据栅格地图；

根据最大高度标记单帧高程栅格地图的高程数据，利用卡尔曼滤波更新全局高程栅格地图。

作为本发明的进一步的方案：所述步骤S4的具体步骤包括：

根据得到的全局参考路径进行车辆行驶，并依据点云数据探测可通行路面区域，根据距离连续性指标和角度连续性指标舍弃不连续的路面；

所述距离连续性指标c _d 为：

；

式中，n为该指标当前迭代点前、后需要计算的点的个数，k的坐标从x-n到x+n表示该指标考虑当前点p _x 前n个点和后n个点，p _k 为第k个点，p _k+1为第k+1个点；

所述角度连续性指标c _a 为：

；

式中，n为该指标当前迭代点前后共需要计算的点的个数，p _x 表示当前迭代位置的点，p _x-k 表示当前迭代位置前面第k个点，p _x+k 表示当前迭代位置后面第k个点；

当c _d 小于距离连续性指标阈值c ^th _d ，且c _a 小于角度连续性指标阈值c ^th _a 时，则地面点为连续的，将该地面点纳入到可行驶区域内，同时提取的路面点的区域记为可行驶区域Ω ₁ ；

由于存在雷达无法直接探测到全部可行驶路面，首先对车辆周围实时的3D点云使用欧式距离方法进行聚类，得到车辆周围的障碍物簇；以障碍物簇为壁垒，沿着全局参考路径的垂直方向，向两侧使用洪水填充算法对前方区域进行扩散，形成车辆的可行驶区域Ω ₂ ；

最后计算两个可行驶区域的并集Ω ₁ ∪Ω ₂ 作为最终的可行驶区域。

作为本发明的进一步的方案：所述步骤S5的具体步骤包括：

根据得到的可行驶区域Ω作为路径规划的可行域Φ，计算出该可行域Φ在全局参考路径方向上的纵向长度L _Φ ，然后根据可行驶区域Ω的长度L _Ω 和最小规划距离L _p，min 选择路径规划的预瞄长度L _p ，其中，L _p，min ≤L _p <L _Ω ；

根据路径规划的预瞄长度L _p ，在全局参考路径中找到对应的全局路径坐标点，将该坐标点作为路径规划的预瞄点，以该预瞄点为圆心向四周探索，计算得到在规划可行域Φ内最大的圆C _p,goal ；

初始化随机树T的生长步长ρ，将车辆的后轴中心作为随机树T的初始节点q _start ，在圆C _p,goal 内随机选择N个坐标点作为随机树的目标节点q _goal ，进行计算得到一条无碰撞路径；

基于每一条路径对应的随机树，将随机树的节点作为贝塞尔曲线的控制点，使用贝塞尔曲线进行平滑处理，得到平滑的路径；使用曲线拟合算法对路径进行了平滑处理，有效优化了路径的质量。

对于每条候选路径，通过采用多属性评价函数且设置合理的权值，选出代价最小的路径作为当前最优路径，其中，所述多属性评价函数包括横向偏移的代价、候选路径曲率的代价，以及候选路径到障碍物的距离代价。

作为本发明的进一步的方案：所述随机树的目标节点q _goal ，进行计算得到一条无碰撞路径的具体步骤包括：

对每一个目标节点q ⁱ _goal ，以特定概率在规划可行域Φ中随机采样得到随机节点q ⁱ _rand ，选择随机节点q ⁱ _rand 的最近节点q ⁱ _near ，计算最近节点q ⁱ _near 的父节点与最近节点q ⁱ _near 之间形成的角度θ ⁱ _near 和最近节点q ⁱ _near 与随机节点q ⁱ _rand 之间形成的角度θ ⁱ _randnear ；采用RRT在生成新节点时考虑了节点间的连接角度，使RRT生成的路径更加平缓。

判断|θ ⁱ _near - θ ⁱ _randnear |是否小于节点角度阈值θ ^th ，若大于θ ^th ，则寻找另外的最近节点，直到满足小于节点角度阈值θ ^th ，则新节点q ⁱ _new 的计算方法为：

；

若q ⁱ _near 与q ⁱ _new 之间的连线没有障碍物，则将q ⁱ _near 作为q ⁱ _new 的父节点，q ⁱ _new 作为q ⁱ _near 的子节点，然后继续随机采样扩展随机树T；否则舍弃当前新节点q ⁱ _new ，重新随机采样；

循环搜索，当随机树T新节点q ⁱ _new 扩展达到目标点q ⁱ _goal 附近时，从目标节点q ⁱ _goal 通过对应的父节点反向追溯到根节点q _start ，就得到了一条连接q _start 到q ⁱ _goal 的无碰撞路径。

作为本发明的进一步的方案：所述通过采用多属性评价函数且设置合理的权值的具体步骤包括：

所述横向偏移的代价Q _d 为：

；

所述候选路径曲率的代价Q _k 为：

；

所述候选路径到障碍物的距离代价Q _obs 为：

；

；

总代价为：

J _cost =ω ₁ Q _d +ω ₂ Q _k +ω ₃ Q _obs ；

其中，d _i 为离散候选路径点相对于参考路径的横向偏移量，k _i 为离散候选路径点的曲率值，l _mix 和l _max 分别为障碍物相对于候选路径的最小和最大横向安全距离，l _obs,i 为障碍物i相对于候选路径的横向距离，d _obs,i 为障碍物i相对于候选路径的距离代价，ω ₁ 、ω ₂ 、ω ₃ 分别为横向偏移代价、候选路径曲率代价，以及候选路径到障碍物的距离代价的权重参数。

与现有技术相比，本发明存在以下技术效果：

采用上述的技术方案，通过提取IMU、轮式里程计、GPS、激光雷达等传感器数据，并增加约束，使得自动驾驶车辆在复杂越野环境下，能够获得精确和鲁棒的定位作用，适应多种场景；采用二维栅格地图的构建算法，将单帧的点云观测通过贝叶斯滤波更新到全局地图中，显著减小了感知盲区，并提高了地图精度；利用改进RRT算法仅在规划可行驶区域内进行采样，精简了RRT的采样空间，大大提高了采样效率。

附图说明

下面结合附图，对本发明的具体实施方式进行详细描述：

图1为本申请公开的一些实施例的越野环境导航方法的流程图；

图2为本申请公开的一些实施例的多传感器融合进行SLAM和导航的系统结构框图；

图3为本申请公开的一些实施例的全局路径规划的结果图；

图4为本申请公开的一些实施例的进行SLAM时构建的环境点云地图；

图5为本申请公开的一些实施例的构建的全局占据栅格地图；

图6为本申请公开的一些实施例的构建的全局高程栅格地图；

图7为本申请公开的一些实施例的提取的车辆当前位置的可行驶区域示意图；

图8为本申请公开的一些实施例的RRT路径规划算法流程图；

图9为本申请公开的一些实施例的RRT节点扩展示意图；

图10为本申请公开的一些实施例的在越野环境下的实时路径规划示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1，本发明实施例中，一种基于多传感器融合的越野环境导航方法，具体步骤包括：

如图2所示，图示为本发明的多传感器融合进行SLAM和导航的系统结构框图；

步骤S1、基于高分辨率卫星地图建立全局路网，根据任务文件的坐标信息使用Astar算法进行全局路径规划，得到全局参考路径，具体步骤包括：

首先预设自主无人车辆的任务区域，根据基于高分辨率卫星地图的图片获取对应任务区域的任务地图；

根据获得的任务地图选取基准点GPS坐标，提取任务地图中所有道路信息建立全局路网；

根据导航任务文件中的起点、任务点，以及终点的坐标，使用Astar算法进行全局路径规划，获得全局路网中从起点到终点之间的节点拓扑信息；

再按照拓扑关系拼接初始全局路径，并使用贝塞尔曲线拟合算法对初始全局路径进行平滑处理，得到任务区域内的全局参考路径，如图3所示，图示为全局路径规划的结果图。

步骤S2、利用车载设备采集车载多传感器数据进行数据融合，将融合的多模态数据输入SLAM算法中，且添加多种传感器约束，使用基于图优化的框架求解获得精准位姿；

具体实施方式中，步骤S2中利用车载设备采集车载多传感器数据进行数据融合的具体步骤包括：

首先通过车载工控机实时采集读取的多模态数据，所述多模态数据的采集方式具体包括通过3D激光雷达采集周围环境信息得到的3D点云数据、通过IMU采集车辆的高频线加速度和角速度、通过轮式里程计采集的车辆速度，以及利用GPS采集当前车辆的实时经纬度坐标；

对采集的多模态数据进行预处理，预处理方式包括:

3D激光雷达的3D点云数据：分别计算每个激光点的曲率，根据曲率赋予激光点的线或面的特征信息，并将点云聚类以剔除离散点；

IMU的高频线加速度和角速度、轮式里程计的车辆速度：通过批处理构建预积分观测；

GPS的实时经纬度坐标：将经纬度坐标转换成空间位置坐标；

将预处理后的多模态数据进行融合处理，并作为后续SLAM算法的输入。

具体实施方式中，所述步骤S2中将融合的多模态数据输入SLAM算法中，且添加多种传感器约束，使用基于图优化的框架求解获得精准位姿的具体步骤包括：

根据得到的多模态数据构建激光雷达点云残差、IMU和轮式里程计预积分残差、GPS先验残差，并建立非线性最小二乘问题；

再通过图优化的框架进行求解，获得精准的车辆位姿，最后将点云根据车辆位姿进行拼接，得到环境点云地图，如图4所示，图示为进行SLAM时构建的环境点云地图。图中矩形框标出的A和B为路边的杆状标识物，最上方的图中的A和B为杆状标识物在全局点云图中的效果。下方的四张图中，右侧的两张图为杆状标识物A和B在车载相机中的形状，左侧为其对应的在三维点云中的效果。

步骤S3、利用获得的精准位姿拼接多个环境观测，基于滤波融合算法构建越野地形图和全局占据栅格图，具体步骤包括：

如图5所示，图示为构建的全局占据栅格地图。

获取单帧的激光雷达点云数据并根据坡度进行障碍物标记，获取单帧栅格地图中的可通行区域数据，使用高斯过程推断不可观测的点云数据，最后将单帧的点云观测通过二值贝叶斯滤波更新到全局占据栅格地图；

如图6所示，图示为构建的全局高程栅格地图。

根据最大高度标记单帧高程栅格地图的高程数据，利用卡尔曼滤波更新全局高程栅格地图。

步骤S4、根据得到的全局参考路径实时提取车辆当前可行驶区域，具体步骤包括：

根据沿着得到的全局参考路径的前进方向进行车辆行驶，并依据点云数据探测可通行路面区域，根据距离连续性指标c _d 和角度连续性指标c _a ，其中舍弃不连续的路面；

所述距离连续性指标c _d 为：

；

式中，n为该指标当前迭代点前、后需要计算的点的个数，k的坐标从x-n到x+n表示该指标考虑当前点p _x 前n个点和后n个点，p _k 为第k个点，p _k+1为第k+1个点；

所述角度连续性指标c _a 为：

；

式中，n为该指标当前迭代点前后共需要计算的点的个数，p _x 表示当前迭代位置的点，p _x-k 表示当前迭代位置前面第k个点，p _x+k 表示当前迭代位置后面第k个点；

具体的，当c _d 小于距离连续性指标阈值c ^th _d ，且c _a 小于角度连续性指标阈值c ^th _a 时，则地面点为连续的，将该地面点纳入到可行驶区域内，同时提取的路面点的区域记为可行驶区域Ω ₁ ；

考虑到由于障碍物的遮挡等情况，导致存在雷达无法直接探测到全部可行驶路面，鉴于这种情况，首先对车辆周围实时的3D点云使用欧式距离方法进行聚类，得到车辆周围的障碍物簇；

以障碍物簇为壁垒，沿着全局参考路径的垂直方向，向两侧使用洪水填充算法对前方区域进行扩散，形成车辆的可行驶区域Ω ₂ ；

最后计算两个可行驶区域的并集Ω ₁ ∪Ω ₂ 作为最终的可行驶区域，如图7所示，图示为本实施例中的直道和弯道中车辆的可行驶区域提取结果。

步骤S5、当前可行驶区域内使用基于多属性评估的RRT路径规划算法生成局部最优路径进行避障导航，具体步骤包括：

首先，根据步骤S4中得到的可行驶区域Ω作为路径规划的可行域Φ，计算出该可行域Φ在全局参考路径方向上的纵向长度L _Φ ，然后根据可行驶区域Ω的长度L _Ω 和最小规划距离L _p,min 选择路径规划的预瞄长度L _p ，其中，L _p 满足L _p,min ≤L _p <L _Ω ；

根据路径规划的预瞄长度L _p ，在全局参考路径中找到对应的全局路径坐标点，将该全局路径坐标点作为路径规划的预瞄点，以该预瞄点为圆心向四周探索，计算得到在规划可行域Φ内最大的圆C _p,goal ；

如图8所示，图示为RRT路径规划算法的流程图，首先初始化随机树T的生长步长ρ，并将车辆的后轴中心作为随机树T的初始节点q _start ，在圆C _p,goal 内随机选择N个坐标点作为随机树的目标节点q _goal ，进行计算得到一条无碰撞路径；

具体的计算步骤为：

如图9所示，图示RRT节点扩展示意图，具体的，采用RRT在生成新节点时考虑了节点间的连接角度，使RRT生成的路径更加平缓。

对每一个目标节点q ⁱ _goal ，以特定概率在规划可行域Φ中随机采样得到随机节点q ⁱ _rand ，选择随机节点q ⁱ _rand 的最近节点q ⁱ _near ，计算最近节点q ⁱ _near 的父节点与最近节点q ⁱ _near 之间形成的角度θ ⁱ _near 和最近节点q ⁱ _near 与随机节点q ⁱ _rand 之间形成的角度θ ⁱ _randnear ；

判断|θ ⁱ _near - θ ⁱ _randnear |是否小于节点角度阈值θ ^th ，若大于θ ^th ，则寻找另外的最近节点，直到满足小于节点角度阈值θ ^th ，则新节点q ⁱ _new 的计算方法为：

；

若q ⁱ _near 与q ⁱ _new 之间的连线没有障碍物，则将q ⁱ _near 作为q ⁱ _new 的父节点，q ⁱ _new 作为q ⁱ _near 的子节点，然后继续随机采样扩展随机树T；否则舍弃当前新节点q ⁱ _new ，重新随机采样；

循环搜索，当随机树T新节点q ⁱ _new 扩展达到目标点q ⁱ _goal 附近时，从目标节点q ⁱ _goal 通过对应的父节点反向追溯到根节点q _start ，就得到了一条连接q _start 到q ⁱ _goal 的无碰撞路径。

基于每一条路径对应的随机树，将随机树的节点作为贝塞尔曲线的控制点，使用贝塞尔曲线进行平滑处理，得到平滑的路径，其中N条路径便组成了一个候选路径簇；使用曲线拟合算法对路径进行了平滑处理，有效优化了路径的质量。

对于每条候选路径，通过采用多属性评价函数且设置合理的权值，选出代价最小的路径作为当前最优路径，其中，所述多属性评价函数包括横向偏移的代价、候选路径曲率的代价，以及候选路径到障碍物的距离代价。

如图10所示，图示为本实施例中在越野环境下的实时路径规划示意图。

具体的，所述通过采用多属性评价函数且设置合理的权值的具体步骤包括：

所述横向偏移的代价Q _d 为：

；

Q _d 为横向偏移的代价，代表相对于参考路径，候选路径的偏离程度；

所述候选路径曲率的代价Q _k 为：

；

Q _k 为候选路径曲率的代价，用来挑选平均曲率最小的路径；

所述候选路径到障碍物的距离代价Q _obs 为：

；

；

总代价为：

J _cost =ω ₁ Q _d +ω ₂ Q _k +ω ₃ Q _obs ；

其中，d _i 为离散候选路径点相对于参考路径的横向偏移量，k _i 为离散候选路径点的曲率值，l _mix 和l _max 分别为障碍物相对于候选路径的最小和最大横向安全距离，l _obs,i 为障碍物i相对于候选路径的横向距离，d _obs,i 为障碍物i相对于候选路径的距离代价，ω ₁ 、ω ₂ 、ω ₃ 分别为横向偏移代价、候选路径曲率代价，以及候选路径到障碍物的距离代价的权重参数。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定，均应包含在本发明的保护范围之内。

Claims (9)

1.一种基于多传感器融合的越野环境导航方法，其特征在于，具体步骤包括：

步骤S1、基于高分辨率卫星地图建立全局路网，根据任务文件的坐标信息使用Astar算法进行全局路径规划，得到全局参考路径；

步骤S2、利用车载设备采集车载多传感器数据进行数据融合，将融合的多模态数据输入SLAM算法中，且添加多种传感器约束，使用基于图优化的框架求解获得精准位姿；

步骤S3、利用获得的精准位姿拼接多个环境观测，基于滤波融合算法构建越野地形图和全局占据栅格图；

步骤S4、根据得到的全局参考路径实时提取车辆当前可行驶区域；

步骤S5、当前可行驶区域内使用基于多属性评估的RRT路径规划算法生成局部最优路径进行避障导航。

2.根据权利要求1所述一种基于多传感器融合的越野环境导航方法，其特征在于，所述步骤S1的具体步骤包括：

首先预设自主无人车辆的任务区域，根据基于高分辨率卫星地图的图片获取对应任务区域的任务地图；

根据获得的任务地图选取基准点GPS坐标，提取任务地图中所有道路信息建立全局路网；

根据导航任务文件中的起点、任务点，以及终点的坐标，使用Astar算法进行全局路径规划，获得全局路网中从起点到终点之间的节点拓扑信息；

再按照拓扑关系拼接初始全局路径，并使用贝塞尔曲线拟合算法对初始全局路径进行平滑处理，得到任务区域内的全局参考路径。

3.根据权利要求1所述一种基于多传感器融合的越野环境导航方法，其特征在于，所述步骤S2中利用车载设备采集车载多传感器数据进行数据融合的具体步骤包括：

通过车载工控机实时采集读取的多模态数据，所述多模态数据包括通过3D激光雷达采集周围环境信息得到的3D点云数据、通过IMU采集车辆的高频线加速度和角速度、通过轮式里程计采集的车辆速度，以及利用GPS采集当前车辆的实时经纬度坐标；

对采集的多模态数据进行预处理，包括:分别计算每个激光点的曲率，根据曲率赋予激光点的线或面的特征信息，并将点云聚类以剔除离散点；对通过IMU采集的加速度和角速度和轮式里程计采集的车辆速度的数据采用批处理构建预积分观测；将实时经纬度坐标转换成空间位置坐标；

将预处理后的多模态数据进行融合处理。

4.根据权利要求1所述一种基于多传感器融合的越野环境导航方法，其特征在于，所述步骤S2中将融合的多模态数据输入SLAM算法中，且添加多种传感器约束，使用基于图优化的框架求解获得精准位姿的具体步骤包括：

根据得到的多模态数据构建激光雷达点云残差、IMU和轮式里程计预积分残差、GPS先验残差，并建立非线性最小二乘问题；

再通过图优化的框架进行求解，获得精准的车辆位姿，最后将点云根据车辆位姿进行拼接，得到环境点云地图。

5.根据权利要求1所述一种基于多传感器融合的越野环境导航方法，其特征在于，所述步骤S3的具体步骤包括：

获取单帧的激光雷达点云数据并根据坡度进行障碍物标记，获取单帧栅格地图中的可通行区域数据，使用高斯过程推断不可观测的点云数据，最后将单帧的点云观测通过二值贝叶斯滤波更新到全局占据栅格地图；

根据最大高度标记单帧高程栅格地图的高程数据，利用卡尔曼滤波更新全局高程栅格地图。

6.根据权利要求1所述一种基于多传感器融合的越野环境导航方法，其特征在于，所述步骤S4的具体步骤包括：

根据得到的全局参考路径进行车辆行驶，并依据点云数据探测可通行路面区域，根据距离连续性指标和角度连续性指标舍弃不连续的路面；

所述距离连续性指标c _d 为：

；

式中，n为该指标当前迭代点前、后需要计算的点的个数，k的坐标从x-n到x+n表示该指标考虑当前点p _x 前n个点和后n个点，p _k 为第k个点，p _k+1为第k+1个点；

所述角度连续性指标c _a 为：

；

式中，n为该指标当前迭代点前后共需要计算的点的个数，p _x 表示当前迭代位置的点，p _x-k 表示当前迭代位置前面第k个点，p _x+k 表示当前迭代位置后面第k个点；

当c _d 小于距离连续性指标阈值c ^th _d ，且c _a 小于角度连续性指标阈值c ^th _a 时，则地面点为连续的，将该地面点纳入到可行驶区域内，同时提取的路面点的区域记为可行驶区域Ω ₁ ；

由于存在雷达无法直接探测到全部可行驶路面，首先对车辆周围实时的3D点云使用欧式距离方法进行聚类，得到车辆周围的障碍物簇；以障碍物簇为壁垒，沿着全局参考路径的垂直方向，向两侧使用洪水填充算法对前方区域进行扩散，形成车辆的可行驶区域Ω ₂ ；

最后计算两个可行驶区域的并集Ω ₁ ∪Ω ₂ 作为最终的可行驶区域。

7.根据权利要求1所述一种基于多传感器融合的越野环境导航方法，其特征在于，所述步骤S5的具体步骤包括：

根据得到的可行驶区域Ω作为路径规划的可行域Φ，计算出该可行域Φ在全局参考路径方向上的纵向长度L _Φ ，然后根据可行驶区域Ω的长度L _Ω 和最小规划距离L _p，min 选择路径规划的预瞄长度L _p ，其中，L _p，min≤Lp <L _Ω ；

根据路径规划的预瞄长度L _p ，在全局参考路径中找到对应的全局路径坐标点，将该坐标点作为路径规划的预瞄点，以该预瞄点为圆心向四周探索，计算得到在规划可行域Φ内最大的圆C _p,goal ；

初始化随机树T的生长步长ρ，将车辆的后轴中心作为随机树T的初始节点q _start ，在圆C _p,goal 内随机选择N个坐标点作为随机树的目标节点q _goal ，进行计算得到一条无碰撞路径；

基于每一条路径对应的随机树，将随机树的节点作为贝塞尔曲线的控制点，使用贝塞尔曲线进行平滑处理，得到平滑的路径；

对于每条候选路径，通过采用多属性评价函数且设置合理的权值，选出代价最小的路径作为当前最优路径，其中，所述多属性评价函数包括横向偏移的代价、候选路径曲率的代价，以及候选路径到障碍物的距离代价。

8.根据权利要求7所述一种基于多传感器融合的越野环境导航方法，其特征在于，所述随机树的目标节点q _goal ，进行计算得到一条无碰撞路径的具体步骤包括：

对每一个目标节点q ⁱ _goal ，以特定概率在规划可行域Φ中随机采样得到随机节点q ⁱ _rand ，选择随机节点q ⁱ _rand 的最近节点q ⁱ _near ，计算最近节点q ⁱ _near 的父节点与最近节点q ⁱ _near 之间形成的角度θ ⁱ _near 和最近节点q ⁱ _near 与随机节点q ⁱ _rand 之间形成的角度θ ⁱ _randnear ；

判断|θ ⁱ _near - θ ⁱ _randnear |是否小于节点角度阈值θ ^th ，若大于θ ^th ，则寻找另外的最近节点，直到满足小于节点角度阈值θ ^th ，则新节点q ⁱ _new 的计算方法为：

；

若q ⁱ _near 与q ⁱ _new 之间的连线没有障碍物，则将q ⁱ _near 作为q ⁱ _new 的父节点，q ⁱ _new 作为q ⁱ _near 的子节点，然后继续随机采样扩展随机树T；否则舍弃当前新节点q ⁱ _new ，重新随机采样；

循环搜索，当随机树T新节点q ⁱ _new 扩展达到目标点q ⁱ _goal 附近时，从目标节点q ⁱ _goal 通过对应的父节点反向追溯到根节点q _start ，就得到了一条连接q _start 到q ⁱ _goal 的无碰撞路径。

9.根据权利要求7所述一种基于多传感器融合的越野环境导航方法，其特征在于，所述通过采用多属性评价函数且设置合理的权值的具体步骤包括：

所述横向偏移的代价Q _d 为：

；

所述候选路径曲率的代价Q _k 为：

；

所述候选路径到障碍物的距离代价Q _obs 为：

；

；

总代价为：

J _cost =ω ₁ Q _d +ω ₂ Q _k +ω ₃ Q _obs ；

其中，d _i 为离散候选路径点相对于参考路径的横向偏移量，k _i 为离散候选路径点的曲率值，l _mix 和l _max 分别为障碍物相对于候选路径的最小和最大横向安全距离，l _obs,i 为障碍物i相对于候选路径的横向距离，d _obs,i 为障碍物i相对于候选路径的距离代价，ω ₁ 、ω ₂ 、ω ₃ 分别为横向偏移代价、候选路径曲率代价，以及候选路径到障碍物的距离代价的权重参数。

Images