[发明专利]基于点云配准的位姿计算方法和系统

说明书

本发明公开了一种基于点云配准的位姿计算方法和系统，所述方法包括：获取目标区域的连续帧原始图像，并将相邻两帧原始图像分别转换为待配准的两幅三维点云图；对待配准的三维点云图进行点云筛选，以得到目标像素点集合；计算所述目标像素点集合中，目标像素点组的误差值，其中，所述像素点为组为待配准的两幅三维点云图中相配准的两个像素点；以所述误差值作为约束条件，计算配准后的车身位姿矩阵。解决了现有技术中位姿计算准确性较差、鲁棒性较低的技术问题。

技术领域

本发明涉及智能驾驶技术领域，具体涉及一种基于点云配准的位姿计算方法和系统。

背景技术

随着自动驾驶的发展，在保障车辆及驾驶员安全的基础上，舒适性的要求也越来越高。车辆行驶的稳定性一定程度上由汽车悬架性能决定，汽车悬架自动调整功能可以很大程度上提升驾驶的舒适度，也在一定程度上提升了驾驶的安全性。

在悬架自动调整时，首先由传感器获取前方图像，然后经过算法处理提取重点数据，再输出前方路况信息，最后根据路况动态调整悬架硬度，以适应不同的路面情况。可见，想要得到准确的路况信息，一般需要多帧数据进行融合，以消除单帧数据的误差，而采集每帧数据时，车辆处于运动状态。因此，获取两帧之间车辆的运动状态成为整体算法非常重要的一个环节，这直接影响了路面状况的判断准确性。

车辆运动状态获取方法有很多，可以直接从车身信息获取，例如，IMU，车辆CAN协议，GPS等。但是，传统的车身信息存在一定的弊端，例如存在误差累积问题，随着车辆行驶距离的增加，误差会越来越大。比较理想的方法是，基于ICP点云配准算法，根据每一帧图像的信息计算车辆的运动状态，这样既可以消除累积误差，也减少了对外部输入的依赖。

但是，传统ICP点云配准方案存在一定的弊端。

首先是计算量巨大。假设两个待配准的点云A和B，A中的每一个三维点都需要去B中寻找其对应点，称之为点云匹配，找到所有点的匹配点后再进行后续计算。点云匹配的耗时随着点云数量的增加而直线上升，且该匹配需要进行多次迭代计算，总的匹配时间占整个ICP点云配准计算时间的90%以上。

其次是在特殊场景下，会存在配准失效的情况，也就是计算得到的位姿错误。这种情况常见于弱纹理或特征不明显的待配准场景，例如平直路面、障碍物较小的路面等。这些场景计算结果错误的原因主要是，两个点云的ICP配准是在三维空间的配准，存在3个旋转和3个平移一共6个自由度，如图1所示，图中Z轴，X轴，Y轴为立体坐标系的三个坐标轴，那么沿着三个坐标轴平移为三个平移自由度，沿着三个坐标轴旋转为三个旋转自由度。如果待配准的两个点云都是如图1所示的路面点云，那么在进行配准的时候，由于缺少沿Z轴方向的平移约束，所以配准得到的结果中，沿Z方向的平移结果有很大可能是不准确或错误的。

发明内容

为此，本发明一种基于点云配准的位姿计算方法和系统，以至少部分解决现有技术中位姿计算准确性较差、鲁棒性较低的技术问题。

为了实现上述目的，本发明实施例提供如下技术方案：

一种基于点云配准的位姿计算方法，所述方法包括：

获取目标区域的连续帧原始图像，并将相邻两帧原始图像分别转换为待配准的两幅三维点云图；

对待配准的三维点云图进行点云筛选，以得到目标像素点集合；

计算所述目标像素点集合中，目标像素点组的误差值，其中，所述像素点为组为待配准的两幅三维点云图中相配准的两个像素点；

以所述误差值作为约束条件，计算配准后的车身位姿矩阵。

进一步地，对待配准的三维点云图进行点云筛选，具体包括：

基于预存的语义分割模型，在所述原始图像中分割并提取目标障碍物所在位置的目标图像；

根据所述目标像素点在所述目标图像中的位置进行路面点云筛选。

进一步地，根据所述目标像素点在所述目标图像中的位置进行路面点云筛选，具体包括：