移动激光雷达的瞬时三维构像方法

张爱武，宫辉力，Jiaguo QI，胡少兴，肖 杨

1. 首都师范大学三维空间信息获取与应用教育重点实验室，北京 100048； 2. 密歇根州立大学全球变化和地球观测中心，美国密歇根州 东兰辛 48823； 3. 北京航空航天大学机械工程及自动化学院，北京 100191

本文提出并介绍作为基于计算机视觉的数字摄影测量智能化新方法之二的“移动激光雷达瞬时三维构像方法”。移动激光雷达在遥感、测绘、机器人、交通等众多行业和部门广泛应用，但是主要应用集中在室外[1-2]，比如车载激光雷达系统和航空激光雷达系统，很少在室内、地下应用。其原因在于常规的移动激光雷达必须依靠GPS/IMU(IMU惯性测量单元)组成的高精度POS支持，通过高精度的定向定位数据与激光点位数据事后融合，才能生成环境的三维点云图。然而，在高大建筑群和高大树林等环境内，常常接受不到GPS信号；在室内和地下GPS不可用，需要其他手段来解决。移动机器人常用的SLAM(同步定位与地图创建)方法[3]虽然可以某种程度解决上述问题，但是固化了应用环境[4-9]，大多算法不可移植。室内可用的算法室外、地下未必可用，地上可用的算法空中未必可用，结构化环境可用的算法非结构化环境未必可用。

针对上述问题，本文提出了一种移动激光雷达的瞬时三维构像方法，无须GPS辅助，不用昂贵的IMU，自主定位，瞬时构像，并将定位和构像误差约束在有限范围内，可在多种环境内应用。

1 瞬时三维构像原理

本文以本研究团队开发的手持激光雷达系统(图1)为例，说明本文提出的瞬时构像方法。手持激光雷达系统由3个2D激光雷达感器和1个低精度的惯性测量单元组成，如图1。顶部激光传感器水平扫描，左右2个激光传感器垂直扫描。3个激光雷达分别为Top-Lidar、Left-Lidar和Right-Lidar。Left-Lidar和Right-Lidar对称分布，由于单一激光雷达视场有限，左右对称的设计可以在扫描平面上形成全视场(full field of view，FOV)扫描。

图1 手持激光雷达系统结构像Fig.1 Structure model of hand-held LiDAR system

首先将手持激光雷达移动轨迹看成时间的函数，由激光点位、惯性测量等其他辅助信息计算出移动轨迹，给出每一时刻的位置和姿态。同时根据刚刚计算出的位置和姿态信息将激光脚点映射在一个统一的全局坐标系内，生成三维点云图。

图2描述了本文手持激光雷达系统的工作原理，导航模块负责处理惯性测量单元等传感器的输入数据，解算出系统载体的位姿，有可扩展性。构像模块将导航模块的位姿输出和激光雷达的扫描数据融合，实现系统的定位并同步构建三维点云图，结合优化平差算法去除累积误差。各传感器数据的姿态解算、坐标转换与数据融合在文献[10—12]中有论述，这里不再赘述。本文重点论述其中的3个主要环节：①定标，将视准轴角误差预先检校出来；②有效特征提取，使其适应不同应用环境，提高计算速度；③快速平差，消除累计误差。

图2 手持激光雷达系统原理模型Fig.2 Principle model of hand-held LiDAR system

1.1 视准轴角误差自定标方法

不同类型的传感器采集信息的数据模式不同，信息描述空间也不同，物理属性不同、局部坐标系也不同，多传感器获取的多模式数据需要融合，构建出同一空间描述的连续的三维环境点云图。在这个过程中，影响激光雷达三维构像质量的因素很多[13]，除了传感器本身的误差外，最重要的限制因素就是定位误差和系统的安装误差。安装误差包括视准轴角误差和偏心距误差。视准轴角误差是激光传感器坐标系与IMU本体坐标系的三个轴向不能精确对齐导致的。偏心距是激光传感器中心与惯性测量单元中心的偏差，可以在设计时给定，视准轴角误差只能通过试验的方法定标，且不可忽略不计。

视准轴角误差对点云坐标误差的影响可以用式(1)近似表示

εboresight[m]=Range[m]×θboresight

(1)

式中，θboresight为视准轴角误差(以弧度表示)；εboresight为点云坐标误差，Range为LiDAR到被测目标点的距离。

如图3所示，当在不同方位下扫描同一平面特征时，由于有视准轴角误差的存在，会导致LiDAR到被测目标点的距离变化。根据式(1)可知，两次扫描得到的点云将分别位于红色和绿色平面上，没有完全共面，而黑色平面才是视准轴角误差消除后点云所属的理想平面。

图3 基于平面特征的视准轴角误差校正原理Fig.3 Principle of bore-sight error calibration based on plane features

常规的移动激光雷达系统视准轴角误差定标方法有很多[14-15]，以视准轴角误差会导致多方位扫描同一平面特征时所获得的点云不共面为依据，文献[16]提出了多平面组合的点扫传感器视准轴角误差定标方法[19]。不需要额外控制点，以场景中构成平面几何特征的点云的原始观测值作为输入，建立针对平面特征点云的数学模型，然后线性化得到Gauss-Helmert平差模型，并推导出计算视准轴角误差的方程，以各传感器制造商所提供的数据精度建立随机模型，通过迭代计算视准轴误差。

常用的SLAM方法总是借助滤波的方法[3,9]推算出下一时刻的位姿，并抑制位置漂移，减少累积误差。其主要消除定位误差，对于其他误差的限制并不明显。

1.2 有效特征提取

本文将激光传感器获取的每一时刻的点位数据称为“单帧”三维点云。帧间三维点云需要关联与配准，不断预测与调整位置，增量构像。那么，关联与配准好不好、快不快、在不同环境的适应能力强不强与参与计算的特征有直接关系。本次研究团队提出一种直线特征提取算法[10-11]及扫描线配准方法[17]，但在解决实际复杂问题有时受限。文献[18—22]提出了不同的有效特征，虽然各有特点，但是不适合瞬时构像。文献[23]提出用学习方法构建语义几何特征，数据关联与配准的效果虽好，但很难获取大量的学习训练样本。本文提出用平整度构造有效特征的方法。

每条激光扫描线会形成互不相交的扫描平面。特征提取以扫描线为单位，提取每条扫描线中的边界特征点和平面特征点。用少数代表性最强的点描述一条扫描线包含的信息，而不是直接用全部点参加数据关联配准，提高处理速度。对于某点i，选择i两侧相同个数的点作为相邻点集S，计算i到S中每个点的向量和的模，再除以i点到激光传感器发射中心的距离，获得该点“平整度”c。取出该扫描线中c值最大的n个点作为边界点，取出c值最小(接近0)的n个点作为平面点。这两类特征点存在于绝大部分应用环境中。

(2)

式中，Pi为点i坐标。

此时的特征点仅仅是一个孤立点，如果直接用这些提取的特征点进行帧间配准，往往出现二义性。笔者采用点特征直方图描述子(point feature histogram，PFH)代替这孤立的特征点。以提取的特征点为中心点计算PFH。PFH是对中心点邻域范围内空间差异的一种量化，通过数理统计的方法获得一个用于描述中心点邻域几何信息的直方图[24]。图4表示的是中心点p0的PFH计算影响区域，p0用圆点标注，深色线表示以p0为球心r为半径的球，p0的所有邻域点(即与点p0的距离小于半径r的所有点)全部互相连接在一个网中。通过统计邻域内所有两点之间的四参数[24]，求得PFH描述子。PFH代表特征点邻域的几何信息。接下来的帧间配准用特征点演绎出的PFH参与计算。

图4 影响区域Fig.4 Sphere of influence

每一个特征点的PFH都有一个到上一帧数据的特征点PFH的特征距离d，如果能找到一个位姿变换尽量让所有特征距离都为0，那这个位姿变换就是载体在这两帧数据之间的运动。这个问题可以转化为一个最优化问题：求能使全部距离d最小的位姿变换Tk=[txtytzrxryrz]，tx、ty、tz为平移距离；rx、ry、rz为绕各轴旋转的角度。优化问题概括为f(Pk,Tk)=dk→0，Pk为k时刻特征点矩阵，dk为k时刻特征距离矩阵，Tk为k-1时刻到k时刻载体的位姿变换。根据计算的Tk更新点云位置，不断循环，增量构像。

1.3 快速优化平差

持续的累加数据必然会造成位置轨迹漂移，需要优化平差。以往的算法是最后进行全局优化平差，速度慢，达不到瞬时构像的目的。本文提出快速优化平差算法，换句话就是几帧一平差。在构像的同时进行平差，根据平差结果继续构像。

假设在移动过程中，连续扫描了n+1帧S0,S1,…,Sn。若利用ICP算法，从S0,S1,…,Sn依次配准，会产生累积误差。文献[7,25]提出了6自由度的全局一致性激光扫描数据匹配方法，进行全局位置优化，减小随着载体移动而产生的累计导航误差。设S0,S1,…,Sn对应的载体在导航坐标系内的位姿向量为V0,…,Vn；Vi=(xi,yi,zi,rxi,ryi,rzi),i=0,…,n。假如Sn和Sm有Cnm对应点对〈i1,j1〉,…,〈iCnm,jCnm〉，那么，根据ICP算法，位姿向量Vn和Vm约束条件是在导航坐标系内对应点对距离平方和最小。

l(Vn,Vm)=

(3)

(4)

式中，q1和q2仅表示序号。经过上述几何校正，重新调整了三维点位坐标，减少了坐标转换过程中的累积误差。

2 试验结果与分析

为了评估本文算法性能，用图2的装备在3种截然不同的环境中做构像试验。室内试验验证算法对累积误差的处理能力，爬楼梯试验验证算法的泛化能力，林地试验验证算法对非结构化环境的适应能力。

2.1 试验1：室内走廊构像试验

本文手持激光雷达系统不依靠GPS传感器，可以在室内应用。因此，本试验选择了室内走廊通道，手持激光雷达绕走廊行走一周，最后回到起点。由于实施了视准轴误差定标、有效特征提取、优化平差等措施，试验者可一边走一边瞬时构像，且回环处均无缝闭合(图5)，没有跳跃，没有不连续，在直角拐弯处，点云图保持直角。从图5的局部放大图中，获的三维点云清晰表述了窗户、门及屋内房顶的钢架。本文采用了快速优化平差，起点和终点处的三维点云完全闭合。

图5 室内走廊试验获取的三维激光点云(从屋内走向走廊)Fig.5 3D loop-closed point cloud from an experiment done in interior corridors

图6是室内走廊点云的俯视图，可以看到所有立面的水平投影均成直线。从放大图上看立面投影的点均匀分布，没有离散点。说明本文算法的定位和构像误差控制在了允许的范围内，没有造成大的跳跃和漂移。

图6 室内走廊试验获取的三维激光点云的俯视图Fig.6 Top view of the closed 3D point cloud

同样的走廊通道，采用文献[18]提出的拓展卡尔曼滤波的数据处理方法，数据处理时间长，达不到瞬时构像。

为了说明累计误差修正的重要性，图7显示了累计误差消除前后的对比结果。在增量构像中能有效地抵制定位所带来的发散误差，却不能直接消除定位累计误差。图7(a)中深色圈部分显示了由于累计误差在回环处出现了点云不连续，图7(b)显示经过快速优化平差后的结果。可见优化平差减小累计误差是移动三维构像一个重要方面。

图7 累计误差消除前后的对比图Fig.7 Comparison diagram between with accumulative errors and without cumulative errors

2.2 试验2：爬楼梯构像试验

爬楼梯实际上是对移动三维构像非常大的挑战，特别是本文这种无GPS，没有昂贵高精度IMU支持的移动激光雷达三维构像系统。爬楼梯的过程属于垂直构像，而不是水平构像。常规的移动激光雷达，如车载激光雷达和机载激光雷达的构像过程是水平构像。垂直构像对定位准确性和抗干扰能力提出更高的要求。本文方法属于自主定位与瞬时构像，对外界的适应能力更强。图8是爬楼梯获得的连续三维点云图，深色线表示试验者手持装备行走的轨迹，包括上楼梯和下楼梯，点云连续不间断、无跳跃。图8中包含一小(a)一大(b)两个楼梯，小楼梯是三级楼梯，试验者手持装备从小楼梯走上平台，再从大楼梯走下平台。这个过程是一个不断攀升再下降的过程，在攀升下降过程中本文算法给出定位与构像结果。实验结果的点云图中所有回环处没有跳跃，点云连续不间断。试验证明了本文构像方法对环境的自适应能力和抗干扰能力。

图8 爬楼梯获得的连续三维点云图Fig.8 3D laser point clouds collected while climbing stairs

2.3 试验3：林荫小路构像试验

试验者手持激光雷达装备自由穿梭在林荫小路中，并且过桥，一边走一边获取环境的三维数据，环境中包含众多非结构化特征，如图9。从结果中可以清晰的看到小桥，河边的垂柳和石梯小路，在回环处没有出现跳跃。常规的依靠GPS/IMU定位构像的移动激光雷达在林地中往往因GPS信号失锁而得不到理想的构像数据，但是本文方法却得到了闭合的环境三维点云。林荫小路构像试验进一步证明，本文提出的方法不但适合结构化的环境，也同样适合非结构化环境，可以在狭小空间内构像。

上述试验均是一边走一边构像，是一种瞬时构像模式。这种无需GPS和昂贵IMU的自定位构像模式，操作灵活，非常适合应急救援等需要快速响应的环境。

图9 林荫小路三维点云图Fig.9 3D point clouds of the tree-lined lane

但是，本文方法只与本文作者前期提出的方法做了简单的比较，还需要进一步与其他方法比较。此外，本文试验测试范围相对较小，接下来将在更大范围内测试。

3 结 论

室内、室外、地下等环境灵活自主定位同步构像是移动激光雷达研究的主要难点问题。本文提出的移动激光雷达瞬时三维构像方法，作为基于机器视觉的数字摄影测量智能化新方法，不依赖GPS和高精度IMU，自主定位瞬时构像，其理论研究和试验结果表明：

(1) 提出的移动激光雷达瞬时三维构像方法，诠释了移动激光雷达自定位与瞬时构像的原理。

(2) 本文方法不需要GPS和高精度IMU支持，在室内、林地等环境，甚至爬楼梯均可一边走一边构像，可用在多种环境。

(3) 爬坡构像属于垂直构像，超出了平时意义上的水平构像的范畴。在爬坡试验中，激光点云没有跳跃、各回环处闭合好。说明本文方法自适应能力强。

常规的依靠GPS/IMU的激光雷达构像方法不能在室内构像，也很少能在林地内穿梭构像。本文方法自主定位瞬时构像，不受GPS限制，在室内、室外、结构化环境和非结构化环境都可构像。

文献[26]也研发了手持激光雷达移动三维构像装置，但是构像借助一个弹簧装置，构像原理和方法较为复杂。文献[27]开发了室内推车型的移动激光雷达三维构像装置，针对室内结构化环境三维构像，获取室内环境三维点云，与本文算法比较泛化性稍差些。文献[28]提出了一种背包式的室内激光雷达移动构像装置和方法，在形式上与本文方法相似，但装置较为复杂，且仅针对室内结构化环境。目前现有方法针对性强，大多固化了应用环境，而本文方法可水平构像，也可垂直构像；可在结构化环境中构像，也可在非结构化环境中构像；可在开阔环境中应用，也可在狭小空间内应用。

参考文献：

[1] 杨必胜, 董震, 魏征, 等. 从车载激光扫描数据中提取复杂建筑物立面的方法[J]. 测绘学报, 2013, 42(3): 411-417.

YANG Bisheng, DONG Zhen, WEI Zheng, et al. Extracting Complex Building Facades from Mobile Laser Scanning Data[J]. Acta Geodaetica et Cartographica Sinica, 2013, 42(3): 411-417.

[2] 浦石, 李京伟, 郭四清. 融合语义特征与GPS位置的地面激光点云拼接方法[J]. 测绘学报, 2014, 43(5): 545-550. DOI: 10.13485/j.cnki.11-2089.2014.0075.

PU Shi, LI Jingwei, GUO Siqing. Registration of Terrestrial Laser Point Clouds by Fusing Semantic Features and GPS Positions[J]. Acta Geodaetica et Cartographica Sinica, 2014, 43(5): 545-550. DOI: 10.13485/j.cnki.11-2089.2014.0075.

[3] DURRANT-WHYTE H, BAILEY T. Simultaneous Localization and Mapping: Part I[J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99-110.

[4] PATHAK K, BIRK A, VASKEVICIUS N, et al. Online Three-dimensional SLAM by Registration of Large Planar Surface Segments and Closed-form Pose-graph Relaxation[J]. Journal of Field Robotics, 2010, 27(1): 52-84.

[5] SHALAL N, LOW T, MCCARTHY C, et al. Orchard Mapping and Mobile Robot Localisation Using on-board Camera and Laser Scanner Data Fusion-Part A: Tree detection[J]. Computers and Electronics in Agriculture, 2015, 119: 254-266.

[6] FRESE U. An O(logn) Algorithm for Simultaneous Localization and Mapping of Mobile Robots in Indoor Environments[D]. Nürnber Germany: University of Erlangen-Nürnber,

2004.

[7] LU Feng, MILIOS E. Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans[J]. Journal of Intelligent and Robotic Systems, 1997, 18(3): 249-275.

[8] KOENIG N, HOWARD A. Design and Use Paradigms for Gazebo, An Open-source Multi-robot Simulator[C]∥Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems. Sendai, Japan: IEEE, 2004: 2149-2154.

[9] NÜCHTER A, LINGEMANN K, HERTZBERG J. 6D SLAM-3D Mapping Outdoor Environments[J]. Journal of Field Robotics, 2007, 24(8-9): 699-722.

[10] 胡少兴, 陈春朋, 张爱武. 同步定位及地图创建算法在车载移动测绘系统中的应用[J]. 中国激光, 2012, 39(11): 1108012.

HU Shaoxing, CHEN Chunpeng, ZHANG Aiwu. Application of SLAM in Vehicle-borne Mobile Mapping System[J]. Chinese Journal of Lasers, 2012, 39(11): 1108012.

[11] HU Shaoxing, CHEN Chunpeng, ZHANG Aiwu, et al. A Small and Lightweight Autonomous Laser Mapping System without GPS[J]. Journal of Field Robotics, 2013, 30(5): 784-802.

[12] 张爱武, 孙卫东, 李风亭. 大规模三维场景几何信号基本处理方法[J]. 计算机辅助设计与图形学学报, 2005, 17(7): 1486-1491.

ZHANG Aiwu, SUN Weidong, LI Fengting. Basic Processing Methods of 3D Geometrical Signals from Large-scale Scenes[J]. Journal of Computer-Aided Design & Computer Graphics, 2005, 17(7): 1486-1491.

[13] GLENNIE C, KUSEVIC K, MRSTIK P. Performance Analysis of A Kinematic Terrestrial LiDAR Scanning System[C]∥Proceedings of MAPPS/ASPRS Fall Conference. San Antonio, Texas: [s.n.], 2006.

[14] 王致华, 张爱武, 王书民, 等. 基于重叠航带的机载激光雷达系统检校[J]. 中国激光, 2014, 41(2): 0214003.

WANG Zhihua, ZHANG Aiwu, WANG Shumin, et al. Airborne Radar Calibration System Based on the Overlap Strip[J]. Chinese Journal of Lasers, 2014, 41(2): 0214003.

[15] SKALOUD J, LICHTI D. Rigorous Approach to Bore-sight Self-calibration in Airborne Laser Scanning[J]. ISPRS Journal of Photogrammetry and Remote Sensing, 2006, 61(1): 47-59.

[16] 孟宪刚, 胡少兴, 张爱武, 等. 一种地面移动激光成像系统的视准轴误差自标定方法研究[J]. 中国激光, 2014, 41(11): 1108008.

MENG Xiangang, HU Shaoxing, ZHANG Aiwu, et al. A Self-calibration Method for Bore-sight Error of Ground-based Mobile Mapping System[J]. Chinese Journal of Lasers, 2014, 41(11): 1108008.

[17] 王都虎. 基于LiDAR/IMU的同步定位与地图创建方法研究[D]. 北京: 北京航空航天大学, 2016.

WANG Duhu. Research on Simultaneous Localization and Mapping Based on LiDAR/IMU[D]. Beijing: Beihang University, 2016.

[18] TOMBARI F, SALTI S, DI STEFANO L. Unique Shape Context for 3D Data Description[C]∥3DOR’10 Proceedings of the ACM Workshop on 3D Object Retrieval. New York, NY: ACM, 2010: 57-62.

[19] TOMBARI F, SALTI S and DI STEFANO L. Unique Signatures of Histograms for Local Surface Description[M]∥DANIILIDIS K, MARAGOS P, PARAGIOS N. Computer Vision-ECCV 2010. Berlin, Heidelberg: Springer, 2010: 356-369.

[20] STEDER B, RUSU R B, KONOLIGE K, et al. Point Feature Extraction on 3D Range Scans Taking into Account Object Boundaries[C]∥IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE, 2011.

[21] GUO Yulan, SOHEL F, BENNAMOUN M, et al. Rotational Projection Statistics for 3D Local Surface Description and Object Recognition[J]. International Journal of Computer Vision, 2013, 105(1): 63-86.

[22] RUSU R B, BRADSKI G, THIBAUX R, et al. Fast 3D Recognition and Pose Using the Viewpoint Feature Histogram[C]∥IEEE/RSJ International Conference on Intelligent Robots and Systems. Taipei, Taiwan: IEEE, 2010: 2156-2162.

[23] ZENG A, SONG Shuran, NIEβNER M, et al. 3DMatch: Learning Local Geometric Descriptors from RGB-D Reconstructions[C]∥IEEE Conference on Computer Vision and Pattern Recognition. Honolulu, Hawaii: IEEE, 2017: 199-208.

[24] 张爱武, 李文宁, 段乙好, 等. 结合点特征直方图的点云分类方法[J]. 计算机辅助设计与图形学学报, 2016, 28(5): 795-801.

ZHANG Aiwu, LI Wenning, DUAN Yihao, et al. Point Cloud Classification Based on Point Feature Histogram[J]. Journal of Computer-Aided Design & Computer Graphics, 2016, 28(5): 795-801.

[25] PFAFF P, TRIEBEL R, STACHNISS C, et al. Towards Mapping of Cities[C]∥IEEE International Conference on Robotics and Automation. Roma, Italy: IEEE, 2007: 4807-4813.

[26] BOSSE M, ZLOT R, FLICK P. Zebedee: Design of A Spring-mounted 3D Range Sensor with Application to Mobile Mapping[J]. IEEE Transactions on Robotics, 2012, 28(5): 1104-1119.

[27] TRIMBLE. Trimble Indoor Mobile Mapping Solution[EB/OL]. Trimble Navigation Limited, 2010. [2017-05-05].http:∥www.trimble.com/Indoor-Mobile-Mapping-Solution/pdf/Trimble_

Indoor_Mapping_brochure_0610.pdf.

[28] LIU T, CARLBERG M, CHEN G, et al. Indoor Localization and Visualization Using a Human-Operated Backpack System[C]∥International Conference on Indoor Positioning and Indoor Navigation. Zurich, Switzerland: IEEE, 2010.