CN114964230B - 一种车载组合导航陀螺漂移修正方法 - Google Patents

Abstract

本发明提供了一种车载组合导航陀螺漂移修正方法，首先判断车体运动状态；若车体处于静止状态，判断车体持续静止时间大于第一阈值后，计算陀螺输出角速率均值作为静态陀螺漂移修正值，进行实时补偿；若车体处于运动状态，进行卡尔曼滤波估计确定动态陀螺漂移估计值；判断动态陀螺漂移估计值满足收敛条件后，采用限幅修正方法计算动态陀螺漂移修正值；若陀螺已采用静态陀螺漂移补偿处理，则在静态陀螺漂移补偿基础上进行动态陀螺漂移补偿，否则在陀螺敏感角速率基础上进行动态陀螺漂移补偿，记录修正时刻。本发明根据车体不同运动状态，采用双通道陀螺漂移估计方法，实时准确估计、修正陀螺漂移，完成陀螺角速率补偿，实现高精度导航定位的目的。

Description

一种车载组合导航陀螺漂移修正方法

技术领域

本发明属于组合导航技术领域，具体涉及一种车载组合导航陀螺漂移修正方法，可提高导航系统的定位、定姿精度。

背景技术

随着传感器技术以及融合算法的发展，多传感器组合导航方法被车载导航系统广泛应用。其中，惯性导航拥有完全自主、不受外界环境干扰的特殊优势，被各种融合导航定位方法亲赖，但其惯性器件存在误差，导致导航误差随时间积累，是影响导航精度的重要误差源。

陀螺、加速度计是惯性导航核心传感器，其中，陀螺可以敏感角度变化，是系统定姿的关键部件。加速度计可以敏感系统加速度，通过角度关系投影，再积分，可以得到系统位置信息。由于陀螺漂移在误差方程中可观性较差，通过滤波估计较难准确估计并进行补偿修正。因此陀螺是影响导航定位、定姿精度的关键器件，陀螺漂移误差是导航误差积累的关键原因。

一般情况下，陀螺漂移误差会通过惯导标定进行补偿。但是陀螺漂移误差是随机误差，且随上电时间、温度变化等随机变化，惯导标定无法获得精准的陀螺漂移误差，因此需要实时估计修正。并且载体运动状态也会影响陀螺漂移估计精度，在载体运动时陀螺漂移可观性较强，可以通过卡尔曼滤波实时估计，在载体静止时，陀螺漂移可观性差，很难通过卡尔曼滤波实时估计，但其精度会影响航向测量精度，尤其在卫星不定位的情况下，会使系统定位误差发散，精度大大降低。所以需要根据载体的不同运动状态，充分利用各种传感器信息，对陀螺漂移误差进行估计、修正，补偿陀螺角速率信息，进一步提升导航精度。

发明内容

针对现有技术中陀螺漂移误差严重影响导航测量精度、较难通过卡尔曼滤波实时准确估计的技术问题，本发明提供了一种车载组合导航陀螺漂移修正方法，根据车体不同运动状态，采用双通道陀螺漂移估计方法，实时准确估计、修正陀螺漂移，进而进行陀螺角速率补偿，实现高精度导航定位的目的。

本发明解决上述技术问题采用的技术方案如下：

一种车载组合导航陀螺漂移修正方法，包括如下步骤

判断车体运动状态；

若车体处于静止状态，判断车体持续静止时间大于第一阈值后，计算陀螺输出角速率均值作为静态陀螺漂移修正值，对陀螺角速率进行实时补偿；

若车体处于运动状态，进行卡尔曼滤波估计，确定动态陀螺漂移估计值；判断动态陀螺漂移估计值满足收敛条件后，采用限幅修正方法计算动态陀螺漂移修正值；若陀螺已采用静态陀螺漂移补偿处理，则在静态陀螺漂移补偿基础上进行动态陀螺漂移补偿，否则在陀螺敏感角速率基础上进行动态陀螺漂移补偿，记录修正时刻。

进一步地，所述静态陀螺漂移修正值计算方法如下

计算陀螺输出角速率均值

静态陀螺开展实时补偿，获得的陀螺角速率为

其中，为x、y、z轴陀螺敏感的角速率信息，t_s为第一阈值，f为惯性器件的采样频率。

进一步地，所述判断动态陀螺漂移估计值满足收敛条件的方法如下

滑窗计算Δt_f时间内各轴动态陀螺漂移估计值的均方差：

其中，Δt_f为滑窗计算均方差值的窗口时间，T_f为滤波周期，(X₁₀)_k、(X₁₁)_k、(X₁₂)_k分别为Δt_f窗口时间内第k个滤波周期陀螺漂移ε_x、ε_y、ε_z的估计值；

判断陀螺是否同时满足如下收敛条件：

陀螺漂移对应的协方差阵对角元素小于P_MAX；

陀螺距离上一次修正时刻大于Δt_correct。

进一步地，Δt_f≥50T_f，Δt_f为T_f的整数倍；0＜P_MAX≤(3600ε_g)²，ε_g为陀螺标称的最大的常值漂移值；Δt_correct≥10Δt_f。

进一步地，所述采用限幅修正方法计算动态陀螺漂移修正值的方法如下

其中，th为限幅修正的阈值，X₁₀、X₁₁、X₁₂分别为陀螺漂移ε_x、ε_y、ε_z的估计值；

若已采用静态陀螺漂移补偿处理，则陀螺角速率计算方法如下

若没有采用静态陀螺漂移补偿处理，则陀螺角速率计算方法如下：

其中，分别表示x、y、z轴陀螺经过静态陀螺漂移补偿后的值；为x、y、z轴陀螺敏感的角速率信息。

进一步地，th≥2ε_g，ε_g为陀螺标称的最大的常值漂移值。

进一步地，所述车体运动状态判断方法如下

计算每个采样时间陀螺和加速度计的输出值的合成量，通过滑窗计算Δt时间内陀螺和加速度计合成量的均值

通过滑窗计算T时间内 的滑动均值/>和滑动均方差值

若满足以下条件，则载体处于静止状态，否则为运动状态，

其中，g为当地重力加速度；ω_av0为静态角速率均值阈值；ω_sd0为静态角速率均方差阈值；f_av0为静态加速度均值阈值；f_sd0为静态加速度均方差阈值。

进一步地，所述均值的计算方法如下：

其中，Δt为合成采样均值的滑动窗口时间，f为惯性器件的采样频率；分别表示第k个采样时刻x、y、z轴陀螺敏感的角速率；分别表示第k个采样时刻x、y、z轴加速度计敏感的加速度；

所述的滑动均值/>和滑动均方差值/>的计算方法如下

其中，j表示第j个合成采样均值；T为均值滑动窗口时间。

进一步地，Δt≥1s，T≥10Δt，T是Δt的整数倍；所述第一阈值t_s≥10T；ω_av0≤0.5°/s，ω_sd0≤0.03°/s，f_av0≤0.02m/s²，f_sd0≤0.006m/s²。

进一步地，在每次判断出车体处于静止状态且持续静止时间大于第一阈值后，陀螺输出角速率均值计算只进行一次，采用该角速率均值对静态陀螺输出值进行实时修正。

本发明与现有技术相比的有益效果：

本发明提供了一种车载组合导航陀螺漂移修正方法，根据车体不同运动状态，采用双通道陀螺漂移估计方法，实时准确估计、修正陀螺漂移，进而进行陀螺角速率补偿，实现高精度导航定位的目的。

(1)在车体刚刚启动和即将停止阶段，容易出现运动状态误判。本发明车体运动状态判断方法，采用采样均值和滑动均值、滑动均方差值的双重判断，有效减小误判率；同时，该方法仅需要依靠惯性器件即可完成判断，不需要依赖其他传感器，自主性更强。

(2)本发明采用陀螺输出角速率均值作为陀螺静态漂移误差，进行静态陀螺漂移修正，该方法计算量小，可快速准确获得修正值。

(3)本发明采用卡尔曼滤波计算进行动态陀螺漂移修正，分析陀螺数据收敛性，采用限幅修正方法，保证修正准确性、避免误修正。

具体实施方式

下面对本发明的具体实施例进行详细说明。在下面的描述中，出于解释而非限制性的目的，阐述了具体细节，以帮助全面地理解本发明。然而，对本领域技术人员来说显而易见的是，也可以在脱离了这些具体细节的其它实施例中实践本发明。

本发明以车载组合导航为背景，提出一种陀螺漂移修正方法，针对陀螺漂移难估计严重影响导航精度的问题，根据载体车不同运动状态，采用双通道陀螺漂移估计方法，进行实时修正，提高车载导航定位、定姿精度。

本发明提出的一种车载组合导航陀螺漂移修正方法，首先通过惯性器件输出的角速率和加速度信息判断车体运动状态。当判断确认车体已经连续静止一段时间(设定时间段为t_s)后，计算陀螺输出角速率均值，作为陀螺静态漂移误差，进行静态陀螺漂移修正，进而对陀螺角速率进行实时补偿，更新陀螺角增量。当判断车体为运动状态，则根据卡尔曼滤波估计结果，结合误差收敛条件，进行动态陀螺漂移修正，进而实时补偿陀螺角速率，更新陀螺角增量，提高导航测量精度。静态陀螺漂移和动态陀螺漂移的估计与修正，相互独立。

本发明提出的一种车载组合导航陀螺漂移修正方法，具体的计算方法如下：

S1、车体运动状态判断

S11、计算每个采样时间陀螺和加速度计的输出值的合成量，在连续时间Δt内对合成量进行滑窗求均值计算，其中，Δt为合成采样均值的滑动窗口时间，可根据不同应用条件进行设置。此处的滑窗当量为采样周期。

陀螺及加速度计的输出值的合成量进行求均值计算方法如下：

其中，为在窗口时间Δt内三轴陀螺合成量均值，单位：rad/s；/>在窗口时间Δt内三轴加速度计合成量均值；Δt为合成采样均值的滑动窗口时间，单位秒(s)，一般为陀螺采样周期的整数倍，优选的Δt≥1s；f为惯性器件的采样频率，单位赫兹(Hz)；分别表示第k个采样时刻x、y、z轴陀螺敏感的角速率，单位：rad/s；/>分别表示第k个采样时刻x、y、z轴加速度计敏感的加速度，单位m/s²。

S12、滑动窗口计算T时间段内及/>的滑动均值及滑动均方差值，此处的滑窗当量为Δt。

1)滑动窗口计算T时间段内的滑动均值，记作/>

2)滑动窗口计算T时间段内的滑动均值，记作/>

3)滑动窗口计算T时间段内的滑动均方差值，记作/>

4)滑动窗口计算T时间段内的滑动均方差值，记作/>

其中，j表示第j个合成采样均值；T为均值滑动窗口时间，单位为秒(s)，优选的T≥10Δt，且T是Δt的整数倍。

S13、若满足以下条件，则车体处于静止状态，否则为运动状态。

其中，g为当地重力加速度，单位:m/s²。ω_av0为静态角速率均值阈值，可根据系统陀螺指标设定，优选的ω_av0≤0.5°/s；ω_sd0为静态角速率均方差阈值，可根据系统陀螺指标设定，优选的ω_sd0≤0.03°/s；f_av0为静态加速度均值阈值，可根据系统加速度计指标设定，优选的f_av0≤0.02m/s²；f_sd0为静态加速度均方差阈值，可根据系统加速度计指标设定，优选的f_sd0≤0.006m/s²。

在车体刚刚启动和即将停止阶段，容易出现运动状态误判的问题。该车体运动状态判断方法，采用采样均值和滑动均值、滑动均方差值的双重判断，有效减小误判率。同时，该方法仅需要依靠惯性器件即可完成判断，不需要依赖其他传感器，自主性更强。

S2、静态陀螺漂移修正

S21、以车体持续静止时间大于第一阈值为静态陀螺漂移估计计算启动条件。

系统记录车体持续静止时间，若持续静止时间大于等于t_s进行静态陀螺漂移估计计算，优选的t_s≥10T；否则，不进行静态陀螺漂移估计计算，直到满足静态陀螺漂移估计计算条件为止。

静态陀螺漂移估计计算只进行一次，在静态陀螺漂移估计的修正值确定后，对陀螺数据实时补偿该静态陀螺漂移修正值。

S22、计算陀螺输出角速率均值，即为x、y、z轴的静态陀螺漂移修正值如下：

S23、静态陀螺进行实时补偿，获得的陀螺角速率为：

其中，为x、y、z轴陀螺敏感的角速率信息，/> 为经过静态陀螺漂移修正补偿后的x、y、z轴陀螺角速率。

S3、动态陀螺漂移修正

若车体处于运动状态，可在静态陀螺漂移修正补偿的同时，同步进行动态陀螺漂移估计，提高计算精度和效率。

S31、进行卡尔曼滤波估计，确定动态陀螺漂移估计值。

以惯导和卫星组合导航为例，进行说明。

建立卡尔曼滤波状态量：

建立卡尔曼滤波量测量：

其中，δV_N,δV_U,δV_E：惯导北向、天向、东向速度误差；

δL,δH,δλ：惯导纬度、高度、经度误差；

φ_N,φ_U,φ_E：惯导北向、天向、东向失准角；

ε_x,ε_y,ε_z：陀螺常值漂移；

加速度计零位误差；

V_InsN、V_InsU、V_InsE：惯导输出导航坐标系速度；

V_GpsN、V_GpsU、V_GpsE：卫星接收机输出速度；

卫星接收机与惯导系统杆臂速度误差在导航坐标系下北天东向的投影；

L_Ins、H_Ins、λ_Ins：惯导输出的纬度、高度、经度值；

L_Gps、H_Gps、λ_Gps：卫星输出的纬度、高度、经度值；

以接收机为中心，接收机到惯导质心的杆臂在导航坐标系下的投影，单位：m；

ψ_Gps：卫星输出的参考航向，单位：rad；

ψ_Ins：惯导系统导航解算得到的航向，单位：rad；

基于状态量和量测量，建立状态方程和量测方程，进行卡尔曼滤波估计，获得陀螺漂移的估计值。

滤波周期为卫星接收机输出数据周期，记为T_f。

S32、实时滑窗计算Δt_f时间内各轴动态陀螺漂移估计值的均方差，计算方法如下：

其中，Δt_f为滑窗计算均方差值的窗口时间，滑窗当量为滤波周期T_f，优选的Δt_f≥50T_f，Δt_f为T_f的整数倍。(X₁₀)_k、(X₁₁)_k、(X₁₂)_k分别代表Δt_f窗口时间内第k个滤波周期陀螺漂移ε_x、ε_y、ε_z的估计值。

S33、判断动态陀螺漂移估计值是否满足收敛条件。

当滤波估计值满足以下收敛条件时，进行动态陀螺漂移修正：

a.第一收敛条件为陀螺漂移对应的协方差阵对角元素小于P_MAX，单位：(°/s⁾²，保证动态陀螺漂移估计值的收敛性。优选的，0＜P_MAX≤(3600ε_g)²，其中ε_g为陀螺标称的最大的常值漂移值，单位(°/h)。

b.第二收敛条件为其中，i＝x,y,z，分别表示x、y、z三轴陀螺，保证动态陀螺漂移估计值的稳定性。优选的，/>

c.第三收敛条件为距离上一次修正时刻大于Δt_correct，保证动态陀螺漂移修正的稳定性。Δt_correct根据应用环境进行设定，优选的，Δt_correct≥10Δt_f。

S34、进行动态陀螺漂移修正，记录修正时刻。

采用限幅修正的方法，可以有效避免误估计的问题，保证陀螺漂移修正精度。采用限幅修正计算动态陀螺漂移修正值：

其中，th为限幅修正的阈值，优选的th≥2ε_g；ε_dx、ε_dy、ε_dz分别表示x、y、z轴动态陀螺漂移修正值；X₁₀、X₁₁、X₁₂分别为陀螺漂移ε_x、ε_y、ε_z的估计值。

进行动态陀螺误差补偿，若已采用静态陀螺漂移补偿处理，则陀螺角速率计算方法如下：

若没有采用静态陀螺漂移补偿处理，则陀螺角速率计算方法如下：

其中，分别表示x、y、z轴陀螺经过静态陀螺漂移修正补偿后的值；/>为x、y、z轴陀螺敏感的角速率信息。

如上针对一种实施例描述和/或示出的特征可以以相同或类似的方式在一个或更多个其它实施例中使用，和/或与其它实施例中的特征相结合或替代其它实施例中的特征使用。

应该强调，术语“包括/包含”在本文使用时指特征、整件、步骤或组件的存在，但并不排除一个或更多个其它特征、整件、步骤、组件或其组合的存在或附加。

这些实施例的许多特征和优点根据该详细描述是清楚的，因此所附权利要求旨在覆盖这些实施例的落入其真实精神和范围内的所有这些特征和优点。此外，由于本领域的技术人员容易想到很多修改和改变，因此不是要将本发明的实施例限于所例示和描述的精确结构和操作，而是可以涵盖落入其范围内的所有合适修改和等同物。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

本发明未详细说明部分为本领域技术人员公知技术。

Claims (7)

1.一种车载组合导航陀螺漂移修正方法，其特征在于，包括如下步骤

判断车体运动状态；

若车体处于静止状态，判断车体持续静止时间大于第一阈值后，计算陀螺输出角速率均值作为静态陀螺漂移修正值，对陀螺角速率进行实时补偿；

若车体处于运动状态，进行卡尔曼滤波估计，确定动态陀螺漂移估计值；判断动态陀螺漂移估计值满足收敛条件后，采用限幅修正方法计算动态陀螺漂移修正值；若陀螺已采用静态陀螺漂移补偿处理，则在静态陀螺漂移补偿基础上进行动态陀螺漂移补偿，否则在陀螺敏感角速率基础上进行动态陀螺漂移补偿，记录修正时刻；

所述判断动态陀螺漂移估计值满足收敛条件的方法如下

滑窗计算Δt_f时间内各轴动态陀螺漂移估计值的均方差：

其中，Δt_f为滑窗计算均方差值的窗口时间，T_f为滤波周期，(X₁₀)_k、(X₁₁)_k、(X₁₂)_k分别为Δt_f窗口时间内第k个滤波周期陀螺漂移ε_x、ε_y、ε_z的估计值；

判断陀螺是否同时满足如下收敛条件：

陀螺漂移对应的协方差阵对角元素小于P_MAX；

陀螺距离上一次修正时刻大于Δt_correct；

所述采用限幅修正方法计算动态陀螺漂移修正值的方法如下

其中，th为限幅修正的阈值，X₁₀、X₁₁、X₁₂分别为陀螺漂移ε_x、ε_y、ε_z的估计值，ε_g为陀螺标称的最大的常值漂移值；

其中，Δt_f≥50T_f，Δt_f为T_f的整数倍；0＜P_MAX≤(3600ε_g)²，Δt_correct≥10Δt_f；th≥2ε_g。

2.根据权利要求1所述的车载组合导航陀螺漂移修正方法，其特征在于，所述静态陀螺漂移修正值计算方法如下

计算陀螺输出角速率均值

静态陀螺开展实时补偿，获得的陀螺角速率为

其中，为x、y、z轴陀螺敏感的角速率信息，t_s为第一阈值，f为惯性器件的采样频率。

3.根据权利要求1所述的车载组合导航陀螺漂移修正方法，其特征在于，若已采用静态陀螺漂移补偿处理，则陀螺角速率计算方法如下

若没有采用静态陀螺漂移补偿处理，则陀螺角速率计算方法如下：

其中，分别表示x、y、z轴陀螺经过静态陀螺漂移补偿后的值；为x、y、z轴陀螺敏感的角速率信息。

4.根据权利要求3所述的车载组合导航陀螺漂移修正方法，其特征在于，所述车体运动状态判断方法如下

计算每个采样时间陀螺和加速度计的输出值的合成量，通过滑窗计算Δt时间内陀螺和加速度计合成量的均值

通过滑窗计算T时间内的滑动均值/>和滑动均方差值

若满足以下条件，则载体处于静止状态，否则为运动状态，

其中，g为当地重力加速度；ω_av0为静态角速率均值阈值；ω_sd0为静态角速率均方差阈值；f_av0为静态加速度均值阈值；f_sd0为静态加速度均方差阈值。

5.根据权利要求4所述的车载组合导航陀螺漂移修正方法，其特征在于，所述均值的计算方法如下：

其中，Δt为合成采样均值的滑动窗口时间，f为惯性器件的采样频率；分别表示第k个采样时刻x、y、z轴陀螺敏感的角速率；分别表示第k个采样时刻x、y、z轴加速度计敏感的加速度；

所述的滑动均值/>和滑动均方差值/>的计算方法如下

其中，j表示第j个合成采样均值；T为均值滑动窗口时间。

6.根据权利要求5所述的车载组合导航陀螺漂移修正方法，其特征在于，Δt≥1s，T≥10Δt，T是Δt的整数倍；所述第一阈值t_s≥10T；ω_av0≤0.5°/s，ω_sd0≤0.03°/s，f_av0≤0.02m/s²，f_sd0≤0.006m/s²。

7.根据权利要求1所述的车载组合导航陀螺漂移修正方法，其特征在于，在每次判断出车体处于静止状态且持续静止时间大于第一阈值后，陀螺输出角速率均值计算只进行一次，采用该角速率均值对静态陀螺输出值进行实时修正。