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

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

【技术实现步骤摘要】
一种基于核函数的实时地形估计方法
本专利技术涉及一种基于核函数的实时地形估计方法，属于无人车辆环境感知

技术介绍
无人车在越野环境中行驶时，实时的地形估计对于无人车自主导航至关重要。同结构化的行驶环境相比，无人车在越野环境缺乏可供识别的明确标志物，如车道线，道路边沿等。目前国内外无人车普遍装备3D激光雷达，3D激光雷达是一种主动式传感器可以通过向外发射激光返回点云数据，从而获得周围环境的深度信息，是地形估计的主要传感器。通过激光点云重建周围环境，在点云密集的区域或者地形平整的区域实现较为容易。但是激光雷达点云还存在其特殊性，根据其投射原理可以看出在探测距离比较近的区域，点云分布较为密集；远距离时，点云密度会随着探测距离变长急剧下降，在远处较为稀疏；盲区也是激光雷达的特性之一：激光雷达安装位置一般在车身上部，由于发射角限制，在车身周围会产生锥形盲区，这也使得无人车无法获得所处环境中一定半径内的地面点云。另一方面越野环境中地形复杂多变，存在凸起和沟壑，激光扫描这类环境时由于光线沿直线传播的特性，会在坡后和沟前的被遮挡区产生无点云返回的阴影区，在远距离区域，两行激光之间的水平距离变大也会产生环状盲区，针对3D激光雷达在探测距离60m时，会出现大约宽度为10m的盲区，更远距离时将会更宽，因此不容忽略。以上几点原因使得准确的实时地形估计问题变得复杂。除此之外，基于3D激光雷达地形估计在应用于无人车辆时，还存在以下两个技术难点：(1)无人车在行驶过程中，会由于地面不平整产生载体姿态变化，与车体固连的激光雷达也会随之产生倾斜，进而点云数据整体产生倾斜；(2)无人车行驶速度快，环境感知必须保证实时性才能满足自主导航要求，每秒钟需处理大于100万个点。传统的曲面表示方法主要有三种：参数曲面表示、三角形/多边形网络表示以及隐式曲面表示。参数曲面表示方法只能表示较为简单的理想化的曲面，难以适应复杂的地形估计；三角形/多边形网络只能生成一个非连续曲面，并且不能在线更新，不适合无人车点云流的地形估计；隐式曲面表示方法能够表示一个连续的曲面，其通常采用的径向基核函数(RBF)在物理形态上也比较符合每个激光点只能影响它“周边地形”的要求，而且可以逐点处理点云数据流，但是隐式曲面表示方法存在处理量大和显示不方便的问题。
技术实现思路
有鉴于此，本专利技术针对隐式曲面表示方法存在的问题，提出一种基于核函数的实时地形估计方法，该方法能适用于无人车高实时性、海量数据的地形估计，解决运动中盲区的补偿，并给出可以在线更新的显式估计结果。实现本专利技术的技术方案如下：一种基于核函数的实时地形估计方法，具体过程为：步骤一，获取实时激光点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵；步骤二，利用所述旋转平移矩阵对实时激光点云数据进行配准，并对配准后的点云数据采用基于体素的降采样处理；步骤三，遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩阵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。针对步骤二中的点约束，被用于建立点约束中的点为历史点云库中的点，针对步骤四种的点约束，被用于建立点约束的点为实时点云数据上的点。进一步地，本专利技术所述光线约束的过程为：针对实时点云数据上的任意点，连接其与激光投射原点，计算连线上沿途地形矩阵MAP的高度是否高于该连线，若存在光线投射路径上的任意点Qsci＝(xsci，ysci，zsci)满足|Map(Psci)-zsci|＞Zlthreshold，其中，Psci表示从地形矩阵MAP中找到离Qsci最近的元素，zsci表示连线上点Qsci的高度值，Zlthreshold表示光线高度差阈值，则对地形矩阵MAP的Psci点添加适合半径的径向基核函数模板，否则保持地形矩阵MAP不变。进一步地，本专利技术所述合适半径的设定准则为：第一，在地形锐利的边角处和有突兀特征的地区，采用半径较小的径向基函数模板，在平坦区域采用半径较大的径向基函数模板；第二，在点云较为密集的地方，采用半径较小的径向基函数模板，在点云较为稀疏的地方，采用半径较大的径向基函数模板。进一步地，本专利技术所述更新历史点云库过程为：a.若更新次数n＝1，即首次运行则PC1H＝PC1a，直接将配准后的第一帧数据作为历史点云库；b.更新次数n＞1时，将配准后的PCna与PC(n-1)a直接合并得到新的点云集PCntemp1＝PC(n-1)a+PCna；c.对PCntemp1进行尺寸范围裁剪，只保留无人车周围设定范围内的点云，得到PCntemp2；d.在PCntemp2随机删除一定比例的点云得到PCntemp3；e.对PCntemp3进行基于体素的将采样处理，至此点云库的周期更新完成得到更新后的点云库PCnH。进一步地，本专利技术所述设定范围为4倍地形矩阵面积。有益效果：1、本专利技术提出的点云将采样处理、核函数模板和地形矩阵表示方法，因此，该高实时性的点云处理方法能够完成无人车行进间海量点云数据实时的处理任务。2、本专利技术采用行进过程历史点云数据管理更新方法，因此能够在抵抗无人车行进间由于路面不平、车体倾斜造成的点云盲区；在一定程度上改善由于遮挡和路面凹凸造成的盲区。3、本专利技术生成地形矩阵，是一种显式结果，适用于无人车自主导航使用，并且地形矩阵支持在线更新。附图说明图1是本专利技术方法的工作流程示意图；图2是遍历历史点云建立地形矩阵点约束算法流程示意图图3是遍历实时点云建立地形矩阵点约束和光线约束算法流程示意图；图4是基于体素降采样示意图(处理前)；图5是基于体素降采样示意图(处理后)；图6是不同尺寸的栅格化核函数模板示意图图7是历史点云填补车体倾斜盲区示意图(处理前)；图8是历史点云填补车体倾斜盲区示意图(处理后)；图9是历史点云填补遮挡盲区示意图(处理前)；图10是历史点云填补遮挡盲区示意图(处理后)。具体实施方式下面结合附图，举例说明本专利技术的具体实施方式。本专利技术开发了一种基于核函数的实时地形估计方法，该方法所基于的硬件实现平台是无人车平台，无人车平台上装备有64线3D激光雷达，INS-GPS组合导航单元等传感器。基于以上平台，典型工作流程首先需要进行系统初始化，包括：车辆底层初始化，传感器参数初始化，车辆姿态位置初始化，地形矩阵建立，变尺度核函数模板建立；然后判断地形估计任务是否结束，若是直接结束该方法，否本文档来自技高网...

【技术保护点】
一种基于核函数的实时地形估计方法，其特征在于，具体过程为：步骤一，获取实时点云数据和车辆当前状态相对于绝对空间的旋转平移矩阵；步骤二，利用所述旋转平移矩阵对实时点云数据进行配准，并对配准后的点云数据采用基于体素的降采样处理；步骤三，遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩阵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的高度是否高于该连线，若存在光线投射路...

【专利技术属性】
技术研发人员：杨毅，李星河，付梦印，朱昊，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：北京;11