一种融入二维码信息的自适应蒙特卡诺定位方法与流程

本发明属于移动机器人导航领域，特别是一种融入二维码信息的蒙特卡洛定位方法。

背景技术：

在环境地图已知的前提下，移动机器人根据环境感知和自身运动确定其在环境中的位姿问题称为定位问题。蒙特卡诺定位(montecarlolocalization,mcl)算法以运动模型采样，并结合观测模型评估每个粒子的重要性权重，得到系统状态的后验信度分布。成功应用于移动机器人领域，适用于局部定位和全局定位两类问题。里程计运动模型通过整合车轮上的光电编码器信息，进而获得机器人相对于上一采样时刻位姿的相对差，在固定的时间间隔可进行位姿估计。但是由于漂移或打滑等因素的影响，导致运动模型的精度随时间的增大而下降，从而导致蒙特卡诺定位算法的定位误差增大；另外，重采样后粒子会产生退化效应，粒子多样性降低，固定大样本粒子会导致计算资源浪费，因此学者们一直在研究如何解决这两类问题。borenstein等人在90年代就将里程计误差分为系统误差和非系统误差两个部分，并提出了一种里程计标定方法“umbmark”消除系统误差对里程计精度影响，机器人按预定轨迹运动几次便可标定出差动轮模型参数。yap等人利用em算法并结合环境建图来同时计算里程计运动模型和激光器观测模型的参数，最后实现了在线自适应标定。alhashimi等人改进蒙特卡洛算法的观测模型，通过设置的阈值来确定粒子样本集合的大小，有效减小了计算量。黄露等人设计人工路标并建立了路标库来修正里程计的累计误差。但是由于人工路标设计复杂且路标库建立需要大量数据导致定位精度低，无法满足准确性且计算量大。

因此，一种融入二维码信息的自适应蒙特卡诺定位算法，在采样过程中融入二维码携带的绝对位置信息，改善里程计的类合计误差；激光传感器信息建立观测模型用于计算并更新粒子权重；并采用kullback-leibler距离(kullback-leiblerdistance，kld)重采样，根据采样在状态空间的近似分布的统计界限来在线确定粒子数，避免大的计算量。

技术实现要素：

本发明旨在解决以上现有技术的问题。提出了一种修正里程计累计误差、自适应调整粒子集合大小、降低计算量的融入二维码信息的自适应蒙特卡诺定位方法。本发明的技术方案如下：

一种融入二维码信息的自适应蒙特卡诺定位方法，其包括以下步骤：

s1，根据二维码提供的绝对位置信息和里程计控制量ut建立修正误差后的运动模型；

s2，根据修正误差后的运动模型和t-1时刻的采样集合χt-1进行粒子采样，估计机器人的初始位姿；

s3，采用二维激光传感器测距建立观测模型；

s4，根据给定地图m及观测模型的似然域计算各个粒子的重要性权重并跟新权值；

s5，根据粒子在状态空间的分布情况，kullback-leibler距离(kullback-leiblerdistance，kld)重采样自适应调整下一次迭代所需粒子数；

s6，根据粒子的分布情况确定机器人在环境中的位置。

进一步的，步骤s1建立修正误差后的运动模型具体包括：

在时间间隔(t-1,t]内，给定运动信息ut为：

其中分别表示里程计坐标系下t和t-1时刻的位姿，ut被转变成三个步骤的序列：初始旋转δrot1、平移δtrans和第二次旋转δrot2。建立运动误差的模型：

ε表示均值为0、方差为b²的噪声变量。参数α1-α4是针对机器人的误差参数，他们指定运动的累计误差，因此实际位置xt从xt-1经过初始旋转角跟随平移距离再跟随另一个旋转角得到，因此有：

则实际位置xt＝(x′,y′,θ′)。

进一步的，所述步骤s2粒子采样，估计机器人的初始位姿，具体包括：

运动模型采样将初始姿态xt-1、里程计读数ut和相机读数xc作为输入，机器人在运动过程中没有扫描到二维码时，位姿为当相机获取到二维码信息，与此时里程计采样输出的位姿xt进行比较，两者误差大于临界值τ，则采样算法输出的值为二维码坐标的绝对值xc，并令当前时刻的位姿xt为二维码信息提供的位姿，继续进行采样直到扫描下一个二维码。

进一步的，所述步骤s3采用二维激光传感器测距建立观测模型，具体包括以下步骤：

条件概率分布p(zt|xt,m)即为观测模型，每个单一观测的可能性相乘即可得到概率如下所示：

其中，xt是机器人的位姿，zt是t时刻的观测，zt^k表示t时刻的第k个测距值。m是环境地图，假设每个观测束噪声之间独立。

进一步的，所述步骤s4根据给定地图m及观测模型的似然域计算各个粒子的重要性权重并跟新权值，具体包括：

用似然域计算观测概率，以地图m为条件，得到x-y空间中与最近障碍物的距离dist：

由于传感器不同波束的噪声是相互独立的，对k个的值相乘，通过将一个正太分布和一个均匀分布混合得到观测模型的似然结果q：

给定三个参数zhit、zrand和zmax进行加权平均混合，并且zhit+zrand+zmax＝1。

进一步的，所述步骤s5中，kullback-leibler距离(kullback-leiblerdistance，kld)子重采样的计算方法：

kld采样对每次粒子滤波迭代都以概率1-δ确定样本数，使得真实的后验分布与基于采样的近似分布之间的误差小于ε，由此确定重采样样本集合的大小，当粒子数n满足一定值时，可以保证概率的真实值与估计值之间的k-l距离小于阈值ε，此时n的值为：

其中z1-δ是上分位数1-δ的标准正态分布，h表示至少填充了一个粒子的直方图的位数，在满足nx统计界限之前，kld采样将一直产生粒子。

本发明的优点及有益效果如下：

本发明提供了一种融入二维码信息的自适应蒙特卡诺定位方法。利用二维码提供的绝对位置信息修正里程计模型的累计误差后进行采样；有效修正累计误差提高定位精度；重采样部分利用kullback-leibler距离(kld)重采样，根据粒子在状态空间的分布情况自适应调整下一次迭代所需粒子数，减小了计算量。

附图说明

图1是本发明提供优选实施例融入二维码信息的自适应蒙特卡诺定位方法流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

如图1所示，本发明提供了一种融入二维码信息的自适应蒙特卡诺定位方法，其包括以下步骤：

s1，将时刻的采样集合χt-1，每个粒子对应机器人在这个点的估计运动轨迹，t-1时刻施加的控制量ut和二维码提供的全局坐标信息xc作为输入，二维码提供的绝对位置信息能修正里程计运动模型；

在时间间隔(t-1,t]内，给定运动信息ut为：

其中分别表示里程计坐标系下t和t-1时刻的位姿，ut被转变成三个步骤的序列：初始旋转δrot1、平移δtrans和第二次旋转δrot2。建立运动误差的模型：

ε表示均值为0、方差为b²的噪声变量。参数α1-α4是针对机器人的误差参数，他们指定运动的累计误差，因此实际位置xt从xt-1经过初始旋转角跟随平移距离再跟随另一个旋转角得到，因此有：

则实际位置xt＝(x′,y′,θ′)

s2，根据修正后的运动模型进行粒子采样估计机器人的初始位姿；

运动模型采样将初始姿态xt-1、里程计读数ut和相机读数xc作为输入，机器人在运动过程中没有扫描到二维码时，位姿为当相机获取到二维码信息，与此时里程计采样输出的位姿xt进行比较，两者误差大于临界值τ，则采样算法输出的值为二维码坐标的绝对值xc，并令当前时刻的位姿xt为二维码信息提供的位姿，继续进行采样直到扫描下一个二维码。

s3，采用二维激光传感器测距建立观测模型；用似然域计算观测概率，

条件概率分布p(zt|xt,m)即为观测模型，每个单一观测的可能性相乘即可得到概率如下所示：

其中，xt是机器人的位姿，zt是t时刻的观测，zt^k表示t时刻的第k个测距值。m是环境地图，假设每个观测束噪声之间独立。

s4，根据给定地图m及观测模型似然域计算各个粒子的重要性权重并跟新权值；

用似然域计算观测概率，以地图m为条件，得到x-y空间中与最近障碍物的距离dist：

由于传感器不同波束的噪声是相互独立的，对k个的值相乘，通过将一个正太分布和一个均匀分布混合得到观测模型的似然结果q：

给定三个参数zhit、zrand和zmax进行加权平均混合，并且zhit+zrand+zmax＝1。

s5，根据粒子在状态空间的分布情况自适应调整下一次迭代所需粒子数；

kld采样对每次粒子滤波迭代都以概率1-δ确定样本数，使得真实的后验分布与基于采样的近似分布之间的误差小于ε，由此确定重采样样本集合的大小，当粒子数n满足一定值时，可以保证概率的真实值与估计值之间的k-l距离小于阈值ε，此时n的值为：

其中z1-δ是上分位数1-δ的标准正态分布，h表示至少填充了一个粒子的直方图的位数，在满足nx统计界限之前，kld采样将一直产生粒子。

s6，根据粒子的分布情况确定机器人在环境中的位置。本发明可以对机器人进行精确定位。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。