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

Abstract

本发明公开了一种基于可通行地图的矿用车辆自主导航方法，包括以下步骤：对激光雷达数据进行可通行性分析；对多帧点云数据进行联合优化处理；基于激光里程计进行全局地图构建；基于先验地图进行车辆全局定位；基于栅格地图进行全局路径规划。本发明根据可通行区域分析和激光里程计构建可通行地图，并将可通行地图栅格化用于全局路径规划，然后通过先验地图信息、实时传感器数据进行车辆的实时定位和局部路径规划躲避障碍物。本发明利用激光雷达感知矿区环境，并采用点云匹配、里程计和IMU融合方法进行组合定位，能在匹配方法失效时借助里程计和IMU推算定位，能满足夜晚无光照、环境条件恶劣条件下的全时域、全路段的鲁棒定位和导航。

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

B1、根据激光雷达的前视距离Dis_fov和前视角度θ_fov设置可信赖区域，可信赖区域与激光雷达的扫描范围形状相同。可信赖区域的角度为θ_r＝k_d·θ_fov，可信赖区域的距离为Dis_r＝k_dis·Dis_fov，其中k_d为区域角度的比例阈值、k_dis为区域长度比例阈值，阈值变化范围为0～1。

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

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

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

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

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

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

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

其中，Δv_mn、Δp_mn分别为两个激光帧时刻间的x、y、z轴方向上的三维相对速度向量和相对位置向量，ΔR_mn为两个激光帧时刻间的x、y、z轴方向上的三维相对旋转矩阵；v_m、v_n分别为第m、n时刻的速度向量，各个分量代表在x、y、z轴方向上的分速度。p_m、p_n分别为第m、n时刻在x、y、z轴方向上的位置向量；R_m、R_n分别为第m、n时刻对应在x、y、z轴方向上的旋转矩阵；Δt_mn为m、n时刻的时间间隔。计算完预积分测量值后，惯性测量单元的偏差在后续会与激光雷达因子一起被共同优化。

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

其中：

式中，'P_t ^e、'P_t ^p分别为转换坐标后的关键帧的边缘特征和平面特征集合。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中，V_x、V_y分别是车辆沿x和y轴方向的速度，R是车辆的转弯半径，v_vehicle是车辆实际行驶的速度，δ是前轮转角，是航向角，L是前后两轮轴中心的距离。

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

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

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

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

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

附图说明

图1是本发明的矿用车辆自主导航方法流程图。

图2是本发明的联合优化前后示意图。

图3是本发明的车辆运动学模型示意图。

图4是本发明的车辆可通行地图和路径规划示意图。

图5是本发明的基于可通行地图的车辆定位示意图。

具体实施方式

为了让本发明的目的、技术路线以及技术优势更加清晰，以下结合附图及实施例，对本发明作进一步详细阐述。显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。应当理解，本领域的技术人员可根据本说明书所描述的内容了解本发明的特点，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，为了避免混乱或模糊本发明的重点，有些具体技术细节将在描述过程中被省略。

一种基于可通行地图的矿用车辆自主导航方法，用于矿用车辆在非结构化场景下的定位和导航，包括可通行分析、多帧数据联合优化、全局地图构建、全局车辆重定位、车辆路径规划。如图1所示，本发明的流程如下：

步骤一、激光雷达点云数据可通行性分析

为了更好的描述固态激光雷达的扇形扫描区域，将激光点云集合P进行极坐标栅格化，用极坐标网络对其进行表示。首先，将整个扇形区域按照径向半径分为4个弧形区域，每个弧形区域都按照一定的角度分辨率和长度分辨率划分为N_r,m×N_θ,m个小扇形区sector。其次，选取sector区域中的每一个点，对其进行临近点搜索，将包含该点的临近点拟合成曲面，对曲面中的点进行PCA主成分分析，查找对应特征值最小的特征向量。第1次迭代的地面可通行区域点估计为其中/>为选择的最低高度点的平均值，z(p_k)是点的高度值，z_seed为高度阈值。迭代估计时，令第j次迭代的地面可通行区域点为则第j+1次迭代公式为：

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

为了进一步提高地面可通行点的判断精度，对其进行二分类地面似然估计的区域概率检验。是否为地面可通行点由垂直度、高度、平整度三个指标决定，变量分为v_3,n、σ_n。最后，挑选出/>中所有满足条件的点作为最终的地面可通行点集合/>其余的点为不可通行点集合/>

步骤二、多帧数据联合优化

可通行分析后，对激光点云数据进行得到了可通行区域点云集合和不可通行区域点云集合/>使用不可通行区域点云表现地图障碍物构建全局可通行地图。点云存在稀疏性导致远处的扇形区sector无法找到正确的地面点，因此根据激光雷达的前视距离Dis_fov和前视角度θ_fov设置可信赖区域。可信赖区域与激光雷达的扫描范围形状一致，角度为θ_r＝k_d·θ_fov，距离为Dis_r＝k_dis·Dis_fov，其中k_d和k_dis为比例阈值，可以针对不同的应用场景对可信赖区域进行调整。

单纯优化每一帧激光数据既没有全面地考虑周围环境特征，也没有考虑系统运算所带来的时间成本，因此在对通过性点云优化过程中设置了子图策略。为了保证整体系统的实时性和效率，首先对每一帧数据进行可信赖区域优化，去除可信赖区域外的点集；然后根据里程计结果将一定范围内的多帧激光数据组成一个子图，以子图的形式使用半径滤波器和统计滤波器相结合的方法来去除离群点；最后再将多个子图共同拼接融合为一个完整地图。

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

步骤三、基于激光里程计的全局地图构建

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

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

其中，Δv_mn、Δpmn分别为两个激光帧时刻间的x、y、z轴方向上的三维相对速度向量和相对位置向量，ΔR_mn为两个激光帧时刻间的x、y、z轴方向上的三维相对旋转矩阵。vm、vn分别为第m、n时刻的速度向量，各个分量代表在x、y、z轴方向上的分速度。pm、pn分别为第m、n时刻在x、y、z轴方向上的位置向量。R_m、R_n分别为第m、n时刻对应在x、y、z轴方向上的旋转矩阵，Δtmn为m、n时刻的时间间隔。惯性测量单元的偏差会与激光雷达因子一起被联合优化。

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

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

k、u、v、w是相应集合中的特征索引，是关键帧/>中具有边缘特征的点，/>和是体素地图/>对应的点，/>是关键帧/>中具有平面特征的点，/>和/>是体素地图对应的点。最后，可以根据第T_i时刻的位姿和T_i+1时刻帧的位姿计算相对位姿ΔT_i,i+1＝T_i ^TT_i+1。

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

步骤四、基于先验地图的车辆全局定位

使用RTK传感器提供的大地坐标系数据获取当前的位置信息，进而确定车辆在初始状态的位置和姿态。然后，通过激光雷达获得的当前帧数据与地图中的历史帧数据进行ICP配准。ICP配准分为两步，粗配准用于粗略位姿估计，精配准用于获取精确位姿结果。假设当前帧点集为P、地图历史帧点集为Q，通过使用SVD求解当前的位姿R、T，使得当前帧点集P中的每一个点p_i经过改位姿变换后，与历史帧点集Q中每一个q_i的距离误差和最小。其中，近邻点搜索使用速度更快的KDTree方法实现。总体误差的目标函数为：

虽然使用ICP匹配的方法可以获取车辆位姿信息，但是由于矿区环境恶劣、场景过大、传感器噪声，单纯基于地图匹配的定位方法鲁棒性过差。因此，在ICP匹配的相邻时刻间，以多线程的方式添加激光里程计，将匹配的位姿信息作为先验位姿约束，以帧间匹配的方式计算相邻时刻间的相对位姿变换。假设P_r为第r时刻ICP匹配获取到的位姿，从第r时刻到下一次ICP匹配位姿计算输出前，存在m个激光里程计输出的相对位姿R_lo,1,R_lo,2,...,R_lo,m和T_lo,1,T_lo,2,...,T_lo,m。在两次重定位输出间隙或重定位失效的情况下，以第r时刻的位姿P_r为初始位姿，结合激光里程计的相对位姿计算车辆当前的位姿，第m时刻的位姿P_lo,k为

由于在矿区等非结构化道路环境下车辆经常遇到陡坡、狭长路段、夯土墙等路段，且车辆运行速度较快，因此对定位的实时性和定位输出频率要求较高。激光雷达的采样频率为10Hz，相邻两帧采集时间为0.1s，即使在ICP间隔考虑激光里程计也无法满足高频率定位需求。惯性测量单元频率可以达到100Hz，因此在里程计两帧相邻数据间进行惯性测量单元位置推算，得到相邻时刻间时刻车辆的位姿，并将位姿信息补充到激光数据获得的位姿数据中，进一步提高车辆的定位频率。惯性测量单元的测量模型如下：

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

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

步骤五、全局路径规划和局部路径规划

矿车的真实尺寸构建智能车辆二自由度运动学模型，并设置车辆的基础参数。

其中，V_x、V_y是车辆沿x和y轴方向的速度，R是车辆的转弯半径，v是车辆行驶方向，δ是前轮转角，是航向角，L是前后两轮轴中心的距离。

真实环境下，栅格地图在实现导航、避障等方面具有更为突出的优势。为了实现可通行分析后的车辆自主导航，将地图进行栅格化得到栅格地图。可通行地图使得智能矿车能够感知矿区环境的可通行情况，进一步可以根据路径规划A*算法搜索出一条从初始位置到目标位置的最优路径。

在基于全局可通行地图进行全局规划路径的同时，智能矿车在预定路径上对检测到的动静态障碍物需要进行实时处理，实现车辆自主避障功能。在矿车行驶过程中，车辆通过激光点云的重定位获取当前时刻位姿，然后根据周围环境构建局部可通行栅格地图，最后对全局路径进行跟随的同时基于局部路径规划算法实现避开障碍物，更好地实现到目标位置的自主导航。根据局部可通行地图进行局部的路径规划算法，使智能车辆安全避开障碍物的同时，在预测步长范围内的局部避障路径尽量靠近全局路径。智能矿车构建局部可通行栅格地图需考虑车辆的非完整性约束和更加细致的环境信息，因此可以根据需要将地图分辨率提高。

对于本领域普通技术人员而言，可以根据上述实施例的各种方法中全部步骤或部分步骤加以改进或变换，凡依本专利所说的变换和修改，皆属于本发明所附权利要求的保护范围内。

Claims (1)

1.一种基于可通行地图的矿用车辆自主导航方法，其特征在于：包括以下步骤：

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

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

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

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

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

其中，为第j次迭代地面点的法向量，M_d为点到地面设置的距离阈值，/>维第j次迭代时视为地面点/>的平均值；

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

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

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

B1、根据激光雷达的前视距离Dis_fov和前视角度θ_fov设置可信赖区域，可信赖区域与激光雷达的扫描范围形状相同；可信赖区域的角度为θ_r＝k_d·θ_fov，可信赖区域的距离为Dis_r＝k_dis·Dis_fov，其中k_d为区域角度的比例阈值、k_dis为区域长度比例阈值，阈值变化范围为0～1；

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

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

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

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

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

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

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

其中，Δv_mn、Δp_mn分别为两个激光帧时刻间的x、y、z轴方向上的三维相对速度向量和相对位置向量，ΔR_mn为两个激光帧时刻间的x、y、z轴方向上的三维相对旋转矩阵；v_m、v_n分别为第m、n时刻的速度向量，各个分量代表在x、y、z轴方向上的分速度；p_m、p_n分别为第m、n时刻在x、y、z轴方向上的位置向量；R_m、R_n分别为第m、n时刻对应在x、y、z轴方向上的旋转矩阵；Δt_mn为m、n时刻的时间间隔；计算完预积分测量值后，惯性测量单元的偏差在后续会与激光雷达因子一起被共同优化；

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

其中：

式中，分别为转换坐标后的关键帧的边缘特征和平面特征集合；

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

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

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

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

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

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

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

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

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

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

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

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

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

E1、将步骤C2构建好的全局点云地图进行栅格化，得到全局栅格地图；

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

其中，V_x、V_y分别是车辆沿x和y轴方向的速度，R是车辆的转弯半径，v_vehicle是车辆实际行驶的速度，δ是前轮转角，是航向角，L是前后两轮轴中心的距离；

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

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