CN106296693B - 基于3d点云fpfh特征实时三维空间定位方法 - Google Patents

Abstract

一种基于3D点云FPFH特征的实时三维空间定位方法，包括如下步骤1)从深度摄像头获取3D点云数据；2)点云关键帧选取；3)点云预处理；4)特征描述：使用ISS算法得到点云的关键点，获取关键点的FPFH特征；5)点云配准：首先利用采样一致性初始配准算法对两片点云进行基于FPFH特征的初始配准，接着运用ICP算法对初始配准结果进行二次配；6)坐标转换：获得移动机器人三维空间坐标的变化矩阵，将当前点云的坐标通过变换矩阵转换到初始位置。7)重复重复1)～6)，随着机器人的移动计算得到相对于初始位置的机器人的坐标。本发明对于在光照恶劣或者完全黑暗的条件下的移动机器人实时定位具有较好的准确度。

Description

基于3D点云FPFH特征实时三维空间定位方法

技术领域

本发明涉及移动机器人定位领域，对于由于光照不稳定或者光线不足引起的特征提取不稳定导致无法实现定位功能有较大的弥补，对于定位的结果也更加实时和精确。

背景技术

定位是确定机器人在其作业环境中所处的位置的过程，更加具体的说就是利用先验环境地图的信息、当前的机器人位姿估计以及传感器的观测值等输入信息，经过一定的处理和变换，产生更加准确的对机器人的当前位置的估计。利用传感器感知的信息来获得可靠的定位是自主移动机器人最基本、最重要的一项功能，也是移动机器人研究中备受关注、富有挑战性的一个重要研究主题。

Kosaka等人尝试研发了一种基于环境几何描述的室内机器人导航***，通过CAD建模。在正常运行的情况下，通过相机输入图像，在生成期望场景的前提下，锁定特征搜索区域，直接使用Hough变换提取直线，再与CAD模型场景进行配准。迭代至期望与观察值最小误差为止，从而得到当前位置。

室外环境导航主要依赖于公路的特定信息，如斑马线，车道线等.。Navlab***采用颜色和纹理分割来识别道路，然后在通过识别检测障碍物。为了进一步提高视觉识别***的鲁棒性和可靠性，将神经网络和自适应控制规划车辆运动的技术融入其中。

基于尺度不变特性的SIFT算法被成功融合到了移动机器人的定位中。利用SIFT的尺度不变的特性，对两幅相近的图像特征配准，利用视差模型进行三维坐标换算得到机器人的移动位置。随着立体相机的出现，更多的研究人员，直接将双目立体相机或者RGB-D相机运用到该研究中，使得获得的定位结果更加实时和精确。

然而这些基于纹理特征的移动机器人定位方法,对光照极其敏感。光照的明暗直接影响算法的结果。

发明内容

为了克服机器人在光照恶劣或者完全黑暗的条件下的颜色信息获取不精确，导致配准结果存在较大的误差，使定位结果不准确的不足，本发明提供一种不受光照条件影响、精确度较高、运算的时空复杂度较低的基于3D点云FPFH特征实时三维空间定位方法，利用三维点云和三维局部特征对前后连续帧进行配准的方法，直接得到移动机器人的三维位点，能实时获取移动机器人的三维空间定位信息，该方法可以用于但不限于基于视觉的移动机器人三维空间定位。

本发明解决其技术问题所采用的技术方案是：

一种基于3D点云FPFH特征实时三维空间定位方法，包括如下步骤：

1)从深度摄像头获取3D点云数据；

2)点云关键帧选取。第一帧的时候把第一帧当成关键帧，剩下的关键帧选取方法是点云精确匹配后，匹配到的对应点的个数，通过阈值进行过滤；

3)点云预处理：首先对点云进行分割，分割后能够实时精确地给出点云中所有可能的平面；接着采用网格降采样方法对平面进行降采样和滤波；最后进行区域过滤，剔除关键点较少的区域；

4)特征描述：使用ISS算法得到点云的关键点，获取关键点的FPFH特征；

5)点云配准：首先利用采样一致性初始配准算法对两片点云进行基于FPFH特征的初始配准，接着运用ICP算法对初始配准结果进行二次配准，实现点云的精确配准；

6)坐标转换：获得移动机器人三维空间坐标的变化矩阵，将当前点云的坐标通过变换矩阵转换到初始位置。

7)重复重复1)～6)，随着机器人的移动计算得到相对于初始位置的机器人的坐标。

本发明的有益效果主要表现在：基于FPFH特征的实时三维空间定位算法，采用基于光照无关的依赖于3D形状特征描述子的FPFH特征，能够在动态场景中实时获取移动机器人的三维空间定位信息，同时关键帧的提取以及点云的预处理，大大降低了运算的时空复杂度，使该算法能更好地应用于实际环境中。

具体实施方式

下面对本发明做进一步说明。

一种基于3D点云FPFH特征实时三维空间定位方法，包括如下步骤：

1)从深度摄像头获取3D点云数据；

2)点云关键帧选取。第一帧的时候把第一帧当成关键帧，剩下的关键帧选取方法是点云精确匹配后，匹配到的对应点的个数，通过阈值进行过滤；

3)点云预处理：首先对点云进行分割，分割后能够实时精确地给出点云中所有可能的平面；接着采用网格降采样方法对平面进行降采样和滤波；最后进行区域过滤，剔除关键点较少的区域；

4)特征描述：使用ISS算法得到点云的关键点，获取关键点的FPFH特征；

5)点云配准：首先利用采样一致性初始配准算法对两片点云进行基于FPFH特征的初始配准，接着运用ICP算法对初始配准结果进行二次配准，进一步优化配准结果，实现点云的精确配准；

6)坐标转换：获得移动机器人三维空间坐标的变化矩阵，将当前点云的坐标通过变换矩阵转换到初始位置。

7)重复重复1)～6)，随着机器人的移动计算得到相对于初始位置的机器人的坐标。

所述步骤2)中，关键帧选取是定位算法中首先要解决的问题，我们的方法就是根据步骤5)中点云精确匹配后，匹配到的对应点的个数，通过阈值进行过滤。随着视场的变化，当前点云与关键帧之间能够匹配上的对应点的个数是呈减少的趋势；但是对于计算变换矩阵而言，计算得到的结果误差需要保证在一定的范围之内。因此在配准过程中需要保证足够多的配准点。假设最新点云与关键帧配准的对应点个数为C，当C>C_t时，通过对应点计算得到的转换矩阵误差在可接受范围之内。当C<C_t+C_r，将当前帧作为关键帧其中C_r表示波动范围，C_t，C_r作为先验值给出。

所述步骤3)的点云预处理过程如下：

为了在后续的环节中，能够高效执行，需要对点云进行预处理。首先对点云进行分割处理。分割后能够实时精确的给出点云中所有可能的平面，设第c_i个关键帧点云中分割的区域集合为将当前点云分割得到的区域集合R_j与进行匹配，将未能匹配上的区域从R_j中剔除。区域匹配规则表示如下：

R_j＝{P_k}，k∈[1,N] (1)

||S_k-S_l||＜t_s (4)

其中P_k表示在R_j区域集合内的其中一个点云子集，同理P_l也是区域集合内点云子集；表示平面P_k对应的平面法线，同理S_l为平面P_l的面积。为平面P_l对应的平面法线，S_k表示平面P_k的平面大小，通过点的个数表示；同理和t_s通过先验给出。计算各自区域内的平面法线方向的欧式距离，将法线的欧式距离和面积差在给定阈值内的平面作为匹配对。这样做可以提高配准的速度和准确性，因为在点云配准的过程中，只在限定的对应区域内进行匹配点的扫描。尽管这样做的后果是区域与区域之间边界上的关键点会丢失，但通过实验发现，丢失的关键点对匹配的影响不大。

在获取对应区域序列之后，各个区域内对平面进行降采样和滤波。本算法采用网格降采样，该方法的特点就是能够真实的反映情况，保持失真程度降到最低，同时能够剔除噪声。

区域过滤是进一步剔除某些区域，这些区域可能存在关键点，但是关键点数量很少，忽略这些关键点不影响最终结算结果。这样能尽可能减少不必要关键点和特征描述的计算。因此，我们的方法是将小于给定面积的区域直接舍弃。

所述步骤5)的点云配准，过程如下：

配准过程中，融合目前常用两中配准算法：用采样一致性进行初始配准，再用迭代最近点算法进行精确配准。

采样一致性初始配准算法(Sample Consensus Initial Alignment，SAC-IA)内部分为两部分：贪婪的初始对准方法和采样一致性方法。贪婪的初始对准方法使用点云内部旋转不变性的特征，因此具有非常强的鲁棒性。贪婪的初始对准算法计算复杂度较高并且有可能只能得到局部最有解，因此采样一致性方法，试图保持相同的对应的几何关系而不必尝试求解有限个对应关系的所有组合。

采样一致性初始配准算法(Sample consensus Initial Alignment，SAC-IA)：

5.1.1)从点云A中选择s个样本点，同时确定他们的配对距离大于用户设定的最小值d_min；

5.1.2)对s个样本点，在点云B中分别找到满足相似条件的点存入一个列表中，随机选择一些代表采样点的对应关系；

5.1.3)根据点云中关键点的对应关系计算刚体变换矩阵，并通过计算度量来评价转换矩阵的质量，度量由Huber评价公式决定：

重复这三个步骤直至达到最佳度量结果，最后使用一个Levenberg-Marquardt算法进行非线性局部优化；

由SAC-IA得到的刚体变换矩阵使得两个点云数据大致重合，但是配准精确度远远达不到实际工程应用的要求，因此在初始配准的基础上再精确配准。迭代最近点(Iterative Closest Point，ICP)算法是常用的精确的配准算法，它要求待配准的点云数据与较近的初始位置相比较。每次迭代过程中，首先根据一定的准则来对应点集P与Q，其中对应点对的个数为n。然后通过最小二乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小。

直至达到满意的误差要求。ICP迭代过程分为4个主要步骤：5.2.1)对原始点云进行采样；5.2.2)确定初始对应点集；5.2.3)去除错误对应点对；5.2.4)坐标变化求解。

所述步骤6)中，定位过程完全依赖于点云的配准过程，假设点云配准的精确和实时性要求能够完全满足移动机器人的动态定位精确的。我们通过设立关键帧，对实时定位算法进行优化。对于静态场景来说，机器人位置和当前视点的场景是相对固定的。假设将初始位置作为机器人开始移动的起点，相机坐标系和点云坐标系以及世界坐标系是重叠的。随着机器人的移动，相机坐标系也相应移动，此时的相机坐标系和世界坐标系是分离的。定位的目的就是将当前的世界坐标系中的信息换算到世界坐标系中。可见我们所提到的定位是一个相对初始位置的定位。

本实施例通过优化换算方法从而实现机器人的实时定位。假设在点云数据集合中{P_i}，存在k个关键帧点云由于点云配对是成对的，因此关键帧点云与当前帧点云之间按时间顺序存在k-1个旋转变换矩阵T_l。关键帧处在连续场景中的某一个视点，通过步骤2)筛选得到。该方法避免了直接计算两两3D点云之间旋转平移矩阵。如果场景足够大，这种大规模的膨胀过程，将消耗巨大的计算资源。通过设定关键帧，将旋转平移矩阵的个数压缩到最小，充分利用视场之间关联部分，从而实现实时定位的目的。

所述步骤7中，定位过程如下：

7.1)首选需要确定世界坐标系，我们将用传感器的第一帧坐标系作为世界坐标系，将相机坐标原点定义为世界坐标系原点。同时将第一帧作为关键帧，记为在第二个关键帧确定之前中间所有点云通过配准计算得到的旋转平移矩阵换算到世界坐标系下。

7.2)通过关键帧选取规则，产生第二帧关键帧，记为计算和的旋转平移矩阵T₁，那么和之间的所有点云记为P_i，先计算与之间的旋转平移矩阵，将P_i点云换算到坐标系下，记为P_i'，再将P_i'通过旋转平移矩阵T₁，换算到世界坐标系下。该过程的依次迭代，表达式如下：

P＝{p_i},i∈[1,n] (8)

Ti＝(R_i|t_i) (9)

其中P表示三维点p_i的集合，n表示点云数量，Ti表示4×4矩阵。将当前场景的坐标全部转换到世界坐标系中，从而得到定位的效果。

Claims (6)

1.一种基于3D点云FPFH特征实时三维空间定位方法，其特征在于：所述定位方法包括如下步骤：

1)从深度摄像头获取3D点云数据；

2)点云关键帧选取：第一帧的时候把第一帧当成关键帧，剩下的关键帧选取方法是先将点云进行精确匹配，再根据匹配的对应点的个数来确定关键帧，当对应点个数满足阈值条件则将当前帧作为关键帧；

3)点云预处理：首先对点云进行分割，分割后能够给出点云中所有可能的平面；接着采用网格降采样方法对平面进行降采样和滤波；最后进行区域过滤，剔除关键点小于给定数值的区域；

4)特征描述：使用ISS算法得到点云的关键点，获取关键点的FPFH特征；

5)点云配准：首先利用采样一致性初始配准算法对两片点云进行基于FPFH特征的初始配准，接着运用ICP算法对初始配准结果进行二次配准，实现点云的精确配准；

6)坐标转换：获得移动机器人三维空间坐标的变换 矩阵，将当前点云的坐标通过变换矩阵转换到初始位置；

7)重复重复1)～6)，随着机器人的移动计算得到相对于初始位置的机器人的坐标。

2.如权利要求1所述的基于3D点云FPFH特征实时三维空间定位方法，其特征在于：所述步骤2)中，假设最新点云与关键帧配准的对应点个数为C，当C>C_t时，通过对应点计算得到的转换矩阵误差在可接受范围之内，当C<C_t+C_r，将当前帧作为关键帧其中C_r表示波动范围，阈值C_t，C_r作为先验值给出。

3.如权利要求1或2所述的基于3D点云FPFH特征实时三维空间定位方法，其特征在于：所述步骤3)中，点云预处理过程如下：

首先对点云进行分割处理，分割后实时给出点云中所有可能的平面，设第c_i个关键帧点云中分割的区域集合为将当前帧点云分割得到的区域集合R_j与进行匹配，将未能匹配上的区域从R_j中剔除，区域匹配规则表示如下：

R_j＝{P_k}，k∈[1,N] (1)

||S_k-S_l||＜t_s (4)

其中P_k表示在R_j区域集合内的其中一个点云子集，P_l是区域集合内点云子集；表示平面P_k对应的平面法线，S_l为平面P_l的面积，为平面P_l对应的平面法线，S_k表示平面P_k的面积，通过点的个数表示；和t_s通过先验给出；

计算各自区域内的平面法线方向的欧式距离，将法线的欧式距离和面积差在给定阈值内的平面作为匹配对；

在获取对应区域序列之后，各个区域内对平面进行降采样和滤波。

4.如权利要求1或2所述的基于3D点云FPFH特征实时三维空间定位方法，其特征在于：所述步骤5)中，用采样一致性进行初始配准，过程如下：

5.1.1)从点云A中选择s个样本点，同时确定它 们的配对距离大于用户设定的最小值d_min；

5.1.2)对s个样本点，在点云B中分别找到满足相似条件的点存入一个列表中，随机选择一些代表采样点的对应关系；

5.1.3)根据点云中关键点的对应关系计算刚体变换矩阵，并通过计算度量来评价刚体变换矩阵的质量，度量由Huber评价公式决定：

重复这三个步骤直至达到最佳度量结果，最后使用一个Levenberg-Marquardt算法进行非线性局部优化；

运用ICP算法对初始配准结果进行二次配准，过程如下：每次迭代过程中，首先根据设定的准则来获取对应点集P与Q，其中对应点对的个数为n，然后通过最小二乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小：

直至达到满意的误差要求；

ICP迭代过程为：5.2.1)对原始点云进行采样；5.2.2)确定初始对应点集；5.2.3)去除错误对应点对；5.2.4)坐标变换 求解。

5.如权利要求1或2所述的基于3D点云FPFH特征实时三维空间定位方法，其特征在于：所述步骤6)中，通过优化换算方法实现机器人的实时定位，假设在点云数据集合{P_i}中，存在k个关键帧点云关键帧点云与当前帧点云之间按时间顺序存在k-1个旋转变换矩阵T_l，关键帧处在连续场景中的某一个视点，通过步骤2)筛选得到。

6.如权利要求1或2所述的基于3D点云FPFH特征实时三维空间定位方法，其特征在于：所述步骤7)中，定位过程如下：

7.1)首先确定世界坐标系，用传感器的第一帧坐标系作为世界坐标系，将相机坐标原点定义为世界坐标系原点，同时将第一帧作为关键帧，记为在第二个关键帧确定之前中间所有点云通过配准计算得到的旋转平移矩阵换算到世界坐标系下；

7.2)通过关键帧选取规则，产生第二帧关键帧，记为计算和的旋转平移矩阵T₁，和之间的所有点云记为P_i，先计算与之间的旋转平移矩阵，将P_i点云换算到坐标系下，记为P_i'，再将P_i'通过旋转平移矩阵T₁，换算到世界坐标系下，该过程的依次迭代，表达式如下：

P＝{P_i},i∈[1,n] (8)

T_i＝(R_i|t_i) (9)

其中P表示三维点云p_i的集合，n表示点云数量，T_i表示4×4矩阵；

将当前场景的坐标全部转换到世界坐标系中，从而得到定位结果。