一种基于核函数的实时地形估计方法

摘要

本发明提供一种基于核函数的实时地形估计方法，具体过程为：获取实时点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵；利用所述旋转平移矩阵对实时点云数据进行配准，并对配准后的点云数据采用基于体素的降采样处理；遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩阵MAP；利用实时点云数据对所述地形矩阵MAP进行点约束和光线约束，将此时获得的地形矩阵MAP作为当前无人车周围的地形估计；将所述配准后的实时点云数据添加到历史点云库中，更新历史点云数据。该方法能适用于无人车高实时性、海量数据的地形估计，解决运动中盲区的补偿，并给出可以在线更新的显式估计结果。

权利要求

1.一种基于核函数的实时地形估计方法，其特征在于，具体过程为：步骤一，获取实时点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵；

步骤二，利用所述旋转平移矩阵对实时点云数据进行配准，并对配准后的点云数据采用基于体素的降采样处理；

步骤三，遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩阵MAP；

步骤四、利用实时点云数据对所述地形矩阵MAP进行点约束和光线约束，将此时获得的地形矩阵MAP作为当前无人车周围的地形估计；

步骤五、将所述配准后的实时点云数据添加到历史点云库中，更新历史点云数据；

所述点约束的过程为：

(1)选取被用于建立点约束的一个点QHj，判断QHj是否在地形矩阵MAP的覆盖范围之内，如果在，则根据点QHj的坐标(xHj，yHj)从地形矩阵MAP中找到离QHj最近的元素PHj；

(2)读取地形矩阵MAP在PHj处的高度值MAP(PHj)，若|MAP(PHj)-zHj|>Zpthreshold，则对地形矩阵MAP的PHj点添加适合半径的径向基核函数模板，否则保持高度值MAP(PHj)不变，其中，zHj表示点QHj的高度值，Zpthreshold表示点约束高度差阈值；

(3)按照步骤(1)-(2)的方式遍历所有被用于建立点约束的点，构建出被估计区域地形的地形矩阵MAP。

2.根据权利要求1所述基于核函数的实时地形估计方法，其特征在于，所述光线约束的过程为：针对实时点云数据上的任意点，连接其与激光投射原点，计算连线上沿途地形矩阵MAP的高度是否高于该连线，若存在光线投射路径上的任意点Qsci＝(xsci，ysci，zsci)满足|Map(Psci)-zsci|>Zlthreshold，其中，Psci表示从地形矩阵MAP中找到离Qsci最近的元素，zsci表示连线上点Qsci的高度值，Zlthreshold表示光线高度差阈值，则对地形矩阵MAP的Psci点添加适合半径的径向基核函数模板，否则保持地形矩阵MAP不变。

3.根据权利要求1所述基于核函数的实时地形估计方法，其特征在于，所述适合半径的设定准则为：第一，在地形锐利的边角处和有突兀特征的地区，采用半径较小的径向基函数模板，在平坦区域采用半径较大的径向基函数模板；

第二，在点云较为密集的地方，采用半径较小的径向基函数模板，在点云较为稀疏的地方，采用半径较大的径向基函数模板。

4.根据权利要求1所述基于核函数的实时地形估计方法，其特征在于，所述更新历史点云库过程为：a.若更新次数n＝1，直接将配准后的第一帧数据作为历史点云库；

b.更新次数n>1时，将配准后的PCna与PC(n-1)a直接合并得到新的点云集PCntemp1＝PC(n-1)a+PCna；

c.对PCntemp1进行尺寸范围裁剪，只保留无人车周围设定范围内的点云，得到PCntemp2；

d.在PCntemp2随机删除一定比例的点云得到PCntemp3；

e.对PCntemp3进行基于体素的降采样处理，至此点云库的周期更新完成得到更新后的点云库PCnH。

5.根据权利要求4所述基于核函数的实时地形估计方法，其特征在于，所述设定范围为

4倍地形矩阵面积。

说明书

一种基于核函数的实时地形估计方法

技术领域

[0001] 本发明涉及一种基于核函数的实时地形估计方法，属于无人车辆环境感知技术领域。

背景技术

[0002] 无人车在越野环境中行驶时，实时的地形估计对于无人车自主导航至关重要。同结构化的行驶环境相比，无人车在越野环境缺乏可供识别的明确标志物，如车道线，道路边沿等。目前国内外无人车普遍装备3D激光雷达，3D激光雷达是一种主动式传感器可以通过向外发射激光返回点云数据，从而获得周围环境的深度信息，是地形估计的主要传感器。

[0003] 通过激光点云重建周围环境，在点云密集的区域或者地形平整的区域实现较为容易。但是激光雷达点云还存在其特殊性，根据其投射原理可以看出在探测距离比较近的区域，点云分布较为密集；远距离时，点云密度会随着探测距离变长急剧下降，在远处较为稀疏；盲区也是激光雷达的特性之一：激光雷达安装位置一般在车身上部，由于发射角限制，在车身周围会产生锥形盲区，这也使得无人车无法获得所处环境中一定半径内的地面点云。另一方面越野环境中地形复杂多变，存在凸起和沟壑，激光扫描这类环境时由于光线沿直线传播的特性，会在坡后和沟前的被遮挡区产生无点云返回的阴影区，在远距离区域，两行激光之间的水平距离变大也会产生环状盲区，针对3D激光雷达在探测距离60m时，会出现大约宽度为10m的盲区，更远距离时将会更宽，因此不容忽略。以上几点原因使得准确的实时地形估计问题变得复杂。

[0004] 除此之外，基于3D激光雷达地形估计在应用于无人车辆时，还存在以下两个技术难点：(1)无人车在行驶过程中，会由于地面不平整产生载体姿态变化，与车体固连的激光雷达也会随之产生倾斜，进而点云数据整体产生倾斜；(2)无人车行驶速度快，环境感知必须保证实时性才能满足自主导航要求，每秒钟需处理大于100万个点。

[0005] 传统的曲面表示方法主要有三种：参数曲面表示、三角形/多边形网络表示以及隐式曲面表示。参数曲面表示方法只能表示较为简单的理想化的曲面，难以适应复杂的地形估计；三角形/多边形网络只能生成一个非连续曲面，并且不能在线更新，不适合无人车点云流的地形估计；隐式曲面表示方法能够表示一个连续的曲面，其通常采用的径向基核函数(RBF)在物理形态上也比较符合每个激光点只能影响它“周边地形”的要求，而且可以逐点处理点云数据流，但是隐式曲面表示方法存在处理量大和显示不方便的问题。

发明内容

[0006] 有鉴于此，本发明针对隐式曲面表示方法存在的问题，提出一种基于核函数的实时地形估计方法，该方法能适用于无人车高实时性、海量数据的地形估计，解决运动中盲区的补偿，并给出可以在线更新的显式估计结果。

[0007] 实现本发明的技术方案如下：

[0008] 一种基于核函数的实时地形估计方法，具体过程为：

[0009] 步骤一，获取实时激光点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵；

[0010] 步骤二，利用所述旋转平移矩阵对实时激光点云数据进行配准，并对配准后的点云数据采用基于体素的降采样处理；

[0011] 步骤三，遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩阵MAP；

[0012] 步骤四、利用实时点云数据对所述地形矩阵MAP进行点约束和光线约束，将此时获得的地形矩阵MAP作为当前无人车周围的地形估计；

[0013] 步骤五、将所述配准后的实时点云数据添加到历史点云库中，更新历史点云数据。

[0014] 进一步地，本发明所述点约束的过程为：

[0015] (1)选取被用于建立点约束的一个点QHj，判断QHj是否在地形矩阵MAP的覆盖范围之内，如果在，则根据点QHj的坐标(xHj，yHj)从地形矩阵MAP中找到离QHj最近的元素PHj；

[0016] (2)读取地形矩阵MAP在PHj处的高度值MAP(PHj)，若|MAP(PHj)-zHj|＞Zpthreshold，则对地形矩阵MAP的PHj点添加适合半径的径向基核函数模板，否则保持高度值MAP(PHj)不变，其中，zHj表示点QHj的高度值，Zpthreshold表示点约束高度差阈值；

[0017] (3)按照步骤(1)-(2)的方式遍历所有被用于建立点约束的点，构建出被估计区域地形的地形矩阵MAP。

[0018] 针对步骤二中的点约束，被用于建立点约束中的点为历史点云库中的点，针对步骤四种的点约束，被用于建立点约束的点为实时点云数据上的点。

[0019] 进一步地，本发明所述光线约束的过程为：针对实时点云数据上的任意点，连接其与激光投射原点，计算连线上沿途地形矩阵MAP的高度是否高于该连线，若存在光线投射路径上的任意点Qsci＝(xsci，ysci，zsci)满足|Map(Psci)-zsci|＞Zlthreshold，其中，Psci表示从地形矩阵MAP中找到离Qsci最近的元素，zsci表示连线上点Qsci的高度值，Zlthreshold表示光线高度差阈值，则对地形矩阵MAP的Psci点添加适合半径的径向基核函数模板，否则保持地形矩阵MAP不变。

[0020] 进一步地，本发明所述合适半径的设定准则为：

[0021] 第一，在地形锐利的边角处和有突兀特征的地区，采用半径较小的径向基函数模板，在平坦区域采用半径较大的径向基函数模板；

[0022] 第二，在点云较为密集的地方，采用半径较小的径向基函数模板，在点云较为稀疏的地方，采用半径较大的径向基函数模板。

[0023] 进一步地，本发明所述更新历史点云库过程为：

[0024] a.若更新次数n＝1，即首次运行则PC1H＝PC1a，直接将配准后的第一帧数据作为历史点云库；

[0025] b.更新次数n＞1时，将配准后的PCna与PC(n-1)a直接合并得到新的点云集PCntemp1＝PC(n-1)a+PCna；

[0026] c.对PCntemp1进行尺寸范围裁剪，只保留无人车周围设定范围内的点云，得到PCntemp2；

[0027] d.在PCntemp2随机删除一定比例的点云得到PCntemp3；

[0028] e.对PCntemp3进行基于体素的将采样处理，至此点云库的周期更新完成得到更新后的点云库PCnH。

[0029] 进一步地，本发明所述设定范围为4倍地形矩阵面积。

[0030] 有益效果：

[0031] 1、本发明提出的点云将采样处理、核函数模板和地形矩阵表示方法，因此，该高实时性的点云处理方法能够完成无人车行进间海量点云数据实时的处理任务。

[0032] 2、本发明采用行进过程历史点云数据管理更新方法，因此能够在抵抗无人车行进间由于路面不平、车体倾斜造成的点云盲区；在一定程度上改善由于遮挡和路面凹凸造成的盲区。

[0033] 3、本发明生成地形矩阵，是一种显式结果，适用于无人车自主导航使用，并且地形矩阵支持在线更新。

附图说明

[0034] 图1是本发明方法的工作流程示意图；

[0035] 图2是遍历历史点云建立地形矩阵点约束算法流程示意图

[0036] 图3是遍历实时点云建立地形矩阵点约束和光线约束算法流程示意图；

[0037] 图4是基于体素降采样示意图(处理前)；

[0038] 图5是基于体素降采样示意图(处理后)；

[0039] 图6是不同尺寸的栅格化核函数模板示意图

[0040] 图7是历史点云填补车体倾斜盲区示意图(处理前)；

[0041] 图8是历史点云填补车体倾斜盲区示意图(处理后)；

[0042] 图9是历史点云填补遮挡盲区示意图(处理前)；

[0043] 图10是历史点云填补遮挡盲区示意图(处理后)。

具体实施方式

[0044] 下面结合附图，举例说明本发明的具体实施方式。

[0045] 本发明开发了一种基于核函数的实时地形估计方法，该方法所基于的硬件实现平台是无人车平台，无人车平台上装备有64线3D激光雷达，INS-GPS组合导航单元等传感器。

[0046] 基于以上平台，典型工作流程首先需要进行系统初始化，包括：车辆底层初始化，传感器参数初始化，车辆姿态位置初始化，地形矩阵建立，变尺度核函数模板建立；然后判断地形估计任务是否结束，若是直接结束该方法，否则进入循环的系统工作流程，每个处理周期需要包括以下几个步骤，如图1所示：

[0047] 一、获取实时激光点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵；

[0048] 二、利用所述旋转平移矩阵对实时激光点云数据进行配准，并对配准后的点云数据采用基于体素的降采样处理；

[0049] 三、遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩阵MAP；

[0050] 四、利用实时点云数据对所述地形矩阵MAP进行点约束和光线约束，将此时获得的地形矩阵MAP作为当前无人车周围的地形估计，输出该栅格地形图；

[0051] 五、将所述配准后的实时点云数据添加到历史点云库中，更新历史点云数据。

[0052] 下面具体介绍每个环节的工作过程：

[0053] 一、获取实时激光点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵。

[0054] 激光雷达通过发射激光束扫描周边环境采样得到激光点云数据PCn，其中每个点云数据的格式为PCnj＝(X，Y，Z)，其中X，Y，Z为该点相对于激光雷达的坐标值。从INS-GPS获得无人车辆当前的位姿数据，所述位姿数据包括绝对位置(E，N)和当前姿态角(α，β，γ)；然后结合上一帧车辆的位姿数据，能够得到无人车辆当前状态相对于绝对空间的旋转平移矩阵Cn。

[0055] 二、配准和预处理实时点云数据。

[0056] 由于车体运动导致传感器倾斜，传感器也在跟随载体运动，要综合利用多帧数据，由于每一帧点云数据皆为相对激光雷达的坐标值，因此需要对多帧点云数据进行坐标转换，转换成相对于绝对空间的坐标值，实现点云配准，即利用旋转平移矩阵Cn对PCn进行配准PCna＝PCn*Cn，经过配准后的PCna所属的XY平面平行于水平面，X，Y分别指向东向和北向，坐标原点(经度，纬度)处于(E，N)＝(0，0)。

[0057] 图4显示了单帧点云数据的分布特点(90度范围)，由于其航向角分辨率是固定值，而扫描距离不同，势必造成了单帧点云数据近处密度非常高，而远处分辨率较低；在距离较远的区域内，径向分布密度比切向分布密度衰减的更快，即在距离较远时，点与点之间的切向距离远远小于径向距离。

[0058] 本发明采用基于体素的降采样处理对配准后的点云进行预处理。将车体周围有点云的三维空间平均分成尺寸为m个L*W*H的小块体素V，然后对每个体素包含的原点云进行统计，再以每个体素中的点云数据的重心在近似代表该体素中的其他点云，这样该体素就可以用一个重心点来表示。有效降低高密度点云区的点云数量，而不减少低密度区的点云数量，能够在缩减点云规模的同时最大程度保持原点云信息，同时使得点云整体密度更加平均，如图5所示。

[0059] 三、遍历历史点云库建立描述被估计区域地形的地形矩阵MAP。

[0060] 地形矩阵MAP是一个2M*2M的矩阵描述了被估计区域的地形，M为预设参数，矩阵的行和列分别表示东向和北向方向距离，矩阵所表示的区域为以无人车为中心的边长(2M*栅格分辨率)的正方形区域，无人车位置在矩阵中心(M，M)处，矩阵的每个元素的值表示地形高度。具体的建立过程为：

[0061] 用QHj(xHj，yHj，zHj)表示历史点云库中的某点，MAP表示地形矩阵，P表示MAP中的行列值，点约束关系可以表示为：

[0062]

[0063] PHj是地形矩阵MAP中最接近点QHj的一个元素，其元素值应与点QHj的高度值zHj相一致。简言之，历史点云库的点云数据都处在被估计地形的表面。以上约束关系对任意QHj均成立，因此需要遍历历史点云库，对地形矩阵MAP上每一个点建立这种约束关系，如图2所示，具体步骤：

[0064] (1)选取历史点云库中一个点QHj，判断QHj是否在地形矩阵MAP的覆盖范围之内，如果在，则根据点QHj的坐标(xHj，yHj)从地形矩阵MAP中找到离QHj最近的元素PHj。

[0065] (2)读取地形矩阵MAP在PHj处的高度值MAP(PHj)，若|MAP(PHj)-zHj|＞Zpthreshold，则对地形矩阵的PHj点添加适合半径的径向基核函数模板，否则保持高度值MAP(PHj)不变，其中，zHj表示点QHj的高度值，Zpthreshold是点约束高度差阈值；

[0066] (3)按照步骤(1)-(2)的方式遍历历史点云库中的每一个点，构建出被估计区域地形的地形矩阵MAP。

[0067] 本发明步骤(2)中采用基于径向基核函数能够很好地进行曲面重构，本发明提出的变尺度径向基函数模板基于Wu’s径向基函数提出，其原型为：

[0068]

[0069] 该 中的将 简化成

[0070] 其中：

[0071]

[0072] 其中，σ为核函数的支撑半径，由上述公式可知，核函数的支持半径σ与该激光点影响的地形范围大小相关，每个激光点只能影响它周围σ距离内的地形估计结果。常规的曲面估计和地形估计算法中，σ是固定的，或者说全局一致的，本发明采用了变σ的径向基函数，σ的选取原则由两方面：

[0073] 第一，当前激光点与当前估计地面的高度差值：在地形锐利的边角处和有突兀特征的地区会对周边环境造成较大的连带影响，宜采用σ值较小的径向基函数，而平坦区域则宜采用较大σ值得径向基函数；

[0074] 第二，当前激光点距离激光雷达原点的距离：在点云较为密集的地方，宜采用较小的σ值，在点较为稀疏的地方，宜使用较大的σ值。

[0075] 为了兼顾运算速度，在系统初始化阶段，将不同支持半径σ的径向基核函数预制成为模板，如图6所示，该模板与地形矩阵的分辨率相同，使用时直接调用模板内容，而不需要每次重新计算高次多项式。

[0076] 四、利用实时点云数据对步骤三建立的地形矩阵MAP进行点约束和光线约束。

[0077] 如图3所示，具体过程为：

[0078] 点约束：逐点遍历整个实时点云数据，按照步骤三的方式对地形矩阵MAP添加点约束；

[0079] 光线约束：由于实时点云采集自同一点处，由于光线沿直线传播，任何激光点投射路径上的地形高度一定要低于该路径，否则会发生遮挡，此即为实时点云的光线约束条件：

[0080] 设Qci(xci，yci，zci)为实时点云中的某点，Sci为光线投射路径，Qsci(xsci，ysci，zsci)为光线投射路径上的点。P为地形矩阵MAP上的点。

[0081] 对于

[0082]

[0083] 添加光线约束时，对于任意点云，连接其与激光投射原点处，计算该连线上沿途地形矩阵的高度是否高于该连线的情况，若存在点Psci满足|Map(Psci)-zsci＞Zlthreshold，其中zsci表示连线上点Qsci的高度值，Zlthreshold表示光线高度差阈值，则对地形矩阵的Psci点添加适合半径的径向基核函数模板。

[0084] 至此一个周期实时地形估计。

[0085] 五、将所述配准后的实时点云数据添加到历史点云库中，更新历史点云数据。

[0086] 步骤二中用到的历史点云数据库包含了无人车运动路径上的点云信息，即PCn-1，PCn-2，…，PC1。在实时点云PCn来到时，需要利用新的点云信息更新历史数据库PCnH，具体执行方法如下：

[0087] a.若更新次数n＝1，即首次运行则PC1H＝PC1a，直接将配准后的第一帧数据作为历史点云库；

[0088] b.更新次数n＞1时，将配准后的PCna与PC(n-1)a直接合并得到新的点云集PCntemp1＝PC(n-1)a+PCna；

[0089] c.对PCntemp1进行尺寸范围裁剪得到PCntemp2，只保留无人车周围一定范围的点云，此范围大于地形矩阵表示范围，一般取4倍地形矩阵面积。

[0090] d.在PCntemp2随机删除一定比例的点云得到PCntemp3。

[0091] e.对PCntemp3进行如步骤二所述的体素降采样，至此点云库的周期更新完成得到更新后的PCnH。

[0092] 上述步骤c，e保证点云库的容量是稳定的，即使长时间运行，其中包含的点云总量也能够维持在一定水平，步骤d，e能够衰减掉随机产生的干扰点和和运动物体对历史点云库造成的污染。

[0093] 用上述方法得到的地形估计结果能够在抵抗无人车行进间由于路面不平、车体倾斜造成的点云盲区，如图7-8所示；在一定程度上改善由于遮挡和路面凹凸造成的盲区，如图9-10所示。

[0094] 以上结合附图对本发明的具体实施方式作了说明，但这些说明不能被理解为限制了本发明的范围，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。