一种基于粒子滤波器的AGV激光定位方法与流程

本发明涉及导航定位领域，具体涉及一种基于粒子滤波器的agv激光定位方法。

背景技术：

随着我国工业自动化和信息化的大力发展，工业4.0的大潮引领各行业工厂逐步实现智能化生产与运输。在仓储搬运中实现自动化与智能化的agv，作为一种新型物流装置成为市场新宠。

传统的agv导航技术由最开始的电磁导航发展到后来的磁钉导航，该技术通过感知地面的磁钉实现自身的定位与导航。然而磁钉的铺设需要耗费大量的人力和财力，且不具灵活性。随着导航技术的发展，出现了二维码导航方式，该导航方式是利用视觉传感器识别地面二维码来进行导航，这种导航方式要比磁钉导航定位精确，在铺设、改变路径上也较容易。但是二维码导航的agv也需要定期维护，如果有人干涉或别的agv经过，很容易把地上的二维码碾坏，需要频繁更换二维码。

技术实现要素：

(一)解决的技术问题

针对现有技术所存在的上述缺点，本发明提供了一种基于粒子滤波器的agv激光定位方法，能够有效克服现有技术所存在的需要耗费大量人力物力、无法灵活改变路径的缺陷。

(二)技术方案

为实现以上目的，本发明通过以下技术方案予以实现：

一种基于粒子滤波器的agv激光定位方法，包括以下步骤：

s1、根据agv的先验概率p(x0)进行粒子初始化采样；

s2、借助重要性概率密度函数对初始化采样粒子进行重要性采样，生成采样粒子，并更新粒子集；

s3、计算粒子权值并进行归一化处理；

s4、设定粒子权值的方差阈值，判断粒子权值方差与方差阈值的大小关系；

s5、若粒子权值方差较大，则返回s2进行粒子重采样，否则进入s6；

s6、利用粒子权值和粒子集输出agv精准位姿数据。

优选地，所述先验概率p(x0)表示初始时刻运动状态概率分布，所述初始化采样粒子中的i表示粒子采样个数，其中i＝1，2…n。

优选地，所述重要性采样通过采样己知的重要性概率密度函数来间接估计粒子状态。

优选地，所述重要性概率密度函数以agv编码器组建的里程计运动模型构建，从已知分布中进行n个样本的采样。

优选地，所述重要性概率密度函数表示重要性概率密度函数的分布与系统过程噪声分布相同。

优选地，所述粒子权值采用权重递推方式计算，所述粒子权值满足下式：

由于所以其中与激光雷达测量噪声分布相同。

优选地，将k时刻状态估计值作为所述agv精准位姿数据。

(三)有益效果

与现有技术相比，本发明所提供的一种基于粒子滤波器的agv激光定位方法相比传统磁条、磁钉导航方式，无需在场景中增加任何辅助定位标签，使得agv切换搬运场景更加灵活，降低了导航成本；相比利用视觉传感器识别二维码的导航方式，其具有更好的稳定性，定位精度和重复定位精度更高。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明流程示意图；

图2为本发明粒子初始状态示意图；

图3为本发明粒子滤波定位最终状态示意图；

图4为本发明基于粒子滤波器的定位轨迹；

图5为本发明基于粒子滤波器的定位误差示意图。

具体实施方式

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

一种基于粒子滤波器的agv激光定位方法，如图1所示，包括以下步骤：

s1、根据agv的先验概率p(x0)进行粒子初始化采样；

s2、借助重要性概率密度函数对初始化采样粒子进行重要性采样，生成采样粒子，并更新粒子集；

s3、计算粒子权值并进行归一化处理；

s4、设定粒子权值的方差阈值，判断粒子权值方差与方差阈值的大小关系；

s5、若粒子权值方差较大，则返回s2进行粒子重采样，否则进入s6；

s6、利用粒子权值和粒子集输出agv精准位姿数据。

先验概率p(x0)表示初始时刻运动状态概率分布，初始化采样粒子中的i表示粒子采样个数，其中i＝1，2…n。

重要性采样通过采样己知的重要性概率密度函数来间接估计粒子状态。

重要性概率密度函数以agv编码器组建的里程计运动模型构建，从已知分布中进行n个样本的采样。

重要性概率密度函数表示重要性概率密度函数的分布与系统过程噪声分布相同。

粒子权值采用权重递推方式计算，粒子权值满足下式：

由于所以其中与激光雷达测量噪声分布相同。

将k时刻状态估计值作为agv精准位姿数据。

如图2至图5所示，为根据本申请技术方案设计的一个仿真实验。假设运动小车的初始位置、状态方程(运动预测方程)、传感器测量数据的详细参数如表1所示，用粒子滤波方法对其进行定位。

表1仿真预设参数

在仿真过程中，粒子的初始值设置为真实值，定位误差为零。在整个场地内初始化粒子群，将200个粒子群均匀分布，如图2所示。小车按照叠加了控制噪声的运动方程进行运动，运动到达下一位置后，传感器对当前位置进行测量，并得到一个测量位置(不准确，含有测量噪声)，这200个均匀分布粒子群根据随机重采样的粒子滤波算法开始移动。移动结束后，可画出小车实际位置、重采样后粒子群位置、以及粒子群的几何中心，如图3所示。

粒子滤波算法对粒子进行预测、校正、重采样、滤波处理，可以得到整个运动过程中agv的真实轨迹、测量轨迹、粒子群几何中心轨迹，如图4所示，得到定位轨迹后，再计算出200个粒子群的定位误差，如图5所示。由图5可以看出，采用粒子滤波算法得到的定位误差非常小，即本申请技术方案具有较高的定位精度。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不会使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

技术特征：

1.一种基于粒子滤波器的agv激光定位方法，其特征在于：包括以下步骤：

s1、根据agv的先验概率p(x0)进行粒子初始化采样；

s2、借助重要性概率密度函数对初始化采样粒子进行重要性采样，生成采样粒子，并更新粒子集；

s3、计算粒子权值并进行归一化处理；

s4、设定粒子权值的方差阈值，判断粒子权值方差与方差阈值的大小关系；

s5、若粒子权值方差较大，则返回s2进行粒子重采样，否则进入s6；

s6、利用粒子权值和粒子集输出agv精准位姿数据。

2.根据权利要求1所述的基于粒子滤波器的agv激光定位方法，其特征在于：所述先验概率p(x0)表示初始时刻运动状态概率分布，所述初始化采样粒子中的i表示粒子采样个数，其中i＝1，2…n。

3.根据权利要求1所述的基于粒子滤波器的agv激光定位方法，其特征在于：所述重要性采样通过采样己知的重要性概率密度函数来间接估计粒子状态。

4.根据权利要求3所述的基于粒子滤波器的agv激光定位方法，其特征在于：所述重要性概率密度函数以agv编码器组建的里程计运动模型构建，从已知分布中进行n个样本的采样。

5.根据权利要求3或4所述的基于粒子滤波器的agv激光定位方法，其特征在于：所述重要性概率密度函数表示重要性概率密度函数的分布与系统过程噪声分布相同。

6.根据权利要求1所述的基于粒子滤波器的agv激光定位方法，其特征在于：所述粒子权值采用权重递推方式计算，所述粒子权值满足下式：

由于所以其中与激光雷达测量噪声分布相同。

7.根据权利要求1所述的基于粒子滤波器的agv激光定位方法，其特征在于：将k时刻状态估计值作为所述agv精准位姿数据。

技术总结
本发明涉及导航定位领域，具体涉及一种基于粒子滤波器的AGV激光定位方法，根据AGV的先验概率p(x0)进行粒子初始化采样，借助重要性概率密度函数对初始化采样粒子进行重要性采样，生成采样粒子，并更新粒子集，计算粒子权值并进行归一化处理，设定粒子权值的方差阈值，判断粒子权值方差与方差阈值的大小关系，利用粒子权值和粒子集输出AGV精准位姿数据；本发明提供的技术方案能够有效克服现有技术所存在的需要耗费大量人力物力、无法灵活改变路径的缺陷。

技术研发人员：姜跃君;吴正
受保护的技术使用者：安徽意欧斯物流机器人有限公司
技术研发日：2020.03.05
技术公布日：2020.06.09