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

权利要求书

1.一种基于核函数的实时地形估计方法，其特征在于，具体过程为：

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

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

步骤三，遍历历史点云库中的每一点，采用点约束建立描述被估计区域地形的地形矩阵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。

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

3.根据权利要求1所述基于核函数的实时地形估计方法，其特征在于，所述适合半径的设定准则为：

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

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

4.根据权利要求1所述基于核函数的实时地形估计方法，其特征在于，所述更新历史点云库过程为：

a.若更新次数n＝1，直接将配准后的第一帧数据作为历史点云库；

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

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

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

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

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