一种变电站智能巡检机器人高精度定位方法与流程

本发明属于机器人定位
技术领域：
，具体涉及一种变电站智能巡检机器人高精度定位方法。
背景技术：
电能对国民经济和人民的日常生活有着至关重要的作用，电网系统发生故障，将给人们带来不可估量的损失，严重影响正常的生产生活。确保电网系统的安全性和可靠性运行关乎国家和人民的切身利益，也是我国电网系统亟待解决的关键问题。输变电系统是电网系统的关键组成部分，变电站设备的安全、可靠运行对保证整个输变电系统的运行质量至关重要。随着科技的发展，对电网系统中设备的各方面的性能要求也提升到了一个新高度，变电站设备的性能水平直接影响到电网系统的稳定性、可靠性、安全性以及抵抗事故的能力。保障设备的可靠安全运转已是一项重大任务，因此要高度重视变电站设备的监控和管理，做好变电站设备的巡检、巡视、实时监控工作，及时排除潜在的危险，是保证变电站稳定运行的重中之重。依靠人工来检查变电站的故障情况，很大程度上依靠人的感官，效率低下，且对严重事故会对人员生命安全造成威胁，应用智能巡检机器人对变电站进行巡检已经成为了变电站工作发展的主流方向。自主导航系统的开发将推动变电站智能巡检机器人发展，变电站智能巡检机器人辅助运行人员对设备进行巡视，可及时发现电力设备的热缺陷、异物悬挂等设备异常现象，规避设备运行隐患，真正做到提高工作效率和巡视质量，起到减员增效的作用，有效的推进了变电站无人值守的进程。目前，变电站巡检机器人的应用多处于有轨式，这样需要对变电站进行二次施工，增加运营成本。而随着激光技术的成熟，激光设备价格的下降，越来越多的研究人员开始研究无轨机器人。在变电站这种特种环境中，需要机器人拥有高精度定位能力才能完成相应的巡检任务，所以寻求新的机器人定位方法变得很重要。现有技术的机器人定位研究工作主要有以下几种方法：(1)文献“arobustandmodularmulti-sensorfusionapproachappliedtomavnavigation”基于iekf方程总结了multi-sensor-fusionekf来融合传感器数据，文献“robustvision-aidednavigationusingsliding-windowfactorgraphs”综合了长期滑动窗(isam2)和短期滑动窗(sliding-windowfactorgraphs)方法融合位姿感器，从而得到多传感器融合后的机器人位姿。(2)文献“enhancedmontecarlolocalizationincorporatingamechanismforpreventingprematureconvergence”将粒子滤波与移动机器人运动和感知的概率模型相结合，提出了移动机器人蒙特卡罗定位(mcl)的思想。(3)文献“mapbasedindoorrobotnavigationandlocalizationusinglaserrangefinder”采用的基于地图的方法，移动机器人通过地图上的一组顶点与激光数据的一组顶点进行匹配实现全局定位。(4)文献“thepositionaccuracyofmobilerobotlocalizationbasedonparticlefilterscombinedwithscanmatching”公开了一种粒子滤波混合扫描匹配方法，实现移动机器高精度导航定位。以上方法都在一定程度上解决了机器人定位的问题，但依然存在很多局限，主要表现在：(1)定位精度不高。现有技术的定法方法中多采用一种或两种算法来解决机器人定位问题，没有针对不同传感器使用特定算法。ekf算法使用一阶近似项作为原状态方程和测量方程的近似表达式，估计均值只有二阶精度，但这对于一个高精度系统而言还远远不够，ekf假设过程噪声是零均值的高斯噪声，而在实际系统中往不是这样的噪声。而且由于车轮打滑、imu传感器温度漂移会加重ekf的误差。蒙特卡罗定位算法由于粒子数量有限、粒子退化、栅格地图分辨率有限等问题，造成了估计位姿估计不准确，有可能还会出现错误位姿。扫描匹配算法存在误差积累效应，且如果icp的迭代初始解选择不准确会加重计算负担和加大误差。(2)计算效率与定位精度不能兼顾。当以上方法单独使用时可以保证机器人运行的时效性，但是定位精度不高。如果想提高ekf融合数据的精度，可以增加线性近似的阶数，但这样会增加计算时间。如果想要提高蒙特卡洛定位算法估算精度，最直接的方式就是增加粒子数，虽然增加粒子数可以提高位姿估计的精度，但会增加计算成本，影响机器人运行的流畅性。如果想要提高扫描匹配算法中迭代求解方法的匹配精度，最直接的方法是增加迭代次数，但计算耗时会成倍增加。以上方式都存在计算效率和定位精度不能兼顾的问题。技术实现要素：本发明的目的是解决现有技术定位算法在求解机器人位姿时存在较大误差，难以保证变电站巡检任务完成的问题，提出一种变电站智能巡检机器人高精度定位方法。本发明所提出的技术问题是这样解决的：一种变电站智能巡检机器人高精度定位方法，包括以下步骤：步骤1.根据里程计运动学模型求解出机器人相对起点的位姿信息(x，y，θ)，其中，k为当前时刻，为里程计运动的估计状态，为上一时刻里程计运动的精确状态，vk为当前时刻的输入控制量，f为求解运动学模型的函数，x为相对起点在x轴方向上的距离，y为在y轴方向上的距离，θ为相对旋转角度；惯性测试单元(imu)输出角度和角速度，解算出机器人在水平面上的位姿状态(x1，y1，θ1)；步骤2.设定k时刻机器人的位姿状态xk服从高斯分布过程噪声和测量噪声服从零均值的高期分布，其中是机器人的运动位姿的初始状态，y0：k是观测量，v1：k是1-k时刻的输入控制量，是均值，是协方差，p表示概率，n表示高斯分布；根据ekf(扩展卡尔曼滤波)算法的预测方程得到当前时刻机器人的位姿状态预测值预测噪声协方差卡尔曼增益kk；利用观测量和ekf算法的更新方程得到当前时刻机器人的位姿状态精确值噪声协方差的精确值步骤3.令步骤2中利用里程计运动学模型和惯性测试单元求解得到的数据计算当前时刻机器人的位姿状态精确值的运动更新模型为p(xk|xk-1，yk，vk-1)，即由k-1时刻的机器人的位姿状态xk-1、k时刻的观测量yk和k-1时刻的输入控制量vk-1求得k时刻的机器人的位姿状态xk；令步骤2得到的当前时刻机器人的位姿状态精确值为机器人的初步位姿；步骤4.利用运动更新模型p(xk|xk-1，yk，vk-1)和激光感知模型并结合蒙特卡罗定位算法，用多个带权重的粒子估计机器人的全局位姿；步骤4-1.当前时刻状态xk的先验信度：其中bel(xk-1)为上一时刻的状态信度；当前时刻状态xk的后验概率分布：bel(xk)＝p(zk|xk)bel-(xk)/p(zk|z1：k-1)其中，zk为当前激光的测量信息，p(zk|xk)为激光感知模型，p(zk|z1：k-1)为归一化因子的倒数；步骤4-2.利用kld(kullback-leiblerdivergence)采样思想对蒙特卡罗定位算法中的粒子数变化引入自适应机制，样本数与状态空间的大小成正比；步骤4-3.粒子集的均值为机器人的位姿状态的最小方差估计，其中，1≤i≤n，n为粒子个数，为xk的第i个元素，为粒子对应的权重；估计值记为机器人第二次修正后的位姿状态，为全局的信息，真实反应机器人在地图坐标系中的实际位置和方向；步骤5.选择扫描匹配算法中的迭代最近点(icp)算法作为机器人位姿的最终修正算法；以第二次修正后的位姿作为icp算法的迭代初始解，将二维激光实时观测到的点云匹配栅格地图点云，得到机器人估计位姿与真实位姿的旋转和平移关系，从而对机器人位姿进行第三次修正。本发明的有益效果是：(1)实现高精度定位。采用三种算法结合来确定机器人最终位姿，算法之间实现优缺点互补，也充分利用各传感器的优点。ekf算法通过融合里程计和imu数据，改善里程计输出角度信息偏差较大的缺点，从而得到比较准确的航迹信息。蒙特卡罗定位算法用ekf来生成重要性密度函数并从中采样，提高了位姿估计精度。以蒙特卡罗算法估计的位姿信息作为扫描匹配的迭代初始解，并以栅格地图作为参考点云，减少了误差积累。所以本发明中采用的方法提高了机器人的定位精度。(2)提高计算效率和定位精度的兼容性。当多种算法结合来处理多种传感器信息时需要考虑计算成本。本发明采用的ekf算法融合两种相对定位传感器，具体高效的特点。蒙特卡罗定位算法中引入了粒子数自适应机制，提高了位姿估计的效率。扫描匹配算法中设置了最大迭代次数，并根据二维激光线数选择尽量少的关联点对数，同时又以较精确的位姿作为其迭代初始解，从而减少了到达收敛条件时的迭代次数。三种算法的综合使用提高计算效率和定位精度的兼容性。附图说明图1为本发明所述方法的流程框图；图2为实施例所述方法使用的变电站环境的栅格地图和相应的测试点；图3为实施例测试内容示意图；图4为实施例中机器人实际到达位置与目标点的距离dr的波形图；图5为实施例中机器人实际到达位置与目标点的角度偏差θ的波形图；图6为实施例中机器人通过定位算法估计位置与目标点的距离de的波形图；图7为实施例中机器人真实定位误差d的波形图。具体实施方式下面结合附图和实施例对本发明进行进一步的说明。本实施例提供一种电站智能巡检机器人高精度定位方法，其流程图如图1所示，本实施例所用机器人型号为huskya200，机器人内部装有ros系统的工控机、运动控制模块、信息采集模块、电源管理模块、里程计、imu，外部搭载了西克的型号为lms151的二维激光雷达。本实施例的测试环境是200kv的变电站，一共设置了三个测式点，分别进行了50组实验。测试定位数据内容包括机器人实际到达位置与目标点的距离dr、角度偏差θ；机器人通过定位算法估计位置与目标点的距离de、角度偏差β。本实施例所述方法包括以下步骤：步骤1.根据里程计运动学模型求解出机器人相对起点的位姿信息(x，y，θ)，其中，k为当前时刻，为里程计运动的估计状态，为上一时刻里程计运动的精确状态，vk为当前时刻的输入控制量，f为求解运动学模型的函数，x为相对起点在x轴方向上的距离，y为在y轴方向上的距离，θ为相对旋转角度；惯性测试单元(imu)输出角度和角速度，解算出机器人在水平面上的位姿状态(x1，y1，θ1)；步骤2.设定k时刻机器人的位姿状态xk服从高斯分布过程噪声和测量噪声服从零均值的高期分布，其中是机器人的运动位姿的初始状态，y0：k是观测量，v1：k是1-k时刻的输入控制量，是均值，是协方差，p表示概率，n表示高斯分布；根据ekf(扩展卡尔曼滤波)算法的预测方程得到当前时刻机器人的位姿状态预测值预测噪声协方差卡尔曼增益kk；利用观测量和ekf算法的更新方程得到当前时刻机器人的位姿状态精确值噪声协方差的精确值步骤3.令步骤2中利用里程计运动学模型和惯性测试单元求解得到的数据计算当前时刻机器人的位姿状态精确值的运动更新模型为p(xk|xk-1，yk，vk-1)，即由k-1时刻的机器人的位姿状态xk-1、k时刻的观测量yk和k-1时刻的输入控制量vk-1求得k时刻的机器人的位姿状态xk；令步骤2得到的当前时刻机器人的位姿状态精确值为机器人的初步位姿；步骤4.利用运动更新模型p(xk|xk-1，yk，vk-1)和激光感知模型并结合蒙特卡罗定位算法，用多个带权重的粒子估计机器人的全局位姿；步骤4-1.当前时刻状态xk的先验信度：其中bel(xk-1)为上一时刻的状态信度；当前时刻状态xk的后验概率分布：bel(xk)＝p(zk|xk)bel-(xk)/p(zk|z1：k-1)其中，zk为当前激光的测量信息，p(zk|xk)为激光感知模型，p(zk|z1：k-1)为归一化因子的倒数；步骤4-2.利用kld(kullback-leiblerdivergence)采样思想对蒙特卡罗定位算法中的粒子数变化引入自适应机制，样本数与状态空间的大小成正比；步骤4-3.粒子集的均值为机器人的位姿状态的最小方差估计，其中，1≤i≤n，n为粒子个数，为xk的第i个元素，为粒子对应的权重；估计值记为机器人第二次修正后的位姿状态，为全局的信息，真实反应机器人在地图坐标系中的实际位置和方向；步骤5.选择扫描匹配算法中的迭代最近点(icp)算法作为机器人位姿的最终修正算法；以第二次修正后的位姿作为icp算法的迭代初始解，将二维激光实时观测到的点云匹配栅格地图点云，得到机器人估计位姿与真实位姿的旋转和平移关系，从而对机器人位姿进行第三次修正。利用同步定位与构图(slam)算法建立机器人栅格地图栅格地图如图2所示。在栅格地图栅格地图选择三个目标点，并储存到机器人内部存储器。选择的这三个点相距较远，需要转弯才能到达，并且每个目标点的周围环境特征有很大的差距。从而测试机器人定位误差随距离增加而积累的情况，也能测试机器人在不同特征环境下对定位精度是否有影响。静态环境测试：，在变电站环境内没有动态障碍物。机器人开始巡检任务，依次到达三个目标点。测试结果都用绝对值的均值表示如表1所示。表1从表1中可以看出实际角度误差和估计角度误差差距较小，说明估计的角度能真实反应机器人实际角度。动态环境测试：在开始巡检任务后让几个人在机器人面前来回走动以此充当动态障碍物。测试结果都用绝对值的均值表示如表2所示。表2从表2可以看出机器人在动态环境中的定位误差比在静态环境中大，动态障碍物影响了二维激光的测量，使得实时二维激光数据和已经建立好的栅格地图不匹配，从而影响了定位精度。用matlab绘制在静态环境中目标点3的dr、θ、de的数据波形，分别如图4、5、6所示，并分析50次测试数据，计算均方根误差。机器人在角度上偏差较小，所以角度误差忽略不计。计算d＝|dr-de|，d值表示机器人真实定位误差。d的数据波形如图7所示。本实施例方法和现有技术方法得到机器人实际定位误差进行对比，对比结果如表3所示。表3方法ekfmcl扫描匹配本发明方法定位误差15cm6cm10cm2.5cm本发明所述方法实现了变电站智能巡检机器人高精度定位。dr的平均值为2.26cm，均方根误差为2.385cm，从图4和dr的平均值和均方根误差结果可知，机器人在测试环境选择相对理想且稳定的情况下定位误差小，每次数据偏差不大。θ的平均值为1.12°，均方根误差为1.35°，从图5和θ的平均值和均方根误差计算结果可知，机器人在方向上的误差很小，所以就不需要对估计角度差β做单独计算。de的平均值为1.37cm，均方根误差为1.40cm，机器人的运动是根据de的值来控制。d的平均值为1.01cm，均方根误差为1.40cm，d的计算结果为机器人真实的定位精度。通过实际距离误差与估计距离误差做差，来评估机器人的真实定位能力，这个误差排除了机器人硬件和控制精度等影响，是由定位算法、地图、传感器引入的。当前第1页12