CN104677356B - 一种基于角增量和比力输出的划桨速度计算方法 - Google Patents

Abstract

本发明公开了一种基于角增量和比力输出的划桨速度计算方法，包括：(1)在速度算法单次更新周期(t_m‑1，t_m)内，采用N个间隔，分别对角增量陀螺仪和比力加速度计的输出采样，得到角增量输出为Δθ_i，i＝1，2，...，N；比力输出为f_j，j＝1，2，...N+1；(2)计算单次算法更新周期(t_m‑1，t_m)内机体的角增量Δθ_m和速度增量△V_m；(3)计算旋转效应补偿值ΔV_rotm；(4)计算划桨效应补偿值ΔV_sculm；(5)计算(t_m‑1，t_m)周期内比力积分的补偿值ΔV_sfm；(6)根据速度更新方程计算t_m时刻的机体速度V_m。本发明可根据角增量陀螺仪和比力角速度计的输出直接高精度的计算出机体的速度，具有方法简单、精度高的优势。

Description

一种基于角增量和比力输出的划桨速度计算方法

技术领域

本发明涉及一种对惯性传感器采用角增量和比力输出的捷联惯导系统精确计算机体速度的方法，属于捷联惯性导航技术领域。

背景技术

在捷联惯导系统的算法编排中，速度计算方法具有重要作用和影响。速度计算精度的高低，不仅直接决定了机体速度和位置的导航精度，还对下一时刻机体的姿态精度产生影响。在速度计算中，涉及到对比力矢量的积分，在这一过程中会引入不可交换性误差。因此在高精度的速度计算过程中，必须引入相应的不可交换补偿项以减少速度计算的误差。目前一般做法是借鉴姿态计算里的方法，即根据对偶性原理直接套用圆锥姿态算法中的补偿形式和补偿系数。但这种方法只适用于惯性器件，即陀螺仪和加速度计的输出为“纯增量”(角增量和速度增量，这里的速度增量指比力积分)或“纯速率”(角速率和比力)形式的捷联惯导系统。实际系统中陀螺仪和加速度计的输出具有多种形式。如激光陀螺仪是角增量输出，光纤陀螺仪是角速率输出，石英振梁式加速度计是比力输出，而积分式加速度计是速度增量输出等等。因此惯性传感器，即陀螺仪和加速度计的输出有4种组合形式：1)角增量和速度增量；2)角速率和比力；3)角增量和比力；4)角速率和速度增量。实际应用时应根据惯性器件的输出选择相应的速度计算形式。如果惯性传感器输出的物理量纲与算法输入不一致，算法的实际效果会大打折扣。因此，在诸如一个由激光陀螺仪和石英振梁式加速度计构成的高精度捷联惯导系统中，需要采用基于角增量和比力输出的速度算法计算速度。

根据捷联惯性导航原理，机体在t_m时刻的速度V_m值由四项构成：t_m-1时刻机体的速度V_m-1和方向余弦阵C_m-1，(t_m-1，t_m)周期内比力积分的补偿量ΔV_sfm和有害加速度补偿量ΔV_g/corn。计算公式如下：

其中V_m-1，C_m-1在上一更新周期的导航计算中已求出。ΔV_g/corn在单次更新周期内变化不大，可由t_m-1时刻机体的速度和位置算出。因此求解t_m时刻机体速度V_m的关键是要根据惯性传感器的角增量和比力输出求解(t_m-1，t_m)周期内比力积分的补偿值ΔV_sfm。

根据捷联惯性导航系统的速度算法编排，ΔV_sfm又由(t_m-1，t_m)周期内的速度增量ΔV_m，旋转效应补偿项ΔV_rotm和划桨效应补偿项ΔV_sculm构成：

不同速度计算方法之间最大的差异就在于对旋转效应补偿项ΔV_rotm和划桨效应补偿项ΔV_sculm的计算方法和精度有所不同。特别是后者ΔV_sculm的计算方法对速度精度的影响很大。为了衡量不同速度求解方法的性能高低，一般把划桨运动下算法的性能作为衡量标准。划桨运动是指机体一面绕纵轴做间歇周期式运动，同时又沿横轴做同频率的角振动。这时会在机体的立轴方向存在一个速度常值。该速度常值必须在速度更新中被考虑，否则会严重影响速度解算的精度。划桨运动的数学公式描述为：

式中ω是机体旋转角速率，Ω是振动角频率，b是角振动幅值。f是比力，c是比力振动幅值。对应的划桨效应补偿项真值为：

式中H是速度算法更新周期。由上式可见在划桨运动环境下，ΔV_sculm项表现为一常值速度。高精度的速度算法必须利用惯性传感器，即陀螺仪和加速度计的输出对其进行补偿。而传统的划桨补偿方法一般基于纯增量(角增量和速度增量)或纯角速率(角速率和比力)的传感器输出进行。例如，基于“纯增量型”输出的2间隔旋转效应补偿值计算方法为：

对应的划桨效应补偿值计算方法为：

而基于“纯速率型”输出的2间隔旋转效应补偿值计算方法为：

对应的划桨效应补偿值计算方法为：

上述这些传统的计算速度方法只适用于惯性传感器，即陀螺仪和加速度计分别采用角增量和速度增量输出(如式(5))，或者角速率和比力输出(如式(8))的捷联惯性导航系统。而对于采用其它输出组合的系统，无法直接应用以上两种方法。例如在一个由激光陀螺仪(角增量输出)和石英振梁式加速度计(比力输出)构成的捷联惯导系统中，过去必须先将传感器的采样值通过数字积分或微分化为纯增量或纯速率的形式，才能应用上述两种方法计算速度。而数字积分或微分将会带来额外的误差，导致最后的导航结果往往并不理想。

发明内容

为解决现有技术的不足，本发明的目的在于在传统“纯增量”和“纯速率”类型的划桨速度算法基础上，提供一种基于角增量和比力输出的划桨速度计算方法及其应用。

为了实现上述目标，本发明采用如下的技术方案：

一种基于角增量和比力输出的划桨速度计算方法，其特征在于，包括以下步骤：

(1)在速度算法单次更新周期(t_m-1，t_m)内，采用N个间隔，角增量陀螺仪输出N次，比力加速度计输出N+1次，所述的N为≥2的自然数，分别对角增量陀螺仪输出和比力加速度计输出采样，得到角增量输出为Δθ_i，i＝1，2，...，N；比力输出为f_j，j＝1，2，...N+1；

(2)计算单次算法更新周期(t_m-1，t_m)内机体的角增量Δθ_m和速度增量ΔV_m：

Δθ_m＝Δθ₁+Δθ₂+...+Δθ_N，

所述的Δθ_m是(t_m-1，t_m)周期内角增量的输出Δθ₁、Δθ₂...Δθ_N之和，

ΔV_m根据比力加速度计的输出采样f_j经过数字积分公式计算得到；

(3)计算旋转效应补偿值Δv_rotm：

(4)计算划桨效应补偿值ΔV_sculm：

所述的l＝2，...N，k_l-1为划桨优化算法系数，H为速度算法更新周期，H＝t_m-t_m-1；

(5)计算(t_m-1，t_m)周期内比力积分的补偿值ΔV_sfm：

ΔV_sfm＝ΔV_m+ΔV_rotm+ΔV_sculm；

(6)根据速度更新方程计算t_m时刻的机体速度V_m：

所述的，V_m-1为t_m-1时刻机体速度，C_m-1为t_m-1时刻方向余弦阵，g为重力加速度，ω_ie为地理系对惯性系的角速率，ω_en为导航系对地理系的角速率。

进一步，所述的划桨优化算法系数k_l-1的计算方法如下：

所述的角增量陀螺仪输出Δθ_i和比力加速度计输出f_j满足如下方程式：

所述的b为角振动幅值，Ω为振动角频率，c为比力振动幅值，且I代表x轴方向，J代表y轴方向；

根据对偶性原理，将Δθ_i和f_j的方程式代入简化后得到

所述的旋转效应补偿计算值和旋转效应补偿真值相等，

并分别将和进行泰勒展开，合并“ΩH”同类项可得：

A_(N-1)×(N-1)K_(N-1)×1＝D_(N-1)×1，

K为划桨优化算法系数k_l-1构成的列向量，且

而所述的

且所述的N为划桨算法的间隔数；

根据K＝A_-1D，得到N个间隔下的划桨优化算法系数k_l-1为：

更进一步，所述的数字积分公式为矩形积分公式、梯形积分公式、辛普生积分公式、牛顿-柯特斯积分公式中的任一种。

本发明的有益之处在于：本发明可根据角增量陀螺仪和比力角速度计的输出直接高精度的计算出机体的速度，具有方法简单、精度高的优势，适用于采用角增量(如激光陀螺仪)和比力(石英振梁式加速度计)输出的捷联惯性导航系统。

附图说明

图1为本发明所述的速度计算方法与传统速度求解方法的计算流程差异对比；

图2为划桨运动下(仿真)东向速度误差对比；

图3为划桨运动下(仿真)北向速度误差对比；

图4为跑车实验东向速度误差对比；

图5为跑车实验北向速度误差对比。

具体实施方式

以下结合附图和具体实施例对本发明作具体的介绍。

下面以单次更新周期内采用2间隔(N＝2)计算速度的系统为例，一个2间隔的速度更新系统中，(t_m-1，t_m)时间段内角增量陀螺仪的输出采样Δθ_i只有2次，分别发生在t_m-1/2和t_m时刻，对应(t_m-1，t_m-1/2)，(t_m-1/2，t_m)时间段内机体的角增量，而比力加速度计的输出采样f_j有3次，分别发生在t_m-1，t_m-1/2和t_m时刻。

从速度更新方程可知，计算机体速度的关键是计算比力积分补偿项ΔV_sfm。而ΔV_sfm又由速度增量ΔV_m，旋转效应补偿项ΔV_rotm和划桨效应补偿项ΔV_sculm构成。理论值为：

式中Δθ_m是(t_m-1，t_m)周期内角增量陀螺的输出Δθ₁、Δθ₂...Δθ_N之和：

Δθ_m＝Δθ₁+Δθ₂+...+Δθ_N (10)。

单次更新周期内采用2间隔(N＝2)计算速度的具体计算步骤为：

(1)在速度算法单次更新周期(t_m-1，t_m)内，采用2个间隔，角增量陀螺仪输出2次，比力加速度计输出3次，分别对角增量陀螺仪和比力加速度计的输出采样，得到角增量输出为Δθ_i，i＝1，2，即为Δθ₁，Δθ₂；比力输出为f_j，j＝1，2，3，即为f₁，f₂，f₃；

(2)计算单次算法更新周期(t_m-1，t_m)内机体的角增量Δθ_m和速度增量ΔV_m：

Δθ_m＝Δθ₁+Δθ₂，所述的Δθ_m是(t_m-1，t_m)周期内角增量的输出Δθ₁，Δθ₂之和，

ΔV_m根据比力加速度计的输出采样f_j经过数字积分公式中的辛普生积分公式计算得到(当然还可以利用矩形积分公式、梯形积分公式、或牛顿-柯特斯积分公式计算)：

H为速度算法更新周期，即H＝t_m-t_m-1；

(3)计算旋转效应补偿值ΔV_rotm：

(4)计算划桨效应补偿值ΔV_sculm：

ΔV_sculm＝k_l-1(Δθ₁×f₂-Δθ₂×f₁)H，k_l-1为划桨优化算法系数；

而k_l-1的计算步骤为：

所述惯性传感器的角增量输出Δθ_i和比力输出f_j满足如下方程式：

所述的b为角振动幅值，Ω为振动角频率，c为比力振动幅值，且I代表x轴方向，J代表y轴方向；

根据对偶性原理，将Δθ_i和f_j的方程式代入

简化后得到

所述的(式15)和(式4)相等，并分别将式(15)和式(4)进行泰勒展开，合并“ΩH”同类项可得：

A_(N-1)×(N-1)K_(N-1)×1＝D_(N-1)×l (16)，

K为划桨优化算法系数k_l-1构成的列向量，N为划桨算法的间隔数，且：

根据式(16)的解K＝A^-1D，即可得到N个间隔下的划桨优化算法系数k_l-1为：

，当N＝2时，因此：

(5)计算(t_m-1，t_m)周期内比力积分的补偿值ΔV_sfm：

ΔV_sfm＝ΔV_m+ΔV_rotm+ΔV_sculm；

(6)根据速度更新方程计算t_m时刻的机体速度V_m：

式(19)中，Δθ_m由式(10)计算，ΔV_m根据比力加速度计的输出采样f_j(f₁...f_N+1)通过数字积分计算，k_l-1根据式(17)和K＝A^-1D计算。对于2间隔的捷联导航系统(N＝2)，将式(12)、(18)代入上式可得到完整的机体t_m时刻速度计算方法

式(20)中，V_m-1为t_m-1时刻机体速度，C_m-1为t_m-1时刻方向余弦阵。V_m-1和C_m-1均已在上一更新周期求出。g为重力加速度，采用固定公式计算，ω_ie为地理系对惯性系的角速率，ω_en为导航系对地理系的角速率ω_ie和ω_en的计算公式与传统方法一样，例如：可在速度更新之前的姿态更新里求出。

当速度更新采用的是N(N≠2)间隔时，发明实施的具体步骤同上。

图1为本发明所述的速度计算方法与传统速度求解方法的计算流程差异对比。

如图1所示：相比传统方法，本发明的速度求解方法在传感器采用角增量和比力输出的捷联惯导系统中具有优良的精度。为了说明这一点，分别通过数字仿真和实际跑车实验结果来说明发明方法的优良性能。

图2为划桨运动下(仿真)东向速度误差对比；图3为划桨运动下(仿真)北向速度误差对比。

如图2和图3所示：在典型划桨运动环境下，本发明所述的方法与传统“纯增量型”速度求解方法的导航结果对比：

式(21)中，式中J、K分别代表y和z轴方向，由不可交换误差引起的划桨误差出现在x轴(也即东向速度方向)，其中划桨运动角振动振幅b＝1°，比力振幅c＝10g，振动频率Ω＝2πrad/s。采用东-北-天导航坐标系，x轴-右翼，y轴-机头，z轴-天向机体系。初始姿态角为：[0°，0°，0°]，初始速度都为0m/s，初始位置为：[118.78333°，32.05°，10m]。算法更新周期H＝0.01s，仿真时长600s。图中虚线是传统“纯增量型”速度求解方法的速度误差曲线，实线是本发明所述的方法的速度误差曲线。

采用本发明所述的新方法解算的东向速度和北向速度精度，比采用传统“纯增量型”速度算法的精度提高了一个数量级以上。

图4为跑车实验东向速度误差对比；图5为跑车实验北向速度误差对比。

如图4和图5所示：实验先在校综合楼前地面静止20分钟进行预热，关闭发动机进行系统初始对准，对准完成后车辆出发，系统进入导航状态。车辆先向东再向北，绕御园后从西侧回到综合楼门口，再继续向东行驶...如此循环4圈，最后在综合楼门口停止。采集的实验数据长度为1000s。实验采用高精度的惯性级激光陀螺(角增量输出，零偏0.01°/hr)和比力输出型加表(零偏3*10^-5g)。器件的数据输出率为200Hz，2间隔算法的更新周期H为0.01s。导航系采用东-北-天坐标系。以INS/GPS组合导航的输出作为真值。将新的基于角增量/比力的划桨优化算法速度解算结果，与采用传统“纯增量型”划桨算法的速度解算结果对比。图中虚线是传统“纯增量型”速度求解方法的速度误差曲线，实线是发明方法的速度误差曲线。

在惯性器件输出为角增量和比力的环境下，采用新的基于角增量/比力的划桨速度优化算法进行速度解算，其结果明显比基于传统“纯增量型”划桨速度算法的速度解算结果精度高。而且速度误差的抖动比传统“纯增量型”速度算法小。这是因为后者需先将比力输出f经数字积分变为速度增量ΔV后才能进行速度求解，而在数字积分的过程中引入了额外的计算误差所致。

综上所述：在惯性器件输出为角增量和比力的环境下，采用基于角增量/比力的划桨速度优化算法进行速度解算，其精度比基于传统“纯增量型”划桨速度优化算法的导航结果高。这是因为后者需先进行单位转换，将比力通过数字积分变为速度增量才能进行速度求解。而在这一过程中引入了额外误差所致。而发明的速度求解方法无需把惯性传感器的输出进行转换，从而大大提高了导航精度。因此，发明的速度求解方法非常适合应用于惯性传感器采用角增量和比力输出的捷联惯性导航系统。

以上显示和描述了本发明的基本原理、主要特征和优点。本行业的技术人员应该了解，上述实施例不以任何形式限制本发明，凡采用等同替换或等效变换的方式所获得的技术方案，均落在本发明的保护范围内。

Claims (3)

1.一种基于角增量和比力输出的划桨速度计算方法，其特征在于，包括以下步骤：

(1)在速度算法单次更新周期(t_m-1，t_m)内，采用N个间隔，角增量陀螺仪输出N次，比力加速度计输出N+1次，所述的N为≥2的自然数，分别对角增量陀螺仪和比力加速度计输出采样，得到角增量输出为Δθ_i，i＝1，2，...，N；比力输出为f_j，j＝1，2，...N+1；

(2)计算单次算法更新周期(t_m-1，t_m)内机体的角增量Δθ_m和速度增量ΔV_m：

Δθ_m＝Δθ₁+Δθ₂+...+Δθ_N，

所述的Δθ_m是(t_m-1，t_m)周期内角增量的输出Δθ₁、Δθ₂...Δθ_N之和，

ΔV_m根据比力加速度计的输出采样f_j经过数字积分公式计算得到；

(3)计算旋转效应补偿值ΔV_rotm：

${\mathrm{\ΔV}}_{rotm}=\frac{1}{2}{\mathrm{\Δ\θ}}_m\×{\mathrm{\ΔV}}_m;$

(4)计算划桨效应补偿值ΔV_sculm：

所述的l＝2，...N，k_l-1为划桨优化算法系数，H为速度算法更新周期，H＝t_m-t_m-1；

(5)计算(t_m-1，t_m)周期内比力积分的补偿值ΔV_sfm：

ΔV_sfm＝ΔV_m+ΔV_rotm+ΔV_sculm；

(6)根据速度更新方程计算t_m时刻的机体速度V_m：

$\begin{array}{c}{V}_m=V_{m-1}+C_{m-1}{\mathrm{\ΔV}}_{sfm}+{\mathrm{\ΔV}}_{g/corn}\\ =V_{m-1}+C_{m-1}\[{\mathrm{\ΔV}}_m+\mathrm{\Δ}{\hat{V}}_{rotm}+\mathrm{\Δ}{\hat{V}}_{sculm}\]+{\mathrm{\ΔV}}_{g/corn}\\ =V_{m-1}+C_{m-1}\[{\mathrm{\ΔV}}_m+\frac{1}{2}{\mathrm{\Δ\θ}}_m\×{\mathrm{\ΔV}}_m+\underset{l=2}{\overset{N}{\mathrm{\Σ}}}{k}_{l-1}\left({\mathrm{\Δ\θ}}_1\×f_l-{\mathrm{\Δ\θ}}_l\×f_1\right)H\]+\[g-\left(2{\mathrm{\ω}}_{ie}+{\mathrm{\ω}}_{en}\right)\×V_{m-1}\]H\end{array}$

所述的，V_m-1为t_m-1时刻机体速度，C_m-1为t_m-1时刻方向余弦阵，g为重力加速度，ω_ie为地理系对惯性系的角速率，ω_en为导航系对地理系的角速率，ΔV_g/corn为在(t_m-1，t_m)时间段内由有害加速度引起的速度补偿量。

2.根据权利要求1所述的一种基于角增量和比力输出的划桨速度计算方法，其特征在于，所述的划桨优化算法系数k_l-1的计算方法如下：

在经典划桨运动下，所述的惯性传感器的角增量输出Δθ_i和比力输出f_j满足如下方程式：

$\left\{\begin{array}{c}{\mathrm{\Δ\θ}}_i=2bsin\frac{\mathrm{\Ω}H}{4}cos\mathrm{\Ω}(t_{m-1}+\frac{2i-1}{2N}H)I,i=1,2,...,N\\ f_j=csin\[\mathrm{\Ω}(t_{m-1}+\frac{j-1}{N}H)\]J,j=1,2,...N+1\end{array}\right.,$

所述的b为划桨运动的角振动幅值，Ω为振动角频率，c为比力振动幅值，且I代表x轴方向，J代表y轴方向；

根据对偶性原理，将Δθ_i和f_j的方程式代入简化后得到

所述的划桨效应计算值与真值相等，

并分别将和进行泰勒展开，合并“ΩH”同类项可得：

A_(N-1)×(N-1)K_(N-1)×1＝D_(N-1)×1，

K为划桨优化算法系数k_l-1构成的列向量，且

而所述的

且所述的N为速度算法单次更新周期内的间隔数；

根据K＝A^-1D，得到N个间隔下的划桨优化算法系数k_l-1为：

3.根据权利要求1所述的一种基于角增量和比力输出的划桨速度计算方法，其特征在于，所述的数字积分公式为矩形积分公式、梯形积分公式、辛普生积分公式、牛顿-柯特斯积分公式中的任一种。