一种移动机器人的同时定位与地图创建的动态纠正方法与流程

本发明涉及移动机器人定位技术领域，具体为一种移动机器人的同时定位与地图创建的动态纠正方法。

背景技术：

移动机器人通过定位与地图创建领域(slam)技术实现自主导航。

在实际情况中，道路表面、温度、轮胎状态以及履带的松紧度、驾驶的速度影响惯性导航系统的误差以及敏感的声纳传感器的探测传输数据时会发生无效数据信息的录入。还有探索未知环境会发生不可预知的障碍物干扰，比如遇到障碍物无法动弹，轮子打滑、遭遇“绑架”、路标受外力影响，针对未知环境下的移动机器人定位和地图创建时，在错误的无效数据影响或外部干扰情况下的准确性差。

对于控制和观测误差的无效数据筛选随着环境而动态变化的机制是尤为重要的。

所以目前缺少一种移动机器人在路径规划中的动态修正并形成新路径的算法。

技术实现要素：

本发明的目的在于提供一种移动机器人的同时定位与地图创建纠正方法，以解决由于传感器和外界障碍物造成的无效数据的误差因素对定位与地图影响导致偏离的问题。

为实现上述目的，本发明提供如下技术方案：本发明的一种移动机器人的同时定位与地图创建的动态纠正方法，包括以下步骤：

step1：数据采集校验，

分析整理历史消息记录在即时定位与地图构建时导致的偏差因素；

step2：调整修订参数，

首先，初始化移动机器人,默认起始位置为零位,

然后，根据移动机器人t-1时刻的位姿信息,即t-1的后验概率密度函数,通过已有的先验知识对当前时刻的状态进行预测,得到t时刻先验密度函数,根据先验概率密度函数生成采集粒子集,采集n个粒子,对粒子的权值进行初始化处理均为1/n；

再设定适应度函数，通过t-1时刻与t时刻观测的偏差因素,即设定控制误差协方差矩阵qt和观测误差协方差矩阵rt,

最后构建搜索更新模型，

由改进的分数阶粒子群算法搜索出q和r的一个预估值，根据预估值，完成qt和rt的修正；

step3：同时定位与地图创建，

根据qt和rt因素加入数据关联、位姿实现位姿更新、权重计算、路标更新步骤完成粒子集的更新；

二次采样，计算有效粒子数，进行归一化重采样。

进一步地，在步骤step1中，由专用的激光雷达和惯性导航传感器采集数据，然后对其进行数据化处理，激光雷达数据作为观测数据，转换距离和方向角,惯性导航数据作为控制数据，转换为速度和转向角，分析整理历史数据的偏差因素。

进一步地，在步骤step2中，所述的调整修订参数过程为

1)以上次位置为t-1时刻初始化移动机器人位置和地图，标记为第一位姿；

2)极坐标下预测t时刻位姿，标记为第二位姿；

3)计算第一位姿与第二位姿的误差协方差矩阵qt和误差协方差矩阵rt的适应度函数；

4)将第一位姿和第二位姿的数据关联；

5)根据搜索更新模型计算出t+1时刻的预估值，标记为第三位姿；

6)以此类生成有效粒子，根据粒子集推绘示出观测的概率分布，进行历史数据的调整，调整参数。

更进一步地，所述的构建搜索更新模型为使用极坐标系得到的qt和rt调整粒子x^[ex]如下：

其中，被设置为初始化原点[0,0,0]，基于笛卡尔坐标系下的运动和观测模型，极坐标下的运动和观测模型如下：

进一步地，q和r的适应度函数为

进一步地,所述的第三位姿搜索过程为

其中，α是阶数，根据搜索的轨迹信息动态调整，

与现有技术相比，本发明的有益效果是：

在fastslam框架中设定动态修订适应度函数，分数阶粒子群搜索模型，从而提高了移动机器人路径规划，动态筛选并修正无效数据带来的干扰，提高线路规划的精准度以及稳定性、可靠性。

附图说明

图1为本发明的基于不同先验知识的fastslam算法比较图；

图2为本发明的基于极坐标的观测模型图；

图3为本发明的算法详细流程图；

图4为本发明的两种算法基于错误q下的实验结果图；

图5为本发明的两种算法基于错误r下的实验结果图；

图6为本发明的错误q下机器人位姿上的均方根误差图；

图7为本发明的错误q下路标位置上的均方根误差图；

图8为本发明的错误r下机器人位姿上的均方根误差图；

图9为本发明的错误r下路标位置上的均方根误差图；

图10为本发明的两种算法基于错误q下的实验结果图；

图11为本发明的两种算法基于错误r下的实验结果图；

图12为本发明的两种算法基于临时控制错误下的实验结果图；

图13为本发明的错误q下机器人位姿上的均方根误差图；

图14为本发明的错误q下路标位置上的均方根误差图；

图15为本发明的错误r下路标位置上的均方根误差图；

图16为本发明的错误r下路标位置上的均方根误差图；

图17为本发明的基于错误的q在“carparkdataset”数据集下slam实验图；

图18为本发明的基于错误的r在“carparkdataset”数据集下slam实验图。

具体实施方式

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

为了更好的阐述，其中，错误的先验知识为传感器录入的无效数据或干扰数据，实施例中先验知识以及公开的在先实例的推导，是对更好说明本领域技术人员的操作性说明。

实施例1

如图1-3所示，本实施例提供了一种移动机器人的同时定位与地图创建的动态纠正方法，包括以下步骤：

step1：数据采集校验，

分析整理历史消息记录在即时定位与地图构建时导致的偏差因素；

step2：调整修订参数，

首先，初始化移动机器人,默认起始位置为零位,

然后，根据移动机器人t-1时刻的位姿信息,即t-1的后验概率密度函数,通过已有的先验知识对当前时刻的状态进行预测,得到t时刻先验密度函数,根据先验概率密度函数生成采集粒子集,采集n个粒子,对粒子的权值进行初始化处理均为1/n；

再设定适应度函数，通过t-1时刻与t时刻观测的偏差因素,即设定控制误差协方差矩阵qt和观测误差协方差矩阵rt,

最后构建搜索更新模型，

由改进的分数阶粒子群算法搜索出q和r的一个预估值，根据预估值，完成qt和rt的修正；

step3：同时定位与地图创建，

根据qt和rt因素加入数据关联、位姿实现位姿更新、权重计算、路标更新步骤完成粒子集的更新；

二次采样，计算有效粒子数，进行归一化重采样。

在步骤step1中，由专用的激光雷达和惯性导航传感器采集数据，然后对其进行数据化处理，激光雷达数据作为观测数据，转换距离和方向角,惯性导航数据作为控制数据，转换为速度和转向角，分析整理历史数据的偏差因素。

在步骤step2中，所述的调整修订参数过程为

1)以上次位置为t-1时刻初始化移动机器人位置和地图，标记为第一位姿；

2)极坐标下预测t时刻位姿，标记为第二位姿；

3)计算第一位姿与第二位姿的误差协方差矩阵qt和误差协方差矩阵rt的适应度函数；

4)将第一位姿和第二位姿的数据关联；

5)根据搜索更新模型计算出t+1时刻的预估值，标记为第三位姿；

6)以此类生成有效粒子，根据粒子集推绘示出观测的概率分布，进行历史数据的调整，调整参数。

所述的构建搜索更新模型为使用极坐标系得到的qt和rt调整粒子x^[ex]如下：

其中，被设置为初始化原点[0,0,0]，基于笛卡尔坐标系下的运动和观测模型，极坐标下的运动和观测模型如下：

临近两次观测zt-1和zt不一致性定义的适应度函数来实现的；

假定移动机器人在t-1时刻处于原点，其位姿和路标位置均采用极坐标，在t-1时刻，机器人极坐标下的位置和转向角均为0；

zt-1用来表示地图信息，地图估计呈现以zt-1为均值的正态分布：

由于存在控制和观测误差，zt和zt-1一般是很难一致的，基于前后观测的不一致定义了适应度函数，它是机器人位姿的估计和基于位姿的观测估计的乘积：

和代表的是能使适应度函数值最大的控制误差协方差、观测误差协方差和t时刻机器人的位姿。表示机器人在t时刻的位姿估计，机器人位姿估计服从如下的正态分布：

函数f是极坐标系下的运动模型，fu是函数f的雅可比矩阵，基于位姿的观测估计服从如下的正态分布：

函数s是极坐标系下的观测模型，基于rao-blackwellise分解的思想，可以写成下面的形式：

函数s一阶泰勒展开式如下：

和是根据极坐标下运动和观测模型预测的t时刻机器人位姿和观测信息。是第j个路标t-1时刻的位置估计，sx和sm是观测函数关于机器人位姿和路标位置的雅可比矩阵，通过观测函数的近似表示，基于位姿的观测估计服从均值为协方差为zt的正态分布：

通过移动机器人位姿的估计和基于位姿的观测估计的分别推导，适应度函数yt形式如下：

故而，所述q和r的适应度函数为

所述的第三位姿搜索过程为

其中，α是阶数，根据搜索的轨迹信息动态调整，

实施例2

本实施例提供一种移动机器人的同时定位与地图创建的动态纠正方法的推演过程，每个粒子记录着当前的位置、速度和已访问过的最优位置，代表了算法的一种解决方案，迭代更新过程中，每个粒子都朝着全局最优和自己曾到达过最优位置的方向上移动：

xi(k)＝xi(k)+vi(k+1)

xi(k)表示第i个粒子在第k次迭代中的位置，粒子基于vi(k+1)的速度从位置xi(k)飞行到xi(k+1)位置，vi(k+1)的速度由惯性、认知和社会三个部分组成，惯性部分模拟鸟儿上一次的运行轨迹，ω是惯性权重，代表了以前的轨迹对当前新轨迹的影响，c1和c2是认知部分和社会部分的加速系数，r1和r2是随机生成的数值，pib(k)和pgb(k)分别代表鸟儿和鸟群已经到达过的最优位置。

基于传统pso算法收敛速度慢、局部收敛问题以及qt和rt调整的适应度函数的多模特性，提出基于基于grünwald–letnikov形式分数阶和分形分布的粒子群搜索算法来搜索最优的和使得适应度函数最大化，粒子的更新结合分数阶微积分具有的记忆特性，融入了轨迹信息，提高了算法的收敛速度，使用分形分布代替均匀分布使得粒子在一定概率条件下可以逃逸局部极小点，提高粒子的全局搜索能力，使得搜索得到的和更接近q和r的真实值；

grünwald–letnikov是一种应用广泛的分数阶微积分定义形式，其离散形式如下：

其中αd^t表示α阶微积分，α为负是分数阶积分，正则是分数阶微分，t是采样区间，r是截断次数，将上述公式惯性权重ω设置为1，可写为下面的形式：

vi(k+1)-vi(k)＝φ1+φ2

其中φ1＝c1r1(pib(k)-xi(k))，φ2＝c2r2(pgb(k)-xi(k))，公示左边部分vt+1,i-vt,i是一阶差分，在pso算法中，粒子飞行是基于离散时间的，其时间间隔t＝1，粒子速度的差分形式如下：

αd^tf(t)＝φ1+φ2

根据公式和伽玛函数的递推公式，差分公式前4项的近似表达式如下：

α将随着搜索的过程而动态调整。阶数α根据搜索的轨迹信息动态调整。初始值为0.5，区间为[0.4，0.8]，每50步调整一次。

dgbest是最优粒子和各个粒子距离的总和。dmax是任意两个粒子之间最大的距离，dmin是任意两个粒子之间距离

分形分布的分布通过尺度因子、特性指数、移位参数和偏度参数来表示，其概率密度函数可以用特征函数的连续傅立叶变换来定义；改进的分数阶粒子群优化算法使用分形分布代替公式中随机数生成的方法，生成基于分形分布的随机数，随机数生成流程如下：

首先生成两个独立的随机变量v和w，v是一个在区间均匀分布的变量，w是一个均值为1的指数分布变量，然后根据v和w生成服从s(α,β,1,0)随机变量x：

其中sα,β和bα,β定义如下：

最后根据随机变量x生成服从s(α,β,σ,μ)的随机变量y：

提出算法采用α＝1.5的莱维分布，这种分布介于高斯分布α＝2.0和柯西分布α＝1.0；基于该分布的随机函数在一定的概率条件下可以获得较大的值，从而使得粒子有机会逃逸极小值点，扩大其搜索范围，算法描述了使用基于grünwald–letnikov形式的分数阶粒子群搜索算法搜索最优和使得适应度函数最大的过程，具体流程如下：

输入：

t时刻移动机器人预测位姿(假定t-1时刻机器人处于原点)，t-1时刻控制和观测协方差矩阵qt-1、rt-1，t和t-1时刻的观测输入z和zt-1；

步骤：

//初始化粒子群

//根据观测路标数量、搜索迭代次数和收敛情况确定搜索是否结束

while(isstop()＝＝false)

//根据公式计算出适应度值，更新每个粒子的最优值和全局最优值

calculateevaluation(particles)；

fori＝1tolength(particles)

updatevelocity(particles(i))；//根据公式(3-31)更新粒子速度

updateposition(particles(i))；//根据公式(3-27)更新粒子位置

endfor

endwhile

输出：

使得适应度函数尽可能大的控制和观测误差协方差以及t时刻机器人位姿和

实施例3

本实施例提供一种移动机器人的同时定位与地图创建的动态纠正方法，包括以下步骤：

建立用于qt和rt调整粒子的运动和观测模型；

提出的先验知识修正fastslam算法在粒子更新之前增加了qt和rt调整的过程，每个粒子更新和重采样的方法仍采用fastslam2.0的算法；由于qt和rt调整的过程类似于fastslam2.0粒子的更新过程，将调整过程中用于调整qt和rt的移动机器人位姿和环境信息(基于不同坐标系和原点位置)作为一个特殊粒子x^[ex]看待，qt和rt调整过程如下：

1)初始化：如果本次或者上次观测的路标数量过少，则无法确定控制和观测误差在临近两次观测不一致性中的相应影响。当本次或者上次观测的路标数量小于3，不执行qt和rt的调整；否则将移动机器人上一时刻的位置设为原点，转向角设为0，地图信息由上一次的观测zt-1表示：

2)位姿预测：根据极坐标下的运动模型预测t时刻机器人的位姿，

3)数据关联：该过程将临近两次观测到的路标进行关联，调整过程使用动态联合最近邻算法(dynamicjointnearestneighbor,djnn)实现数据关联，该算法在最近邻算法基础上进行了改进，通过观测之间的相关性消除了关联匹配结果之间的干涉情况，同时通过预处理滤除观测特征中的伪特征，不仅提高了数据关联的准确度，也提高了数据关联的效率；

4)通过基于grünwald–letnikov形式的分数阶粒子群搜索算法搜索最优参数：将预测的位姿上一时刻qt-1和rt-1以及观测zt和zt-1作为输入参数，通过基于grünwald–letnikov形式的分数阶粒子群搜索算法搜索最优的和使得适应度函数值尽可能大：

5)观测预测，根据机器人位姿和地图信息预测t时刻的观测信息：

6)qt和rt的调整：根据预测的机器人位姿和通过粒子群搜索的进行对比，当两者之间差值较大的时候，调整qt和rt

是第j个路标的观测值，是第j个路标预测的观测值，mo是临近两次重复观测到的路标数量，

使用表示运动误差，dxt～n(0,1)服从正态分布，为了避免频繁地调整，只有当一定概率事件发生的情况下才调整qt，如果|dxt|大于1.4或者小于0.2，qt调整如下：

否则qt不调整：

qt＝qt-1

ωq是一个根据观测到的路标数量动态调整的参数，设定代表第j个路标的观测误差，mn表示绝对值大于1.4的数量，ln表示绝对值小于0.2的数量，

如果mnp或者lnp大于设定的阀值nmax，rt调整如下：

其中,ωr是一个调整系数，当m0减少的时候，ωq和ωr减少；反之，ωq和ωr增加；

第五步：对模型进行仿真实验；

用于调整qt和rt的特殊粒子x^[ex]使用极坐标系来表示移动机器人位姿和路标位置：

被设置为初始化原点[0,0,0]，参照上述公式笛卡尔坐标系下的运动和观测模型，极坐标下的运动和观测模型如下：

预测的机器人位置第j个路标预测位置和三个点构成如图2所示的三角形：

和分别是端点和的角度，三条边的长度分别是和通过观测模型，计算出第j个路标的预测位置：

通过三角公式计算和的距离即移动机器人t时刻对第j个路标的预测距离：

通过三角公式计算端点的角度

如果γo在[0,π]和[-2π,-π]区间，在端点到连线之下，则在区间

如果γo在[π,2π]和[-π,0]区间，在端点到连线之上，则在区间

考虑移动机器人的转向，其t时刻对第j个路标预测的观测角度为：

第六步：实验结果及分析：为了测试算法的性能，设计仿真环境实验对比fastslam2.0和基于先验知识修正的改进fastslam算法流程，仿真环境基于matlab2013平台，移动机器人的运动速度是3米/秒，最大转向角弧度。速度误差εv是0.3米/秒，转向转角误差为弧度，机器人配置了π弧度，最远观测距离为30米的激光雷达，观测的距离误差为0.2米，角度误差为弧度：

实施例4

稀疏路标环境下的实验；

设计一个大小为250m×200m包含35个路标的环境，移动机器人的初始位置为(0,0)，先验知识q是错误的，被放大6倍，r是正确的。基于此设定，分别采用fastslam2.0算法和基于先验知识修正的改进fastslam算法流程对移动机器人的路径和环境中的路标进行估计，两种算法使用的粒子数量均为20，仿真实验结果如图4所示，

其中,+号表示路标的真实位置，

○表示提出算法路标估计位置，

□表示fastslam2.0算法路标估计位置，

实线表示移动机器人的运动轨迹，

虚线代表提出算法和fastslam2.0算法对于移动机器人路径的估计；

由于仿真环境中q被放大，所以移动机器人估计的位置将更加接近于观测的信息，这种对观测信息的过分信任降低了slam估计的精确度，在此实验仿真环境中，机器人能够观测到的路标大部分情况下在3个左右，虽然不是非常充分，但是经过若干次迭代调整后的qt和rt值也能接近于真实值，为了评估对于先验知识的估计，进行了50次仿真实验对比，经过前期的调整，对于qt的大部分的修正可以达到85％以上的准确度，提出算法经过前期先验知识的修正提高了slam估计的精度，从图中可以看出提出算法比fastslam2.0算法在机器人位姿和路标位置估计精度上有了明显的提高；

基于与图4相同的仿真环境，设定先验知识r是错误的，被放大6倍，q是正确的，使用fastslam2.0算法和基于先验知识修正的改进fastslam算法对移动机器人的路径和环境中的路标进行估计，由于先验知识r被放大，机器人位姿估计更接近于由运动模型预测的结果，这种对控制信息的过分信任降低了估计的精确度，而提出算法通过引入先验知识修正环节提高了slam估计的精度；图5的仿真实验结果可以看出基于先验知识修正的的改进fastslam算法优于fastslam2.0算法；

由于在仿真环境中控制和观测噪声是随机产生的，每次仿真实验的结果都是不一样的，为了获得更精确的比较，以均方根误差(rmse)为标准，执行50次仿真实验；表1显示仿真实验结果，其中rmse_p表示位置估计的均方根误差，rmse_l表示路标估计的均方根误差，提出算法为了获得更精确的slam估计，引入了qt和rt的调整，增加了算法的执行时间；

表1在稀疏路标环境下的仿真实验

基于错误先验知识q的50次仿真实验中，移动机器人每次运动了大约220秒，图6显示了两种算法基于时间均方根误差曲线图，图中包含四副子图，分别是x轴、y轴、方向角和位置的误差随时间变化的情况，移动机器人在最后时刻重新观测到初始时刻观测到的路标，从而导致位姿估计误差开始减小，从图中可以看出基于先验知识修正的改进fastslam算法比fastslam2.0算法在所有时间段都更接近于移动机器人的真实位姿；图7显示移动机器人观测到的30个路标在最终时刻位置估计的均方根误差，从图中可以看出提出算法对于路标位置的估计要优于fastslam2.0算法：

图8显示两种算法在错误先验知识r下经过50次仿真实验的移动机器人位姿均方根误差曲线图，图9显示30个路标位置估计的均方根误差，从图中可以看出提出算法比fastslam2.0算法估计的均方根误差要小，估计更加精确。

实施例5

密集路标环境下的实验：

其仿真环境是一个250m×200m的包含135个路标的区域，设定先验知识r是错误的，被放大6倍，q是正确的，在此环境下分别采用fastslam2.0算法和基于先验知识修正的改进fastslam算法对移动机器人的路径和环境中的路标进行估计，图10显示了实验结果；由于q被放大6倍，移动机器人位姿和路标的估计更接近于观测的信息，这种对观测信息的过分信任降低了估计的精确度；在密集路标仿真环境中，机器人能够观测到的路标都超过10个，提供充足的信息用于先验知识的调整，所以在改进算法中调整后的qt和rt非常接近于真实值；为了评估对于先验知识的估计，进行50次仿真实验对比；其中43次实验经过32次迭代后对于rt的估计误差在10％以内，对于qt的估计误差在20％以内，提出算法经过前期的修正获得了比较精确的对于先验知识的估计，提高了机器人位置估计的精度，从图中可以看出提出算法比fastslam2.0算法在机器人位姿和路标位置估计精度上有了明显的提高；

图11显示基于错误先验知识r下的仿真实验结果，r被放大6倍，被放大的r导致对控制信息的过分信任降低了估计的精确度，尤其是在密集路标的环境中，图11验证了提出算法通过先验知识的修正可以改进slam的估计精度；

在密集路标环境下，模拟由于各种内外部原因导致控制误差临时极具增大的情况，仿真实验中，移动机器人在4到8秒的时候实际运行速度是0.1米/秒，而传感器获得的速度仍然是3米/秒，8秒以后运行速度恢复到3米/秒，真实环境中也经常会遇到这类情况，比如机器人被某些物体阻碍了前进，虽然后轮的速度没有变化，但实际速度是变慢了，在slam迭代更新移动机器人系统状态时，能够观测到的路标还是比较充分的，所以能够很快调整qt和rt的值；图12显示实验结果，可以看出基于先验知识修正的改进fastslam算法在足够路标观测的情况下，对于slam的估计基本不受外部干扰的影响，而fastslam2.0算法由于临时控制错误导致估计精度明显降低；

在密集路标环境下获得两种算法更精确的比较，执行50次仿真实验；表2显示实验结果，其中rmse_p表示位置估计的均方根误差，rmse_l表示路标估计的均方根误差，从表中可以看出基于先验知识修正的改进fastslam算法无论是机器人位置估计还是路标位置估计都优于fastslam2.0算法；

表3.2在密集路标环境下的仿真实验

基于错误先验知识q的50次仿真实验中，机器人每次在仿真环境中运动220秒左右。图13显示了两种算法x轴、y轴、方向角和位置均方根误差随时间变化的曲线图。从图中可以看出提出算法在移动机器人位姿的估计上优于fastslam2.0算法，图14显示路标位置均方根误差曲线图，同机器人的位姿一样，提出算法路标位置的估计误差也小于fastslam2.0算法；

参阅图15：显示了两种算法在错误先验知识r的经过50次仿真实验的均方根误差，由于仿真环境中路标数量较多，所以错误先验知识r对于算法精确度的影响更大，从图15和16中可以看出在错误先验知识r下，提出算法对于机器人位姿和路标位置的估计具有更高的精度；通过上面的仿真实验可以验证提出算法在基于错误先验知识的情况下可以获得更精确的估计，同时针对诸如遇到障碍物卡住，轮子打滑导致机器人测量的控制数据与实际差距很大的问题，也能做到尽可能的修正。

实施例6

在真实环境中的可行性和有效性，使用slam研究领域的一个标准数据集“universitycarpark”来进行实验；该数据集采集自悉尼大学acfr的智能车辆，

停车场环境大约为45×30m²，放置了一些人工路标，智能车辆在停车场中低速行驶，记录了惯导传感器、激光雷达和gps在各个时刻的测量值，通过惯性导航系统观测到的测量值代入公式可以预测移动机器人在下一时刻的位姿，通过激光雷达观测到的数据来修正移动机器人位姿和地图估计；

图17和18显示两种算法在“universitycarpark”数据集下基于不同先验知识情况下的实验结果：图17基于错误的先验知识q，q被放大3倍，图18是基于错误的先验知识r，r被放大3倍，从图中可以看出，改进算法估计的路径比fastslam2.0算法要更接近gps，估计的路标位置也更接近路标真实的位置：

表3显示了基于先验知识修正的改进fastslam算法估计的路标误差小于fastslam2.0算法，由于增加了调整环节，运行时间也大于fastslam2.0算法，由于该数据集中观测到的路标数量并不多，提出算法总共调用了57次，单步执行时间是0.0020秒，小于0.025秒的采样周期，能够满足实时性要求：

表3两种算法的路标位置误差

综上所述：本发明提供的一种基于先验知识修正的改进fastslam算法，针对错误先验知识影响slam估计精度的问题，提出算法将运动和观测噪声的协方差设定为动态的，协方差修正的过程被融入到fastslam框架中；临近两次观测不一致性被定义为适应度函数更能反映出真实误差情况，使用基于grünwald–letnikov形式的分数阶粒子群搜索算法搜索先验知识的预估值，搜索过程融入历史轨迹信息提高了收敛速度，使用alpha分形分布提高了全局搜索能力，仿真以及slam领域标准数据集“universitycarpark”的实验表明在错误先验知识的情况下，提出算法可以提高移动机器人定位和地图创建的精度。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。