CN110221603A - 一种基于激光雷达多帧点云融合的远距离障碍物检测方法 - Google Patents

Abstract

本发明公开了一种基于激光雷达多帧点云融合的远距离障碍物检测方法。建立局部坐标系和世界坐标系，根据局部坐标系下原始点云数据在激光雷达的环形扫描线上计算每个激光点的提取特征点，经过帧间特征点匹配以及地图特征点匹配得到当前位置相对于起始位置的全局位姿和世界坐标系下的去畸变点云；融合当前帧和之前帧的去畸变点云得到更致密的去畸变点云数据并统一到局部坐标系下，之后向二维栅格做投影，根据每个二维栅格的高度变化特征筛选出障碍物。本发明解决激光点云稀疏导致的远处障碍物检测率低的问题，能够有效检测出远距离障碍物，误检率和漏检率低，还可极大地降低系统成本。

Description

一种基于激光雷达多帧点云融合的远距离障碍物检测方法

技术领域

本发明涉及机器人障碍检测方法，特别是涉及面向无人车导航的一种基于激光雷达多帧点云融合的远距离障碍物检测方法。

背景技术

在无人车自主导航领域，传统的用于障碍物检测的方法有基于立体视觉的、基于结构光的、基于毫米波雷达和激光雷达的检测方法。基于立体视觉的检测方法，在图像中利用障碍物的颜色、边缘、纹理特征进行特征提取，与先验模型进行对比，提取出障碍物，之后利用同一物体在不同相机之间的视差信息，获取障碍物的深度，从而确定障碍物的位置。这种方法的缺点是计算资源消耗大，容易受光照影响，并且由于图像像素到真实物理空间的转换误差随距离增加，检测出的远距离障碍物的位置准确度低。基于结构光的检测方法，通过主动向物体发射红外光并接受返回的光，测出物体与相机之间的距离，这种方法无需消耗大量资源计算视差，但是易受日光干扰，并且这种方法的测量范围小，同样无法检测远距离物体。毫米波雷达利用多普勒效应，可以直接得到环境中运动障碍物的速度和距离信息，其测量范围远且对环境的适应性强，在低速自动驾驶场景已有广泛应用。但是毫米波雷达的探测角度小，无法实现对周围环境的全方位感知。三维激光雷达使用机械旋转的方式获取周围环境的点云数据，其不受光照等环境条件的影响，在水平角度上可以获取周围环境的全方位信息，并且距离精度高。因此成为无人车障碍检测和环境感知最有前途的传感器之一。

目前已有的激光雷达检测方法，大多使用旋转一圈所得到的单次扫描数据检测障碍物。由于垂直扫描线之间的夹角固定，随着距离的变远，扫描线之间的物理间隔变得越来越大。因此这类单帧检测方法无法得到两个扫描线之间的障碍物分布情况，并且由于远处的激光点云稀疏，无法可靠检测远距离的障碍物，存在大面积的检测盲区。为提高激光雷达的远距离障碍物检测能力，可以在车辆运动的同时将当前和之前采集到的多帧点云进行融合，得到更高密度的点云后再进行障碍物检测。现有的融合多帧点云的方法，大多借助GPS、IMU或里程计等外部设备测量车辆自身运动。此类方法增加了成本，并且当外部运动测量设备提供的信息不准确时，会错误融合多帧点云，造成大面积误检。

发明内容

在自动驾驶车辆行驶过程中，需要感知周围环境存在的建筑物、车辆、行人等影响车辆自主通行的障碍物。为了解决这个问题，解决背景技术中存在的问题，本发明提供了一种不依赖外部运动测量设备的基于激光雷达多帧点云融合的远距离障碍物检测方法，通过在融合后的高密度点云上做障碍检测，可以增大障碍物的检测距离，误检率和漏检率低。

本发明方法可解决单帧激光激光点云稀疏导致的无法有效检测远距离障碍物的问题。

本发明所述的障碍物检测方法，先在原始点云中根据每个点的曲率提取特征点，之后经过前后帧特征点匹配以及地图特征点匹配两个步骤，得到精准融合好的多帧点云数据。将匹配好的多帧点云数据向二维栅格地图做投影，根据每个栅格的高度变化特征，筛选得到障碍物位置。

本发明采用的具体技术方案包括如下步骤：

1)以激光雷达当前位置为原点建立局部坐标系，以激光雷达起始位置为原点建立世界坐标系，根据局部坐标系下激光雷达实时每帧所采集的原始点云数据在激光雷达的环形扫描线上依次计算每个激光点的曲率并提取特征点，经过帧间特征点匹配以及地图特征点匹配两个步骤得到当前位置相对于起始位置的全局位姿以及世界坐标系下的去畸变点云；

2)在世界坐标系下，融合当前帧和之前帧的去畸变点云得到更致密的去畸变点云数据，作为多帧全局去畸变点云，将多帧全局去畸变点云统一到局部坐标系下，并向二维栅格做投影，根据每个二维栅格的高度变化特征筛选出障碍物。

所述步骤1)是为了获得激光雷达当前位置相对于起始位置的全局位姿，以及去畸变点云，具体采用以下方式处理：

1.1)在障碍物检测过程中，激光雷达被固定于车辆上并跟随车辆一起运动，以车辆起始位置的激光雷达中心为原点建立世界坐标系，以车辆当前位置的激光雷达中心为原点建立局部坐标系L；

激光雷达水平旋转一圈所采集的点云数据构成了一帧原始点云，这一帧原始点云分布在垂直角度不同的多条环形扫描线上，每条环形扫描线上的点云数据是由一圈激光点构成；

在每条环形扫描线上按水平角度递增的顺序依次计算每个激光点的曲率，并根据曲率大小从当前帧原始点云中提取出若干特征点，特征点分为角点和平滑点，其中曲率大于平滑阈值的激光点为角点，曲率小于等于平滑阈值的激光点为平滑点；

1.2)对于当前帧原始点云中的每个特征点，在前一帧的第一去畸变点云中找到相对应的角点和平滑点，计算前后帧对应特征点的距离之和，通过非线性优化的方式迭代计算得到帧间位姿，并根据该帧间位姿消除数据采集过程中由于车辆运动导致的原始点云畸变，得到当前帧的第一去畸变点云；

1.3)累积步骤1.2)中的帧间位姿，得到激光雷达当前在世界坐标系中的全局位姿，建立特征点地图，对于当前帧第一去畸变点云中的每个特征点，在特征点地图中重新寻找对应的角点和平滑点并计算距离，通过非线性优化的方式迭代计算得到更精确的全局位姿，得到当前帧的第二去畸变点云。

所述步骤1.1)具体为：

1.1.1)计算激光点曲率

在同一环形扫描件线上，针对每个激光点采用以下方式处理获得曲率：

以当前激光点为中心，前后各取与当前激光点相邻的N₁个激光点，计算当前激光点的曲率：

其中，u、i均表示激光点在原始点云中的序号，x_u、y_u、z_u表示第u个激光点在局部坐标系下的三维坐标分量；x_i、y_i、z_i分别表示当前激光点在局部坐标系下的三维坐标分量，当前激光点为第i个激光点，N₁表示激光点序数取值范围；

1.1.2)挑选特征点

一帧原始点云有多条环形扫描线，位于同一环形扫描线上的激光点依照水平角度不同划分为N_f个区域，对于一个区域中的激光点，进行以下判断：若其曲率大于平滑阈值Ths_curv，则将其加入候选角点；若其曲率小于等于平滑阈值Ths_curv，则将其加入候选平滑点；在一个区域的所有候选角点中挑选曲率最大的N_c个点作为该区域的角点，在一个区域的所有候选平滑点中挑选曲率最小的N_s个点作为该区域的平滑点；对于一帧原始点云中的所有区域重复上述角点和平滑点筛选过程获得各个区域的角点与平滑点，所有区域中的角点构成当前帧的角点集合所有区域中的平滑点构成当前帧的平滑点集合

1.1.3)角点与平滑点作为特征点并用表示，表示为第k帧第i个激光点在局部坐标系L下的点向量，其中s_i表示从开始时刻到第i个激光点的采集时刻之间所占据一帧扫描的时间比例；激光雷达以机械旋转的方式采集数据，采用以下公式计算获得时间比例s_i：

其中，t_start表示每一帧原始点云扫描的开始时刻，t_end表示每一帧原始点云扫描的结束时刻，t_recv表示第i个激光点的采集时刻。

所述步骤1.2)具体为：

1.2.1)初始化帧间位姿

采集一帧原始点云的时间较短，在短时间内激光雷达运动的速度恒定，角速度恒定。建立各帧的帧间位姿设置第一帧间位姿的元素均设置为零，设置上一次帧间位姿作为当前帧的帧间位姿的初始值，用向量的形式表示为：

其中，表示当前第k帧的帧间位姿，为局部坐标系下激光雷达中心的位置，为局部坐标系下激光雷达中心的三种姿态角，分别为翻滚角roll、俯仰角pitch和偏航角yaw，t表示矩阵转置；

各个姿态角用旋转矩阵表示为：

其中，表示以翻滚角构成的翻滚角旋转矩阵，表示以俯仰角构成的俯仰角旋转矩阵，表示以偏航角构成的偏航角旋转矩阵。

每帧的局部坐标系方向定义如下：x轴指向车辆前方，y轴指向车辆左方/右方，z轴垂直地面向上。

1.2.2)然后采用以下方式分别针对角点和平滑点进行匹配

1.2.2.a)角点匹配

以第一帧的原始点云作为第一帧的第一去畸变点云；对于第二帧开始的每一帧，以当前帧角点集合中的一个角点根据帧间位姿和时间比例s_i，将角点转换到上一帧第一去畸变点云的局部坐标系中：

在上一帧角点集合中，根据欧式距离寻找离角点的最近点j表示最近点在第一去畸变点云中的序数，最近点位于垂直角度上的第b根环形扫描线上，则在序号为[b-index,b)∪(b,b+index]的环形扫描线范围内寻找角点的次近点index为搜索范围，l表示次近点在去畸变点云中的序数；

最近点与次近点连接构成一条连线，计算角点到上述连线的局部点线距离

1.2.2.b)平滑点匹配

以第一帧的原始点云作为第一帧的第一去畸变点云；对于第二帧开始的每一帧，以当前帧平滑点集合中的一个平滑点根据帧间位姿和时间比例s_i，将平滑点转换到上一帧第一去畸变点云的局部坐标系中：

在上一帧平滑点集合中，根据欧式距离寻找离平滑点的最近点j表示最近点在去畸变点云中的序数，最近点位于垂直角度上的第b根环形扫描线上，则在第b根环形扫描线上寻找次近点index为搜索范围，l表示次近点在去畸变点云中的序数；

然后在序号为[b-index,b)∪(b,b+index]的环形扫描线范围内寻找第三近点m表示第三近点在去畸变点云中的序数；不在同一直线上的三点最近点次近点和第三近点确定一个平面，计算平滑点到平面的局部点面距离

1.2.3)更新帧间位姿

将上述步骤1.2.2)的两种特征点作坐标转换并计算到对应直线或者对应平面距离的过程合并视为一个第一距离函数f:d^L表示角点到直线的距离和平滑点到平面的距离合并后的距离；对于当前第k帧原始点云中提取的所有n个特征点，得到如下第一距离函数：

…

其中，表示由帧间位姿计算的第n个特征点的第一距离函数，表示第n个特征点在局部坐标系L下的距离，n表示一帧点云中的特征点总数，n＝(N_c+N_s)×N_f；

然后计算上述第一距离函数的雅克比行列式

再计算帧间位姿变化量

其中，分别为帧间位姿变化量中的三轴方向位置变化量，分别为帧间位姿变化量中的三种姿态角变化量；

然后在原有帧间位姿基础上加上帧间位姿变化量进行更新；

1.2.4)迭代计算

重复步骤1.2.2)～1.2.3)进行迭代处理直至达到第一最大迭代次数IteraCountL，或满足如下公式：

其中，deltaTL为帧间位置终止阈值，deltaRL为帧间姿态终止阈值；

1.2.5)运动畸变去除

一帧原始点云中，扫描开始时刻的局部坐标系作为帧头坐标系，扫描结束时刻的局部坐标系作为帧尾坐标系；对于当前帧原始点云，依据帧间位姿以及时间比例s_i将所有激光点坐标转换到以t_start时刻为准的帧头坐标系中，再将所有点转换到以t_end时刻为准的帧尾坐标系，由此去除运动畸变得到当前帧的第一去畸变点云，并由其中去除运动畸变后的角点和平滑点分别构成了第一去畸变角点集合和第一去畸变平滑点集合实现去除运动畸变，供下一帧原始点云做特征点匹配使用。

所述步骤1.3)，具体为：

1.3.1)对于第一帧的激光雷达的齐次坐标变换矩阵M₁计算为：

对于第二帧以后的每一帧，根据前一帧的帧间位姿对当前帧的激光雷达位姿的齐次坐标变换矩阵M_k进行更新处理：

其中，M_k表示第k帧激光雷达位姿的齐次坐标变换矩阵，每一帧的激光雷达位姿的齐次坐标变换矩阵M_k是和当前帧激光雷达在世界坐标系中的全局位姿相一一对应，齐次坐标变换矩阵M_k表示从局部坐标到世界坐标的转化， 为世界坐标系下激光雷达中心的位置，为世界坐标系下激光雷达中心的三种姿态角；

1.3.2)特征点地图匹配

特征点地图分为角点地图和平滑点地图；

1.3.2.a)角点地图分配

建立长方体形的角点地图Map_c，以当前激光雷达中心为长方体的中心，角点地图的范围大小设定为长length、宽width和高height，将所有帧原始点云中落在角点地图Map_c之内的角点加入角点地图Map_c，将所有帧原始点云中未落在角点地图Map_c之内的角点不加入角点地图Map_c；

对于第一去畸变角点集合中的每一角点均按照如下处理：将角点转换到世界坐标系下得到去畸变角点在角点地图Map_c中，寻找距离去畸变角点最近的五个角点，计算五个角点之间的质心坐标为然后计算协方差分量并组成协方差矩阵A：

其中，a_xx表示x分量自相关系数，a_yy表示y分量自相关系数，a_zz表示z分量自相关系数，a_xy表示xy分量互相关系数，a_xz表示xz分量互相关系数,a_yz表示yz分量互相关系数；分别表示质心坐标在世界坐标系下的三维坐标；

采用以下公式计算获得协方差矩阵A的特征值和特征向量：

Av₁＝λ₁v₁Av₂＝λ₂v₂Av₃＝λ₃v₃

其中，v₁、v₂、v₃表示第一、第二、第三特征向量，λ₁、λ₂、λ₃表示第一、第二、第三特征值；且特征值按降序排列为λ₁≥λ₂≥λ₃，λ₁、λ₂、λ₃对应的特征向量分别为v₁、v₂、v₃；

若满足λ₁≥3λ₂，则距离最近的五个点呈直线分布，再计算该直线的单位向量则用以下两个三维点表示该直线：

其中，p_s为起始三维点，p_e为结束三维点；

计算去畸变角点到两个三维点p_s和p_e所形成直线的全局点线距离

若不满足λ₁≥3λ₂，则针对该去畸变角点不计算距离。

1.3.2.b)平滑点地图匹配

建立长方体形的平滑点地图Map_s，以当前激光雷达中心为长方体的中心，平滑点地图的范围大小设定为长length、宽width和高height，将所有帧原始点云中落在平滑点地图Map_c之内的平滑点加入平滑点地图Map_s，将所有帧原始点云中未落在平滑点地图Map_c之内的平滑点不加入平滑点地图Map_s；

对于第一去畸变平滑点集合中的每一平滑点均按照如下处理：将平滑点转换到世界坐标系下得到去畸变平滑点在平滑点地图Map_s中，寻找距离去畸变平滑点最近的五个平滑点，由该五个平滑点拟合处理建立拟合平面，分别计算上述五个平滑点分别到拟合平面的五个距离，并进行以下判断：若五个距离均小于平面拟合阈值DisThs，则认为满足五个点的平面分布，计算去畸变平滑点到该拟合平面的全局点面距离若五个距离其中任一一个不小于平面拟合阈值DisThs，则该去畸变平滑点不计算距离；

1.3.3)更新世界位姿

角点以及平滑点特征匹配，均是根据当前估计的世界坐标计算特征点到拟合直线或拟合平面的距离。将上述步骤1.3.2)的两种特征点处理并计算到拟合直线或拟合平面的过程合并视为一个第二距离函数：

其中，表示由全局位姿计算的第二距离函数，d^W表示在全局坐标系W下特征点到拟合直线或拟合平面的距离；

采用与帧间匹配中的过程相同方式，使用非线性优化的方式更新世界坐标系下位姿的估计值：

其中，h表示第二距离函数对全局位姿求导得到的雅克比矩阵；

再计算全局位姿变化量

其中，分别为全局位姿变化量中的三轴方向位置变化量，分别为全局位姿变化量中的三种姿态角变化量；

然后在原有全局位姿基础上加上全局位姿变化量进行更新；

1.3.4)迭代计算

重复步骤1.3.2)～1.3.3)进行迭代处理直至达到第二最大迭代次数IterCountW，或满足如下公式：

其中，deltaTW为全局位置终止阈值，deltaRW为全局姿态终止阈值；

1.3.5)根据求出的全局位姿对局部坐标系下的第一去畸变点云做转换，得到世界坐标系下的去畸变点云作为第二去畸变点云。

1.3.6)更新特征点地图

因为激光雷达一直在跟随车辆运动，局部坐标系的原点也在发生变化，所以需要更新特征点地图。在激光雷达随车辆运动过程中，

对于角点地图和平滑点地图中的特征点分别进行实时更新，将位于地图范围内的特征点保留，并将实时加入到地图范围内的特征点加入特征点地图，否则其余特征点均抛弃。

将实时的特征点地图划分为体素栅格，再做降采样，每个体素栅格是边长为VoxelSize的立方体，计算落在同一个体素栅格中所有角点和所有平滑点各自的质心，用质心代替同一个体素栅格中所有角点/所有平滑点，作为更新后的角点和平滑点分别对应新的特征点，一个体素栅格具有两个质心，两个质心分别为角点质心和平滑点质心，用该质心表示体素栅格内所有点的分布情况。

所述步骤2)，具体为：

2.1)多帧激光点云融合

针对每一当前帧，如第k帧，由步骤1)获得当前帧的世界坐标系下的去畸变点云叠加融合上当前帧以前的N_fusion帧点云，得到多帧全局去畸变点云P^W，这样能得到周围环境的更精确数据：

其中，N_fusion表示叠加的点云帧数量，q表示叠加的点云帧序数；

2.2)点云坐标系转换

在当前帧的局部坐标系下，构建栅格地图，对于多帧全局去畸变点云P^W中的每个激光点v表示多帧全局去畸变点云P^W中的激光点的序数， 分别表示第v个激光点在全局坐标系下的三维分量坐标，根据步骤1)中得到的全局位姿进行转换处理得到局部坐标系下的激光点 分别表示第v个激光点在局部坐标系下的三维坐标分量；

当局部坐标系下的激光点满足如下要求时，将其加入多帧局部点云集合P^L：

其中，DistValid是有效距离阈值；

这样能消除在激光雷达数据采集过程中，部分激光点会照射到车辆上对障碍物检测造成的干扰，去除这部分点的影响。

2.3)点云数据栅格化

在局部坐标系的xy轴平面上建立划分二维栅格，以z轴方向作为高度值，使用二维栅格的方式描述激光雷达周围的障碍物分布情况，在x轴方向建立row行栅格，在y轴方向建立col列栅格，每个二维栅格是边长为gridSize的正方形，每个二维栅格记录落在二维栅格中所有激光点的最大高度值H_max和最小高度值H_min并作为高度分布特征；对于多帧局部点云集合P^L做上述处理，得到二维栅格中的高度分布特征；

2.4)筛选障碍栅格

在所有row×col个二维栅格中，根据落入二维栅格中的激光点云的分布情况，将二维栅格属性按照以下方式判断为未知、可通行、障碍物三种：若二维栅格中没有激光点落入，则二维栅格属性值为未知；若二维栅格中至少有一个点落入，且满足H_max-H_min<H_ths，H_ths表示障碍物高度阈值，则二维栅格属性为可通行；若二维栅格中至少有一个点落入，且满足H_max-H_min≥H_ths，则二维栅格属性为障碍物；所有被标记为障碍物的二维栅格所在位置，作为使用融合后的多帧局部点云检测出的障碍物所在位置。

与背景技术相比较，本发明具有的有益效果是：

1)使用三维激光雷达检测障碍物，不受光照等环境因素影响，可以准确检出障碍物所在位置，可实现水平角度全方位检测，范围广；

2)单帧激光点云在远处比较稀疏，多帧融合后的点云对于障碍物的检测距离更远，并且误检率和漏检率低；

3)通过多帧点云融合检测，可以利用低线数的激光雷达实现以往高线数的雷达才能达到的障碍物检测距离，降低了系统成本；

4)点云匹配过程不引入外部运动测量传感器，成本低。

综合来说，本发明可解决激光点云稀疏导致的远处障碍物检测率低的问题，能够有效检测出远距离障碍物，误检率和漏检率低，还可以仅仅利用低扫描线数的激光雷达实现以往需要高线数的雷达才能达到的远距离障碍物检测效果，极大地降低系统成本。

附图说明

图1展示了特征点提取过程，灰色的点是原始点云，黑色的点是特征点。

图2展示了点云融合过程，正上方是实际采集场景的图像，左下方是当前场景的单帧原始点云，右下方是多帧局部去畸变点云。在图像与点云图中，使用灰色矩形框标出了一辆汽车的位置。可以看出多帧局部去畸变点云更加密集，并且对于车辆这类障碍物体的细节描述更加丰富。

图3、图4、图5、图6是使用公开数据集KITTI展示的多个实际场景的障碍物处理结果图。每张图中正上方是实际场景图，下面两张是经过本障碍物检测方法处理后输出的栅格图。左下为单帧障碍检测结果，右下为多帧点云融合后的障碍检测结果。在实际场景图中以及障碍栅格图中，使用灰色矩形标出了远处一辆车的位置，在四个场景中均能看到，因为单帧点云较为稀疏，车辆被误检为可通行区域；而经过多帧融合后的点云可以正常将车辆检测为障碍物。

图7是使用自行采集数据验证的障碍物检测结果，左边为真实环境场景图片，右边为多帧点云融合后的障碍检测结果。

具体实施方式

下面结合附图和实例对本发明做进一步说明。

本发明使用了公开数据集KITTI提供的图片以及激光激光点云数据，作障碍物检测实施步骤以及效果说明。

步骤一：对单帧原始点云实施本发明之1.1)，计算每个点的曲率并提取特征点，其中N₁取值5，Ths_curv取值0.1。得到的特征点如图1所示，其中较小的为原始点云，较大的为特征点。

步骤二：对单帧原始点云实施本发明之1.2)，通过帧间特征点匹配求出帧间位姿，并对原始点云做运动畸变去除，得到帧尾坐标系下的第一去畸变点云。其中IterCountL取值25，deltaTL取值0.1，deltaRL取值0.1。

步骤三：对步骤二得到的第一去畸变点云实施本发明之1.3)，在特征点地图中进行特征点匹配，得到全局位姿以及世界坐标系下的第二去畸变点云，更新特征点地图。其中IterCountW取值为10，deltaTW取值为0.05，deltaRW取值为0.05。

步骤四：对步骤三得到的世界坐标系下的第二去畸变点云实施本发明之2.1)～2.2)，在世界坐标系下融合多帧全局去畸变点云，并将点云转换到当前局部坐标系下，得到多帧局部去畸变点云。其中N_fusion取值为10，DistValid取值为2.85。如图2右下所示为融合后的多帧局部去畸变点云。

步骤五：对步骤四得到的融合后的多帧局部去畸变点云数据实施本发明之2.3)～2.4)，将点云数据做栅格化，并根据二维栅格高度特征，筛选出影响车辆通行的二维栅格。其中row取600，col取400，gridSize取20，H_ths取值为30。图4右下角为最终检测得出的代表障碍物位置的栅格图。

图3、图4、图5、图6测试了KITTI数据集中四个典型场景的使用效果，每个子图中，上方为真实场景的图像，左下为单帧原始点云障碍物检测的结果，右下使用多帧局部去畸变点云检测的结果。在每个场景中，均使用灰色矩形框标出了远处车辆的位置，该车辆使用单帧原始点云数据无法被检测出，使用多帧局部去畸变点云可以被检测出。在表1中对各个场景的检测结果做了统计，其中总帧数f₀为目标车辆出现在图像中，并且有激光点打到车辆上的数据帧；成功帧数f₁为在栅格图中顺利检测出目标车辆的数据帧；误检帧数f₂为在栅格图中出现了明显的将可通行区域标为了障碍的数据帧；漏检帧数f₃为目标车辆所在栅格属性被标为可通行的数据帧。表中的首次检测距离为目标车辆第一次被检测出时，距离激光雷达中心的位置，成功率＝f₁/f₀，误检率＝f₂/f₀，漏检率＝f₃/f₀。

图3、图4、图5、图6是使用64线激光雷达的障碍物检测结果，图7是使用32线雷达的障碍物检测结果，对比两者可以看出使用本发明方法的32线雷达在探测栅格数量以及障碍物检测距离上，可以达到64线雷达的效果。

表1

由上述实施可看出，本发明提出的方法，检测距离远，检测成功率高，误检率和漏检率低，通过使用低线数激光雷达就可以实现以往使用高线数激光雷达才能达到的障碍物检测效果，具有降低系统成本的优点。

Claims (6)

1.一种基于激光雷达多帧点云融合的远距离障碍物检测方法，其特征在于包括如下步骤：

1)以激光雷达当前位置为原点建立局部坐标系，以激光雷达起始位置为原点建立世界坐标系，根据局部坐标系下激光雷达实时每帧所采集的原始点云数据在激光雷达的环形扫描线上依次计算每个激光点的曲率并提取特征点，经过帧间特征点匹配以及地图特征点匹配两个步骤得到当前位置相对于起始位置的全局位姿以及世界坐标系下的去畸变点云；

2)在世界坐标系下，融合当前帧和之前帧的去畸变点云得到更致密的去畸变点云数据，作为多帧全局去畸变点云，将多帧全局去畸变点云统一到局部坐标系下，并向二维栅格做投影，根据每个二维栅格的高度变化特征筛选出障碍物。

2.根据权利要求1所述的一种基于激光雷达多帧点云融合的远距离障碍物检测方法，其特征在于：所述步骤1)具体采用以下方式处理：

1.1)在障碍物检测过程中，激光雷达被固定于车辆上并跟随车辆一起运动，以车辆起始位置的激光雷达中心为原点建立世界坐标系，以车辆当前位置的激光雷达中心为原点建立局部坐标系L；

激光雷达水平旋转一圈所采集的点云数据构成了一帧原始点云，这一帧原始点云分布在垂直角度不同的多条环形扫描线上；

在每条环形扫描线上按水平角度递增的顺序依次计算每个激光点的曲率，并根据曲率大小从当前帧原始点云中提取出若干特征点，特征点分为角点和平滑点，其中曲率大于平滑阈值的激光点为角点，曲率小于等于平滑阈值的激光点为平滑点；

1.2)对于当前帧原始点云中的每个特征点，在前一帧的第一去畸变点云中找到相对应的角点和平滑点，计算前后帧对应特征点的距离之和，通过非线性优化的方式迭代计算得到帧间位姿，并根据该帧间位姿消除数据采集过程中由于车辆运动导致的原始点云畸变，得到当前帧的第一去畸变点云；

1.3)累积步骤1.2)中的帧间位姿，得到激光雷达当前在世界坐标系中的全局位姿，建立特征点地图，对于当前帧第一去畸变点云中的每个特征点，在特征点地图中重新寻找对应的角点和平滑点并计算距离，通过非线性优化的方式迭代计算得到更精确的全局位姿，得到当前帧的第二去畸变点云。

3.根据权利要求2所述的一种基于激光雷达多帧点云融合的远距离障碍物检测方法，其特征在于：所述步骤1.1)具体为：

1.1.1)计算激光点曲率

在同一环形扫描件线上，针对每个激光点采用以下方式处理获得曲率：

以当前激光点为中心，前后各取与当前激光点相邻的N₁个激光点，计算当前激光点的曲率：

其中，u、i均表示激光点在原始点云中的序号，x_u、y_u、z_u表示第u个激光点在局部坐标系下的三维坐标分量；x_i、y_i、z_i分别表示当前激光点在局部坐标系下的三维坐标分量，当前激光点为第i个激光点，N₁表示激光点序数取值范围；

1.1.2)挑选特征点

一帧原始点云有多条环形扫描线，位于同一环形扫描线上的激光点依照水平角度不同划分为N_f个区域，对于一个区域中的激光点，进行以下判断：若其曲率大于平滑阈值Ths_curv，则将其加入候选角点；若其曲率小于等于平滑阈值Ths_curv，则将其加入候选平滑点；在一个区域的所有候选角点中挑选曲率最大的N_c个点作为该区域的角点，在一个区域的所有候选平滑点中挑选曲率最小的N_s个点作为该区域的平滑点；对于一帧原始点云中的所有区域重复上述角点和平滑点筛选过程获得各个区域的角点与平滑点，所有区域中的角点构成当前帧的角点集合所有区域中的平滑点构成当前帧的平滑点集合

1.1.3)角点与平滑点作为特征点并用表示，表示为第k帧第i个激光点在局部坐标系L下的点向量，其中s_i表示从开始时刻到第i个激光点的采集时刻之间所占据一帧扫描的时间比例；激光雷达以机械旋转的方式采集数据，采用以下公式计算获得时间比例s_i：

其中，t_start表示每一帧原始点云扫描的开始时刻，t_end表示每一帧原始点云扫描的结束时刻，t_recv表示第i个激光点的采集时刻。

4.对于权利要求2所述的一种基于激光雷达多帧点云融合的远距离障碍物检测方法，其特征在于：所述步骤1.2)具体为：

1.2.1)初始化帧间位姿

建立各帧的帧间位姿设置第一帧间位姿的元素均设置为零，设置上一次帧间位姿作为当前帧的帧间位姿的初始值表示为：

其中，表示当前第k帧的帧间位姿，为局部坐标系下激光雷达中心的位置，为局部坐标系下激光雷达中心的三种姿态角，分别为翻滚角roll、俯仰角pitch和偏航角yaw，t表示矩阵转置；

各个姿态角用旋转矩阵表示为：

其中，表示以翻滚角构成的翻滚角旋转矩阵，表示以俯仰角构成的俯仰角旋转矩阵，表示以偏航角构成的偏航角旋转矩阵；

1.2.2)然后采用以下方式分别针对角点和平滑点进行匹配

1.2.2.a)角点匹配

以第一帧的原始点云作为第一帧的第一去畸变点云；对于第二帧开始的每一帧，以当前帧角点集合中的一个角点根据帧间位姿和时间比例s_i，将角点转换到上一帧第一去畸变点云的局部坐标系中：

在上一帧角点集合中，根据欧式距离寻找离角点的最近点j表示最近点在第一去畸变点云中的序数，最近点位于第b根环形扫描线上，则在序号为[b-index,b)∪(b,b+index]的环形扫描线范围内寻找角点的次近点index为搜索范围，l表示次近点在去畸变点云中的序数；

最近点与次近点连接构成一条连线，计算角点到上述连线的局部点线距离

1.2.2.b)平滑点匹配

以第一帧的原始点云作为第一帧的第一去畸变点云；对于第二帧开始的每一帧，以当前帧平滑点集合中的一个平滑点根据帧间位姿和时间比例s_i，将平滑点转换到上一帧第一去畸变点云的局部坐标系中：

在上一帧平滑点集合中，根据欧式距离寻找离平滑点的最近点j表示最近点在去畸变点云中的序数，最近点位于第b根环形扫描线上，则在第b根环形扫描线上寻找次近点index为搜索范围，l表示次近点在去畸变点云中的序数；

然后在序号为[b-index,b)∪(b,b+index]的环形扫描线范围内寻找第三近点m表示第三近点在去畸变点云中的序数；不在同一直线上的三点最近点次近点和第三近点确定一个平面，计算平滑点到平面的局部点面距离

1.2.3)更新帧间位姿

将上述步骤1.2.2)的两种特征点作坐标转换并计算到对应直线或者对应平面距离的过程合并视为一个第一距离函数f:d^L表示角点到直线的距离和平滑点到平面的距离合并后的距离；对于当前第k帧原始点云中提取的所有n个特征点，得到如下第一距离函数：

…

其中，表示由帧间位姿计算的第n个特征点的第一距离函数，表示第n个特征点在局部坐标系L下的距离，n表示一帧点云中的特征点总数，n＝(N_c+N_s)×N_f；

然后计算上述第一距离函数的雅克比行列式

再计算帧间位姿变化量

其中，分别为帧间位姿变化量中的三轴方向位置变化量，分别为帧间位姿变化量中的三种姿态角变化量；

然后在原有帧间位姿基础上加上帧间位姿变化量进行更新；

1.2.4)迭代计算

重复步骤1.2.2)～1.2.3)进行迭代处理直至达到第一最大迭代次数IteraCountL，或满足如下公式：

其中，deltaTL为帧间位置终止阈值，deltaRL为帧间姿态终止阈值；

1.2.5)运动畸变去除

一帧原始点云中，扫描开始时刻的局部坐标系作为帧头坐标系，扫描结束时刻的局部坐标系作为帧尾坐标系；

对于当前帧原始点云，依据帧间位姿以及时间比例s_i将所有激光点坐标转换帧头坐标系中，再将所有点转换到以t_end时刻为准的帧尾坐标系，由此去除运动畸变得到当前帧的第一去畸变点云，并由其中去除运动畸变后的角点和平滑点分别构成了第一去畸变角点集合和第一去畸变平滑点集合供下一帧原始点云做特征点匹配使用。

5.对于权利要求2所述的一种基于激光雷达多帧点云融合的远距离障碍物检测方法，其特征在于：所述步骤1.3)，具体为：

1.3.1)对于第一帧的激光雷达的齐次坐标变换矩阵M₁计算为：

对于第二帧以后的每一帧，根据前一帧的帧间位姿对当前帧的激光雷达位姿的齐次坐标变换矩阵M_k进行更新处理：

其中，M_k表示第k帧激光雷达位姿的齐次坐标变换矩阵，每一帧的激光雷达位姿的齐次坐标变换矩阵M_k是和当前帧激光雷达在世界坐标系中的全局位姿相一一对应， 为世界坐标系下激光雷达中心的位置，为世界坐标系下激光雷达中心的三种姿态角；

1.3.2)特征点地图匹配

特征点地图分为角点地图和平滑点地图；

1.3.2.a)角点地图分配

建立长方体形的角点地图Map_c，以当前激光雷达中心为长方体的中心，角点地图的范围大小设定为长length、宽width和高height，将所有帧原始点云中落在角点地图Map_c之内的角点加入角点地图Map_c；

对于第一去畸变角点集合中的每一角点均按照如下处理：

将角点转换到世界坐标系下得到去畸变角点在角点地图Map_c中，寻找距离去畸变角点最近的五个角点，计算五个角点之间的质心坐标为然后计算协方差分量并组成协方差矩阵A：

其中，a_xx表示x分量自相关系数，a_yy表示y分量自相关系数，a_zz表示z分量自相关系数，a_xy表示xy分量互相关系数，a_xz表示xz分量互相关系数，a_yz表示yz分量互相关系数；分别表示质心坐标在世界坐标系下的三维坐标；

采用以下公式计算获得协方差矩阵A的特征值和特征向量：

Av₁＝λ₁v₁ Av₂＝λ₂v₂ Av₃＝λ₃v₃

其中，v₁、v₂、v₃表示第一、第二、第三特征向量，λ₁、λ₂、λ₃表示第一、第二、第三特征值；且特征值按降序排列为λ₁≥λ₂≥λ₃，λ₁、λ₂、λ₃对应的特征向量分别为v₁、v₂、v₃；

若满足λ₁之3λ₂，则距离最近的五个点呈直线分布，再计算该直线的单位向量则用以下两个三维点表示该直线：

其中，p_s为起始三维点，p_e为结束三维点；

计算去畸变角点到两个三维点p_s和p_e所形成直线的全局点线距离

1.3.2.b)平滑点地图匹配

建立长方体形的平滑点地图Map_s，以当前激光雷达中心为长方体的中心，平滑点地图的范围大小设定为长length、宽width和高height，将所有帧原始点云中落在平滑点地图Map_c之内的平滑点加入平滑点地图Map_s；对于第一去畸变平滑点集合中的每一平滑点均按照如下处理：将平滑点转换到世界坐标系下得到去畸变平滑点在平滑点地图Map_s中，寻找距离去畸变平滑点最近的五个平滑点，由该五个平滑点拟合处理建立拟合平面，分别计算上述五个平滑点分别到拟合平面的五个距离，并进行以下判断：若五个距离均小于平面拟合阈值DisThs，则认为满足五个点的平面分布，计算去畸变平滑点到该拟合平面的全局点面距离

1.3.3)更新世界位姿

将上述步骤1.3.2)的两种特征点处理并计算到拟合直线或拟合平面的过程合并视为一个第二距离函数：

其中，表示由全局位姿计算的第二距离函数，d^W表示在全局坐标系W下特征点到拟合直线或拟合平面的距离；

使用非线性优化的方式更新世界坐标系下位姿的估计值：

其中，h表示第二距离函数对全局位姿求导得到的雅克比矩阵；

再计算全局位姿变化量

其中，分别为全局位姿变化量中的三轴方向位置变化量，分别为全局位姿变化量中的三种姿态角变化量；

然后在原有全局位姿基础上加上全局位姿变化量进行更新；

1.3.4)迭代计算

重复步骤1.3.2)～1.3.3)进行迭代处理直至达到第二最大迭代次数IterCountW，或满足如下公式：

其中，deltaTW为全局位置终止阈值，deltaRW为全局姿态终止阈值；

1.3.5)根据求出的全局位姿对局部坐标系下的第一去畸变点云做转换，得到世界坐标系下的去畸变点云作为第二去畸变点云；

1.3.6)更新特征点地图

在激光雷达随车辆运动过程中，对于角点地图和平滑点地图中的特征点分别进行实时更新，将位于地图范围内的特征点保留，并将实时加入到地图范围内的特征点加入特征点地图，否则抛弃。

6.根据权利要求1所述的一种基于激光雷达多帧点云融合的远距离障碍物检测方法，其特征在于：所述步骤2)，具体为：

2.1)多帧激光点云融合

针对每一当前帧，由步骤1)获得当前帧的世界坐标系下的去畸变点云叠加融合上当前帧以前的N_fusion帧点云，得到多帧全局去畸变点云P^W：

其中，N_fusion表示叠加的点云帧数量，q表示叠加的点云帧序数；

2.2)点云坐标系转换

在当前帧的局部坐标系下，构建栅格地图，对于多帧全局去畸变点云P^W中的每个激光点v表示多帧全局去畸变点云P^W中的激光点的序数， 分别表示第v个激光点在全局坐标系下的三维分量坐标，根据步骤1)中得到的全局位姿进行转换处理得到局部坐标系下的激光点 分别表示第v个激光点在局部坐标系下的三维坐标分量；

当局部坐标系下的激光点满足如下要求时，将其加入多帧局部点云集合P^L：

其中，DistValid是有效距离阈值；

2.3)点云数据栅格化

在局部坐标系的xy轴平面上建立划分二维栅格，以z轴方向作为高度值，在x轴方向建立row行栅格，在y轴方向建立col列栅格，每个二维栅格是边长为gridSize的正方形，每个二维栅格记录落在二维栅格中所有激光点的最大高度值H_max和最小高度值H_min并作为高度分布特征；对于多帧局部点云集合P^L做上述处理，得到二维栅格中的高度分布特征；

2.4)筛选障碍栅格

在所有row×col个二维栅格中，根据落入二维栅格中的激光点云的分布情况，将二维栅格属性按照以下方式判断为未知、可通行、障碍物三种：

若二维栅格中没有激光点落入，则二维栅格属性值为未知；

若二维栅格中至少有一个点落入，且满足H_max-H_min＜H_ths，H_ths表示障碍物高度阈值，则二维栅格属性为可通行；

若二维栅格中至少有一个点落入，且满足H_max-H_min≥H_ths，则二维栅格属性为障碍物；

所有被标记为障碍物的二维栅格所在位置，作为使用融合后的多帧局部点云检测出的障碍物所在位置。