采用奇异摄动方法进行圆锥误差与划船误差补偿的捷联惯导系统

摘要

本发明公开了一种采用奇异摄动方法进行圆锥误差与划船误差补偿的捷联惯导系统，该捷联惯导系统是在惯性测量单元与姿态计算单元之间嵌入圆锥误差补偿奇异摄动模型；在惯性测量单元与导航计算单元之间嵌入划船误差补偿奇异摄动模型。本发明的补偿是在一个采样周期T内载体运动的变化是一种摄动过程，利用惯性元件在起、始采样点的实际输出值建立强迫奇异摄动模型。根据强迫奇异摄动模型对圆锥误差、划船误差进行补偿，获得采样周期内姿态更新旋转矢量以及速度更新量，通过调节可变参数逼近准确的姿态更新量和速度更新量，从而达到提高捷联惯性导航计算单元算法精度的要求。

说明书

技术领域

本发明涉及一种对高动态环境(6～2000Hz)下的捷联惯性导航测量精度的补偿处理方法，更特别地说，是指一种采用奇异摄动方法进行圆锥误差与划船误差补偿的捷联惯导系统。

背景技术

捷联惯导系统一般包括有(1)惯性组件；(2)惯性组件信号处理单元；(3)姿态计算单元；(4)导航计算单元。惯性组件由三个陀螺和三个加速度计组成。惯性组件和惯性组件信号处理单元一起被安装在一个六面形的台体内，组成惯性测量单元。安装在台体内的目的是保证惯性组件的三个测量轴相互正交。惯性测量单元通过减震器安装在载体上，用于部分消除载体振动对惯性测量单元的影响。惯性组件信号处理单元主要完成模/数转换和对信号的滤波处理；姿态计算单元用于计算载体的姿态；导航计算单元用于计算载体的速度和位置。

奇异摄动理论起源于二十世纪初prandtl对流体力学绕流问题的研究。奇异摄动理论是把高阶含快变现象的奇异摄动问题，化成低阶的慢变(忽略快变部分)的问题来求解，然后通过在不同时间尺度内引入边界层修正得到具有任意精度的解析解。奇异摄动理论适用于现实生活中普遍存在的包含快慢现象的线性、非线性奇异摄动问题以及常、偏微分方程描述的定解问题等的分析和求解。

Savage，P.G.Strapdown Inertial Navigation Integration AlgorithmDesign Part 1：Attitude Algorithms[J].Journal of Guidance，Control，andDynamics，1998，21(1)：19-28中公开了一种等效旋转矢量姿态更新算法。在本发明的以下说明中简称为文献一。

Savage，P.G.Strapdown Inertial Navigation Integration AlgorithmDesign Part 2：Velocity and Position Algorithms[J].Journal of Guidance，Control，and Dynamics，1998，21(2)：208-220中公开了一种速度更新算法。在本发明的以下说明中简称为文献二。

陈哲所著的《捷联惯导系统原理》，于1986年12第一次出版。参见P145页到P148页中公开了一种解析得到捷联矩阵的公式。在本发明的以下说明中简称为文献三。

目前，对捷联惯导系统在高动态、高精度导航的研究基本可以分为两个方向，一个强调提高硬件性能，采用高精度的光学惯性元件和高性能的信号处理器，设计高速采集装置等；另一个着重研究新的导航算法，提高在复杂环境中导航算法的精度。两者各有其特点，也各有其存在的问题。改善硬件性能可以提高惯性系统的响应速度，但对惯性系统精度的影响是有限的，且成本比较高；而新的导航算法前提条件过多，使得很难评价这类算法在工程实际中的性能。

发明内容

为了提高在6～2000Hz高动态环境下的捷联惯性导航算法的精度，本发明提出了一种采用奇异摄动方法进行圆锥误差与划船误差补偿的捷联惯导系统，该基于奇异摄动方法补偿的捷联惯导系统认为在一个采样周期T内载体运动的变化(角速度和加速度)是一种摄动过程，利用惯性元件(陀螺和加速度计)在起、始采样点的实际输出值建立强迫奇异摄动模型。根据强迫奇异摄动模型对圆锥误差、划船误差进行补偿，获得采样周期内姿态更新旋转矢量以及速度更新量，通过调节可变参数逼近准确的姿态更新量和速度更新量，从而达到提高捷联惯性系统中导航计算单元中算法精度的要求。

本发明是一种采用奇异摄动方法进行圆锥误差与划船误差补偿的捷联惯导系统，该捷联惯导系统是在惯性测量单元与姿态计算单元之间嵌入圆锥误差补偿奇异摄动模型；在惯性测量单元与导航计算单元之间嵌入划船误差补偿奇异摄动模型。惯性组件用于输出载体的角速度载体的加速度所述角速度加速度经信号处理单元进行滤波处理后输出滤波后角速度滤波后加速度滤波后角速度经圆锥误差补偿奇异摄动模型补偿后输出补偿后的角速度给姿态计算单元；姿态计算单元对接收的补偿后的角速度进行解析获得该角速度下的位姿信息并输出给导航计算单元；滤波后加速度经划船误差补偿奇异摄动模型补偿后输出补偿后的加速度给导航计算单元；导航计算单元对接收的位姿信息、补偿后的加速度进行导航解析，从而获得导航信息输出。

所述的采用奇异摄动方法进行圆锥误差与划船误差补偿的捷联惯导系统，其圆锥误差补偿奇异摄动模型$A=\left(\begin{array}{c}{ϵ}_1\stackrel{··}{\overrightarrow{w}}+\stackrel{·}{\overrightarrow{w}}+{\overrightarrow{\beta }}_1=0\\ {\overrightarrow{w}}_n=w_1,{\overrightarrow{w}}_{n+1}=w_2\end{array}\right).$

所述的采用奇异摄动方法进行圆锥误差与划船误差补偿的捷联惯导系统，其划船误差补偿奇异摄动模型$B=\left(\begin{array}{c}{ϵ}_2\stackrel{··}{\overrightarrow{f}}+\stackrel{·}{\overrightarrow{f}}+{\overrightarrow{\beta }}_2=0\\ {\overrightarrow{f}}_n=f_1,{\overrightarrow{f}}_{n+1}=f_2\end{array}\right).$

附图说明

图1是本发明基于奇异摄动补偿模型的结构框图。

图2是经本发明奇异摄动补偿前后的圆锥误差关系图。

图3是经本发明奇异摄动补偿前后的划船误差关系图。

具体实施方式

下面将结合附图和实施例对本发明做进一步的详细说明。

参见图1所示，本发明是一种适用于在高动态环境(6～2000Hz)下的、采用奇异摄动方法进行圆锥误差与划船误差补偿的捷联惯导系统，该捷联惯导系统是在惯性测量单元与姿态计算单元之间嵌入圆锥误差补偿奇异摄动模型；在惯性测量单元与导航计算单元之间嵌入划船误差补偿奇异摄动模型。本发明是对由惯性组件、惯性组件信号处理单元、姿态计算单元和导航计算单元组成的捷联惯导系统进行补偿的。

在本发明中，惯性组件用于输出载体的角速度载体的加速度所述角速度加速度经信号处理单元进行滤波处理后输出滤波后角速度滤波后加速度滤波后角速度经圆锥误差补偿奇异摄动模型补偿后输出补偿后的角速度给姿态计算单元；姿态计算单元对接收的补偿后的角速度进行解析获得该角速度下的位姿信息并输出给导航计算单元；滤波后加速度经划船误差补偿奇异摄动模型补偿后输出补偿后的加速度给导航计算单元；导航计算单元对接收的位姿信息、补偿后的加速度进行导航解析，从而获得导航信息输出。关于惯性组件是由三个陀螺和三个加速度计构成，信号处理单元为现有捷联惯导系统中的一部分，惯性组件与信号处理单元构成现有捷联惯导系统的惯性测量单元。本发明只是利用惯性测量单元输出的滤波后角速度滤波后加速度作为本发明补偿模型的信息输入。

本发明的圆锥误差补偿奇异摄动模型A为：

$A=\left(\begin{array}{c}{ϵ}_1\stackrel{··}{\overrightarrow{w}}+\stackrel{·}{\overrightarrow{w}}+{\overrightarrow{\beta }}_1=0\\ {\overrightarrow{w}}_n=w_1,{\overrightarrow{w}}_{n+1}=w_2\end{array}\right)---\left(1\right)$

式(1)中ε₁表示第一奇异摄动小参数，其取值为[0，1]；

表示在6～2000Hz高动态环境下快变状态的角加加速度矢量；

表示在6～2000Hz高动态环境下慢变状态的角加速度矢量；

表示在一个采样周期T内载体的角振动频率f_w有关参数矢量；

即${\overrightarrow{\beta }}_1=\frac{N}{T}({\overrightarrow{w}}_n-{\overrightarrow{w}}_{n+1}),$N表示与角振动频率f_w相关的一个常量，该常量为浮点数；

T表示一个采样周期；

表示陀螺在当前采样点n的实际输出的角速率w₁；

表示陀螺在下一时刻采样点n+1的实际输出的角速率w₂。

在复杂角振动条件下，载体角速度矢量为：

$\overrightarrow{w}=(w_1-{\overrightarrow{\beta }}_{1}T-w_2)e^{-\left(\frac{t}{{ϵ}_1}\right)}-{\overrightarrow{\beta }}_{1}t+{\overrightarrow{\beta }}_{1}T+w_2---(1-1)$

式(1-1)中，表示从n采样点到n+1采样点的载体角速度矢量，t表示从n采样点到n+1采样点的时间，取值[0，T]，e表示指数。

本发明的划船误差补偿奇异摄动模型B为：

$B=\left(\begin{array}{c}{ϵ}_2\stackrel{··}{\overrightarrow{f}}+\stackrel{·}{\overrightarrow{f}}+{\overrightarrow{\beta }}_2=0\\ {\overrightarrow{f}}_n=f_1,{\overrightarrow{f}}_{n+1}=f_2\end{array}\right)---\left(2\right)$

式(2)中ε₂表示第二奇异摄动小参数，其取值为[0，1]；

表示在6～2000Hz高动态环境下快变状态的载体加加速度矢量；

表示在6～2000Hz高动态环境下慢变状态的载体加速度矢量；

表示在一个采样周期T内载体的线振动频率f_a有关参数矢量；

即${\overrightarrow{\beta }}_2=\frac{M}{T}({\overrightarrow{f}}_n-{\overrightarrow{f}}_{n+1}),$M表示与线振动频率f_a相关的一个常量，该常量为浮点数；T表示一个采样周期；

表示加速度计在当前采样点n的实际输出的加速度f₁；

表示加速度计在下一时刻采样点n+1的实际输出的加速度f₂。

在复杂角振动和线振动条件下，载体加速度矢量为：

$\overrightarrow{f}=(f_1-{\overrightarrow{\beta }}_{2}T-f_2)e^{-\left(\frac{t}{{ϵ}_2}\right)}-{\overrightarrow{\beta }}_{2}t+{\overrightarrow{\beta }}_{2}T+f_2---(2-1)$

式(2-1)中，表示从n采样点到n+1采样点的载体加速度矢量，t表示从n采样点到n+1采样点的时间，取值[0，T]，e表示指数。

一、圆锥误差的补偿

本发明对圆锥误差的补偿为：将载体角速度矢量代入文献一中的公式34从而获得基于奇异摄动理论的等效旋转矢量φ。

$\phi ={ϵ}_{1}a-\frac{1}{2}b{T}^2+\mathrm{cT}+\frac{1}{2}a\times b(\frac{1}{2}{ϵ}_1{T}^2+2{{ϵ}_1}^3)+\frac{1}{2}c\times a({{2ϵ}_1}^3-{ϵ}_{1}T)+\frac{1}{12}b\times {\mathrm{cT}}^3---(1-2)$

式(1-2)中，$a_1=(w_1-{\overrightarrow{\beta }}_{1}T-w_2),$$b_1={\overrightarrow{\beta }}_1,$$c_1={\overrightarrow{\beta }}_{1}T+w_2.$

根据基于奇异摄动理论的等效旋转矢量φ代入文献一中的公式62解析得到四元素q_BI(m)^BI(m-1)。利用文献一中的公式57和公式58得到更新后的四元素，然后将更新后的四元素代入文献三中的公式(6-3)中解析得到捷联矩阵，依据捷联矩阵中的元素信息得到补偿圆锥误差后的载体姿态。

本发明对捷联惯导系统中圆锥误差和划船误差的补偿方法，在复杂角振动环境和惯性测量组件(三个陀螺仪和三个加速度计)采样频率不高(100Hz)的情况下进行的。在复杂角振动环境中，考虑到惯性测量组件一个采样周期T内的载体角加速度的变化相对于角速度的变化是快变的变化量，通过在采样周期T内的载体角加速度的变化量前强制添加时变的奇异摄动小参数ε₁，得到了惯性测量组件在采样周期T内的载体角速度变化的奇异摄动模型A。

通过调节奇异摄动小参数ε₁和与一个采样周期T内载体的角振动频率f_w有关参数矢量载体角速度变化奇异摄动模型逼近载体角速度在采样周期内的真实变化。其中奇异摄动小参数ε₁的取值范围为[0，1]。与采样周期T内载体的角振动频率f_w有关参数矢量${\overrightarrow{\beta }}_1=\frac{N}{T}({\overrightarrow{w}}_n-{\overrightarrow{w}}_{n+1}).$在角振动频率f_w中，N的取值越大，则角振动频率越高。如，当载体角振动频率f_w为20Hz时，N取值为501。

对于引用文献一的等效旋转矢量姿态更新算法φ_m＝α_m+β_m，其中，${\alpha }_m={\int }_{t_{m-1}}^{t_m}{\omega }_{\mathrm{IB}}^B\mathrm{dt},$α_m＝α(t_m)，${\beta }_m=\frac{1}{2}{\int }_{t_{m-1}}^{t_m}(\alpha \left(t\right)\times {\omega }_{\mathrm{IB}}^B)\mathrm{dt}.$而在本发明中，载体角速度矢量相当于ω_IB^B，n采样点相当于m-1采样点，n+1采样点相当于m采样点。在等效旋转矢量姿态更新算法中β_m为圆锥误差。${\beta }_m=\frac{1}{2}{\int }_{t_{m-1}}^{t_m}(\alpha \left(t\right)\times {\omega }_{\mathrm{IB}}^B)\mathrm{dt}$即为圆锥误差的表达式。

二、划船误差的补偿

本发明对划船误差的补偿为：将载体加速度矢量和载体角速度矢量代入文献二中的划船误差模型，从而获得基于奇异摄动理论的划船误差估计值。所述划船误差估计值包括有载体角速度矢量的积分α(t)、载体加速度矢量(即$\overrightarrow{f}=a_{\mathrm{SF}}^B$)、载体加速度矢量的积分v(t)、载体角速度矢量(即$\overrightarrow{w}={\omega }_{\mathrm{IB}}^B$)。在本发明中划船误差的补偿值C为：

$C=\frac{1}{2}{\int }_{t_{m-1}}^{t_m}(\alpha \left(t\right)\times \overrightarrow{f}+v\left(t\right)\times \overrightarrow{w})\mathrm{dt}---(2-2)$

式(2-2)中，$\alpha \left(t\right)=-{ϵ}_1{a}_1(e^{-\frac{t}{{ϵ}_1}}-1)-\frac{1}{2}{b}_1{t}^2+c_{1}t,$

$v\left(t\right)=-{ϵ}_2{a}_2(e^{-\frac{t}{{ϵ}_2}}-1)-\frac{1}{2}{b}_2{t}^2+c_{2}t,$

$\overrightarrow{f}=a_2{e}^{-\left(\frac{t}{{ϵ}_2}\right)}-b_{2}t+c_2,$

$\overrightarrow{w}=a_1{e}^{-\left(\frac{t}{{ϵ}_1}\right)}-b_{1}t+c_1,$

$a_1=(w_1-{\overrightarrow{\beta }}_{1}T-w_2),$

$b_1={\overrightarrow{\beta }}_1,$

$c_1={\overrightarrow{\beta }}_{1}T+w_2,$

$a_2=(f_1-{\overrightarrow{\beta }}_{2}T-f_2),$

$b_2={\overrightarrow{\beta }}_2,$

$c_2={\overrightarrow{\beta }}_{2}T+f_2.$

将划船误差估计值代入文献二中的公式36获得载体系下的补偿划船误差后的速度增量，该补偿划船误差后的速度增量代入文献二中的公式4中，得到载体更新的速度。对载体更新速度进行积分得到载体的位置。

在复杂角振动和线振动的环境中，考虑到惯性测量组件在一个采样周期T内的载体加加速度的变化相对于加速度的变化是快变的变化量，通过在采样周期内的载体加加速度的变化量前强制添加时变的奇异摄动小参数ε₂(t)，得到了惯性测量组件采样周期内的载体加速度变化的奇异摄动模型B。

通过调节奇异摄动小参数ε₂(t)和与采样周期T内载体的线振动频率f_a有关参数矢量载体加速度变化奇异摄动模型逼近载体加速度在采样周期内的真实变化。其中奇异摄动小参数ε₂(t)的取值范围为[0，1]。与采样周期内载体的线振动频率f_a有关参数矢量${\overrightarrow{\beta }}_2=\frac{M}{T}({\overrightarrow{f}}_n-{\overrightarrow{f}}_{n+1}).$当采样周期T＝10ms，常量M为与载体线振动频率有关参数，振动频率越高，M的取值越大，当载体线振动频率f_a＝20Hz时，常量M＝5.118。

对于引用文献二的公式36中的$\frac{1}{2}{\int }_{t_{m-1}}^{t_m}(\alpha \left(t\right)\times f_{\mathrm{SF}}^B+v\left(t\right)\times {\omega }_{\mathrm{IB}}^B)\mathrm{dt}$为划船误差。

本发明基于奇异摄动理论捷联惯性导航补偿方法的优点在于：由于工程中的采样频率(100Hz)不高，因此在高动态(6～2000Hz)环境下采样间隔内的真实载体角速率是不可知的，依据起、始两点的惯性元件采样值，在采样间隔内建立奇异摄动模型调节可调参数，可以模拟载体在采样间隔内的实际输出，然后根据模拟的惯性元件输出带入惯性导航的基本姿态更新和速度更新方程，从而达到提高惯性导航算法精度的要求。该方法具有灵活性和鲁棒性，通过调节参数可以模拟载体的任意飞行动作，使得捷联惯性导航算法不受实际载体运动的影响。

对于本发明的补偿步骤为：

(A)根据起、始两点采样值w₁，w₂，代入到姿态更新旋转矢量方程得到姿态更新后的旋转矢量；

(B)通过调节参数矢量第一奇异摄动小参数ε₁，模拟在采样周期T内的载体的真实角速率，从而使计算的姿态更新旋转矢量逼近准确的姿态更新旋转矢量；

(C)根据起、始两点采样值f₁，f₂，代入到速度更新方程进行速度更新；

(D)通过调节参数矢量第二奇异摄动小参数ε₂，模拟在采样周期T内的载体真实的比力输出，从而逼近更新周期内准确的速度更新。

实施例

一、给定经典的圆锥运动作为检验，假设飞行器沿两正交轴存在同频的角振动设：

$\alpha \left(t\right)=\left(\begin{array}{c}0\\ {\theta }_y\sin \left(2\pi f_{w}t\right)\\ -{\theta }_z\cos \left(2\pi f_{w}t\right)\end{array}\right),$$\overrightarrow{w}\left(t\right)=\left(\begin{array}{c}0\\ 2\pi f_w{\theta }_y\cos \left(2\pi f_{w}t\right)\\ 2\pi f_w{\theta }_z\sin \left(2\pi f_{w}t\right)\end{array}\right)$

θ_y，θ_z分别为载体系下沿y轴方向和沿z轴方向的姿态角，α(t)为载体坐标下的姿态角矢量，为载体坐标下的角速度矢量，α(t)和均为理想情况下的矢量真值，则可得此条件下采样周期内的圆锥误差真实值为：

$\frac{1}{2}\underset{t_n}{\overset{t_{n+1}}{\int }}\alpha \times \overrightarrow{w}\mathrm{dt}=\left(\begin{array}{c}\pi f_w{\theta }_y{\theta }_z\\ 0\\ 0\end{array}\right)$

给定采样周期T＝10ms，载体姿态运动频率f_w＝20Hz，在该条件下，采样周期内实际产生的圆锥误差项为0.0057rad。

对于实际的陀螺角速度矢量，若将基于奇异摄动圆锥误差补偿方案中的参数做如下调整：

${\beta }_1=\frac{w_1-w_2}{T}+500\frac{w_1-w_2}{T},ϵ=0.00002$

可得在更新周期内计算的动态圆锥误差为0.0056617rad。可见，此时补偿的动态圆锥误差(0.0056617rad)与实际的圆锥误差(0.0057rad)基本一致。当仿真时间t＝30s时，将上述采样周期内的圆锥误差补偿结论代入姿态计算的仿真中。仿真结果如图2所示。

采用本发明的奇异摄动补偿前后圆锥误差如图2所示。其中，虚线段为载体运动过程中未补偿的圆锥误差，实线段为经过基于奇异摄动模型补偿的圆锥误差，可以明显看出，经过奇异摄动算法补偿的圆锥误差较未补偿的圆锥误差明显减小。

二、给定经典的划船运动作为检验，假设飞行器沿两正交轴存在同频的角振动和线振动设：

$\alpha \left(t\right)=\left(\begin{array}{c}{A}_{\alpha }\cos \left(\mathrm{\Omega t}\right)\\ 0\\ 0\end{array}\right),$$\overrightarrow{w}\left(t\right)=\left(\begin{array}{c}-A_{\alpha }\Omega \sin \left(\mathrm{\Omega t}\right)\\ 0\\ 0\end{array}\right);$

$p\left(t\right)=\left(\begin{array}{c}0\\ A_p\cos \left(\mathrm{\Omega t}\right)\\ 0\end{array}\right),$$v\left(t\right)=\left(\begin{array}{c}0\\ -A_p\Omega \sin \left(\mathrm{\Omega t}\right)\\ 0\end{array}\right),$$\overrightarrow{f}\left(t\right)=\left(\begin{array}{c}0\\ -A_p{\Omega }^2\cos \left(\mathrm{\Omega t}\right)\\ 0\end{array}\right)$

上式中A_α为角振动幅值；A_p为线振动幅值；Ω为角振动以及线振动频率，α(t)为载体坐标下的姿态角矢量，为载体坐标下的角速度矢量，p(t)为载体坐标下的位置矢量，v(t)为载体坐标下的速度矢量，为载体坐标下的加速度矢量。

α(t)、p(t)、v(t)、均为理想情况下的矢量真值，则可得此条件下更新周期内的动态划船误差项为：

$\frac{1}{2}\underset{t_n}{\overset{t_{n+1}}{\int }}(v\times \overrightarrow{w}+\alpha \times {\overrightarrow{f}}^b)\mathrm{dt}=\left(\begin{array}{c}\\ 0\\ 0\\ -\frac{A_{\alpha }{A}_p{\Omega }^{2}T}{2}\end{array}\right),$其中，T＝t_n+1-t_n为更新周期。

设仿真条件A_α＝1°，A_p＝0.0001m，Ω＝2πf_a，f_a＝20Hz，速度采样周期为T＝10ms，在该条件下，实际产生的动态划船误差项为-0.00013781m/s。

若将基于奇异摄动划船误差补偿方案中的参数做如下调整：

${\beta }_1=\frac{w_1-w_2}{T}+4.118\frac{w_1-w_2}{T},$

${\beta }_2=\frac{f_1-f_2}{T}+4.118\frac{f_1-f_2}{T},$

ε＝0.00004。

可得在采样周期内计算的动态划船误差为-0.00013779m/s，此时补偿的动态划船误差(-0.00013779m/s)与实际的划船误差(-0.00013781m/s)基本一致，当仿真时间t＝30s时，将上述采样周期内的划船误差补偿结论代入速度计算的仿真中，参见图3所示。在图3中，虚线段为载体运动过程中未补偿的划船误差，实线段为经过基于奇异摄动模型补偿的划船误差，可以明显看出，经过奇异摄动算法补偿的划船误差较未补偿的划船误差明显减小。