一种基于NDT算法的里程计方法

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

方差计算公式为：

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

先预设一个变换参数P＝(t

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

x′

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

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

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

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

与现有技术相比，本发明的有益效果是：通过雷达在运动过程中扫描的两个相邻帧激光数据，将前帧作为参考点云网络，后帧的数据经过变换匹配参考点云网络后，即可获知变换的量为物体运动的里程计，因此通过该方式可以无需在意雷达的初始位置，计算的结果也不受运动物体本身的影响，测量精度高，稳定性好。

附图说明

图1为本发明的整体里程计计算方法流程图。

图2为本发明的偏移参数中的偏移量的计算方法流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1所示，一种基于NDT算法的里程计方法，包括以下步骤：

S1、通过雷达绘制运行环境区域，并将区域划分成大小相同的单元格，在绘制时，雷达选择一个2D雷达，将该雷达固定在一个可以行动的AGV小车上，将计算里程计的计算机安装在AGV车体上，与雷达连接，用来实时获取雷达数据，并通过计算机来绘制单元格区域图；

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

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

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

方差计算公式为：

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

先预设一个变换参数P＝(t

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

x′

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

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

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

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

其中q对变换参数p

根据上面梯度的计算结果，继续求s关于变量p

若对P的二次求导收敛就说明有极值点，则可以返回计算P＝(t

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。