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

权利要求书

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对先验概率分布进行矫正，获得后验概率分布：

式中，η为归一化系数。