CN110702104B - 一种基于车辆零速检测的惯性导航误差修正方法 - Google Patents

Abstract

本发明涉及一种基于车辆零速检测的惯性导航误差修正方法，在车辆行驶时，采集轮速传感数据及INS数据，在车辆行驶过程中零速检测算法通过轮速、加速度计、陀螺仪进行零速检测；零速检测的原理是对轮速、加速度计、陀螺仪信号进行基于黎曼皮尔逊准则的联合概率，零速修正的原理是：1、在车辆检测出零速时，对速度和角速度进行置零处理，防止速度误差和角速度误差累积，导致位置和姿态出现偏差；2、在检测出零速后通过加速度计和角速度计的输出，分别对姿态、陀螺仪零偏进行再估计；3、在车辆再次运动退出零速状态后，利用再估计出的姿态和陀螺仪零偏对INS进行修正。与现有技术相比，本发明具有抑制导航误差的漂移，提高检测的准确度等优点。

Description

一种基于车辆零速检测的惯性导航误差修正方法

技术领域

本发明涉及车载导航与定位领域，尤其是涉及一种基于车辆零速检测的惯性导航误差修正方法。

背景技术

惯性导航系统(Inertial Navigation System，INS)能根据惯性传感器(陀螺仪、加速度计)提供的载体相对于惯性空间的线速度和角速度信息，成为车载组合定位系统关键部件。惯性导航系统具有独立工作、精度高、不易被电子干扰的优点，但最明显的缺点是其导航误差会随时间积累，因而长时间工作后会产生不同程度的积累误差。

零速修正技术是克服惯导定位定姿累计误差发散最为实用的一种约束方法，该技术包含零速区间探测和零速更新两个部分，其中，零速检测是零速更新的基础。此外，零速检测信息可以作为INS相对于地球静止的参考，通过准静态对准来标定INS的速度、姿态和IMU(Inertial measurement unit，惯性测量单元)误差。利用传统的INS零速检测技术可对惯性导航误差进行修正，传统的INS零速检测技术将车辆加速度计和陀螺仪采集的数据作为检测量，然而，该方法的检测的准确度不够高，且无法抑制导航误差的漂移。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种修正准确率高的基于车辆零速检测的惯性导航误差修正方法。

本发明的目的可以通过以下技术方案来实现：

一种基于车辆零速检测的惯性导航误差修正方法，包括零速检测步骤和零速修正步骤：

零速检测步骤：

采用三轴加速度数据、三轴陀螺仪进行数据采集。

1.1、车辆启动后，采集车辆INS数据及轮速传感器的轮速数据，车辆INS数据包括加速度计数据和陀螺仪数据。

假设某一时刻k的所有传感器观测值为y_k，式中，为k时刻三轴加速度计的观测向量，/>为k时刻三轴陀螺仪的观测向量，为k时刻轮速传感器的观测向量。选择观测窗口期为n至n+N-1，n为窗口期开始时间点的数据，N为窗口期数据数量，得到一组连续观测序列/>

1.2、基于采集的数据，在车辆静止时，利用零速检测方法对车辆零速进行实时检测，若检测出零速，则进入零速修正步骤。

121)基于采集的数据，构建分布参数的最大似然估计，计算所有传感器联合概率的广义似然比。具体地：

a)定义H₀表示载体为运动状态，H₁表示载体为静止状态，零速检测器的性能由检测概率P_D＝p(H₀|H₁)和虚警概率P_FA＝p(H₁|H₀)决定，根据Neyman-Pearson理论，对于给定的P_FA＝α，使判断为H₁状态时检测概率P_D最大，需满足：

式中，L(q_n)为观测序列q_n的似然比，其中，阈值γ由下式决定：

b)考虑到假设H₀下信号较难描述，但H₁情况下车辆静止且比力只有重力，三轴加速度计输出应为当地重力加速度，三轴陀螺仪输出应为0，轮速传感器输出也应为0，则设定信号约束条件为：

式中，为方向为重力方向的单位矢量，/>观测窗口期为n至n+N-1，n为窗口期开始时间点的数据，N为窗口期数据数量，l为窗口期中任意的一个数据，g为重力加速度，/>分别为加速度计、陀螺仪及轮速传感器真实值；/>分别为加速度计、陀螺仪及轮速传感器的测量噪声。

c)结合步骤a)、b)获取所有传感器联合概率的广义似然比的表达式。

所有传感器联合概率的广义似然比的表达式为：

式中，γ'＝-2(lnγ)/N，γ为阈值；其中/>为k时刻加速度计的观测向量，/>为k时刻陀螺仪的观测向量，/>为k时刻轮速传感器的观测向量；/>分别为加速度计、陀螺仪及轮速传感器的噪声方差值。

d)计算IMU及轮速传感器输出数据，若满足所有传感器联合概率的广义似然比的表达式，则认为载体处于静止状态。

122)计算IMU及轮速传感器输出数据，若满足所有传感器联合概率的广义似然比，则认为载体处于静止状态，即检测出零速。

零速修正步骤：

2.1、利用零速状态下的加速度计数据，对车辆姿态角中的俯仰角θ和翻滚角Φ的估计，其表达式为：

式中，a_x、a_y、a_z为三轴加速度计输出，Ω_n＝{l∈N:n≤l＜N-1}，观测窗口期为n至n+N-1，n为窗口期开始时间点的数据，N为窗口期数据数量，l为窗口期中任意的一个数据。

2.2、利用零速时各传感器的输出，对陀螺仪的零偏进行再估计。

假设陀螺仪零偏为[b_x b_y b_x]，利用零速时各传感器的输出，对陀螺仪的零偏进行再估计的表达式为：

式中，ω_x,ω_y,ω_z分别为原始的三轴陀螺仪的输出角速度。

2.3、当车辆启动后，利用估计的车辆姿态及陀螺仪的零偏对INS误差进行修正。修正方法的表达式为：

式中，分别为修正后的三轴陀螺仪的输出角速度。

与现有技术相比，本发明具有以下优点：

一、本发明面向车载组合导航，提出了一种零速检测方法和惯导误差修正方法，该方法相比于传统的INS零速检测，加入了轮速传感器作为新的检测量，通过采集轮速传感器和INS数据，利用黎曼皮尔逊假设理论构造多传感器联合概率密度的最大似然比，对车辆零速状态进行检测，在检测出车辆零速后，通过零速信息对速度、姿态进行更新，并对陀螺仪零偏以及车辆这翻滚角、俯仰角进行估计，本发明方法提高了检测的准确度；

二、本发明方法考虑到车辆需要启停的特性，能够有效地应用在车辆行驶的过程中，利用零速检测和零速修正对导航误差的漂移进行抑制，并对导航累计误差进行有效的修正。

附图说明

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

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

本发明涉及一种基于车辆零速检测的惯性导航误差修正方法，本发明分为两个模块，分别是零速检测模块和零速修正模块。零速检测模块为零速修正模块的输入，提供车辆是否处于零速的判断，以下称零速Flag信号。若车辆处于零速，则运行零速修正模块；若车辆不处于零速状态，则不允许零速修正模块。

零速检测模块包括以下步骤：

步骤1、车辆启动后，采集加速度计、陀螺仪数据和轮速传感器采集的轮速数据。

通过加速度方差检测器、加速度幅值检测器、角速度能量检测器，计算得到加速度方差、加速度幅值、角速度能量。优选地，采用三轴加速度数据、三轴陀螺仪进行数据采集。即车辆行驶过程中采集的数据包括惯性测量数据，即三轴加速度数据、三轴陀螺仪数据以及轮速传感器采集的轮速数据。

假设某一时刻k的所有传感器观测值为y_k，式中，为k时刻三轴加速度计的观测向量，/>为k时刻三轴陀螺仪的观测向量，/>为k时刻轮速传感器的观测向量。选择观测窗口期为n至n+N-1，n为窗口期开始时间点的数据，N为窗口期数据数量，得到一组连续观测序列/>

步骤2、通过构建分布参数的最大似然估计，计算所有传感器联合概率的广义似然比。

假设传感器测量值可以用下式进行描述：

y_k＝s_k+v_k

其中：

式中，分别表示三轴加速度计、三轴陀螺仪及轮速传感器真实值；/>分别表示三轴加速度计、三轴陀螺仪及轮速传感器的测量噪声。假设示三轴加速度计、三轴陀螺仪及轮速传感器的噪声项均为独立分布的高斯白噪声，即协方差矩阵为：

式中，I₃表示三维单位矩阵，0_1×3(0_3×1)表示大小为1×3(3×1)的零矩阵； 分别表示三轴加速度计、三轴陀螺仪及轮速传感器的噪声方差值。

零速检测可作为二元假设检验问题进行研究，并定义假设为：

H₀:载体为运动状态

H₁：载体为静止状态

零速检测器的性能由检测概率(P_D＝p(H₀|H₁))和虚警概率(P_FA＝p(H₁|H₀))决定。根据Neyman-Pearson理论，对于给定的P_FA＝α，要使得判断为H₁状态时检测概率P_D最大，有：

式中，L(q_n)为信号q_n的似然比，该式即为似然比检验(Likelihood Ratio Test)。

式中，阈值γ由下式决定：

考虑到假设H₀下信号较难描述，但H₁情况下车辆静止且比力只有重力，三轴加速度计输出应为当地重力加速度，三轴陀螺仪输出应为0，轮速传感器输出也应为0。两种假设条件下的信号约束应为：

式中，为方向为重力方向的单位矢量，/>l表示窗口期中任意的一个数据，g为重力加速度。推导得到融合轮速的零速检测器似然比检验式如下：

式中，γ'＝-2(lnγ)/N。

计算IMU及轮速传感器输出数据，若满足上式，则认为载体处于静止状态。

步骤3、通过广义似然比检验，比较广义似然比与设定的检验阈值，并输出零速检测的结果。即若零速检测不符合上式要求，零速检测输出的零速Flag为Fasle，则不运行零速修正模块，INS正常输出。若零速检测符合上式要求，零速检测输出的零速Flag为True，则运行零速修正模块。

若运行零速修正模块，则零速修正模块首先检测零速Flag信号，若零速Flag信号为True，则运行零速修正模块；若零速Flag信号为False，则停止运行零速修正模块。

零速修正模块包括以下三个步骤：

1)速度姿态位置更新：

利用零速信息，对速度和位置进行置零更新，即v＝0,[ω_x ω_y ω_z]＝[0 0 0]，式中ω_x、ω_y、ω_z分别为原始的三轴陀螺仪的角速度输出。此时由于速度和角速度输入均为0，则载体的位置和姿态会维持停止前状态，不会进行更新。此时陀螺仪噪声及加速度计噪声无法对车辆位姿造成影响。

利用零速状态下的加速度计，对车辆姿态进行重新估计。具体地：

假设加速度输出则姿态角中的俯仰角θ和翻滚角Φ可以通过加速度进行计算：

2)误差估计：利用零速时候传感器的输出，对陀螺仪的零偏进行再估计。

假设陀螺仪零偏为[b_x b_y b_z]，利用零速状态下陀螺仪的输出对陀螺仪零偏重新进行估计，具体如下式：

3)零偏、姿态修正：当车辆启动后，利用步骤1)、步骤2)估计的车辆姿态和陀螺零偏对INS误差进行修正。修正方法如下式所示：

式中，分别表示修正后的三轴陀螺仪输出角速度。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (1)

1.一种基于车辆零速检测的惯性导航误差修正方法，其特征在于，包括：

零速检测步骤：

11)车辆启动后，采集车辆INS数据及轮速传感器的轮速数据，车辆INS数据包括加速度计数据和陀螺仪数据；

12)基于采集的数据，在车辆静止时，利用零速检测方法对车辆零速进行实时检测，若检测出零速，则进入零速修正步骤；

零速修正步骤：

21)利用零速状态下的加速度计，对车辆姿态进行重新估计；

22)利用零速时各传感器的输出，对陀螺仪的零偏进行再估计；

23)当车辆启动后，利用估计的车辆姿态及陀螺仪的零偏对INS误差进行修正；

所述的零速检测步骤中，步骤12)的具体内容为：

121)基于采集的数据，构建分布参数的最大似然估计，计算所有传感器联合概率的广义似然比；

122)计算IMU及轮速传感器输出数据，若满足所有传感器联合概率的广义似然比，则认为载体处于静止状态，即检测出零速；

步骤121)具体包括以下内容：

a)定义H₀表示载体为运动状态，H₁表示载体为静止状态，零速检测器的性能由检测概率P_D＝p(H₀|H₁)和虚警概率P_FA＝p(H₁|H₀)决定，根据Neyman-Pearson理论，对于给定的P_FA＝α，使判断为H₁状态时检测概率P_D最大，需满足：

式中，L(q_n)为观测序列q_n的似然比，其中，阈值γ由下式决定：

观测序列为：

其中：

y_k为k时刻的所有传感器的观测值

y_k＝s_k+v_k

其中：

式中，分别表示三轴加速度计、三轴陀螺仪及轮速传感器真实值；/>分别表示三轴加速度计、三轴陀螺仪及轮速传感器的测量噪声

b)设定信号约束条件，即：

H₀:

H₁:

式中，为方向为重力方向的单位矢量，/>观测窗口期为n至n+N-1，n为窗口期开始时间点的数据，N为窗口期数据数量，l为窗口期中任意的一个数据，g为重力加速度；

c)结合步骤a)、b)获取所有传感器联合概率的广义似然比的表达式；

d)计算IMU及轮速传感器输出数据，若满足所有传感器联合概率的广义似然比的表达式，则认为载体处于静止状态；

所有传感器联合概率的广义似然比的表达式为：

式中，γ'＝-2(lnγ)/N，γ为阈值；其中/>为k时刻加速度计的观测向量，/>为k时刻陀螺仪的观测向量，/>为k时刻轮速传感器的观测向量；/>分别为加速度计、陀螺仪及轮速传感器的噪声方差值；

采用三轴加速度数据、三轴陀螺仪进行数据采集；

步骤21)的具体内容为：

利用零速状态下的加速度计数据，对车辆姿态角中的俯仰角θ和翻滚角Φ的估计，其表达式为：

式中，a_x、a_y、a_z为三轴加速度计输出，

步骤22)中，假设陀螺仪零偏为[b_x b_y b_z]，则利用零速时各传感器的输出，对陀螺仪的零偏进行再估计的表达式为：

式中，ω_x,ω_y,ω_z分别为原始的三轴陀螺仪的输出角速度；

步骤23)中，修正的表达式为：

式中，分别为修正后的三轴陀螺仪的输出角速度。