单线激光雷达与GNSS/INS的空间重构

王 锐，常 锴，符国浩，王世峰，徐熙平*，王作斌

(1.长春理工大学 光电工程学院，吉林 长春 130022；2.长春理工大学 电子信息工程学院，吉林 长春 130022；3.长春理工大学 国家纳米操纵与制造国际联合研究中心，吉林 长春 130022)

1 引 言

随着无人驾驶车辆[1]、机器视觉[2]的快速发展，三维空间重构技术[3]在诸多领域都发挥着不可或缺的作用。该技术广泛应用于文物保护、建筑设计、道路检测识别等场景，同时在游戏开发、临床医学等研究上也起到辅助作用[4]。三维空间重构技术通过数据获取、预处理、点云配准和融合、生成空间表面等过程，把真实场景描述成符合计算机逻辑表达的数学模型。目前，国内外已经究出多种三维重构的方法：基于双目立体视觉成像[5]，它属于被动三维测量，该方法匹配计算代价较高，并且计算无纹理表面深度信息的准确度差；结构光测量[6]，结构光测量技术通过投影结构光可控地增加被测物体的纹理信息，测量精度高，但是投影仪标定与校正非线性误差的过程较为复杂；应用较多的方法是利用CCD摄像机[7]获取致密的图像灰度信息和激光雷达提供的深度信息得到空间三维重构结果，但在两种不同类型的数字信息的配准过程中易引入较大误差，标定精度受限；利用多线激光雷达[8]进行三维空间的重构是目前应用最为广泛的方法之一，但该方法的主要缺陷是采集数据量过大，点云数据配准的实时性有延时，对运算处理器配置要求较高以及价格较昂贵等。

本文提出了一种结合单线激光雷达和惯性导航模块GNSS/INS系统来进行空间三维重构的研究。利用单线激光雷达反馈的数据确定二维平面数据点的距离角度信息，并结合GNSS/INS测量航向角的大小，确定扫描点经过的空间三维数字信息，将在不同帧获取的数据分别融合、配准，最终得到该传感系统的绝对空间扫描结果。该方法在显著提高运算效率的同时保证了点云配准的精度。

图1 UTM-30LX雷达的工作扫描区域Fig.1 Working scanning area of UTM-30LX lidar

2 系统设计

单线激光雷达利用飞行时间法(Time of Flight，TOF)[9]实现距离的测量，并且结合测角技术直接获取目标点的角度信息。实验中选用的激光雷达传感器是由日本HOKUYO公司生产的UTM-30LX单线激光雷达。该款激光雷达拥有30 m的有效测量距离、270°水平测量角度以及25 ms的扫描时间，能够较为精确地获取短距离、广角度的平面深度数据，并以此进行实验仿真。图1是Hokuyo激光雷达的工作参数示意图。为了更加便捷地实现本文所提出的三维空间重构方法，本文将人为地给单线激光雷达提供不同的航向角来增加扫描维度，将不同帧的二维面融合配准得到重构的三维空间。同时选用GNSS/INS组合导航系统来确定实验中任意提供的不同方向的航向角。该组合导航系统集成了三个MEMS陀螺仪和三个MEMS加速度计，GNSS卫星导航系统等。系统在静态、动态和冲击振动状态下均有很好的响应，可以精确地获取传感器的姿态和加速度信息。UTM-30LX和GNSS/INS的详细参数如表1所示。

表1UTM-30LX和GNSS/INS的工作参数

Tab.1 Working parameters of UTM-30LX and GNSS/INS

性能参数UTM-30LXGNSS/INS工作电压12VDC±10%4.2～17 V频率40 Hz800 Hz(max)工作温度-10 to 50 ℃-40 to 85 ℃测量精度0.1 mm±0.5°

3 空间三维重建过程

当传感器获得空间扫描信息数据时，依据其反馈数据的上位机的Linux时间戳，两传感器的初始采集数据起始时间一致，并结合在MATLAB实验平台上的插值函数找出在时间戳上匹配的激光雷达扫描距离数据信息和GNSS/INS的航向角以及对应的三个方向的加速度信息，利用四元数[10-11]算法整合在各个不同位置(采集数据点)的扫描点云信息，生成不同帧的图像。分别对GNSS/INS三个方向的角加速度值施以频域积分，实现点云空间位置的粗配准，近似得到点云空间平移矩阵，接着使用在数据更新前提下的迭代最近点算法(Iterative Closet Point, ICP)算法获得前后两幅点云图像的精确旋转矩阵与平移向量，实现空间三维数字化重构目标。图2为结合单线激光雷达和GNSS/INS的系统三维重构工作流程。

图2 空间三维重构流程Fig.2 Flow chart of spatial three-dimensional reconstruction

3.1 传感器空间、时间配准

在实验中将单线激光雷达和GNSS/INS惯性测量单元固定在一起，由于实验测试的空间场景相对于两个传感器的体积较大，在实验过程中可近似将它视作一个在空间内重心重合的质点，那么GNSS/INS反馈的位置姿态信息亦可视为激光雷达的姿态信息。但是二者之间会始终存在一个由初始状态决定的角度偏差，若采用直接测量该误差角度的方案，则会存在较大的测量误差且该偏角不易测量。针对此问题，在研究系统中运用相对角度不变性的原理——前后状态下的角度差值为定值，来实现数据的坐标变换，即各轴之间存在的相对空间角位置固定，可将它理解为刚体的转动，避免了偏角测量的难题，同时简化了坐标转换时的计算量，提高了计算效率。

除了传感器数据在空间上存在角度的偏差，数据在时间上的配准也存在一些问题。激光雷达和GNSS/INS之间数据采集频率不同，当分别在两个传感器上采集数据时，易导致起始信号不一致，使数据错位。为解决数据的时间配准问题，实验中借助各传感器内置的Linux时间戳来获得起始数据时间，并在MATLAB平台实现传感器反馈数据的预处理，从而实现激光雷达极坐标下的角度、距离信息和GNSS/INS航向角姿态参数在时间上的配准。进行数据的插值运算时，对于近似直线运行轨迹，采用线性内插；对于折线运行轨迹，采用多项式或分段样条内插。为简化后续处理数据的操作步骤，可在MATLAB内设计一个GUI(Graphical User Interface)将上述步骤程序化，大大提高了数据预处理的效率，节省时间与空间上的资源。

3.2 四元数坐标转换与姿态解算

单线激光雷达是对其激光束扫描面的二维信息进行获取，所以本实验使用了GNSS/INS增加三个航向角扫描的维度，获取更多的三维空间信息。为此，需要将在时间上配准的传感器数据应用四元数进行坐标转换，结合GNSS/INS采集到的姿态角信息解算出扫描数据点在空间中的坐标位置。

传感器的姿态解算是实现三维重构的核心技术之一。实验中，三维重构研究系统的姿态和航向角能够有效地反映各个时刻下其所在空间的位置，从而绘制在空间某一位置的三维扫描图像。运用四元数进行姿态解算，能够减小因航向角分析顺序不同而导致的误差，并解决在某一个航向角的旋转值为90°时会出现万向节锁的问题，最终导致刚体丧失旋转自由度。随着捷联式惯性导航技术的发展，采用四元数法来描述刚体的角运动变得更加简单方便，四元数因其方程计算量不大、工作效率较高而广泛应用于描述空间载体的刚性运动。根据GNSS/INS获取的姿态数据可以得到载体在空间上的姿态信息：pitch(围绕X轴逆时针旋转的俯仰角)、yaw(围绕Y轴逆时针旋转的偏航角)、roll(围绕Z轴逆时针旋转的翻滚角)。图3表示载体姿态的解算流程。

图3 载体姿态解算流程Fig.3 Vehicle attitude solution procedure

由于单线激光雷达所获取的数据以极坐标形式展示，为简化角度在空间上的旋转运算，对数据进行坐标变换，将极坐标转化至笛卡尔坐标系内。设激光雷达某一数据点的极坐标为(ρ,θ)，其中ρ为距离值，θ为水平扫描角。为提高扫描空间每一帧点云图像的可视化程度，在姿态解算过程中将激光雷达起始扫描线旋转π/4，使两坐标系下极轴与笛卡尔坐标系下横轴重合。由于三个角度具有空间对称性，这里以围绕X轴即激光雷达的极径旋转的俯仰角α为例，说明该数据点的三维笛卡尔直角坐标(x,y,z)：

(1)

在仅存在单一角度旋转时，此方程的适应性较强，但当三个方向均存在旋转角度时，对各个方向的角度运算顺序会对实验结果产生比较大的影响，会出现一组角度对应多个空间点，尤其是当其中的一个绕轴旋转角度为π/2时，会出现万向节锁，导致其中两个轴发生重合，进而失去一个自由度的情形。

为了避免万向节锁的出现，应用四元数刻画其绕任意轴的旋转。Faugeras和Hebert[12]应用四元数算法来处理移动物体的配准问题。四元数是一种高阶复数，单位四元数是一个包含4个矢量的列阵，即有：

qR=[q0,q1,q2,q3]T,

(2)

四元数对航向角的运算过程为：

q=Rq′，

(3)

其中:R代表四元数航向角的旋转矩阵，q′是作用旋转矩阵前的扫描数据点在空间的坐标，q是原始数据运算四元数旋转矩阵后的结果。空间扫描数据任取一点p，设p点绕x轴、y轴、z轴的旋转角度分别为α，β，γ。那么四元数内绕x轴、y轴和z轴的旋转矩阵将可以表示为：

(4)

(5)

(6)

由各个轴向的旋转矩阵，在空间上任意航向角的旋转可由以上矩阵复合得到点云数据的旋转作用矩阵。

4 点云图像配准

点云配准是点云数据处理的关键技术之一，其实质是确定点云间的变换参数，从而将不同扫描坐标系下的点云转换至全局坐标系下，实现空间三维重构。运用四元数进行姿态解算后，实验中需要将位于局部坐标系的点云图像转化到全局坐标系内实现点云配准，进而获得空间三维重构的效果。鉴于采集的数据图像在时间上是连续的，故可将点云图像划分为多帧图像实现配准。定义前帧数据上的任意一点在全局坐标系下的坐标记为pn=[xn,yn,zn]T，当前帧数据上的任意一点在全局坐标系下的坐标为pn+1=[xn+1,yn+1,zn+1]T。pn到pn+1的三维旋转矩阵R和平移向量t为外部参数矩阵，它们的转换关系如下：

pn+1=R*·pn+t,

(7)

其中：R*·pn表示矩阵R中的元素与矩阵pn中的元素按照相同位置进行相乘，得到的结果作为新矩阵中相同位置的元素。其中，旋转矩阵的变换表现为空间刚体变换，将源点云变换到目标点云相同的坐标系下。

4.1 迭代最近点算法与数据更新

ICP算法[13]主要用于实现多视角下的数据配准。ICP算法是一种高效的三维配准算法，普遍应用于激光雷达扫描数据处理。初代ICP算法可以概括为以下几个步骤：

(1)根据点集pn中的点坐标，在统一曲面S上搜索相应最近点点集pn+1；

(2)计算两个点集的重心位置坐标，点集中心化生成新的点集；

(3)由新的点集计算正定矩阵N，进而得到矩阵N的最大特征值及最大特征向量；

(4)由于最大特征向量等价于残差平方和最小时的旋转四元数，将四元数转化为旋转矩阵R；

(5)确定旋转矩阵R后，通过两个坐标系中的重心点确定平移向量；

(6)以连续两次距离和之差的绝对值作为迭代判断阈值；

(7)执行迭代过程直到满足阈值条件。

ICP算法具有较高的配准精度，但同时存在运算量较大、效率低等问题。其主要原因是ICP算法处理的数据量较大且数据是连续的。为了减少在查找最近点时耗费的时间，本文首先利用k-d tree[14]提高数据查找速度，并基于八叉树的空间坐标分割管理数据存储结构。通过松散八叉树数据结构管理场景节点，进行可见性数据分割，可有效减少数字信号传递的数量，且对于连续点云图像配准通过时间维度上的扫描数与角度分割数据，即间断性地配准点云数据。这样不仅可以保留点云的数字特征，而且在配准时也可以有效减少算法的运算量。

4.2 加速度积分粗配准

通过GNSS/INS中的加速度计可以得出在空间坐标系下的各个方向的加速度值，对加速度数据施以二次积分运算即可得到传感器的位移变化量，但是此过程中易出现二次趋势的问题，在此使用低通滤波滚动平均值的方法将一些低频信号去除。实验中传感器的频率为中频振动，在此选用频域积分来获取位移量。

图4 采集数据场景Fig.4 Data acquisition scenarios

4.3 实验及结果

为了验证本文提出的空间三维重构方法的可行性，实验选择200 mm×200 mm×300 mm的立方体作为实验数据采集对象；实验数据处理平台为i5-4200M cpu，8 GB内存，win7操作系统。如图4所示，利用GNSS/INS收集的多维欧拉角信息和单线激光雷达数据，可以得到两组未配准的图像。

单线激光雷达在俯仰方向上进行50帧扫描。将这50帧的点云数据结合GNSS/INS提供的俯仰角进行坐标转换，转移到同一坐标系下。由于每帧数据转换算法在步骤上是相同的，所以每组图像都需迭代50次，如图5(a)所示。然后，通过ICP算法实现旋转矩阵和平移向量测量，结果如图5(b)所示。

作为比较，利用16线激光雷达VLP-16实现相同的过程，结果如图6所示。多线激光雷达的三维构造过程可以更均匀地分布点数据，并且比单层激光雷达具有更详细的构造效果。表2是实验过程中的点云数量以及其处理时间。多线激光雷达比单线激光雷达消耗更多的时间，消耗的时间约为单线激光雷达的5倍。

图6 多线雷达数据处理后的结果

Fig.6 Processed results of multi-layer LIDAR

表2 点云数量与运算时间

表3是结合ICP算法所得到的各自点云数据配准所需的旋转矩阵与平移向量。为了精准地对单线激光雷达与多线激光雷达的配准精度进行量化对比，引入ICP配准评价指标[15]，其计算公式如下：

(8)

其中：Lpi是源点云中对应原始点的最优配准点，Iqi为Lpi的最近邻点。首先计算配准后对应点云之间的欧氏距离，再对得到的所有数值求和后除以配准点总数进行平均，得到的平均距离差值即为ICP配准评价指标Q。配准评价指标数值越小，配准效果越好。在本文实验环境下，当Q<0.3时，配准精度较高。

表3中数据代表了单线激光雷达和多线激光雷达重构比较过程中的准确性。结合上述的ICP配准评价指标，可以得到单线激光雷达的重构配准精度较好，且与多线激光雷达效果接近。

表3 点云配准旋转矩阵、平移向量以及配准得分

5 结 论

本文详细阐述了单线激光雷达和GNSS/INS的空间三维重构技术，并在实验中验证了这一系统在空间重构方面的可行性。利用相对角度不变性原理，解决了单线激光雷达与GNSS/INS相对角度偏差的问题。并且，利用四元数对激光雷达的点云数据进行坐标转换。结合GNSS/INS的姿态角度信息，完成姿态解算，获得空间点云图像。在本文实验环境下，根据ICP配准的评价指标可以得到单线激光雷达与多线激光雷达的配准得分较为接近，单线激光雷达与GNSS/INS的空间三维重构系统与多线激光雷达构成的三维重构系统同样具有较高的配准精度。使用ICP算法进行点云空间配准时，利用k-d tree方法提高数据筛选速度，并且结合八叉树的空间坐标分割管理数据存储结构特征，对待匹配的点云数据进行更新。在保留点云数字特征的前提下，能够显著提高运算速率。实验结果表明，本文提出的基于单线激光雷达结合GNSS/INS的方法，相较于多线激光雷达方法可以节省76%的数据处理时间，而且单线激光雷达与GNSS/INS的空间三维重构系统在俯仰方向上得到的角度范围更大且垂直角度分辨率也是可控的，可以针对特定感兴趣区域通过增加俯仰角度测量范围以及缩小垂直角度分辨率，使得数据更加详细、均匀。本文方法能够在某些领域的三维空间重构中替代多线激光雷达。