一种实现移动机器人同时定位与地图构建的方法技术

本发明专利技术公开了一种实现移动机器人同时定位与地图构建的方法。该方法针对移动机器人在未知环境中利用航位推测传感器数据以及路标观测数据，借助于改进的强跟踪滤波技术实现移动机器人的自主定位，并同时构建出环境地图。本发明专利技术利用“强跟踪滤波器”，自适应调整卡尔曼增益；引入一种新的多重渐消因子，保证协方差矩阵的对称性，降低求解卡尔曼增益的计算复杂度；针对同时地位与地图构建问题中的观测不连续问题，提出了一种新的算法流程，并给出了一种新的多重渐消因子的计算方法。与传统方法相比，本发明专利技术降低了线性化过程引入的误差，提高了机器人的定位精度以及构建地图的精度，同时也能将协方差抑制在一个较小的范围内，提高了所建地图的可信度。

【技术实现步骤摘要】

本专利技术涉及移动机器人自主定位与环境感知
，尤其涉及一种基于强跟踪滤波实现移动机器人同时定位与地图构建的方法，可用于根据航位推测传感器数据以及路标观测数据估计未知环境中移动机器人的位姿并构建环境地图。
技术介绍
随着机器人技术的发展，具有移动行走功能、环境感知能力以及自主规划能力的智能移动机器人得到各国研究人员的普遍重视。实现自主定位与导航是智能机器人最基本的功能之一，也是其完成各种任务的重要前提。而移动机器人实现自主导航的关键是使机器人拥有完整的地图信息和具有良好的自主定位能力。机器人同时定位与地图构建(SimultaneousLocalization and Mapping, SLAM) 为实现自主定位与导航提供了一个新的解决思路机器人利用自身配备的传感器得到环境的测量数据，在构建所在环境地图的同时，利用环境地图来计算自身在环境中的位姿。 由于SLAM理论具有重要的应用价值，被很多学者认为是实现真正意义上全自主移动机器人的关键(参见"Dissanayake, M. W. M. G. , et al. , A solution to the simultaneous localization and map building (SLAM)problem. IEEE Transactions on Robotics and Automation, 2001. 17(3) :pp. 229-241. ”)。SLAM常用的算法大致有以下几种扩展卡尔曼滤波(Extended Kalman Filter)、 Rao-Blackwellised 粒子滤波(fcio-Blackwellised Particle Filter,也被称为 FastSLAM)、稀疏扩展信息滤波(Sparse Extended Information Filter, SEIF)以及基于最大似然估计的SLAM方法。EKF-SLAM对运动模型和观测模型进行线性化近似处理，并假设其概率密度概述为高斯分布。采用增广的系统状态空间，运用卡尔曼滤波器对系统状态进行递归地估计，对增广的系统状态和增广的系统协方差矩阵进行递归地更新替换，从而解决非线性系统模型的估计问题。EKF是解决SLAM问题使用的最多的方法，也是现有多种SLAM算法的基础。但是，当估计值与真实值偏离较大时，EKF对非线性模型进行一阶线性近似会引入较大的线性化误差。RBPF-SLAM采用粒子集来表示机器人的位姿，而环境特征的估计依然利用EKF解析计算。使用机器人的路径来估计SLAM的后验概率，根据条件无关特性将SLAM后验分解为一系列的低维的估计问题，使得RBPF-SLAM的计算复杂度为O(MN)，其中M，N分别为粒子数目和特征的个数。对于给定的粒子数目，RBPF-SLAM的计算复杂度与特征个数呈线性关系，因此 RBPF-SLAM 也被称为 FastSLAM0 (参见“Montemerlo，Μ.，et al.，FastSLAM 2. 0 :An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges, in Proceedings of the International Conference on Artificial Intelligence，2003，pp. 1151-1156.，，)。该算法在处理大环境地图建立问题比较有效，不过对于一些特征比较少的不是很大的环境来说，速度上的优势并不是很明显。信息滤波IF是卡尔曼滤波的另一种表示形式，信息滤波器中使用信息矩阵和信息向量代替KF中的协方差矩阵和状态均值向量。稀疏扩展信息滤波(Sparse Extended Information Filter, SEIF)是信息滤波的一种扩展近似。SEIF的基本观点在于，即使协方差矩阵为稠密阵，但其逆矩阵可能是稀疏矩阵，或者逆矩阵的很多元素值非常小。于是在SEIF的每一步，有选择地将逆矩阵较小的元素置为零，从而达到恒定时间的近似滤波器更新(参见“Thrun，S.，et al.，Simultaneous Localization and Mapping with Sparse Extended Information Filters. The International Journal of Robotics Research， 2004. 23(7-8) :pp. 693-716. ”)。该处理方法使得观测更新执行效率更高，同时精度损失很小。但其在实际应用中也存在着缺点每次更新时都需要将信息向量和信息矩阵转换为状态向量的估计均值和方差，用于运动模型和观测模型的线性化。该转换过程计算复杂度与路标个数成立方关系、计算开销庞大。基于最大似然估计(Maximum Likelihood Estimation)的SLAM方法首先利用运动模型估计在假定当前地图确定情况下，给定控制作用下机器人可能移动的新位置；然后计算机器人在此位置形成的环境地图中每个特征同以往地图相比出现的频率(作为最大相似性估计器)。经过计算，较准确存在的环境特征在地图中的概率值增加了，而较不准确存在的环境特征在地图中的概率值降低了。该方法充分利用过去若干时间步的数据，因而对观测值具有鲁棒性；观测的环境特征不用非常清晰，甚至可以是错误的。与EKF相比，该方法绕过了对观测值与地图中元素数据相关的准确性的依赖，提高了算法的鲁棒性。但是该方法处理的数据量过大，对运算速度和存储空间要求都非常高，制约了其应用于大规模环境。对于实时SLAM算法来说，计算复杂度是算法使用的主要限制因素。通常情况下，EKF-SLAM的算法复杂度是0 (N2)，FastSLAM的算法复杂度是0 (MN)， 其中N是地图中路标的个数，M是使用的粒子个数。随着粒子个数的增加，FastSLAM的精 it^Sffft EKF(Thrun, S. ， W. Burgard, and D. Fox, Probabilistic Robotics. Cambridge, Massachusetts :The MIT Press, 2005, pp. 470-471，，)。一般情况下，M 的经验取值会在50 500左右，在特征个数较多的情况下，FastSLAM的计算效率方面的优势才能体现出来。稀疏扩展信息滤波(Sparse Extended Information Filter)在观测更新执行效率更高，同时精度损失很小。但是每次更新时都需要将信息向量和信息矩阵转换为状态向量的估计均值和方差，该转换过程计算复杂度与路标个数成立方关系、计算开销庞大。一般基于拓扑结构的室内环境地图的节点数不会很多(100以内)，而EKF-SLAM采用状态增广(state augmentation)、分块更新等优化技巧后计算开销会明显降低，能够很好地满足室内环境SLAM的实时性要求。但是，当估计值与真实值偏离较大时，EKF对非线性模型进行一阶线性近似会引入较大的线性化误差。周东华提出了“强跟踪滤波器”(Strong Tracking Fil本文档来自技高网...

【技术保护点】

【技术特征摘要】

【专利技术属性】
技术研发人员：温丰，原魁，柴晓杰，
申请(专利权)人：中国科学院自动化研究所，
类型：发明
国别省市：