基于点云匹配的AGV局部定位方法

本发明涉及机器人控制领域，具体是一种基于点云匹配的agv局部定位方法。

背景技术：

1、为满足产业自动化、智能化需求，现有的仓储物流系统已经能够初步实现agv自动搬运、自主移动等功能，但对于运动过程中所处位置判断是否正确，从而避免在移动至错误工位而影响生产工作成为agv自主移动过程的一个关键问题。

2、现有的机器人定位技术有通过在环境中布置额外设备，传递信息来实现，如专利号202022655299.8，名称为“一种基于uwb的agv定位导航装置”公开了一种定位装置，通过布置和测量agv上uwb定位标签与环境中uwb基站数据结算坐标，这种方法需要在生产环境布置uwb基站，若生产规模较大，需要花费很多经费进行布置，且不易维护。也有基于视觉传感的定位方法，如专利号202011359423.4，名称为“一种基于改进二维码的agv定位及导航方法”公开了一种定位方法，通过先定位二维码位置再识别二维码包含的信息完成定位，这种方法需要预先在环境中布置二维码并设定相应信息。布置过程繁琐且不适合需要二维码标签众多的实际情况。另有使用激光和反光板的定位方法，如专利号202010920562.3，名称为“基于反光板网络的agv定位方法及装置”公开了一种定位方法，通过布置反光板并计算agv相对反光板的位置，这种方法需要在合适高度布置多个反光板且要求agv激光传感器同一时刻能检测到至少三个反光板才能完成计算，且反光板不适合部分生产环境如潮湿等。此外也有使用slam实现agv定位的，如专利号202110545126.7，名称为“一种基于激光slam的室内agv自适应定位系统”公开了一种定位方法，使用激光雷达建立栅格地图后通过自主选择使用icp和优化的方法进行匹配，但其选择提取直角角点作为匹配特征点，由于激光雷达数据稀疏特性，在实际检测中极易丢失中远距离处的特征导致匹配失败。

技术实现思路

1、本发明为了解决现有技术的问题，提供了一种基于点云匹配的agv局部定位方法，能够在无需额外布置设备前提下，通过使用激光雷达扫描获得的环境深度数据完成agv运动状态的优化估计。

2、一种基于点云匹配的agv局部定位方法，包括以下步骤：

3、1)数据获取与预处理：在agv运动过程中对周围环境获取特征数据，预处理后进行存储；

4、2)运动估计部分，基于运动追踪思想搭建数据匹配模型，构建运动限制公式计算获得对应点集，基于欧氏距离构建误差函数，使用m估计稳健回归思想进行加权最小二乘法计算并通过irls算法迭代获得运动估计。

5、3)运动输出部分，估计失效时对结果进行修正，输出估计状态，实现相对之前机器人的局部定位。

6、进一步改进，步骤1)所述特征数据根据agv机械结构设计、已知运动状态与传感器硬件参数获取。所述agv机械结构设计提供激光雷达安装位置与agv的相对位置关系，以agv前进方向为x轴正方向，根据右手法则，参数包括二维空间变换量(xl,yl,θl)；已知运动状态参数源于前次运动估计，包括空间变换量(dxt-1,dyt-1,dθt-1)和运动估计数据传感器硬件参数包括激光雷达的增角θinc和视野fov、起始角度、数据规模、扫描频率。

7、进一步改进，步骤1)所述预处理过程包括基于速度模型进行去运动畸变和滤波算法去噪声。

8、所述去运动畸变过程具体如下：基于速度模型，通过速度模型计算每一个数据点扫描时间间隔的激光雷达的空间运动，对数据进行矫正；依据运动参数和数据点对应的时间间隔，获取变换量并以旋转矩阵ri和平移矩阵ti表示，计算公式如下：

9、

10、上式中，pi，分别代表原始数据点和去畸变后的数据点，ri，ti代表由机器人运动数据获得的旋转矩阵和平移矩阵，即通过前一次运动估计获得的机器人运动数据对每一个数据点进行微小位移和微小旋转，减少激光雷达旋转测量时间段内，机器人空间微小运动导致的误差。

11、所述滤波算法去噪声过程具体如下：使用体素滤波去噪，对数据点所在矩形区域划分栅格，不同栅格内数据点的数量与位置不同，通过计算每个栅格的质心拟合出新的数据点，减少原始数据测量噪声的影响。

12、进一步改进，步骤2)所述运动估计过程具体如下：

13、2.1)搭建匹配模型，构建运动限制公式，计算对应点集；

14、2.11)统一数据空间坐标系，构建运动限制公式，对修正数据帧中点计算参考数据帧q＝{q1,q1,…,qn}中空间上相邻的数据点的向上索引j，获得相邻的数据点和

15、2.12)利用方向限制特性，拟合相邻点，计算与点所在激光线的交点，组成对应点集，完成对应数据点的匹配；

16、2.2)构建误差函数，根据m估计稳健回归赋予样本权重，通过irls迭代获得运动估计；对匹配点集计算每对数据点的欧式距离，构建最小二乘函数进行irls算法迭代w为取权重函数，使用m稳健估计获取。

17、进一步改进，步骤2.11)中所述数据空间坐标系公式表达如下：

18、

19、其中，和分别代表经预处理后的数据点和统一坐标系后的数据点，r，t分别代表旋转矩阵和平移矩阵，可由迭代估计获得的运动变换量(dxt-1,dyt-1,dθt-1)获得，dxt-1,dyt-1,dθt-1分别代表x轴方向位移量，y轴方向位移量和角度变换量；

20、步骤2.11)中所述构建运动限制公式获得相邻的数据点和的具体过程如下：

21、对于当前待匹配数据点基于运动追踪思想，通过点序和迭代估计出的旋转变换计算得到已存储数据中和其在空间上相邻点的点序，公式如下：

22、

23、dθt-1是迭代估计出的旋转角度，θinc是激光雷达的增角，以逆时针方向为正，i是目标数据点的点序，j是计算出的与目标点空间相邻的点序，最后结果即与相邻的点为和

24、进一步改进，步骤2.2)中构建误差函数获得运动估计具体过程如下：计算匹配的点集每对数据点的欧式距离，构建最小二乘函数如下：

25、

26、上式中，代表计算数据点与对应点qi的欧式距离，f(rpi+t)代表步骤2)的计算对应点。对异常值问题，使用加权最小二乘对每个样本点赋予权重，减少偏离较大的离散值对结果的不良影响，函数如下：

27、

28、其中w代表权重计算函数，使用m估计获得。对加权最小二乘法进行求解，获得运动估计r和t；进行irls算法迭代，对收敛条件进行判定，若点云平均误差符合要求或迭代次数达到阈值则结束迭代，完成估计。

29、进一步改进，步骤3)所述估计失效过程，完成判断后使用低通滤波对本次估计和前次估计ξt-1进行整定，输出最终结果ξt和(dxt,dyt,dθt)，完成本次估计。

30、本发明有益效果在于：

31、(1)本发明为基于点云匹配的agv定位方法，能够满足agv运动估计需求，运用方便，无需在场地布置额外的设施，计算资源小，且结果准确度高。

32、(2)基于速度模型对原始数据进行修正，对于扫描频率较低的激光雷达(小于10hz)而言可以显著提高数据的正确度，更加贴合实际扫描曲线，对于较高扫描频率的激光雷达也有一定优化。

33、(3)基于运动追踪思想搭建的匹配模型省去了获取对应点集时暴力搜寻导致的耗时，同时充分使用每帧数据，避免数据稀疏导致的匹配失误。m估计稳健回归思想与irls迭代对每个样本分别赋予权重，不同迭代还可根据前次估计离散程度动态调整取值，可加速收敛过程，减少迭代次数，更快地获得较优结果。

34、(4)低通滤波对估计结果的再处理使得本方法可应对一些结构化的环境，如agv在长廊环境中行走也能获得可用估计。