CN114061583A - 基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法 - Google Patents

Abstract

本发明公开了一种基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法，旨在解决现有的SLAM技术无法处理非线性不确定系统、定位导航精度不高、计算量大的技术问题。本发明中自适应晶格卡尔曼滤波‑SLAM（ALKF‑SLAM）自主导航算法对于移动机器人的系统模型的噪声方差是否精确已知没有要求，并通过减少状态采样点，降低计算复杂度和计算成本，提高状态估计的精度和数值稳定性。利用晶格采样点，并根据量测残差序列和方差对量测方差进行修正。其次，利用基于上述修正的量测方差和晶格采样点设计的衰减因子对状态方差进行了修正。本发明的ALKF‑SLAM算法，有利于提高移动机器人在未知环境中地图构建和自身定位的精度。

Description

基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导 航方法

技术领域

本发明涉及移动机器人自主导航技术领域，具体涉及一种基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法。

背景技术

随着人工智能和机器人技术的快速发展，移动机器人在慢慢应用到社会的各个领域，比如办事助理、AGV搬运、安保巡逻、灾难救援、空间探测、变电站巡检等等。移动机器人在未知环境中对多传感器采集的数据进行融合，对未知环境标志物进行地图构建以及定位自身位置。机器人的环境感知和精确定位问题是移动机器人实现自主导航的关键技术，是研究移动机器人的热门领域，其被誉为移动机器人界的“圣杯”。

同步地定位与地图创建(Simultaneous Localization and Mapping，SLAM)技术，是解决机器人在未知环境中导航问题的最佳手段。机器人的SLAM技术在移动过程中通过传感器来精确估计自身位姿，同时增量式的构建真实环境地图。SLAM问题的求解方法可分为基于平滑的方法和基于滤波器的方法。基于滤波器的方法主要利用递归贝叶斯估计原理来实现，现有扩展卡尔曼滤波(EKF)-SLAM技术解决了卡尔曼滤波只能处理线性高斯系统的限制，但EKF是利用泰勒展开的一阶项来近似非线性系统，无法处理强非线性系统问题，处理过程中出现的截断误差严重影响了机器人的导航定位精度以及雅克比矩阵的求解大大增加计算度。

发明内容

本发明提供一种基于自适应晶格卡尔曼滤波(ALKF)的移动机器人状态估计及自主导航方法，以解决现有的SLAM技术无法处理非线性不确定系统、定位导航精度不高、计算量大的技术问题。

为解决上述技术问题，本发明采用如下技术方案：

设计一种基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法，包括：

S1)初始化移动机器人的初始状态

和初始方差P₀：

式中，E表示期望，

为零时刻即移动机器人起始点的坐标位置(x,y)和方位角(θ)；

S2)对移动机器人的位置、方位角等状态进行采样，得到状态的晶格采样点集为：

式中，χ_k-1,i为移动机器人第k-1时刻的状态

的第i个采样点，共有N个样本点；

表示的是P_k-1平方根的第i列向量；s_i为状态采样点的权值；

S3)对移动机器人第k步的状态进行一步预测：

式中，

为第i个采样点的预测值；

为移动机器人的状态预测值；

S4)获取方差的一步预测：

式中，

为移动机器人的一步预测误差；Q_k-1为过程噪声的方差；

S5)对步骤S3)中的状态一步预测重新进行晶格采样：

式中，

为

的第i个采样点；

S6)计算量测预测值

和量测预测方差P_zz：

式中，Z_k,i为第i个采样点的量测预测值；

为移动机器人的量测预测值；

为移动机器人量测预测方差；

S7)为了提高自适应晶格卡尔曼滤波算法的稳定性，引入自适应算法增加噪声估计器，以确保状态估计误差方差保持正定，设计量测方差

和状态方差

来逼近方差真实值，

量测方差

根据量测残差

计算，对于自适应晶格卡尔曼滤波，

的估计公式：

式中，

状态方差由衰减系数μ_k调整，其计算公式为：

式中，

S8)计算滤波增益矩阵K_k：

式中，P_zz为自适应后的移动机器人量测预测方差；P_xz为状态预测值与量测预测值的方差；

S9)更新状态：

S10)计算状态估计误差的方差P_k：

其中P_k,k-1为自适应过后的状态方差，

S11)循环迭代步骤S1)至步骤S1)，得到移动机器人的状态估计值

进一步的，所述步骤S2)中，得到状态的晶格采样点集的方法如下：利用确定的晶格点逼近多元积分的拟蒙特卡罗方法(QMC)。在这里，生成的格点由正态累积分布的逆映射函数对格点进行变换，得到样本点的权重。为了简化计算和提高计算效率，采用秩为1格的晶格规则对进行采样，是一种产生低差点的模化算术方法。

对状态采样用秩为1格的晶格规则如下：

式中，g(a)包含d个整数[a⁰,a²,a³,...,a^d-1]^T，a是一个具有点数N的互质整数。

为了获得无偏积分计算并提高估计精度，根据克兰利-帕特森位移方法(模1位移)生成随机位移格点，则状态采样的位移格点及其权重为：

式中，Δ是一个位移因子，它由超立方体上均匀分布生成的[0,1)^d，s_i为状态采样的位移晶格点的权重，φ^-1是一维正态累积分布φ的逆映射函数

本发明的第二方面是：

设计一种基于自适应晶格卡尔曼滤波的移动机器人自主导航方法，包括如下步骤：

Ⅰ)建立移动机器人的导航系统模型方程，并初始化；

其中，

为k时刻移动机器人的坐标位置(x,y)和方位角(θ)，

是控制量，V_k为移动机器人的速度，γ_k为轮子的方位角，z_k是量测量测值，

为特征点的坐标，w_k-1和v_k分别为系统噪声和量测噪声；

Ⅱ)建立SLAM概率模型并采用贝叶斯滤波进行预测和更新；

Ⅲ)采用上所述移动机器人状态估计方法对机器人的位置和方位角进行更新并输出。

进一步的，在所述步骤Ⅱ)中，建立SLAM概率模型为

式中

为k时刻的地图特征点；首先预测，通过移动机器人的运动模型

和k-1时刻的后验概率分布获得k时刻的先验概率分布：

其次是观测更新，利用传感器k时刻的量测测数据z_k对先验概率分布进行矫正，获得后验概率分布：

式中，η为归一化系数。

进一步的，在所述步骤Ⅰ)中，w_k和v_k的方差阵分别为Q_k和R_k，并且满足：

进一步的，

与现有技术相比，本发明的有益技术效果在于：

本发明对于移动机器人的系统模型的噪声方差是否精确已知没有要求，并通过减少状态采样点，降低计算复杂度和计算成本，提高状态估计的精度和数值稳定性。首先，利用晶格采样点，根据量测残差序列和方差，修正了量测方差。其次，利用基于上述修正的量测方差和晶格采样点设计的衰减因子对状态方差进行了修正。本发明的ALKF-SLAM算法，有利于提高移动机器人在未知环境中地图构建和自身定位的精度。

附图说明

图1为本发明在移动机器人导航时使用LKF(晶格卡尔曼滤波)-SLAM和自适应ALKF-SLAM算法的x轴均方根误差对比图。

图2为本发明在移动机器人导航时使用LKF-SLAM和自适应LKF-SLAM算法的y轴的均方根误差对比图。

具体实施方式

下面结合附图和实施例来说明本发明的具体实施方式，但以下实施例只是用来详细说明本发明，并不以任何方式限制本发明的范围。

实施例1：一种设计一种基于自适应晶格卡尔曼滤波的移动机器人状态估计方法，包括如下步骤：

S1)初始化移动机器人的初始状态

和初始方差P₀：

式中，E表示期望，

为零时刻即移动机器人起始点的坐标位置(x,y)和方位角(θ)；

S2)对移动机器人的位置、方位角等状态进行采样，得到状态的晶格采样点集为：

式中，χ_k-1,i为移动机器人第k-1时刻的状态

的第i个采样点，共有N个样本点；

表示的是P_k-1平方根的第i列向量；s_i为状态采样点的权值；

上式对状态进行晶格采样的方法如下：

利用确定的晶格点逼近多元积分的拟蒙特卡罗方法(QMC)。在这里，生成的格点由正态累积分布的逆映射函数对格点进行变换，得到样本点的权重。为了简化计算和提高计算效率，采用秩为1格的晶格规则对进行采样，是一种产生低差点的模化算术方法。

对状态采样用秩为1格的晶格规则如下：

式中，g(a)包含d个整数[a⁰,a²,a³,...,a^d-1]^T，a是一个具有点数N的互质整数。

为了获得无偏积分计算并提高估计精度，根据克兰利-帕特森位移方法(模1位移)生成随机位移格点，则状态采样的位移格点及其权重为：

式中，Δ是一个位移因子，它由超立方体上均匀分布生成的[0,1)^d，s_i为状态采样的位移晶格点的权重，φ^-1是一维正态累积分布φ的逆映射函数

S3)对移动机器人第k步的状态进行一步预测：

式中，

为第i个采样点的预测值；

为移动机器人的状态预测值；

S4)获取方差的一步预测：

式中，

为移动机器人的一步预测误差；Q_k-1为过程噪声的方差；

S5)对状态一步预测的进行重新晶格采样：

式中，

为

的第i个采样点；

S6)计算量测预测值

和量测预测方差P_zz：

式中，Z_k,i为第i个采样点的量测预测值；

为移动机器人的量测预测值；

为移动机器人量测预测方差；

S7)为了提高晶格卡尔曼滤波算法的稳定性，引入自适应算法增加了噪声估计器，以确保状态估计误差方差保持正定。设计了量测方差

和状态方差

来逼近方差真实值。

量测值的方差

根据量测量测残差

计算。对于自适应晶格卡尔曼滤波方法，

的估计公式：

式中，

状态方差由衰减系数μ_k调整，其计算公式为：

式中，

S8)计算滤波增益矩阵K_k：

式中，P_zz为自适应后的移动机器人量测预测方差；P_xz为状态预测值与量测预测值的方差；

S9)更新状态：

S10)计算状态估计误差的方差P_k：

其中P_k,k-1为自适应过后的状态方差。

S11)循环迭代步骤S1)至步骤S1)，得到移动机器人的状态估计值

基于自适应晶格卡尔曼滤波-SLAM的移动机器人自主导航方法，包括如下步骤：

Ⅰ)建立移动机器人的导航系统模型方程，并初始化如下；

其中，

为k时刻移动机器人的坐标位置(x,y)和方位角(θ)，

是控制量，V_k为移动机器人的速度，γ_k为轮子的方位角，z_k是量测量测值，

为特征点的坐标，w_k-1和v_k分别为系统噪声和量测噪声；

Ⅱ)建立SLAM概率模型并采用贝叶斯滤波进行预测和更新；

Ⅲ)采用上所述移动机器人状态估计方法对机器人的位置和方位角进行更新并输出。

在所述步骤Ⅱ)中，建立SLAM概率模型

式中

为k时刻的地图特征点；

首先预测，通过移动机器人的运动模型

和k-1时刻的后验概率分布获得k时刻的先验概率分布：

其次是观测更新，利用传感器k时刻的量测测数据z_k对先验概率分布进行矫正，获得后验概率分布：

式中，η为归一化系数。

在所述步骤Ⅰ)中，w_k和v_k的方差阵分别为Q_k和R_k，并且满足：

进一步的，

上面结合附图和实施例对本发明作了详细的说明，但是，所属技术领域的技术人员能够理解，在不脱离本发明宗旨的前提下，还可以对上述实施例中的各个具体参数进行变更，形成多个具体的实施例，均为本发明的常见变化范围，在此不再一一详述。

Claims (6)

1.一种基于自适应晶格卡尔曼滤波的移动机器人状态估计方法，其特征在于，包括：

S1)初始化移动机器人的初始状态

和初始方差P₀：

式中，E表示期望，

为零时刻即移动机器人起始点的坐标位置(x,y)和方位角(θ)；

S2)对移动机器人的位置、方位角等状态进行采样，得到状态的晶格采样点集为：

式中，χ_k-1,i为移动机器人第k-1时刻的状态

的第i个采样点，共有N个样本点；

表示的是P_k-1平方根的第i列向量；s_i为状态采样点的权值；

S3)对移动机器人第k步的状态进行一步预测：

式中，

为第i个采样点的预测值；

为移动机器人的状态预测值；

S4)获取方差的一步预测：

式中，

为移动机器人的一步预测误差；Q_k-1为过程噪声的方差；

S5)对步骤S3)中的状态一步预测重新进行晶格采样：

式中，

为

的第i个采样点；

S6)计算量测预测值

和量测预测方差P_zz：

式中，Z_k,i为第i个采样点的量测预测值；

为移动机器人的量测预测值；

为移动机器人量测预测方差；

S7)为了提高自适应晶格卡尔曼滤波算法的稳定性，引入自适应算法增加噪声估计器，以确保状态估计误差方差保持正定，设计量测方差

和状态方差

来逼近方差真实值，

量测方差

根据量测残差

计算，对于自适应晶格卡尔曼滤波，

的估计公式：

式中，

状态方差由衰减系数μ_k调整，其计算公式为：

式中，

S8)计算滤波增益矩阵K_k：

式中，P_zz为自适应后的移动机器人量测预测方差；P_xz为状态预测值与量测预测值的方差；

S9)更新状态：

S10)计算状态估计误差的方差P_k：

其中P_k,k-1为自适应过后的状态方差，

S11)循环迭代步骤S1)至步骤S1)，得到移动机器人的状态估计值

2.根据权利要求1所述的基于自适应晶格卡尔曼滤波的移动机器人状态估计方法，其特征在于，所述步骤S2)中，得到状态的晶格采样点集的方法如下：利用确定的晶格点逼近多元积分的拟蒙特卡罗方法(QMC)。在这里，生成的格点由正态累积分布的逆映射函数对格点进行变换，得到样本点的权重。为了简化计算和提高计算效率，采用秩为1格的晶格规则对进行采样，是一种产生低差点的模化算术方法。

对状态采样用秩为1格的晶格规则如下：

式中，g(a)包含d个整数[a⁰,a²,a³,...,a^d-1]^T，a是一个具有点数N的互质整数。

为了获得无偏积分计算并提高估计精度，根据克兰利-帕特森位移方法(模1位移)生成随机位移格点，则状态采样的位移格点及其权重为：

式中，Δ是一个位移因子，它由超立方体上均匀分布生成的[0,1)^d，s_i为状态采样的位移晶格点的权重，φ^-1是一维正态累积分布φ的逆映射函数。

3.一种基于自适应晶格卡尔曼滤波的移动机器人自主导航方法，其特征在于，包括如下步骤：

Ⅰ)建立移动机器人的导航系统模型方程，并初始化；

其中，

为k时刻移动机器人的坐标位置(x,y)和方位角(θ)，

是控制量，V_k为移动机器人的速度，γ_k为轮子的方位角，z_k是量测量测值，

为特征点的坐标，w_k-1和v_k分别为系统噪声和量测噪声；

Ⅱ)建立SLAM概率模型并采用贝叶斯滤波进行预测和更新；

Ⅲ)采用上所述移动机器人状态估计方法对机器人的位置和方位角进行更新并输出。

4.根据权利要求3所述的基于自适应晶格卡尔曼滤波的移动机器人自主导航方法，其特征在于，在所述步骤Ⅱ)中，建立SLAM概率模型为

式中

为k时刻的地图特征点；首先预测，通过移动机器人的运动模型

和k-1时刻的后验概率分布获得k时刻的先验概率分布：

其次是观测更新，利用传感器k时刻的量测测数据z_k对先验概率分布进行矫正，获得后验概率分布：

式中，η为归一化系数。

5.根据权利要求3所述的基于自适应晶格卡尔曼滤波的移动机器人自主导航方法，其特征在于，在所述步骤Ⅰ)中，w_k和v_k的方差阵分别为Q_k和R_k，并且满足：

6.根据权利要求5所述的基于自适应晶格卡尔曼滤波的移动机器人自主导航方法，其特征在于，

Images