一种室外变电站巡检机器人定位系统及其方法

技术领域

本发明涉及机器人定位技术领域，尤其是涉及一种室外变电站巡检机器人定位系统及其方法。

背景技术

变电站巡检机器人由移动载体、通信设备和检测设备等组成，是一种釆用遥控或全自主运行模式，用于变电站设备巡检作业的移动巡检装置。为保证巡检任务的可靠完成，必须对巡检机器人进行定位，目前，机器人的定位系统主要可划分为相对定位系统和绝对定位系统二种，相对定位一般使用航迹推演法，这也是比较传统的一种方法，通过里程计的数据信息，并在知道初始方向和位置前提下，获取机器人的姿态，但是这个方法会产生位置偏差累积现象；而绝对定位系统的研发方法则更具备普遍性，主要有激光定位、无线射频定位、视觉传感器定位、超声波定位和GPS定位等方式。

GPS定位本身的精确度能做到得非常高，但容易受高电磁波的影响，而室外变电站环境中部分区域存在高电磁波，会导致GPS定位产生较大的误差，其受干扰时定位精度仅仅只有米级，这对于巡检机器人来说，将存在很大的安全隐患；航迹推算定位通过里程计推算机器人的位置，其定位的精度会随着移动机器人移动距离的增加而不断降低，其误差也会不断累积；激光雷达在大环境的情况下，难以采集到足够的特征信息使机器人完成自定位；视觉传感器器定位计算量大、实时性差；无线射频定位投资大，定位精度低；IMU提供的信息只有角速度比较准确。由此可知，这些定位方式都难以实现巡检机器人的精确定位，导致机器人偏离规划的路径，而且在室外变电站中，其地面较为粗糙，进一步增加了巡检机器人的定位误差，使得巡检机器人更加容易偏离设定的巡检路线。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种室外变电站巡检机器人定位系统及其方法，以能够对室外变电站巡检机器人进行精准定位，避免发生机器人偏离巡检路线的问题。

本发明的目的可以通过以下技术方案来实现：一种室外变电站巡检机器人定位系统，包括安装在巡检机器人主体上的里程计、IMU和激光雷达，所述里程计、IMU和激光雷达分别连接至数据采集模块，所述数据采集模块连接至处理器，所述数据采集模块用于实时获取里程计数据、IMU数据和激光雷达信息，并将获取的数据信息传输给处理器；

所述处理器用于对里程计数据、IMU数据、激光雷达信息进行融合处理分析，以确定出巡检机器人的位姿数据。

进一步地，所述处理器包括依次连接的第一融合模块和第二融合模块，所述第一融合模块用于融合里程计数据和IMU数据，以得到第一融合数据；

所述第二融合模块用于融合第一融合数据和激光雷达信息，以确定出巡检机器人的位姿数据。

进一步地，所述第一融合模块采用EKF(Extended Kalman Filter，扩展卡尔曼滤波)进行数据融合处理。

进一步地，所述第二融合模块采用AMCL(Adaptive Monte Carlo Localization，自适应蒙特卡洛定位)算法进行数据融合处理。

进一步地，所述巡检机器人具体为双轮差速机器人。

一种室外变电站巡检机器人定位方法，包括以下步骤：

S1、获取里程计数据、IMU数据和激光雷达信息；

S2、采用EKF融合里程计数据和IMU数据，得到EKF融合信息；

S3、采用AMCL算法将EKF融合信息与激光雷达信息进行融合，以确定出巡检机器人位姿数据。

进一步地，所述步骤S2具体包括以下步骤：

S21、根据巡检机器人的运动模式和里程计数据，构建巡检机器人的运动模型；

S22、将巡检机器人的运动模型展开成线性方程，并计算雅克比矩阵；

之后结合IMU数据，通过构建观测方程，计算得到后验证位姿数据。

进一步地，所述巡检机器人的运动模型具体为：

其中，C

进一步地，所述步骤S22的具体过程为：

利用一阶泰勒展开公式将运动模型展开成线性方程：

其中，

IMU的真实测量值为(0,0,θ

z

其中，z

根据观测矩阵H

计算先验估计值

P

计算卡尔曼增益矩阵K

其后验证位姿

再次更新协方差矩阵：

其中，I为单位矩阵。

进一步地，所述步骤S3中将EKF融合信息与激光雷达信息进行融合的具体过程为：

S31、初始化处理：设置初始值x

其中，

S32、重要性采样：

预测步：

通过EKF阶段的融合结果为预测步提供先验分布

预测步采样更新的粒子为：

经过状态转移方程更新后的粒子集为：

更新步：

将激光雷达和点云匹配后的定位结果x

其中，f

对所有粒子的权重进行归一化：

S33、重采样：

利用KLD算法进行重采样，设定有效粒子数阈值为N

其中，

若N

重采样完成后返回步骤S32，循环执行；

S34、改进提议分布：

在AMCL算法中，带权重的粒子集合为

其中，

与现有技术相比，本发明通过数据采集模块将里程计数据、IMU数据以及激光雷达信息传输至处理器，利用处理器对数据信息进行融合处理，通过融合多种传感器信息，使各数据信息互补，以确定机器人位姿数据，实现机器人定位，能够有效提高定位精度、从而避免后续机器人偏离设定的巡检路线。

本发明分别采用EKF融合里程计数据和IMU数据、采用AMCL算法将EKF融合信息与激光雷达信息进行融合，由此能够保证数据信息融合的准确性，使得各数据信息之间能够很好地互补、进一步减小各数据信息本身的定位误差，从而保证最终定位的准确性。

附图说明

图1为本发明的系统结构示意图；

图2为本发明的方法流程示意图；

图3为实施例中单独使用里程计以及采用本发明方法得到的机器人运动曲线对比示意图；

图4为实施例中单独使用里程计以及采用本发明方法得到的定位误差对比示意图；

图中标记说明：1、巡检机器人，102、里程计，102、IMU，103、激光雷达，2、数据采集模块，3、处理器。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

如图1所示，一种室外变电站巡检机器人定位系统，包括安装在巡检机器人1主体上的里程计101、IMU102和激光雷达103，里程计101、IMU102和激光雷达103分别连接至数据采集模块2，数据采集模块2连接至处理器3，其中，数据采集模块2用于实时获取里程计数据、IMU数据和激光雷达信息，并将获取的数据信息传输给处理器3；

处理器3用于对里程计数据、IMU数据、激光雷达信息进行融合处理分析，以确定出巡检机器人1的位姿数据。

具体的，处理器3包括依次连接的第一融合模块和第二融合模块，第一融合模块用于融合里程计数据和IMU数据，以得到第一融合数据；第二融合模块则用于融合第一融合数据和激光雷达信息，以确定出巡检机器人的位姿数据。

本实施中，第一融合模块采用EKF(Extended Kalman Filter，扩展卡尔曼滤波)进行数据融合处理，第二融合模块采用AMCL(Adaptive Monte Carlo Localization，自适应蒙特卡洛定位)算法进行数据融合处理；巡检机器人1采用双轮差速机器人。

将上述系统应用于实际，以实现一种室外变电站巡检机器人定位方法，如图2所示，包括以下步骤：

S1、获取里程计数据、IMU数据和激光雷达信息；

S2、采用EKF融合里程计数据和IMU数据，得到EKF融合信息；

S3、采用AMCL算法将EKF融合信息与激光雷达信息进行融合，以确定出巡检机器人位姿数据。

在利用EKF融合里程计和IMU数据时，本实施例采用的是双轮差速机器人和里程计圆弧运动模型，其运动模型为：

其中，C

利用一阶泰勒展开公式将运动模型展开成线性方程：

计算雅克比矩阵

IMU的真实测量值为(0,0,θ

z

式中，z

根据观测矩阵H

计算先验估计值

P

计算卡尔曼增益矩阵K

其后验证位姿

再次更新协方差矩阵：

其中，I为单位矩阵。

在利用AMCL算法融合EKF融合后信息和激光雷达信息时，自适应蒙特卡洛算法的预测步是使用EKF融合里程计和IMU输出的后验位姿(x

再根据激光雷达和点云匹配得到的定位结果x

x

z

①初始化

设初始值x

②重要性采样

预测步：

通过EKF阶段的融合结果为预测步提供先验分布

预测步采样更新的粒子为：

经过状态转移方程更新后的粒子集为：

更新步：

将激光雷达和点云匹配后的定位结果x

其中，f

对所有粒子的权重进行归一化：

③重采样

用KLD算法进行重采样，有效粒子数阈值为N

重采样完成后返回到第二步，循环执行。

④改进提议分布

在AMCL算法中，带权重的粒子集合为

其中提议分布为：

在自适应蒙特卡洛算法中，其提议分布用机器人运动学模型代替，因此在粒子权值的计算中存在误差，由于当前粒子的权值是由上一时刻权值推算而来，随着粒子权值计算次数的增加，其误差也会累计，权值方差变大，导致随着机器人移动的粒子权值越来越大，其他粒子权值越来越小，即粒子退化问题。

因此将提议分布改为：

为验证本发明方法的有效性，本实施例分别采用单独使用里程计以及本发明方法进行机器人定位实验，具体过程为：

(1)实验时，首先启动gazebo环境和可视化工具rviz，然后人为控制巡检机器人以恒定的速度运动一段距离，使用bag工具记录巡检机器人里程计定位数据以及多传感器融合后的定位数据；

(2)然后，使用Matlab读取bag包文件，根据bag包的数据生成对应的轨迹，并输出里程计定位和融合算法的定位误差，其轨迹和误差如图3和图4所示。使用Matlab绘制机器人输出的位置信息，绘制融合多传感器数据后的定位数据、单独使用里程计定位数据和机器人的参考路径，参考轨迹为实际运动的曲线，如图3所示。由图3可以得出，与参考轨迹相比，里程计的定位信息随着机器人移动距离的增长不断偏离机器人的实际轨迹，而使用本发明融合定位算法后，机器人的移动轨迹明显更接近与参考轨迹。从图4中可以得出，里程计的定位误差随着距离不断增大，而使用本发明融合定位算法的误差波动在一个很小范围、定位误差更小。

综上所述，本技术方案提出一种可以用于室外变电站巡检机器人的定位方法，通过拓展卡尔曼算法和自适应蒙特卡洛算法，以融合里程计、IMU和激光雷达的数据信息，从而进行融合定位，此外，还通过改善自适应蒙特卡洛算法中的提议分布、优化粒子权值，可以进一步提高室外变电站巡检机器人定位精度，使得巡检机器人能够在无人干预的情况下顺利且快速地完成巡检任务。本技术方案通过融合多传感器信息，提高了室外变电站巡检机器人定位精度，使巡检机器人可以较为准确到达指定区域对电气设备进行巡检，获取各项运行数据来综合评价电气设备是否处于正常工作状况、保证室外变电站的安全运行。

需要说明的是，在实际应用中，大多采用二维激光雷达，其所采集的环境特征较少，而变电站作为一个相对空旷的环境，二维激光雷达难以采集到足够的特征信息，所以为了充分利用环境中的信息，可以将二维雷达换成三维雷达，以提高机器人定位成功率；而且可以进一步考虑到轮胎打滑和道路不平等问题，依靠里程计、IMU和激光雷达的定位误差也会越来越大，为了提高定位准确率，可以增加传感器数量或者添加信标的方式增加机器人定位成功率和准确性。