吕 科1，施泽南1，李一鹏2
(1. 中国科学院大学工程科学学院 北京 石景山区 100049；2. 清华大学信息科学技术学院 北京 海淀区 100084)
【摘要】同步定位与环境建模(SLAM)是实现无人机自主飞行和智能导航的关键技术。该文提出了适用于微型无人机的视觉定位与环境建模方法，针对RGB-D传感器使用point-plane ICP点云匹配算法实现视觉自主定位，利用并行计算加速以满足无人机控制的实时性要求；采用TSDF算法融合多帧观测的点云数据，实现了无人机对未知目标环境区域的模型重建；将视觉SLAM系统与无人机载IMU传感器融合，进一步提升了自主定位和建模精度。实际搭建了微型无人机视觉与环境建模验证系统，室内环境下可以达到0.092 m的定位偏差和60 Hz的更新速率，满足了无人机控制的精度和实时性要求，验证了该方法的有效性。
关 键 词：环境建模; 微型无人机; 多传感器; 同步定位与环境建模; 视觉定位
近年来，无人机以其低成本、良好的机动性和环境适应能力强等特点，越来越多地被应用于航拍、测绘、电力巡检[1]、灾害检测、地质勘探等特种行业。世界主要国家都非常重视无人机的发展[2]，随着技术的演进，人们不满足于无人机依赖人和外部设备的指引来完成任务，如何使其具有更高的智能、更强的自主性乃至多机组成机群协同完成任务[3-4]成为了研究的重点。
自主定位和环境感知能力是移动机器人实现自主和智能的关键，相关技术研究具有重要的理论和应用价值。与地面机器人相比，无人机的运动范围更广阔，6自由度运动模式需要构建三维的环境模型来完成飞行任务，此类问题被称为6自由度的同步定位与环境建模问题(SLAM)[5-6]。应用于无人机系统的SLAM算法，在鲁棒性、精确性等诸多方面面临着重大挑战。特别对于微型无人机而言，更需要考虑负重、体积和飞行时间，对于自主定位和环境模型构建的计算复杂性和实时性提出了更高的要求。国内外目前典型的SLAM研究工作多停留在二维定位和建图上，典型的工作如文献[7]提出的利用下视相机完成地面二维地图绘制和定位，文献[8]提出的利用激光扫描仪实现的平面环境的建图和定位，文献[9]提出的使用视觉里程计的机器人定位等。
无人机对定位要求较高，容错率低，一旦失去定位会面临失控甚至坠毁的风险，因此应用于无人机的SLAM算法需要充分考虑系统的稳定性和可靠性。相关工作中通常采用多种传感器数据融合的方法以提升整体的稳定性和抗干扰能力[10-11]。
本文以RGB-D相机为主要视觉传感器，首先使用point-plane ICP[12]点云匹配算法实现无人机的视觉自主定位，通过并行计算加速以满足实时性要求；然后在自主定位的基础上，通过对所观测的环境三维点云进行融合，实现了无人机对未知目标环境区域的模型重建；最后将视觉SLAM系统和无人机飞行控制系统整合，提出了视觉定位及无人机传感器融合以提高定位和导航精度的方法，并在实际搭建的四旋翼无人机系统上验证了该算法的有效性和精确性。
1.1 传感器选择
本文选取RGB-D相机作为主要的视觉传感器。RGB-D相机可直接获取环境深度信息，从而计算出环境表面的三维点云坐标，相比于一般的摄像头，RGB-D相机对环境纹理不敏感，且对光照条件依赖性较小，因此适用作微型无人机的定位传感器，具有较强的适应性和可靠性。
目前商用消费级RGB-D相机，如Microsoft kinect和华硕Xtion,，重量约为400～500 g，摄像头分辨率约为640×480，深度测量精度达到厘米级，而功耗仅为5 W，适合用作无人机机载传感器。
1.2 Poing-plane ICP定位算法
在定位系统中，使用point-plane ICP算法[12]来匹配不同位置所观测的点云数据，从而实现定位。Point-plane ICP是一个直接匹配算法，即不对图像进行任何的稀疏化处理，如提取特征点、提取边缘等方法，而是直接用图像的全部信息进行计算。因充分利用了全部的信息，在精度、可靠性、和鲁棒性上相比基于特征点和特征信息的匹配算法具有相当优势。
记相机第k帧观测的深度图像为Zk:Ω→R+，其中Ω为像素集合，使用相机小孔成像模型，已知相机内参矩阵为：
可以计算当前观测点云Vk:Ω→R3和法向量Nk:?→R3分别为：
k时刻相机的6自由度位姿可用齐次坐标变换表示为：
式中，Rk∈ SO(3)，tk∈R3。该位姿变换对应的基准坐标系为世界坐标系，以符号g表示，点云坐标也以齐次坐标表示。k时刻所观测的点云和法向量投影至世界坐标系下为：
不失一般性，假设Tk?1已知，point-plane ICP算法认为Tk的最佳估计值将k?1帧点云映射到第k帧之后，使得对应点之间的误差会达到最小。这里使用L2范数作为误差函数，则将计算k时刻位姿Tk的问题转化成最小化误差的问题，有：
该问题为非线性优化问题，采用局部线性化的高斯?牛顿法即可求解。高斯?牛顿法是迭代算法，每次迭代的增量只需计算目标函数在当前位置的一阶微分，由于避免了求解目标函数的二阶微分，因此每次迭代均为线性计算，易于快速计算。
注意到该优化问题的凸性只在局部满足，当初值距离最优解较远时，可能收敛至错误的局部极小值。为了进一步提升算法的精度和可靠性，对每一帧观测的深度图均构建3层的图像金字塔结构，每一层的分辨率为上一层的1/2，以此完成一个由粗糙至精细的迭代计算。
由于需要对所有的点云进行计算，因此pointplane ICP算法虽然能够提供较好的精度，但是难以满足匀速速度的要求。考虑到迭代过程中每一步计算中各像素的数值是逐像素计算，该算法实际上具有高度并行化的特点，如果利用GPU进行加速，可以大大提高运算速度。本文采用CUDA对GPU进行编程，将该算法利用GPU并行计算实现，并测试其计算时间(测试平台：Nvidia-Geforce GTX 980)如表1所示。可以看出，经CUDA加速后，该算法足够让定位系统以较高的帧率(＞60 Hz)运行，并且相邻两帧点云匹配准确度高。图1展示了两帧点云匹配的结果。
表1 point-plane ICP算法运算时间测试结果
分辨率 迭代次数 单次迭代/ms 总耗时/ms160×120 4 0.275 8 1.103 2320×240 5 0.443 6 2.218 0640×480 10 1.068 2 10.681 7
图1 point-plane ICP算法匹配的两帧点云
1.3 点云融合与场景重建
本文使用TSDF算法实现了多帧观测点云数据的去噪、融合，并最终实现了对无人机所观测的目标场景较为精细的三维重建。SDF(signed distance function)模型由文献[13]提出。该模型将空间分为密集的栅格，每个栅格的数值代表当前格点到物体表面的距离，负值代表当前格点在物体内部(不可见)，正值代表当前格点为空，而所有的过零点连接在一起即为所观测的场景的可见表面的轮廓。在工程实际中，通常将SDF数值归一化至(?1,1)，即为TSDF。基于TSDF的场景点云融合算法已在文献[14-16]中得到成功应用。图2所示为TSDF模型表达下的某物体模型。
本文使用TSDF模型进行点云融合时，使用高斯分布作为观测的噪声模型，因此在TSDF模型中的每一个栅格内均记录了SDF和该SDF的高斯分布权重(F(p),W(p))，p∈R3。第k帧观测后，p点处的TSDF值使用式(6)和式(7)进行融合更新：
图2 TSDF模型示意图
TSDF模型由于采用了高斯噪声模型下的最优估计，因此对三维点云的噪声有很强的抑制作用，能够很好地消除因无人机抖动等因素带来的测量误差，具有非常好的鲁棒性，并且能够重建出比较精细的场景模型。
图3a和图3b分别展示了未经TSDF模型融合处理的点云和经TSDF融合后的点云。可以看出，融合了多帧数据的TSDF模型比原始的场景模型更加精确和细致。
图3 TSDF点云融合效果
在视觉定位和环境建模算法的基础上，用四旋翼无人机搭建了完整的系统，实现了无人机在未知环境下的自主定位、导航和场景模型重建，验证了本文算法的有效性。另外，通过将视觉定位数据与无人机飞控所搭载的多种传感器，如加速度计、陀螺仪等数据进行融合，提升了整体测量和控制的可靠性。
2.1 视觉定位系统与机载传感器的融合
为了保证无人机的稳定飞行，无人机上通常搭载了高速、高精度的陀螺仪、用于姿态估计和自稳的加速度计，以及用于飞行时的高度增稳气压计、超声等传感器。在室内飞行时不考虑外部定位设备(如GPS)，自主定位中数据融合对提升无人机飞行控制的精度及抗干扰能力非常重要[17-18]。其中常用的滤波算法有粒子滤波[8,19]和扩展卡尔曼滤波(EKF)，其中粒子滤波虽然精度较高，但是运算量偏大，不适合对实时性要求很高的无人机控制，因此本文研究工作中利用EKF中的最优估计原理将视觉定位数据与其他传感器的数据进行融合，实现了以视觉定位为主，其他传感器为辅的自主定位系统，增强了整体定位的精度、稳定性和可靠性。同时该定位系统由于充分利用了陀螺仪、加速度计的数据(250～ 1 000 Hz)，填补了相机两个采样周期之间的空白，使得整体定位和控制效果更加流畅。
假设对于无人机速度、位置的测量和估计噪声均满足高斯分布，令
设无人机利用陀螺仪和加速度计做出的姿态估计矩阵为R，加速度计在t时刻观测的机身坐标系下的加速度矢量为a (t)=[ax(t),ay(t),az(t )]T，则速度估计模型和位置估计模型分别为：
式中，
2.2 系统整体架构
图4描述了结合视觉定位和场景重建的无人机系统算法架构。在该系统中，图像数据所提供的定位信息结合无人机载的多种传感器数据，经过融合、滤波，得到对无人机自身位置和姿态较为精确的估计。通过对环境的持续观察，该系统可以构建较为精确的环境模型，并以此模型为依据，在无人机飞行控制器的协同下，实现无人机的定位和导航。
图4 系统软件架构框图
因为三维点云匹配算法需要依靠GPU并行计算加速来达到满足无人机实时控制的要求，因此本文系统对于计算平台的运算性能要求较高，完整的无人机系统采用机载处理器与地面站协同计算的方案来实现。无人机机载飞行控制器选用Pixhawk飞控，在其开源控制算法的基础上整合视觉定位及导航系统。无人机机载处理器通过无线局域网和地面站实现数据通信，将观测数据发送至地面站，由地面站进行计算后再发送回无人机，供无人机控制系统进行位置和轨迹控制，如图5所示。
图5 无人机和地面站协同计算流程图
实验时使用微型四旋翼无人机作为实验平台，搭载Microsoft kinect作为RGB-D相机，选用典型的室内环境进行自主定位和环境建模测试。将无人机放置在完全未知，并且没有外部辅助定位的室内环境下，由系统自动完成无人机的自主定位和环境建模，在完全无人工干预的情况下实现对目标区域的探索，并同时对环境进行建模。测试中，选用不同的室内环境，反复进行多次实验，以验证其有效性和可靠性。图6为无人机对目标环境进行自主探索，并重建出环境模型的过程。
图6 无人机自主定位和环境建模过程
实际飞行结果显示，该系统在不借助GPS等外部定位设备的帮助下，可以实现微型无人机的自主定位，并且可以自动完成对目标区域的自主探索。在飞行测试中，对室内区域的10次探索均顺利完成，显示了该定位系统融合了无人机机载多传感器的数据后，对实际飞行中偶尔遇到的气流、振动等因素导致的干扰具有一定抵抗能力，同时其对环境模型建立的稠密程度也能满足微型无人机的导航需要。
为了进一步验证定位算法和所建立模型的准确性，使用公用测试数据集FR2-Desk检验定位算法的偏差，并对比相似基于RGB-D相机的定位算法[14]，结果如表2所示。可以看出，point-plane ICP算法由于充分利用了点云信息，精度更高，整体路径偏差更小。
表2 定位偏差测试结果
算法 平均路径偏差/m poing-plane ICP 0.092 RGB-D SLAM 0.132 FOVIS 0.212
图7为了无人机对目标空间探索后的重建的环境模型，由图可以看出，重建的模型精度较高，人体表面及衣物轮廓均清晰可见，整体模型比较精确和细致。
图7 无人机对目标区域探索后建立的模型
实验结果表明，本文提出的搭载RGB-D相机作为主要传感器，利用point-plane ICP算法实现的无人机视觉定位和建模系统，在定位速度、精度上均能满足微型无人机飞行控制的需求。由于采用了合适的场景建模点云融合算法，对环境模型重建的精度较好，并且在实际飞行测试中，视觉定位和环境建模系统与无人机飞行控制器中的其他传感器融合，整体控制的可靠性和稳定性较好，能够实现微型无人机对室内位置环境的自主定位、探索并最终重建出目标环境的模型。本文研究中用于无人机定位和自主导航的主要传感器为RGB-D相机，受RGB-D相机传感器的距离限制，适合在较小范围的场景中应用。考虑到工程和实际的结合，未来的研究工作可在本文提出的多传感器融合的框架基础上，通过多种传感器结合和改进算法的方式，拓展观测距离，以适应不同场景的需求，提升其实用性。
参 考 文 献：
[1] 王淼, 杜毅, 张忠瑞. 无人机辅助巡视及绝缘子缺陷图像识别研究[J]. 电子测量与仪器学报, 2015, 29(12): 1862-1869.
WANG Miao, DU Yi, ZHANG Zhong-rui. Study on power transmission lines inspection using unmanned aerial vehicle and image recognition of insulator defect[J]. Journal of Electronic Measurement and Instrument, 2015, 29(12): 1862-1869.
[2] United States Department of Defense. Unmanned systems integrated roadmap[R]. [S.l.]: [s.n.], 2014: 22-47.
[3] 姚敏, 王绪芝, 赵敏. 多旋翼无人机群协同作战任务分配方法研究[J]. 电子科技大学学报, 2013, 42(5): 723-727.
YAO Min, WANG Xu-zhi, ZHAO Min. Coop erative combat task assignment optimization design for unmanned aerial vehicles cluster[J]. Journal of University of Electronic Science and Technology of China, 2013, 42(5): 723-727.
[4] 赵敏, 姚敏. 无人机群变航迹多任务综合规划方法研究[J]. 电子科技大学学报, 2010, 39(4): 560-563.
ZHAO Min, YAO Min. Study on changeable path planning and multi-task assignment optimization design forunmanned aerial vehicles cluster[J]. Journal of University of Electronic Science and Technology of China, 2010, 39(4): 560-563.
[5] DURRANT-WHYTE H, BAILEY T. Simultaneous localization and mapping: Part I[J]. IEEE Robotics and Automation Magazine, 2006, 13(3): 99-108.
[6] DURRANT-WHYTE H, BAILEY T. Simultaneous localization and mapping: Part II[J]. IEEE Robotics and Automation Magazine, 2006, 13(3): 108-117.
[7] ACHTELIK M W, LYNEN S, WEISS S, et al. Visual-inertial SLAM for a small helicopter in large outdoor environments[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vilamoura: IEEE, 2012: 2651-2652.
[8] GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with rao-blackwellized particle filters[J]. IEEE Transactions on Robotics, 2007, 23(1): 34-46.
[9] 任沁源, 李平. 结合视觉里程计的微小型空中机器人SLAM研究[J]. 仪器仪表学报, 2013, 34(2): 475-480.
REN Qin-yuan, LI Ping. The SLAM combing with visual odometry for small-scale aerial robots[J]. Chinese Journal of Scientific Instrument, 2013, 34(2): 475-480.
[10] 夏凌楠, 张波, 王营冠, 等. 基于惯性传感器和视觉里程计的机器人定位[J]. 仪器仪表学报, 2013, 34(1): 166-172.
XIA Ling-nan, ZHANG Bo, WANG Ying-guan, et al. Robot localization algorithm based on inertial sensor and video odometry[J]. Chinese Journal of Scientific Instrument, 2013, 34(1): 166-172.
[11] LYNEN S, ACHTELIK M W, WEISS S, et al. A robust and modular multi-sensor fusion approach applied to mav navigation[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Tokyo: IEEE, 2013: 3923-3929.
[12] BESL P, MCKAY N. A method for registration of 3D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), 1992, 14(2): 239-256.
[13] CURLESS B, LEVOY M. A volumetric method for building complex models from range images[J]. In ACM Transactions on Graphics (SIGGRAPH), 1996, 3: 303-312.
[14] WHELAN T, JOHANNSSON H, KAESS M, et al. Robust real-time visual odometry for dense RGB-D mapping[C]// 2013 IEEE International Conference on Robotics and Automation (ICRA). [S.l.]: IEEE, 2013: 5724-5731.
[15] NEWCOMBE R A, IZADI S, HILLIGES O, et al. KinectFusion: Real-time dense surface mapping and tracking[C]//2011 10th IEEE International Symposium on Mixed and Augmented Reality (ISMAR). [S.l.]: IEEE, 2011: 127-136.
[16] WHELAN T, KAESS M, FALLON M, et al. Kintinuous: spatially extended Kinect fusion[R/OL]. (2012-07-05) [2015-11-15]. https://www.researchgate.net/publication/ 279843674_Kintinuous_Spatially_Extended_KinectFusion.
[17] KIM J H, SALAH S. 6DoF SLAM aided GNSS/INS navigation in GNSS denied and unknown environments[J]. Journal of Global Positioning Systems 4, 2005, 1: 120-128.
[18] STRASDAT H, MONTIEL J M M, DAVISON A J. Real-time monocular SLAM: Why filter?[C]//IEEE International Conference on Robotics and Automation (ICRA). [S.l.]: IEEE, 2010, 58(8): 2657-2664.
[19] 刘志花, 李淑芬, 李宝安. 基于粒子滤波的无人机航迹预测方法研究[J]. 电子测量与仪器学报, 2010, 24(7): 643-649.
LIU Zhi-hua, LI Shu-feng, LI Bao-an. Research of UAV track forecast based on particle filter[J]. Journal of Electronic Measurement and Instrument, 2010, 24(7): 643-649.
编 辑 漆 蓉
Visual Localization and Environment Mapping for Micro Aerial Vehicles
Lü Ke1, SHI Ze-nan1, and LI Yi-peng2
(1. School of Engineering Science, University of Chinese Academy of Sciences Shijingshan Beijing 100049; 2. School of Information Science and Technology, Tsinghua University Haidian Beijing 100084)
Abstract：Simultaneous localization and mapping (SLAM) is a key technology for unmanned aerial vehicle (UAV) autonomous flight and navigation. This paper presents a visual localization and environment mapping method for micro UAV. In the method, the point-plane ICP point cloud matching algorithm is used to achieve visual localization based on RGB-D sensor, the parallel computing acceleration is applied to meet the real-time requirements of UAV control, then the truncated signed distance function (TSDF) algorithm is used for multi-frame point cloud data fusion to achieve environment model reconstruction for an unknown environment, and at last the fusion of visual SLAM system and UAV IMU sensor data is applied to enhance the precision of autonomous localization and model reconstruction. Finally, we set up a micro UAV system with full visual SLAM algorithm and test the whole system in a typical indoor environment. The result shows a 0.092 m positioning deviation and 60 Hz update rate, capable for real-time UAV controlling accuracy requirement, thus verifying the effectiveness of the method.
Key words：environment mapping; micro aerial vehicle; multi-sensor; simultaneous localization and mapping (SLAM); visual odometry
中图分类号：TP242.6
文献标志码： A
doi：10.3969/j.issn.1001-0548.2017.03.011
收稿日期：2016 ? 03 ? 09；
修回日期：2016 ? 06 ? 30
基金项目：国家自然科学基金面上项目(61271268)
作者简介：吕科(1971 ? )，男，博士，教授，主要从事遥感图像处理、医学影像处理和计算图形学等方面的研究.
联系客服
