CN112097771A - Gps延迟时间自适应的扩展卡尔曼滤波导航算法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于自适应组合导航算法领域，尤其涉及一种测量的速度、位置存在延迟时，INS/GPS自适应组合导航算法。

背景技术

近年来，随着无人驾驶汽车、无人机领域的发展，对运载体的姿态、速度、位置等导航参数的求解精度提出了越来越高的要求。精确可靠的导航算法，是实现运载体无人制导的前提。

在民用领域，大多使用低成本的MEMS惯导传感器进行导航，但是由于低成本MEMS传感器精度不高，仅仅依赖MEMS传感器进行导航会导致解算过程极易发散；若使用纯GPS导航，则存在信号遮挡失效、信号延迟、导航频率过低等缺点，无法应用于高机动的运载体，所以使用INS/GPS组合导航一直是目前的主流方法。一种常见的组合方式是，使用三轴磁力计，MEMS传感器提供的陀螺角速度、加速度，以及GPS接收机提供的速度、位置，进行扩展卡尔曼滤波融合。

保证INS/GPS组合导航求解精度的一个关键要求是，所有传感器的采集出的数据必须在同一个时间点上进行融合，但现实情况是，GPS接收机送入扩展卡尔曼滤波器的速度、位置，相较于角速度、加速度等参数，存在随机延迟，这种延迟来自于以下几个方面：卫星和GPS接收机的钟差、GPS接收机处理器的解算速度、大气传播延迟、主控CPU解析处理GPS报文的延迟，以及GPS接收机穿越遮挡区发生丢星等诸多因素。

然而，一旦获取到了GPS测量的数据的延迟时间，就可以通过缓存MEMS传感器、地磁传感器的数据，在历史时间上进行扩展卡尔曼数据融合，做出精度较高的INS/GPS组合导航。但是由于GPS测量的数据延迟时间，并不是一个固定值，难以在历史时间上对齐GPS的测量值与MEMS传感器的测量值，因此有必要对传统的组合导航算法做出改进，以进一步提高导航参数的求解精度。

发明内容

为解决背景技术中提到的缺陷，本发明展示了一种GPS延迟时间自适应的扩展卡尔曼滤波导航算法。

为实现上述目的，现提供技术方案如下：

一种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为当地重力加速度标量；

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

为大地系东北天三轴位置向量；

D_BG为从载体系到大地系的方向余弦矩阵，其值可由缓存的姿态四元数q获得；

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

定义状态向量

2.3、建立卡尔曼先验估计误差的协方差矩阵递推模型:

P(t+Δt)＝F·P(t)·F^T+Q(t)，

Q为过程误差的协方差矩阵，F为状态转移矩阵，

其中E为3×3的单位矩阵，0为3×3的零矩阵；

2.4、定义观测向量Z＝[v_x v_y v_z p_x p_y p_z]^T；

建立卡尔曼观测矩阵H：

其中三轴速度观测值v_x、v_y、v_z，三轴位置观测值p_x、p_y、p_z可由GPS接收机获得；

2.5、根据步骤一给出的延迟时间td，自适应计算速度、位置的测量误差协方差：

r(t_d)＝r₀·(1+k·arctan(t_d))

其中，r0为速度或位置误差协方差的基准值，k代表协方差随时间延迟而做出非线性自适应增大的增益系数；

2.6、根据自适应的测量值误差协方差，更新卡尔曼增益：

K(t+Δt)＝P(t+Δt)H^T(t+Δt)(H(t+Δt)P(t+Δt)H(t+Δt)+R(t+Δt,t_d))

尤其注意到，上式中测量误差的协方差定义为：R＝R(t+Δt,t_d)；

2.7、更新卡尔曼后验估计：

2.8、更新后验估计的误差协方差矩阵：

P⁺(t+Δt)＝(E-K(t+Δt)H(t+Δt))P^-(t+Δt)；

步骤3，执行无监督惯导递推，同时返回步骤1，去准备新一轮的数据。

本发明的有益效果：

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

附图说明

图1为本发明算法流程图示意图。

具体实施方式

为使本领域技术人员更加清楚和明确本发明技术方案，下面结合附图对本发明技术方案进行详细描述，但本发明的实施方式不限于此。

为了克服GPS测量时间延迟，对组合导航参数求解精度的不良影响，本发明的目的是，提供一种把GPS测量延迟时间考虑在内的组合导航算法，即使GPS测量时间存在随机延迟，仍能够使得GPS的测量值与MEMS传感器的测量值能够在历史时间上自适应对齐，为后续导航参数的求解过程提供良好的保障。

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

首先定义：与运载体(例如无人车、无人机)固连的坐标系称为“载体坐标系”，与大地固连的坐标系称为“大地坐标系”。

本发明提出的算法迭代过程中需要用到方向测量工具，如磁力计或双GPS测向等，这些内容对于本领域工程人员来说属于已公开知识，下文中关于测向的内容不再出现。

对于GPS接收机采集的速度，进行差分处理，得出加速度a_S，通过先入先出队列FIFO将M秒内的a_S进行缓存。

为避免差分处理所导致的a_S数值异常跳动，可对GPS采集的速度施加二阶以上高阶低通滤波，截止频率记为fa，进而获取平滑的加速度值。

通过姿态四元数将IMU输出的载体坐标系下的加速度a_B，和加速度计0偏，转换为大地系下的加速度a_G，通过先入先出队列FIFO将N秒内采集的a_G进行缓存，N至少比M多Tr秒,Tr＝2。

对IMU采集的加速度，亦施加截止频率为fa的二阶以上高阶低通滤波。

建立关于GPS延迟时间t_d的代价函数:

以代价函数取得最小值为优化目标,以GPS的采集周期为步长，对t_d从0到M进行进行扫描，获取优化后t_d的值。

优化过程在大机动过程中应增加权重，在平稳过程中存在较大误差，应减小权重。

一旦GPS接收机的速度、位置获得更新，则立即从FIFO中取出t_d秒之前的加速度度计信息，执行式(2)所述的先验估计过程,并建立离散的非线性标准运动状态模型：

其中：

Δt为算法迭代周期；

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

为大地系的加速度向量；

为加速度计在载体系的测量值向量；

g为当地重力加速度标量；

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

为大地系东北天三轴位置向量；

D_BG为从载体系到大地系的方向余弦矩阵，其值可由缓存的姿态四元数q获得：

定义状态向量X＝[v_x v_y v_z p_x p_y p_z a_Bbx a_Bby a_Bbz]^T，建立卡尔曼先验估计误差的协方差矩阵递推模型：

P(t+Δt)＝F·P(t)·F^T+Q

其中：

Q为过程误差的协方差矩阵；F为状态转移矩阵，而且，F可由式(1)来获得：

其中E为3×3的单位矩阵，0为3×3的零矩阵。

定义观测向量Z＝[v_x v_y v_z p_x p_y p_z]^T，建立卡尔曼观测矩阵H：

其中三轴速度观测值v_x、v_y、v_z，三轴位置观测值p_x、p_y、p_z可由GPS接收机获得。

基于前述状态转移矩阵、观测矩阵等，执行后续卡尔曼迭代算法：计算卡尔曼增益、计算后验估计、计算后验估计的误差协方差矩阵。

在计算卡尔曼增益时，速度、位置测量误差的协方差的对角元要与GPS延迟时间自适应，公式如下：

r(t_d)＝r₀·(1+k·arctan(t_d)) (3)

其中，r₀为速度或位置误差协方差的基准值，k代表协方差随时间延迟而做出非线性自适应增大的增益系数。

在计算出后验估计的基础上，从FIFO中取出从t-t_d到t时刻的加速度计数据，再次利用式(1)所述的运动模型进行无监督的惯导推进。

以上所述，仅是本发明的最佳实施例而已，并非对本发明的任何形式的限制，任何熟悉本领域的技术人员，在不脱离本发明技术方案范围的情况下利用上述揭示的方法和内容对本发明做出的许多可能的变动和修饰，均属于权利要求书保护的范围。

Claims (1)

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为当地重力加速度标量；

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

为大地系东北天三轴位置向量；

D_BG为从载体系到大地系的方向余弦矩阵，其值可由缓存的姿态四元数q获得；

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

定义状态向量

2.3、建立卡尔曼先验估计误差的协方差矩阵递推模型:

P(t+Δt)＝F·P(t)·F^T+Q(t)，

Q为过程误差的协方差矩阵，F为状态转移矩阵，

其中E为3×3的单位矩阵，0为3×3的零矩阵；

2.4、定义观测向量Z＝[v_x v_y v_z p_x p_y p_z]^T；

建立卡尔曼观测矩阵H：

其中三轴速度观测值v_x、v_y、v_z，三轴位置观测值p_x、p_y、p_z可由GPS接收机获得；

2.5、根据步骤一给出的延迟时间td，自适应计算速度、位置的测量误差协方差：

r(t_d)＝r₀·(1+k·arctan(t_d))

其中，r0为速度或位置误差协方差的基准值，k代表协方差随时间延迟而做出非线性自适应增大的增益系数；

2.6、根据自适应的测量值误差协方差，更新卡尔曼增益：

K(t+Δt)＝P(t+Δt)H^T(t+Δt)(H(t+Δt)P(t+Δt)H(t+Δt)+R(t+Δt,t_d))

尤其注意到，上式中测量误差的协方差定义为：R＝R(t+Δt,t_d)；

2.7、更新卡尔曼后验估计：

2.8、更新后验估计的误差协方差矩阵：

P⁺(t+Δt)＝(E-K(t+Δt)H(t+Δt))P^-(t+Δt)；

步骤3，执行无监督惯导递推，同时返回步骤1，去准备新一轮的数据。

Images