本发明属于移动机器人，涉及一种移动机器人slam定位方法，用于解决在杂波环境下移动机器人定位与建图精度不佳的情况，提高移动机器人定位与建图的质量及算法运行速度，并将该算法推广至三维场景。
背景技术：
1、即时定位与建图(simultaneous localization and mapping，slam)技术是指利用机器人所携带的传感器的观测信息对未知环境的地图进行构建，同时完成机器人平台位姿估计的技术。近几年，slam技术因其广泛的应用而引起了很多关注，包括移动机器人，导航，自动驾驶，虚拟现实等领域。传统的slam算法对杂波和传感器的漏检较为敏感，在密集杂波的环境下极易失效。因此当量测来源未知且受到密集杂波的影响时，如何解决数据关联(data association，da)问题对slam的效果至关重要。
2、基于概率假设密度滤波器(probability hypothesis density，phd)的phd-slam将周围环境地标建模成随机有限集，并递归传播其一阶矩，利用其传播的一阶矩对机器人的位姿及周围环境进行估计。该算法基于贝叶斯框架，将周围环境地标看成多个需要跟踪的目标，将slam问题变为多目标跟踪问题。将传统多目标跟踪领域的phd技术引入到slam问题中，有助于解决slam算法在密集杂波环境下数据关联困难导致的算法性能不佳的问题。
3、2011年，mullane等人首次将phd引入slam问题，对slam问题进行概率分解后，在rbpf滤波器的基础上，使用随机有限集中的phd的高斯混合(gaussian mixture)滤波器对slam问题进行求解，依靠phd滤波器的特性，避免了slam问题中的显式数据关联，对传感器所获得的环境信息进行了杂波、漏检和真实地图特征的区分，大大提高了该算法在杂波环境下的精度；2013年chee sing lee等人提出一种single-cluster phd slam滤波器，将机器人的位姿估计和地图地标的估计建模成父过程和子过程，相较于基于rb分解的phd slam滤波器，sc-phd slam在机器人的轨迹估计上有明显的改进。2019年孙陶莹等人提出基于记忆衰减滤波的平方根无迹卡尔曼滤波phd-slam，一方面使用cholesky分解因子代替协方差矩阵p向后传播，避免ukf滤波器存在的发散问题；另一方面将phd-slam与衰减记忆滤波相结合，增大了当前观测数据的权重，降低了旧数据对滤波结果的不利影响。2020年，lin gao等人phd-slam的提议分布进行改进，利用机器人位姿和环境特征的联合phd对车辆位姿进行采样并更新，大大减少了逼近机器人后验位姿所需要的粒子数量，提高了计算速度。2021年，章飞等人提出基于幅值信息的phd-slam，将观测信息中的幅值信息加以利用进而提高算法的精度，同时论文中也对phd-slam的出生信息部分进行改进，将地图特征有选择的添加到当前时刻的出生地图信息中，提高了算法的精度以及速度。然而，上述phd-slam算法存在两个问题，一是对机器人位姿的估计均基于粒子滤波器，因此在满足算法精度要求的前提下，粒子滤波器通常所使用的粒子数较多，存在算法计算负担大，无法满足slam算法的实时性要求等问题；二是上述phd-slam算法所适用的场景为二维场景，无法满足三维场景下的移动机器人定位与建图的需求。
技术实现思路
1、本发明的目的是，针对目前现有的phd-slam滤波算法存在的计算负担大、无法满足slam算法实时性以及无法适用于三维场景的问题，提供了一种移动机器人slam定位方法，适用于三维场景、计算成本低且算法估计精度满足要求的基于随机有限集的移动机器人同步定位与地图构建方法。
2、采用的技术方案如下：
3、本发明的一个目的是提供一种移动机器人slam定位方法，包括：
4、步骤(1)、利用条件概率将slam问题的概率模型分解成移动机器人位姿状态后验概率密度和在该移动机器人位姿状态后验概率密度条件下的环境地图后验概率密度；
5、步骤(2)、利用随机有限集对slam问题中的环境地图表示进行建模；
6、步骤(3)、移动机器人运动学建模，即利用物理模型对移动机器人的位姿状态和观测信息进行建模；
7、步骤(4)、对当前时刻的移动机器人位姿状态和地图信息进行粗略估计，得到当前时刻移动机器人位姿状态的先验密度和地图信息的先验密度；
8、步骤(5)、利用地图信息和观测信息对移动机器人粗略估计的位姿状态进行更新，得到当前时刻机器人位姿状态的后验密度，即当前时刻机器人位姿状态的精确估计；并基于观测信息和机器人位姿状态的后验密度对当前时刻环境地图的gms进行更新；
9、步骤(6)、对步骤(5)中得到的环境地图的gms进行剪枝、合并处理，将经过处理后的gms作为当前时刻周围环境地图的估计值，得到当前时刻环境地图的后验密度；
10、步骤(7)、重复步骤(4)-(6)，进行下一时刻的机器人位姿状态估计和环境地图估计。
11、本发明的另一个目的是提供一种移动slam系统，其特征在于，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的程序指令执行所述方法。
12、本发明的有益效果是：
13、本发明提出了一种先对机器人位姿状态和环境地图进行粗略估计，然后利用粗略估计的环境地图和观测信息对机器人位姿状态进一步精确估计，再利用精确估计过的机器人位姿状态和观测信息精确估计环境地图的机器人同步定位与建图的流程，该方法可以有效提高机器人同步定位与建图的定位精度和建图精度。
14、另外，本发明还提出误差位姿状态变量和协方差矩阵估计更新过程中引入phd强度作为融合权重，提高机器人位姿状态的定位精度。本发明配置结构明了，计算量小，计算精度高，可广泛应用于移动机器人领域，该方法可以有效提高机器人定位与建图的精度和效率。
1.一种移动机器人slam方法，其特征在于所述方法包括以下步骤：
2.根据权利要求1所述方法，其特征在于步骤(4)具体是：
3.根据权利要求2所述方法，其特征在于步骤4-1具体是：
4.根据权利要求2所述方法，其特征在于步骤4-2具体是：
5.根据权利要求1所述方法，其特征在于步骤(5)具体是：
6.根据权利要求5所述方法，其特征在于步骤5-1具体是：
7.根据权利要求6所述方法，其特征在于步骤5-2具体是：
8.一种移动slam系统，其特征在于，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的程序指令执行如权利要求1-7任一项所述方法。
