GPS延迟时间自适应的扩展卡尔曼滤波导航算法制造技术

本发明专利技术属于自适应组合导航算法领域，尤其涉及一种测量的速度、位置存在延迟时，INS/GPS自适应组合导航算法。本发明专利技术基于缓存的历史IMU数据和GPS数据，提出了一种通过代价函数优化算法提取GPS的速度、位置测量的延迟时间的方法，可有效解决GPS/IMU数据对齐的问题。在执行GPS/INS扩展卡尔曼滤波组合导航算法过程中，基于延迟时间进行测量值误差协方差的自适应调整，有效提高了在测量值大延迟环境下的导航精度。

【技术实现步骤摘要】
GPS延迟时间自适应的扩展卡尔曼滤波导航算法
本专利技术属于自适应组合导航算法领域，尤其涉及一种测量的速度、位置存在延迟时，INS/GPS自适应组合导航算法。
技术介绍
近年来，随着无人驾驶汽车、无人机领域的发展，对运载体的姿态、速度、位置等导航参数的求解精度提出了越来越高的要求。精确可靠的导航算法，是实现运载体无人制导的前提。在民用领域，大多使用低成本的MEMS惯导传感器进行导航，但是由于低成本MEMS传感器精度不高，仅仅依赖MEMS传感器进行导航会导致解算过程极易发散；若使用纯GPS导航，则存在信号遮挡失效、信号延迟、导航频率过低等缺点，无法应用于高机动的运载体，所以使用INS/GPS组合导航一直是目前的主流方法。一种常见的组合方式是，使用三轴磁力计，MEMS传感器提供的陀螺角速度、加速度，以及GPS接收机提供的速度、位置，进行扩展卡尔曼滤波融合。保证INS/GPS组合导航求解精度的一个关键要求是，所有传感器的采集出的数据必须在同一个时间点上进行融合，但现实情况是，GPS接收机送入扩展卡尔曼滤波器的速度、位置，相较于角速度、本文档来自技高网...

【技术保护点】
1.一种GPS延迟时间自适应的扩展卡尔曼滤波导航算法，其特征在于：包括/n步骤1，准备数据；/n1.1、读取传感器IMU采集的加速度和GPS采集的速度；/n1.2、对原始数据做高阶低通滤波处理，以便消除数据跳动；/n1.3、将处理后的数据送入先入先出队列FIFO进行缓存；/n1.4、以一定频率，取出一段时间内的缓存数据，以最小化代价函数为目标，进行迭代优化直至找出最佳延迟时间td，建立关于GPS延迟时间的代价函数:/n

【技术特征摘要】
1.一种GPS延迟时间自适应的扩展卡尔曼滤波导航算法，其特征在于：包括
步骤1，准备数据；
1.1、读取传感器IMU采集的加速度和GPS采集的速度；
1.2、对原始数据做高阶低通滤波处理，以便消除数据跳动；
1.3、将处理后的数据送入先入先出队列FIFO进行缓存；
1.4、以一定频率，取出一段时间内的缓存数据，以最小化代价函数为目标，进行迭代优化直至找出最佳延迟时间td，建立关于GPS延迟时间的代价函数:



以代价函数取得最小值为优化目标,以GPS的采集周期为步长，对td从0到M进行进行扫描，获取优化后td的值；
步骤2，执行自适应扩展卡尔曼滤波迭代；
2.1、取出步骤1中FIFO中的最新数据，以及延迟时间td；
2.2、执行先验估计过程：



其中：
Δt为算法迭代周期；

为载体系加速度计0偏向量；

为大地系的加速度向量；

为加速度计在载体系的测量值向量；
g为当地重力加速度标量；

为大地系东北天三轴速度向量；

为大地系东北天三轴位置向量；
DBG为从载体系到大地系的方向余弦矩阵，其值可由缓存的姿态四元数q获得；



按照先验估计过程的非线性运动状态方程进行状态一步预测，这一过程可描述为：

【专利技术属性】
技术研发人员：刘月娟，赵秋玲，丁晓玲，张威，吕英杰，
申请(专利权)人：青岛职业技术学院，
类型：发明
国别省市：山东;37