[发明专利]一种基于NDT算法的里程计方法

权利要求书

1.一种基于NDT算法的里程计方法，其特征在于，包括以下步骤：

S1、通过雷达绘制运行环境区域，并将区域划分成大小相同的单元格；

S2、获取雷达相邻帧的激光数据，利用前一帧数据建立参考点云网络；

S3、通过对后一帧激光数据进行偏移参数转换，使转换后的激光点匹配参考点云网络中的激光点；

S4、计算偏移参数中的偏移量，作为里程计。

2.根据权利要求1所述的一种基于NDT算法的里程计方法，其特征在于，所述步骤S2中参考点云网络的建立方法为：

计算激光点的协方差矩阵，描绘点云在单元格中的分布；

单元格内所有激光点Xi＝1,…,n的坐标均值计算公式为：

方差计算公式为：其中T表示转置运算。

3.根据权利要求1所述的一种基于NDT算法的里程计方法，其特征在于，所述步骤S3中通过偏移函数转换激光点的方式为：

先预设一个变换参数P＝(t_x，t_y,Φ)^T，并将各参数置零，t_x、t_y、Φ参数分别为x轴平移量、y轴平移量、角度偏移量；

则转后的激光点可表示为：

x′_i＝T(x_i,p)

4.根据权利要求3所述的一种基于NDT算法的里程计方法，其特征在于，所述步骤S4中计算偏移参数中的偏移量的方法为：

S401、对步骤S3中转换后的激光点进行正态分布，计算出激光点在每个单元格中的概率密度；

S402、将概率密度累加得到NDT配准得分；

S403、通过牛顿优化算法对配准得分进行优化，计算出配准得分最大时，变换参数P的值，从而计算出x轴平移量、y轴平移量、角度偏移量，作为里程计。