隧道内无人机位姿估计与控制

陈艳超，田 伟，楼 勇，陈俊柱，沈润杰*

（1.华东天荒坪抽水蓄能有限责任公司，浙江安吉 313302；2.同济大学电子与信息工程学院，上海 201804）

大型的基础设施建设，如水电站高压输水管道、隧道等，随着使用年限的增加，难免会出现局部破损现象。为了保证生产和作业的安全，必须定期进行检修。目前，对上述工业场景进行检修，大多采用传统的人工方法，需要搭建大量的脚手架，耗费大量的人力和物力资源，检修效率低下，并且存在较大的安全隐患。随着无人机技术的发展，越来越多的工业环境逐步借助无人机进行检修和勘察，提高了检修效率，降低了安全隐患。实现无人机导航与控制的关键技术在于对无人机进行位姿估计。在室外GPS 信号较强的场景，可以依靠GPS 对无人机进行定位。遗憾的是，对于上述工业场景，其相对封闭，存在GPS 信号不强甚至被屏蔽的现象。因此，依靠GPS 进行定位的方法在上述场景将无法使用。近年来，随着人工智能、机器视觉和激光雷达技术的发展，这类技术在室内地面移动机器人中的应用较为成熟。因此，激光雷达和机器视觉等人工智能方法是研究封闭空间内无人机定位的可行方案。然而，由于无人机不同于地面移动机器人，其具有更多的运动自由度，运动的不确定性增大，给无人机的室内定位提出更大的挑战。近年来，国内外研究者对此展开了大量的研究。

1 研究现状

国内外的研究者对于封闭空间内无人机定位与导航的研究，主要有两大方法，一种是基于激光雷达，另一种是基于视觉。其中，基于视觉的方法又可分为机载视觉和外部视觉。此外，为了进一步提高无人机在无GPS 环境下定位的准确度，有研究者采用基于激光雷达和视觉相结合的多传感器融合的方法。

天津大学古训等人利用二维激光雷达，通过对激光雷达返回的数据进行分析匹配，结合惯性导航单元计算出无人机的相对位置姿态信息［1］。长春理工大学汉鑫团队基于地标的单目相机，实现无人机在仓库环境中对自身的精确定位［2］。北京航空航天大学吴琦团队采用光流和地标融合的方法，给出了无人机在室内的精确位置和速度估计［3］。但是，这种基于地标的方法在类似隧道这样的场景很难实现。

也有国外研究者对此展开相关研究。麻省理工学院Abraham Bachrach团队采用扫描匹配算法和视觉里程测量法实现无人机相对位置的估计，利用扩展卡尔曼滤波（EKF）将这些相对位置估计与惯性测量数据相融合，估计出微型飞行器的完整状态［5］。卡内基梅隆大学张继等人采用三维激光雷达、摄像头和IMU 数据构建环境地图，实现了在高度动态运动以及黑暗、无纹理、无结构环境中的位姿估计，在9. 3 公里的导航范围内相对位置漂移率仅为0. 22%［6］。宾夕法尼亚大学Tolga Özaslan 等人设计了搭载IMU、四个摄像头和两个激光雷达的水坝管道巡检无人机，通过机载照明进行补光，实现弱光环境下沿管道轴线方向的位置估计［7］。

综合国内外对无人机在封闭空间内的位姿估计研究状况，目前单靠激光雷达进行无人机位姿估计的研究仍然较少，大多数需要融合视觉传感器或者进行外部标识，而视觉传感器对于室内环境的特征点有较高的要求，对于特征点不明显的场景则很难适用。对于水电站高压输水管道和隧道等光线较差的场景，需要进行补光来提高图像的识别度。并且由于这类工业场景的特殊性，特征不明显且难以进行外部标识，种种约束条件给无人机在上述封闭空间中的定位与导航提出了巨大的挑战。本文的研究目的是设计一种能够适应隧道场景的无人机位姿估计与控制方案。

2 无人机位姿估计算法

由国内外研究可知，目前对于封闭空间中无人机的定位主要采用的是激光SLAM 和视觉SLAM 的方法。由于隧道中存在光线较弱的情况，以视觉主导的方法很难有较好的效果。因此本方案采用激光SLAM的方法。而对于激光雷达定位的方法，从激光雷达的角度划分，有采用单线二维激光雷达，也有采用多线三维激光雷达。由于多旋翼无人机的载重有限，考虑到多线激光雷达重量较重的问题，本方案采用两个正交放置的单线激光雷达对无人机进行位姿估计。水平放置的激光雷达采用Hector SLAM［8］算法对水平方向的位姿进行估计，竖直放置的激光雷达对垂直方向上的环境进行感知，对无人机的高度进行定位，从而实现无人机在隧道中的三维位姿估计。

假设在一个连续的地图中，有一点的坐标为Pm，如图1 所示。 用M(Pm) 表示该坐标的占有值，则M(Pm)沿x轴和y轴的方向梯度为

图1 栅格坐标图Fig.1 Grid diagram

其中，M(Pm)可由最邻近点P00，P01，P10，P11估算获得。

其结果为

进一步可计算得到M(Pm)的方向梯度为

Hector SLAM 算法采用扫描匹配的方法，将激光扫描数据与当前已建立的地图进行匹配，通过高斯牛顿方法寻找当前的扫描点在地图中的最优位姿。其计算过程如下：

式（5）中，ξ= (px，py，ψ)T为刚体旋转变换矩阵，即无人机在世界坐标系下的位置坐标，Si(ξ)为激光扫描点si=(si，x，si，y)T在世界坐标系下的坐标。Si(ξ)的计算公式如下：

在给定ξ的初值后，需要估计Δξ，可通过以下方法对测量误差进行优化，即

对式（7）进行一阶泰勒展开并且求解关于Δξ的偏导数，可得到位姿变化量为

式（8）中，

至此，完成了无人机水平位置以及偏航角的估计。在此基础上，通过竖直放置的激光雷达对垂直方向上的环境进行感知，对无人机的高度进行定位，从而实现了无人机在隧道中的三维位姿估计。

3 无人机飞行位置控制策略

在完成了无人机在隧道中的位姿估计后，为了使无人机能够沿隧道的中轴线飞行，需要估算出隧道的中轴线。以水平方向为例，以水平方向的激光雷达中心Os为坐标原点建立平面直角坐标系XsOsYs，如图2所示。其中两侧的竖直直线代表隧道内壁，以Os为起点的射线为激光线束，d为Os到隧道内壁的距离，Ɵ为线束与Xs轴正方向的夹角。

图2 水平激光雷达扫描图Fig.2 Horizontal lidar scan

单线激光雷达获取的点云数据是以极坐标(dsi，θsi)的形式表示的，通过该数据可以将点云数据转换为激光雷达坐标系下的直角坐标。转换公式如下：

另外，由于无人机在飞行的过程中可能存在一定的滚转角φ，水平面和垂直面的激光雷达扫描面不是相对于世界坐标系下的水平面和竖直面，其水平面的扫描面如图3 所示，其中d为获取的距离数据，d′为实际的距离数据。

图3 机体非水平时的激光雷达扫描图Fig.3 Lidar scan of the body when it is not horizontal

激光扫描面与水平面存在一定的夹角φ。为了得到水平面的实际距离，需要对获取的点云数据进行修正，采用融合IMU 的方法。通过IMU 获取机体的滚转角φ，根据下式进行修正，即

修正后，在激光雷达坐标系下激光点云的直角坐标为

无人机飞行过程中存在俯仰角和偏航角时的修正方法与上述方法类似。

在得到点云的空间直角坐标后，隧道内壁就已确定，接着通过隧道内壁的坐标确定中轴线上点的坐标。由于激光雷达在隧道中的扫描具有对称性，以水平面为例，在误差允许的范围内，可以找到关于中轴线对称的两个点(xs1，ys1)和(xs2，ys2)，接着利用以下公式求出两个点的中点，即为中轴线上的点。

垂直方向的中轴线解算方法与水平方向类似。将水平方向的中轴线和垂直方向的中轴线融合，即可得到隧道的中轴线。至此，就得到了在激光雷达坐标系下的中轴线坐标。最后，通过激光雷达坐标系与世界坐标系之间的变换关系可求得在世界坐标系下的中轴线坐标。

将解算的隧道中轴线作为无人机飞行的期望位置，同时结合位姿估计算法估计无人机当前位置，实现无人机在隧道中位置的闭环控制。

4 无人机仿真实验

为了验证无人机位姿估计算法和位置控制策略的有效性和准确度，本文进行了仿真实验。该仿真实验基于ROS 和Gazebo 仿真环境。该仿真环境可以模拟机器人的运动情况，也可以导入相关的传感器插件，在机器人上安置不同的传感器，如摄像头、激光雷达和IMU 等，通过这些传感器可以获取机器人当前所在仿真环境中的感知数据。通过SolidWorks绘制简易的隧道场景，并通过Urdf插件生成Gazebo仿真环境。仿真环境如图4 所示。该场景为田字形隧道，隧道的最大长度为20 m，最大宽度为2 m。

图4 仿真实验场景Fig.4 Simulation experiment scene

实验时，无人机通过位姿估计算法估计无人机当前的位姿，同时根据设定的目标运行轨迹和隧道中轴线解算方法获得无人机的期望位置，无人机沿隧道中轴线自主飞行。实验中，水平环境的建图效果和无人机的飞行轨迹如图5所示。可以看到，水平环境的建图效果和真实场景相符，无人机的飞行轨迹与预设的航迹一致。

图5 无人机飞行轨迹与建图效果Fig.5 Uav’s flight trajectory and building diagram effect

为了验证位姿估计算法的准确度，在实验过程中，采用两种方法对无人机进行位姿估计。一种是本文所提出的位姿估计算法，另一种是通过在仿真环境中无人机所搭载的GPS 进行无人机的位姿估计。通过记录两种方法估计的无人机位姿数据，绘制无人机的飞行轨迹曲线如图6所示。从图6中可以看出，通过本文所提出的位姿估计算法估计出来的无人机飞行轨迹与通过GPS获得的飞行轨迹基本重合。

图6 GPS与本文算法估计的位置对比图Fig.6 A comparison of positions estimated by GPS and the algorithm

为了定量地描述位姿估计算法与GPS 估计的误差，进一步求出同一时刻用位姿估计算法估计的位置与用GPS估计的位置的距离。实验位置误差如图7所示。其误差主要受激光雷达的扫描频率和无人机飞行状况的影响。由位置误差图可见，在当前的实验条件下，通过位姿估计算法估计出来的无人机位置误差较小，准确度较高，满足无人机定位需求。

图7 位置误差图Fig.7 Position error diagram

5 结论

本文针对隧道内无人机无法通过GPS 进行定位的问题，设计了一套无人机位姿估计与控制方案。通过在Gazebo 仿真环境中开展相关的实验，验证了该方案的有效性和准确度，为隧道无人机巡检提出了可行的定位和控制方案。