面向巡检机器人多源融合鲁棒定位与高逼真仿真方法

文 刚，夏宋鹏程，周仿荣，张 辉，潘 浩，曹 俊，高振宇，刘亚东

（1.云南电网有限责任公司电力科学研究院，云南昆明 650217；2.上海交通大学，上海 200240；3.云南电网有限责任公司，云南昆明 650051）

0 引言

近年来，随着智能电网的快速发展，利用移动巡检机器人代替人工巡检已经成为电力行业研究的热点[1-2]。在智能电网全面推进的过程中，巡检机器人覆盖了从发电到用电各个环节的巡视、操作工作，起着举足轻重的作用[3]。

早期的巡检机器人仅停留在实验阶段。2009年加拿大研发了使用手柄的远程操控轮式巡检机器人，可同时装备RGB 相机和红外相机对电厂进行巡视[4]。2012 年新西兰梅西大学研发了一款机器人控制系统并搭载了视觉采集装置用来监测电网环境的变化[5]。近年来国内的电力巡检机器人也有着长足的发展[6]：2010 年鲁能、中科院等单位相继开展巡检机器人的研究，并在青岛等地的变电站成功投入使用；2015 年深圳朗驰欣创科技公司推出了具有自主导航的巡检机器人。但是现有的巡检系统大多采用引导定位方法，铺设过程存在工作量过大、路径固定灵活性低、不能精确锚定巡检机器人的具体位置等缺点[7]。因此，研究准确且鲁棒的导航定位算法能够为自主巡检机器人的稳定运行奠定基础[8]。

导航定位是巡检机器人实现智能化巡检的关键技术。巡检机器人普遍采用预设磁轨和射频识别（Radio Frequency Identification，RFID）技术进行定位[9]，但这种方法定位精度较低、抗干扰能力较弱。近年来尽管传感器性能不断得到提升，但单一传感器在恶劣环境中的定位能力仍不足：如卫星导航定位在电磁干扰较大的环境中存在失效可能[10]；惯性导航系统受环境影响较小，但是高精度的惯性导航测量元件（Inertial Measurement Unit，IMU）价格高昂，低精度的IMU 容易累积定位误差[11]；视觉传感器进行导航定位存在纹理特征不明显和高动态工况下失效问题[12]；激光雷达传感器在空旷环境进行定位存在退化问题[13]。因此，为了保证巡检机器人导航定位的鲁棒性和连续性，进行多传感器融合定位是巡检机器人导航定位领域的重要研究方向之一[14-15]。

本文针对巡检机器人在电网环境中存在定位鲁棒性不足和测试困难的问题，基于AAA 级游戏引擎搭建了智能电网平台，构建虚拟传感器[16-18]用于智能电网巡检机器人导航定位算法的验证，避免在带电环境中进行过多的现场测试，保证实验的安全性和便利性。提出一种基于联邦卡尔曼滤波（Federated Kalman Filtering，FKF）的多传感器融合算法，对故障检测与信息恢复算法进行集成，并在仿真智能电网场景中进行验证和测试。仿真结果显示，巡检机器人具有分米级定位能力，能够较好地支持巡检机器人在电网的复杂环境中进行大范围自由巡检。

1 巡检机器人导航定位方法

1.1 卫星导航定位

全球卫星导航系统（Global Navigation Satellite System，GNSS）包括全球覆盖的卫星定位系统、区域覆盖的卫星定位系统以及天基增强系统[19]。在GNSS 中，卫星向地面发送包含其位置信息的信号，地面接收机在接收卫星信号后，可解码得到卫星星历并测量得到伪距、载波相位等测量值，进而解算出接收机的三维位置。GNSS 虽然能够在全球范围内提供较高精度的服务，但其受环境影响较大，在信号受到干扰或遮挡的情况下（如电网环境中的强电磁干扰），GNSS 接收机可能无法得到较高精度的结果甚至无法解算出定位结果。

1.2 惯性导航系统定位

惯性导航系统（（Inertial Navigation System，INS））利用加速度计和陀螺仪来测量加速度及角速度，并经过机械编排得到导航状态量[20]。INS 通过姿态更新、速度更新和位置更新得到惯性导航的推算结果。影响惯性导航定位性能的主要因素是航迹推算的累计误差[11]。各种级别的惯性导航器件都存在着确定性和随机性等误差，并且随着惯性导航工作时间的增加不断累积，最终导致导航定位结果的发散。惯性导航的优点为导航信息连续、更新频率高、受外界环境的影响小。IMU 的误差主要来自3 个方面，包括随机噪声、尺度误差和轴向误差[11]。IMU 的测量模型为：

式中：aout，ωout分别为IMU 中加速计和陀螺仪的测量输出值；Ta,Tg分别为IMU 中加速计和陀螺仪的轴向偏差转换矩阵；Ka,Kg分别为IMU 中加速计和陀螺仪的尺度误差矩阵；atrue,ωtrue分别为IMU 中加速计和陀螺仪的真实值；ba,bg分别为IMU 中加速计和陀螺仪的零偏；na,ng分别为IMU 中加速计和陀螺仪的随机噪声。

1.3 视觉和激光定位

实时定位与地图构建（Simultaneous Localization and Mapping，SLAM）已经成为机器人领域的热点问题[21]。视觉SLAM 是指搭载视觉传感器的载体不依靠场景的先验信息，独立地估计载体自身的运动状态并建立地图模型。视觉SLAM 的经典框架一般由5 个模块组成，分别为传感器信息获取、视觉里程计、后端优化、回环检测以及建图。除了基于视觉传感器的定位研究，使用激光雷达进行定位的SLAM 系统也愈发的成熟[22]。在激光SLAM 算法中最常用的方法是迭代最近点算法，即使用迭代最近点的方法估计巡检机器人载体位置与姿态。

通过激光雷达扫描得到的1 组3D 点（P，P′），其 中P={p1,...,pJ}，P′={p′1,...,p′J}，p1,…,pJ和p′1,…,p′J分别为扫描得到的J个3D 点坐标。构建误差方程如式（2）所示，解算得到载体的旋转和平移数值：

式中：R为载体的旋转矩阵；T为载体的位移矢量。

2 算法分析与仿真平台搭建

2.1 多源融合定位算法

多源融合定位算法需要有效的融合各个传感器的有效信息，获得更加鲁棒的定位性能。GNSS的优点是定位精度高且能够覆盖全球，但受环境影响较大，有时会出现信号中断的现象[23-25]；INS 的优点是不受外界环境影响和无源，但长时间运行会产生较大累计误差；视觉SLAM 系统的优点是在室内以及纹理特征明显的区域运行良好且能够建图，但在速度过快及特征稀疏的场景定位较差；激光SLAM 系统的优点是在大场景区域运行良好，但在空旷场景的定位性能不稳定。由于各个传感器的应用场景不同、优点与不足各异，所以巡检机器人在复杂的电网环境中进行鲁棒定位，需要对传感器进行有效的融合。利用传感器之间的互补特性，结合环境、应用和精度的不同，选择不同的传感器组合，以提高导航定位系统在定位方面的准确性与鲁棒性。

基于高逼真仿真平台和FKF 结构进行多传感器融合，验证多源融合定位算法在各种传感器组合下的定位效果。为了能够在电网环境中获得鲁棒的定位结果，本文设计了多传感器融合滤波结构如图1 所示。

图1 多传感器融合滤波结构Fig.1 Filter structure of multi-sensor fusion

由图1 可知，多传感器融合滤波结构由2 部分组成。一部分是3 个并行的FKF 滤波器，分别估计得到INS 与GNSS、INS 与视觉SLAM 以及INS 与激光SLAM 在FKF 系统中的局部最优状态估计值；另一部分是将局部最优状态估计通过主滤波器融合3个子滤波器得到全局最优估计。

本文将INS 作为融合的主系统，选取姿态误差的等效旋转矢量、速度误差、位置误差以及IMU 零偏误差构成k时刻估计状态量X(k)，建立多传感器融合系统状态方程为：

式中：(k)为k时刻估计状态量求导；W(k)为k时刻系统噪声；F为状态递推函数。

融合导航定位系统的测量由3 部分组成，分别为：

式中：δzG,k为INS 与GNSS 输出的位置与速度的差值；δzC,k为INS 与视觉SLAM 输出的位置与姿态差值；δzL,k为INS 与激光SLAM 输出的位置与姿态的差值；HG,k，HC,k，HL,k分别为INS 与GNSS、INS 与视觉SLAM、INS 与激光SLAM 这3 个Kalman 子系统的观测矩阵；vG,k，vC,k，vL,k分别为上述3 个Kalman 子系统的观测残差。

基于经典FKF 的预测更新和测量更新[11]，在3个子滤波器中分别获得子滤波器的局部最优估计以及其协方差矩阵。根据线性最小方差准则[16]，得到全局最优估计值X*(k)。应用FKF 的方差上界法消除2 个局部状态量之间的相关性，即子系统之间的协方差为0。在FKF 的实际更新过程中，将各子滤波器的协方差扩大以保持滤波器的稳定性，状态量的最优估计为：

式中：XG(k)，XC(k) 和XL(k) 分别为INS 与GNSS、INS 与 视 觉SLAM、INS 与 激 光SLAM 这3 个Kalman 子系统的状态量局部最优估计；PGG(k)，PCC(k)和PLL(k)分别为上述3 个Kalman 子系统的状态协方差阵。

2.2 鲁棒滤波融合算法

面对电网的复杂环境，多源融合定位算法需更加鲁棒来抵抗干扰，本文设计的多传感器融合滤波结构使用鲁棒核函数和一致性检验的FKF 技术来应对电网中的强电磁干扰。

2.2.1 鲁棒核函数

鲁棒核函数包含Huber 核函数、Tukey 核函数等，是被广泛应用的抗差方法，其通过降低外点的权重来降低异常数据的影响。在多传感器融合滤波结构中，鲁棒核函数主要应用在测量方程上，抗差Kalman 滤波的观测优化目标为：

式中：f为鲁棒核函数；Z(k)i为第i个子滤波器k时刻的测量值；I为子滤波器总数；Hi为第i个子滤波器的观测矩阵。

在FKF 的更新流程中，使用鲁棒核函数的FKF调整新息的权重，得到状态量估计为：

式中：X(k|k-1)为待估计状态量在预测更新后的一步预测；K(k)为k时刻FKF 增益；De(k)为使用鲁棒核函数的新息。

在实际实验中，由于Huber 核函数混合使用了绝对损失和平方损失，能够很好地平衡有效性与鲁棒性，因此我们选取Huber 核函数来抑制估计误差。

2.2.2 一致性检验的FKF技术

多传感器融合滤波结构由3 个子滤波器和1个主滤波器构成。子滤波器的融合结果受到辅助参考定位系统精度的影响，并进一步传递到主滤波器中。由于电网环境中具有复杂的电磁场，为了得到更加鲁棒的定位结果，本文研究将FKF 与一致性检验相结合，来解决辅助定位系统定位误差导致的故障。每个传感器的观测时刻均能够得到一个FKF 新息，不同的新息互相独立，构建评价函数对融合的数据源的质量进行评估，评价值方程为：

式中：ηik为第i个子滤波器第k时刻评价函数器的评价值；Hik为观测矩阵；为观测矩阵的转置；Pik为系统协方差；Qik为观测协方差；rik为测量新息；为测量新息的转置。

鲁棒滤波融合算法能够检测滤波是否存在故障，并及时进行信息恢复。其在检测出子滤波器存在故障后，将每个子滤波器结果与预测结果进行对比，再使用置信度较高的参考系统进行融合，以保证总体系统的稳定性。

2.3 智能电网仿真平台搭建

本文基于AAA 级游戏引擎UE4，结合Airsim系统搭建智能电网巡检机器人导航定位仿真平台，并从光影渲染和传感器仿真2 个方面开展相应工作：（1）基于渲染引擎和虚幻编辑器在计算机中高逼真还原电网场景，包括光影、天气等；（2）传感器仿真与现实传感器具有一致性，保证定位算法评估的高度有效性。

具体设计中，基于Unreal 强大的渲染引擎和物料库，在仿真系统中最大限度还原电网场景的外观，达到高逼真的视觉重建效果，得到智能电网虚拟仿真场景，如图2 所示。

图2 智能电网虚拟仿真场景Fig.2 Smart grid virtual simulation scenario

3 仿真分析

本文仿真平台实验电脑的配置如下：处理器为Intel I7-8750，显卡为NVIDIA RTX-2080，内存为32G DDR4-RAM，UE4 引擎版本为4.18。视觉定位算法采用双目定位模式，相机基线长为1.205 m。

为验证智能巡检机器人导航定位仿真系统的有效性和多源融合定位算法的鲁棒性，本文设计了3 组实验进行仿真：（1）相机仿真评估与双目基线对比实验：针对相机仿真设计了视觉SLAM 的相关实验，比较不同的双目基线对视觉定位的影响。（2）点云仿真与定位性能实验：验证点云仿真噪声证定位结果带来的影响，证明高逼真仿真平台点云仿真的有效性。（3）鲁棒FKF 实验：仿真惯性测量单元、GNSS、相机图像和激光点云数据，设置传感器观测异常数据并模拟复杂异常环境，验证本文设计的多源融合算法在智能电网环境中的可靠性。

3.1 相机仿真评估与双目基线对比实验

进行相机仿真评估与双目基线对比实验，首先在实验中评估仿真相机成像与实际图像成像的几何一致性，再通过仿真系统设置双目相机基线长度来评估基线长度对于视觉定位的影响。

3.1.1 仿真相机成像几何一致性评估

仿真平台中的相机采用几何一致性指标进行评估和误差分析。在仿真系统中设置对应于现实世界真实值的虚拟标定板和虚拟相机的位置及姿态，分别采集现实和虚拟世界中标定板的图片，通过匹配检测的角点，得到几何一致性误差。实验表明本文设计的虚拟仿真相机的成像水平在亚豪米级，平均误差为0.95 px，中位数误差为0.84 px。

3.1.2 双目相机基线长度影响评估

本文以视觉传感器为例，通过在高逼真仿真平台中调整2 个相机之间的基线，来验证双目SLAM算法在不同基线长度下的运行效果。实验中设置3种基线长度，分别为短基线0.125 m、中长基线1.205 m 和长基线3.005 m，设置每个相机的视场角为90°。3 种基线长度图片采集结果如图3 所示。

图3 3种基线长度图片采集结果Fig.3 Collection results of three types of baseline image

将采集图像数据导入视觉SLAM 系统，得到不同基线长度定位误差对比，如表1 所示。

表1 不同基线长度定位误差对比Table 1 Errors comparison of different baseline m

由表1 可知，在基线过长的情况下双目视觉SLAM 初始化困难，在运行一段时间后由于特征跟踪丢失导致视觉SLAM 不可用。由于2 个相机传感器之间的平移量过大，致使图像上物体的形状角度发生明显变化，特征提取和描述也产生较大差异，使匹配过程变得更加困难，从而导致解算的失效。比较另外2 组基线较短的实验可知，基线在较短的情况下视觉SLAM 定位精度下降较多。

3.2 点云仿真与定位性能实验

对仿真得到的点云depth 数据进行性能分析，验证在不同噪声条件下的激光点云定位结果。在点云理论深度值上叠加不同强度的高斯白噪声，噪声等级定义为在深度图中对每个像素添加的高斯噪声的程度，叠加噪声后的点云为Pixnew：

式中：Pixold为原始仿真点云；N（0，1）为噪声分布；σ为噪声等级。

设置4 种不同噪声点云数据如表2 所示。

表2 不同噪声点云数据Table 2 Point cloud data with different noise levels

通过点云仿真与定位性能实验得到不同噪声点云数据下的定位误差，如表3 所示。

表3 不同噪声点云数据下的定位误差Table 3 Positioning error of cloud data with various noise levels m

由表3 可知，随着点云噪声的增大，定位结果也随之变差，噪声较大时会造成定位算法的失效。在实际测试的过程中，误差的引入会在转弯处造成更大误差。

3.3 鲁棒FKF实验

实验利用本文搭建的仿真系统验证鲁棒FKF架构在电网复杂环境下的定位性能。实验设置了战术级和消费级2 种噪声参数的IMU，构成3 种不同的融合实验方案。方案1：GNSS 与战术级IMU组合；方案2：GNSS、消费级IMU 与双目视觉定位组合；方案3：GNSS、消费级IMU、双目视觉定位和激光点云定位组合。实验得到不同组合导航方案定位结果如表4 所示。

表4 不同组合导航方案定位结果Table 4 Positioning results of different integrated navigation schemes

由表4 可知，使用FKF 架构引入额外的传感器信息，不仅能够提升系统的精度，并且能够增加系统的鲁棒性并提升可用性。

为了适应电网的复杂环境，本文引入粗差噪声来检验一致性估计模块的有效性。第45～55 s 在GNSS 中加入粗差噪声，得到一致性检测模块对多传感器融合滤波误差影响，如图4 所示。

图4 一致性检测模块对多传感器融合滤波误差影响Fig.4 Effect of multi-sensor fusion error on consistency detection module

由图4 可知，增加一致性检测模块可有效抵抗误差的干扰，使导航定位算法更具鲁棒性。

综上所述，在具有较多干扰因素的场景中，本文提出的多源融合定位算法具有分米级定位能力，能够较好地支持巡检机器人在复杂的电网环境中进行大范围自由巡检，完成更多自主的巡检操作和任务。可以最大限度避免巡检机器人由于定位精度较低、无法到达预检位置而导致任务失败的情况。

4 结语

本文针对智能巡检机器人在实际场景测试中存在的系统安全问题及潜在风险，基于UE4 引擎和Airsim 插件搭建了面向巡检机器人的虚拟仿真平台，并提出一种基于一致性检测的FKF 融合方法。实验基于智能电网导航定位仿真平台，验证了视觉图像和点云算法在不同仿真设置下的定位结果，并对所设计的FKF 算法进行了测试和验证。发现有效的融合多种传感器可提升定位精度、抑制传感器误差，增加巡检机器人导航定位系统的可用性与鲁棒性。虚拟测试方法可降低后续研究的难度，为电网的数字孪生技术在导航定位领域做出了初步的探索，具有一定的实际意义和应用前景。