[发明专利]一种基于具体化模型的可迭代的卡尔曼滤波定位方法

权利要求书

1.一种基于具体化模型的可迭代的卡尔曼滤波定位方法，其特征在于，该方法包括以下步骤：

步骤1：对地图进行初始化，即对地图的变量设置初值；

步骤2：建立机器人的系统状态方程，并对运动模型初始化；

步骤3：基于具体化模型的可迭代卡尔曼滤波算法进行位姿估计，以提高位姿估计的精度。

2.根据权利要求1所述的一种基于具体化模型的可迭代的卡尔曼滤波定位方法，其特征在于，所述的步骤1中，地图的变量包括路标点的状态和地图的原点，并将路标点的状态的初值设为0，将机器人的初始位姿设为地图的原点。

3.根据权利要求2所述的一种基于具体化模型的可迭代的卡尔曼滤波定位方法，其特征在于，所述的步骤2中，对运动模型初始化具体为：

设置初始时刻机器人的状态的初值和对应的协方差的初值。

4.根据权利要求3所述的一种基于具体化模型的可迭代的卡尔曼滤波定位方法，其特征在于，机器人的系统状态方程为：

x_t＝g(u_t,x_t-1)+ε_t

z_t＝h(x_t)+δ_t

其中，x_t为t时刻的状态向量，g(·)为运动模型，x_t-1为t-1时刻的状态向量，u_t为系统从t-1到t时刻的控制输入，ε_t为运动模型的噪声，服从高斯分布(0,Q_t)，z_t为t时刻路标点的观测向量，h(·)为观测模型，δ_t为观测模型的噪声，服从高斯分布(0,R_t)。

5.根据权利要求4所述的一种基于具体化模型的可迭代的卡尔曼滤波定位方法，其特征在于，所述的步骤3中，基于具体化模型的可迭代卡尔曼滤波算法进行位姿估计的过程包括以下步骤：

步骤31：EKF预测；

步骤32：EKF更新；

步骤33：状态增广。

6.根据权利要求5所述的一种基于具体化模型的可迭代的卡尔曼滤波定位方法，其特征在于，所述的步骤31中，EKF预测的过程具体为：

根据t-1时刻的状态估计值和t时刻的控制输入预测t时刻的系统位姿，得到先验状态估计值和对应的协方差矩阵

7.根据权利要求6所述的一种基于具体化模型的可迭代的卡尔曼滤波定位方法，其特征在于，所述的先验状态估计值的表达式为：

其中，为t时刻的先验状态估计值，g(·)为运动模型，u_t为系统从t-1时刻到t时刻的控制输入，μ_t-1为t-1时刻的状态估计值。

8.根据权利要求7所述的一种基于具体化模型的可迭代的卡尔曼滤波定位方法，其特征在于，所述的协方差矩阵的表达式为：

其中，为t时刻的先验状态估计值对应的协方差矩阵，G_t为运动模型g(·)的雅克比矩阵，表示运动模型g(·)对状态估计值μ_t求偏导，Σ_t-1为t-1时刻的状态估计值μ_t-1对应的协方差矩阵，R_t为观测模型的噪声的协方差矩阵。