一种基于可通行地图的矿用车辆自主导航方法

本发明属于车辆定位导航，尤其涉及一种基于可通行地图的矿用车辆自主导航方法。

背景技术：

1、随着自动驾驶技术的不断发展和应用，实现车辆在各种极端条件下的高精度、高鲁棒性和高实时性的定位导航和路径规划成为自动驾驶技术的核心问题。传统的车辆定位导航系统主要依赖gnss(全球导航卫星系统)和ins(惯性导航系统)的组合，但由于gnss信号缺失、ins误差累积、矿区面积过大等原因，这种组合导航系统容易失效。为了实现自动驾驶技术的商业化和广泛应用，不依赖外部信息交互实现持续可靠、准确的定位导航是至关重要的。特别是在特种作业车辆领域，如矿用运输车等，对不依赖外来信息的车载感知系统的需求不断增加。

2、激光雷达基本上不受光照变化的影响，能够在不需要良好、稳定的光照条件的前提下实现全时域、全天候的可通行性分析和导航定位，适用于没有额外光照补充的非结构化道路场景。激光雷达能够精细地重构三维环境，并基于三维特征描述地面的几何信息和可通行信息，而且运算效率比视觉更高，更加方便提取可通行区域和实时位姿估计。由于激光雷达不可避免地会接收到一些错误的反射信号，这会导致激光三维点云中存在一些噪点，若对其不进行处理，会对后续可通行地图构建、路径规划、目标识别等带来影响，使智能驾驶系统产生错误决策。在实际构建地图中，需要在考虑运算时间成本下，联合优化多帧激光雷达传感器数据全面地考虑周围环境特征。由于矿用车辆的运行环境的复杂性、车辆自身状态的不确定性和单一传感器信息的局限性，仅依靠单一车载传感器很容易导致导航的失效。因此，在激光数据基础上融合其他传感器数据辅助进行地图构建和定位，提高系统在矿区环境下运行的鲁棒性、可靠性。

技术实现思路

1、本发明的目的在于提供一种基于可通行地图的矿用车辆自主导航方法，以解决现有技术中存在的矿用车辆系统无法保证全时域、全路段的鲁棒定位和导航。

2、为了实现上述目的，本发明的技术方案如下：一种基于可通行地图的矿用车辆自主导航方法，包括以下步骤：

3、a、对激光雷达数据进行可通行性分析

4、a1、固态激光雷达的扫描区域为一定半径和角度的扇形区域，使用极坐标网络表示扇形区域。将整个扇形区域按照径向分为4个弧形区域，每个弧形区域都按照一定的角度分辨率和长度分辨率划分为nr,m×nθ,m个小扇形区sector。其中，nθ,m是按角度划分的个数，nr,m是按径向半径长度划分的个数。

5、a2、选取sector区域中的每一个点，对其进行临近点搜索，将包含该点的临近点拟合成曲面，对曲面中的点根据主成分分析法进行分析，迭代计算地面可通行区域点云具体迭代步骤如下：

6、第1次迭代的地面可通行区域点估计为：

7、

8、其中，pk为第m个弧形区域archedm内的第k个激光雷达点，m＝1、2、3、4，为选择的最低高度点的平均值，z(pk)是点的高度值，zseed为高度阈值。迭代估计时，令第j次迭代的地面可通行区域点为则第j+1次迭代公式为：

9、

10、其中，为第j次迭代地面点的法向量，md为点到地面设置的距离阈值，维第j次迭代时视为地面点的平均值。

11、a3、根据垂直度、高度、平整度三个指标对点云进行二分类地面似然估计的区域概率检验，挑选出点云中满足设定条件的点作为最终的地面可通行点集合其余的点为不可通行点集合具体挑选方法如下：

12、根据垂直度v3,n、高度平整度σn三个指标，利用地面似然估计l(θ|x)挑选出中所有满足条件的点作为最终的地面可通行点集合其中，θ为所有参数，x为服从密度函数f和连续概率分布的随机变量。

13、b、对多帧点云数据进行联合优化处理

14、b1、根据激光雷达的前视距离disfov和前视角度θfov设置可信赖区域，可信赖区域与激光雷达的扫描范围形状相同。可信赖区域的角度为θr＝kd·θfov，可信赖区域的距离为disr＝kdis·disfov，其中kd为区域角度的比例阈值、kdis为区域长度比例阈值，阈值变化范围为0～1。

15、b2、根据车辆运行的时间和距离对分割出去的不可通行点集合进行累积，进而根据距离阈值和时间阈值构成一个子图，然后针对子图使用半径滤波器和统计滤波器相结合的方法去除离群点，具体步骤如下：

16、首先对每一帧数据进行可信赖区域优化，去除可信赖区域外的点集；然后根据里程计结果将一定范围内的多帧激光数据组成一个子图，以子图的形式使用半径滤波器和统计滤波器相结合的方法来去除离群点；最后再将多个子图共同拼接融合为一个完整地图。

17、半径滤波对子图点云进行一次迭代计算，对每个点进行半径为r的邻域搜索，如果邻域其他点的个数低于某一设定的阈值，则该点将被视为噪声点并被移除。子图点云密度相对一致，设定近邻数量阈值为n，数据点ai为中心、r为半径的邻域内的实际近邻数目为ni。当该点邻域内ni<n时，剔除该离群点。统计滤波对子图点云进行两次迭代计算，第一次迭代计算中假设三维点云中p0(x,y,z)与临近的k个点p1、p2…pk的距离均值为d，d的分布服从均值为μd、方差为d的高斯分布f(d)，进而确定距离阈值。在第二次迭代中，基于点到所有邻近点的距离分布特征，如果该点的平均邻域距离不在阈值范围内，说明该点远离密集点云区域，删除不满足要求的离群点。

18、c、基于激光里程计进行全局地图构建

19、c1、对车辆的状态和运动轨迹进行因子图建模，因子图中包括惯性测量单元预积分因子、激光雷达里程计因子，使用因子图优化对车辆状态和位姿进行估计，具体步骤如下：

20、利用来自三维激光雷达、惯性测量单元的传感器观测数据来估计车辆的状态和运动轨迹。将状态估计问题表述为一个最大后验概率问题即map问题，使用因子图来对map问题进行建模。在高斯噪声模型的假设下，map问题等同于解决一个非线性最小二乘法问题。考虑的因子有惯性测量单元预积分因子、激光雷达里程计因子。

21、使用惯性测量单元预积分的方法来获取两个相邻激光帧时间戳之间的相对运动。第m时刻和第n时刻之间的预积分测量值由以下公式进行计算：

22、

23、

24、

25、其中，δvmn、δpmn分别为两个激光帧时刻间的x、y、z轴方向上的三维相对速度向量和相对位置向量，δrmn为两个激光帧时刻间的x、y、z轴方向上的三维相对旋转矩阵；vm、vn分别为第m、n时刻的速度向量，各个分量代表在x、y、z轴方向上的分速度。pm、pn分别为第m、n时刻在x、y、z轴方向上的位置向量；rm、rn分别为第m、n时刻对应在x、y、z轴方向上的旋转矩阵；δtmn为m、n时刻的时间间隔。计算完预积分测量值后，惯性测量单元的偏差在后续会与激光雷达因子一起被共同优化。

26、里程计接收到新的一帧激光数据时，通过局部区域点的曲率/粗糙度提取边缘和平面特征。粗糙度值较大的点被归类为边缘特征，相反则归类为平面特征。假设，第t时刻激光数据的全部点集为pt，提取的平面特征和边缘特征分别为ptp、pte。当车辆的姿态超过阈值时，设置激光雷达帧pt+1为关键帧，新保存的关键帧pt+1与因子图中的最新车辆状态节点xt+1相关联。将子关键帧集合{pt-k,..,pt}通过已知变换关系{tt-k,..,tt}进行坐标转换并合并成一个体素地图mt。由于之前提取了两种特征，因此mt由边缘特征体素地图和平面特征体素地图组成，即：

27、

28、其中：

29、

30、

31、式中，'pte、'ptp分别为转换坐标后的关键帧的边缘特征和平面特征集合。

32、通过惯性测量单元预测的车辆状态得到初始变换关系，然后根据关键帧的边缘特征和平面特征在体素地图和中扫描匹配对应的边缘和平面。通过最小化目标误差函数获取当前关键帧和体素地图最佳的变换：

33、

34、式中，k、u、v、w分别是相应集合中的特征索引，是关键帧中具有边缘特征的点，和是体素地图对应的点，是关键帧中具有平面特征的点，和是体素地图对应的点。最后，根据第ti时刻的位姿和ti+1时刻帧的位姿计算相对位姿δti,i+1＝titti+1。

35、c2、根据上述激光里程计计算得到当前时刻的车辆位姿，将可通行性分析后的激光点云经过坐标投影到地图坐标系下，并根据距离阈值和时间阈值构建子图。子图经过优化处理之后，与其他子图进行拼接和融合，最后构成用于定位的全局点云地图。

36、d、基于先验地图进行车辆全局定位

37、d1、确定车辆在可通行点云地图的初始位置，然后根据地图和当前帧激光点云数据进行迭代最近点匹配计算即icp匹配计算，寻找最优的车辆匹配位姿作为当前的车辆位姿，具体步骤如下：

38、使用实时差分传感器提供的大地坐标系数据获取当前的位置信息，进而确定车辆在初始状态的位置和姿态。然后，通过激光雷达获得的当前帧数据与地图中的历史帧数据进行icp匹配。icp匹配分为两步，粗配准用于粗略位姿估计，精配准用于获取精确位姿结果。假设当前帧点集为p、地图历史帧点集为q，通过使用奇异值分解求解当前的旋转矩阵r和平移矩阵t，使得当前帧点集p中的每一个点pi经过改位姿变换后，与历史帧点集q中每一个qi的距离误差和最小。其中，近邻点搜索使用速度更快的kd树检索方法实现。总体误差的目标函数为：

39、

40、d2、在两次重定位输出间隙或重定位失效的情况下，以最近一次icp匹配获取的车辆位姿为起始位姿，结合激光里程计计算车辆当前的位姿。具体步骤如下：

41、在icp匹配的相邻时刻间，以多线程的方式添加激光里程计，将匹配的位姿信息作为先验位姿约束，以帧间匹配的方式计算相邻时刻间的相对位姿变换。假设pr为第r时刻icp匹配获取到的位姿，从第r时刻到下一次icp匹配位姿计算输出前，存在m个激光里程计输出的相对位姿rlo,1,rlo,2,...,rlo,m和tlo,1,tlo,2,...,tlo,m。在两次重定位输出间隙或重定位失效的情况下，以第r时刻的位姿pr为初始位姿，结合激光里程计的相对位姿计算车辆当前的位姿，第m时刻的位姿plo,k为：

42、

43、d3、在两次激光里程计输出定位结果的间隙加入惯性测量单元位姿估计。每当激光里程计输出一次位姿结果后，以激光里程计获取的位姿为起始位姿，惯性测量单元在短时间内推算车辆在下一次激光里程计输出前的位姿。具体步骤如下：

44、在里程计两帧相邻数据间进行惯性测量单元位置推算，得到相邻时刻间车辆的位姿，并将位姿信息补充到激光数据获得的位姿数据中，进一步提高车辆的定位频率。惯性测量单元的测量模型如下：

45、

46、其中，分别是惯性测量单元第k时刻的加速度、角速度观测值，分别是惯性测量单元加速度、角速度的真实值，分别是服从高斯分布的传感器加速度、角速度的噪声，分别是传感器加速度、角速度的偏置bias。根据第k时刻的惯性测量单元数据实现惯性测量单元离散运动模型，如下所示

47、

48、

49、

50、

51、

52、其中，分别是惯性测量单元在第k时刻的加速度、速度、位移，gw为重力加速度分量。在激光里程计输出位姿的第t时刻和t+1时刻的间隔中，以第t时刻的位姿为起始位姿，根据惯性测量单元位姿数据以里程计相同的方法递推车辆的位姿，实现更高频率的定位输出。

53、e、基于栅格地图进行全局路径规划

54、e1、将步骤c2构建好的全局点云地图进行栅格化，得到全局栅格地图。

55、e2、根据矿用车辆的真实尺寸构建车辆二自由度运动学模型，并设置车辆的基础参数。车辆二自由度运动学模型如下：

56、

57、

58、

59、

60、其中，vx、vy分别是车辆沿x和y轴方向的速度，r是车辆的转弯半径，vvehicle是车辆实际行驶的速度，δ是前轮转角，是航向角，l是前后两轮轴中心的距离。

61、e3、根据全局栅格地图获取全局可通行道路信息，结合运动学模型判断车辆通行情况。设置车辆起点和终点后使用求解最短路径最有效的直接搜索方法即a*方法进行全局路径规划。

62、e4、根据实时传感器数据构建局部点云地图，将局部点云地图栅格化获取局部栅格地图，并结合车辆运动学模型，在尽可能贴合e3步骤搜索出的全局规划路径的前提，进行局部路径规划算法，躲避实时障碍物。

63、与现有技术相比，本发明的有益效果如下：

64、1、本发明根据可通行区域分析和激光里程计构建可通行地图，并将可通行地图栅格化用于全局路径规划，然后通过先验地图信息、实时传感器数据进行车辆的实时定位和局部路径规划躲避障碍物。

65、2.本发明利用激光雷达感知矿区环境，并采用点云匹配、里程计和imu融合方法进行组合定位，系统不受外界光线变换影响且能够在匹配方法失效时借助里程计和imu推算定位，能够满足夜晚无光照、环境条件恶劣条件下的全时域、全路段的鲁棒定位和导航。