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

权利要求书

1.一种多重渐消因子的机器人无迹快速同步定位与建图方法，其特征在于，具体的实现步骤如下：

步骤1.系统初始化确定全局坐标系以及在该坐标系下移动机器人k时刻初始位姿，初始状态估计均值及协方差矩阵，机器人位姿X_r＝[x_r y_r θ_r]^T，路标位置m_i＝[x_i y_i]^T均采用二维平面直角坐标，全局坐标系以机器人初始位置为原点，初始航向为X轴正方向；

步骤2.依据移动机器人系统的过程模型、观测模型进行时间更新，计算粒子在k+1时刻的预测位姿和协方差；

步骤3.根据k+1时刻观测数据与预测观测数据进行数据关联，根据获得的观测数据，判断是否为环境中已有的地图特征，若为已存在特征，则采用单一兼容最近邻方法进行数据关联，判断若为新特征则转至步骤6进行地图更新；

步骤4.根据粒子观测值，计算多重渐消因子，与UT变换结合进行量测更新计算粒子的位姿估计均值和协方差，并构造高斯分布函数作为建议分布并采样；

步骤5.有效粒子数将有效粒子数与设定阈值N_g相比较，当N_eff＜N_g时进行重采样，得到新的粒子集，完成机器人位姿估计；

步骤6.根据数据关联后的结果，使用EKF算法对新路标进行更新，完成对地图特征的估计。

2.根据权利要求1所述的一种多重渐消因子的机器人无迹快速同步定位与建图方法，其特征在于，步骤2所述的移动机器人系统为

其中x_k为机器人k时刻状态变量，在控制变量u_k作用下，服从p(x_k|x_k-1,u_k)概率分布，w_k表示方差为Q_k的系统噪声，z_k表示机器人对环境特征m_k的观测信息，v_k表示方差为R_k的量测噪声，观测过程服从p(z_k|x_k,m_k)分布，f和h分别表示机器人运动方程和观测方程；

步骤2所述的时间更新采用无迹卡尔曼滤波，根据k时刻位姿估计和位姿控制信息，利用移动机器人运动模型，估计k+1时刻位姿信息，其具体过程为：

步骤2.1.扩充并初始化第i个粒子中机器人的状态向量的均值和方差，机器人位姿为其中和代表机器人在二维平面坐标系内的坐标位置，代表机器人的航向角，角度范围(-π,π)，

其中为状态增广后机器人位姿状态向量，为机器人位姿状态向量，为状态增广后机器人位姿协方差矩阵；

步骤2.2.采用对称采样策略，采样L＝2N+1个sigma点，N为状态向量的维数，

其中n为矩阵的第n列，λ＝α²(N+κ)-N，选取α(0＜α＜1)，κ为权衡调节参数，L个sigma采样点通过运动模型和观测模型进行传递，得到预测后的采样点

步骤2.3.将预测后的sigma采样点加权处理，得到机器人位姿状态经过时间更新的均值和协方差估计