基于图优化方法的点云三维环境重构

陈 文，岳惊涛，李 华，韩栋斌，齐 尧

(1.陆军军事交通学院 研究生管理大队，天津 300161； 2.军事交通运输研究所，天津 300161； 3.陆军军事交通学院 军用车辆系，天津 300161)

● 基础科学与技术BasicScience&Technology

基于图优化方法的点云三维环境重构

陈 文1，岳惊涛2，李 华3，韩栋斌1，齐 尧1

(1.陆军军事交通学院 研究生管理大队，天津 300161； 2.军事交通运输研究所，天津 300161； 3.陆军军事交通学院 军用车辆系，天津 300161)

点云的三维重构对无人车感知和高精度地图的制作具有重要作用。为得到与真实环境一致的点云三维环境，提出以GPS差分状态良好的点云为参考，将闭环图优化延伸到非闭环条件下，分别对局部差分不好的路段进行ICP配准和图优化，在此基础上建立全局图结构实施精细优化的算法。通过实验发现，运用该算法消除了配准的累积误差和轨迹重合区域的点云误差，得到全局相对位置一致的点云，达到与真实环境一致的三维重构效果。

配准；图优化；点云；三维重构；无人车

Abstract: Three-dimensional reconstruction of point cloud plays an important role in perceiving unmanned vehicles and making high precision maps. In order to obtain three-dimensional environment of point cloud which is consistent with real environment, the paper firstly proposes a method that extending the closed-loop graph to non-closed-loop graph with point cloud of good GPS differential state as reference. Then, it conducts ICP registration and graph optimization on partial bad differential section respectively, and establishes global graph structure to implement fine optimization. The experiment shows that this algorithm eliminates cumulative error of the registration and point cloud error in the intersection area, and it obtains the point cloud which has relative position in global and achieves three-dimensional reconstruction effect consistent with real environment.

Keywords: registration; graph optimization; point cloud; three-dimensional reconstruction; unmanned vehicles

三维环境重构对无人车的感知和高精度地图的制作具有重要作用。目前，三维环境重构的方法主要有基于视觉和激光雷达[1-2]。视觉易受环境因素的影响，比如光线、天气等，而激光雷达则具有较高的环境适应性和精准的测距精度。因此，本文主要是基于激光雷达点云数据来离线构建无人车三维道路环境。

基于点云的三维重构主要是将相对位置有偏差的点云进行纠正，使所有点云间的相对位置达到全局一致。对于相对位置有偏差的点云常采用配准的方法解决，目前国内外配准算法的研究已经很成熟，主要包括基于几何特征、RANSAC的初始配准和基于ICP、改进ICP的精细配准以及多种算法融合的配准[3-5]。但是这些配准算法对于两帧或少数几帧效果很好，对于连续配准来说，会随着点云数量的增加，细小的配准误差就会被累积而放大，从而造成配准的末端与实际有很大的偏离。因此，针对无人车这种相对位置有偏差且连续配准的情况，还需要对配准后的累积误差进行消除。Dorit Borrmann等[6]提出了一种图优化的算法，该算法在配准的过程中采用闭环检测，进行累积误差的初始分配，构建图结构实施图优化可以达到全局一致的效果，但该算法是将所有的点云进行配准且需要闭环，有很多冗余处理和一定的局限性。

本文主要利用GPS差分状态信息，将闭环图优化延伸到非闭环条件下，只对差分不好路段进行配准和局部图优化，最后进行全局图优化，实现无人车三维道路环境重构。

1 研究的总体思路

GPS在差分状态下，位置精度非常高，通常在2 cm左右，可以作为参考点云。本文主要依托GPS差分信号的准确性来进行非闭环条件下的点云优化。首先，根据GPS状态信号，搜索差分丢失路段序列；其次，以差分信号良好的点云为参考，构建图结构实施局部图优化，由于点云配准的累积误差比较大，为避免图优化陷入局部最优，这里将局部图优化分为初始调整和精细优化；最后，在全局实施精细优化。

2 点云配准

2.1初始转换矩阵

虽然ICP(最近点迭代算法)具有很高的配准精度，但它受初始条件的限制，当初始条件不好时很容易陷入局部最优而带来较大的配准误差。因此，在进行ICP配准之前需要对点云进行初始位姿调整以适用ICP精细配准。

本文采用的是GPS/IMU所发出的点云位姿信息，经过滤波处理，轨迹比较平滑，相邻点云的相对位置偏差比较小。因此，在对激光雷达点云配准时可以认为前一帧的转换矩阵与当前帧的转换矩阵相近，满足ICP精细配准的初始条件。本文在对连续点云序列两两配准时，将前一帧的转换矩阵作为下一帧ICP 配准前的初始转换矩阵。

2.2ICP精细配准

本文以前一帧点云为源点云s(n)，当前帧点云为目标点云T(n)，其对应点对集分别表示为M(m)和D(m)。以目标点云相对于源点云的平移和旋转为变量，根据最小二乘原理建立误差目标函数：

(1)

式中：R为3×3的旋转矩阵；t为3×1的列向量；Mi与Di为一对对应点对；m为对应点对的数目。

以误差目标函数F(R，t)的值最小为最优，运用四元数法求解R和t，通过不断迭代，使误差目标函数值收敛来获取最优转换位置。

ICP算法主要流程：①在点云数据降采样后，对源点云数据构建k-d树；②利用k-d树，以最近欧式距离cldist(默认为50 cm)为对应点选取阈值，寻找两帧点云对应点对；③运用四元数法求解R和t，更新目标点云位置；④重复步骤②和③，直到前后两次迭代的目标函数差值小于预设阈值(默认为0.000 01 cm)或达到迭代次数(默认为150次)时停止。

3 基于图结构的优化处理

3.1局部优化处理

3.1.1 初始调整

为将ICP的累计误差以一种理想的方式分配给图结构中的各个顶点，用以减少直接精细优化所带来的时间消耗，避免陷入局部最优，因此在配准后对不好路段的点云进行整体初始调整[7]。

假设不好路段的点云为P0，P1，…,Pn，其位姿用欧拉角表示，则Pi=(xi,yi,zi,θxi，θyi,θzi)，其中P0和Pn为差分状态良好的点云，P1到Pn-1为差分状态不好的点云。对P0到Pn-1进行配准，之后以前、后两帧点云位置为顶点，以它们的点云关系为边构建图结构。此时将Pn-1的位置另记为V1，接着将Pn-1的位置转换到配准前的原始位置并与Pn进行配准，这里认为此时的配准位置才是Pn-1最接近真实的位置，另记为V2，将ΔV=V2-V1作为累计误差。而后将Pn加入到顶点中，并以Pn-1与Pn相对位置关系为边，至此图结构构建完毕。

接下来是计算每个顶点的权重(这里权重为分配误差与ΔV的比值，用wi表示)。设P0和Pn的权重为0，表示不进行调整；Pn-1的权重为1，表示按照ΔV误差进行调整。其他的顶点权重按照如下公式进行分配：

(2)

式中：d(vs,vi)为P0到Pi之间所有边之和；d(vs,ve)为P0到Pn-1之间所有边之和。

从上述初始调整的思路可以看出，边的选取直接影响着各点误差分配的比例，这里边的选取为马氏距离协方差矩阵Cij。

3.1.2 精细优化

虽然经过初始调整后，解决了ICP配准的偏离问题，但相邻点云间的相对位置还是存在一些细小的偏差，特别是路段中间位置的点云。为消除这种细小的误差，还需要对其实施精细优化。

精细图优化的实质是将优化相关的变量和约束条件以图的形式表述的优化问题，即这里将点云的位置作为顶点，点云间的相对位置关系为约束边，引用马氏距离建立误差目标函数，通过求解目标函数的极值来得到最优估计。

假设图结构中某相邻顶点Pi=(xi,yi,zi,θxi,θyi，θzi)与Pj=(xj,yj,zj,θxj,θyj，θzj)，则目标函数为

(3)

(4)

(5)

Cij=s2(MTM)-1

(6)

(7)

D=HP

(8)

则马氏距离函数变为

(9)

通过使W最小化来解算该误差目标函数，即

(10)

Cp=(HTC-1H)-1

(11)

(12)

式中：Gij为矩阵G的子项；Bi为B的子项。

因此，将求解线性最优估计问题转化为求解线性等式方程组，即

GP=B

(13)

(14)

此时虽然已经找到了图结构中位置顶点的最优估计，但从式(4)以及观测边和估计边的求解可以看出，其位置估计值是依赖位置更新前观测位置的最小配准误差，图优化的关键思想就是让所有边相连点云间的相对位置估计误差尽可能地接近其最小配准误差，因此图结构精细优化也需要类似ICP的迭代，算法流程如下：

(4)根据线性等式方程组GP=B，运用乔里斯基分解求解P。

(6)判断w和迭代次数，若w小于误差阈值δg(一般默认为0.5 cm)或达到迭代次数(一般默认为50次)停止迭代，否则重复步骤(1)—(5)。

3.2全局优化处理

全局的优化处理主要是针对轨迹重合区域内点云间的误差，这里以它们的点云位置为顶点，以3.1.2小节中所描述的相对位置关系为边，在全局构建图结构，实施精细优化。

假设无人车采集的点云为x0，x1，…，xn，相邻两帧间隔为5 m，以它们的点云位置为顶点，为保证相邻点云间的相对位置关系准确，这里为相邻两帧的点云添加一个相应的边。为保证不同采集路线间交叉区域中重叠较大的点云相对位置关系比较准确，也需要添加相应的约束，这里以两帧点云的欧式距离为判断，差值小于5 m的为其添加一个相应的边。这样在全局上保证了相邻间和交叉区域内偏差比较大的点云间的相对准确的位置关系，从而实现与实际环境一致的点云三维重构。

4 实验验证

4.1实验条件及过程

本实验验证的平台为JJUV-5无人车(如图1所示)。车上安装了GPS/IMU组合导航系统，其单点定位精度小于2 m，RTK定位精度可达2 cm+1 ppm(CEP)，姿态精度为0．02°，数据更新率最高为200 Hz，可以为无人车提供精确的位姿信息。车的顶部安装了velodyne-64HDL激光雷达，其垂直扫描范围为-24．8°±2°，转速为600 r/min，每帧采集的点数约为13万个，可以很清楚地描述无人车周身的三维环境。

图1 JJUV-5实验平台

为验证算法的有效性，利用JJUV-5无人车实验平台，在某校园内连续采集一部分数据，其中包括有交叉区域的路口或道路、GPS差分良好的路段以及受遮挡导致差分丢失的路段。

4.2结果分析

4.2.1 局部图优化分析

图2所示是对GPS信号不好路段(序列号为333-353)进行ICP配准前后对比图，其中图2(a)是在差分信号丢失的情况下所呈现出的点云环境，从图中可以明显看出，重复区域内的特征物位置依次错开，造成环境视图比较模糊。而图2(b)则是ICP配准后所呈现出的点云环境，与图2(a)形成鲜明的对比，图2(b)整个点云环境比较清晰，特别是重复区域里可以明显地辨别出特征物的形态和属性。但是ICP只是保证了相邻两帧相对准确位置关系，它会随着配准数量的增加而导致误差的累积。如图2(c)所示，在进行ICP配准后，可以明显地看出配准的最后一帧与其下一帧GPS差分状态良好的点云有很大累积误差(如图2(c)两箭头之间所示)。

(a)局部差分信号丢失点云环境视图

(b)配准末端局部放大点云环境视图

(c)配准后局部点云环境视图图2 ICP配准前后对比

图3所示是对ICP配准后的点云进行图优化后的对比图。为消除累积误差，先进行图优化的初始调整，如图3(a)所示，这是进行初始调整后的点云环境视图，从中可以看出配准时最后一帧的偏离已经消除，点云环境视图比较清晰，重复区域特征物明显。但是经过检查发现轨迹中段的340帧和341帧点云间存在一定的误差，如图3(b)中矩形区域所示，仔细观察可以发现路牌出现重影，道路边界线不重合等。经过精细优化后，如图3(c)中矩形区域所示，误差已经消除且点云环境视图较之前清晰，特别是路牌重影消失、道路边界重合。

(a)初始调整后末端点云环境视图

(b)初始调整后中段环境视图

(c)精细优化后中段环境视图图3 局部图优化对比

由以上分析可知，差分信号丢失路段经过ICP配准虽然消除相邻点云间相对位置误差，但是会带来较大的累积误差。经过图优化的初始调整后可以初步消除累积误差，但有些点云还是存在一些细小的误差。通过精细优化后所有相邻点云间的相对位置准确，有效地解决了配准累积误差消除的问题。

4.2.2 全局图优化分析

图4所示是对全局进行精细图优化前后点云环境对比图。由于外参标定、单帧点云位置校准等细小误差导致重复路线交叉区域内的点云间存在一定的误差。如图4(a)所示，可以很明显地观察到矩形区域内特征物出现重影、道路边界不重合。通过为这些区域增加约束边而重新构建图结构，实施精细图优化后，如图4(b)所示，可以看到图中矩形区域重影消失、道路边界重合，与图4(a)形成鲜明的对比，图4(b)中点云环境很清晰，可以明显辨识出特征物的形态和属性。

(a)精细优化前局部放大点云环境视图

(b)精细优化后局部放大点云环境视图图4 全局精细图优化前后对比

由以上分析可知，在解决ICP累积误差消除的基础上，对重复路线交叉区域内的点云添加约束边，通过全局的图优化可以很好地解决此区域内的点云误差，能够在全局上得到相对位置比较准确的点云，达到与实际环境一致的点云三维环境重构，证实了整个算法的有效性。

5 结 语

本文主要对点云的三维重构进行了研究。首先，利用无人车GPS/IMU组合导航系统的GPS状态信息，搜索出点云差分效果不好的路段，分别对其进行ICP配准，配准时间为每帧60～120 ms，配准精度在5 cm以内；其次，以差分良好的点云为参考，将闭环图优化延伸到非闭环条件下，消除ICP配准带来的累积误差，与差分状态良好点云的相对位置精度在10 cm以内；最后，在此基础上从全局的角度出发，构建图结构实施全局精细优化，消除重复采集路线交叉区域内点云误差，得到了点云间相对位置精度为10 cm左右(个别点云间会达到20 cm)的三维环境，实现了与实际环境基本一致的点云三维重构。但也存在一定的问题：在进行全局精细优化的时候，本文只采用了第一帧作为固定点云，整个点云环境相对于第一帧有0.5°左右的旋转和20 cm左右的平移，导致整体绝对精度不高。下一步，将在差分状态良好的点云中选取更多的关键点云作为支撑框架，对全局点云实施分区域精细图优化来提高整个点云环境的绝对精度。

[1] 李竞超.基于立体视觉的三维重建[D].北京：北京交通大学,2010.

[2] 刘超.点云数据处理与三维重构研究[D].南京：东南大学,2015.

[3] 刘新.三维点云数据的配准算法研究[D].秦皇岛：燕山大学,2015.

[4] 朱新宇,万剑华,刘善伟,等. 改进的ICP点云配准算法[J]. 海洋测绘,2015,35(2):77-79.

[5] 陆军,彭仲涛,夏桂华.点云多法向量邻域特征配准算法[J].光电子激光 ,2015,26(4):780-787.

[6] BORRMANN D, ELSEBERG J, LINGEMANN K, et al. Globally consistent 3D mapping with scan matching[J]. Robotics and Autonomous Systems, 2008, 56(2): 130-142.

[7] SPRICKERHOF J, NU CHTER A, LINGEMANN K, et al. A heuristic loop closing technique for large-scale 6D SLAM[J]. Automatika: Journal for Control, Measurement, Electronics, Computing & Communications, 2011, 52(3)：199-222.

[8] 吴雄华.矩形论[M]．上海：同济大学出版社，1994：191-193．

(编辑：史海英)

Three-dimensionalReconstructionofPointCloudBasedonGraphOptimizationMethod

CHEN Wen1, YUE Jingtao2, LI Hua3, HAN Dongbin1, QI Yao1
(1.Postgraduate Training Brigade, Army Military Transportation University, Tianjin 300161, China; 2.Military Transportation Institute， Tianjin 300161, China; 3.Military Vehicle Department, Army Military Transportation University, Tianjin 300161, China)

10．16807/j.cnki.12-1372/e.2017.09.020

TP18

A

1674-2192(2017)09- 0085- 06

2017-04-20；

2017-05-10．

国家自然科学基金重大项目(91220301)；国家重大研发计划(2016YFB0100903)．

陈 文(1989—)，男，硕士研究生；岳惊涛(1971—)，男，博士，高级工程师，硕士研究生导师．