[发明专利]一种基于核函数的实时地形估计方法

说明书

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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