CN113532416A - 基于抗差秩卡尔曼滤波的轮式机器人状态估计方法及自主导航方法 - Google Patents

Abstract

本发明公开了一种基于抗差秩卡尔曼滤波的轮式机器人状态估计方法及自主导航方法，旨在解决现有的SLAM技术无法处理非线性不确定系统、导航定位精度不高的技术问题。本发明中抗差秩卡尔曼滤波‑SLAM自主导航方法对于轮式机器人的系统模型是否是高斯分布没有要求，且当量测数据的噪声统计受未知环境影响不准确时具有更强的鲁棒性和导航定位精度，有利于提高轮式机器人在未知环境中的定位与建图的精度。

Description

基于抗差秩卡尔曼滤波的轮式机器人状态估计方法及自主导 航方法

技术领域

本发明涉及机器人自主导航技术领域，具体涉及一种基于抗差秩卡尔曼滤波的轮式机器人状态估计方法及自主导航方法。

背景技术

轮式机器人在人们的生产、生活的各个领域发挥着重要作用，比如生活助理、流水线生产、巡逻、灾难救援、空间探测等等。轮式机器人要实现在未知环境中的自主移动，首先要对周围环境进行感知和识别，以及精确定位。机器人的精确定位和环境感知问题是机器人实现自主导航的关键技术，也是机器人研究领域的热点。

同步地定位与地图创建(Simultaneous Localization and Mapping，SLAM)技术，是解决机器人在未知环境中导航问题的最佳手段。机器人的SLAM技术在移动过程中通过传感器来精确估计自身位姿，同时增量式的构建真实环境地图。SLAM技术主要包括两方面内容：建立准确的环境地图和实现精确的定位能力。现有扩展卡尔曼滤波(EKF)-SLAM技术解决了卡尔曼滤波智能处理线性高斯系统的限制，但EKF是利用泰勒展开的一阶项来近似非线性系统，无法处理强非线性系统问题，处理过程中出现的截断误差严重影响了机器人的导航定位精度。

发明内容

本发明提供一种基于抗差秩卡尔曼滤波-SLAM(抗差RKF-SLAM)的轮式机器人状态估计方法及自主导航方法，旨在解决现有的SLAM方法无法处理非线性不确定系统、导航定位精度不高的技术问题。

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

设计一种基于抗差秩卡尔曼滤波的轮式机器人状态估计方法，包括如下步骤：

S1)初始化轮式机器人的初始状态

和初始方差P₀：

S2)对轮式机器人的位置、方位角等状态进行采样，得到秩采样点集为：

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

的第i个采样点，共有4n个样本点；n为

的维数；

表示的是P_k-1平方根的第i列向量；u_p1与u_p2为正态偏量，其中p₁＝0.6852,p₂＝0.8704,u_p1＝0.4823,u_p2＝1.1281；

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

式中，

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

为轮式机器人的状态预测值；

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

式中，P_k/k-1为轮式机器人的一步预测误差；Q_k-1为过程噪声的方差；

为协方差权重系数；

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

式中，χ_k,i为

的第i个采样点；

S6)计算量测预测值

和量测预测方差P_zz：

Z_k,i＝h(χ_k,i),i＝1,2,…,4n (8)；

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

为轮式机器人的量测预测值；

为轮式机器人量测预测方差；

S7)计算量测噪声方差阵R_k的修正矩阵

式中，

为等加权矩阵；

为

的第i行第j列元素；

为量测预测误差分量，r′_i＝r_i/σ_ri为相应的标准残差分量，

为r_i的标准差；σ_i,j为量测噪声方差阵R_k的第i行第j列元素；c为一个待定的常数，其取值范围为1.3到2.0之间，基于长期的实践经验，在本发明中优选取值为1.5；max(|r′_i|,|r′_j|)表示取|r′_i|和|r′_j|之间的最大值；

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

式中，P_zz为修正后的轮式机器人量测预测方差；P_xz为状态预测值与量测预测值的协方差；

S9)更新状态：

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

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

基于抗差秩卡尔曼滤波-SLAM的轮式机器人自主导航方法，包括如下步骤：

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

其中，

为k时刻轮式机器人的坐标位置(x，y)和方位角(θ)，u_k是控制量，z_k是观测量，w_k-1和v_k分别为系统噪声和量测噪声；

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

Ⅲ)采用权利要求1所述轮式机器人状态估计方法对机器人的位置和方位角进行更新并输出。

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

式中

为k时刻的地图特征点；

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

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

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

式中，η为归一化系数。

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

进一步的，

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

本发明状态估计方法及自主导航方法对于轮式机器人的系统模型是否是高斯分布没有要求，且当量测数据的噪声统计受未知环境影响不准确时，具有更强的鲁棒性和导航定位精度，有利于提高轮式机器人在未知环境中的定位与建图的精度。

附图说明

图1为本发明在轮式机器人导航时使用RKF-SLAM和抗差RKF-SLAM算法的x轴均方根误差对比图。

图2为本发明在轮式机器人导航时使用RKF-SLAM和抗差RKF-SLAM算法的y轴的均方根误差对比图。

具体实施方式

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

在以下实施例中所涉及的量测方法或测算方法，如无特别说明，均为常规方法。

实施例：基于抗差秩卡尔曼滤波-SLAM的轮式机器人自主导航方法，包括以下步骤：

步骤一：针对某轮式机器人建立对应的导航系统模型：

其中，

为k时刻轮式机器人的位置(x和y)和方位角(θ)，u_k是控制量，z_k是观测量，w_k-1和v_k分别为系统噪声和量测噪声；

步骤二：建立SLAM概率模型

式中

为k时刻的地图特征点；

SLAM概率模型采用贝叶斯理论进行计算。

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

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

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

式中，η为归一化系数。

因此，整个轮式机器人的SLAM自主导航方法就可以用抗差秩卡尔曼滤波来解决。

步骤三：初始化轮式机器人的初始状态：

步骤四：对轮式机器人的位置、方位角等状态进行采样，得到秩采样点集为：

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

的第i个采样点，共有4n个样本点；n为

的维数；

表示的是P_k-1平方根的第i列向量；u_p1与u_p2为正态偏量，其中p₁＝0.6852,p₂＝0.8704,u_p1＝0.4823,u_p2＝1.1281；

步骤五：对轮式机器人第k步的状态进行一步预测：

式中，

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

为轮式机器人的状态预测值；

步骤六：获取协方差的一步预测：

式中，P_k/k-1为轮式机器人的一步预测误差；Q_k-1为过程噪声的方差；

为协方差权重系数；

步骤七：对状态一步预测的进行重新秩采样：

式中，χ_k,i为

的第i个采样点；

步骤八：计算量测预测值

和量测预测方差P_zz：

Z_k,i＝h(χ_k,i),i＝1,2,…,4n (11)；

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

为轮式机器人的量测预测值；

为轮式机器人量测预测方差；

步骤九：计算量测噪声方差阵R_k的修正矩阵

式中，

为等加权矩阵；

为

的第i行第j列元素；

为量测预测误差分量，r′_i＝r_i/σ_ri为相应的标准残差分量，

为r_i的标准差；σ_i,j为量测噪声方差阵R_k的第i行第j列元素；c为一个待定的常数，其取值范围为1.3到2.0之间，在本例中取值1.5；max(|r′_i|,|r′_j|)表示取|r′_i|和|r′_j|之间的最大值。

步骤十：计算滤波增益矩阵K_k：

式中，P_zz为修正后的轮式机器人量测预测方差；P_xz为状态预测值与量测预测值的协方差；

步骤十一：更新状态：

步骤十二：计算状态估计误差的方差P_k：

步骤十三：循环迭代步骤三至步骤十二，得到轮式机器人的状态估计值

步骤一中w_k和v_k的方差阵分别为Q_k和R_k，并且满足：

本例中，Q_k和R_k分别为：

和

在本实施例中，采样时间间隔h＝0.025[s]，小车速度v＝3[m/s]，角速度γ＝π/6[rad/s]小车轴距d＝4[m]，路标检测时间间隔t＝0.3[s]；由图1和图2所示，采用本例所构建的抗差秩卡尔曼滤波-SLAM导航方法时，位置的均方根误差较小，x方向的误差小于0.06m范围之内，y方向的误差小于0.13m，即抗差秩卡尔曼滤波-SLAM导航方法的整体估计误差小、效果好，对无人车在未知环境中的定位与建图精度更高，更能适应未知环境。

上面结合附图和实施例对本发明作了详细的说明，但是，所属技术领域的技术人员能够理解，在不脱离本发明构思的前提下，还可以对上述实施例中的各个具体参数进行变更，或者是对相关方法、步骤等进行等同替代，从而形成多个具体的实施例，均为本发明的常见变化范围，在此不再一一详述。

Claims (5)

1.一种基于抗差秩卡尔曼滤波的轮式机器人状态估计方法，其特征在于，包括如下步骤：

S1)初始化轮式机器人的初始状态

和初始方差P₀：

S2)对轮式机器人的位置、方位角进行采样，得到秩采样点集为：

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

的第i个采样点，共有4n个样本点；n为

的维数；

表示的是P_k-1平方根的第i列向量；u_p1与u_p2为正态偏量，其中p₁＝0.6852,p₂＝0.8704,u_p1＝0.4823,u_p2＝1.1281；

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

式中，

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

为轮式机器人的状态预测值；

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

式中，P_k/k-1为轮式机器人的一步预测误差；Q_k-1为过程噪声的方差；

为协方差权重系数；

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

式中，χ_k,i为

的第i个采样点；

S6)计算量测预测值

和量测预测方差P_zz：

Z_k,i＝h(χ_k,i),i＝1,2,…,4n (8)

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

为轮式机器人的量测预测值；

为轮式机器人量测预测方差；

S7)计算量测噪声方差阵R_k的修正矩阵

式中，

为等加权矩阵；

为

的第i行第j列元素；

为量测预测误差分量，r′_i＝r_i/σ_ri为相应的标准残差分量，

为r_i的标准差；σ_i,j为量测噪声方差阵R_k的第i行第j列元素；c为常数，其取值范围为1.3～2.0；max(|r′_i|,|r′_j|)表示取|r′_i|和|r′_j|之间的最大值；

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

式中，P_zz为修正后的轮式机器人量测预测方差；P_xz为状态预测值与量测预测值的协方差；

S9)更新状态：

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

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

2.一种基于抗差秩卡尔曼滤波-SLAM的轮式机器人自主导航方法，其特征在于，包括如下步骤：

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

其中，

为k时刻轮式机器人的坐标位置(x，y)和方位角(θ)，u_k是控制量，z_k是观测量，w_k-1和v_k分别为系统噪声和量测噪声；

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

Ⅲ)采用权利要求1所述轮式机器人状态估计方法对机器人的位置和方位角进行更新并输出。

3.根据权利要求2所述的基于抗差秩卡尔曼滤波-SLAM的轮式机器人自主导航方法，其特征在于，在所述步骤Ⅱ)中，

建立SLAM概率模型

式中

为k时刻的地图特征点；

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

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

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

式中，η为归一化系数。

4.根据权利要求2所述的基于抗差秩卡尔曼滤波-SLAM的轮式机器人自主导航方法，其特征在于，在所述步骤Ⅰ)中，w_k和v_k的方差阵分别为Q_k和R_k，并且满足：

5.根据权利要求4所述的基于抗差秩卡尔曼滤波-SLAM的轮式机器人自主导航方法，其特征在于，

Images