[发明专利]一种基于GPS闭环检测的变电站巡检机器人建图方法

权利要求书

1.一种基于GPS闭环检测的变电站巡检机器人建图方法，其特征在于，包括：

获取激光点云数据的步骤：

机器人按照指定路线行驶，在行驶过程中记录二维激光数据、里程计数据、惯性导航单元(IMU)数据和GPS数据；

对机器人的位姿初值进行估算；

求取相邻两帧激光数据中位姿转换关系；

将二维激光数据处理处理为激光点云数据；

计算激光点云在激光传感器坐标系中的位姿；

拼接激光点云数据的步骤：

在全局坐标系中，利用所述相邻两帧激光中位姿间转换关系和上一帧激光点云数据，对帧间激光点云数据进行拼接；

瓦片式建图的步骤：

利用连续的激光点云数据生成子图，连续的指定帧数的激光数据生成一张子图，后一子图中包含前一子图中在后指定帧数的激光点云数据；

闭环检测的步骤：

选取子图的指定帧为关键帧；

根据GPS数据实时定位出机器人的当前位置，以该位置为圆心，指定距离为半径，界定闭环候选区域；

筛选闭环候选区域中各子图中各个位候选位姿，当某位姿符合筛选条件时，将该位姿对应的激光帧与关键帧进行闭环连接。

2.根据权利要求1所述的基于GPS闭环检测的变电站巡检机器人建图方法，其特征在于，所述对机器人的位姿初值进行估算的方法包括：

根据里程计数据、IMU数据建立机器人的位姿预测方程，用于更新粒子的状态；

利用里程计数据、二维激光传感器数据、GPS数据建立观测模型，用于计算粒子的权重；

通过计算粒子群的加权平均值得到机器人位姿的初值估算值。

3.根据权利要求1所述的基于GPS闭环检测的变电站巡检机器人建图方法，其特征在于，所述瓦片式建图的步骤中，子图采用栅格地图的形式表示，区别表示每个栅格是空闲还是占据状态。

4.根据权利要求1所述的基于GPS闭环检测的变电站巡检机器人建图方法，其特征在于，所述瓦片式建图的步骤中，每建立一张子图，进行一次闭环检测。

5.根据权利要求1所述的基于GPS闭环检测的变电站巡检机器人建图方法，其特征在于，所述筛选的具体方法为：

计算闭环候选区域中各子图的置信度，计算公式如下：

其中，m为该子图中某一帧激光数据对应的激光点云占据的地图栅格总数，设第n个地图栅格的地图坐标为(x_n,y_n)，则该地图栅格置信度是

根据每个子图中机器人的每个位姿与当前帧对应位姿的位姿差来计算各子图中每个候选位姿对应的置信度权重，公式如下：

其中，x_offset是某个子图中某个候选位姿与当前位姿间沿x轴的位移，y_offset是某个子图中某个候选位姿与当前位姿间沿y轴的位移，trans_weight是位移权重，candiate.rotation是某个子图中某个候选位姿与当前位姿间旋转角度，rot_weight是旋转角度权重；

计算每个子图的置信度candidate_probability与该子图中某个候选位姿的置信度权值candidate_weight的乘积，作为当前闭环候选位姿的置信度分值，公式如下：

score＝candidate_probability*candidate_weight

选择置信度分值score最高的位姿，当该位姿的置信度分值score大于某一阈值时，则该位姿符合筛选条件。

6.根据权利要求1～5任一所述的基于GPS闭环检测的变电站巡检机器人建图方法，其特征在于，所述将二维激光数据处理处理为激光点云数据时还包括，在每得到一帧激光数据后，先检查激光数据的帧率是否满足阈值的要求，如果小于阈值则表示不满足阈值的要求，此时不使用该激光数据并上报告警，等待接收到下一帧激光数据。

7.根据权利要求6所述的基于GPS闭环检测的变电站巡检机器人建图方法，其特征在于，在每得到一帧激光数据后，如该激光数据的帧率满足阈值的要求，则先对该激光数据中的各激光反射点进行过滤，去掉各激光反射点中相距较近的点和较远的点，剩余的各激光反射点再用于后续各激光反射点在在激光传感器坐标系中的位姿的计算。