本发明涉及机器人导航领域，具体涉及一种全局优化的并发定位与建图方法。
背景技术
随着计算机和人工智能等学科的发展，移动机器人相关技术得到了突飞猛进的发展，是目前科学技术最前沿的一个领域。移动机器人应用场景非常广阔，主要包括卫生清洁，医疗辅助，安保消防等领域。而为了完成这些任务，就需要移动机器人具有感知周围环境信息和自身状态的能力。在已知环境下，机器人可以通过对路标点的感知，纠正自身的位姿信息。然而在周围环境信息未知时，移动机器人需要在没有先验信息的情况下，在自身运动的过程中一边建立周围环境的模型即建图，一边估计自身的运动状态即定位。在这个过程中机器人的建图和定位的精度是相互影响的，这就引出了机器人领域的热门研究同时定位与建图问题(simultaneouslocalizationandmapping，slam)。
slam研究的是机器人在陌生环境下通过本体携带的传感器构建地图，同时确定自己所在地图的位置的问题。slam系统主要有三个组成部分，并且是一个迭代的过程。首先通过当前机器人的位姿信息和运动方程估计下一时刻机器人的位姿信息。然后通过相机或者激光雷达等传感器获取路标信息，并且把路标信息和机器人的位姿信息相关联。等到下一时刻获取到了新的路标信息和机器人位姿信息之后，对于第一部分的信息进行更新。
通过贝叶斯滤波器解决slam问题的方法主要有两种。一种是基于扩展卡尔曼滤波(extendedkalmanfilter，ekf)的slam算法。一种是基于粒子滤波(particlefilter，pf)的slam算法。ekf的方法解决了卡尔曼滤波(kalmanfilter，kf)仅限于处理线性系统的问题，将之扩展到了非线性系统问题的应用当中。ekf需要保证噪声服从高斯分布，而粒子滤波(particlefilter，pf)是非线性非高斯的，适用性更好。粒子滤波的方法是在机器人的初始位置周围按照某种先验分布随机产生一定数目的粒子，然后根据状态转移方程更新粒子的位姿信息。通过观测模型获取的路标点评估当前粒子的优劣，计算权重根据权值进行重采样，然后进行下一时刻的估计。但是为了提高粒子滤波的效果不得不增加粒子的数目，从而造成计算量的增大。
技术实现要素：
本发明的目的在于：提供一种全局优化的并发定位与建图方法，解决了目前采用粒子滤波的slam算法实现机器人的定位与建图时定位精度低的技术问题。
本发明采用的技术方案如下：
一种全局优化的并发定位与建图方法，包括以下步骤：
步骤1：根据机器人的初始坐标，在初始时刻随机生成n个粒子，得到初始时刻的粒子集；
步骤2：采用粒子滤波的slam算法，利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息；
步骤3：采用全局优化和重采样对当前时刻各粒子进行更新；
步骤4：利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点；
步骤5：重复步骤2-4，直至机器人完成所有时刻的运动。
进一步的，所述步骤2具体为：
步骤21：采用机器人运动模型，利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据，并记录当前时刻粒子观测到的路标点；
步骤22：采用提议分布对当前时刻的位姿数据和路标点位置进行更新，利用更新后的位姿数据和路标点位置计算各粒子的权重；
步骤23：对所述权重进行归一化处理，得到当前时刻各粒子的权重。
进一步的，所述步骤3具体步骤为：
步骤31：对当前时刻的各粒子进行克隆和变异操作，更新粒子集中的粒子；
步骤32：以更新后粒子集中的各个粒子为引领者随机产生n个跟随者，计算跟随者和引领者的可能性，利用可能性筛选所述引领者和跟随者，得到再次更新后的粒子集；
步骤33：利用再次更新后粒子集中粒子的权重计算有效粒子的数目neff，当有效粒子数目neff大于设定的阈值φ，则不进行重采样，否则进行重采样，得到当前时刻最终的粒子集。
进一步的，所述步骤31具体为：
步骤311：利用步骤2得到的粒子信息中的权重
步骤312：利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目ncl，计算公式如下：
步骤313：将新获得的粒子添加至所述粒子集中，更新当前时刻的粒子集，利用所述亲和力faffinity对粒子进行变异操作，变异采用的公式为：
步骤314：利用变异后粒子间的位姿差计算两两粒子间的排斥力freject，计算公式为：
步骤315：当粒子总数大于m时，将排斥力小于设定阈值的两个粒子中的一个抛弃，更新所述粒子集合ω，其中权重
进一步的，所述步骤32具体为：
步骤321：对步骤21更新后粒子集中的每个粒子，以各个粒子为引领者随机产生对应的n个跟随者，分别计算引领者与跟随者的适应度ffitness，计算适应度采用的函数为：
步骤322：利用所述适应度ffitness计算各个粒子的可能性pi，采用的公式为：
步骤323：判断引领者和跟随者的可能性pi，当跟随者可能性大于引领者可能性，即利用可能性最大的跟随者替换引领者，否则不进行替换，得到再次更新后粒子集。
进一步的，所述步骤33具体为：
步骤331：利用更新后粒子集中粒子的权重计算有效粒子的数目neff，计算公式为：
步骤332：当有效粒子数目neff大于设定的阈值φ，则不进行重采样，得到当前时刻最终的粒子集，否则进行重采样后跳转至步骤31。
进一步的，所述重采样为：复制权值大的粒子，去掉权值小的粒子进行，保持粒子集中粒子数目稳定。
进一步的，所述步骤4具体为：
步骤41：利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置，对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新，得到当前时刻的路标点位置；
步骤42：利用当前时刻最终粒子集中各个粒子粒子信息中的位姿数据
步骤43：利用当前时刻最终的路标点位置和路径中各点的位置，得到当前时刻机器人运动路径和路标点。
综上所述，由于采用了上述技术方案，本发明的有益效果是：
1.本发明能够极大的提升机器人的定位精度。
2.采用本发明公开的全局优化方法，粒子复制的数目随着亲和力的增加而减少，也就是对权值高的粒子进行多复制，权值小的粒子减少复制，通过这种方法可以有效的抑制亲和力高的抗体而促进亲和力低的抗体，加快收敛速度；通过变异操作使权值较大的粒子的位姿变化较小，而权值较小的粒子变化较大，从而使权值较小的粒子去寻找更好的位姿，增加粒子的多样性。
附图说明
本发明将通过例子并参照附图的方式说明，其中：
图1是本发明的整体流程图；
图2是本发明中全局优化的流程图；
图3是具体实施例2设定的地图和路标点；
图4是具体实施例2中采用ekflam算法运行的结果图；
图5是具体实施例2中采用fastslam算法运行的结果图；
图6是具体实施例2中采用本发明运行的结果图。
具体实施方式
本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。
下面结合图1、图2对本发明作详细说明。
一种全局优化的并发定位与建图方法，包括以下步骤：
步骤1：根据机器人的初始坐标，在初始时刻随机生成n个粒子，得到初始时刻的粒子集；
步骤2：采用粒子滤波的slam算法，利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息；
步骤3：采用全局优化和重采样对当前时刻各粒子进行更新；
步骤4：利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点；
步骤5：重复步骤2-4，直至机器人完成所有时刻的运动。
进一步的，所述步骤2具体为：
步骤21：采用机器人运动模型，利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据，并记录当前时刻粒子观测到的路标点；
步骤22：采用提议分布对当前时刻的位姿数据和路标点位置进行更新，利用更新后的位姿数据和路标点位置计算各粒子的权重；
步骤23：对所述权重进行归一化处理，得到当前时刻各粒子的权重。
进一步的，所述步骤3具体步骤为：
步骤31：对当前时刻的各粒子进行克隆和变异操作，更新粒子集中的粒子；
步骤32：以更新后粒子集中的各个粒子为引领者随机产生n个跟随者，计算跟随者和引领者的可能性，利用可能性筛选所述引领者和跟随者，得到再次更新后的粒子集；
步骤33：利用再次更新后粒子集中粒子的权重计算有效粒子的数目neff，当有效粒子数目neff大于设定的阈值φ，则不进行重采样，否则进行重采样，得到当前时刻最终的粒子集。
进一步的，所述步骤31具体为：
步骤311：利用步骤2得到的粒子信息中的权重
步骤312：利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目ncl，计算公式如下：
步骤313：将新获得的粒子添加至所述粒子集中，更新当前时刻的粒子集，利用所述亲和力faffinity对粒子进行变异操作，变异采用的公式为：
步骤314：利用变异后粒子间的位姿差计算两两粒子间的排斥力freject，计算公式为：
步骤315：当粒子总数大于m时，将排斥力小于设定阈值的两个粒子中的一个抛弃，更新所述粒子集合ω，其中权重
进一步的，所述步骤32具体为：
步骤321：对步骤21更新后粒子集中的每个粒子，以各个粒子为引领者随机产生对应的n个跟随者，分别计算引领者与跟随者的适应度ffitness，计算适应度采用的函数为：
步骤322：利用所述适应度ffitness计算各个粒子的可能性pi，采用的公式为：
步骤323：判断引领者和跟随者的可能性pi，当跟随者可能性大于引领者可能性，即利用可能性最大的跟随者替换引领者，否则不进行替换，得到再次更新后粒子集。
进一步的，所述步骤33具体为：
步骤331：利用更新后粒子集中粒子的权重计算有效粒子的数目neff，计算公式为：
步骤332：当有效粒子数目neff大于设定的阈值φ，则不进行重采样，得到当前时刻最终的粒子集，否则进行重采样后跳转至步骤231。
进一步的，所述重采样为：复制权值大的粒子，去掉权值小的粒子进行，保持粒子集中粒子数目稳定。
进一步的，所述步骤4具体为：
步骤41：利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置，对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新，得到当前时刻的路标点位置；
步骤42：利用当前时刻最终粒子集中各个粒子粒子信息中的位姿数据
步骤43：利用当前时刻最终的路标点位置和路径中各点的位置，得到当前时刻机器人运动路径和路标点。
具体实施例1
一种全局优化的并发定位与建图方法，包括以下步骤：
步骤1：根据机器人的初始坐标，在初始时刻随机生成n个粒子，得到初始时刻的粒子集；
步骤2：采用粒子滤波的slam算法，利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息；
步骤21：采用机器人运动模型，利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据，并记录当前时刻粒子观测到的路标点；机器人的惯性传感器获取运动数据，所述运动信息中包括加速度数据和角度数据，所述加速度数据和角度数据即为位姿数据；机器人的视觉传感器观测所述路标点；
对于当前时刻观测到的路标，需要和数据库中已经观测过的路标进行关联，得到新的路标的关联表；
步骤22：采用提议分布对当前时刻的位姿数据和路标点位置进行更新，利用更新后的位姿数据和路标点位置计算各粒子的权重；
步骤23：对所述权重进行归一化处理，得到当前时刻各粒子的权重。
步骤3：由于多次迭代会造成粒子退化现象，造成很少一部分粒子权值较大，而大部分粒子权值较小，造成后续的计算很大一部分用在权值较小的粒子上面，而这些粒子又对最终机器人的位姿估计作用很小，造成资源的浪费。如果通过现有的重采样算法进行重采样，复制权值大的粒子，去除权值小的粒子，增加有效粒子数目，又会造成最后所有的粒子可能是由很少的一部分粒子重采样得来的，降低了粒子的多样性，造成粒子耗尽的问题；因此本发明采用全局优化和重采样对当前时刻各粒子进行更新；
步骤31：把粒子集中的粒子作为初始抗体种群存入记忆单元，将机器人的真实位姿作为抗原，粒子集合中的每一个粒子作为抗体。由于每一个粒子的权值可以表明当前粒子位姿估计的准确程度，所以认为每个粒子的亲和力和它自身的权值相关，如果权值大那么亲和力就小，权值小亲和力就大，通过亲和力可以控制粒子的克隆数目以及粒子的变异程度；因此对当前时刻的各粒子进行克隆和变异操作，更新粒子集中的粒子；
步骤311：利用步骤2得到的粒子信息中的权重
步骤312：利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目ncl，计算公式如下：
通过cos函数使得粒子复制的数目随着亲和力的增加而减少，也就是对权值高的粒子进行多复制，权值小的粒子减少复制，通过这种方法可以有效的抑制亲和力高的抗体而促进亲和力低的抗体，加快收敛速度。
步骤313：将新获得的粒子添加至所述粒子集中，更新当前时刻的粒子集，利用所述亲和力faffinity对粒子进行变异操作，通过变异操作使权值较大的粒子的位姿变化较小，而权值较小的粒子变化较大，从而使权值较小的粒子去寻找更好的位姿，增加粒子的多样性，变异采用的公式为：
步骤314：利用变异后粒子间的位姿差计算两两粒子间的排斥力freject，计算公式为：
步骤315：当粒子总数大于m时，将排斥力小于设定阈值的两个粒子中的一个抛弃，更新所述粒子集合ω，其中权重
步骤32：通过步骤31保留了权值较低的粒子的同时增加了粒子集的多样性，缓解了粒子退化问题，以更新后粒子集中的各个粒子为引领者随机产生n个跟随者，计算跟随者和引领者的可能性，利用可能性筛选所述引领者和跟随者，得到再次更新后的粒子集；
步骤321：以各个粒子为引领者在其领域内随机产生对应的n个跟随者(替代粒子)，分别计算引领者与跟随者的适应度ffitness，计算适应度采用的函数为：
步骤322：利用所述适应度ffitness计算各个粒子的可能性pi，采用的公式为：
步骤323：判断引领者和跟随者的可能性pi，当跟随者可能性大于引领者可能性，即利用可能性最大的跟随者替换引领者，否则不进行替换，得到再次更新后粒子集。
步骤33：利用再次更新后粒子集中粒子的权重计算有效粒子的数目neff，当有效粒子数目neff大于设定的阈值φ，则不进行重采样，否则进行重采样，得到当前时刻最终的粒子集。
步骤331：利用更新后粒子集中粒子的权重计算有效粒子的数目neff，计算公式为：
步骤332：当有效粒子数目neff大于设定的阈值φ，则不进行重采样，得到当前时刻最终的粒子集，否则进行重采样后跳转至步骤31。
所述重采样为：复制权值大的粒子，去掉权值小的粒子进行，保持粒子集中粒子数目稳定。
步骤4：机器人在运动过程中，粒子集中每一个粒子对于观测的路标点都有自己的估计，所以每个粒子都有自己的地图，利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点；
步骤41：利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置，对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新，得到当前时刻的路标点位置；
步骤42：利用当前时刻最终粒子集中各个粒子粒子信息中的位姿数据
步骤43：利用当前时刻最终的路标点位置和路径中各点的位置，得到当前时刻机器人运动路径和路标点。
步骤5：完成当前时刻后，机器人继续运动进行下一时刻的运动，重复步骤2-4，直至机器人完成所有时刻的运动，生成地图信息和机器人的定位信息。
具体实施例2
本实施例基于具体实施例1，对本发明进行验证。
构建一个200m*200m的环形地图，如图3所示，其中路径点有21个，同时在路径点周围标定65个路标点用于模仿双目相机观察到的路标信息，在实验开始之前设定机器人从(0，0)开始运动，运动的速度是5m/s，运动的误差是0.5m/s，运动最大转向角是40度，最大转向角变化率25度/s，误差是4度。观测的距离误差是0.2m，角度误差是1度。
采用ekfslam算法进行处理，得到的位置估计均方根误差为2.1325，角度估计均方根误差为0.2379；
采用fastslam算法进行处理，得到的位置估计均方根误差为1.3826，角度估计均方根误差为0.1910；
而采用本发明进行处理，得到的位置估计均方根误差为1.0187，角度估计均方根误差为0.1473；
从以上数据可看出，采用本发明能提高机器人运动的定位精度。