[发明专利]一种多重渐消因子的机器人无迹快速同步定位与建图方法

摘要

本发明提供一种多重渐消因子的机器人无迹快速同步定位与建图方法，属于移动机器人自主导航技术领域，系统初始化确定移动机器人k时刻初始位姿，初始状态估计均值及协方差矩阵；进行时间更新，计算粒子在k+1时刻的预测位姿和协方差；将观测数据与预测观测数据关联；根据粒子观测值，计算多重渐消因子，量测更新计算粒子的位姿估计均值和协方差，构造高斯分布函数并采样；进行重采样，得到新的粒子集，完成机器人位姿估计；根据数据关联后的结果，使用EKF算法对新路标进行更新，完成对地图特征的估计。本发明降低了线性化过程引入的误差，提高了系统的滤波精度，提高了滤波器对系统状态发生突变时的调节应变能力，增强了机器人导航定位系统的鲁棒性。

主权项

1.一种多重渐消因子的机器人无迹快速同步定位与建图方法，其特征在于，具体的实现步骤如下：步骤1.系统初始化确定全局坐标系以及在该坐标系下移动机器人k时刻初始位姿，初始状态估计均值及协方差矩阵，机器人位姿Xr＝[xr yr θr]T，路标位置mi＝[xi yi]T均采用二维平面直角坐标，全局坐标系以机器人初始位置为原点，初始航向为X轴正方向；步骤2.依据移动机器人系统的过程模型、观测模型进行时间更新，计算粒子在k+1时刻的预测位姿和协方差；步骤3.根据k+1时刻观测数据与预测观测数据进行数据关联，根据获得的观测数据，判断是否为环境中已有的地图特征，若为已存在特征，则采用单一兼容最近邻方法进行数据关联，判断若为新特征则转至步骤6进行地图更新；步骤4.根据粒子观测值，计算多重渐消因子，与UT变换结合进行量测更新计算粒子的位姿估计均值和协方差，并构造高斯分布函数作为建议分布并采样；步骤5.有效粒子数将有效粒子数与设定阈值N_g相比较，当N_eff＜N_g时进行重采样，得到新的粒子集，完成机器人位姿估计；步骤6.根据数据关联后的结果，使用EKF算法对新路标进行更新，完成对地图特征的估计。