[发明专利]一种平方根容积模糊自适应卡尔曼滤波SLAM方法

权利要求书

1.一种平方根容积模糊自适应卡尔曼滤波SLAM方法，其特征在于，所述方法包括：

对移动机器人进行建模，建立动力学模型和观测模型；

模糊自适应噪声动态调整算法，对动力学模型和观测模型中的运动噪声和观测噪声设置控制权值，通过动态调整新息均值和方差的方式对噪声的权值进行了模糊调整；

通过k-1时刻的机器人位姿信息预测k时刻机器人位姿信息；

经过预设次数迭代后，对计算器位姿信息进行更新。

2.根据权利要求1所述的模糊自适应卡尔曼滤波SLAM方法，其特征在于，所述“对移动机器人进行建模，建立动力学模型和观测模型”的步骤具体为：

定义为移动机器人的位姿信息，为k时刻地图的特征信息，则移动机器人系统状态信息向量为：

对移动机器人进行建模，其动力学模型和观测模型如下：

其中，U_k为n_u维运动信息向量，Z_k为n_z维的观测向量，Q_k、R_k分别表示运动噪声和观测噪声。

3.根据权利要求2所述的模糊自适应卡尔曼滤波SLAM方法，其特征在于，所述“模糊自适应噪声动态调整算法，对动力学模型和观测模型中的运动噪声和观测噪声设置控制权值，通过动态调整新息均值和方差的方式对噪声的权值进行了模糊调整”的步骤具体为：

对于动力学模型和观测模型中的运动噪声和观测噪声设置控制权值，动力学模型和观测模型变换为：

其中，a_k，b_k分别表示控制的权值；

模糊自适应算法通过动态调整新息均值和方差的方式对噪声的权值进行了模糊调整，则新息和新息方差定义如下：

在时间窗M内新息和方差的均值如下所示：

根据新息和方差的均值得到其置信度为：

引入高斯函数来动态的确定a_k，b_k的值，高斯函数如下所示：

其中，c表示高斯函数的中心，σ表示高斯函数的宽度；

得到自适应控制a_k，b_k的值为：

其中，η为[0,1]之间的常数，表示对运动噪声和观测噪声的调节率，σ的值通过公式为：

4.根据权利要求3所述的模糊自适应卡尔曼滤波SLAM方法，其特征在于，所述“通过k-1时刻的机器人位姿信息预测k时刻机器人位姿信息”的步骤具体为：

确定容积点信息矩阵公式如下所示：

其中，j为容积点序号，取值为1，2，…，2(n_u+n_s)；矩阵包括机器人的位姿信息、特征点信息以及运动控制信息，ξ_j表示互相正交的完全对称容积点集，

通过k-1时刻的机器人位姿信息预测k时刻机器人位姿信息，其容积点先验估计和位姿信息公式如下：

可得地图特征误差向量如下所示：

5.根据权利要求4所述的模糊自适应卡尔曼滤波SLAM方法，其特征在于，所述“经过预设次数迭代后，对计算器位姿信息进行更新”的步骤具体为：

设置迭代的初始值为S_k|k-1和C_k|k-1，当迭代l次后，机器人的位姿信息和平方根因子分别为和其对应的k时刻观测向量模型为：

经过迭代后的容积点公式变为：

计算第l次迭代后的卡尔曼增益为：

计算观测误差向量为：

其中，为迭代l次后卡尔曼增益的平均值；

解得状态预测协方差阵、地图特征误差向量与观测误差向量的交互协方差矩阵以及观测新息协方差平方根因子如下所示：

因此卡尔曼增益矩阵为：

计算迭代l+1次后的机器人位姿矩阵和平方根因子矩阵公式如下所示：

当达到预先设定的迭代次数后，数据更新如下：

位姿更新：

强跟踪滤波时变渐消因子公式如下所示：

其中：

N_k＝V_k-H_kQ_k-1H_k^T-βR_k

M_k＝H_kF_k|k-1P_k-1|k-1F_k|k-1^TH_k^T

其中，H_k和F_k|k-1分别表示测量方程与状态方程对于状态变量的一阶偏导矩阵，β为弱化因子，ρ为遗忘因子；

得到：

引入渐消因子后的状态协方差预测平方根因子为：

将此平方根因子重新带入预测更新过程中进行迭代运算。