一种随机有限集框架下的同步定位与地图构建方法
【技术领域】
[0001] 本发明涉及移动机器人领域，特别涉及一种随机有限集框架下的移动机器人同步 定位与地图构建方法。
【背景技术】
[0002] 移动机器人是一种集环境感知、数据融合、任务规划、行为执行等多种功能于一体 的综合化智能系统，已经被广泛应用于社会生产生活的各个领域。移动机器人智能化的首 要条件就是能自主运动，而这一条件的达成要求机器人获取所处外部环境的地图描述以及 自身在环境中的位置信息，即构图与定位。在早期的研究中，二者被认为是完全独立的课 题，并且对于己知环境中的机器人自主定位和已知机器人位置的地图构建已经有了一些解 决方法，然而在很多情况下事先获取机器人工作环境的地图很困难，甚至是不可能的，这大 大限制了移动机器人的实际应用。
[0003] 同步定位与地图构建（simultaneous localization and mapping,SLAM)方法的 出现为上述难题的解决带来了希望，将机器人定位和环境地图构建进行了有机结合。SLAM 也称为并发构图与定位(Concurrent Mapping and Localization，CML)，是指:处于未知环 境的中机器人，在行进过程中通过装配的传感器不断获取外部环境信息，利用这些信息构 建环境地图，同时依据这一地图实现自身定位的过程。SLAM问题被认为是实现机器人真正 自主的关键问题，自提出之日起便成为了机器人研究的热点问题。SLAM最终的目标是要实 现机器人状态和环境地图状态的同时估计，本质上是一个状态估计问题。贝叶斯滤波为 SLAM的实现提供了有效途径，将SLAM求解转换为递归框架下机器人状态和环境地图的联合 后验概率的估计问题。贝叶斯滤波的多种实现方法都被用于求解SLAM问题，并已取得了大 量研究成果，其中最典型的两类方法是基于卡尔曼滤波(Kalman fi 1 ter，KF)的SLAM求解和 基于粒子滤波(particle filter，PF)的SLAM求解。这两类SLAM方法有一个共同特点：都将 环境地图表示为向量形式(vector based，VB)，其中每个分量表示一个环境特征的位置信 息，其SLAM求解过程可以分解为两个阶段:对观测信息进行数据关联，以及依据关联结果对 环境地图和机器人位置进行状态更新。在整个SLAM过程中，有效的数据关联是进行状态估 计的前提，错误的关联不仅会影响机器人的定位，还可能导致地图估计不一致甚至发散，算 法对数据关联非常敏感。在实际应用中，受到机器人位姿的不确定、特征密度的变化、环境 中动态目标的干扰等影响，数据关联变得非常困难。另一方面，由于SLAM中的地图构建是一 个增进式过程，随着机器人的不断探索，新的环境特征被加入地图，数据关联的规模越来越 大，特征管理的复杂度也不断增加，这无疑会使算法计算量急剧上升。目前针对SLAM数据关 联的研究已经成为一个重要课题，但仍然没有一种能够兼顾精度和效率的通用关联方法， 至今数据关联仍然是SLAM的难点之一。
[0004] Mullane等人首次提出了利用有限集统计理论(finite set statistics，FISST) 将SLAM环境地图和传感器观测信息建模为随机有限集合(random finite set，RFS)形式， 使SLAM状态空间映射到集合空间。将SLAM理解为一个多目标跟踪问题，基于贝叶斯估计框 架在单目标状态构成的空间中对SLAM联合后验概率进行递归计算，从而避免了数据关联过 程。算法将复杂的联合后验概率密度估计问题转化为求取其一阶统计矩，即概率假设密度 (probability hypothesis density,PHD)，基于高斯假设通过高斯和（Gaussian mixture, GM)滤波对PHD滤波进行近似求解。观测信息被用于对机器人状态和整个地图集合构成的联 合状态进行更新，无需引入特征管理机制对地图特征进行排序处理，最终环境地图中特征 的位置信息和特征数量分别通过峰值提取和对PHD求积分获取。此外，基于RFS的SLAM模型 能够对传感器观测不确定性及被测目标空间不确定性进行有效描述。在这一成果的基础 上，研究人员充分挖掘FISST理论在表达不确定信息方面的优势，对算法进行了一系列改进 和优化，使基于随机集模型的SLAM研究得到进一步发展。在RFS建模的的基础上，Kaylan等 人利用Rao-Blackwel 1 i sed (RB)分解，将SLAM全概率估计转化为两个估计问题:机器人位姿 估计和以这个估计为条件的环境地图估计，利用粒子滤波进行机器人行驶轨迹的后验估 计，通过序贯蒙特卡洛(sequential Monte Carlo,SMC)方法对环境地图以机器人轨迹为条 件的PHD滤波进行实现。Mullane等人基于SLAM有限集模型的RB分解，分别利用粒子滤波和 GM-PHD滤波对机器人位置和环境地图进行估计，并正式将这种基于RFS建模和PHD滤波实现 的SLAM方法命名为RB-PHD-SLAM滤波。
[0005][0006] 综上所述，传统的基于向量模型的SLAM方法无法回避数据关联问题，导致计算量 较大，并且复杂环境中的数据关联问题本身就是一个极具难度的问题。随机有限集模型为 SLAM问题的求解提供了崭新的方向，但目前这一方法面临两个问题:其一，对SLAM全概率估 计进行RB分解后，通过GM滤波进行环境地图估计的PHD滤波实现实际上是基于线性假设的， 仅能对地图PHD函数的非高斯特性进行近似，而对于非线性地图PHD函数，算法估计精度将 受到较大影响。第二，利用粒子滤波进行机器人位姿估计时，建议分布函数的生成也是一个 难题。
【发明内容】
[0007] 本发明的目的是，针对现有随机有限集模型下的SLAM方法在利用PHD滤波进行地 图估计时无法处理非线性PHD函数以及利用粒子滤波进行机器人位姿估计时建议分布函数 的生成问题做出了相应改进，从而提高算法估计精度和可靠性，使这类方法的应用得到扩 展。本发明中提供的方法区别于现有方法的显著特征在于:在RFS模型框架下，对SLAM全状 态估计进行RB分解后，引入无迹变换(unscented transformation，UT)，一方面利用UKF对 观测信息进行融合生成粒子滤波的建议分布函数，在此基础上形成改进的无迹粒子滤波 (unscented particle filter，UPF)实现机器人位置估计；另一方面，利用GM模型将地图集 合的PHD函数近似为一组高斯分布的加权混合，并引入UT变换对PHD函数中非线性分布的统 计特性进行处理，通过UKF实现PHD函数中高斯构件的预测和更新。本发明提出的方法能够 显著改善算法在非线性非高斯条件下的估计性能，具有较强的鲁棒性，在机器人定位精度、 环境地图特征估计精度以及特征数目估计等方面相比以往方法有所提高。
[0008] 技术方案：
[0009] 本发明基于无迹变换，面向复杂环境中的移动机器人，在随机有限集模型框架下 提供了一种能够有效处理非线性非高斯问