CN113075687A - 一种基于多传感器融合的电缆沟智能巡检机器人定位方法 - Google Patents

Abstract

本发明公开了一种基于多传感器融合的电缆沟智能巡检机器人定位方法，涉及智能机器人技术领域。基于扩展卡尔曼滤波将编码器、陀螺仪、深度相机RGB图中得到的移动机器人位姿信息进行融合；使用RBPF(Rao‑Blackwellised Partical Filter)粒子滤波算法进行定位和建图；针对地下电缆沟通道复杂环境进行多传感器融合使得移动机器人实现精确的定位和建图。解决了移动机器人在电缆沟环境下因沙地行走导致的转弯误差，地面不平以及细小障碍物的轻微阻塞而导致的前进误差；使得移动机器人在电缆沟环境下也能进行准确的定位，具有极高的实用性。

Description

一种基于多传感器融合的电缆沟智能巡检机器人定位方法

技术领域

本发明涉及智能机器人技术领域，尤其涉及一种基于多传感器融合的电缆沟智能巡检机器人定位方法。

背景技术

在如今高速发展的21世纪，输配电线路的数量逐渐增加，但由于电缆沟多存在于地下，且长时间的运作导致的电缆外绝缘老化放电，以及电缆沟内部积水、积潮的管理，加上在电缆沟中一些动植物尸体经过微生物分解产生的甲烷等可燃性气体积累，最终可能引起地下电缆沟发生火灾***。而且地下电缆沟内部环境狭小复杂，工作人员巡检尤为困难。企业和高校也开始使用移动机器人来代替工人进行类似如此危险的工作，作为移动机器人最关键的技术，自主导航技术的研究变得尤为重要。移动机器人自主导航的核心技术在于机器人通过传感器的信息在未知环境下进行自身定位。让机器人在巡检的过程中知道自己处于电缆沟的哪个位置才能进行巡检，并实时对周围的环境进行建图还原让工作人员监控。

于是，各种先进的地下电缆沟检测设备应运而生。2019年，湖南省电力公司带电作业中心和国网浙江新昌电力公司变电站为了满足电力***运维过程中电缆沟巡检的任务要求，与长沙理工大学设计了外部搭载激光雷达、编码器与陀螺仪组成的里程计的基于RBPF-SLAM(Rao－Blackwellised粒子滤波器定位算法)的地下电缆沟巡检小车。并在实际环境中进行了测试。

安康水力发电厂，在2019年为了满足电力***运维过程中电缆沟巡检的任务要求，设计了一种智能型电缆沟巡检机器人***，通过基于单独激光雷达的hector-slam算法进行定位与建图。

华南理工大学在2017年针对在室内机器人定位和三维稠密地图构建***中，现有方法无法同时满足高精度定位、大范围和快速性要求的问题，应用具有跟踪、地图构建和重定位三平行线程的ORB-SLAM算法估计机器人三维位姿的定位；然后拼接深度摄像头KINECT获得的三维稠密点云，提出空间域上的关键帧提取方法剔除冗余的视频帧；接着提出子地图法进一步减少地图构建的时间，最终提高算法的整体速度。

南京大学图书馆在2020年将RFID技术、模糊逻辑与机器人自动化技术相结合，设计了机器人自主导航算法，通过在固定点设计标签让机器人识别进行定位，使图书盘点机器人能够在高度同构、空间狭窄的图书馆环境中进行精准定位。

山东大学控制科学与工程学院在2020年针对单个机器人在大场景下建图与定位效果不佳的问题，提出一种基于路标信息与云架构的多机器人实时协作融合建图方法.首先，利用“云端-终端”的网络架构在云端服务器实时处理各机器人上传的数据；其次，单个机器人构建地图的同时检测环境中路标，完成路标与各局部地图的绑定；然后，云端服务器根据路标检测结果确定多个地图间存在重叠区域，构建位置坐标的超定方程组并求解地图间最优变换矩阵，同时实现对变换参数的持续更新与优化；最后，通过融合算法完成多个地图的处理，形成全局地图。经过多场景的实验验证表明：算法实现了多机器人协作实时建图与融合。

安康水力发电厂所设计的智能型电缆沟巡检机器人只采取了只使用激光雷达传感器基于hector-slam对周围环境的信息获取，但hector-slam采用泰勒展开近似优化激光雷达数据的匹配过程，因此两次激光雷达采集数据时位姿变化不能太大，否则余项误差过大。需要雷达的更新频率较高，测量噪声小且对环境要求严格。在制图过程中，机器人速度控制在比较低且地面平整的情况下，定位才会较准确，对于电缆沟复杂且危险的环境中无法保持准确的定位。

湖南省电力公司带电作业中心和国网浙江新昌电力公司变电站与长沙理工所设计的地下电缆沟巡检小车通过编码器与陀螺仪组成的陀螺仪与激光雷达进行回环反馈。但由于激光雷达仅仅只是对雷达当前的2D平面的障碍物进行反馈，在地下电缆沟中的复杂的环境中，仅仅以单一2D平面作为障碍物信息反馈，无法检测其他不在激光雷达平面的障碍物导致机器人行动受阻不动，反而因为回环反馈影响导致机器人定位产生过大的误差无法调节，不及时解决将导致建图和定位完全错误。

华南理工大学通过Kinect深度相机获取的视觉传感器通过图像信息实现移动机器人三维的定位建图获取较多的信息估计机器人三维位姿，但是视觉传感器易受光照的影响，且精度低，计算复杂度高，实时性差。

南京大学图书馆通过机器人识别标签来辅助机器人辅助定位，但对于地下缆沟环境一是标签无法整齐规划的进行安置，且对于阴暗，复杂不规则障碍物较多的环境机器人也无法很流畅的去进行对标志的获取。

山东大学控制科学通过云端对机器人得到激光雷达信息与路标信息进行处理，虽然减少了机器人的负担，但在一是地下电缆沟环境较为密闭，通讯上很难做到时刻保持非常良好的信号，二是路标不易安置。在该环境下实时性不高。

如今大部分的电缆沟检测考虑周围环境都过于理想。在电缆沟这样较为复杂的环境下无法实际实施。

因此，本领域的技术人员致力于开发一种基于多传感器融合的电缆沟智能巡检机器人定位方法。针对地下电缆沟通道复杂环境进行多传感器融合来使得移动机器人实现精确的定位和建图。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是移动机器人在复杂的电缆沟环境下进行定位时单一的传感器无法对周围未知环境的突然情况提供更多的解决方案。

为实现上述目的，本发明提供了一种基于多传感器融合的电缆沟智能巡检机器人定位方法，包括以下步骤：

步骤1、基于扩展卡尔曼滤波将编码器、陀螺仪、深度相机的RGB图得到的机器人位姿信息进行融合；

步骤2、基于RBPF粒子滤波算法进行定位和建图。

进一步地，所述步骤1扩展卡尔曼滤波的状态方程与观测方程如下式：

其中X(k)∈Rⁿ是所述机器人k时刻***状态即所述机器人k时刻的位姿向量，A(k-1)∈R^n,n为经过线性化处理得到的状态转移矩阵，O(k-1)∈R^n,h表示k时刻***输入到***状态的映射关系，B(k-1)∈R^n,h是过程噪声分布矩阵，W(k-1)∈R^h是过程向量，u(k-1)∈R^h是所述机器人k-1时刻得到的里程计控制输入信息；Y_i(k-1)∈R^m是第i个传感器的测量信息向量，C_i(k-1)是相应传感器测量矩阵，V_i(k-1)∈R^m为测量噪声。

进一步地，所述步骤1用协方差矩阵表示***状态估计的不确定性。

进一步地，所述步骤1协方差之间的关系满足：

Σ(k)＝A(k)Σ(k-1)A(k)^T+O(k)Q(k)Q(k)^T

Σ(k)表示k时刻***状态的不确定性对应的协方差矩阵，Q(k)表示k时刻***输入控制信息的不确定性对应的协方差。

进一步地，所述编码器的初始协方差矩阵为[100000；010000；000000；000000；000000；000001]。

进一步地，所述深度相机的RGB图的初始协方差矩阵为[100000；010000；000000；000000；000000；000001]。

进一步地，所述机器人路过障碍物信息如式：z_1:k＝(z(1),z(2),…,z(k))。

进一步地，所述RBPF粒子滤波算法包括重要性重采样算法。

进一步地，所述重要性重采样算法通过不断迭代来估计每一时刻所述机器人的位姿。

进一步地，所述重要性重采样算法包括以下步骤：采样-计算权重-重采样-地图估计。

本发明与现有技术相比较，具有如下显而易见的实质性特点和显著优点：

对于地下电缆沟内部自动定位，以往的案例都是引入单独激光雷达或者摄像头进行定位，在电缆沟中复杂情况下对于电缆沟复杂且危险的环境中无法保持准确的定位。无法检测其他不在激光雷达平面的障碍物导致机器人行动受阻不动，视觉传感器易受光照的影响，且精度低，计算复杂度高，实时性差。考虑周围环境都过于理想趋于平面化。无法实际实施。本发明针对上述情况，提出了针对地下电缆沟通道复杂环境进行多传感器融合来使得移动机器人实现精确的定位和建图。将陀螺仪、编码器以及深度相机中的RGB图像三个传感器得到位姿变换进行扩展卡尔曼滤波融合，解决了移动机器人在电缆沟环境下因沙地行走导致的转弯误差，地面不平以及细小障碍物的轻微阻塞而导致的前进误差。使得移动机器人在电缆沟环境下也能进行准确的定位，具有极高的实用性。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明的一个较佳实施例的移动机器人总体设计框图；

图2是本发明的一个较佳实施例的扩展卡尔曼滤波多传感器融合仿真图。

具体实施方式

以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。

在附图中，结构相同的部件以相同数字标号表示，各处结构或功能相似的组件以相似数字标号表示。附图所示的每一组件的尺寸和厚度是任意示出的，本发明并没有限定每个组件的尺寸和厚度。为了使图示更清晰，附图中有些地方适当夸大了部件的厚度。

本发明涉及一种机器人在复杂环境电缆沟下实现精确定位的方式，特别涉及到由编码器和陀螺仪组成的里程计信息、深度相机视觉传感器得到的三维建图图像信息以及激光雷达与超声波两个测距传感器所得到距离信息进行融合实现基于多传感器融合的复杂环境电缆沟定位***。

相对于室内平面移动机器人，在层次复杂的电缆沟环境下，移动机器人仅仅通过单一的传感器在电缆沟内部想获取较好的定位是异常困难的。

本发明针对此单一传感器固有的缺陷和不足，提出一种基于多传感器融合方式进行搭建定位***的移动机器人，以应对地下电缆沟复杂场地。对于电缆沟的实时环境进行考察，移动机器人在地下电缆沟实现定位主要有以下几个问题。

1)在地下电缆沟内部充斥着瓦砾与垃圾，机器人行走的路径不可能为平地。

2)电缆沟内部通道通常使用沙地铺满，且由于下雨，地下电缆沟积水问题，移动机器人移动过程中易打滑。

3)地下电缆沟存在悬挂的电缆线以及较大石子，机器人在行走过程中搭配的云台热成像摄像头易被悬挂的电缆线卡住，以及机器人被大石卡住。

综合上述实时环境的影响，机器人在行走过程中通过携带深度相机、超声波、激光雷达的外部传感器，探索环境中的信息，借此实现自身位置的不断校正，以弥补内部传感器带来的定位不确定性。本发明移动机器人总体设计框图如图1所示。使用基于扩展卡尔曼滤波将编码器、陀螺仪、RGB图三个传感器得到的移动机器人位姿信息进行融合，实现了在复杂电缆沟环境下的移动机器人精准定位。

随着SLAM(同时定位与建图)研究的不断发展,对于同时定位与地图构建的概率形式表述框架主要包括扩展卡尔曼滤波器，RBPF粒子滤波器(Rao-Blackwellised ParticalFilter)和极大似然估计的方法。

本发明使用的SLAM大体框架是基于RBPF粒子滤波算法的Gmapping算法，将定位和建图过程分离，先进行定位再进行建图。

假设状态监测机器人在t时移动,移动机器人携带的内部传感器编码器得到机器人k-1到k之间左右轮子速度即机器人控制信息u_t，机器人里程计(历史控制信息)如式：u_1:k＝(u₁,u₂,…,u_k)。

机器人上的外部传感器激光雷达测量得到障碍物信息z_k，机器人路过全部障碍物信息如式：z_1:k＝(z(1),z(2),…,z(k))。

SLAM要解决的问题就是由控制数据u_1:k和观测数据z_1:k来求位姿和地图的联合分布P(X_1:k,M|z_1:k,u_1:k-1)，其中X_1:k＝(X(1),X(2),…,X(k))表述移动机器人整个k时间的位姿向量，X_k＝(x(k),y(k),′z(k),pitch(k),roll(k),yaw(k))，M＝(M(1),M(2),…,M(k))表述移动机器人运动的整个过程中通过激光雷达数据或者其他观测数据得到周围障碍物位姿信息。定位需要建图，建图需要先定位，RBPF算法在X_1:k与M相互独立，通过全概率公式展开可得下式，先定位后建图。

P(X_1:k,M|z_1:k,u_1:k-1)＝P(M|X_1:k,z_1:k)·P(X_1:k|z_1:k,u_1:k-1)

在上列式子中P(X_1:k|z_1:k,u_1:k-1)是移动机器人里程计u_1:k-1与激光雷达数据z_1:k得到的移动机器人得到初始位姿后预测下一时刻机器人位姿后验概率密度分布。P(M|X_1:k,z_1:k)是将移动机器人预测的位姿后验概率密度分布结合激光雷达数据得到的观测数据z_1:k来预测得到电缆沟内部障碍物地标位姿地图概率密度分布。为了估计位姿，RBPF使用粒子滤波来估计机器人位姿。粒子滤波中最常用的是重要性重采样算法。这个算法通过不断迭代来估计每一时刻机器人的位姿，总共包括四个步骤：采样-计算权重-重采样-地图估计。

如果里程计u_1:k-1是通过简单的编码器得到轮子速度通过差速运动机器人逆解计算得到的，那么在上述地下电缆沟内部条件2下，机器人在该环境轮子易打滑，路面不平，速度控制不够稳定，且有爬坡的情况，这些因素误差将急剧增加，很容易导致移动机器人所预测的轨迹概率分布严重出错，建图失败。

本发明在基于扩展卡尔曼滤波(EKF)的多传感器融合，加入陀螺仪计得到的偏航角算出移动机器人位姿改善机器人旋转的定位性能，加入深度相机中的RGB图，通过ORB特征提取从地下电缆沟内部图像中查找特殊区域，计算得到特征向量，从而算出相邻两帧图片的位姿移动估计以此改善机器人前进时的定位性能。

在离散时间k轴轴上，将非线性动态扩展卡尔曼滤波多传感器的状态方程与观测方程如下式：

其中X(k)∈Rⁿ是移动机器人k时刻***状态即机器人k时刻的位姿向量，A(k-1)∈R^n,n为经过线性化处理得到的状态转移矩阵，O(k-1)∈R^n,h表示k时刻***输入到***状态的映射关系，B(k-1)∈R^n,h是过程噪声分布矩阵，W(k-1)∈R^h是过程向量，u(k-1)∈R^h是移动机器人k-1时刻得到的里程计控制输入信息。Y_i(k-1)∈R^m是第i个传感器的测量信息向量，C_i(k-1)是相应传感器测量矩阵，V_i(k-1)∈R^m为测量噪声。

如果***状态的估计服从高斯分布，那么可以用一个协方差矩阵表示***状态的估计的不确定性。***输入同样带有不确定性。协方差之间的关系满足，如式：

Σ(k)＝A(k)Σ(k-1)A(k)^T+O(k)Q(k)Q(k)^T

Σ(k)表示k时刻***状态的不确定性对应的协方差矩阵，Q(k)表示k时刻***输入控制信息u(k)的不确定性对应的协方差。

利用***状态的额外的传感器观测Y(k)对***状态和***状态的协方差进行更新的公式为：

P(X(k))⁺＝P(X(k))^-+K(k)(Y_i(k)-C_i(k)P(X(k))^-)

P(Σ(k))⁺＝(I-K(k)C_i(k))P(Σ(k))^-(I-K(k)C_i(k))^T+K(k)R(k)K(k)^T

式中，I表示满足维数要求的一个单位矩阵,P(X(k))⁺，P(X(k))^-分别表示***状态的先验估计和***状态的后验估计。P(Σ(k))⁺，P(Σ(k))^-分别表示***的先验概率和后验概率对应的协方差矩阵。R(k)表示的是观测值的不确定性对应的协方差矩阵。在实际应用中，R(k)的值由传感器性质所决定。

K(k)是利用额外观测对***状态进行修正的过程中的重要变量，也是扩展卡尔曼多传感器融合的重要参数。根据传感器不同而特定的变换，如下式在卡尔曼滤波算法中被称为卡尔曼增益。

K(k)＝1/((H/P(Σ(k))^-)+1)

其中H表示表示测量噪声协方差，与传感器在地下电缆沟中实际环境相关。

本发明的所融合的传感器的***状态量与对应的协方差矩阵如下：

编码器的***状态量[x,y,z,pitch,roll,yaw]^T

由于本发明***为二维平面的移动机器人，根据右手直角坐标系。z＝0，(x,y)为移动机器人在地图中的预测坐标位姿。pitch是围绕X轴旋转，也叫做俯仰角。roll是围绕Y轴旋转，也叫翻滚角。yaw是围绕Z轴旋转，也叫偏航角。本发明的移动机器人只存在绕Z轴运动，故roll＝pitch＝0。

编码器的初始协方差矩阵为[100000；010000；000000；000000；000000；000001]

陀螺仪***状态量[pitch,roll,yaw]^T

其中x＝y＝z＝0，陀螺仪的初始协方差矩阵为[000100；000010；000001]

深度相机的RGB图的***状态量[x,y,z,pitch,roll,yaw]^T

其中z＝0，深度相机的RGB图的初始协方差矩阵为[100000；010000；000000；000000；000000；000001]

第一个传感器的观测方程通过更新后得到***的状态量X(t)及***协方差矩阵Σ(t)。将二者作为下一个传感器更新过程的***预测状态量

和***预测协方差矩阵

进行状态更新。将最后一个传感器更新后得到的***的状态量X(k)及***协方差矩阵Σ(k)作为融合后输出。

图2为将相邻图像帧的得到的机器人位姿信息与陀螺仪得到的机器人位姿信息与编码器得到的机器人位姿信息经过扩展卡尔曼滤波后的新里程计预测得到的移动机器人位姿、真实机器人位姿、单一编码器得到的预测位姿的对比图。

将机器人小车放于电缆沟下，通过以太网与地面终端计算机通信，终端计算机通过发布机器人小车目的地命令进行自动导航，实时显示移动机器人在位置环境下的定位与建图。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (10)

1.一种基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，包括以下步骤：

步骤1、基于扩展卡尔曼滤波将编码器、陀螺仪、深度相机的RGB图得到的机器人位姿信息进行融合；

步骤2、基于RBPF粒子滤波算法进行定位和建图。

2.如权利要求1所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述步骤1扩展卡尔曼滤波的状态方程与观测方程如下式：

其中X(k)∈Rⁿ是所述机器人k时刻***状态即所述机器人k时刻的位姿向量，A(k-1)∈R^n,n为经过线性化处理得到的状态转移矩阵，O(k-1)∈R^n,h表示k时刻***输入到***状态的映射关系，B(k-1)∈R^n,h是过程噪声分布矩阵，W(k-1)∈R^h是过程向量，u(k-1)∈R^h是所述机器人k-1时刻得到的里程计控制输入信息；Y_i(k-1)∈R^m是第i个传感器的测量信息向量，C_i(k-1)是相应传感器测量矩阵，V_i(k-1)∈R^m为测量噪声。

3.如权利要求2所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述步骤1用协方差矩阵表示***状态估计的不确定性。

4.如权利要求3所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述步骤1协方差之间的关系满足：

Σ(k)＝A(k)Σ(k-1)A(k)^T+O(k)Q(k)Q(k)^T

Σ(k)表示k时刻***状态的不确定性对应的协方差矩阵，Q(k)表示k时刻***输入控制信息的不确定性对应的协方差。

5.如权利要求4所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述编码器的初始协方差矩阵为[100000；010000；000000；000000；000000；000001]。

6.如权利要求4所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述深度相机的RGB图的初始协方差矩阵为[100000；010000；000000；000000；000000；000001]。

7.如权利要求1所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述机器人路过障碍物信息如式：z_1:k＝(z(1),z(2),…,z(k))。

8.如权利要求1所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述RBPF粒子滤波算法包括重要性重采样算法。

9.如权利要求8所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述重要性重采样算法通过不断迭代来估计每一时刻所述机器人的位姿。

10.如权利要求8所述的基于多传感器融合的电缆沟智能巡检机器人定位方法，其特征在于，所述重要性重采样算法包括以下步骤：采样-计算权重-重采样-地图估计。

Images