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

摘要

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

主权项

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