复杂环境下微小飞行器惯性/激光雷达Robust-SLAM方法

石 鹏，赖际舟，吕 品，邹冰倩，付相可

(南京航空航天大学，南京 211106)

0 引言

微小型飞行器凭借其体积小、高机动性和低成本的特点，已经被广泛用于实际生活中，如巡检、搜救、探测、航拍和地图构建等任务[1-3]。随着对飞行任务需求的不断增加，对飞行器的自主导航性能提出了越来越高的要求。无人机在室外常用全球定位系统(Global Positioning System，GPS)和惯性进行组合导航；在室内环境中，GPS不可用，常用导航手段有视觉导航[4-6]和激光雷达导航[7-8]。相较于视觉传感器，激光雷达是一种主动式感知传感器，不依赖于外界光照，更加适用于复杂环境中微小型飞行器的自主导航。

激光雷达导航通常通过同步定位与构图技术(Simultaneous Localization and Mapping，SLAM)实现，激光雷达SLAM方法经过多年的发展，已经被广泛应用于机器人领域。激光雷达分为二维激光雷达与三维激光雷达，受体积、载重、成本等因素的限制，微小型飞行器通常采用二维激光雷达进行SLAM。

目前激光雷达SLAM中常用扫描匹配的方法估计载体位姿，所谓扫描匹配，就是将当前环境激光扫描点与参考扫描数据进行配准[9]。根据参考扫描数据的不同，扫描匹配的方法主要分为两类：1)相邻两帧扫描点匹配；2)当前帧扫描点与地图进行匹配。第一种方法中，通过求解相邻两帧时间内载体的相对位姿变化，然后通过累加的方式计算载体的位姿[10]，这种方法会快速累积位姿算的误差。第二种扫描匹配方法中，将全局地图作为参考扫描数据，利用当前帧扫描点与世界坐标系下的地图进行匹配，以此来限制位姿解算误差的累积[11,13]，这也是目前主流的扫描匹配方法。

就地面机器人而言，通过二维激光雷达扫描匹配估计位姿的方法已经相当成熟，但是对于具有六自由度的无人机，这些扫描匹配方法的使用存在着较大的局限性。由于二维雷达只能扫描一个平面的环境，而飞行器存在着高度方向的运动以及横滚角和俯仰角的姿态变化，导致二维激光雷达所扫描的二维环境会发生突变，且匹配中的参考扫描数据与当前扫描点不在同一平面，致使匹配存在较大的误差。针对该问题，目前主要有以下三种应对方案：

1)假设无人机维持在同一高度运动，不存在较大的姿态运动，周围障碍物大多为竖直墙面，仍然使用传统的二维扫描匹配方法估计载体位姿[12]。该方法将三维环境简化成二维环境处理，局限性大。

2)无人机搭载带有旋转装置的激光雷达，通过旋转激光雷达获取三维扫描点数据，然后利用三维扫描匹配的方法估计载体位姿[13-14]。该方法要求无人机具备足够的载重能力和能源，并且算法复杂，对机载计算机的运算能力和存储提出了苛刻的要求，实际应用时的效果不佳。

3)采用2.5维环境假设，即假设环境中障碍物表面为连续的竖直面；根据飞行器的姿态将二维扫描点数据投影于水平面，然后使用二维扫描匹配的方法估计位姿[16-18]。该方法对环境的构成做出了2.5D的假设，仍然不能解决高程方向激光雷达感知环境发生突变对定位造成的影响。

针对于微小型飞行器，上述方法都不能较好地解决其精准可靠导航的问题。本文提出了一种利用惯性辅助的激光雷达Robust-SLAM方法，考虑到微小型飞行器的载重、能耗等限制因素，该方法仍然使用二维激光雷达扫描匹配方法估计载体位姿，其主要贡献可总结为以下两点：

1)改进了激光雷达SLAM方法的框架，在传统框架的基础上，引入了感知环境突变检测的环节。

2)相对于传统的惯性与激光雷达SLAM组合的方法，该方法充分利用惯性信息辅助整个SLAM过程，加强了组合程度。

该方法仅假设环境中障碍物表面主要为竖直面，适用于存在多个高低不同竖直平面的环境，相对于2.5D环境假设适用范围更广，在高程方向存在感知环境突变的环境中，使用本文提出的方法能够稳定可靠地对微小型飞行器进行实时精准的导航。

1 惯性辅助的激光雷达Robust-SLAM

图1所示为传统激光雷达SLAM方法框架，主要包含3个部分：扫描匹配、参考扫描数据更新和构图。本文提出的SLAM方法框图如图2所示，相对于传统方法，该方法的主要改进归纳为以下几点：

1)在利用IMU信息预测载体位姿的基础上进行扫描匹配。

2)引入突变检测环节，根据检测结果决定滤波器和参考扫描数据的更新策略。

3)提出了利用IMU状态预测信息检测激光雷达感知环境是否发生突变的方法。

4)利用扫描匹配的位姿估计结果与预测的状态融合之后的结果更新参考扫描数据和构图。

图1 传统激光雷达SLAM方法Fig.1 Typical LIDAR SLAM method

图2 本文提出的SLAM方法Fig.2 The proposed SLAM method

1.1 状态预测

(1)

(2)

1.2 扫描匹配方法

目前,二维扫描匹配方法发展成熟，其中迭代最近点(Iterative Closest Point，ICP)算法不依赖于环境结构化特征，适用范围广。本文基于ICP进行扫描匹配，ICP具体原理参照文献[19]，为抑制误差累积，扫描匹配过程中以二维全局地图作为参考扫描数据。

考虑到飞行器的姿态运动和高机动性，文中在ICP的基础上，对扫描匹配进行两点改进：1)点云数据预处理中补偿飞行器姿态；2)利用IMU预测的位姿作为ICP迭代初值。

1.2.1 点云数据姿态补偿

记h系为激光雷达测量理想坐标系，其XOY面为水平面，相对于世界系的偏航角与机体系相同。坐标系关系如图3所示。

图3 坐标系示意图Fig.3 The general view of the coordinate system

(3)

1.2.2 初值预测

(4)

利用IMU进行预测迭代初值，具备以下两点优势：1)加速了ICP匹配迭代过程；2)在一定程度上避免了迭代过程中得到局部最优解。

1.3 突变检测方法

在实际应用中，由于飞行器飞行过程中的高度变化，搭载于飞行器上的二维激光雷达感知的二维环境结构会发生突变现象。该突变会造成扫描匹配估计的位姿产生较大误差，本文提出了一种利用IMU信息检测该突变现象的方法，构建突变检测函数fd，若fd≥1则认为发生突变，fd的表达式为：

(5)

式中，εxk、εyk和εψk根据式(6)计算：

(6)

(7)

(8)

式(5)中，Pξk为ICP中当前帧激光点数据在参考扫描数据中成功找到对应的匹配点的比例值[19]，Eratio为设置的Pξk对应的阈值。Eεx、Eεy和Eεψ分别为设置的位姿变化误差εxk、εyk和εψk的阈值，根据使用IMU预测状态在扫描匹配迭代周期时间内的误差累积情况设定。

1.4 滤波器更新

本文中通过扩展卡尔曼滤波器(Extended Kalman Filter,EKF)校正利用IMU信息预测得到的状态量，根据突变检测环节的结果,确定量测模型选取策略：

Zk=HkXk+μk

(9)

(10)

式中，μk为量测噪声，偏航角ψ和四元数之间的关系如下：

(11)

(12)

(13)

1.5 参考扫描数据更新

扫描匹配估计位姿的原理为：利用当前扫描数据和参考扫描数据中的重叠部分进行配准。因此，若激光雷达感知环境发生突变，当前扫描到的环境结构与参考扫描数据重叠部分很少，甚至没有重叠部分，从而导致位姿估计产生较大误差。

在本文的扫描匹配算法中，选择二维全局地图作为参考扫描数据，根据上述的位姿误差产生原因，参考扫描数据更新过程，由突变检测结果选择如下不同的策略：

1)若激光雷达感知环境没有发生突变，则更新策略为扩增二维全局地图，插入之前地图中不存在的激光点数据。

2)若检测到感知环境发生突变，则重新初始化二维全局地图。首先清空二维地图中的点数据，再根据滤波器更新的状态量和当前时刻的扫描点数据重新构建二维全局地图。

1.6 构图

本文提出的SLAM方法中构建的地图，包括：飞行器所在高度的二维全局地图和三维全局地图。其中二维全局地图作为扫描匹配的参考数据，更新过程如1.5节所述。本文中构建的三维地图为三维栅格地图，每个立方体元素的大小为5cm×5cm×5cm，使用三维构图工具octomap完成地图构建，octomap中以八叉树结构存储地图，高效利用存储空间，建立的地图直观且分辨率调节方便。新的扫描点在世界坐标系下的位置坐标通过式(14)计算：

(14)

2 飞行实验与分析

2.1 实验设计

图4所示为实验的飞行平台实物图，以DJI M100飞行器开发平台为基础建立实验平台，其飞行控制系统模块为DJI N1飞控，M100飞行平台上具备IMU、气压计和磁罗盘模块；机载计算机为DJI Manifold；使用的激光雷达为单线HOKUYO UTM-30LX，仅具备2D扫描能力，最大探测距离为30m，扫描范围为270°，更新频率为40Hz。图4中,红色标记1传感器为UWB移动端标签，其二维定位精度为分米级，用于评估本文提出的导航算法的水平定位精度。

图4 飞行器平台Fig.4 Aerial vehicle platform

实验环境为室内车库，其场景简化示意图如图5所示，其中黄色标记1、2对应的蓝色模块为纸箱，标记3棕色模块为车库中的通风管道，灰色模块为车库中墙壁。

图5 实验环境示意图Fig.5 The general view of test environment

为验证该算法在激光雷达感知环境发生突变时的导航精度，飞行实验中飞行器将从图5中标记1处的纸箱中央的地面起飞，飞行场景如图6所示，三维飞行轨迹如图12中黑色曲线所示。

图6 飞行场景Fig.6 The flight scene in the parking garage

飞行过程中由于激光束扫描到的障碍物发生突变，存在以下3处感知环境发生突变的区域：飞行器飞过实验场景中标记1、2 对应的纸箱上端，以及3对应的通风管道上下端的高度区域。

实验环境中搭建了分米级别定位精度的UWB定位系统，用于评估本文提出的导航算法的性能，该系统中移动端发送其与各节点的距离信息至PC端，在PC端解算移动端的位置信息，其示意图如图7所示。

图7 UWB定位系统Fig.7 The UWB localization system

2.2 实验结果与分析

本文算法主要针对三维环境中传统二维激光雷达SLAM算法存在的定位问题进行改进，因此主要分析实验结果中水平二维的导航结果。为了更好地分析导航性能，将实验结果中本文算法与无载波通信技术(Ultra Wideband，UWB)和一种传统的SLAM方法(mrpt-SLAM)进行了对比。

图8～图10中黑色实线为本文算法结果，蓝色虚线为mrpt-SLAM结果，红色点画线为UWB定位结果。图8所示为飞行实验的二维飞行轨迹，图9所示为偏航角结果；以分米级别精度的UWB定位信息作为室内基准轨迹，传统方法与本文提出方法的定位误差如图10所示，本文算法和mrpt-SLAM的定位均方根误差(m)分别为[σx,σy]=[0.2,0.2]和[σx,σy]=[4.8,5.0]。可见本文算法能够实现较为精准的导航；而mrpt-SLAM导航的误差较大。

图8 飞行轨迹Fig.8 The flight trajectory

图9 偏航角Fig.9 The yaw angle

图10 位置误差Fig.10 The position estimation errors

从图中可以看出，飞行器飞过纸箱上端时，激光雷达激光束从标记1处纸箱突然扫描到标记2处纸箱和墙面，感知环境发生突变，此时mrpt-SLAM定位误差突然变大，其偏航角结果也发生了突变；与此同时，在之后的飞行过程中，由于mrpt-SLAM并未对其参考扫描数据(二维全局地图)[19]进行修正，其导航误差逐渐累积。本文算法经过突变检测环节及相应的参考扫描数据更新，解决了环境突变造成的影响，从而实现了精准鲁棒的位姿估计。

飞行过程中，本文算法突变检测函数结果如图11所示，一共检测到5个时间段发生突变(数字标记处)，分别对应图12中的5个数字标记处的飞行区域；对应飞行环境如图5所示，按时间顺序5个发生突变的飞行过程：1)上升经过纸箱上端；2)上升经过标记2纸箱上端；3)下降经过标记2纸箱上端的对应高度；4)上升经过标记2纸箱上端；5)上升和下降经过标记3通风管道下端高度。结果表明，利用本文提出的突变检测算法能够有效检测出激光雷达感知发生突变的情况。

图11 突变检测函数值Fig.11 The value of the abrupt change detection function

图12所示为本文算法的三维构图，立方体元素的颜色代表不同高度，红色对应最大高度，图中黑线为飞行器的三维飞行轨迹，数字标记处为激光雷达感知环境发生突变的区域。对应图5和图6的飞行环境图可见，本算法构建的地图与实际情况一致，能够较好地反映出飞行环境的三维结构。

综上所述，通过使用本文算法能够解决激光雷达感知环境突变对导航造成的影响，实现微小型飞行器在三维环境中的精准可靠导航。

图12 三维构图结果Fig.12 The 3D map

3 结论

本文针对使用激光雷达SLAM的微小型飞行器进行自主导航的问题展开了研究，提出了一种基于惯性辅助的激光雷达Robust-SLAM方法，解决了在高程方向激光雷达感知环境突变时位姿估计误差大的问题。主要内容为以下几点：

1)在SLAM中引入激光雷达感知环境突变检测，并结合IMU信息和匹配中点配对率，构建突变检测函数。

2)根据检测结果实施不同的滤波器更新策略和SLAM中参考扫描数据更新策略。

3)利用滤波后的位姿信息进行三维构图。

在室内车库进行了飞行实验，将本文算法的导航结果与分米级别精度的UWB定位系统结果进行对比，表明本文提出的方法能够有效实现微小型飞行器在三维空间中的精准可靠导航。在后续的工作中，将会针对激光雷达SLAM中回环检测部分展开研究，进一步提高SLAM导航的鲁棒性和精度。