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

Abstract

本发明公开了一种基于多传感器融合的电缆沟智能巡检机器人建图方法，涉及智能机器人技术领域，此建图方法包括：采集巡检机器人内部传感器和外部传感器的观测信息，获得巡检机器人的定位；根据RBpf‑slam算法在自身定位的基础上建立环境地图，获得位姿和地图的联合分布公式，其中，建立环境地图包括将观测信息均转换为激光数据格式并进行坐标变换；通过采样、计算权重、重采样和地图估计实现不断迭代来估计每一时刻巡检机器人的位姿算法。本发明将激光雷达的激光数据、深度相机的深度信息和超声波的距离信息转换而来的伪激光数据进行融合，丰富了外部观测量的信息，使得激光回环效果更容易实现，巡检机器人实现精确的建图。

Description

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

技术领域

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

背景技术

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

于是，各种先进的地下电缆沟检测设备和检测方法应运而生。比如为了满足电力系统运维过程中电缆沟巡检的任务要求而设计的地下电缆沟巡检小车，外部搭载激光雷达、编码器与陀螺仪组成的里程计，并基于RBPF-SLAM(Rao－Blackwellised粒子滤波器定位算法)，但由于激光雷达仅仅只是对雷达当前的2D平面的障碍物进行反馈，在地下电缆沟中的复杂的环境中，仅仅以单一2D平面作为障碍物信息反馈，无法检测其他不在激光雷达平面的障碍物，从而导致机器人行动受阻不动，因为回环反馈影响导致机器人定位产生过大的误差无法调节，不及时解决将导致建图和定位完全错误；另一种智能型电缆沟巡检机器人，通过基于激光雷达的hector-slam算法，建立地图以此来得到机器人的定位，使用一个激光雷达传感器所得到的单一2D平面障碍物信息进行机器人在电缆沟中的定位和建图，但hector-slam采用泰勒展开近似优化激光雷达数据的匹配过程，因此两次激光雷达采集数据时位姿变化不能太大，否则余项误差过大，此方法需要雷达的更新频率较高，测量噪声小且对环境要求严格，在制图过程中，机器人速度控制在比较低且地面平整的情况下，定位建图效果才会较准确，对于电缆沟复杂且危险的环境中无法保持准确的定位和建图。

同时其他场景中的检测方法也不适用于地下电缆沟环境。比如针对室内机器人的定位和三维稠密地图构建系统，应用具有跟踪、地图构建和重定位三平行线程的ORB-SLAM算法估计机器人三维位姿的定位，然后拼接深度摄像头KINECT获得的三维稠密点云，提出空间域上的关键帧提取方法剔除冗余的视频帧，接着提出子地图法进一步减少地图构建的时间，最终提高算法的整体速度，但是视觉传感器易受光照的影响，精度低，此方法计算复杂度高，实时性差。

比如针对无人车领域在室外进行负障碍物检测，提出的一种基于单线激光雷达和单目视觉的负障碍检测算法，为弥补单线激光雷达在覆盖能力方面的不足对检测到的负障碍区域在摄像机画面中进行跟踪，结合跟踪结果对负障碍区域做进一步判别，但是此方法在地下电缆沟环境中，光照不足对图像处理有严重影响。

比如针对图书馆环境，将RFID技术、模糊逻辑与机器人自动化技术相结合，设计机器人自主导航算法，通过在固定点设计标签让机器人识别进行定位，使图书盘点机器人能够在高度同构、空间狭窄的图书馆环境中进行精准定位，但对于地下电缆沟环境，标签无法整齐规划的进行安置，且对于阴暗、复杂不规则障碍物较多的环境，机器人也无法很流畅的去进行标志的获取。

比如针对单个机器人在大场景下建图与定位效果不佳的问题，提出一种基于路标信息与云架构的多机器人实时协作融合建图方法，首先利用“云端-终端”的网络架构在云端服务器实时处理各机器人上传的数据，其次在单个机器人构建地图的同时检测环境中路标，完成路标与各局部地图的绑定，之后云端服务器根据路标检测结果确定多个地图间存在重叠区域，构建位置坐标的超定方程组并求解地图间最优变换矩阵，同时实现对变换参数的持续更新与优化，最后通过融合算法完成多个地图的处理，形成全局地图，但是地下电缆沟环境较为密闭，通讯上很难做到时刻保持非常良好的信号，且路标不易安置，在该环境下实时性不高。

综上所述，移动机器人在地下电缆沟主要面临的问题有：地下电缆沟内部充斥着瓦砾与垃圾，障碍物较多；地下电缆沟存有悬挂在电缆沟道半空中的电缆线，搭配了云台红外热成像摄像头的移动机器人会被电缆线绊倒。以往案例都是引入单独激光雷达或者摄像头，无法保持准确的定位和建图，且如今大部分的检测方法对周围环境要求过高，都过于理想和平面化，在这种类似电缆沟较为复杂的环境下无法实施。

因此，本领域的技术人员致力于开发一种基于多传感器融合的电缆沟智能巡检机器人建图方法，克服单一传感器固有的缺陷和不足，在电缆沟复杂且危险的环境中实现精确的定位和建图。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是移动机器人在复杂的电缆沟环境下进行建图时无法对周围未知环境提取完整的信息，无法检测由于激光雷达或深度相机单一传感器而导致的死角障碍物，最终造成移动机器人运动受阻回环检测出错，可靠性差。

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

步骤1、采集巡检机器人内部传感器和外部传感器的观测信息，获得巡检机器人的定位；

步骤2、根据RBpf-slam算法在自身定位的基础上建立环境地图，获得位姿和地图的联合分布公式，其中，所述建立环境地图包括将所述的观测信息均转换为激光数据格式并进行坐标变换；

步骤3、通过采样、计算权重、重采样和地图估计实现不断迭代来估计每一时刻巡检机器人的位姿算法。

进一步地，所述步骤1包括：

基于巡检机器人的内部传感器的观测信息获得里程计；

基于巡检机器人的外部传感器的观测信息获得观测数据；

所述内部传感器的观测信息为巡检机器人控制信息，所述外部传感器的观测信息为巡检机器人测量得到的障碍物信息，包括激光雷达测得的激光信息、深度相机测量得到的深度信息和超声波测量得到的距离信息。

进一步地，所述里程计计算如下：

u_1:k＝(u₁,u₂,…,u_k)

式中，u_t表示巡检机器人控制信息，所述控制信息是巡检机器人在k-1到k之间左右轮子的速度；

所述观测数据是巡检机器人路过全部障碍物信息，计算如下：

z_1:k＝(z₁,z₂,…,z_k)

式中，z_t表示障碍物信息。

进一步地，所述位姿和地图的联合分布公式如下：

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)

式中，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))表示巡检机器人运动的整个过程中观测得到的周围障碍物位姿信息，P(X_1:k|z_1:k,u_1:k-1)表示巡检机器人得到初始位姿后预测下一时刻机器人位姿后验概率密度分布，P(M|X_1:k,z_1:k)表示将巡检机器人预测的位姿后验概率密度分布结合所述观测数据来预测得到电缆沟内部障碍物地标位姿地图概率密度分布。

进一步地，所述建立环境地图包括以下步骤：

步骤1、获取激光数据信息和伪激光数据信息；

步骤2、进行巡检机器人的坐标变换，包括所述外部传感器与巡检机器人本体坐标变换、巡检机器人全局坐标框架与局部坐标框架坐标变换。

进一步地，所述激光数据信息是所述激光雷达测得的以激光雷达原点为中心而获得的激光信息、所述伪激光数据信息是将所述深度信息和所述距离信息转换成对应的激光数据格式。

进一步地，所述深度信息转换成对应的激光数据格式包含以下步骤：

步骤1、所述深度信息为深度图，将所述深度图压缩处理；

步骤2、进行激光格式数据转换：

r_i＝min(r_i1,r_i2,…,r_ij,…,r_i340)

式中，ri是所检测到的距离深度相机最近的障碍物。

进一步地，所述超声波的测量范围设置为2.5m。

进一步地，所述外部传感器与巡检机器人本体坐标变换是通过把所述激光数据信息和所述伪激光数据信息转换成以基础坐标原点为原点的坐标系下的数据，以实现机器人根据所述基础坐标原点得到所述观测数据，所述基础坐标原点是机器人本体的中心位置。

进一步地，所述巡检机器人全局坐标框架与局部坐标框架坐标变换是通过建立巡检机器人全局坐标系和局部坐标系之间的关系，以描述巡检机器人在二维空间中的位姿，其中，所述全局坐标系为{X_t,Y_t}，原点以运动起点机器人几何中心定义，X_t表示起点时机器人的正前方向，Y_t表示起点时机器人车体第一关节垂直方向；所述局部坐标系为{X_r,Y_r}，原点以机器人第一关节当前位置几何中心定义，X_r表示当前机器人的正前方向，Y_r表示当前机器人车体第一关节垂直方向；在所述全局坐标系下，巡检机器人的位置为(x,y)，β为机器人的偏航角，机器人的位姿如下表示：

旋转矩阵把机器人所述全局坐标系中的位姿对应到所述局部坐标系下：

由位姿以及所述全局坐标系与局部坐标系旋转角度差得出最终所述全局坐标系和所述局部坐标系的映射公式如下：

从而测到机器人在运动过程中的观测信息在全局地图中的位姿：

与现有技术相比，本发明至少具有如下有益技术效果：

本发明将深度相机的深度信息和超声波的距离信息转换为伪激光数据与激光雷达的激光数据融合进行建图，得到了更加丰富、精确的电缆沟通道环境信息，解决了悬挂的电缆沟线以及存在于死角的障碍物导致巡检机器人导航过程中出现阻塞和车祸的问题。

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

附图说明

图1是本发明的一个较佳实施例的方法流程示意图；

图2是本发明的一个较佳实施例的装置结构示意图；

图3是本发明的一个较佳实施例的深度信息图；

图4是本发明的一个较佳实施例的外部传感器探测范围示意图；

图5是本发明的一个较佳实施例的外部传感器与巡检机器人本体坐标变换示意图；

图6是本发明的一个较佳实施例的全局坐标系示意图。

具体实施方式

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

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

如图1所示，是本实施例中的基于多传感器融合的电缆沟智能巡检机器人建图方法流程示意图，包括以下步骤：

步骤1、采集巡检机器人内部传感器和外部传感器的观测信息，获得巡检机器人的定位；

步骤2、根据RBpf-slam算法在自身定位的基础上建立环境地图，获得位姿和地图的联合分布公式，其中，建立环境地图包括将观测信息均转换为激光数据格式并进行坐标变换；

步骤3、通过采样、计算权重、重采样和地图估计实现不断迭代来估计每一时刻巡检机器人的位姿算法。

如图2所示，是本实施例的装置结构示意图，包括移动机器人主脑、外部传感器和内部传感器，其中，外部传感器包括激光雷达、深度相机、超声波，内部传感器包括陀螺仪和编码器。

具体地，假设巡检机器人在t时移动，巡检机器人携带的内部传感器编码器得到机器人k-1到k之间左右轮子速度即机器人控制信息u_t，里程计计算如下：

u_1:k＝(u₁,u₂,…,u_k)

外部传感器的观测信息为巡检机器人测量得到的障碍物信息，包括激光雷达测得的激光信息、深度相机测量得到的深度信息和超声波测量得到的距离信息，将这些信息进行处理后得到巡检机器人全部障碍物信息，即观测数据计算如下：

z_1:k＝(z₁,z₂,…,z_k)

式中，z_t表示障碍物信息。

位姿和地图的联合分布公式如下：

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)

式中，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))表示巡检机器人运动的整个过程中观测得到的周围障碍物位姿信息，P(X_1:k|z_1:k,u_1:k-1)表示巡检机器人得到初始位姿后预测下一时刻机器人位姿后验概率密度分布，P(M|X_1:k,z_1:k)表示将巡检机器人预测的位姿后验概率密度分布结合观测数据来预测得到电缆沟内部障碍物地标位姿地图概率密度分布。

其中，根据RBpf-slam算法在自身定位的基础上建立环境地图包括以下步骤：

步骤1、获取激光数据信息和伪激光数据信息。

激光数据信息是激光雷达测得的以激光雷达原点为中心而获得的激光信息，伪激光数据信息是将深度信息和距离信息转换成对应的激光数据格式。

将深度相机放置激光雷达上方，得到对应RGB图的深度信息如图3所示，本实施例的深度图的分辨率为240x320,将深度图进行压缩处理，之后转换成激光数据格式；根据本实施例的巡检机器人安置的激光雷达位置到云台最高点达实际高度与深度图得到的信息，本实施例选取第100行到240行每列中距离成像平面最近的点，如下式：

r_i＝min(r_i1,r_i2,…,r_ij,…,r_i340)(100<i<240)

式中，r_i是所检测到的距离深度相机最近的障碍物，从而实现既保留障碍物的高度信息，又有效地减少了数据量的大小。

将三个超声波放置在巡检机器人的左前方、前方、右前方，用于检测存在低于激光雷达与深度相机检测死角的瓦砾与石头，由于超声波测距精度超过3m有较大的误差，本发明将超声波的测量范围设置在2.5m，之后得到的距离位置换算成为激光信息。

本实施例通过对三个传感器的融合，可在电缆沟下获取大量的视角、丰富的环境信息和准确的外部测量数据，如图4所示，是本实施例的外部传感器的探测范围。

步骤2、进行巡检机器人的坐标变换。

步骤2.1外部传感器与巡检机器人本体坐标变换。

将巡检机器人本体的中心位置称为基础坐标原点，激光雷达的几何中心位置称为激光雷达原点，深度相机的几何中心位置称为深度相机中心，超声波的几何中心位置称为超声波中心，激光雷达测得的数据为以激光雷达原点为中心获得的数据，深度相机获取移动机器人前方的悬挂电缆沟线，超声波获取低于激光雷达扫描平面的大石子与电缆线。在地下电缆沟内部巡检机器人运动过程中，需要把激光雷达、深度相机以及超声波测得的数据变换成以巡检机器人的几何中心位置为原点的坐标系下的数据，从而让巡检机器人准确的收集数据，以实现机器人根据自身为中心得到观测数据。

本实施例的激光雷达、深度相机以及超声波放置位置如图5所示，激光雷达几何中心位于巡检机器人几何中心正前方20cm，高15cm处；深度相机几何中心位于巡检机器人几何中心正前方5cm高25cm处，三个超声波的几何中心分别位于巡检机器人几何中心的正前方40cm正右方15cm、正前方40cm正左方15cm、正前方40cm处；激光雷达测得障碍物位于以激光雷达为中心的二维地图(x:1.2,y:0.3)处，经过坐标变换，障碍物位于机器人几何中心(x:1.55,y:0.3)处；对于超声波测得障碍物位于以超声波为中心的二维地图(x:0.5,y:0.5)处，经过坐标变换，障碍物位于机器人几何中心(x:0.9,y:0.3)处；对于深度相获取机障碍物位于以深度相机中心的三维地图中距离深度相机最近的点(x:0.8,y:0.3,z:0.5)处，经过深度图激光数据格式转换得到(x:0.8,y:0.3)，通过坐标变换，障碍物位于机器人几何中心(x:0.85,y:0.3)处。

步骤2.2巡检机器人全局坐标框架与局部坐标框架坐标变换。

通过建立巡检机器人全局坐标系和局部坐标系之间的关系，以描述巡检机器人在二维空间中的位姿，如图6所示，全局坐标系为{X_t,Y_t}，原点以运动起点机器人几何中心定义，X_t表示起点时机器人的正前方向，Y_t表示起点时机器人车体第一关节垂直方向；局部坐标系为{X_r,Y_r}，原点以机器人第一关节当前位置几何中心定义，X_r表示当前机器人的正前方向，Y_r表示当前机器人车体第一关节垂直方向；在全局坐标系下，巡检机器人的位置为(x,y)，β为机器人的偏航角，机器人的位姿如下表示：

当β＝0°时，机器人的航向为X_t轴方向；当β＝90°时，机器人的航向为Y_t轴方向。没有航向的位姿称为位置，位置关系到机器人在地下电缆沟中对四周的感知，位置用一个二维向量表示：

旋转矩阵R(β)，把机器人全局坐标系中的位姿对应到局部坐标系下：

由位姿以及全局坐标系与局部坐标系旋转角度差得出最终全局坐标系和局部坐标系的映射公式如下：

从而测到机器人在运动过程中的观测信息在全局地图中的位姿：

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

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

Claims (4)

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

步骤1、采集巡检机器人内部传感器和外部传感器的观测信息，获得巡检机器人的定位；基于巡检机器人的内部传感器的观测信息获得里程计；基于巡检机器人的外部传感器的观测信息获得观测数据；所述内部传感器的观测信息为巡检机器人控制信息，所述外部传感器的观测信息为巡检机器人测量得到的障碍物信息，包括激光雷达测得的激光信息、深度相机测量得到的深度信息和超声波测量得到的距离信息；

步骤2、根据RBpf-slam算法在自身定位的基础上建立环境地图，获得位姿和地图的联合分布公式，其中，所述建立环境地图包括将所述的观测信息均转换为激光数据格式并进行坐标变换；

所述步骤2包括：

步骤2.1、获取激光数据信息和伪激光数据信息；

所述激光数据信息是所述激光雷达测得的以激光雷达原点为中心而获得的激光信息；

所述伪激光数据信息是将所述深度信息和所述距离信息转换成对应的激光数据格式；所述深度信息转换成对应的激光数据格式包括：

所述深度信息为深度图，将所述深度图压缩处理；

进行激光格式数据转换：

r_i＝min(r_i1,r_i2,…,r_ij,…,r_i340)

式中，r_i是所检测到的距离深度相机最近的障碍物；

步骤2.2、进行巡检机器人的坐标变换，包括所述外部传感器与巡检机器人本体坐标变换、巡检机器人全局坐标框架与局部坐标框架坐标变换；

所述外部传感器与巡检机器人本体坐标变换是通过把所述激光数据信息和所述伪激光数据信息转换成以基础坐标原点为原点的坐标系下的数据，以实现机器人根据所述基础坐标原点得到所述观测数据，所述基础坐标原点是机器人本体的中心位置；

所述巡检机器人全局坐标框架与局部坐标框架坐标变换是通过建立巡检机器人全局坐标系和局部坐标系之间的关系，以描述巡检机器人在二维空间中的位姿，其中，所述全局坐标系为{X_t,Y_t}，原点以运动起点机器人几何中心定义，Xt表示起点时机器人的正前方向，Y_t表示起点时机器人车体第一关节垂直方向；所述局部坐标系为{X_r,Y_r}，原点以机器人第一关节当前位置几何中心定义，Xr表示当前机器人的正前方向，Y_r表示当前机器人车体第一关节垂直方向；在所述全局坐标系下，巡检机器人的位置为(x,y)，β为机器人的偏航角，机器人的位姿如下表示：

旋转矩阵R(β)把机器人所述全局坐标系中的位姿对应到所述局部坐标系下：

由位姿以及所述全局坐标系与局部坐标系旋转角度差得出最终所述全局坐标系和所述局部坐标系的映射公式如下：

从而测到机器人在运动过程中的观测信息在全局地图中的位姿：

步骤3、通过采样、计算权重、重采样和地图估计实现不断迭代来估计每一时刻巡检机器人的位姿算法。

2.如权利要求1所述的基于多传感器融合的电缆沟智能巡检机器人建图方法，其特征在于，所述里程计计算如下：

u_1:k＝(u₁,u₂,…,u_k)

式中，u_t表示巡检机器人控制信息，所述控制信息是巡检机器人在k-1到k之间左右轮子的速度；

所述观测数据是巡检机器人路过全部障碍物信息，计算如下：

z_1:k＝(z₁,z₂,…,z_k)

式中，z_t表示障碍物信息。

3.如权利要求1所述的基于多传感器融合的电缆沟智能巡检机器人建图方法，其特征在于，所述位姿和地图的联合分布公式如下：

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)

式中，X_1:k＝(X(1),X(2),…,X(k))表示巡检机器人整个k时间的位姿向量，M＝(M(1),M(2),…,M(k))表示巡检机器人运动的整个过程中观测得到的周围障碍物位姿信息，P(X_1:k|z_1:k,u_1:k-1)表示巡检机器人得到初始位姿后预测下一时刻机器人位姿后验概率密度分布，P(M|X_1:k,z_1:k)表示将巡检机器人预测的位姿后验概率密度分布结合所述观测数据来预测得到电缆沟内部障碍物地标位姿地图概率密度分布。

4.如权利要求1所述的基于多传感器融合的电缆沟智能巡检机器人建图方法，其特征在于，所述超声波的测量范围设置为2.5m。