CN112977443A - 一种井下无人驾驶无轨胶轮车的路径规划方法 - Google Patents

Abstract

本发明公开了一种井下无人驾驶无轨胶轮车的路径规划方法，数据采集装置安装在有人驾驶的无轨胶轮车，然后匀速在井下巷道内行进，通过数据采集装置实时采集行进过程中的道路数据并反馈给NUC电脑，最终遍历整个井下巷道的行驶区域获得原始点云数据包；然后根据原始点云数据包依次通过算法先建立井下巷道三维点云地图，接着对三维点云地图进行分割，获得条带状地面点云，最后对条带状点云进行迭代漂移得到巷道中心点集；接着将得到的巷道线状中心点集拆分成若干段无分叉的点集；最终拼接成具有拓扑关系的井下巷道路网；最终根据该巷道路网进行路径规划。本发明得出的巷道路网能有效还原井下巷道的真实走向，确保无轨胶轮车以最短的时间到达目标点。

Description

一种井下无人驾驶无轨胶轮车的路径规划方法

技术领域

本发明涉及一种无人驾驶车的路径规划方法，具体是一种井下无人驾驶无轨胶轮车的路径规划方法。

背景技术

近年来,随着科学技术的迅速发展,无人驾驶车辆已成为当今汽车领域研究的新热点,是未来汽车工业发展的主流方向。矿用无轨胶轮车作为煤矿井下辅助运输的重要组成部分,是制约煤炭开采量及生产效率的瓶颈。矿井辅助运输的工作环境较为恶劣,属于高危环境,如何实现其无人驾驶,对于减少财产损失及人员伤亡率,降低运输成本,提高采矿工作效率,具有重大的应用前景和战略价值。

路径规划是实现井下无轨胶轮车无人驾驶的基础。现今国内外路径规划主要的研究方向是在高速公路环境和城市道路环境下,而针对于井下特殊环境下的路径规划至今少有人提及。并且相对于地面道路，井下巷道结构复杂、狭窄多弯，使得在井下巷道环境下规划目标路径十分困难，费时费力。现有的方式一般首先对井下巷道进行抽象简化，但是这种方式对井下巷道真实走向还原度低，难以规划出实际最优路径。

发明内容

针对上述现有技术存在的问题，本发明提供一种井下无人驾驶无轨胶轮车的路径规划方法，能有效还原井下巷道的真实走向，从而能在给出起点与终点的前提下，搜索出一条从起点至终点的实际最优路径，确保无轨胶轮车以最短的时间到达目标点。

为了实现上述目的，本发明采用的技术方案是：一种井下无人驾驶无轨胶轮车的路径规划方法，具体步骤为：

(1)使用固态激光雷达、NUC电脑和移动电源搭建数据采集装置，NUC电脑分别与移动电源和固态激光雷达连接，所述NUC电脑的操作系统为Ubuntu18.04，并安装ROS与PCL库；

(2)将数据采集装置安装在有人驾驶的无轨胶轮车上，使无轨胶轮车在井下巷道内遍历所有行驶区域，数据采集装置的固态激光雷达实时采集数据并反馈给NUC电脑，最终采集获得原始点云数据包；

(3)根据采集到的数据，使用Cartographer算法离线建立井下巷道三维点云地图，其中设定地图原点为井口位置；

(4)对得到的三维巷道地图进行地面分割处理，使用Ground Plane Filter算法分割出地面点云数据，得到条带状地面点云；

(5)使用Meanshift算法对条带状点云进行迭代漂移，使边缘点向着道路中心聚集，从而使条带状分布的道路点细化成线状，得到巷道中心点集；

(6)由于井下巷道具有多个分叉点与端点，在得到的巷道中心点集中将所有巷道分叉点与端点手动标识出来，从而使巷道线状中心点集拆分成若干段无分叉的点集；

(7)根据各标识点之间线状点云数据，分段拟合出各路段中心线，最终拼接成具有拓扑关系的井下巷道路网；

(8)在进行路径规划时，首先确定起点与终点在巷道拓扑网络中的位置，然后使用Dijsktra算法搜索从起点至终点距离最短的路径，最终完成路径规划。

进一步，所述步骤(2)采集获得原始点云数据包的具体过程为：将数据采集装置安装在有人驾驶的无轨胶轮车前部车头处，接着开启固态激光雷达，以井口位置为起点，使无轨胶轮车以0.3m/s以下的速度在巷道内向前匀速移动，同时使用NUC电脑录制固态激光雷达测量数据的bag包；无轨胶轮车在井下巷道内遍历所有行驶区域，最终获得原始点云数据包。

进一步，所述步骤(4)的具体过程为：

首先沿车辆行进方向将点云分成若干个子点云段，使每个子点云段中地面近似为平面，减小坡度变化对分割结果的影响，然后对每个子点云段中点云数据使用GroundPlane Filter分割出地面点云；

Ground Plane Filter算法分割地面具体流程如下：

首先选定点云中n个最低点，计算其平均值，得到LPR(即最低点代表)值，根据设定的高度阈值Th，将点云数据中高度在阈值范围内的点提取出来作为种子点集；之后根据种子点集估计平面模型，采用如下线性模型进行估计：

ax+by+cz+d＝0

n^Tx＝-d

其中n＝[a,b,c]^T，x＝[x,y,z]^T；a、b、c、d为平面模型方程的四个参数。使用协方差矩阵求解此线性模型，种子点集S∈R³的协方差矩阵为：

式中，C为协方差矩阵，S为种子点集，S_i代表此种子点集中第i个点，

为种子点集中所有点均值。此协方差矩阵的三个奇异向量能通过奇异值分解求得，这三个奇异向量描述点集在三个主要方向的散布情况；通过计算具有最小奇异值的奇异向量来求得垂直于平面的法向量n，之后代入

求得d，即能求出平面模型；

求出平面模型以后，计算点云数据中每一个点到该平面的正交投影的距离，与设定阈值比较，若小于阈值，该点为地面点，若大于阈值，则为非地面点。

进一步，所述步骤(5)的具体过程为：

漂移均值计算基本公式如下：

其中s_h是一个半径为h的高维球区域，满足以下关系的y点的集合

S_h(x)＝{y:(y-x)^T(y-x)＜h²}

k是落入s_h区域内的所有样本点的个数，M_h即为求得的MeanShift向量；考虑到样本点与被偏移点的距离不同，其偏移量对均值偏移向量的贡献也不同，加入高斯核函数，得到如下改进的MeanShift向量形式：

其中，

为高斯核函数，其带宽选为4；h_i为区域s_h半径，由于点云分布于三维空间中，本发明中s_h采用三维球形区域，选取巷道宽度平均值的1/2为此球形半径。

计算出被偏移点MeanShift向量后，根据计算值更新被偏移点位置：

x_t+1＝x_t+M_h(x_t)

式中，x_t为被偏移点当前位置，M_h(x_t)为当前点的MeanShift向量，x_t+1为根据MeanShift向量计算出的被偏移点新的位置。

如此循环迭代，由于巷道中心点云密度最大，被偏移点会逐渐往中心迭代漂移，最终收敛至巷道中心线附近；依次对所有点云数据进行迭代漂移，最终条带状点云会收敛为线状点云。

进一步，所述步骤(7)具体过程为：

将拆分成若干段的地下巷道线状点云分别进行曲线拟合，得到多段无分叉的曲线，每段曲线即为对应某段巷道地面中心线；地下巷道分叉点既是某段曲线的起点，也是另一段曲线的终点，根据这一特征将所有曲线拼接起来，形成能反映真实井下巷道走向的巷道拓扑网络，分叉点即为拓扑网络中的各节点；同时，在进行曲线拟合时计算出各曲线长度信息，保存到生成的巷道拓扑网络中，为最短路径规划算法提供各节点之间距离信息。

与现有技术相比，本发明先采用数据采集装置安装在有人驾驶的无轨胶轮车，然后使有人驾驶的无轨胶轮车匀速在井下巷道内行进，通过数据采集装置中的固态激光雷达实时采集行进过程中的道路数据并反馈给NUC电脑，最终遍历整个井下巷道的行驶区域获得原始点云数据包；然后根据原始点云数据包依次通过算法先建立井下巷道三维点云地图，接着对三维点云地图进行分割，获得条带状地面点云，最后对条带状点云进行迭代漂移得到巷道中心点集；接着将得到的巷道线状中心点集拆分成若干段无分叉的点集；分段拟合出各路段中心线，最终拼接成具有拓扑关系的井下巷道路网；最终根据该巷道路网进行路径规划。本发明得出的巷道路网能有效还原井下巷道的真实走向，从而在给出起点与终点的前提下，搜索出一条从起点至终点的实际最优路径，确保无轨胶轮车以最短的时间到达目标点。

附图说明

图1是本发明中数据采集装置的安装示意图；

图2是本发明中分割出的部分井下巷道地面点云数据示意图；

图3是本发明中得到的线状巷道中心点集示意图；

图4是本发明中巷道点云拆分结果图；

图5是本发明路径规划结果示意图；

图6是本发明的整体流程图。

具体实施方式

下面将对本发明作进一步说明。

如图1至图4所示，本发明的具体步骤为：

(1)使用固态激光雷达、NUC电脑和移动电源搭建数据采集装置，NUC电脑分别与移动电源和固态激光雷达连接，所述NUC电脑的操作系统为Ubuntu18.04，并安装ROS与PCL库；

(2)将数据采集装置安装在有人驾驶的无轨胶轮车上，使无轨胶轮车在井下巷道内遍历所有行驶区域，数据采集装置的固态激光雷达实时采集数据并反馈给NUC电脑，最终采集获得原始点云数据包；

(3)根据采集到的数据，使用Cartographer算法离线建立井下巷道三维点云地图，其中设定地图原点为井口位置；

(4)对得到的三维巷道地图进行地面分割处理，使用Ground Plane Filter算法分割出地面点云数据，得到条带状地面点云；

(5)使用Meanshift算法对条带状点云进行迭代漂移，使边缘点向着道路中心聚集，从而使条带状分布的道路点细化成线状，得到巷道中心点集；

(6)由于井下巷道具有多个分叉点与端点，在得到的巷道中心点集中将所有巷道分叉点与端点手动标识出来，从而使巷道线状中心点集拆分成若干段无分叉的点集；

(7)根据各标识点之间线状点云数据，分段拟合出各路段中心线，最终拼接成具有拓扑关系的井下巷道路网；

(8)在进行路径规划时，首先确定起点与终点在巷道拓扑网络中的位置，然后使用Dijsktra算法搜索从起点至终点距离最短的路径，最终完成路径规划。

进一步，所述步骤(2)采集获得原始点云数据包的具体过程为：将数据采集装置安装在有人驾驶的无轨胶轮车前部车头处，接着开启固态激光雷达，以井口位置为起点，使无轨胶轮车以0.3m/s以下的速度在巷道内向前匀速移动，同时使用NUC电脑录制固态激光雷达测量数据的bag包；无轨胶轮车在井下巷道内遍历所有行驶区域，最终获得原始点云数据包。

进一步，所述步骤(4)的具体过程为：

首先沿车辆行进方向将点云分成若干个子点云段，使每个子点云段中地面近似为平面，减小坡度变化对分割结果的影响，然后对每个子点云段中点云数据使用GroundPlane Filter分割出地面点云；

Ground Plane Filter算法分割地面具体流程如下：

首先选定点云中n个最低点，计算其平均值，得到LPR(即最低点代表)值，根据设定的高度阈值Th，将点云数据中高度在阈值范围内的点提取出来作为种子点集；之后根据种子点集估计平面模型，采用如下线性模型进行估计：

ax+by+cz+d＝0

n^Tx＝-d

其中n＝[a,b,c]^T，x＝[x,y,z]^T；a、b、c、d为平面模型方程的四个参数。使用协方差矩阵求解此线性模型，种子点集S∈R³的协方差矩阵为：

式中，C为协方差矩阵，S为种子点集，S_i代表此种子点集中第i个点，

为种子点集中所有点均值。此协方差矩阵的三个奇异向量能通过奇异值分解求得，这三个奇异向量描述点集在三个主要方向的散布情况；通过计算具有最小奇异值的奇异向量来求得垂直于平面的法向量n，之后代入

求得d，即能求出平面模型；

求出平面模型以后，计算点云数据中每一个点到该平面的正交投影的距离，与设定阈值比较，若小于阈值，该点为地面点，若大于阈值，则为非地面点。

进一步，所述步骤(5)的具体过程为：

漂移均值计算基本公式如下：

其中s_h是一个半径为h的高维球区域，满足以下关系的y点的集合

S_h(x)＝{y:(y-x)^T(y-x)＜h²}

k是落入s_h区域内的所有样本点的个数，M_h即为求得的MeanShift向量，x_i代表此区域内第i个点；考虑到样本点与被偏移点的距离不同，其偏移量对均值偏移向量的贡献也不同，因此加入高斯核函数，得到如下改进的MeanShift向量形式：

其中，

为高斯核函数，其带宽选为4；h_i为区域s_h半径，由于点云分布于三维空间中，本发明中s_h采用三维球形区域，选取巷道宽度平均值的1/2为此球形半径。

计算出被偏移点MeanShift向量后，根据计算值更新被偏移点位置：

x_t+1＝x_t+M_h(x_t)

式中，x_t为被偏移点当前位置，M_h(x_t)为当前点的MeanShift向量，x_t+1为根据MeanShift向量计算出的被偏移点新的位置。

如此循环迭代，由于巷道中心点云密度最大，被偏移点会逐渐往中心迭代漂移，最终收敛至巷道中心线附近；依次对所有点云数据进行迭代漂移，最终条带状点云会收敛为线状点云。

进一步，所述步骤(7)具体过程为：

将拆分成若干段的地下巷道线状点云分别进行曲线拟合，得到多段无分叉的曲线，每段曲线即为对应某段巷道地面中心线；地下巷道分叉点既是某段曲线的起点，也是另一段曲线的终点，根据这一特征将所有曲线拼接起来，形成能反映真实井下巷道走向的巷道拓扑网络，分叉点即为拓扑网络中的各节点；同时，在进行曲线拟合时计算出各曲线长度信息，保存到生成的巷道拓扑网络中，为最短路径规划算法提供各节点之间距离信息。

Claims (5)

1.一种井下无人驾驶无轨胶轮车的路径规划方法，其特征在于，具体步骤为：

(1)使用固态激光雷达、NUC电脑和移动电源搭建数据采集装置，NUC电脑分别与移动电源和固态激光雷达连接；

(2)将数据采集装置安装在有人驾驶的无轨胶轮车上，使无轨胶轮车在井下巷道内遍历所有行驶区域，数据采集装置的固态激光雷达实时采集数据并反馈给NUC电脑，最终采集获得原始点云数据包；

(3)根据采集到的数据，使用Cartographer算法离线建立井下巷道三维点云地图，其中设定地图原点为井口位置；

(4)对得到的三维巷道地图进行地面分割处理，使用Ground Plane Filter算法分割出地面点云数据，得到条带状地面点云；

(5)使用Meanshift算法对条带状点云进行迭代漂移，使边缘点向着道路中心聚集，从而使条带状分布的道路点细化成线状，得到巷道中心点集；

(6)由于井下巷道具有多个分叉点与端点，在得到的巷道中心点集中将所有巷道分叉点与端点手动标识出来，从而使巷道线状中心点集拆分成若干段无分叉的点集；

(7)根据各标识点之间线状点云数据，分段拟合出各路段中心线，最终拼接成具有拓扑关系的井下巷道路网；

(8)在进行路径规划时，首先确定起点与终点在巷道拓扑网络中的位置，然后使用Dijsktra算法搜索从起点至终点距离最短的路径，最终完成路径规划。

2.根据权利要求1所述的一种井下无人驾驶无轨胶轮车的路径规划方法，其特征在于，所述步骤(2)采集获得原始点云数据包的具体过程为：将数据采集装置安装在有人驾驶的无轨胶轮车前部车头处，接着开启固态激光雷达，以井口位置为起点，使无轨胶轮车以0.3m/s以下的速度在巷道内向前匀速移动，同时使用NUC电脑录制固态激光雷达测量数据的bag包；无轨胶轮车在井下巷道内遍历所有行驶区域，最终获得原始点云数据包。

3.根据权利要求1所述的一种井下无人驾驶无轨胶轮车的路径规划方法，其特征在于，所述步骤(4)的具体过程为：

首先沿车辆行进方向将点云分成若干个子点云段，使每个子点云段中地面近似为平面，减小坡度变化对分割结果的影响，然后对每个子点云段中点云数据使用Ground PlaneFilter分割出地面点云；

Ground Plane Filter算法分割地面具体流程如下：

首先选定点云中n个最低点，计算其平均值，得到LPR值，根据设定的高度阈值Th，将点云数据中高度在阈值范围内的点提取出来作为种子点集；之后根据种子点集估计平面模型，采用如下线性模型进行估计：

ax+by+cz+d＝0

n^Tx＝-d

其中n＝[a,b,c]^T，x＝[x,y,z]^T；a、b、c、d为平面模型方程的四个参数，使用协方差矩阵求解此线性模型，种子点集S∈R³的协方差矩阵为：

式中，C为协方差矩阵，S为种子点集，S_i代表此种子点集中第i个点，

为种子点集中所有点均值；此协方差矩阵的三个奇异向量能通过奇异值分解求得，这三个奇异向量描述点集在三个主要方向的散布情况；通过计算具有最小奇异值的奇异向量来求得垂直于平面的法向量n，之后代入

求得d，即能求出平面模型；

求出平面模型以后，计算点云数据中每一个点到该平面的正交投影的距离，与设定阈值比较，若小于阈值，该点为地面点，若大于阈值，则为非地面点。

4.根据权利要求1所述的一种井下无人驾驶无轨胶轮车的路径规划方法，其特征在于，所述步骤(5)的具体过程为：

漂移均值计算基本公式如下：

其中s_h是一个半径为h的高维球区域，满足以下关系的y点的集合

S_h(x)＝{y:(y-x)^T(y-x)＜h²}

k是落入s_h区域内的所有样本点的个数，M_h即为求得的MeanShift向量，x_i代表此区域内第i个点；考虑到样本点与被偏移点的距离不同，其偏移量对均值偏移向量的贡献也不同，因此加入高斯核函数，得到如下改进的MeanShift向量形式：

其中，

为高斯核函数，其带宽选为4；h_i为区域s_h半径，由于点云分布于三维空间中，本发明中s_h采用三维球形区域，选取巷道宽度平均值的1/2为此球形半径；

计算出被偏移点MeanShift向量后，根据计算值更新被偏移点位置：

x_t+1＝x_t+M_h(x_t)

式中，x_t为被偏移点当前位置，M_h(x_t)为当前点的MeanShift向量，x_t+1为根据MeanShift向量计算出的被偏移点新的位置；

如此循环迭代，由于巷道中心点云密度最大，被偏移点会逐渐往中心迭代漂移，最终收敛至巷道中心线附近；依次对所有点云数据进行迭代漂移，最终条带状点云会收敛为线状点云。

5.根据权利要求1所述的一种井下无人驾驶无轨胶轮车的路径规划方法，其特征在于，所述步骤(7)具体过程为：

将拆分成若干段的地下巷道线状点云分别进行曲线拟合，得到多段无分叉的曲线，每段曲线即为对应某段巷道地面中心线；地下巷道分叉点既是某段曲线的起点，也是另一段曲线的终点，根据这一特征将所有曲线拼接起来，形成能反映真实井下巷道走向的巷道拓扑网络，分叉点即为拓扑网络中的各节点；同时，在进行曲线拟合时计算出各曲线长度信息，保存到生成的巷道拓扑网络中，为最短路径规划算法提供各节点之间距离信息。

Images