本发明涉及动态系统估算技术领域，更具体地说是涉及一种基于augmentedekf的提高里程计精度的方法。
背景技术：
目前，里程计作为对移动机器人agv进行航位推算的主要传感器，具有稳定性高、成本低的特点；但是，agv中里程计的精度主要取决于对agv物理模型的估算，而在agv硬件制作工艺和环境温度导致的硬件形变等因素的限制下，其物理模型实际上是动态变化的，传统的基于里程计的agv航位推算精度受限于agv错误的物理模型，导致航位推算结果随着时间的累计误差不断增大，极大的降低了对agv定位的精度。
卡尔曼滤波是一种高效的自回归滤波，能够在存在诸多不确定性情况的组合信息中估计动态系统的状态，非常适合不断变化的系统，且内存占用较小、速度快，是实时问题和嵌入式系统的理想选择。
因此，如何将agv的物理模型参数拟合到卡尔曼滤波中，根据agv的全局定位动态估算物理模型的参数值，以期提高里程计的精度，是本领域技术人员亟待解决的问题。
技术实现要素：
有鉴于此，本发明提供了一种基于augmentedekf的提高里程计精度的方法，通过将agv的物理模型参数拟合到卡尔曼滤波中，再通过扩展卡尔曼滤波融合全局定位的信息，计算出agv物理模型参数，以期提高里程计精度。
为了实现上述目的，本发明采用如下技术方案：
一种基于augmentedekf的提高里程计精度的方法，包括如下步骤：
s1、将移动机器人agv的初始状态量xk-1以及初始状态量xk-1的协方差矩阵pk-1输入至预测模型中，获得预测状态量
s2、将预测状态量
s3、将获得的更新状态量
具体的，设定初始状态量xk-1＝[xyθrlrrd]；其中，x,y为agv的初始位置坐标值，θ为agv的偏转角度值，rl和rr分别表示agv两个左轮和右轮的半径，d表示agv左轮和右轮之间的距离。
具体的，预测阶段的卡尔曼滤波的动态方程为
具体的，所述初始状态量xk-1＝[xyθrlrrd]在卡尔曼滤波中的动态方程为：
具体的，在每次循环中的jacobian矩阵a为：
其中，
wl为左轮的角速度，wr为右轮角速度，
具体的，所述状态量和观测结果的转换关系h＝[i3o3]；其中，i3为单位矩阵，o3为零矩阵。
具体的，更新模型的卡尔曼增量为：
具体的，将所述状态转换jacobian矩阵a和状态量和观测结果的转换关系h带入更新模型中，动态获得rl、rr和d的值。
具体的，所述步骤s3中导入agv运动学模型的更新状态量
本发明具有以下有益效果：
经由上述的技术方案可知，基于现有技术，本发明提出了一种基于augmentedekf的提高里程计精度的方法，确保里程计在进行航位推算时，能够动态校准agv物理模型参数，减少噪声的干扰，提高了里程计航位推算的精确度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1为基于augmentedekf的提高里程计精度的方法流程图；
图2为雷达与地图完全匹配的初始图；
图3(a)(b)校准前后只基于里程计的3次360度逆时针旋转的航位推算的对比图，图3(a)为未经过校准的只基于里程计的3次360度逆时针旋转的航位推算图，图3(b)为经过校准的只基于里程计的3次360度逆时针旋转的航位推算图。
具体实施方式
下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1所示，本发明提供了如下方法：
一种基于augmentedekf的提高里程计精度的方法，该方法分为预算阶段和更新阶段；
当有里程计数据输入时执行预测阶段，预测阶段过程如下：
s1、将移动机器人agv的初始状态量xk-1以及初始状态量xk-1的协方差矩阵pk-1输入至预测模型中，获得预测状态量
首先设定初始状态量xk-1＝[xyθrlrrd]；其中，x,y为agv的初始位置坐标值，θ为agv的偏转角度值，rl和rr分别表示agv两个左轮和右轮的半径，d表示agv左轮和右轮之间的距离。
具体的，卡尔曼滤波的动态方程为
具体的，初始状态量xk-1＝[xyθrlrrd]在卡尔曼滤波中的动态方程为：
卡尔曼滤波中所有状态均为高斯分布，分布的中心值就是状态值估计值，不确定性即为协方差矩阵，因此，预测状态量
在每次循环中的jacobian矩阵a为：
其中，
wl为左轮的角速度，wr为右轮角速度，
由此获得了状态转换jacobian矩阵a的值；此时，状态量
当有全局观测值输入时执行更新步骤，更新阶段步骤如下：
s2、将预测状态量
更新模型的卡尔曼增量为：
agv的位置(x,y)与偏转角度值θ直接通过全局观测获得，全局观测的位置和偏转角度值可以从自适应蒙特卡洛粒子滤波(amcl)中导出，也可以从其他全局定位方法，例如超宽带uwb，或者基于路标的定位系统中导出。当输入观测结果时，执行更新过程；观测结果和状态量之间的转换公式为：
其中，状态量和观测结果的转换关系h＝[i3o3]；其中，i3为单位矩阵，o3为零矩阵。
因此将状态转换jacobian矩阵a和状态量和观测结果的转换关系h带入更新模型中，即获得了更新状态量
s3、将获得的agv物理模型参数rl、rr和d的值导入agv运动学模型，里程计根据动态的agv物理模型参数进行航位推算。
如图2所示，为雷达与地图完全匹配的初始图，白色点运为雷达读数。
如图3(a)(b)所示，为校准前后的只基于里程计的3次360度逆时针旋转的航位推算图，图3(a)为未经过校准的只基于里程计的3次360度逆时针旋转的航位推算图，图3(b)为经过校准的只基于里程计的3次360度逆时针旋转的航位推算图。很显然校准前如图3(a)所示，里程计对agv的航位推算与初始图2有较大偏差，而经卡尔曼滤波校准后，如图3(b)所示，里程计对agv的航位推算的校准较精确。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
1.一种基于augmentedekf的提高里程计精度的方法，其特征在于，包括如下步骤：
s1、将移动机器人agv的初始状态量xk-1以及初始状态量xk-1的协方差矩阵pk-1输入至预测模型中，获得预测状态量
s2、将预测状态量
s3、将获得的更新状态量
2.根据权利要求1所述的一种基于augmentedekf的提高里程计精度的方法，其特征在于，设定初始状态量xk-1＝[xyθrlrrd]；其中，x,y为agv的初始位置坐标值，θ为agv的偏转角度值，rl和rr分别表示agv两个左轮和右轮的半径，d表示agv左轮和右轮之间的距离。
3.根据权利要求1所述的一种基于augmentedekf的提高里程计精度的方法，其特征在于，预测阶段的卡尔曼滤波的动态方程为
4.根据权利要求1所述的一种基于augmentedekf的提高里程计精度的方法，其特征在于，所述初始状态量xk-1＝[xyθrlrrd]在卡尔曼滤波中的动态方程为：
5.根据权利要求1所述的一种基于augmentedekf的提高里程计精度的方法，其特征在于，在每次循环中的jacobian矩阵a为：
其中，
wl为左轮的角速度，wr为右轮角速度，
6.根据权利要求1所述的一种基于augmentedekf的提高里程计精度的方法，其特征在于，agv的角度与偏转角度值θ直接通过全局观测获得，当输入观测结果时，执行更新过程；观测结果和状态量之间的转换公式为：
7.根据权利要求6所述的一种基于augmentedekf的提高里程计精度的方法，其特征在于，所述状态量和观测结果的转换关系h＝[i3o3]；其中，i3为单位矩阵，o3为零矩阵。
8.根据权利要求1所述的一种基于augmentedekf的提高里程计精度的方法，其特征在于，更新模型的卡尔曼增量为：
更新状态量
9.根据权利要求8所述的一种基于augmentedekf的提高里程计精度的方法，其特征在于，将所述状态转换jacobian矩阵a和状态量和观测结果的转换关系h带入更新模型中，动态获得rl、rr和d的值。
10.根据权利要求1所述的一种基于augmentedekf的提高里程计精度的方法，其特征在于，所述步骤s3中导入agv运动学模型的更新状态量
