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

说明书

本发明提出了一种基于Augmented EKF的提高里程计精度的方法，包括如下步骤：S1、将移动机器人AGV的初始状态量X_k‑1以及初始状态量X_k‑1的协方差矩阵P_k‑1输入至预测模型中，获得预测状态量和预测状态量的协方差矩阵S2、将预测状态量和预测状态量的协方差矩阵作为更新过程的输入，输入至更新模型中，获得更新状态量S3、将获得的更新状态量导入AGV运动学模型，里程计根据动态的AGV物理模型参数进行航位推算。通过将AGV的物理模型参数拟合到卡尔曼滤波中，再通过扩展卡尔曼滤波融合全局定位的信息，计算出AGV物理模型参数，确保里程计在进行航位推算时，能够动态校准AGV物理模型参数，减少噪声的干扰，提高了里程计航位推算的精确度。

技术领域

本发明涉及动态系统估算技术领域，更具体地说是涉及一种基于Augmented EKF的提高里程计精度的方法。

背景技术

目前，里程计作为对移动机器人AGV进行航位推算的主要传感器，具有稳定性高、成本低的特点；但是，AGV中里程计的精度主要取决于对AGV物理模型的估算，而在AGV硬件制作工艺和环境温度导致的硬件形变等因素的限制下，其物理模型实际上是动态变化的，传统的基于里程计的AGV航位推算精度受限于AGV错误的物理模型，导致航位推算结果随着时间的累计误差不断增大，极大的降低了对AGV定位的精度。

卡尔曼滤波是一种高效的自回归滤波，能够在存在诸多不确定性情况的组合信息中估计动态系统的状态，非常适合不断变化的系统，且内存占用较小、速度快，是实时问题和嵌入式系统的理想选择。

因此，如何将AGV的物理模型参数拟合到卡尔曼滤波中，根据AGV的全局定位动态估算物理模型的参数值，以期提高里程计的精度，是本领域技术人员亟待解决的问题。

发明内容

有鉴于此，本发明提供了一种基于Augmented EKF的提高里程计精度的方法，通过将AGV的物理模型参数拟合到卡尔曼滤波中，再通过扩展卡尔曼滤波融合全局定位的信息，计算出AGV物理模型参数，以期提高里程计精度。

为了实现上述目的，本发明采用如下技术方案：

一种基于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为过程噪声矩为阵，在AGV状态转移的过程中，除了状态具有不确定性外，移动也具有不确定性，将移动的不确定性定义为过程噪声矩阵Q，该噪声源于外部干扰和系统误差，例如AGV左轮或者右轮打滑等，总体的不确定度即过程不确定度之和。