激光里程计误差修正方法、系统、存储介质及计算设备

1.本发明涉及自动驾驶领域，特别是关于一种激光里程计误差修正方法、系统、存储介质及计算设备。


背景技术：

2.激光雷达对于环境光照变化较为鲁棒，能够直接获得环境的三维高精度点云信息，因此在环境感知、定位、高精度地图构建等任务中得到广泛应用，是自动驾驶中重要的车载传感器。按照激光扫描的实现方式，可以分为机械式激光雷达和固态激光雷达两类，本发明提出的修正方法针对机械式激光雷达。
3.从工作原理上，机械式激光雷达内装有垂直排列的不同俯仰角的激光发射器，通过电机驱动进行水平旋转，通过旋转编码器(rotaryencoder)获得方位角测量ω，通过测量激光从发射后经目标表面反射返回激光雷达的飞行时间(timeofflight，tof)获得距离测量d。根据机械式激光雷达的内部结构和工作原理建立内部参数(以下简称内参)模型对于方位角测量ω和距离测量d进行解算，获得点云坐标p
l
。以一种常见的内参模型为例进行说明，如式(1)所示：
[0004][0005]
其中，α为方位角修正量，用于修正方位角测量ω；β为俯仰角，为每条激光线束相对于水平面的夹角。同时需要说明的是，也存在参数量更多的内参模型，但是其原理均为对于方位角测量ω以及距离测量d进行修正，将球坐标系下的测量转换到直角坐标系下，因此采用式(1)中的内参模型进行说明具有一般性。
[0006]
上述内参在激光雷达出厂前完成标定，在后续使用过程中视为已知常量。但是，由于标定误差、机械结构在长期使用中磨损等原因，不可避免地存在误差，这会导致解算获得的点云存在畸变。选取在结构化平面道路上采集的单帧点云进行说明，首先对于点云进行地面分割，获得地面点云，如图1所示；之后统计地面点云高度关于水平方向距离的分布，如图2所示，可以看出地面点云存在着“近低远高”的畸变趋势。需要指出的是，虽然是以地面点云为例进行的说明，但是从机械式激光雷达的工作原理可知，这种畸变趋势存在于整体点云中，因此需要对于整体点云进行修正。
[0007]
激光里程计(lidarodometry)通过匹配相邻帧点云估计位姿增量，是一种基于航迹递推(deadreckoning，dr)原理的定位方法，在自动驾驶汽车定位领域得到广泛应用。在实际应用中发现，激光里程计垂向位姿估计结果存在较大误差，经过长时间累计后在高度方向上存在较为严重的漂移，这限制了激光里程计的使用场景和范围。针对激光里程计误差修正，现有方法主要将轨迹建模为高斯过程(gaussianprocess，gp)，利用轨迹真值和点云特征分布拟合高斯过程回归模型参数，在应用中直接对于激光里程计的位姿估计结果进行修正，进而减少误差。这种方法实现简单，在工程中易于应用，但是模型参数通过数据
驱动的方法获得，在训练过程中大多需要位姿真值，可解释性较差。


技术实现要素：

[0008]
针对上述问题，本发明的目的是提供一种激光里程计误差修正方法、系统、存储介质及计算设备，其能对获得的激光点云进行去畸变修正，减小激光里程计位姿估计误差。
[0009]
为实现上述目的，本发明采取以下技术方案：一种激光里程计误差修正方法，其包括：获取原始点云；所述原始点云包括地面点云及车辆周围障碍物的点；根据所述原始点云中的地面点云，建立地面点云形状模型，确定地面点云分布的平面及需要修正标定的参数；根据所述地面点云分布的平面及需要修正标定的参数标定修正模型，由标定后的所述修正模型对所述原始点云进行在线实时修正；将修正后的所述原始点云输入激光里程计中进行位姿估计。
[0010]
优选的，所述建立地面点云形状模型，包括：对所述原始点云进行地面分割，获取地面点云；设定地面点云分布在单位法向量为的地面平面上，并根据实际地面点云形状的影响因素确定对来源于激光雷达内参建模标定误差的畸变趋势进行俯仰角修正量标定。
[0011]
优选的，所述根据所述地面点云分布的平面及需要修正标定的参数标定修正模型，包括：对地面平面的所述单位法向量进行估计；根据所述单位法向量估计结果构建代价函数，完成对所述仰俯角修正量标定。
[0012]
优选的，所述对地面平面的单位法向量进行估计，包括：设激光雷达具有 n条线束，每条线束旋转一圈获得m个扫描点；其中，第i条线束中有mi个点被分割为地面点，i＝1，2，
…
，n；估计第i条所述线束上mi个所述地面点所在平面的单位法向量并计算其权重；分别计算所述单位法向量在球坐标系下的俯仰角和方位角；利用所述权重将所述俯仰角和所述方位角分别进行加权融合，获得地面平面的单位法向量在球坐标系下的俯仰角和方位角估计结果；通过所述地面平面的单位法向量在球坐标系下的俯仰角和方位角估计结果，计算出所述地面平面的单位法向量的估计结果。
[0013]
优选的，所述根据所述单位法向量估计结果构建代价函数，完成对仰俯角修正量的标定，包括：对地面点云集合进行拟合，获得所述单位法向量为和距离误差阈值为δ的平面，并获得该平面参数、符合平面模型的内点集合以及不符合平面模型的外点集合，计算某点到所述平面的误差；根据所述误差计算该点对应的代价函数值；计算单帧点云对应的代价函数值，作为所述地面点云去畸变效果的度量；对所有点云进行遍历，构建优化问题，完成对仰俯角修正量的标定。
[0014]
优选的，所述计算单帧点云对应的代价函数值，包括：对于离群点采用鲁棒核函数进行代价函数值的计算；采用huber核函数或cauchy核函数。
[0015]
优选的，所述计算单帧点云对应的代价函数值，包括：按照内外点数量比例对于内点平均代价函数值以及外点平均代价函数值进行加权平均计算得到。
[0016]
一种激光里程计误差修正系统，其包括：数据获取模块、地面点云形状模型构建模块、修正模块和估计模块；所述数据获取模块，用于获取原始点云；所述原始点云包括地面点云及车辆周围障碍物的点；所述地面点云形状模型构建模块，根据所述原始点云中的地
面点云，建立地面点云形状模型，确定地面点云分布的平面及需要修正标定的参数；所述修正模块，根据所述地面点云分布的平面及需要修正标定的参数标定修正模型，由标定后的所述修正模型对所述原始点云进行在线实时修正；所述估计模块，将修正后的所述原始点云输入激光里程计中进行位姿估计。
[0017]
一种存储一个或多个程序的计算机可读存储介质，所述一个或多个程序包括指令，所述指令当由计算设备执行时，使得所述计算设备执行上述方法中的任一方法。
[0018]
一种计算设备，其包括：一个或多个处理器、存储器及一个或多个程序，其中一个或多个程序存储在所述存储器中并被配置为所述一个或多个处理器执行，所述一个或多个程序包括用于执行上述方法中的任一方法的指令。
[0019]
本发明由于采取以上技术方案，其具有以下优点：
[0020]
1、本发明的修正模型参数量少，可有效减小激光里程计位姿估计误差。
[0021]
2、本发明的模型参数具有实际物理意义，可解释性强。
[0022]
3、本发明在标定过程中无需标定板、标定锥桶等人工标志物，形式简单，易于应用。
[0023]
4、由于点云匹配算法主要依靠点云的几何特征，点云坐标误差会影响激光里程计的位姿估计结果。地面点云占据了一帧点云中的绝大部分，主要提供了垂直方向上的位姿约束。因此，本发明对原始点云进行修正，有助于提高点云整体质量，改善地面点云“近低远高”的畸变趋势，进而减小激光里程计的位姿估计误差。
附图说明
[0024]
图1是现有技术中的地面点云分割结果示意图；
[0025]
图2是现有技术中的地面点云高度分布统计结果示意图；
[0026]
图3为本发明一实施例中的激光里程计误差修正方法流程示意图；
[0027]
图4是本发明一实施例中的地面点云形状建模示意图；
[0028]
图5是本发明一实施例中的俯仰角修正量标定算法流程图；
[0029]
图6是本发明一实施例中的地面点云修正效果示意图；
[0030]
图7是本发明一实施例中的激光里程计平移部分位姿修正效果示意图；
[0031]
图8是本发明一实施例中的激光里程计旋转部分位姿修正效果示意图；
[0032]
图9是本发明一实施例中的计算设备结构示意图。
具体实施方式
[0033]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例的附图，对本发明实施例的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的本发明的实施例，本领域普通技术人员所获得的所有其他实施例，都属于本发明保护的范围。
[0034]
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本技术的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0035]
本发明考虑机械式激光雷达内参建模标定误差，对于获得的激光点云进行去畸变修正，进而减小激光里程计位姿估计误差的修正方法。首先将点云“近低远高”的畸变趋势建模为俯仰角β的误差；其次设计标定算法，以激光雷达采集的点云作为信息源，标定俯仰角修正量δβ；之后根据内参模型对于原始点云进行修正；最后将修正后点云送入位姿估计算法，减小位姿估计误差。
[0036]
在本发明的一个实施例中，考虑机械式激光雷达内部参数(intrinsic)建模标定误差，如图3所示，提供一种激光里程计误差修正方法，本实施例以该方法应用于终端进行举例说明，可以理解的是，该方法也可以应用于服务器，还可以应用于包括终端和服务器的系统，并通过终端和服务器的交互实现。本实施例中，该方法包括以下步骤：
[0037]
步骤1、获取原始点云；原始点云包括地面点云及车辆周围障碍物的点；
[0038]
其中，原始点云为激光雷达旋转一周获得的一帧点云；
[0039]
步骤2、根据原始点云中的地面点云，建立地面点云形状模型，确定地面点云分布的平面及需要修正标定的参数；
[0040]
步骤3、根据地面点云分布的平面及需要修正标定的参数标定修正模型，由标定后的修正模型对原始点云进行在线实时修正；
[0041]
步骤4、将修正后的原始点云输入激光里程计中进行位姿估计。
[0042]
上述步骤2中，根据原始点云中的地面点云，建立地面点云形状模型，具体为：从图2中可以看出，实际地面点云形状受到两方面因素影响：一方面来源于激光雷达相对于地面的角度，包括：受到地面本身坡度、车身相对于地面的俯仰、激光雷达相对于车身的安装角度影响；另一方面来源于激光雷达内参建模标定误差，实际点云是上述两种趋势的叠加，如图4所示。
[0043]
需要说明的是，来源于激光雷达相对于地面的角度的变化趋势对于位姿估计的精度没有影响，因为在数据采集过程中，激光雷达与真值采集设备(例如高精度惯导)刚性固连在车身上，二者之间的位姿通过传感器之间的外参进行关联，因此这种趋势不是造成位姿估计结果垂向漂移的主要原因，无需进行修正。但是在标定过程中需要先消除这部分趋势的影响，之后再对来源于激光雷达内参建模标定误差的畸变趋势进行修正量标定。
[0044]
因此，对地面点云形状模型进行建模，包括以下步骤：
[0045]
步骤21、对原始点云进行地面分割，获取地面点云；
[0046]
分割出一帧点云中的地面部分，后续操作只需使用地面点云。本发明对于具体的地面分割方法不做限制，但是应尽量保证地面点云的分割质量，准确的地面点云分割结果有助于提高后续标定算法的效果。实施例中使用bogoslavskyi等人提出的基于2.5d深度图像的地面分割算法，地面点云分割结果如图1所示。
[0047]
步骤22、设定地面点云分布在单位法向量为的地面平面上，并根据实际地面点云形状的影响因素确定对来源于激光雷达内参建模标定误差的畸变趋势进行俯仰角修正量标定；
[0048]
具体为：设定地面点云分布在单位法向量为的地面平面上(为方便后续操作，此处统一规定nz》0，即平面法向量指向地面上方，后续处理均沿用这一约定)，由于激光雷达内参模型中俯仰角β存在误差出现“近低远高”的畸变趋势。因此，需要利用地面点云标定俯仰角修正量δβ，获得修正后的点云坐标p
′
l
，进而修正点云整体
的畸变趋势，修正方式如式(2)所示，地面点云修正效果如图5所示。
[0049][0050]
上述步骤3中，根据地面点云分布的平面及需要修正标定的参数标定修正模型，包括以下步骤：
[0051]
步骤31、对地面平面的单位法向量进行估计；
[0052]
估计地面平面的法向量方向，供代价函数构建使用。具体包括以下步骤：
[0053]
步骤311、设激光雷达具有n条线束，每条线束旋转一圈获得m个扫描点；其中，第i条线束中有mi个点被分割为地面点，i＝1，2，
…
，n；
[0054]
从对于地面点云形状的建模方式易知，同一条线束上的地面点处在同一平面上，如图5所示。
[0055]
步骤312、估计第i条所述线束上mi个地面点所在平面的单位法向量并计算其权重；
[0056]
在本实施例中，采用随机采样一致性(random sample consensus，ransac) 算法估计第i条扫描线上mi个地面点所在平面的单位法向量向量并计算其权重，如式(3)所示：
[0057][0058]
步骤313、分别计算单位法向量在球坐标系下的俯仰角θi和方位角φi；
[0059]
如式(4)、式(5)所示：
[0060][0061][0062]
步骤314、利用权重将俯仰角θi和方位角φi分别进行加权融合，获得地面平面的单位法向量在球坐标系下的俯仰角和方位角估计结果；
[0063]
如式(6)、式(7)所示：
[0064][0065][0066]
步骤315、通过地面平面的单位法向量在球坐标系下的俯仰角和方位角估计结果，计算出地面平面的单位法向量的估计结果；
[0067]
如式(8)所示：
[0068][0069]
步骤32、根据单位法向量估计结果构建代价函数，完成对仰俯角修正量标定；
[0070]
如图6所示，具体包括以下步骤：
[0071]
步骤321、对地面点云集合进行拟合，获得所述单位法向量为和距离误差阈值为δ的平面，并获得该平面参数、符合平面模型的内点集合以及不符合平面模型的外点集合，计算某点到所述平面的误差；
[0072]
在本实施例中，采用ransac算法对地面点云集合p
ground
进行拟合，获得单位法向量为和距离误差阈值为δ的平面，并获得该平面参数plane、符合平面模型的内点集合p
inlier
以及不符合平面模型的外点集合p
outlier
，如式(9)所示：
[0073][0074]
式中，plane＝[a b c d]为平面参数，表示平面a
·
x+b
·
y+c
·
z+d＝ 0。
[0075]
对于点pi＝[x
i y
i zi]
t
，计算该点到平面误差ei，如式(10)所示：
[0076][0077]
步骤322、根据误差计算该点对应的代价函数值costi；
[0078]
在本实施例中，需要计算单个点对应的代价函数值costi。由于实际地面形状与平面存在差异，且地面分割算法客观上存在误差，对于离群点采用鲁棒核函数进行代价函数值的计算，可以采用huber核函数或者是cauchy核函数，huber核函数如式(11)所示，cauchy核函数如式(12)所示：
[0079][0080][0081]
步骤323、计算单帧点云对应的代价函数值cost，作为地面点云去畸变效果的度量；
[0082]
在本实施例中，综合考虑内点集合p
inlier
以及外点集合p
outlier
对于误差函数值的影响，按照内外点数量比例对于内点平均代价函数值以及外点平均代价函数值进行加权平均，如式(13)所示：
[0083][0084]
步骤324、对所有点云进行遍历，构建优化问题，完成对仰俯角修正量的标定：
[0085][0086]
上述步骤4中，将修正后的所述原始点云输入激光里程计中进行位姿估计，具体为：
[0087]
在标定出俯仰角修正量δβ后，按照(2)式对于原始点云进行在线修正，将修正后
点云送入激光里程计中进行位姿估计。
[0088]
在本实施例中，激光里程计使用tixiaoshan等人提出的lego-loam算法，评估修正前后激光里程计平移部分、旋转部分六自由度位姿估计精度，结果如图7、图8所示。由此可以看出，对于平移自由度中沿z轴方向位移以及旋转自由度中横滚角(roll)、俯仰角(pitch)的估计精度明显获得提高。
[0089]
在本发明的一个实施例中，提供一种激光里程计误差修正系统，其包括：数据获取模块、地面点云形状模型构建模块、修正模块和估计模块；
[0090]
数据获取模块，用于获取原始点云；原始点云包括地面点云及车辆周围障碍物的点；
[0091]
地面点云形状模型构建模块，根据原始点云中的地面点云，建立地面点云形状模型；根据地面点云分布的平面及需要修正标定的参数标定修正模型；
[0092]
修正模块，根据地面点云分布的平面及需要修正标定的参数标定修正模型，由标定后的修正模型对原始点云进行在线实时修正；
[0093]
估计模块，将修正后的原始点云输入激光里程计中进行位姿估计。
[0094]
本实施例提供的系统是用于执行上述各方法实施例的，具体流程和详细内容请参照上述实施例，此处不再赘述。
[0095]
如图9所示，为本发明一实施例中提供的计算设备结构示意图，该计算设备可以是终端，其可以包括：处理器(processor)、通信接口(communicationsinterface)、存储器(memory)、显示屏和输入装置。其中，处理器、通信接口、存储器通过通信总线完成相互间的通信。该处理器用于提供计算和控制能力。该存储器包括非易失性存储介质、内存储器，该非易失性存储介质存储有操作系统和计算机程序，该计算机程序被处理器执行时以实现一种激光里程计误差修正方法；该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该通信接口用于与外部的终端进行有线或无线方式的通信，无线方式可通过wifi、管理商网络、nfc(近场通信)或其他技术实现。该显示屏可以是液晶显示屏或者电子墨水显示屏，该输入装置可以是显示屏上覆盖的触摸层，也可以是计算设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。处理器可以调用存储器中的逻辑指令，以执行如下方法：
[0096]
获取原始点云；原始点云包括地面点云及车辆周围障碍物的点；根据原始点云中的地面点云，建立地面点云形状模型，确定地面点云分布的平面及需要修正标定的参数；根据地面点云分布的平面及需要修正标定的参数标定修正模型，由标定后的修正模型对原始点云进行在线实时修正；将修正后的原始点云输入激光里程计中进行位姿估计。
[0097]
此外，上述的存储器中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。
[0098]
本领域技术人员可以理解，图9中示出的结构，仅仅是与本技术方案相关的部分结构的框图，并不构成对本技术方案所应用于其上的计算设备的限定，具体的计算设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
[0099]
在本发明的一个实施例中，提供一种计算机程序产品，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述各方法实施例所提供的方法，例如包括：获取原始点云；原始点云包括地面点云及车辆周围障碍物的点；根据原始点云中的地面点云，建立地面点云形状模型，确定地面点云分布的平面及需要修正标定的参数；根据地面点云分布的平面及需要修正标定的参数标定修正模型，由标定后的修正模型对原始点云进行在线实时修正；将修正后的原始点云输入激光里程计中进行位姿估计。
[0100]
在本发明的一个实施例中，提供一种非暂态计算机可读存储介质，该非暂态计算机可读存储介质存储服务器指令，该计算机指令使计算机执行上述各实施例提供的方法，例如包括：获取原始点云；原始点云包括地面点云及车辆周围障碍物的点；根据原始点云中的地面点云，建立地面点云形状模型，确定地面点云分布的平面及需要修正标定的参数；根据地面点云分布的平面及需要修正标定的参数标定修正模型，由标定后的修正模型对原始点云进行在线实时修正；将修正后的原始点云输入激光里程计中进行位姿估计。
[0101]
上述实施例提供的一种计算机可读存储介质，其实现原理和技术效果与上述方法实施例类似，在此不再赘述。
[0102]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和 /或方框图一个方框或多个方框中指定的功能的装置。
[0103]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/ 或方框图一个方框或多个方框中指定的功能。
[0104]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0105]
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。