[发明专利]一种基于多传感器融合的越野环境导航方法

权利要求书

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

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

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

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