[发明专利]一种井下无人驾驶无轨胶轮车的路径规划方法

权利要求书

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，即能求出平面模型；

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