(1.山东科技大学电气与自动化工程学院，山东 青岛 266590；2.山东科技大学电气信息系，山东 济南 250031;3.中国移动通信集团江苏有限公司淮安分公司，江苏 淮安 223021)

近年来随着电网的快速发展，变电站数量激增和变电站维护人员数量不足的矛盾日益凸显，同时变电站工作是一项难度较大且危险的工作，因此无人巡检技术和设备研发十分必要。目前无人巡检大多采用循迹或者激光即时定位与地图构建(simultaneous localization and mapping,SLAM)技术，循迹主要是利用计算机视觉技术，依靠路径上的条带标记引导无人车进行自主行驶，循迹的导航方式虽然可靠性、抗干扰性比较好，但是需要预先铺设磁条，施工量大，成本较高，而且无法应对突发事件进行应急处理，无法随时更改路线。激光SLAM技术主要是利用激光器发射激光束通过三角测距原理或飞行时间测距法(time of flight，TOF)来测量周围物体与自身的距离，从而获得高精度的距离信息，结合SLAM技术实现同时定位与建图。目前激光SLAM主要应用在移动机器人定位和导航、无人驾驶和测绘等领域[1]。国外的TU Munich公司的LSD-SLAM和Zaragoza大学的ORB-SLAM功能都十分完善和稳定，国内SLAM起步较晚，目前有思岚科技、百度等多个公司研发。

现有的激光SLAM主要有基于滤波和基于图优化两种技术。基于卡尔曼滤波的SLAM主要有EKF扩展卡尔曼滤波(extended Kalman filter，EKF)、UKF无迹卡尔曼滤波(unscented Kalman filter，UKF)等。扩展卡尔曼滤波算法结构简单，具有一定的精度，因此得到了较好的应用，但也存在一些不足:例如受环境、设备等因素影响，测量误差较大,对于有突变的信号数据，收敛速度较慢[2-3]。其代表技术是Hector SLAM，使用一个更新频率高、测量噪声小的激光雷达，优点是不需要里程计的信息就可以完成实时地图创建和定位[4]，但是对于雷达的帧率要求很高。基于粒子滤波(particle filter，PF)的SLAM能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于解决SLAM问题。其中目前应同最广泛的是Gmapping, Gmapping是一种2D SLAM建图方法，主要基于粒子滤波算法，在长廊和特定的场合中应用效果较好，缺点是严重依赖里程计，没有回环检测[5]。这些方案中大多只能利用2D或者3D激光雷达，实现对应的2D、3D地图构建，各自优缺点无法互相补足[6]。

基于图优化框架SLAM代表技术是Google开发的实时室内项目Cartographer，采用基于Ceres非线性优化的方法,其基本思想是将机器人不同时刻的位姿抽象为点，机器人在不同位置上观测所产生的约束被抽象为点之间的边[7]。采用了主流的SLAM框架，即特征提取、闭环检测、后端优化，由一定数量的LaserScan组成一个Submap子图，一系列的Submap子图构成全局地图，Cartographer SLAM最大优点是具有回环检测功能，可以同时满足二维和三维的建图需要[8]。通过一系列激光SLAM的对比，本研究选择Cartographer SLAM方案，在Ubuntu环境下，基于ROS进行仿真建图，采用双2D激光雷达互相垂直的方式实现三维建图优化，利用水平激光雷达采集数据，垂直激光雷达对水平数据进行弥补，得到了良好的三维数据结果。

1 Cartographer的原理及其优化

1.1 Cartographer SLAM的原理

SLAM也称为并发建图与定位[9](concurrent mapping and localization，CML)，即时定位与地图构建。利用局部连续扫面数据建立子图，并不断地与全局进行扫面匹配。Cartographer的建图理论本质是基于图优化，将机器人在不同时刻的位姿抽象化成一个点，再将机器人在不同点观测到的约束抽象成点与点之间的边。随着机器人的运动，这些点和边就形成地图，这个过程会随着时间的累积产生误差，后面需要再进行全局优化[10-11]。从二维建图的角度，由每一个扫描到的Laser scan数据得到一个位姿关系ε=(εx,εy,εθ)，对于子图的构造，其实是反复扫描和子图坐标框架迭代的过程。随着时间推进，位姿关系会从激光扫描框架转化成子图框架Tε，Tε和ε的关系如下：

(1)

在子图的构建过程中引入给定分辨率的离散网格概念，这里对于所有网格的映射值会有被阻挡的概率。假设是一个r×r的网格，当前进行扫描时，对于新观察到的命中点将赋予新的概率p。随着迭代过程的进行，误差会不断地累积。利用求解非线性最小二乘法(公式(2))，通过闭环的方式消除误差，可以很容易地添加残差E来考虑额外的数据，残差的计算可以通过公式(3)和公式(4)得到，寻找每个位姿的最大可能性，并通过双三插值法将整个子图进行平滑解算。

(2)

(3)

(4)

f(i+u,j+v)=(1-u)(1-v)f(i,j)+u(1-v)f(i+1,j)+(1-u)vf(i,j+1)+uvf(i+1,j+1),

(5)

(6)

1.2 SLAM建图方案的优化

在实际实验中，数据的精确性和完整性是最关键的，其次是通过方案优化来降低成本，利用二维激光雷达满足实验所需的三维建图要求。但是传统的单个2D采用舵机控制俯仰角，虽然能在一定程度上满足三维建图要求，但是实时性和完整性大大降低。因此在实验过程中，针对上述问题，设计并实现了采用两台互相垂直的二维激光雷达实现三维建图，其中水平激光雷达采集数据，垂直激光雷达对水平数据进行弥补。

Cartographer的算法中三维建图的要求是点云数据源，然而二维激光雷达本身发出的是Scan数据，因此首先要解决的是数据源的转化问题。这里利用Transform转换的方法，利用四元数旋转矩阵解决刚体三维向量的坐标转换时所带来的冗余性和奇异性，假设四元数q=q0+q1i+q2j+q3k,四个参数分别代表三维坐标和旋转角度，则对应的旋转矩阵为公式(7)。其中i,j,k为四元数的虚数部分，如果简化虚数部分为向量v，则四元数可以简化为公式(8)，为了解决相对坐标的旋转问题，假设坐标为w=(wx,wy,wz)，以向量v旋转θ角度下得到新的坐标w，根据公式(7)的计算方式，则可以得到其解析式为公式(9)，那么在系统中的各个框架的相对旋转便可以欧拉角的方式利用公式(10)获得。发送变化信息，将每个单独的激光线转化成设定的框架，通过收集激光雷达的数据，能够有效解决设备的移动和出现俯仰角偏差时所带来数据不匹配[14-15]。

(7)

q=(v,q0)，

(8)

(9)

(10)

将Laser Geometry数据包和统一的Sensor Msg数据格式通过算法将二维激光的Laserscan数据转化成Pointcloud2数据，使得激光雷达在驱动节点启动时，自动完成数据类型的转化，满足3D建图的需要。然而两个二维激光雷达的初步建图效果并不是十分理想，如图1所示，对比系统的转换关系时，发现垂直传感器和水平传感器的坐标系并不能在数据上进行融合，存在着完全分裂的现象。因此，为了得到更加实际可靠的点云数据和地图数据，通过对传感器进行精确的标定，确定各个传感器和基坐标的父子关系，并通过旋转垂直传感器的坐标来配合水平坐标系，实现对水平坐标系的数据弥补。并通过QT Reader对点云数据进行了优化，得到更加合理和符合实际的3D点云数据图，如图2所示。

图1 未融合的三维效果图

图2 优化的二维激光雷达3D点云数据

2 SLAM建图仿真实验

仿真主要利用在Ubuntu 16.04的环境下的ROSKinetic和Rviz。实际的建图过程中，选用了北阳公司的二维激光雷达Hokuyo-30lx-ew和Velodyne的Vlp-16的三维激光雷达，二维激光雷达Hokuyo-30lx-ew具有高精度、高分辨率及宽视场设计的特点给自主导航机器人提供了良好的环境识别能力，且重量低、功耗低。Vlp-16的三维激光雷达也同样具有高帧率、高精度的优点[16]。

图3 二维和三维地图比较

在图3(b)中可以发现三维激光雷达的建图明显要优于二维激光雷达，不仅参考数据更真实，相对于二维激光雷达建图(图3(a))，在面对复杂地形和具有垂直高度的直立机器人尤其适用，在三维空间里能够有效规避障碍物的同时进行合理的路径规划，而且三维地图显然更加具有实际意义。

由图4可以看出，在机器人上下坡时可以发现，在有室内或者有多个高度较高的障碍物时，无论是障碍物的高度、方位，还是障碍物的大小，都可以利用栅格地图进行判断。但是从数据的完整性上，单个三维激光雷达在近距离上出现了一定数据丢失，而且没有顶层数据。在复杂的环境里，诸如室内、变电站等，缺少了大量可以参考的数据，因而数据是不完整的。

另外，三维激光雷达的造价昂贵，制作工艺复杂，体积大，功耗也大，在图像采集时工作频率受到限制，扫描速度慢，难以用来处理变电站工作的无人巡检导航的任务，因此提出了一种通过双2D激光雷达实现三维建图的方式解决上述缺陷。

3 基于ROS的建图系统的原型机搭建

图4 机器人室内上下坡的仿真结果

图5 算法流程框图

机器人操作系统(robot operating system,ROS)是一套开源操作系统，可用于机器人编程，把原来分散部分耦合在一起，并且为这些部分提供通信架构。ROS提供了一系列程序库和工具，其功能包含硬件抽象描述、底层驱动程序管理、共用功能的执行、程序间的消息传递、程序发行包管理等[17-18]。ROS采用分布式框架，通过点对点设计让机器人的进程可以分别运行，便于模块化的修改和定制，提高了系统的容错能力，可以有效地对机器人、无人机、无人驾驶等应用进行仿真和设计。对于不同的传感器采用统一的消息传递模式和格式，非常适合算法的开发[19-20]。

图5为系统的建图算法框图，算法采用了双循环的并行结构，分别读取数据节点和地图算法节点。启动的角度互相独立，条件判断数据互相依赖，算法结构在开发和修改上更加友好，只要格式统一便可以将服务于相同主题的所有节点同时调用。当某一节点出现问题时可以通过逻辑判断快速地查找到出错的语句，最终生成的地图数据在算法端是完整的，可以保障地图的正确性。在算法结构的基础上，搭建了复杂环境地图构建的原型系统如图6所示。

从图7中可以看出，利用原型系统得到的优化后变电站点云数据相比较二维SLAM地图更加全面，优化后的点云数据补充了顶层数据和机器人上方障碍物数据，复杂环境下的点云数据补充了更多障碍物的尺寸和细节。对比前文图3(b)的俯视图可以清楚地发现，通过二维激光雷达进行三维建图的方案是可行的，并且补充了地图的数据和细节，例如路牙边线、树木伸出的枝丫等障碍物。从经济的角度看，优化后的方案也更加节约成本。从系统的冗余和负荷角度来说，优化后的方案降低了系统的冗余，两台二维激光雷达的负荷也小于一台三维激光雷达。综上，本系统的优化方案具有可实施性。

图6 复杂环境地图构建原型系统

图7 变电站复杂环境地图的点云数据

4 结论

对几种主流激光SLAM算法进行对比和分析，选用效果理想的Cartographer算法进行激光雷达的建图，基于TX2建立了一套完整的通讯结构，并搭建了实验原型机。利用初步搭建的原型系统对真实环境进行了模拟实验和实际建图实验，分别得到了二维和三维的建图实验结果以及区域内复杂环境下的实际三维建图的点云数据，为变电站巡检机器人系统降低了成本并提高了建图精度。系统实现了二维激光雷达的三维建图，降低了系统的整体成本，提高了整体的精度，并展现了高度的可移植性和适应性，后续将进行底盘和导航功能优化工作，继续提升基于本系统的自主移动机器人的实用性和功能性。