基于三维激光雷达高精度地图生成测绘背包研究

王志伟，薄孟德，王思元

（哈尔滨师范大学）

0 引言

高精度地图（high－precision map），又称三维地图、自动驾驶图，是指具有高精度、地图元素更加详细，属性更加丰富、面向机器人（智能车）使用地图.传统电子地图误差可达10 m，三维信息一般不包含在电子地图上；并且高精度地图数据误差通常在0.2 m 以内，并包含三维信息，不但能让自动驾驶级别高车辆快速完成匹配定位，更能将车道等级信息提供给整个驾驶系统路径规划层，进而帮助车辆实现厘米等级路径规划和导航.国内百度智能汽车部、阿里旗下高德、四维图新等各大地图公司针对高速公路环境、城市重要路段高精度地图制作，都有相关产品和方法，纷纷着手打造全国高精度地图，抢占自动驾驶细分市场.但鉴于各大高校、封闭小区等封闭园区路段道路结构与主要道路存在较大差异，且相对来说，道路情况较为狭窄、结构较为复杂，不完全适用构建主要道路高精度地图方法，因此构建高精度地图这一特殊情况仍需继续深入研究.高精度地图制作过程主要包括数据采集、数据处理、语义特征提取、人工质量检测、发布等过程，其中数据处理阶段是将采集到地图数据进行算法处理，从而获得高精度初始地图模板（通常以点云地图形式存在），无语义信息，而高精度地图精度则取决于这一关键环节，该文研究内容也主要以此为主.该文针对封闭园区环境高精度地图构建，基于图优化方法，结合多传感器融合方案，研究设计了一套便携式适配于封闭园区高精度地图测绘背包，该测绘背包通过采集三维激光雷达点云数据和其他辅助传感器数据，利用该文优化多传感器融合算法对真实场景进行三维重建，形成高精度地图，并把构建好高精度地图在无人配送车上进行定位导航实验，实验结果证明：背包生成高精度地图精度在0.1 m以内，可以很好完成地图构建任务.

1 多传感器融合方法

同步定位与地图构建（Simultaneous Localization and Mapping，SLAM），是指移动机器人利用搭载传感器确定自身位置及周围环境过程，是实现自主导航重要前提.目前，移动机器人使用场景越来越复杂，这使得单一传感器SLAM算法无法很好地在复杂使用环境下运行，鲁棒性差，比如在光线不足、强光干扰、特征缺失、运动物体受到干扰场景下，视觉摄像机会出现跟踪失效情况；2D激光雷达只能对某一平面2D信息进行扫描，不能对现场信息进行完整还原；惯性测量单元IMU存在零偏，采用积分方式求解位姿会产生累计误差，GPS差分信号在建筑密集情况下状态不佳，误差精度较差.那么如何高效完成复杂场景下目标任务建图导航就是当务之急了.Multi－Sensor融合SLAM根据不同传感器特性成为潮流.该文旨在提高测绘背包在室外复杂环境下定位准确性和鲁棒性，因此研究移动机器人常用可携带式传感器.通过研究其传感器模型及自身特性，在总结了各种传感器优势和缺点基础上，研究将三维激光雷达、IMU 和GNSS－RTK 等传感器进行多传感器信息融合，以实现测绘背包在封闭园区未知环境下更加精确地定位建图.

1.1 IMU外参标定

由加速度计和陀螺仪组成惯性测量单元（IMU）可以测量载体自身运动加速度和角速度，目前九轴IMU 还配备了磁力计，可以起到消除累积误差作用.IMU是一种可以获得高频惯性测量值传感器，这种传感器成本低，体积小，精度高，在移动载体定位中经常使用.IMU 主要受到高斯白噪声和BIAS 随机游走两种随机误差影响.高斯白噪声是指在一段时间内，IMU 数据经历平均值为0，方差为σ，高斯过程n（t），每个时刻彼此独立：

其中，δ（）表示狄拉克函数.在实际环境中，IMU测量数据需要进行离散采样，在只考虑高斯白噪声积分时有式（3）：

协方差为：

L离散和连续高斯白噪声转换关系为：

1.2 IMU运动模型

IMU测量模型可以根据1.1 节分析误差模型得到，只考虑高斯白噪声和BIAS随机游走：

其中，上标g为陀螺仪，上标a为加速度计，w 为世界坐标系，b为IMU 坐标系，gw为世界坐标系下重力，ω、a 分别为IMU 角速度和加速度真实值，～ω，～a分别为IMU 角速度和加速度测量值，用四元数q表示变换矩阵.由于位置对时间导数为速度，速度对时间导数为加速度，以四元数字表示旋转，因此可以用IMU运动模型来表示：

1.3 IMU预积分

IMU传感器在IMU坐标系B 下采集三轴角速度ωB，在一定频率下采集三轴线性加速度aB.但陀螺仪和加速度计会受到零均值高斯噪声ηa和ηg，以及随时间推移而变化零偏ba和bg影响.系统状态估计可以通过积分IMU 测量值在系统初始状态已知情况下传递出去.但这种方法以系统初始状态为基础，在非线性优化调整系统初始状态情况下，需要重复积分过程.

1.4 激光雷达－IMU外参旋转矩阵标定

激光雷达－IMU之间外参旋转矩阵RCE对于整个系统鲁棒性十分重要，过大偏差会导致系统初始化不成功而崩溃.但因为激光雷达－IMU 晚餐旋转矩阵对精度敏感，所以容易被激励计算.该文使用手眼标定方法，对齐IMU 和激光雷达观测系统相互旋转，从而标定激光雷达－IMU 外参旋转矩阵，如图1所示.

图1 三维激光雷达与IMU手眼标定方法

2 高精度地图生成算法研究

2.1 因子图

因子图是由表示优化变量变量节点，表示因子因子节点这两种节点组成无向图.贝叶斯网络之所有可以转换成一个因子图，是因为最大后验概率由许多项因子乘积而成.对于由J（x）矩阵在高斯牛顿法中所组成线性增量方程组，其求解方法为矩阵分解.J（x）矩阵是由误差函数f（x）决定，而在SLAM 问题中J（x）矩阵是维数很高矩阵.但由于J（x）矩阵稀疏特性，利用QR 分解J（x）矩阵构成高维矩阵效率就会变得很高.在机器人运行过程中，因子图一般是逐渐成长和增大.对于逐渐往外延伸一个因子图，在机器人往前移动过程中，增加了一组节点，随着观测数据越来越多，增加了越来越多因子进来.每一次求解因子图都是比上次求解因子图大一些，而且大部分因子图与之前因子图基本上是一致，这是增量推理问题.如果从零开始求解优化问题，所求矩阵会越来越大，求解速度会越来越慢，而且绝大部分是重复性工作.所以这里采用增量QR 分解方法来构建因子图，共采用4 个因子，分别是预积分因子、激光雷达因子、GPS 因子和回环检测因子.该框架基于ROS 机器人操作系统，ROS作为点对点设计、多语言支持、架构精简、集成度高、工具包丰富、免费开源全球最受欢迎开源机器人操作系统.图2所示为整体软件系统框架.

图2 测绘背包软件系统架构图

2.2 激光运动畸变矫正

首先通过当前激光帧起止瞬间IMU 数据估计旋转增量，IMU 里程计数据计算平移增量，然后对该帧激光各个瞬间激光点（利用相对于激光帧起始瞬间位姿增量，将当前激光点转化为激光帧起始瞬间坐标系下，实现校正）进行运动畸变纠正；同时对当前帧激光位姿进行粗略初始化，利用IMU数据姿态角（RPY、ROLL、Pitch、YAW）和IMU 里程计数据位姿，这部分对IMU 精度要求更高，因此选择了九轴IMU（带磁力计）.此部分订阅来自ImuPreintegration 原始IMU 数据、订阅IMU里程计数据，表示每一时刻对应位姿；订阅原始激光点云数据；发布当前帧激光运动畸变校正之后有效点云，用于RVIZ展示，发布当前帧激光运动畸变校正之后点云信息，包括点云数据、初始位姿、姿态角、有效点云数据等，发布给FeatureExtration用于特征提取.用关键帧和之前n＋1 个关键帧中特征集合构成地图，进行匹配，构建点到线、点到面约束.

2.3 点云特征提取

假设一帧激光数据，是在一个很小时间段内获取，但是由于这段时间内，激光雷达可能处于运动状态，那么就可能导致运动畸变.点云特征提取模块通过运动畸变校正后当前帧激光点云，对各点曲率进行计算，然后对角点和平面点进行提取（判断曲率大小）.从ImageProjection订阅当前激光帧运动畸变修正后点云信息.发布当前激光帧提取特征后点云信息，包括历史数据有：移动畸变校正、点云数据、初始位置、姿态角度、有效点云数据、角点云、平面点云等，发布给MapOptimization；放出目前激光画幅抽取角点云进行RVIZ展示；用于RVIZ 展示当前激光帧提取平面点点云发布.

2.4 IMU预积分

这部分主要是接收激光里程计和IMU 里程计，根据前一刻激光里程计，以及从这一刻到这一刻IMU里程计变换增量，计算出这一刻IMU里程计；RVIZ显示局部IMU里程计轨迹，利用激光里程计，以优化后状态为基础，对当前帧状态（包括位姿、速度、偏置）进行优化，在两帧激光里程计之间IMU 预积分量构建因子图，对IMU预积分量进行优化，得到每一瞬间IMU 里程计.当关键帧集合为空时，用激光帧imu 原始数据角度数据初始化，并且把数据用lastImuTransformation保存.该功能模块订阅了激光里程计、IMU里程计、IMU原始数据、激光里程计，发布了IMU里程计轨迹，只在激光里程计最近一帧瞬间到当前瞬间之间显示出轨迹.

2.5 因子图优化

此功能模块主要包括SCAN－TO－MAP 匹配，局部关键帧map 特征点，提取当前帧特征点（平面点、角点），更新当前帧位置与姿态，实施SCAN－TO－MAP 迭代优化；关键帧因子图优化（更新全部关键帧位姿，关键帧加入因子图，加入激光里程计因子、GPS因子、闭环因子，执行因子图优化）；做闭关式检测，在历史关键帧中寻找相近距离，相隔时间较远帧设置为匹配帧，在匹配帧四周获取局部关键帧MAP，实施SCAN－TOMAP 匹配，获取最新位置与姿态变换，建立闭环式因子数据，并加入因子图优化.这个功能模块订阅了目前激光帧点云信息（来自FeatureExtration），GPS里程计，以及由外部闭环检测程序提供数据闭环，并将历史上关键帧里程计进行了发布；局部关键帧地图特征点云；激光里程计运动路线；服务保存地图；局部关键帧MAP 匹配闭环；经过闭环优化后当前关键帧位置变换后特征点云；局部MAP 使用滤波器后平面点集合；历史帧滤波后角点集、平点集；通过当前帧配准之后点云.

3 实验平台与硬件系统

为了满足不同封闭园区环境，最终采用了搭载三维激光雷达、九轴IMU、GNSS－RTK 三种传感器便携式移动背包作为算法载体，为方便后续扩展而留有视觉外接接口，操作系统采用Linux系统，如图3所示.RTK采用是电台传输方案，在园区楼顶放置了一根能够在4 km 内广播信号4.5 m长天线，图4 为在园区内收到卫星信号，实验证明这个RTK差分信号在建筑群密度不是特别大情况下信号状态良好.通过测绘背包生成两个园区高精度地图状态良好，信息丰富，如图5所示.

图3 测绘背包硬件架构图

图4 封闭园区内各卫星信号图

图5 园区高精度地图

实验平台选用是园区无人配送车和无人巡检车，如图6所示，作为封闭园区主要应用，这两个实验平台分别采用Autoware、Apollo 自动驾驶系统.Autoware为世界上第一个开源自动驾驶框架，整体性能拥有强大鲁棒性.Apollo 作为百度开发自动驾驶系统，是国内应用广泛自动驾驶开源系统，在环境感知、路径规划、车辆操控等方面优势较强.无人配送车采用是阿克曼模型作为地盘，无人巡检车采用是差动运动模型，选用这两个实验平台可以很好测试高精度地图精度.对两个不同园区进行高精度地图制作，充分测试环境对移动背包影响，测试实验证明，两个园区高精度地图在两个不同实验平台上均可以很好地完成导航和避障工作.

图6 无人巡检车（左）和无人配送车（右）

4 结论

三维高精度地图具备三维空间表达能力，可以更为直观真实而精准地实现对现实物理世界数字化呈现和智慧化应用.在实现将现实物理世界映射到虚拟数字空间同时，三维高精度地图可视化平台将位置、时间、环境等动态信息进行整合，形成统一时空基准全场景化、多源多维多视角数字孪生世界，实现了既满足道路交通地理信息系统基础需求，又满足道路交通地理信息系统基础需求三维向多维升级应用.并且能够支持智能交通领域前沿应用，如自动驾驶、车路协同、智能网联等，通过对传感器数据时间对齐，以因子图优化方式，以激光雷达、IMU、GNSS 为传感器，形成数据紧耦合融合，获得高质量数据关键帧，完成真正意义上封闭园区数据采集.一个小型GNSS、IMU 和3D 激光雷达数据集已经建立起来，算法有效性已经在不同实验平台得到验证.无人驾驶中建图与定位存在许多不同算法，该文所研究只是SLAM 算法一个分支，属于激光SLAM中一种算法.虽然该文在此基础上做了一些改进与优化，但是仍然存在许多地方需要改进.首先已有很多SLAM算法将视觉SLAM和激光SLAM进行了融合，加入了视觉传感器，将相机和激光雷达进行联合标定，丰富了高精度地图语义信息.如何在现有传感器基础上加上视觉传感器，构建精度更高地图生成算法是需要进一步研究内容，其次该文只是对激光雷达和IMU 数据进行紧耦合，对于GNSS 数据只是进行简单位姿校正.而现在市面上已经存在成熟GNSS／IMU组合导航系统，是通过GNSS和IMU紧耦合或者松耦合实现.如何将其运用到SLAM中点云配准和图优化，也是接下来需要考虑的问题.