基于因子图的GNSS／LiDAR外参在线估计方法*

季博文，吕 品，赖际舟，方 玮，郑国庆

（南京航空航天大学自动化学院，江苏 南京 211106）

0 引 言

随着自动驾驶技术的发展，无人车在巡检、物流、安防等各个工业场景下应用也越加广泛［1］。对于该类已知场景下的任务，常用的导航方案为预先构建场景的点云地图，后续利用地图匹配算法获取无人车的位姿［2～5］。因此，如何构建大范围场景下的高精度点云地图是目前科研人员研究的重点之一。

位姿估计算法的精度是影响地图构建质量的重要因素之一。在室外环境下，位姿通常由全球导航卫星系统（global navigation satellite system，GNSS）和惯性导航系统（inertial navigation system，INS）融合获得［6～8］。借助实时运动学（real-time kinematic，RTK）技术，GNSS／INS 融合导航系统可以实现高精度的六自由度定位。然而GNSS／INS的定向精度通常依赖于双天线的支持，但双天线对基线长度存在要求（建议基线长度大于2 m），因此，在小型机器人或小型构图平台上的使用存在一定限制。在室外场景下，GNSS信号容易受到高楼或者树木遮挡，导致定位性能下降。在这类GNSS受扰环境下，GNSS／INS融合导航方案仅能依靠INS系统进行位姿解算，无法保持长时间定位精度。

激光雷达（LiDAR）作为一种主动式传感器，拥有全天时、高精度的优点，结合同步定位与构图（simultaneous localization and mapping，SLAM）技术可以实时提供位姿信息［9～11］，是无人车导航常用的传感器之一。通过将GNSS与LiDAR进行融合，可以实现无人车在室外环境下的高精度位姿估计［12，13］。

GNSS与LiDAR的安装误差又称为外参误差，其会对位姿估计精度产生较大影响。针对该问题，本文基于单天线RTK和LiDAR，对GNSS／LiDAR之间的外参标定方法展开研究，通过构建GNSS 与LiDAR 之间量测量的联系，对GNSS与LiDAR之间外参进行初始化，基于因子图框架对GNSS／LiDAR外参进行在线估计。相较于传统融合算法，本文算法可以有效地隔离外参误差对位姿估计精度的影响。

1 GNSS／LiDAR融合算法

本文的融合算法框架如图1 所示，系统首先获取LiDAR里程计解算的位姿和GNSS得到的位置，然后通过构建传感器之间位姿关系对外参进行初始化，最后基于因子图框架，将GNSS信息和LiDAR里程计进行融合，对传感器外参以及载体位姿进行估计。

1.1 坐标系介绍

本文采用的坐标系定义如图2所示。1）LiDAR坐标系（l系）：以LiDAR中心位原点，X轴沿LiDAR水平轴方向指向前方，Y轴垂直于LiDAR 对称面指向左方，Z轴垂直于X-Y平面上，形成一个右手坐标系，如图2 中虚线所示；2）导航坐标系（n系）：导航坐标系为以GNSS 天线初始位置为原点的东北天系；3）机体坐标系（b系）：机体坐标系用于表示位姿估计的结果，其与LiDAR 坐标系重合；4）GNSS坐标系（g系）：原点位于GNSS天线中心，坐标轴方向与Li-DAR坐标系相同。

图2 坐标系定义

对于外参标定算法而言，需要估计的参数为GNSS 与LiDAR之间存在杆臂以及初始时刻机体系至导航系的旋转矩阵。在本文后续过程中，GNSS 得到的经纬高信息首先会被转换至导航系下。

1.2 GNSS／LiDAR外参初始化算法

假设k时刻，激光里程计解算的位姿为，GNSS解算的位置为，则构建GNSS量测与LiDAR量测之间的关系为

通过奇异值分解（singular value decomposition，SVD）对式（3）进行求解得到。然后根据多个时刻GNSS量测信息以及LiDAR量测信息，以为初值，对式（2）进行非线性优化求解，从而得到外参，的初始估计值。由于无人车在运动过程中不存在较大的俯仰和滚转运动，杆臂Z轴分量引起的误差很难被激励出来，因此，本文仅对杆臂的X轴分量以及Y轴分量进行估计。

1.3 基于因子图的外参在线优化方法

本文构建的因子图框架如图3所示。

图3 因子图框架示意

为减小计算量，本文按照距离选取点云关键帧，仅对关键帧的位姿进行优化，关键帧的选取距离为0.5 m。后端采用非线性优化方法对状态量进行估计，待估计的状态量为

式中 xk为k时刻无人车位于导航系下的位姿。

本文构建的损失函数为

1）GNSS测量残差

则构建GNSS测量残差为

2）LiDAR测量残差

为减少位姿估计受里程计漂移的影响，LiDAR 构建的为两帧之间的位姿约束。假设k-1 时刻和k时刻LiDAR解算的位姿为；则LiDAR量测量为

构建LiDAR测量残差为

式中 ［·］xyz为仅提取四元数的虚部。

在后端图优化中，节点数会随着时间增加而逐渐增加，伴随着计算量逐渐增加。为限制计算量，本文采用滑动窗口策略，剔除旧的节点，保存固定数量的节点和约束进行优化。

2 验证与分析

本文通过Unreal Engine 4（UE4）搭建仿真场景，利用Airsim模拟传感器数据进行仿真验证。在仿真中，LiDAR线数为16，垂直扫描角度为-22.5°～22.5°，水平扫描角度为0°～360°，输出频率为10 Hz。本节利用无人车的真实位置添加噪声以模拟RTK的导航信息，其精度设置为0.05 m，频率为1 Hz。相关算法在机器人操作系统（robot operating system，ROS）中基于C ＋＋实现，在i7-9750 CPU 处理器中运行，仿真轨迹如图4所示，其模拟的为码头场景。无人车由起始点出发，沿既定路线行驶。

图4 仿真试验设计

LiDAR与GNSS之间的外参标定结果如图5、图6、表1所示。

表1 GNSS与LiDAR外参标定精度

图5 X轴和Y轴杆臂估计结果

图6 初始姿态角估计结果

本文算法与传统算法定位误差对比如图7、表2 所示，其中传统算法外参误差设置为0.4 m。由表1 可以看出，本文算法可以有效地对GNSS 和LiDAR 之间外参进行估计，杆臂估计精度优于3 cm，姿态角估计精度优于0.1°。由表2可以看出，本文算法位姿估计精度不受外参误差的影响，位姿估计精度高于传统算法。

表2 本文算法与传统算法定位均方根误差m

图7 本文算法与传统算法定位误差

3 结 论

本文针对传统点云地图构建算法受外参误差影响的问题，研究了GNSS／LiDAR外参在线估计算法，构建了仿真平台，对算法进行了仿真验证。结果表明：本文外参估计算法杆臂估计精度优于3 cm，角度估计精度优于0.1°。相较于传统的点云地图构建算法，本文方法可以有效地隔绝外参误差对位姿估计精度的影响，具有较强的应用前景。