CN105806345B - 一种用于变电站巡检机器人激光导航的初始化定位方法 - Google Patents

Abstract

本发明公开了一种用于变电站巡检机器人激光导航的初始化定位方法，包括以下步骤：1）绘制出二维环境地图，记录下地图上关键节点的位姿信息；2）按照高斯分布以记忆位姿为中心局部播撒粒子；3）对粒子进行重新计算，按照粒子滤波算法中采样与重采样的计算方法，对粒子簇进行实时更新；4）经过短暂的运动，粒子簇会呈现收敛的趋势，最终收敛成一个较小的粒子簇；5）对所有粒子进行加权平均计算，得到一个估计位姿，即该时刻机器人的定位结果。本发明采用局部播撒粒子的做法，来快速精确地确认机器人的初始坐标，能够快速实现机器人的初始化定位，使机器人的巡检更加安全流畅。

Description

一种用于变电站巡检机器人激光导航的初始化定位方法

技术领域

本发明属于变电站巡检机器人激光导航技术，特别涉及一种用于变电站巡检机器人激光导航的初始化定位方法。

背景技术

现有变电站巡检机器人的导航定位技术多采用有轨式导航定位，即巡检机器人通过识别自身所在的轨道位置信息，从而得知自身的位置，然后由导航定位系统发布运动命令，使得机器人沿着提前铺设好轨道，到达指定的作业位置。

但是，采用有轨式巡检机器人导航定位技术需要提前铺设好轨道，这带来了大量的基础设施建设工作，而且轨道铺设好以后，巡检机器人的行走路线固定不灵活，不能够根据管理人员的需要进行变动。而采用基于激光导航技术的巡检机器人在工作时将不再依赖轨道进行定位，既能够节省轨道铺设的成本以及工作量，又使巡检工作变得十分灵活，可根据巡检需要进行路线调整。

但是，基于激光导航定位的巡检机器人起始时并不知道自己的位姿(包括位置和运动方向)，通常需要在整个地图播撒粒子，经过机器人的运动实现粒子的收敛，从而最终确定机器人的位姿。然而在整张地图播撒粒子时所需粒子数与地图面积成正比，当地图达到一定面积时，巨额的粒子数所带来的计算时间是我们所不能容忍的，例如为了保证精度，假设我们平均每0.1m*0.1m范围内播撒1个粒子，那么在一个100m*100m的区域内，我们需要播撒100万个粒子来进行初始化，但是如果我们只在一个10m*10m的范围内局部播撒粒子，那么我们所需的粒子数可以缩减到1万个，计算复杂度仅为原来的百分之一。另外，全局播撒粒子的做法，需要在机器人移动一定时间后，我们才能确认机器人的位置，而在变电站里，我们显然不能够让机器人恣意移动，这样可能会导致机器人脱离运动路径，甚至撞向变电站的设备，带来很大的安全隐患。

发明内容

本发明的目的是为了解决上述问题，提供一种能够实现初始阶段粒子的迅速收敛从而快速定位机器人位姿的用于变电站巡检机器人激光导航的初始化定位方法。

为此，本发明的技术方案是：一种用于变电站巡检机器人激光导航的初始化定位方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；其特征在于：包括以下步骤：

1)采用同步定位与地图构建SLAM方法在机器人操作系统ROS中绘制出一张二维环境地图；绘制地图的同时，记录下地图上关键节点的位姿信息；

2)按照高斯分布以记忆位姿为中心局部播撒粒子；从步骤1)中记录的关键节点中选取一个作为巡检机器人的起始位姿，然后以该点为中心，按照高斯分布向周围随机播撒粒子，每一个粒子代表一个可能的机器人的位姿；初始时刻就有了一个以记忆位姿为中心的粒子簇；局部播撒粒子时，假设机器人的初始位姿为需要n个粒子，以为中心按照高斯分布，在机器人周围播撒粒子，利用下列公式来产生一个粒子，

其中用E_trs、E_drft分别表示机器人的移动误差和旋转误差，它们均服从均值为0的高斯分布，j＝1,2,3....n；由此可完成局部播撒粒子的初始化；

3)随着机器人的运动，将激光扫描传感器返回的数据与二维地图做匹配，对粒子进行重新计算，按照粒子滤波算法中采样与重采样的计算方法，对粒子簇进行实时更新；

4)经过短暂的运动，粒子簇会呈现收敛的趋势，最终收敛成一个较小的粒子簇；

5)对所有粒子进行加权平均计算，得到一个估计位姿，该估计位姿就是该时刻机器人的定位结果，由此完成了机器人初始化的定位工作。

本发明所述的同步定位与地图构建SLAM(Simultaneous Localization andMapping)可以描述为:机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。所谓粒子滤波算法就是指：通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被形象的称为“粒子”，故而叫粒子滤波。重采样的目的是剔除权重小的粒子，复制权重大的粒子以满足粒子数目的需要，节省计算时间，它将原有粒子集合进行重新选择并赋予新的等重权，形成新的粒子集。

本发明采用局部播撒粒子的做法，来快速精确地确认机器人的初始坐标；降低了巡检机器人在巡检起始阶段定位的计算复杂度以及计算时间，能够快速实现机器人的初始化定位，使机器人的巡检更加安全流畅。

附图说明

以下结合附图和本发明的实施方式来作进一步详细说明

图1为本发明步骤1)中所建立的二维地图；

图2为本发明步骤2)中以记忆位姿为中心的粒子簇；

图3为本发明步骤4)中粒子收敛后的粒子簇；

图4为本发明的流程框图。

具体实施方式

参见附图。本实施例所述的初始化定位方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；包括以下步骤：

首先需要给予机器人一个记忆位姿作为初始位置，不需要特别精确，在机器人实际位置附近即可，建图的时候，可以获取机器人在每个实际位置的坐标，而该点也作为以后机器人每次巡检的起始点。因此只要机器人的起始点是固定的，就可以以该点为中心，采用局部播撒粒子的做法，来快速精确地确认机器人的初始坐标。

要实现巡检机器人的激光导航定位首先需要有一张地图，同步定位与地图构建SLAM(Simultaneous Localization and Mapping)提供了一个良好的解决办法，采用此方法在机器人操作系统ROS(robot operation system)中可以绘制出一张二维环境地图，如图1所示。在绘制地图的同时，可以记录下关键节点的位姿信息。

接着，需要按照高斯分布以记忆位姿为中心局部播撒粒子，从上述步骤中记录的关键节点中选取一个作为巡检机器人的起始位姿，然后以该点为中心，按照高斯分布向周围随机播撒粒子，每一个粒子代表一个可能的机器人的位姿；初始时刻就有了一个以记忆位姿为中心的粒子簇；如图2所示，图中左下角即为初始时刻的粒子簇A；

局部播撒粒子时，假设机器人的初始位姿为需要n个粒子，以为中心按照高斯分布，在机器人周围播撒粒子，利用下列公式来产生一个粒子，

其中用E_trs、E_drft分别表示机器人的移动误差和旋转误差，它们均服从均值为0的高斯分布，j＝1,2,3....n；由此可完成局部播撒粒子的初始化。

随着机器人的运动，根据激光扫描传感器返回的数据与二维地图做匹配，对粒子进行重新计算，按照粒子滤波算法中采样与重采样的计算方法，对粒子簇进行实时更新。经过短暂的运动，粒子簇会呈现收敛的趋势，最终收敛成一个较小的粒子簇B，如图3中所示。此时，对所有粒子进行加权平均计算，得到一个估计位姿。那么该估计位姿就是该时刻机器人的定位结果，完成了机器人初始化的定位工作。

Claims (1)

1.一种用于变电站巡检机器人激光导航的初始化定位方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；其特征在于：包括以下步骤：

1)采用同步定位与地图构建SLAM方法在机器人操作系统ROS中绘制出一张二维环境地图；绘制地图的同时，记录下地图上关键节点的位姿信息；

2)按照高斯分布以记忆位姿为中心局部播撒粒子；从步骤1)中记录的关键节点中选取一个作为巡检机器人的起始位姿，然后以该点为中心，按照高斯分布向周围随机播撒粒子，每一个粒子代表一个可能的机器人的位姿；初始时刻就有了一个以记忆位姿为中心的粒子簇；局部播撒粒子时，假设机器人的初始位姿为需要n个粒子，以为中心按照高斯分布，在机器人周围播撒粒子，利用下列公式来产生一个粒子，

<mrow> <mover> <mi>&amp;theta;</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>=</mo> <mover> <mi>&amp;theta;</mi> <mo>^</mo> </mover> <mo>+</mo> <msub> <mi>E</mi> <mrow> <mi>d</mi> <mi>r</mi> <mi>f</mi> <mi>t</mi> </mrow> </msub> </mrow>

<mrow> <mi>x</mi> <mrow> <mo>(</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>x</mi> <mo>+</mo> <msub> <mi>E</mi> <mrow> <mi>t</mi> <mi>r</mi> <mi>s</mi> </mrow> </msub> <mo>*</mo> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <mover> <mi>&amp;theta;</mi> <mo>^</mo> </mover> <mo>(</mo> <mi>j</mi> <mo>)</mo> <mo>)</mo> </mrow> </mrow>

<mrow> <mi>y</mi> <mrow> <mo>(</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>y</mi> <mo>+</mo> <msub> <mi>E</mi> <mrow> <mi>t</mi> <mi>r</mi> <mi>s</mi> </mrow> </msub> <mo>*</mo> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <mover> <mi>&amp;theta;</mi> <mo>^</mo> </mover> <mo>(</mo> <mi>j</mi> <mo>)</mo> <mo>)</mo> </mrow> </mrow>

其中用E_trs、E_drft分别表示机器人的移动误差和旋转误差，它们均服从均值为0的高斯分布，j＝1,2,3....n；由此可完成局部播撒粒子的初始化；

3)随着机器人的运动，将激光扫描传感器返回的数据与二维地图做匹配，对粒子进行重新计算，按照粒子滤波算法中采样与重采样的计算方法，对粒子簇进行实时更新；

4)经过短暂的运动，粒子簇会呈现收敛的趋势，最终收敛成一个较小的粒子簇；

5)对所有粒子进行加权平均计算，得到一个估计位姿，该估计位姿就是该时刻机器人的定位结果，由此完成了机器人初始化的定位工作。