基于智能空间的变电站机器人复合全局定位系统设计∗

梁林勋，杨俊杰，楼志斌

（1．上海电力学院 电子与信息工程学院，上海200090；2．上海科学院，上海201203）

0 引 言

随着智能电网［1］及机器人技术的发展，变电站作为整个电网体系的重要一环，为确保变电站实现无人值守、安全稳定的运行，变电站巡检机器人成为了各国研究的热点。其中，精确的自主定位与导航是巡检机器人实现一切服务的前提，通常，机器人定位问题可分为两种：全局定位［2］与位置跟踪。全局定位是指机器人在缺乏有效的初始位姿先验知识前提下，通过不确定的传感器观测来确定位姿的过程，是后续位置跟踪的基础，主要用于处理机器人启动时位置初始化及移动过程中机器人绑架等问题。

卡尔曼滤波［3］（Kalman Filter， KF）和基于蒙特卡洛的粒子滤波［4］（Particle Filter， PF）是目前广泛使用的全局定位方法。文献［5］采用基于卡尔曼滤波的组合定位导航算法，通过DR推算与LMS高精度激光雷达组合滤波，提高了强电磁环境下变电站机器人定位系统的稳定性与准确性；文献［6］提出了一种与遗传算法结合的自适应进化粒子滤波算法，通过引入自适应控制参数控制重采样次数，有效提升了定位精度；文献［7］提出了一种联合蒙特卡罗与改进粒子滤波的IUPF-MCL算法，该方法通过迭代Sigma点KF滤波来设计粒子滤波提议分布以改善滤波退化问题，最后利用MCL算法实现机器人精确定位。相较于卡尔曼滤波，粒子滤波更适合处理复杂的非线性非高斯问题，因而在机器人全局定位与地图构建中广泛使用，但在变电站这样的室外大范围环境下，全局粒子滤波定位通常又存在以下问题：（1）由于环境较大，全局定位通常需要较多的粒子数，而粒子数与存储空间和计算复杂度密切相关；（2）若地图过大且粒子数有限情况下，粒子稀疏会造成定位精度的下降甚至失败；（3）随着算法迭代，不可避免的会产生粒子退化问题［8］，而重采样方法的引入又会导致粒子多样性的丧失，从而加剧算法发散的可能。

针对以上问题，本文将智能空间［9］与机器人技术相结合，提出一种智能空间下基于ZigBee指纹定位与PSO-PF的复合全局定位方法。智能空间是一个嵌入了计算、信息设备和多模态传感器的空间系统，通过设备间的交互与分布式计算，统一管理空间中的各种资源，为用户提供方便的服务，与机器人结合的思想是将感知、执行器件分布地安装在空间相应位置，从而能够实现对空间的全面感知并为机器人提供更完备的信息，降低机器人对于本体传感器的要求。因此，该方法首先通过UPnP（Universal Plug and Play）技术将机器人接入智能空间，利用智能空间中的BP神经网络进行ZigBee指纹初定位，然后机器人结合ZigBee定位结果，在误差范围进行粒子撒点，将全局定位问题转化为局部搜索问题，利用粒子群优化算法将最新观测信息引入粒子滤波采样分布中，改善样本后验概率分布，从而完成定位过程。实验结果表明，该方法可有效提高变电站机器人全局定位精度，改善算法性能并缩短迭代时间，具有一定可行性。

1 机器人全局定位系统框架

如图1所示，机器人复合全局定位系统主要由Zig-Bee空间感知单元、智能空间调度系统和ROS变电站机器人组成。其中，空间感知单元主要负责各节点RSSI强度的采集并通过Wifi网络转发给智能空间调度系统；智能空间调度系统负责全局系统管理，协调网络通信以及利用神经网络模块进行初步位置估计；机器人平台接收到粗略定位结果后初始化粒子撒点范围，结合观测信息进行粒子滤波完成定位过程。

图1 机器人定位系统框架Fig．1 Framework of robot localization system

1．1 空间感知模块

ZigBee空间感知单元主要在智能空间中进行各参考节点 RSSI强度采集、预处理、发送等任务，由CC2530模块和控制器组成。其中，CC2530模块将扫描到的节点及其信号强度通过RS232发送给控制器；控制器负责感知模块初始化以及信息预处理，并通过接入智能空间的Wifi网络将信息上传到神经网络处理模块，完成ZigBee指纹初定位过程。

1．2 智能空间系统

智能空间调度系统主要由ZigBee指纹数据库、神经网络模块以及调度系统组成，以BP神经网络为核心，采用建立的ZigBee指纹库进行训练，通过对接收到的指纹信息并行处理得到初步的位置估计，同时负责设备与信息流管理，响应ROS机器人接入请求并将指纹定位结果通过initialpose消息发送回机器人本体，初始化位姿估计并完成全局定位。

1．3 机器人平台

本文采用自主开发的ROS机器人平台，ROS［10］（Robot Operating System）是一种开源的机器人操作系统，它提供部分标准系统级服务，以其较好的硬件抽象及常用库集成的特点，近年来受到了广泛的关注。

如图2所示，在ROS机器人中，全局定位系统主要由以下几部分组成，其中，UPnP为机器人设备抽象节点，它通过UPnP协议和XML文件将机器人描述成具有统一接口的抽象设备，并将设备和服务描述文档广播出去，当机器人接入智能空间网络时，智能空间调度模块即可以发现并调用相应服务，从而实现与智能空间的零配置与松耦合；Odom和Laserscan节点分别负责里程计和激光数据的采集和发布，通过ROS标准通信框架，实现模块间数据共享；map_server提供地图服务，负责加载已生成的环境地图并发布map数据；Location节点通过initialpose主题初始化粒子撒点范围，融合里程计、激光观测以及地图数据通过粒子群优化的粒子滤波进行位姿估计，然后发布estimatePose主题，通过调用move_base完成系统定位与导航。

图2 机器人软件体系描述Fig．2 Software architecture description of robot

2 复合全局定位算法设计

基于智能空间ZigBee指纹与PSO-PF的复合全局定位算法主要分为两步：首先，采集智能空间中ZigBee指纹，利用建立的BP神经网络进行粗略估计；然后在误差范围内初始化粒子撒点范围，利用粒子群优化的粒子滤波融合传感器观测以及地图数据进一步得到机器人的精确位姿。

2．1 智能空间ZigBee指纹定位

ZigBee是基于IEEE802．15．4标准的低功耗局域网协议，由于ZigBee特有的优点，在电力等工业领域得到了广泛应用，其定位方法多种多样，以TOF、RSSI和位置指纹法最为常见。RSSI定位［11］是依据节点接收到的信号强度（RSSI），计算信号传播过程中的能量损耗，通过信号衰减理论模型转化为传播距离；指纹定位［12］是基于与位置相关的RSSI指纹，通过与建立的指纹库匹配得到盲节点的位置，一般分为两个阶段：离线阶段和在线阶段。离线阶段根据环境大小，选取一组位置作为采样点，通过采集参考节点的RSSI指纹建立空间指纹库；在线阶段通过采集的各参考节点信号强度与指纹库匹配，从而估计出盲节点位置。BP神经网络［13］是一种误差逆向传播的多层前馈神经网络，一般由输入层、隐含层（至少一层）和输出层组成，理论上，一个3层的BP网络可以逼近任意非线性系统。由于位置指纹法简单易行，同时BP网络具有较好的容错性和泛化自学习能力，因此指纹定位的离线学习及在线匹配过程可以采用BP网络来实现。

假设BP网络输入数据为各参考节点RSSI强度，输出为对应采样点坐标，则输入和期望输出向量为：

其中，xi和oj分别为此时第i个参考节点RSSI强度和坐标向量第j个分量，m、n分别为参考节点总数和坐标分量个数。

BP网络具体训练过程如下：

Step1：初始阶段，初始化BP网络各神经元间权重及阈值，并将输入输出向量归一化。

Step2：正向传播过程，假设训练过程中第k组样本输入为X（k），节点i和节点j之间权重为wij，节点j阈值为θi，则隐含层与输出层节点j状态为：

其中，f（·）为激活函数，以Sigmoid函数为例：

Step3：误差反向修正，误差e以实际输出yi与期望输出oi之间的均方差表示：

若学习误差e小于给定的阈值ε，训练结束；否则根据梯度下降原则反向调节各层节点权重及阈值进行误差修正：

其中，k为学习次数，η为学习因子。然后重新输入样本训练，直至BP网络误差小于给定的阈值，算法结束。

2．2 PSO优化的粒子滤波定位

上述基于BP网络定位的结果具有较大误差，一般难以满足机器人定位要求，因此需要在此基础上融合激光观测、里程计及地图信息进一步确定机器人的精确位置。

粒 子 群 算 法［14］（Particle Swarm Optimization，PSO）是一种基于群智能的仿生优化算法，其基本思想是通过群体中大量简单个体的协作与信息共享来寻找最优解。假设粒子为D维空间中一批随机分布的点，每个PSO粒子都具有两个初始参数：速度与位置。算法实质是通过空间中大量粒子的飞行，不断搜索群体中个体极值和全局极值，然后追随全局极值以速度与位置公式更新粒子下一时刻运动状态，并逐渐使整个群体逼近于最优目标的。与粒子滤波相比，二者具有相似的机理，因此可以将两种算法融合，通过粒子群算法良好的全局寻优能力，使粒子滤波粒子集在权重更新前更逼近于真实分布，从而使得粒子权重普遍增大，降低了粒子间“贫富”差距，使滤波过程中有效的粒子数目增多，同时在重采样过程中减少了由于权重低而舍弃的粒子数目，从而改善了样本贫化现象，进一步提高了全局定位算法的稳定性与鲁棒性。

基于PSO优化的粒子滤波定位过程如下：

（1）初始状态。

根据上述指纹定位结果，在误差范围内随机撒点以模拟真实的后验概率分布，将常规全局滤波问题转化为局部滤波问题，降低粒子搜索代价。

（2）转移阶段。

粒子滤波预测过程中，每一个粒子都表示一个机器人可能的状态，机器人下一时刻位姿预测可以通过运动模型获得：

其中，xit-1为t-1时刻第i个粒子状态，ut-1为t-1时刻控制输入量。

（3）粒子速度-位置更新。

对预测后的粒子状态使用速度与位置公式进行更新：

其中，c1、c2为学习因子，Rand为对角矩阵，其对角线元素为符合高斯分布的随机数，pp和pg分别表示机器人位置预测的个体极值与全局最优解。

（4）适应度评价。

引入适应度函数F对（3）中的粒子进行评价：

其中，RK为测量噪声协方差矩阵，ZtN和ZtP分别为路标实际观测和状态预测值。

设定参考阈值σ，若F远大于σ，则表示粒子集分布在真实状态附近，可以停止优化；若F＜σ，则表明粒子适应度普遍较低，整体状态不符合真实概率分布，应不断使用式（9）、式（10）更新粒子位置与速度，驱动粒子集向高似然区域运动。

（5）决策阶段。

结合最新路标观测及地图数据，计算粒子权重并归一化：

（6）重采样。

为了解决粒子退化问题，粒子集权重归一化后，以权重最大的粒子作为机器人最终位置估计；并依据权重大小选取较大的粒子复制添加到新的粒子集中，然后返回（2）进行下一阶段的粒子滤波。

3 实验及结果分析

本文以实验室自主开发的ROS变电站机器人为平台，在布有ZigBee节点的室内模拟变电站环境进行实验。如图3所示，搭载的传感器主要有2 000线增量式光电编码器，ZigBee CC2530模块以及RoboPeak团队开发的低成本2D RPLidar A2M8激光雷达；软件平台采用基于Ubuntu 16．04的ROS Kinetic系统；室内环境大小约为 40×16m，地图分辨率 0．05。

图3 ROS机器人移动平台Fig．3 Robot mobile platform based on ROS

为了验证本文算法的有效性，本文以常规粒子滤波全局定位为参照进行比较。图4为两种算法对比情况，图4（a）、图4（b）分别为常规 PF全局定位与本文算法的初始粒子集分布状态，图中可以看出，由于本算法ZigBee初定位结果的引入，将全局定位问题转化为了局部定位问题，粒子撒点范围大大缩小；图4（c）、图4（d）分别为两种算法迭代50次后最终位置估计情况，相较而言，本算法位置估计更接近于真实位置，具体误差随迭代次数变化曲线如图5所示。

图5（a）、图5（b）分别为常规 PF定位与本算法定位误差随迭代次数的变化曲线，可以看出最终本算法定位误差约为0．5 m，优于常规PF算法的1．6 m。

图4 定位结果对比Fig．4 Comparison of localization results

图5 定位误差对比Fig．5 Comparison of localization error

为了进一步考察本算法在算法性能方面的改善情况，本文分别在初始粒子数目不同情况下，对两种算法进行了检验。图6（a）为常规PF算法定位误差随粒子数目变化曲线，图6（b）为本文算法定位误差与粒子数目关系曲线，图中可以看出，粒子数目的多少是影响两种算法定位精度的关键因素，在本文实验环境下，本算法在粒子数目为800时精度已达到较高状态，全局定位精度约为0．5 m；而常规PF算法在粒子数目为2 000时约为1．5 m，进一步说明了本算法可以有效减少迭代所需的粒子数，只需较少的粒子就可以达到较高的精度。

同时当粒子数均为2 000时，对两种算法各迭代50次，所需时间如图7所示，结合图4、图5，可以看出本算法由于ZigBee定位结果的引入，将全局粒子搜索问题转化为局部问题，可以有效缩短迭代时间，加快算法运行速度。因此，通过以上与常规粒子滤波方法的对比说明，本文设计的算法可以有效的提高初始全局定位精度并改善算法性能，缩短迭代时间，具有一定可行性。

图6 定位误差-粒子数目关系Fig．6 Relation between localization error and particle number

图7 迭代时间对比Fig．7 Comparison of iteration time

4 结束语

精确的定位导航是实现机器人自主移动的先决条件，本文针对巡检机器人在缺乏有效的初始位姿先验知识情况下全局定位的问题，结合变电站特点，提出了一种智能空间与ROS机器人相结合的复合全局定位系统。该方法首先在智能空间中利用 BP网络进行ZigBee指纹初定位，在此基础上结合激光观测、里程计以及历史地图数据采用PSO优化的粒子滤波精确定位，从而完成系统定位与导航。

实验结果表明，该方法结合智能空间特点，增强了机器人定位系统的准确性与稳定性，同时改善算法性能，提高了变电站机器人自主运行能力。目前采用单一机器人进行实验，如何利用智能空间优势，并实现变电站多机器人协同定位，这将是需要深入研究的工作。