CN114511600A

CN114511600A - 基于点云配准的位姿计算方法和系统

Info

Publication number: CN114511600A
Application number: CN202210413069.1A
Authority: CN
Inventors: 王欣亮; 苏文秀; 裴姗姗; 肖志鹏
Original assignee: Beijing Smarter Eye Technology Co Ltd
Current assignee: Beijing Smarter Eye Technology Co Ltd
Priority date: 2022-04-20
Filing date: 2022-04-20
Publication date: 2022-05-17

Abstract

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

Description

基于点云配准的位姿计算方法和系统

技术领域

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

背景技术

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

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

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

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

在所述目标图像中，每隔N行和M列选择一个点作为目标像素点，以完成路面点云筛选；

其中，N、M均为大于或等于2的正整数。

进一步地，N和M的取值均为6。

进一步地，计算所述目标像素点集合中，目标像素点组的误差值，具体包括：

分别计算所述目标像素点组在三维空间中，三个坐标方向的空间误差值；

分别计算所述目标像素点组的颜色误差值；

基于所述空间误差值和所述颜色误差值，计算所述目标像素点组的误差值。

进一步地，利用以下公式计算所述目标像素点组的误差值E：

其中：

分别代表RGB三个色彩分量的颜色误差值；

代表空间误差值；

k代表比例系数。

进一步地，利用以下公式，计算配准后的车身位姿矩阵RT：

其中：

n代表两个点云可匹配上的点对数量；

代表两幅三维点云图中一幅图的第i个点，

代表两幅三维点云图中另一幅图的第i个点；

R代表3*3的旋转矩阵，T代表3*1的平移矩阵，且对该公式求解得到所述车身位姿矩阵RT；

代表空间误差值。

本发明还提供一种基于点云配准的位姿计算系统，其特征在于，所述系统包括：

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

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

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

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

本发明还提供一种智能终端，所述智能终端包括：数据采集装置、处理器和存储器；

所述数据采集装置用于采集数据；所述存储器用于存储一个或多个程序指令；所述处理器，用于执行一个或多个程序指令，用以执行如上所述的方法。

本发明还提供一种计算机可读存储介质，所述计算机存储介质中包含一个或多个程序指令，所述一个或多个程序指令用于执行如上所述的方法。

本发明所提供的基于点云配准的位姿计算方法，通过获取目标区域的连续帧原始图像，并将相邻两帧原始图像分别转换为待配准的两幅三维点云图；对待配准的三维点云图进行点云筛选，以得到目标像素点集合；计算所述目标像素点集合中，目标像素点组的误差值，其中，所述像素点为组为待配准的两幅三维点云图中相配准的两个像素点；以所述误差值作为约束条件，计算配准后的车身位姿矩阵。这样，本发明所提供的位姿计算方法在三维点云基础上引入图像彩色信息，并且在进行ICP点云配准前，对点云有选择性的进行筛选，从而有效提升了ICP点云配准的准确性和鲁棒性，进而解决了现有技术中位姿计算准确性较差、鲁棒性较低的技术问题。

附图说明

为了更清楚地说明本发明的实施方式或现有技术中的技术方案，下面将对实施方式或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是示例性的，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图引伸获得其它的实施附图。

本说明书所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容得能涵盖的范围内。

图1为点云示意图；

图2为本发明所提供的基于点云配准的位姿计算方法一种具体实施方式的流程图；

图3为一个使用场景的点云图；

图4为图3所示使用场景的灰度图；

图5为筛选后的点云图；

图6为配准后的点云重叠效果图；

图7为本发明所提供的基于点云配准的位姿计算系统一种具体实施方式的结构框图。

具体实施方式

以下由特定的具体实施例说明本发明的实施方式，熟悉此技术的人士可由本说明书所揭露的内容轻易地了解本发明的其他优点及功效，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

针对现有技术中位姿计算准确性较差、鲁棒性较低的技术问题，本发明通过增加彩色信息和点云筛选两部分改进，并对两个改进点进行了融合，以解决传统ICP点云配准的弊端，并有效提升配准结果的准确性。

请参考图2，图2为本发明所提供的基于点云配准的位姿计算方法一种具体实施方式的流程图。

在一种具体实施方式中，如图2所示，本发明所提供的基于点云配准的位姿计算方法包括以下步骤：

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

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

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

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

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

根据所述目标像素点在所述目标图像中的位置进行路面点云筛选；具体地，在所述目标图像中，每隔N行和M列选择一个点作为目标像素点，以完成路面点云筛选；其中，N、M均为大于或等于2的正整数。

优选地，N和M的取值均为6。

下面以具体使用场景为例，简述点云筛选处理的原理和过程。

在进行点云配准时，如果直接将所有点进行点云配准，那么算法处理时间会非常长，很难保证实时性。另外如果全部点进行等权重的点云配准，那么一些点云数量占比较小但是很重要的物体就不会在配准过程中起到关键作用。以路面为例，路面上的车道线、斑马线、坑洼、石块、减速带等物体，由于存在物理特征（高度或深度）是非常理想的配准目标，但是其点云数量与路面点云数量相比的比例很小（只有1:100左右），因此如果直接进行配准，那么它们的物理特征会被数量更多路面点云淹没，使得最终结果存在偏差。

根据以上理论，只要提高非路面点与路面点的比例，那么配准结果将会更加准确，并且由于降低了路面点云数量，因此运算时间也可以大大降低。因此，首先需要根据图像获取路面小障碍物的图像位置，可以选择传统图像处理方法，但是由于路面情况复杂，传统图像处理方法很难囊括所有场景。本发明使用深度学习的语义分割进行目标检测，根据路面常见目标对训练数据进行标注，经过这些数据训练得到的模型，可以对输入图像较为准确的分割出路面的大部分目标。然后根据图像点对应的三维点，可以将原始三维点云进行筛选，以斑马线和井盖为例，保留下来的点云如图3所示，该位置灰度图像如图4所示。

但是仅保留路面障碍物点云会增加点云配准的自由度，导致配准结果不稳定，因此需要适当增加路面点云数量。本发明根据点云在图像中的位置进行路面点云筛选，具体策略是图像每隔6行和6列选择一个点，这样可以有效降低路面点云数量。同时增加路面物体的点云，整体效果如图5所示。

经过以上处理后，路面物体点云与路面点云比例为2:1左右，总的点云数量降低90%，将处理后的点云进行ICP点云配准，处理时间大幅度下降。且经过筛选后的点云，更加凸显路面物体，因此配准效果及鲁棒性也得到了保障。

在一些实施例中，计算所述目标像素点集合中，目标像素点组的误差值，具体包括：

分别计算所述目标像素点组的颜色误差值；

利用以下公式计算所述目标像素点组的误差值E：

其中：

分别代表RGB三个色彩分量的颜色误差值；

代表空间误差值；

k代表比例系数。

利用以下公式，计算配准后的车身位姿矩阵RT：

其中：

n代表两个点云可匹配上的点对数量；

代表两幅三维点云图中一幅图的第i个点，

代表两幅三维点云图中另一幅图的第i个点；

代表空间误差值。

下面仍以上述使用场景为例，简述增加彩色信息的原理和过程。

从原理上来讲，ICP点云配准是将两幅输入点云Ps（source）和Pt（target）进行配准，输出一个[R, T]矩阵，使得经过该矩阵变换后的Ps点云尽可能的与Pt点云重合。该过程可以抽象成求解以下公式最小值的求最优解问题：

公式中n代表两个点云可匹配上的点对数量，

代表source点云的第i个点，

代表target点云的第i个点，R代表3*3的旋转矩阵，T代表3*1的平移矩阵，Ed代表经过RT矩阵变换后，两个点云所有点在三维空间上xyz三个维度误差的总和。

这样，就找到使得

最小的RT矩阵，求解该方程的方式有很多，例如构造雅克比矩阵利用牛顿-高斯法进行求解等，相关求解方法比较成熟，本专利不做赘述。

从公式表达的意义来看，方程仅考虑了两个点云在三维空间上xyz距离的误差，并没有考虑每个点携带的颜色信息。这样在一些弱纹理场景，例如平直路面，就会出现配准结果错位的问题。如图6所示，两个点云计算得到RT矩阵后，source点云经过RT矩阵变换与target点云同时显示，存在一定的错位。

实际上，原始图像中地面存在斑马线，但是由于斑马线是贴在地面上，高度几乎为0，因此在点云配准过程中并没有提供有效信息。如果将斑马线以及普通路面的色彩信息也作为点云配准的输入信息，效果会有很大提升。

具体做法是在计算得到两个点在三维空间xyz三个方向的误差

时，同时计算这两个点RGB信息的误差，分别计算RGB三个信息分量差值的绝对值，然后取均值作为颜色配准结果

，乘以一定的比例系数k（该实施例中设置为0.1），然后Ed乘以比例系数（1-k），两者求和作为最终的误差，用如下公式表示。

式中

分别代表RGB三个色彩分量的误差值，

代表三维空间xyz三个方向的距离误差，即公式1中

的传统求解方法，k代表比例系数，本实施例设置为0.1。

以该场景为例，由于斑马线与路面的色彩信息不同，在进行点云配准过程中，斑马线上的三维点与路面上的三维点差异较大，因此可以很大程度上避免点云配准错位的情况。增加彩色信息后，重新计算得到RT矩阵后，source点云经过RT矩阵变换与target点云同时显示，错位情况得到消除。

在上述具体实施方式中，本发明所提供的基于点云配准的位姿计算方法，通过获取目标区域的连续帧原始图像，并将相邻两帧原始图像分别转换为待配准的两幅三维点云图；对待配准的三维点云图进行点云筛选，以得到目标像素点集合；计算所述目标像素点集合中，目标像素点组的误差值，其中，所述像素点为组为待配准的两幅三维点云图中相配准的两个像素点；以所述误差值作为约束条件，计算配准后的车身位姿矩阵。这样，本发明所提供的位姿计算方法在三维点云基础上引入图像彩色信息，并且在进行ICP点云配准前，对点云有选择性的进行筛选，从而有效提升了ICP点云配准的准确性和鲁棒性，进而解决了现有技术中位姿计算准确性较差、鲁棒性较低的技术问题。

除了上述方法，本发明还提供一种基于点云配准的位姿计算系统，如图7所示，所述系统包括：

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

目标点云获取单元200，用于对待配准的三维点云图进行点云筛选，以得到目标像素点集合；

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

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

在上述具体实施方式中，本发明所提供的基于点云配准的位姿计算系统，通过获取目标区域的连续帧原始图像，并将相邻两帧原始图像分别转换为待配准的两幅三维点云图；对待配准的三维点云图进行点云筛选，以得到目标像素点集合；计算所述目标像素点集合中，目标像素点组的误差值，其中，所述像素点为组为待配准的两幅三维点云图中相配准的两个像素点；以所述误差值作为约束条件，计算配准后的车身位姿矩阵。这样，本发明所提供的位姿计算系统在三维点云基础上引入图像彩色信息，并且在进行ICP点云配准前，对点云有选择性的进行筛选，从而有效提升了ICP点云配准的准确性和鲁棒性，进而解决了现有技术中位姿计算准确性较差、鲁棒性较低的技术问题。

与上述实施例相对应的，本发明实施例还提供了一种计算机存储介质，该计算机存储介质中包含一个或多个程序指令。其中，所述一个或多个程序指令用于被一种双目相机深度标定系统执行如上所述的方法。

在本发明实施例中，处理器可以是一种集成电路芯片，具有信号的处理能力。处理器可以是通用处理器、数字信号处理器（Digital Signal Processor，简称DSP）、专用集成电路（Application Specific工ntegrated Circuit，简称ASIC）、现场可编程门阵列（FieldProgrammable Gate Array，简称FPGA）或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。

可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。处理器读取存储介质中的信息，结合其硬件完成上述方法的步骤。

存储介质可以是存储器，例如可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。

其中，非易失性存储器可以是只读存储器（Read-Only Memory，简称ROM）、可编程只读存储器（Programmable ROM，简称PROM）、可擦除可编程只读存储器（Erasable PROM，简称EPROM）、电可擦除可编程只读存储器（Electrically EPROM，简称EEPROM）或闪存。

易失性存储器可以是随机存取存储器（Random Access Memory，简称RAM），其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器（Static RAM，简称SRAM）、动态随机存取存储器（Dynamic RAM，简称DRAM）、同步动态随机存取存储器（Synchronous DRAM，简称SDRAM）、双倍数据速率同步动态随机存取存储器（Double Data RateSDRAM，简称DDRSDRAM）、增强型同步动态随机存取存储器（EnhancedSDRAM，简称ESDRAM）、同步连接动态随机存取存储器（Synchlink DRAM，简称SLDRAM）和直接内存总线随机存取存储器（DirectRambus RAM，简称DRRAM）。

本发明实施例描述的存储介质旨在包括但不限于这些和任意其它适合类型的存储器。

本领域技术人员应该可以意识到，在上述一个或多个示例中，本发明所描述的功能可以用硬件与软件组合来实现。当应用软件时，可以将相应功能存储在计算机可读介质中或者作为计算机可读介质上的一个或多个指令或代码进行传输。计算机可读介质包括计算机存储介质和通信介质，其中通信介质包括便于从一个地方向另一个地方传送计算机程序的任何介质。存储介质可以是通用或专用计算机能够存取的任何可用介质。

以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的技术方案的基础之上，所做的任何修改、等同替换、改进等，均应包括在本发明的保护范围之内。