CN109520503A - 一种平方根容积模糊自适应卡尔曼滤波slam方法 - Google Patents

Abstract

本发明公开了一种平方根容积模糊自适应卡尔曼滤波SLAM方法，所述方法包括：对移动机器人进行建模，建立动力学模型和观测模型；模糊自适应噪声动态调整算法，对动力学模型和观测模型中的运动噪声和观测噪声设置控制权值，通过动态调整新息均值和方差的方式对噪声的权值进行了模糊调整；通过k‑1时刻的机器人位姿信息预测k时刻机器人位姿信息；经过预设次数迭代后，对计算器位姿信息进行更新。将迭代方式与强跟踪相结合，并针对机器人存在运动噪声和观测噪声的问题，采用改进的模糊自适应方式对不同的噪声进行自适应动态调整。降低了算法复杂度，解决了采样点在非线性情况下存在失真的问题，校正因特征点增加引起的轨迹偏移现象，提高了位姿精度。

Description

一种平方根容积模糊自适应卡尔曼滤波SLAM方法

技术领域

本发明涉及机器人自主导航技术领域，特别是涉及一种平方根容积模糊自适应卡尔曼滤波SLAM方法。

背景技术

同时定位与地图重建是指移动机器人在未知环境下通过激光雷达进行环境地图的重建并在重建后的地图中表示出该机器人位姿状态的过程。SLAM算法在移动机器人中的应用是该领域的研究热点。传统的算法将扩展卡尔曼滤波引入到SLAM领域，其本质就是使用卡尔曼滤波对线性化的模型进行处理。扩展的卡尔曼滤波(EKF)虽然能够解决非线性系统问题，但是不能很好的处理算法中的数据关联且精度不高。在此基础之上，近年来许多研究学者提出了各种改进的新型算法。针对地图重建过程中受外界环境影响较大的问题，现有技术中提出了一种超声概率栅格地图环境特征点提取算法、以及迭代测量更新的UKF算法，有效解决了无迹卡尔曼滤波(UKF)中Sigma点会逐渐偏离真实状态估计值的问题，但机器人在同时定位与地图重建过程中系统往往存在噪声，随着地图特征点的增加系统的观测维度也会随之增加，导致定位效果不佳。

发明内容

本发明要解决的技术问题在于，针对现有技术的定位效果不佳的问题，提供一种平方根容积模糊自适应卡尔曼滤波SLAM方法。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种平方根容积模糊自适应卡尔曼滤波SLAM方法，其特征在于，所述方法包括：

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

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

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

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

进一步的，所述“对移动机器人进行建模，建立动力学模型和观测模型”的步骤具体为：

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

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

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

进一步的，所述“模糊自适应噪声动态调整算法，对动力学模型和观测模型中的运动噪声和观测噪声设置控制权值，通过动态调整新息均值和方差的方式对噪声的权值进行了模糊调整”的步骤具体为：

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

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

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

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

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

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

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

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

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

进一步的，所述“通过k-1时刻的机器人位姿信息预测k时刻机器人位姿信息”的步骤具体为：

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

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

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

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

进一步的，所述“经过预设次数迭代后，对计算器位姿信息进行更新”的步骤具体为：

设置迭代的初始值为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分别表示测量方程与状态方程对于状态变量的一阶偏导矩阵，β为弱化因子，ρ为遗忘因子；

得到：

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

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

本发明的有益效果是：实施本发明的平方根容积模糊自适应卡尔曼滤波SLAM方法，本方法将迭代方式与强跟踪相结合，并且针对机器人存在运动噪声和观测噪声这一问题，采用改进的模糊自适应方式对不同的噪声进行自适应动态调整。该算法的优点在于大大降低了算法复杂度，解决了采样点在非线性情况下存在失真的问题，能够很好的校正因特征点增加引起的轨迹偏移现象，提高了位姿精度。

附图说明

图1是本发明平方根容积模糊自适应卡尔曼滤波SLAM方法的第一实施例的流程图。

具体实施方式

为更进一步阐述本发明为实现预定发明目的所采取的技术手段及功效，以下结合附图及较佳实施例，对依据本发明的具体实施方式、结构、特征及其功效，详细说明如后。

请参阅图1，为本发明平方根容积模糊自适应卡尔曼滤波SLAM方法的第一实施例的流程图，在平方根容积模糊自适应卡尔曼滤波SLAM方法的第一实施例中，该方法包括以下步骤：

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

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

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

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

具体的，本发明的实现步骤如下：

(1)对移动机器人进行建模:

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

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

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

(2)模糊自适应噪声动态调整算法

对于式(2)中的运动噪声和观测噪声，对其设置控制权值，则式(2)变换为：

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

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

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

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

为了提高模糊自适应模型的适应性，本发明引入高斯函数来动态的确定a_k，b_k的值，高斯函数如下所示：

式中：c表示高斯函数的中心，σ表示高斯函数的宽度。

由式(6)、(7)得到自适应控制a_k，b_k的值为：

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

(3)预测阶段

在预测阶段，首先我们要确定容积点信息矩阵公式如下所示：

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

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

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

(4)更新阶段

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

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

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

计算观测误差向量为：

式中：为迭代l次后卡尔曼增益的平均值。

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

因此卡尔曼增益矩阵为：

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

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

位姿更新：

在预测更新过程中经常会由于系统的不稳定导致滤波器出现发散等问题，因此本发明通过引入渐消因子来改变数据权重从而提高系统的稳定性与定位精度。强跟踪滤波时变渐消因子公式如下所示：

其中：

N_k＝V_k-H_kQ_k-1H_k ^T-βR_k (27)

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

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

将式(19)带入(27)、(28)得到：

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

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

本发明提出的方法在MATLAB下进行的仿真，使用250m×200m的环境地图，选择17个确定的路径点和35个地图特征点。具体仿真参数如表1所示：

表1仿真参数

仿真实验中运动模型：

输入：x_v(k)为机器人的位姿，ΔT为传感器采样时间，v(k)为速度，a(k)为机器人在k时刻的方向角，B为两轴间的轴距。输出：x_v(k+1)为机器人的新位姿。

仿真实验中观测模型：

输入：(x_i,y_i)为探测到的第i个特征的位置坐标，x_v(k)为位姿。输出：通过激光雷达等传感器测得的特征点与移动机器人的距离x和特征点与移动机器人前进方向的夹角θ。

通过实施本发明的方法，有效改善了因为特征点的增加导致的轨迹偏移现象，为了更好的验证本文算法的可靠性。

随着时间的增加，移动机器人的定位误差逐渐减小，由于本发明引入了迭代与时变渐消因子相结合的方式，再加上针对存在系统噪声这一问题，在算法中动态调节新息均值和方差，使得整个算法具有较好的模糊自适应性。

实施本发明的平方根容积模糊自适应卡尔曼滤波SLAM方法，本方法将迭代方式与强跟踪相结合，并且针对机器人存在运动噪声和观测噪声这一问题，采用改进的模糊自适应方式对不同的噪声进行自适应动态调整。该算法的优点在于大大降低了算法复杂度，解决了采样点在非线性情况下存在失真的问题，能够很好的校正因特征点增加引起的轨迹偏移现象，提高了位姿精度。

需要说明的是，本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。对于装置类实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、物品或者装置中还存在另外的相同要素。

本领域技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成，也可以通过程序来指令相关的硬件完成，该程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器，磁盘或光盘等。

以上，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (5)

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分别表示测量方程与状态方程对于状态变量的一阶偏导矩阵，β为弱化因子，ρ为遗忘因子；

得到：

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

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