[发明专利]一种基于Augmented EKF的提高里程计精度的方法

权利要求书

1.一种基于Augmented EKF的提高里程计精度的方法，其特征在于，包括如下步骤：

S1、将移动机器人AGV的初始状态量X_k-1以及初始状态量X_k-1的协方差矩阵P_k-1输入至预测模型中，获得预测状态量和预测状态量的协方差矩阵

S2、将预测状态量和预测状态量的协方差矩阵作为更新过程的输入，输入至更新模型中，获得更新状态量

S3、将获得的更新状态量导入AGV运动学模型，里程计根据动态的AGV物理模型参数进行航位推算；

还包括：设定初始状态量X_k-1＝[x y θ r_L r_R d]；其中，x,y为AGV的初始位置坐标值，θ为AGV的偏转角度值，r_L和r_R分别表示AGV两个左轮和右轮的半径，d表示AGV左轮和右轮之间的距离；

其中，预测阶段的卡尔曼滤波的动态方程为其中，k为时间点，v_k为在k时间点获得的里程计线速度，w_k为在k时间点获得的里程计的转向速度；预测状态量的不确定性用协方差更新方程表示，其中，为预测状态量的协方差，A为状态转换Jacobian矩阵，Q为过程噪声矩阵；

在每次循环中的Jacobian矩阵A为：

其中，

w_L为左轮的角速度，w_R为右轮角速度，和

2.根据权利要求1所述的一种基于Augmented EKF的提高里程计精度的方法，其特征在于，所述初始状态量X_k-1＝[x y θ r_L r_R d]在卡尔曼滤波中的动态方程为：

3.根据权利要求1所述的一种基于Augmented EKF的提高里程计精度的方法，其特征在于，AGV的角度与偏转角度值θ直接通过全局观测获得，当输入观测结果时，执行更新过程；观测结果和状态量之间的转换公式为：其中，H为状态量和观测结果的转换关系，z为观测状态，u为观测噪声。

4.根据权利要求3所述的一种基于Augmented EKF的提高里程计精度的方法，其特征在于，所述状态量和观测结果的转换关系H＝[I₃ O₃]；其中，I₃为单位矩阵，O₃为零矩阵。

5.根据权利要求1所述的一种基于Augmented EKF的提高里程计精度的方法，其特征在于，更新模型的卡尔曼增量为：

更新状态量更新状态量的协方差矩阵其中，所述K_k表示K时刻的卡尔曼增益，R表示观测误差的协方差矩阵，表示观测值和预测值的残差，即观测噪声u。

6.根据权利要求5所述的一种基于Augmented EKF的提高里程计精度的方法，其特征在于，将所述状态转换Jacobian矩阵A和状态量和观测结果的转换关系H带入更新模型中，动态获得r_L、r_R和d的值。

7.根据权利要求1所述的一种基于Augmented EKF的提高里程计精度的方法，其特征在于，所述步骤S3中导入AGV运动学模型的更新状态量包括r_L、r_R和d。