基于三维激光和双目视觉的变电站巡检机器人定位导航系统及方法与流程

本发明涉及移动机器人定位导航技术，尤其涉及一种基于三维激光和双目视觉的变电站巡检机器人定位导航系统及方法。

背景技术：

变电站巡检是电网运维检修中的基础环节，是变电站日常运维工作中的重要组成部分。电网规模的增长与运检人员配置的矛盾日益突出，人工巡检方式难以适应电网运检“精益化”的发展需求，也不符合电网公司运检智能化的要求。变电站巡检机器人投入应用，可有效缓解电网设备规模快速增长与运检人员不足的矛盾，并及时、准确发现设备缺陷和安全隐患，预防变电站内电力安全事故的发生，保障变电站内设备安全、可靠运行。自主定位和导航技术是变电站巡检机器人的基本功能。

目前机器人常用的定位导航技术主要有视觉slam、激光slam、gps定位、uwb定位、wifi定位以及磁导航等。其中，基于gps、uwb、wifi等定位技术存在精度不够、且易受变电站电磁干扰等缺陷；磁导航技术存在破坏变电站道路设施、施工困难、磁条失磁等缺陷，均无法应用于变电站巡检机器人；而基于视觉或激光的slam技术能够在不改变或极小程度的改变环境的情况下有效地解决机器人定位导航问题。

由于变电站的场景一般规模较大且复杂，相似场景较多，采用单一传感器的slam技术无法取得理想的定位效果。

技术实现要素：

发明目的：针对以上问题，本发明提出一种基于三维激光和双目视觉的变电站巡检机器人定位导航系统及方法，通过激光和视觉的融合方式，实现机器人在变电站中的实时自动定位和导航。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种基于三维激光和双目视觉的变电站巡检机器人定位导航系统，包括三维点云数据获取模块、三维地图构建模块、三维地图可视化模块、点云匹配定位模块，双目视觉信息获取处理模块、机器人机动部分。

三维点云数据获取模块，包括用于采集三维点云数据的三维激光雷达，通过发射激光获取所处环境的三维点云信息，并将点云信息按帧进行存储；三维地图构建模块，采用特定的特征提取算法以及点云匹配算法，对点云数据逐帧进行处理，恢复变电场景的三维特征信息，提供完整地图；点云匹配定位模块，用于根据三维点云数据模块实时获取点云信息，与地图场景进行匹配，获取机器人的全局定位数据；双目视觉信息获取模块，包括双目相机和对应的图像处理算法，用于根据图像信息给出机器人的局部精确定位信息；三维地图可视化模块，用于显示所构建的变电站三维地图，同时实时显示机器人在变电站中所处的位置信息。

进一步地，机器人机动部分包括车体、工控主机、电机、轮式里程计。

一种基于三维激光和双目视觉的变电站巡检机器人定位导航方法，包括以下步骤：

(1)搭建基于三维激光雷达和双目相机的机器人平台；

(2)对机器人载体及载体上的三维激光雷达和双目相机进行参数标定；

(3)构建三维点云地图，并载入机器人中；

(4)记录设定的机器人在巡检点的位姿数据；

(5)同时载有地图和巡检点数据的机器人，在变电站中相继对每个设定的巡检点进行巡查，并通过相应算法实时获取自己在地图中的定位信息。

进一步地，所述步骤3具体包括：

(3.1)在变电站场地中选定起始点，作为地图录制的三维坐标原点；

(3.2)控制机器人载体在变电站中进行地图录制，通过点云处理算法对获取的每一帧三维点云数据进行拼接；

(3.3)将经过处理的点云数据绘制成点云地图并存储。

进一步地，所述步骤4的位姿数据包括：通过激光点云匹配获得的机器人位姿数据；在双目视觉的可视范围内加入可分辨的二维码，通过视觉slam获得机器人相对于该二维码的位姿数据。

进一步地，所述步骤5具体包括：

(5.1)进行激光全局定位；通过激光雷达获得实时的周身点云数据，同时将该帧点云数据与整个地图点云进行匹配从而进行位姿估计；

(5.2)通过激光全局定位获得的数据，机器人被导航至巡检点附近；

(5.3)在巡检点附近进行相机局部定位；拍摄该巡检点对应的标定板，通过计算与标定板的位姿变换，并与存储的对应位姿进行对比，通过对相对位姿的矫正，将机器人精确导航至巡检点。

有益效果：本发明通过融合激光和视觉两种方式，提高机器人在大型复杂场景中的定位准确性和精度，减轻人工的巡检强度和需求，实现变电站的机器人自动巡检。本发明能够在环境复杂的大型变电站场景中进行定位和导航，并保证精度和准确性。

附图说明

图1是机器人定位算法框图；

图2是变电站巡检整体示意图；

图3是三维地图构建模块示意图；

图4是激光特征点匹配示意图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

本发明所述的基于三维激光和双目视觉的变电站巡检机器人定位导航系统，包括三维点云数据获取模块、三维地图构建模块、三维地图可视化模块、点云匹配定位模块，双目视觉信息获取处理模块、机器人机动部分。

变电站指的是电网公司的大型变电站，变电站规模庞大，场地内部设施基本为变电立柱、架设电线等，特征相似。机器人系统需要完成该场景的三维可视化建图，并在场景内对相似场景进行区分，实时通过地图更新自己的定位信息。

机器人自主定位系统的软件是基于ros系统，通过ros节点来连接机器人各个硬件组成部分，完成节点的通信。

三维点云数据获取模块，包括用于采集三维点云数据的16线三维激光雷达，通过发射激光来获取所处环境的三维点云信息，并将点云信息按帧进行存储。

三维地图构建模块，包括点云数据处理的相关算法。采用特定的特征提取算法以及点云匹配算法，对点云数据逐帧进行处理，减小点云数据出现误匹配的情况发生，区分场地内部的相似场景，恢复变电站场景的三维特征信息，提供完整地图。

三维地图可视化模块，用于显示所构建的变电站三维地图，同时实时显示机器人在变电站中所处的位置信息。

点云匹配定位模块，用于根据三维点云数据模块实时获取的点云信息，与地图场景进行匹配，获取机器人的全局定位数据。

双目视觉信息获取模块，包括双目相机以及其对应的图像处理算法，用于根据图像信息给出机器人的局部精确定位信息。

机器人机动部分，包括车体，工控主机，电机，轮式里程计。

如图1所示，本发明所述的基于三维激光和双目视觉的变电站巡检机器人定位导航方法，具体包括如下步骤：

步骤a，搭建基于视觉和激光的机器人平台，用以为实践过程中的实时定位提供硬件基础；

机器人平台实体设计为轮式机器人，便于在变电站场地中灵活转向以及行进至目标巡检点。机器人主要包括车体、工控主机、电机、轮式里程计、激光雷达、双目相机；其中，车体用于承载巡检机器人的其他硬件。

工控主机为整个巡检机器人的控制中心，机器人的linux操作系统，ros系统以及对传感器数据的处理算法均部署在工控主机中。

电机为巡检机器人的机动能力提供动力。

轮式里程计可通过自身的里程记录为巡检机器人提供定位支持，主要用于导航部分，本发明涉及的技术将其作为一种行动能力。

激光雷达为16线三维激光雷达，由工控主机控制，在变电站环境中通过扫描环境的三维特征获得对应的点云信息，以帧为单位将点云数据通过适配器传回至工控主机用于定位和建图用。

双目相机同为本发明中的定位传感器之一，通过将获取的视觉信息传至工控主机，由特定的slam算法解算出定位所需的位姿信息，后面将进行详述。

步骤b，对机器人载体以及载体上的三维激光雷达和双目相机进行参数标定，包括相机的内参和传感器以及载体之间的外参标定；

由于巡检机器人的定位涉及到多个传感器，因此在实现整个巡检机器人在变电站场地中的位置会用到多个不同的坐标系，包括：地图坐标系(世界坐标系)、激光坐标系、相机坐标系、巡检机器人车体坐标系。其中，世界坐标系是以地图初次构建时，车体的初始坐标系作为世界坐标系原点，并在后续的地图更新中将新获得的地图点云信息均转换至世界坐标系下完成整个三维地图的构建。

激光坐标系为三维激光传感器硬件本身的初始坐标系，依据于右手坐标系，激光雷达获取的每一帧点云数据的点云坐标均位于此坐标系下，经过点云处理算法可计算获得该帧点云坐标系相对于上述世界坐标系的位姿变换，包括旋转矩阵r和平移矩阵t，通过式(1)，可将激光坐标系下的点云转换至世界坐标系下。

式中，(xl,yl,zl)为点云在雷达坐标系下的三维坐标，(xw,yw,zw)为点云在世界坐标系下的三维坐标，r和t为雷达坐标系和世界坐标系之间的旋转矩阵和平移矩阵。

相机坐标系为以相机光心为原点的坐标系，在定位的过程中我们希望将捕捉的图像中的目标点在真实世界中找到该点的世界坐标，因此必须建立相机坐标系与世界坐标系的姿态变换关系。同时在单个相机中又存在多种坐标系关系，包括图像坐标系、成像坐标系以及上述相机坐标系，其中图像坐标系是以图像边缘为坐标轴设定的，成像坐标系是以成像中心坐标原点进行设定的。

图像坐标系的基本单元是像素，而成像平面坐标系是以实际物理单位作为坐标值，两者的对应关系如式(2)所示：

式中，(u,v)为图像坐标系，(x,y)为成像坐标系，dx表示每个像素单元对应在x轴上的实际物理量大小，同样，dy表示像素单元在y轴实际物理量的对应，s表示两坐标之间的倾斜度。

由双目相机与成像平面的关系可知，相机的焦距轴垂直于成像画面，根据针孔模型即可建立相机与成像平面之间的对应关系，如式(3)所示：

式中，(x,y)为成像坐标系，(xc,yc,zc,1)^t为相机坐标系，f为相机焦距。

相机坐标系与世界坐标系的对应关系如式(4)所示：

式中，(xc,yc,zc,1)^t为相机坐标系，(xw,yw,zw,1)^t为世界坐标系，r为旋转矩阵，t为平移向量，m为坐标变换矩阵。

结合以上(2)、(3)、(4)式，图像坐标系与世界坐标系的对应关系如下式(5)所示：

其中，αu＝f/dx，αv＝f/dy，s＝s′f，p为投影矩阵，包含了投影变换的内参和外参。

其中，相机的内参标定有多种标定方式，不属于本发明的重点内容，因此不作详细介绍。

本发明中由于激光雷达固定在机器人上，其坐标系将相对于车身坐标系是一个静态变换，因此可将激光雷达的坐标系等同于机器人车身坐标系，具体实施者可根据需求做相应更改。

步骤c，在变电站场地中选定起始点，作为地图录制的三维坐标原点，并在程序中进行相关的设置；

如图2所示，变电站场地示意图，此图为变电站实际场地的缩影俯视图，图中宽线段为场地中机器人可行进到达的地带，大圆圈为场地中的电线立柱，小原点为圆形立柱的表盘所在的方位，表盘即为机器人巡检的过程中需要观测到的物件，图示中表盘经过虚线箭头所指向的路径上的终点为设定的可观测点，即巡检机器人在巡检的过程中导航至此处，即可观测并获取到表盘上的读数，完成巡检任务。

在整个巡检过程中，实现机器人自主从出发点行进至观测点1，从观测点1行进至观测点2，以此类推，最终机器人完成巡检任务回到起始点。

步骤d，通过ros节点通信，控制机器人载体在需要巡检的变电站场景中进行地图录制，通过点云处理算法对获取的每一帧三维点云数据进行拼接，逐帧处理以完成整个三维点云地图的构建；

在机器人行进的过程中，尽可能将需要巡检的场景中的特征充分提取。

机器人的软件算法均部署在ubuntu操作系统中的ros系统下，ros是用于编写机器人软件程序的一种具有高度灵活性的软件架构，本发明主要将算法按功能编写成节点，并完成节点间的相互通信。上述步骤中主要包括三维点云采集节点、激光里程计节点、三维地图构建节点，如图3所示。

1)三维点云采集，主要由16线三维激光雷达完成，此时获取的点云坐标系为激光坐标系；

2)激光里程计，主要完成的工作是通过订阅的激光数据来估计激光雷达的位姿变换，相邻两帧点云数据进行拼接，此过程需要进行匹配以及位姿估计，从而将一帧点云转换至某一坐标系下；

变电站的场景规模较大，获取的点云数据量也相对较大，因此对算法的实时性以及计算能力有一定的要求，常规的icp点云匹配算法不适用于此场景，则本发明应用loam的特征提取以及位姿估计算法，大大减小点云匹配的时间，其基本算法步骤如下：

(2.1)对点云进行特征点提取，对每帧扫描得到的点计算其曲率值，遍历点云集，对所有的点分别求出其与前后5个点的偏差，以x坐标为例，如式(6)所示：

同时dy和dz也采用相同的式子进行计算求值，由式(7)计算出每个点的曲率值，作为对三维点进行特征区分的依据。

curvature＝(dx)²+(dy)²+(dz)²(7)

同时，经过三维激光雷达获取的点云并不是每个都可以作为特征点进行匹配，需要将可能会引起较大误差的点进行剔除，本文中将剔除的点称为瑕点。

根据loam的特征匹配要求，将点云中有如下特征的点称为瑕点并进行剔除：

a、当某点所在平面近似平行于激光束时，认为该点不可靠；

b、当某点在被遮挡区域的边缘时，也认为该点不可靠，因为当激光移动一段距离后，被遮挡部分露出来，该点将成为非边缘点。

将不可用于优化的瑕点剔除后，根据设置的阈值，可将点分为平面点和角点，由于16线激光本身将激光点云分为16层，根据点的方位角将每一层的激光点分成6份，在每一份中选取不超过4个平面点和不超过2个角点作为后续用于特征匹配的特征点。

(2.2)对获取的两帧点云不采用一一对应的点云匹配关系，使用类似icp算法进行匹配以及位姿估计；

首先是对角点处理，根据角点的数量循环处理每一个角点，将角点根据点云强度值的值还原到此帧的初始位置，然后在上一帧点云数据中寻找距此点m的最近的一个点n，然后获取点n所在的线的层数，然后在点n的附近两层中再搜寻离点m最近的一个点p，这时(n,p)与点m形成了一个对应关系，如图4a所示，在理想情况下m、n、p三点应该共线，所以按照此理论，算法的迭代过程就是把尽量是点到线的距离最短。求点到线的距离公式如式(8)：

角点处理完毕后，开始处理平面点，对于平面点的处理和角点是一样的，依旧是对本帧中每一个点进行循环遍历，找到上一帧中距离该点最近的三个点，则此三个点构成一个平面，如图4b所示，此点在理想情况下m、n、q、p四点应该共面，理论公式如下：

随后通过构造的匹配关系，使用slam中传统的l-m非线性优化算法，计算出位姿变换矩阵r和t，即可求得两帧点云的位姿关系，并将点云以及相应的位姿通过ros节点发布出去。

3)三维地图构建，通过订阅激光里程计节点发布的激光点云数据以及相应的位姿数据，将每帧点云转换至世界坐标系下，当由三维点云采集节点获取的每一帧点云数据均转换至世界坐标系下，变电站的三维点云地图即构建完毕，具体图示如图3中的点云地图。

步骤e，将经过处理的点云数据绘制成点云地图并存储为pcd文件，为后面机器人的巡检定位提供参考；

机器人通过载入pcd文件，即可获取该pcd文件所代表的地图的点云信息，实现地图的数字化，为后续步骤的定位提供数字化场景地图。

步骤f，对巡检点进行记录；

将装载有地图pcd文件的机器人再次开进变电站中，在需要巡检停顿的地点先进行记录，包括此时通过激光点云匹配获得的当前位姿数据。同时在双目视觉此时的可视范围内，加入可分辨的二维码，通过视觉slam获得此时机器人相对于该二维码的位姿数据，将这两类数据作为巡检点在整个地图中的“标识符”。

上述步骤中所述的巡检点即为图2中的终点，机器人在巡检的过程中需要准确到达对应的巡检点，才能够完成对相应表盘的巡查工作。

激光雷达传感器通过计算巡检点处的点云与地图点云的位姿变换，来记录机器人此时的位姿数据。相机通过计算巡检点处机器人观测到的图像信息，记录机器人此时相对于特定标定板的相对位姿数据。将对应巡检点的位姿数据进行记录，作为下次机器人巡检过程中的定位指标。

步骤g，同时载入地图和巡检点数据的机器人通过适当配置，即可按照要求在变电站中相继对每个设定的巡检点进行巡查，并通过地图实时获取自己的定位信息，即实现机器人在变电站中的自动巡检，如图1所示，巡检定位的算法步骤包括：

1)激光全局定位。

通过载入pcd文件，机器人获得整张地图的点云信息，为了实现定位，机器人在巡检的过程中，需通过激光雷达获得实时的车身周围点云数据，同时将该帧点云数据与整个地图点云进行匹配以及位姿估计，即使用步骤d中的激光里程计的算法部分，用来获取此时机器人在全局地图下的定位信息。

2)相机局部定位。

通过激光全局定位的获得的数据，机器人将被导航至某个巡检点附近，由于算法的精度以及硬件误差等原因，此时机器人可能无法完全重合至巡检点实现表盘的巡查以及拍摄工作。在巡检点附近，此时相机可拍摄到该巡检点对应的标定板，通过计算与标定板的位姿变换，并与上一步骤中存储的对应的位姿进行对比，通过相对位姿的差距，将机器人更精确导航至与巡检点重合，完成巡检工作。

相机局部定位算法步骤如下：

(2.1)首先通过opencv算法获取标定板的角点，并将其作为特征点；

(2.2)然后使用传统epnp算法来计算此时小车的位姿，其中：

(2.2.1)在所有点中，选取4个控制点(一般情况)，而其他点的坐标都可以用这4个点的坐标的权重和来表示，如式(10)所示：

上式中，αi1+αi2+αi3+αi4＝1，c为选取的控制点，α为控制系数，p为特征点。

(2.2.2)坐标系下的控制点和对应的系数求解完毕后，需要求出相机坐标系下对应的4个点的坐标。

推出线性方程组：m2n*12*x12*1＝0，其中，x是由4个控制点的3维坐标构成，所以为4*3＝12维的向量。下面推导m。

根据相机投影，我们得到如式(11)的方程：

上式中，u、v为像素平面坐标，f为相机内参，α为前面所述的控制系数，(x,y,z)为点的三维坐标。

把第3行带到前面两行中就可得到式(12)：

可知，一个点可以得到两个方程，n个点则可以得到2n个方程，所以m是一个2n*12的矩阵，整个方程形式如式(13)所示：

然后求解此方程组，可采用svd分解求解β值，如式(14)所示：

得到了β，就可以求出控制点的坐标，有了控制点的坐标，就有了所有点的相机坐标系下的坐标了，再次使用slam中常用的icp算法，即可求出位姿r和t，将其输出至导航节点，完成巡检机器人的巡检。