一种基于GPS闭环检测的变电站巡检机器人建图方法与流程

本发明属于智能机器人技术领域，具体涉及一种基于gps闭环检测的变电站巡检机器人建图方法。

背景技术：

变电站建图是激光导航变电站机器人运行前的必要环节，主要将变电站的地图尽可能详细和精准的通过数据的形式记录下来。激光导航变电站巡检机器人使用该地图与激光传感器实时获取的周围信息做比对，并通过定位算法确定变电站巡检机器人的位置。所以变电站地图的精度对于激光导航变电站机器人的导航定位至关重要。

变电站机器人的二维激光建图是机器人根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。常用方法是通过激光雷达和编码器共同采集数据，后期将数据融合，从而生成变电站地图，该方法具有以下不足：

(1)采用单一的定位传感器，难以满足机器人的定位精度集可靠性要求；

(2)存在较大的累计误差，导致部分环形道路无法闭合；

(3)变电站建图过程的数据量庞大，建图效率低下。

技术实现要素：

本发明目的是：针对现有技术的不足，提供一种基于gps闭环检测的变电站巡检机器人建图方法。本方法可以有效提高建图效率、精度和准确度。

具体地说，本发明是采用以下技术方案实现的，包括：

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

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

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

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

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

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

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

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

瓦片式建图的步骤：

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

闭环检测的步骤：

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

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

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

进一步而言，所述对机器人的位姿初值进行估算的方法包括：

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

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

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

进一步而言，所述瓦片式建图的步骤中，子图采用栅格地图的形式表示，区别表示每个栅格是空闲还是占据状态。

进一步而言，所述瓦片式建图的步骤中，每建立一张子图，进行一次闭环检测。

进一步而言，所述筛选的具体方法为：

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

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

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

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

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

score＝candidate_probability*candidate_weight

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

进一步而言，所述将二维激光数据处理处理为激光点云数据时还包括，在每得到一帧激光数据后，先检查激光数据的帧率是否满足阈值的要求，如果小于阈值则表示不满足阈值的要求，此时不使用该激光数据并上报告警，等待接收到下一帧激光数据。

进一步而言，在每得到一帧激光数据后，如该激光数据的帧率满足阈值的要求，则先对该激光数据中的各激光反射点进行过滤，去掉各激光反射点中相距较近的点和较远的点，剩余的各激光反射点再用于后续各激光反射点在在激光传感器坐标系中的位姿的计算。

本发明的有益效果如下：采用本发明的基于gps闭环检测的变电站巡检机器人建图方法，采用里程计、imu、gps等多传感器信息融合的定位方式，提高了机器人的定位精度和可靠性；通过提取关键帧，并结合gps定位筛选的闭环检测方法，有效减少了匹配候选集的数量，提高了闭环检测速度，进而提高了变电站建图效率；在闭环检测中利用gps的定位数据，有效减小了由于相似场景造成的误闭环率，提高了闭环检测的精度和准确度，最终使得变电站地图完美闭合。

附图说明

图1是本发明实施例的建图流程图。

图2是本发明实施例的机器人定位方法流程图。

图3是本发明实施例的闭环检测流程。

图4是本发明实施例的闭环候选区域示意图。

具体实施方式

下面结合实施例并参照附图对本发明作进一步详细描述。

实施例：

本发明的一个实施例，介绍了一种基于gps闭环检测的变电站巡检机器人建图方法。

本实施例采用的机器人，其硬件系统传感器主要包括里程计、惯性导航单元、三维激光传感器，其中里程计可以用来校准位置的预测值，惯性导航单元可以用来校准线速度、角速度，三维激光传感器用来获得激光数据。机器人软件系统使用机器人操作系统ros，这是一种常用的机器人软件平台，它能为异质计算机集群提供类似操作系统的功能，在ros系统中包括实现定位功能的节点。当然，可以理解的是，本发明方法也可以通过其他机器人的软件系统加以实现，本实施例采用ros系统仅作为一种实现方式。

本实施例中的地图以及机器人的导航运动假定在一个二维平面以内，使用的坐标系包括地图坐标系、机器人坐标系、激光传感器坐标系。

地图坐标系是全局坐标系。计算机器人位姿时使用的是地图坐标系。

机器人坐标系是以机器人为原点的坐标系，二维导航中，一般是以机器人的中心点为原点。

激光传感器坐标系是以激光传感器的中心位置为原点的坐标系，激光数据的位姿使用的是激光传感器坐标系。

需要把不同坐标系下面的数据转化到同一个坐标系，才能进行位姿比较和计算。不同坐标系间坐标值的转换可以通过ros系统中tf模块(坐标转换模块)实现。

参照图1，本实施例的基于gps闭环检测的变电站巡检机器人建图方法，其实现的主要步骤分别在前端和后端完成。其中，前端处理器为机器人内置的嵌入式工控机，完成激光点云数据的获取和处理、激光点云数据拼接、瓦片式建图，将子图的激光帧发送给后端处理器；后端处理器采用高计算性能的计算机根据接收的子图的激光帧及gps数据进行闭环检测，并将生成的栅格地图返回给前端处理器。具体步骤如下：

1、激光点云数据的获取和处理

1-1)变电站巡检机器人接收巡检任务，按照指定路线行驶，在行驶过程中以固定采样频率记录二维激光数据、里程计数据、惯性导航单元(imu)数据和gps数据。

1-2)利用粒子滤波算法，结合里程计、imu、gps等多传感器融合的数据信息，对机器人的位姿初值进行估算。

在机器人实时定位过程中，有一些粒子可能偏离目标太远。这些粒子是我们想要的系统状态的可能性非常小，因此取得的权值非常小。如果只根据粒子传播来改变粒子的状态的话，偏离的粒子永远是偏离的，很难再回到目标周围。这样，偏离的粒子可能越积越多，从而导致定位出现偏差。本实施例中，使用粒子滤波进行多传感器融合的初始位姿估算。具体而言，根据里程计数据、imu数据建立机器人的位姿预测方程，利用里程计数据、二维激光传感器数据、gps数据建立观测模型。位姿预测方程用于更新粒子的状态，观测模型用于计算粒子的权重，最后通过计算粒子群的加权平均值得到机器人位姿的初值估算值，如图2所示。

1-2-1)通过来自机器人的二维激光传感器的两帧激光数据中位姿，确定机器人初始位姿p0(x0,y0,θ0)以及初始速度

1-2-2)根据最新接收的里程计、imu数据获取机器人的加速度其中为t时刻机器人在x轴方向的加速度，为t时刻机器人在y轴方向的加速度，为t时刻机器人角加速度。结合初始速度更新机器人的运动速度，建立机器人的位姿预测方程。假设t时刻机器人的速度为其中，，为t时刻机器人在x轴方向的速度、为t时刻机器人在y轴方向的速度，为t时刻机器人角速度，则机器人的位姿预测方程可以表示为：

其中，pt、pt-1分别为机器人在t时刻、t-1时刻接收连续两帧激光数据对应的位姿，为t-1时刻机器人的速度，为t-1时刻机器人在x轴方向的速度、为t-1时刻机器人在y轴方向的速度，为t-1时刻机器人角速度，δt为t时刻与t-1时刻间的时间差，at-1为t-1时刻机器人的加速度，ut-1为t-1时刻的系统噪声。

根据最新接收的里程计数据、二维激光传感器数据、gps建立如下观测模型：

zt，i＝(xt，i，yt，i，θt，i)

zt，l＝(xt，l，yt，l，θt，l)

zt，g＝(xt，g，yt，g，θt，g)

zt＝λ1zt，i+λ2zt，l+λ3zt，l＝(xt，yt，θt)

其中，zt,i为里程计的观测模型，xt,i为机器人在t时刻接收的里程计数据中x轴方向的数据，yt,i为机器人在t时刻接收的里程计数据中y轴方向的数据，θt,i为机器人在t时刻接收的里程计数据中角速度；zt,l为二维激光传感器的观测模型，xt,l为机器人在t时刻接收的二维激光传感器数据中x轴方向的数据，yt,l为机器人在t时刻接收的二维激光传感器数据中y轴方向的数据，θt,l为机器人在t时刻接收的二维激光传感器数据中角速度；zt,g为gps的观测模型，xt,g为机器人在t时刻接收的gps数据中x轴方向的数据，yt,g为机器人在t时刻接收的gps数据中y轴方向的数据，θt,g为机器人在t时刻接收的gps数据中角速度；zt是对里程计数据、二维激光传感器数据、gps融合后最终的观测模型，xt为t时刻综合里程计、二维激光传感器、gps三者在x轴方向的数据，最终机器人在x轴方向的数据，yt为综合里程计、二维激光传感器、gps三者在y轴方向的数据，最终机器人在y轴上的数据，θt为t时刻机器人的角速度，λ1、λ2、λ3分别为里程计、二维激光传感器和gps的观测值的权重，在实际中满足λ1+λ2+λ3＝1，且λ2＞λ1＞λ3。

以机器人的位姿预测方程为输入，进行二维激光定位，可以有效避免因粒子退化问题导致的定位漂移现象。

1-2-3)生成粒子。根据预先确定机器人的初始位姿p0＝(x0,y0,θ0)，初始时刻在机器人的运动范围内随机生成n个均匀分布的粒子，且粒子具有三个特征。

1-2-4)利用机器人运动模型预测状态。根据1-2-2)中求取的机器人的运动位姿预测方程，更新1-2-3)中每一个粒子的状态。由于控制系统存在噪声，需加入合理的噪声。合理的噪声通常情况下指的是具有正态分布统计特性的高斯噪声，第一，高斯型白噪声可用具体数学表达式表述，便于推导分析和运算；第二，高斯噪声确实反映了实际控制系统中的加性噪声情况，比较真实地代表了控制系统中噪声的特性。

1-2-5)更新粒子权值。使用里程计、二维激光传感器和gps得到的观测值代入1-2-2)的观测模型更新粒子的权重，随着观测值的依次到达，为每个粒子计算相应的权重。该权重值代表了预测的位姿取每个粒子时获得观测模型的概率。对所有粒子都进行这样的评价。越接近观测值的粒子，获得的权重越高。权重的计算方式如下：

ωi＝1/di

其中，ωi为第i个粒子的权重，di为第i个粒子距离观测模型取值的欧氏距离。

1-2-6)计算状态变量估计值。通过粒子群的加权平均值计算状态变量的估计值即机器人接收第k帧激光数据时在t时刻机器人位姿的估计值pose_estimated。

1-2-7)重采样。为了解决计算过程中的权值退化问题，采用有效粒子数neff衡量粒子的退化程度：

其中ωi为第i个粒子的权重。有效粒子数neff越小，表明权重退化越严重。

当neff的值小于某一阈值nth时，进行重采样；否则，进行下一时刻数据的处理，根据下一时刻的机器人的运动位姿预测方程更新粒子的状态，求取下一时刻机器人的位姿估计值。重采样的具体方法为：根据粒子的权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，舍弃一小部分权重小的粒子，代之以权重较大的粒子。

1-3)求取相邻两帧激光数据中位姿转换关系。根据机器人分别在t时刻、t-1时刻接收连续两帧激光数据对应的位姿pt、pt-1，求解pt＝pt-1rt-1+tt-1，得到机器人连续两帧激光数据对应的位姿间的转换关系(rt-1,tt-1)。其中，rt-1为旋转位移矩阵，代表pt与pt-1之间的旋转角度位移；rt-1为直线位移矩阵，代表pt与pt-1之间的直线位移。

1-4)将激光数据进行处理得到激光点云数据，对激光点云数据进行过滤，即去掉激光点云数据中的各激光反射点中相应的噪点(相距较近的点和较远的点)，剩余作为有效点。计算有效激光点云在激光传感器坐标系中的位姿。具体如下：

每得到一帧来自机器人的二维激光传感器的激光数据laserscan后，ros系统对激光数据进行处理得到激光点云数据pointcloud，该激光点云数据pointcloud是每帧激光数据所包含的laserscan.size()个激光数据点信息的统称，反映了各激光反射点在激光传感器坐标系中的坐标。不同激光传感器的帧率、扫描角度、每帧包含的数据点数量不同，ros系统在定位过程中使用激光点云数据来进行处理和计算。

针对稀疏环境的特点，可以对激光点云数据pointcloud进行过滤，即去掉激光点云数据pointcloud中的各激光反射点中相应的噪点(相距较近的点和较远的点)，剩余作为有效点，可以提高位姿估计的置信度。由于对每一帧激光数据对应的激光点云数据pointcloud进行过滤的结果不同，每一帧激光数据对应的有效点的值不同。例如，在laserscan.size()为1141的传感器上，有效点约为几百个。对于激光数据laserscan，要进行帧率的检测，即在每得到一帧激光数据后，检查激光数据laserscan的帧率是否小于阈值，如果小于阈值则上报告警并等待接收到下一帧激光数据后重新进行相应处理。例如，如果定义的帧率是25hz，而得到相邻两帧激光数据的时间大于40ms，则激光数据laserscan不符合要求，可能激光传感器出现过热等故障，需要上报告警并等待下一帧激光数据。

2、激光点云数据拼接

在全局坐标系中，利用步骤1-3)中求得的相邻两帧激光点云的位姿间转换关系(rt-1,tt-1)和上一帧激光点云数据，对帧间激光点云进行拼接，拼接公式为：

qk＝qk-1rk-1+tk-1

其中，qk-1、qk分别为第k-1帧、第k帧激光点云数据。

3、瓦片式建图

根据转换关系将全局坐标系中帧间激光点云数据映射到地图坐标系中，地图采用栅格地图形式表示，栅格的大小决定地图的分辨率，每个栅格用s＝0或s＝1表示该格点是空闲还是占据状态。对于每一个能观察到的栅格，灰色部分表示激光末端点所落在的栅格，即检测到存在障碍物，表示被占据的状态，白色表示空闲状态。每连续的40帧激光数据生成一张子图，第二张子图包含上一张子图的后20帧激光数据。例如，将第t帧到t+40帧激光数据进行配准，形成子图a1，第t+20帧到t+60帧激光数据进行配准，形成子图a2，第t+40帧到t+80帧激光数据进行配准，形成子图a3，以此类推。

4、闭环检测

连续编号的子图间利用激光帧间位姿进行匹配，从而在子图间建立了连接；而闭环检测是在相关联的不连续编号的子图间间建立连接，以减少这两个子图间的激光帧的累积误差。选择每张子图的指定激光帧(例如最后一帧)或者指定数个激光帧作为关键帧。通过对子图间关键帧的位姿信息进行闭环检测，有效减少匹配候选集的数量，提高检测速度。每当步骤3中建立一张子图，结合gps数据，进行一次闭环检测。具体方法如下：

4-1)如图4所示，假设地图呈s型。gps数据实时定位出机器人的当前位置，以该位置为圆心，指定距离(例如5米)为半径，定位筛选出闭环候选区域，即图4中圆形阴影部分。

4-2)计算闭环候选区域中机器人各子图的置信度candidate_probability，计算公式如下：

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

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

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

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

score＝candidate_probability*candidate_weight

4-3)选择置信度分值score最高的位姿，当该位姿的得分score大于某一阈值时，该位姿对应的激光帧与关键帧进行闭环连接，表明该位姿对应的激光帧相应的子图与该关键帧所在的子图在地图上位置相邻。

虽然本发明已以较佳实施例公开如上，但实施例并不是用来限定本发明的。在不脱离本发明之精神和范围内，所做的任何等效变化或润饰，同样属于本发明之保护范围。因此本发明的保护范围应当以本申请的权利要求所界定的内容为标准。