一种激光点云里程计定位误差的实时修正方法和系统

本发明涉及一种针对路面车辆的激光点云里程计定位误差的实时修正方法和系统，属于智能网联汽车环境感知领域。

背景技术：

智能网联汽车行驶需对周围环境和自身状态进行准确的感知以支撑后续的决策和控制。而对自身位姿的准确估计也正是智能网联车进行轨迹规划、控制等功能的基础。而基于gps的被动位姿估计受到卫星信号的限制，难以支撑高级别自动驾驶的全部行驶工况。激光雷达能够直接测得周围环境的距离信息，有着准确的测量精度和较远的测量范围。因此基于激光雷达点云的里程估计广泛用于智能网联汽车的定位系统中。

但是基于里程计的位姿估计存在累计误差，也就是长时间工作后轨迹会出现漂移，因此应用在智能网联汽车领域存在着一定局限性。另一方面，由于车辆行驶在路面上，采集的激光雷达点云在垂直路面方向分布不均匀，基于激光雷达点云的里程估计多呈现垂直行驶道路方向的漂移。

针对此问题，现有研究的解决方法主要有两种：1、通过检测行驶闭环、引入gps等绝对定位信息每隔一段时间对轨迹进行修正，但这种方法不能实时得到准确的位姿估计且依赖外界信息源的输入；2、通过多次采集估计轨迹漂移，训练校准函数，在行驶一段时间后对之前行驶的轨迹进行统一的修正，同样的，这种方法不够能实时估计自车定位信息。因此，现有方法还不能实时的修正激光点云里程计的定位误差。

技术实现要素：

针对上述问题，本发明的目的是提供一种激光点云里程计定位误差的实时修正方法和系统，主要针对智能网联汽车的应用场景，能够几乎消除垂直于路面反向的轨迹漂移。

为实现上述目的，本发明采取以下技术方案：

本发明的第一个方面，是提供一种激光点云里程计定位误差的实时修正方法，其包括以下步骤：

1)从装备有激光雷达的智能网联车辆所采集的历史激光点云数据中，提取路面点云并构建经验模型，计算得到模型参数；

2)采用构建的经验模型及模型参数对智能网联车辆所采集的实时激光点云数据进行实时修正，得到误差消除后的激光点云数据。

进一步，所述步骤1)中，从装备有激光雷达的智能网联车辆所采集的历史激光点云数据中，提取路面点云并构建经验模型，计算得到模型参数，包括以下步骤：

1.1)对历史激光点云数据的畸变规律进行分析，根据分析结果构建经验模型s；

1.2)从历史激光点云数据中提取多帧路面激光点云拟合球面，对拟合球面的球面半径取平均，得到经验模型的模型参数

进一步，所述步骤1.1)中，所述经验模型s为：

式中，为经验模型的参数，近似等于|z|＝1处平面点云畸变的曲率半径；d²＝x²+y²，x、y、z为激光雷达坐标系下的坐标。

进一步，所述步骤1.2)中，从激光点云数据中提取多帧路面激光点云拟合球面，对拟合球面的球面半径取平均，计算得到经验模型的模型参数的方法，包括以下步骤：

1.2.1)在采集到的激光点云数据中随机抽取m帧激光点云；

1.2.2)对于每一帧激光点云使用平面随机采样一致性方法提取其中属于路面的点{x1，x2，...，xm}，并根据激光雷达的原始坐标系的z轴与地面垂直方向的差距，判断是否需要进行坐标转换，当需要进行坐标转换时，进入步骤1.2.3)，否则进入步骤1.2.5)；

1.2.3)根据路面的归一化法向量{n1，n2，...，nm}算出平均法向量并根据得到的平均法向量计算得到转换矩阵t，对采集的激光点云数据进行坐标转换，并进入步骤1.2.4)；

1.2.4)使用旋转矩阵t对步骤1.2.2)中提取的各组路面的点{x1，x2，...，xm}进行坐标转换，求得各组点的平均z值并基于各组点的平均z值在每组点拟合球面，得到每组点的球面半径

1.2.5)根据步骤1.2.2)中提取的各组路面的点{x1，x2，...，xm}求得每组点的平均值，并在每组点拟合曲面，得到每组点的球面半径；

1.2.6)根据步骤1.2.4)或1.2.5)得到的每组点的球面半径计算得到模型参数

进一步，所述步骤1.2.2)中，根据激光雷达的原始坐标系的z轴与地面垂直方向的差距，判断是否需要进行坐标转换的方法，包括以下步骤：对智能网联车辆的激光雷达的原始坐标系的z轴与地面垂直方向的夹角进行判断，如果智能网联车辆的激光雷达的原始坐标系的z轴与地面垂直方向的夹角大于预设阈值，则需要进行坐标转换，否则，不需要进行坐标转换。

进一步，所述步骤1.2.3)中，根据路面的归一化法向量{n1，n2，...，nm}算出平均法向量并根据得到的平均法向量计算得到转换矩阵t的方法为：

首先，根据平均法向量得到转换公式：

式中，v为旋转轴，θ为旋转角；

然后，根据上述转换公式，构建旋转四元数q：

式中，q0、q1、q2、q3为四元数的四个分量，q0称为四元数的实部，q1，q2，q3称为四元数的虚部；

最后，将旋转四元数转换为旋转矩阵t，得到：

进一步，所述步骤1.2.6)中，模型参数的计算公式为：

式中，(q1，q3)分别为所有球面半径中的上、下四分位数。

进一步，所述步骤2)中，采用构建的经验模型及模型参数对智能网联车辆所采集的实时激光点云数据进行实时修正的方法，包括以下步骤：

2.1)对智能网联车辆的激光雷达的原始坐标系的z轴与地面垂直方向的夹角进行判断，如果智能网联车辆的激光雷达的原始坐标系的z轴与地面垂直方向的夹角大于预设阈值，则进入步骤2.2)，否则进入步骤2.4)；

2.2)对原始激光点云数据进行坐标转换，使得转换后智能网联车辆的激光雷达z轴与地面垂直方向的夹角小于预设阈值，并进入步骤2.3)；

2.3)采用构建的经验模型对智能网联车辆所采集的实时激光点云数据进行实时修正，并对实时修正后的点云数据进行坐标逆变换，使其变换回原始坐标系，得到误差消除后的激光点云数据；

进行实时修正的计算公式为：

z′＝z·s

式中，z′为修正后的激光点云z坐标；

2.4)采用构建的经验模型对智能网联车辆所采集的实时激光点云数据进行实时修正，得到误差消除后的激光点云数据。

本发明的第二个方面，是提供一种激光点云里程计定位误差的实时修正系统，其包括：经验模型构建模块，用于从装备有激光雷达的智能网联车辆所采集的历史激光点云数据中，提取路面点云并构建经验模型，计算得到模型参数；误差修正模块，用于采用构建的经验模型及模型参数对智能网联车辆所采集的实时激光点云数据进行实时修正，得到误差消除后的激光点云数据。

进一步，所述经验模型构建模块包括：模型构建模块，用于对激光点云数据的畸变规律进行分析，并根据分析结果构建经验模型s；模型参数计算模块，用于从历史激光点云数据中提取多帧路面激光点云拟合球面，对拟合球面的球面半径取平均，得到经验模型的模型参数

本发明由于采取以上技术方案，其具有以下优点：1、本发明采用修正模型来修正激光点云里程计的累计定位误差，能够提供更准确的自车位姿估计；2、本发明相对于闭环、后处理等方法，能够在线的修正激光点云里程计的误差；3、本发明适用于多种机械式激光雷达，算法与硬件解耦；4、本发明提出方法只需对输入点云进行预处理，运算量小，不会影响里程计的实时性。因此，本发明可以广泛应用于智能网联汽车环境感知领域。

附图说明

图1是激光点云测量结果出现畸变实例；

图2是本发明点云畸变处理流程示意图；

图3是本发明一个特定实施例的示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细的描述。然而应当理解，附图的提供仅为了更好地理解本发明，它们不应该理解成对本发明的限制。

由于激光雷达采用基于tof(timeofflight)进行测距，而测距精度受到入射角、外界光照以及空气湿度等许多影响，因此使用同一个激光雷达在相似的场景下的测量结果往往呈现横偏误差，导致返回的点云存在畸变，进而影响多帧点云间的特征匹配进而导致基于激光点云的里程计出现恒定的漂移。

如图1所示，为velodyne-64e激光雷达测量的路面点云，图中测量点的灰度反映出其在水平面上的高度(单位：米)。本应水平的路面(可以通过差分gps测量的自车行驶轨迹估计出路面近似水平)，测量结果呈现明显的圆弧形弯曲。

如图2所示，基于上述分析，本发明提供一种激光点云里程计定位误差的实时修正方法，包括以下步骤：

1)根据装备有激光雷达的智能网联车辆所采集的历史激光点云数据，提取路面点云并构建经验模型，计算得到模型参数。

2)采用构建的经验模型对智能网联车辆所采集的实时激光点云数据进行实时畸变消除，得到误差消除后的激光点云数据。

上述步骤1)中，构建经验模型的方法，包括以下步骤：

1.1)对已有激光点云数据的畸变规律进行分析，并根据分析结果构建经验模型s。

由于激光点云测距误差原因比较复杂，因此本发明通过观察行车环境下多帧点云的变形情况，发现测量后的点云畸变存在固定规律：1、本应该在同一个水平面的点弯曲成同一个半径的曲面；2、在一定范围内，激光雷达距该水平面越远，弯曲的曲率半径越大。

基于这两个结果本发明构建了一个经验模型s：

式中，为经验模型的参数，它近似等于|z|＝1处平面点云畸变的曲率半径；d²＝x²+y²，x、y、z为激光雷达坐标系下的坐标。

为提高计算效率，上式(1)可做如下化简：

将(2)式进一步化简为：

由于在参数选取中因此趋于零，对进行一阶泰勒展开可将式(3)化简为：

1.2)从已有激光点云数据中提取多帧路面激光点云拟合球面，对拟合球面的球面半径取平均，计算得到经验模型的模型参数

在确定经验模型s后，要合理选取模型参数如上文所说，值近似等于|z|＝1处平面点云畸变的曲率半径。由于实际几乎不可能找到能够保证水平的且与激光雷达测距范围相当的平面，因此本发明采集多帧路面点云拟合球面，并取平均的方法来计算参数具体实施方法如下。应当说明的是，如下模型参数的计算结果并不一定使修正结果最佳，可后续依据激光点云里程计的估计结果与实际行驶轨迹对比，人工对模型参数进行微调。

具体的，包括以下步骤：

1.2.1)在采集到的激光点云数据中随机抽取m帧激光点云，通常选取50帧左右点云即可。

其中，在获取激光点云数据时，令装备激光雷达的智能网联车辆在预设范围的路上行驶并形成闭环，要求路面尽可能平整。具体地，可以在实际应用场景中选取一部分场景，在这部分场景中行驶以对于参数进行标定。在实际操作中可以视标定结果而定，在标定结果变化不大或者认为标定结果满足要求后即可停止标定。

1.2.2)对于每一帧激光点云使用平面随机采样一致性(ransac)方法提取其中属于路面的点{x1，x2，...，xm}，当需要进行坐标转换时，进入步骤1.2.3)，否则进入步骤1.2.5)。

其中，在进行拟合球面前，如果智能网联车辆的激光雷达的原始坐标系的z轴与地面垂直方向相差较大，主要是指激光雷达特意倾斜安装在车顶的情况，如果激光雷达近似水平安装在车顶上，对于安装引起的误差可以不予考虑，则可先对激光点云进行坐标转换令二者方向近似重合，即使得激光雷达z轴近似垂直于路面放置。

1.2.3)根据路面的归一化法向量{n1，n2，...，nm}算出平均法向量并根据得到的平均法向量计算得到转换矩阵t，用于对采集的激光点云数据进行坐标转换，并进入步骤1.2.4)。

如上文所说，要求激光雷达z轴与地面尽量垂直，因此可根据平均法向量将采集到的激光点云进行坐标转换来符合这一条件，转换公式如下：

式中，v为旋转轴，θ为旋转角。

根据上述转换公式，构建旋转四元数q：

式中，q0、q1、q2、q3为四元数的四个分量，q0称为四元数的实部，q1，q2，q3称为四元数的虚部。

将旋转四元数转换为旋转矩阵t，得到：

1.2.4)使用旋转矩阵t对步骤1.2.2)中提取的各组路面的点{x1，x2，...，xm}进行坐标转换，求得各组点的平均z值并基于各组点的平均z值在每组点拟合球面，得到每组点的球面半径

此处需注意由于激光点云测量结果变形情况受很多因素影响，因此弯曲方向不定，所以这里为有符号的半径。为与下文保持统一，这里规定路面拟合球面圆心在路面以下为正，反之为负。

1.2.5)根据步骤1.2.2)中提取的各组路面的点{x1，x2，...，xm}求得每组点的平均值，并在每组点拟合曲面，得到每组点的球面半径。

1.2.6)根据步骤1.2.4)或步骤1.2.5)得到的每组点的球面半径，计算得到模型参数

为确定模型参数本发明通过上述多组结果计算。由于车辆行驶路面状况复杂，因此半径分布往往比较离散，存在较多的干扰，本发明以所有球面半径中的上、下四分位数(q1，q3)作为阈值的方法来减弱这些影响。另外，一般拟合球面半径较大，有可能会造成正负较大的跳变，因此通过曲率来计算参数计算公式如下：

上述步骤2)中，根据确定的经验模型和模型参数，对激光雷达采集到的所有点进行处理，即可实现畸变消除。具体的，包括以下步骤：

2.1)对智能网联车辆的激光雷达的原始坐标系的z轴与地面垂直方向的夹角进行判断，如果智能网联车辆的激光雷达的原始坐标系的z轴与地面垂直方向的夹角大于预设阈值，则进入步骤2.2)，否则进入步骤2.4)；

2.2)对原始激光点云数据进行坐标转换，使得转换后智能网联车辆的激光雷达z轴与地面垂直方向的夹角小于预设阈值，并进入步骤2.3)；

2.3)采用构建的经验模型对智能网联车辆所采集的实时激光点云数据进行实时修正，并对实时修正后的点云数据进行坐标逆变换，使其变换回原始坐标系，得到误差消除后的激光点云数据；

进行实时修正的计算公式为：

z′＝z·s(11)

式中，z′为修正后的激光点云z坐标；

2.4)采用构建的经验模型对智能网联车辆所采集的实时激光点云数据进行实时修正，得到误差消除后的激光点云数据。

本发明还提供一种激光点云里程计定位误差的实时修正系统，其包括：经验模型构建模块，用于从装备有激光雷达的智能网联车辆所采集的历史激光点云数据中，提取路面点云并构建经验模型，计算得到模型参数；误差修正模块，用于采用构建的经验模型及模型参数对智能网联车辆所采集的实时激光点云数据进行实时修正，得到误差消除后的激光点云数据。

进一步，经验模型构建模块包括：模型构建模块，用于对激光点云数据的畸变规律进行分析，并根据分析结果构建经验模型s；模型参数计算模块，用于从历史激光点云数据中提取多帧路面激光点云拟合球面，对拟合球面的球面半径取平均，得到经验模型的模型参数

实施例

在一个实例中，本发明使用开源数据集kitti-odometry的点云，其点云由velodyne-64e激光雷达采集，且已经完成运动补偿。首先，随机选取50帧点云中属于路面点{x1，x2，...，x50},并根据拟合球面半径以及每组点的平均z值计算参数在本实例中从数据集序列00中均匀抽取50帧计算得到模型参数由于其激光雷达垂直水平面放置，因此可忽略式(6)到(8)中的坐标转换步骤。随后选取此模型参数应用本发明方法对输入的所有激光点云进行处理。处理后的激光点云可采用多种方法进行里程推算。在本实例中在第k帧点云中根据每个点的曲率提取平面点和角点然后基于迭代最近点(icp)算法与和进行匹配，进而计算出第k帧相对于第k-1帧之间的位姿变换本实例的整个流程如图3所示。

在本实例中选取的参数应用在本数据集中所有序列中都有较好的效果，这是由于本实例中所有数据都采用同一个激光雷达在相似的场景下采集。应该说明的是当使用不同的激光雷达时或场景变化较大时需要选取不同的参数

经实车试验测试多种激光雷达和运行环境，本发明都能够很好的修正现有激光点云里程计定位的误差，并且本发明在开源数据集上测试也很好的提高了激光点云里程计定位的精度。

此实施例仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。