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

说明书

本发明公开了一种基于NDT算法的里程计方法，属于里程计计算技术领域，包括S1、通过雷达绘制运行环境区域，并将区域划分成大小相同的单元格；S2、获取雷达相邻帧的激光数据，利用前一帧数据建立参考点云网络；S3、通过对后一帧激光数据进行偏移参数转换，使转换后的激光点匹配参考点云网络中的激光点；S4、计算偏移参数中的偏移量，作为里程计。通过雷达在运动过程中扫描的两个相邻帧激光数据，将前帧作为参考点云网络，后帧的数据经过变换匹配参考点云网络后，即可获知变换的量为物体运动的里程计，因此通过该方式可以无需在意雷达的初始位置，计算的结果也不受运动物体本身的影响，测量精度高，稳定性好。

技术领域

本发明涉及里程计计算技术领域，特别涉及一种基于NDT算法的里程计方法。

背景技术

随着人工智能的快速发展,叉车AGV在许多物流仓库领域中得到应用。里程计为机器人提供基础位姿信息,是机器人完成SLAM,路径规划及导航等任务的基础。

在现有技术中，常用的计算方式一般有两种，第一中：基于ICP点云匹配算法，有耗时多，和初值相关性大且容易陷入局部最优,从而导致计算的里程计数据精度不高，不够鲁棒的缺陷；第二种：轮式里程计，是将单位采样时间(一般为10ms)内的编码脉冲累计值作为里程数据发送给AGV工控机，利用航迹推演算法求解出里程计信息，该方法在地面打滑的情况下误差很差且极不稳定。

发明内容

本发明的目的就在于为了解决上述AGV在运动过程中里程计计算复杂，准确率低的问题而提供一种基于NDT算法的里程计方法，具有可不受初始值影响，计算结果更加精确，计算简单，效率高的优点。

本发明通过以下技术方案来实现上述目的，一种基于NDT算法的里程计方法，包括以下步骤：

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

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

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

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

优选的，所述步骤S2中参考点云网络的建立方法为：

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

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

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

优选的，所述步骤S3中通过偏移函数转换激光点的方式为：

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

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

x′_i＝T(x_i,p)

优选的，所述步骤S4中计算偏移参数中的偏移量的方法为：

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

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

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