一种基于核函数的实时地形估计方法
【技术领域】
[0001] 本发明涉及一种基于核函数的实时地形估计方法，属于无人车辆环境感知技术领 域。
【背景技术】
[0002] 无人车在越野环境中行驶时，实时的地形估计对于无人车自主导航至关重要。同 结构化的行驶环境相比，无人车在越野环境缺乏可供识别的明确标志物，如车道线，道路边 沿等。目前国内外无人车普遍装备3D激光雷达，3D激光雷达是一种主动式传感器可以通过 向外发射激光返回点云数据，从而获得周围环境的深度信息，是地形估计的主要传感器。
[0003] 通过激光点云重建周围环境，在点云密集的区域或者地形平整的区域实现较为容 易。但是激光雷达点云还存在其特殊性，根据其投射原理可以看出在探测距离比较近的区 域，点云分布较为密集；远距离时，点云密度会随着探测距离变长急剧下降，在远处较为稀 疏；盲区也是激光雷达的特性之一：激光雷达安装位置一般在车身上部，由于发射角限制， 在车身周围会产生锥形盲区，这也使得无人车无法获得所处环境中一定半径内的地面点 云。另一方面越野环境中地形复杂多变，存在凸起和沟壑，激光扫描这类环境时由于光线沿 直线传播的特性，会在坡后和沟前的被遮挡区产生无点云返回的阴影区，在远距离区域，两 行激光之间的水平距离变大也会产生环状盲区，针对3D激光雷达在探测距离60m时，会出 现大约宽度为l〇m的盲区，更远距离时将会更宽，因此不容忽略。以上几点原因使得准确的 实时地形估计问题变得复杂。
[0004] 除此之外，基于3D激光雷达地形估计在应用于无人车辆时，还存在以下两个技术 难点：（1)无人车在行驶过程中，会由于地面不平整产生载体姿态变化，与车体固连的激光 雷达也会随之产生倾斜，进而点云数据整体产生倾斜；（2)无人车行驶速度快，环境感知必 须保证实时性才能满足自主导航要求，每秒钟需处理大于100万个点。
[0005] 传统的曲面表示方法主要有三种：参数曲面表示、三角形/多边形网络表示以及 隐式曲面表示。参数曲面表示方法只能表示较为简单的理想化的曲面，难以适应复杂的地 形估计；三角形/多边形网络只能生成一个非连续曲面，并且不能在线更新，不适合无人车 点云流的地形估计；隐式曲面表示方法能够表示一个连续的曲面，其通常采用的径向基核 函数（RBF)在物理形态上也比较符合每个激光点只能影响它"周边地形"的要求，而且可以 逐点处理点云数据流，但是隐式曲面表示方法存在处理量大和显示不方便的问题。
【发明内容】
[0006] 有鉴于此，本发明针对隐式曲面表示方法存在的问题，提出一种基于核函数的实 时地形估计方法，该方法能适用于无人车高实时性、海量数据的地形估计，解决运动中盲区 的补偿，并给出可以在线更新的显式估计结果。
[0007] 实现本发明的技术方案如下：
[0008] -种基于核函数的实时地形估计方法，具体过程为：
[0009] 步骤一，获取实时激光点云数据和车辆当前状态相对于绝对空间的旋转平移矩 阵；
[0010] 步骤二，利用所述旋转平移矩阵对实时激光点云数据进行配准，并对配准后的点 云数据采用基于体素的降采样处理；
[0011] 步骤三，遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地 形矩阵MAP ;
[0012] 步骤四、利用实时点云数据对所述地形矩阵MAP进行点约束和光线约束，将此时 获得的地形矩阵MAP作为当前无人车周围的地形估计；
[0013] 步骤五、将所述配准后的实时点云数据添加到历史点云库中，更新历史点云数据。
[0014] 进一步地，本发明所述点约束的过程为：
[0015] (1)选取被用于建立点约束的一个点Qm，判断Qm是否在地形矩阵MAP的覆盖范围 之内，如果在，则根据点Qm的坐标（x m，yHp从地形矩阵MAP中找到离％最近的元素P Hj; [0016] (2)读取地形矩阵 MAP 在 PHj处的高度值 MAP (P Hj)，若 | MAP (PHj) -zHj | > ZpthreshQld， 则对地形矩阵MAP的P&.点添加适合半径的径向基核函数模板，否则保持高度值MAP (Py不 变，其中，zHj表示点QHj的高度值，Zptostold表示点约束高度差阈值；
[0017] (3)按照步骤（1)_(2)的方式遍历所有被用于建立点约束的点，构建出被估计区 域地形的地形矩阵MAP。
[0018] 针对步骤二中的点约束，被用于建立点约束中的点为历史点云库中的点，针对步 骤四种的点约束，被用于建立点约束的点为实时点云数据上的点。
[0019] 进一步地，本发明所述光线约束的过程为：针对实时点云数据上的任意点，连接其 与激光投射原点，计算连线上沿途地形矩阵MAP的高度是否高于该连线，若存在光线投射 路径上的任意点 Qsci (X sci ' Ysci，Zsci)$两足|Map(Psci)_zsci|〉Zlthresh()ld，其中，Psci表不从 地形矩阵MAP中找到离Qsc;i最近的元素，Z sc;i表示连线上点Q sc;i的高度值，Z lthrash()ld表示光 线高度差阈值，则对地形矩阵MAP的Psc;i点添加适合半径的径向基核函数模板，否则保持地 形矩阵MAP不变。
[0020] 进一步地，本发明所述合适半径的设定准则为：
[0021] 第一，在地形锐利的边角处和有突兀特征的地区，采用半径较小的径向基函数模 板，在平坦区域采用半径较大的径向基函数模板；
[0022] 第二，在点云较为密集的地方，采用半径较小的径向基函数模板，在点云较为稀疏 的地方，采用半径较大的径向基函数模板。
[0023] 进一步地，本发明所述更新历史点云库过程为：
[0024] a.若更新次数n = 1，即首次运行则PC1H=PCla，直接将配准后的第一帧数据作为 历史点云库；
[0025] b.更新次数n > 1时，将配准后的PCn# PCta_1)a直接合并得到新的点云集PC ntenipl =PC(n-l)a+PCna；
[0026] c.对PCntempl进行尺寸范围裁剪，只保留无人车周围设定范围内的点云，得到 PC ? 1 v-/ntemp2?
[0027] d.在PCntemp2随机删除一定比例的点云得到PC ntemp3;
[0028] e.对PCntemp3进行基于体素的将采样处理，至此点云库的周期更新完成得到更新后 的点云库PCf
[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] 本发明开发了一种