激光雷达的内参建模与点云修正方法*

张晓龙，江 昆，孙 恺，刘茂林，邵振雷，肖鹏川，杨殿阁

（1.清华大学车辆与运载学院，汽车安全与节能国家重点实验室，北京100084；2.上海禾赛科技股份有限公司，上海201702）

前言

针对激光测距的理论研究早在20世纪60年代开始，在2000年前后迎来快速发展，相关产品在测绘领域得到广泛应用［1］。近年来，激光雷达（light detection and ranging，LiDAR）已成为自动驾驶中重要的车载传感器。相比于视觉传感器，激光雷达不受环境光照变化影响，可以直接获得周围道路环境的高精度三维点云信息，因此在高精度地图构建、环境感知和定位等任务中得到广泛应用［2］。根据光束扫描实现方式的不同，激光雷达可分为机械式激光雷达（mechanical LiDAR）和固态激光雷达（solid⁃state LiDAR）两大类［3］。

激光雷达通过测量激光的飞行时间（time of flight，ToF）获得被测物的距离ρ；通过旋转编码器（rotary encoder）确定激光的方位角（azimuth angle）φ，从而获得球坐标系下的测量量M=[ρ φ]，如图1所示。激光的俯仰角（elevation angle）θ通过激光发射器的安装角度确定，属于激光雷达内部参数（intrinsic，后文简称内参）的一部分，将在后文详述。之后通过内参模型进行解算，获得被测物在激光雷达中心空间直角坐标系下的点坐标PL=[x y z]T。

图1 球坐标系示意图

正确的激光雷达内参建模具有重要意义：首先，这保证了获得的测量点坐标能够准确反映实际物理环境；其次，这也为后续利用激光雷达进行定位和建图提供了准确的点云数据。在实践中发现，不准确的激光雷达内参建模和标定会导致获得的点云平面边缘不平整［4］、平面厚度增加［5-6］。

目前，针对激光雷达内参的建模主要针对于机械式激光雷达，通常依赖于机械式激光雷达的物理结构以及工作过程［7］。一类内参模型认为距离测量量ρ和方位角测量量φ是相对于每个激光发射器获得的，对于每条激光线束都定义了多个参数，通过解算测量量M=[ρ φ]获得点云坐标，如式（1）所示：

式中：s为尺度因子，用于修正与距离相关的测距误差［8］；δρ为对于距离测量的修正量，δθ为俯仰角（可理解为相对于水平面的修正量），δφ为对于方位角的修正量；h和v分别为激光发射器相对于激光雷达中心在水平和垂直方向上的偏移量。

这样的内参模型将所有参数都定义在激光线束上，对于每条激光线束使用6个参数[δρ δθ δφs h v]进行描述，在使用前须对这些参数进行标定。模型参数较多，在实际应用中为内参标定带来了不便。

上述内参模型得到了广泛应用，但在实际使用中，也常常舍去部分模型参数，使得上述模型发生退化。根据模型中6个参数的有无，对于不同文献中的内参建模方法进行整理汇总，结果如表1所示。

表1 内参建模方法整理

Pandar XT机械式激光雷达由于收发时所有激光线束均通过相同的发射透镜和接收透镜中心，具有结构和光路上的对称性，因此可以带来内参建模和标定上的简化。本文提出一种针对该型机械式激光雷达内参建模和标定方法；并考虑其偏心结构对于点云的影响，提出角度修正和距离修正两种方法对于获得点云进行修正；通过仿真模拟两种模型的修正效果，并通过实际点云进行验证；最后在位姿估计算法中验证修正算法的有效性。

1 内参建模

由于不同型号机械式激光雷达间存在结构差异，本章从Pandar XT激光雷达的机械结构与工作原理出发，建立内参模型，并给出各个参数的定义和标定方式，进而提出两种点云修正方法，后文将沿用本章中参数的符号和定义。

1.1 机械结构与工作原理

Pandar XT机械式激光雷达机械结构如图2所示。

图2 Pandar XT机械式激光雷达结构示意图

图中：O为激光雷达坐标系原点，S为发射透镜光心，二者等高共面，之间有一恒定偏移量；T为测量点位置；Laser为激光发射器；APD为雪崩光电二极管（avalanche photon diode，APD）。

每条激光线束从激光发射器发出后经过光路折叠反射镜两次反射，从发射透镜光心S发出；在目标表面发生漫反射后经过接收透镜返回激光雷达，再经过光路折叠反射镜两次反射，在雪崩光电二极管处进行光电信号转换，完成飞行时间测量，进而获得距离测量值d；通过旋转编码器获得激光雷达的方位角ω。之后通过内参模型对于获得的测量量M=[dω]进行解算，获得激光雷达坐标系下的点坐标PL，该过程可以用一个抽象函数f(∙)表示：

激光雷达坐标系为一右手系，采用“左-后-上”方式定义；方位角ω相对于y轴正方向定义，俯视图中顺时针方向为正，如图3所示。

图3 激光雷达坐标系及方位角示意图

1.2 内参模型与参数标定

由于所有激光线束在收发时均通过相同的发射透镜和接收透镜中心，该型机械式激光雷达具有结构和光路上的对称性，因此可以带来内参建模和标定上的简化。定义如下模型参数：

（1）发射透镜光心S相对于激光雷达中心O的偏移量b和h，如图2所示；

（2）发射透镜光心S处球坐标系下每条激光线束的方位角修正量α和俯仰角β。

可以看出，第1项与激光雷达自身结构相关，对于每条激光线束只须标定第2项中的两个参数。通过引入激光雷达结构参数减少了需要标定的参数量，简化了标定流程。在标定过程中，偏移量b和h通过激光雷达CAD模型获得；方位角修正量α和俯仰角β相对于发射透镜光心S标定，距离测量值d相对于激光雷达中心O标定。

在激光雷达驱动中，目前直接基于球坐标系下的激光发射角以及距离测量来解算点的坐标，如式（3）所示。

但是，由于激光雷达中心O和发射透镜光心S在空间上不重合，获得的距离测量d=|OT|≠|ST|，如图4所示。因此在采用上述方法进行坐标解算时会带来误差，造成获得的点云存在畸变，需要对于点云进行修正。

图4 测量示意图

1.3 修正方法建模

考虑通过两种方法进行修正：一方面，考虑将发射透镜光心S下的方位角修正量α和俯仰角β修正到激光雷达中心O下的方位角修正量α′和俯仰角β′，利用在该处标定的距离测量值d解算坐标，该方法称为角度修正；另一方面，考虑将激光雷达中心O下的距离测量值d修正到发射透镜光心S下的距离测量值d′，利用在该处标定的方位角修正量α和俯仰角β解算坐标，该方法称为距离修正。

1.3.1 角度修正模型

首先对方位角修正量α进行修正，如图5所示。

图5 方位角修正示意图

图中T′为测量点T在水平面上的投影。

在俯视图中ΔSOT′应用正弦定理，有

可得：

由于β′≈β，进行如下近似：

之后对俯仰角β进行修正，如图6所示。

图6 俯仰角修正示意图

图中O′为将激光雷达中心O向平面STT′投影获得的垂足。依据俯仰角定义，有∠TST′=β，近似∠TO′T′≈β′。

在ΔSO′T中应用正弦定理，有

由 于x=d⋅cosβ′⋅sinα′≈d⋅cosβ⋅sinα，进行如下近似：

通过式（5）和式（6）可以获得激光雷达中心O下的方位角修正量α′和俯仰角β′：

可得角度修正模型为

1.3.2 距离修正模型

在任何旋转角度下的激光雷达坐标系中，T点坐标均有如下两种表达方式：

其中：d=|OT|；d′=|ST|

由于|OT|2=x2+y2+z2，联立可得关于d′的一元二次方程：d′为该方程的一个实根，令B=cosβ⋅(h⋅cosα-b⋅sinα)>0，可得

可得距离修正模型为

2 激光雷达位姿估计算法

考虑到上述修正模型的效果难以通过点云直接进行定量验证和分析，因此考虑定量评估修正前后点云对于激光雷达位姿估计算法的影响，进而验证本文提出方法的有效性。

2.1 即时定位与地图构建技术简介

即时定位与地图构建（simultaneous localization and mapping，SLAM）技术在高精度地图构建过程中得到广泛应用［17-18］。一个经典的SLAM系统主要由前端、回环检测、后端构成［19］，其系统框图如图7所示。

图7 SLAM算法框图

图中，前端负责接收并预处理来自传感器的数据，并进行特征提取和数据关联，估计帧间相对位姿，但是这一步获得的估计结果存在噪声，经过累加会造成轨迹漂移，进而造成地图的结构性错误。回环检测负责计算当前场景与历史场景的相似度，进而判断自身是否返回到之前经过的地点，为后端提供新的约束，进而修正地图的结构性错误。后端负责接收位姿估计结果，并对位姿进行最大后验概率（maximum a posteriori，MAP）估计，在高斯噪声假设下该问题可以转化为最小二乘问题，通过非线性优化的方式进行求解［20］。建图结果分为度量地图（metric map）和拓扑地图（topologi⁃cal map），其中度量地图又可以分为稀疏地图和稠密地图［19］。

2.2 LeGO⁃LOAM位姿估计算法

2014年，Zhang等人提出激光里程计和建图（LiDAR odometry and mapping，LOAM）算法［21-22］，将激光SLAM问题分解为高频低精度的前端激光里程计和低频高精度的后端地图构建，较好地兼顾了算法的实时性和精度。LOAM算法通过定义的点的曲率值大小将点云中的点分为边缘点和平面点两类，如式（15）所示。

2018年，在LOAM算法的基础上，Shan等人提出LeGO⁃LOAM算法［23］，算法框图如图8所示。

图8 LeGO⁃LOAM算法框图

LeGO⁃LOAM算法在前端添加了分割聚类模块，用于提取点云中的地面点，并对非地面点进行聚类，提高了处理速度和特征提取精度，同时增强了对于非结构化环境的鲁棒性。沿用了LOAM算法的特征提取和误差函数定义方式，在帧间匹配过程中引入地面分割结果，提高了特征匹配的效率和准确性，采用双步列文伯格⁃马夸尔特（Levenberg⁃Marquardt，L⁃M）算法估计6自由度位姿。在后端地图构建模块中采用因子图优化位姿估计结果。

2.3 点云误差对于SLAM算法的影响分析

在实践中发现，LeGO⁃LOAM算法存在较为严重的垂向位姿估计漂移。通过上述算法框图可以看出，垂向位姿是通过将当前时刻的地面平面点与上一时刻的所有平面点进行匹配获得的。匹配过程中的对应关系通过如图9中所示的方式确定。

图9 平面点匹配示意图

对于当前帧中的一个地面平面点，在上一帧中寻找同一线束上与之距离最近的两个平面点以及相邻线束上与之距离最近的一个平面点。这3个平面点可以构成一个局部小平面，由此确定点到面的对应关系。通过上述方式确定当前帧中所有地面平面点的对应关系，将点到面距离之和作为代价函数进行优化，进而获得位姿估计结果。

通过定性分析可知，在点云坐标存在误差的情况下，对应局部小平面的位置和法线方向将会发生变化，如图10所示，会影响代价函数计算，进而影响位姿估计结果。因此，对于原始点云进行修正，减少点云坐标误差，提高地面点云质量，对于LeGO⁃LOAM算法垂向位姿估计精度有改善作用。

图10 畸变点云影响示意图

3 仿真验证

搭建仿真模型对上述修正模型进行验证，使用Pandar XT机械式激光雷达内参进行仿真，其中h=30.94 mm、b=13 mm；在32条线束中等间隔地选取线束，获得其中8条线束参数，如表2所示。

表2 线束参数选择

表中俯仰角β向上为正，方位角修正量α正方向与方位角ω相同。

对于每条线束生成0.5~50 m的距离测量值d，按照角度修正和距离修正两种方式对测量点坐标进行修正，对修正前后结果进行比较。

3.1 角度修正模型

采用角度修正模型对方位角修正量α和俯仰角β进行修正，效果如图11和图12所示。

图11 方位角修正量变化Δα

图12 俯仰角变化Δβ

可以看出：随着距离增大，对于方位角修正量α的修正效果快速减小，在20 m外修正效果变化不大；从修正效果上来看，不同线束间的修正量大小基本相同。

可以看出：随着距离增大，对于俯仰角β的修正效果快速减小，在20 m外修正效果变化不大；从修正效果上来看，对于大俯仰角线束的修正效果更加明显，且修正后点云具有整体向xOy平面靠近的趋势。

3.2 距离修正模型

采用距离修正模型对于距离测量值d进行修正，效果如图13所示。

图13 距离测量值变化Δd

可以看出：随着距离增大，对于距离测量值d的修正效果快速减小，在5 m外修正效果变化不大；从修正效果上来看，对于大俯仰角线束的修正效果更加明显，且修正后点云具有整体向原点靠近的趋势。

4 实验

实验中使用的原始点云均通过式（3）中的方法解算获得，未经修正。

4.1 单帧点云修正效果

在标定间中放置多个平面标定板，使用激光雷达采集单帧点云数据，手动选出属于同一标定板的点云，如图14所示。

图14 标定板点云

分别对于原始标定板点云进行角度修正和距离修正，利用M-估计量样本一致性（M⁃estimator sample consensus，MSAC）算法［24］拟合各个标定板的平面参数，之后分别计算3种条件下各个平面上的点到拟合平面的平均距离误差，将其作为平面质量的评价指标；并计算激光雷达到标定板的距离，结果如表3所示。

表3 标定板平面误差统计结果

可以看出，在使用修正方法后，点云中平面点的质量较修正前得到了提高。

4.2 对激光里程计的影响分析

利用采集车采集3段不同场景中的点云序列，利用GPS/IMU设备获得车辆位姿并转换到激光雷达坐标系下作为位姿真值，其基本信息如表4所示。

表4 点云序列基本信息

由于实际采集点云的xy坐标存在较大运动畸变，考虑仅对于点云的z坐标进行修正。使用原始点云、角度修正点云和距离修正点云运行LeGO⁃LOAM算法，并定量评估激光里程计的精度，结果如图15所示。

图15 里程计和建图结果

在评价指标上，选择绝对位姿误差（absolute pose error，APE）和相对位姿误差（relative pose er⁃ror，RPE），计算公式为

按照式（16）计算绝对位姿误差，结果如表5所示。

表5 绝对位姿误差统计结果

可以看出，使用修正后的点云获得的绝对位姿误差的平移和旋转部分均小于原始点云组，说明对于原始点云进行修正可以提高位姿估计精度。

绝对位姿误差反映了单个位姿估计结果与位姿真值之间的差距，受到位姿估计结果漂移的影响较大；相对位姿误差反映了一段时间或路程下的位姿估计增量与位姿真值增量之间的差距，用于评估里程计精度更加合适。对于相对位姿误差，采用KITTI数据集的方式进行处理［25］，选择轨迹中所有长度为100，200，…，600 m的位姿片段按照式（17）分别计算原始点云、角度修正和距离修正条件下在不同距离上的相对位姿误差，这样做是为了通过长距离来消除GPS/IMU设备在单一位置处定位误差对于里程计精度评估结果的影响。相对位姿误差结果整理如图16所示，将平移部分误差和旋转部分误差分别列出，其中左侧纵坐标对应的条形图表示原始点云结果，右侧纵坐标对应的折线图表示角度修正和距离修正后结果的变化情况。

图16 相对位姿误差

之后，将每个相对位姿误差相对于片段长度归一化后再取平均，获得里程计的平移部分误差以及旋转部分误差，计算公式为

式中：Etrans代表平移部分误差，%；Erot代表旋转部分误差，（°）/m。

按照式（18）和式（19）计算出里程计的平移部分误差和旋转部分误差，结果如表6所示。

表6 里程计误差统计结果

可以看出，使用修正后的点云获得的平移和旋转误差均小于原始点云组，说明对于原始点云进行修正对于减小激光里程计误差有改善作用。

最后，考虑到修正算法需要对点云中每个点进行处理，会带来计算资源的额外消耗和计算时间的增加，因此分析修正算法对实时性的影响。实验中点云修正发生在LeGO⁃LOAM算法的特征提取模块中，因此分别统计该模块在使用原始点云、角度修正和距离修正方式下的单帧计算时间，结果如图17所示。

图17 单帧计算时间统计

分别计算3种条件下单帧计算时间的最小值、最大值和平均值，结果如表7所示。

表7 修正算法对于实时性的影响

可以看出：使用角度修正算法的平均单帧计算时间最长，距离修正算法次之；但是从平均值来看两种修正算法导致单帧处理时间的增加均小于0.01 s，因此可以认为修正算法对于实时性的影响不大。

5 结论

本文中根据Pandar XT机械式激光雷达的结构和工作原理，提出一种内参建模方法，通过引入激光雷达结构参数减少了模型参数量。考虑激光雷达偏心结构对点云的影响，本文中提出角度修正和距离修正两种点云修正方法。通过仿真模拟两种方法的修正效果，并通过实际采集点云进行验证。最后通过LeGO-LOAM位姿估计算法进行验证，证明使用修正后的点云对于减小位姿估计误差和里程计误差有改善作用。

从实际应用角度，部分激光SLAM算法需要激光雷达的俯仰角β作为算法参数输入。因此，为方便使用，应尽量采用距离修正方式以保证出厂前标定的俯仰角β不变。对于大尺寸激光雷达（b和h数值较大）、大俯仰角（β数值较大）、近距离测距（例如激光雷达斜装在车顶）的情况下，偏心结构带来的影响愈加不能忽视，修正模型带来的影响将更加显著。

后续工作中将考虑将该内参模型推广到具有类似结构的机械式激光雷达型号中，并对于其他来源的误差进行建模；并设计针对内参的自动化标定方法。