CN104713559B - 一种高精度sins模拟器的设计方法 - Google Patents

Abstract

本发明提供了一种高精度SINS模拟器的设计方法，涉及捷联惯导系统的仿真领域。所述方法包括：利用高精度载波相位差分GNSS与中等精度IMU融合的组合导航算法，结合Kalman滤波的部分反馈修正和三次样条拟合方法，生成平滑的轨迹参数，再通过推导SINS反演算法实现了惯性器件的模拟采样仿真，反演算法中充分考虑了姿态圆锥误差和速度划船误差的补偿。这样设计出的SINS模拟器具有极高的精度性能和良好的频率特性，满足高精度SINS对数据源仿真精度和频率复杂性的要求。

Description

一种高精度SINS模拟器的设计方法

技术领域

本发明涉及捷联惯导系统的仿真领域，尤其涉及一种高精度SINS模拟器的设计方法。

背景技术

在惯性导航系统及其组合导航的仿真研究中，运载体的运动轨迹参数和惯性器件(陀螺和加速度计)的数据源仿真是项基础性工作，特别是研究高精度的SINS(StrapdownInertial Navigation System捷联惯导系统)算法，更是需要高精度的数据模拟器作为支撑。以机载SINS仿真为例，传统的基于纯数学模型的轨迹模拟器，只能给出由静止、加速、爬升、巡航和盘旋转弯等简单飞行动作组合的载机运动轨迹，运动形式过于简单，效果不够逼真，若要进一步考虑载机飞控特性和气动环境的影响，物理和数学模型往往又过于复杂，并且理想模型与实际飞行情况之间或多或少总会存在一些差距。

发明内容

本发明的实施例提供一种高精度SINS模拟器的设计方法，设计出的SINS模拟器具有极高的精度性能和良好的频率特性，满足高精度SINS对数据源仿真精度和频率复杂性的要求。

为达到上述目的，本发明的实施例采用如下技术方案：

一种高精度捷联惯导系统SINS模拟器的设计方法，包括以下步骤：

S1、由实际捷联惯导原始数据，即陀螺角增量Δθ_m和加速度计速度增量Δv_m，进行捷联惯导更新算法，推算获得当前时刻的惯导姿态、速度和位置信息；

S2、利用高精度GPS与捷联惯导构成组合导航系统，进行Kalman滤波，估计捷联惯导误差；

S3、根据Kalman滤波估计的捷联惯导误差，使用部分反馈修正方法修正所述当前时刻的惯导姿态、速度和位置信息，得到精确并且平滑的姿态、速度和位置组合导航参数；

S4、对姿态和位置组合导航参数作三次样条函数拟合，获得分段连续的轨迹函数，即姿态函数A(t)和位置函数p(t)，由位置函数p(t)微分并乘以相应系数得到速度函数vⁿ(t)；

S5、对轨迹函数A(t)、vⁿ(t)和p(t)作等间隔插值，得到离散的轨迹序列，即姿态序列A_k、速度序列和位置序列p_k；

S6、根据联惯导航反演算法进行惯性器件模拟，即获得陀螺角增量Δθ_k和加速度计速度增量Δv_k模拟采样序列。

上述技术方案提供的高精度SINS模拟器的设计方法，利用高精度载波相位差分GNSS(全球卫星导航系统)与中等精度IMU(惯性测量单元)融合的组合导航算法，结合Kalman滤波的部分反馈修正和三次样条拟合方法，生成平滑的轨迹参数，再通过推导SINS反演算法实现了惯性器件的模拟采样仿真，反演算法中充分考虑了姿态圆锥误差和速度划船误差的补偿。通过机载飞行实验数据验证表明，所提惯性器件模拟器具有极高的精度性能和良好的频率特性，满足高精度SINS对数据源仿真精度和频率复杂性的要求。

附图说明

图1为本发明实施例提供的一种高精度SINS模拟器的设计方法的流程示意图；

图2为本发明实施例提供的载机1小时飞行轨迹的水平投影图；

图3为本发明实施例提供的x轴陀螺和x轴加速度计在1000s-1100s的功率谱结果图示；

图4为本发明实施例提供的纯惯导解算结果与组合导航结果比较图示。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

首先，简要说明本发明实施例中涉及到的几个坐标系：惯性坐标系表示为i系，地球坐标系表示为e系，“东-北-天”导航坐标系为n系，“右-前-上”载体坐标系为b系。

SINS导航算法包含一组微分方程，即姿态微分方程、速度微分方程和位置微分方程，可分别表示为：

其中：

M_pv＝diag(sec L/R_Nh 1/R_Mh 1)，R_Mh＝R_M+h，R_Nh＝R_N+h

gⁿ＝[0 0 -g]^T，g＝g₀(1+β₁sin²L+β₂sin⁴L)-β₃h

并且各符号含义如下：

是从b系到n系的坐标系变换矩阵，即SINS的姿态矩阵；

和分别表示陀螺测量的角速率和加速度计测量的比力；

是n系下的惯导速度矢量，表示沿“东-北-天”方向的投影分量；

p＝[λ L h]^T是SINS位置矢量，λ，L，h分别表示地理经度、纬度和海拔高度；

R_e、f：地球长半轴半径和扁率，可分别取R_e＝6378137(m)，f＝1/298.257；

ω_ie、g₀：地球自转角速率和赤道海平面处的重力加速度大小，可分别取ω_ie＝7.2921151467×10^-5(rad/s)、g₀＝9.7803267714(m/s²)；

β₁，β₂，β₃：与地理纬度和海拔高度有关的重力加速度系数，可分别取β₁＝5.27094×10^-3、β₂＝2.32718×10^-5、β₃＝2g₀/R_e＝3.086×10^-6(s^-2)。

本发明实施例提供了一种高精度SINS模拟器的设计方法，所述方法包括以下步骤：

S1、由实际捷联惯导原始数据，即陀螺角增量Δθ_m和加速度计速度增量Δv_m，进行捷联惯导更新算法，推算获得当前时刻的惯导姿态、速度和位置信息。

捷联惯导更新算法是一种航位推算算法，这是现有惯用的一种方法，现简述如下：

它根据上一时刻(t_m-1时刻)的已知姿态、速度和位置导航信息，结合惯性器件在更新周期内的采样输出，递推计算当前时刻(t_m时刻)的导航信息。记T_m＝t_m-t_m-1为SINS更新周期。假设陀螺采样输出为角增量并且加速度计输出为速度增量，高精度的惯导系统往往都采取这种采样方式。

考虑到数值更新算法的精度和为了后续推导反演算法的方便，下面以“单子样+前一周期”误差补偿方法给出SINS数值更新算法的主体框架。

(A)姿态更新算法

根据姿态变换矩阵的链乘规则，t_m时刻的姿态矩阵可表示为如下形式

其中

为t_m-1时刻的姿态矩阵；

是在更新周期T_m内n系相对于i系的转动变换矩阵，可根据旋转矢量确定，计算方法参见式(6)。这里下标m-1/2表示[t_m-1，t_m]的中间时刻，即t_m-T_m/2时刻(或简记为t_m-1/2)，t_m-1/2时刻涉及的有关参数可采用线性外推方法进行估计，比如L_m-1/2＝L_m-1+(L_m-1-L_m-2)/2；

由旋转矢量Φ_m确定(方法亦见式(6))，这里考虑等效旋转矢量圆锥误差补偿，采用“单子样+前一周期”陀螺角增量补偿算法，则有

Φ_m＝Δθ_m+1/12·Δθ_m-1×Δθ_m (5)

上式中Δθ_m-1，Δθ_m分别是陀螺在[t_m-2，t_m-1]和[t_m-1，t_m]时间段内的角增量输出，即

下面再给出由等效旋转矢量Φ计算变换矩阵C的公式：

(B)速度更新算法

由t_m-1时刻导航速度递推计算t_m时刻速度的数值方法如下

与前面姿态更新中的计算方法类似，式(7)中可由旋转矢量确定。如果考虑速度划船误差补偿，同样采用“单子样+前一周期”陀螺角增量和加速度计速度增量补偿算法，则在式(8)中有

Δv_rot，m＝1/2·Δθ_m×Δv_m (10)

Δv_scull，m＝1/12·(Δθ_m-1×Δv_m+Δv_m-1×Δθ_m) (11)

上式中Δv_m-1，Δv_m分别是加速度计在[t_m-2，t_m-1]和[t_m-1，t_m]时间段内的速度增量输出，即

(C)位置更新算法

在完成导航速度的更新之后，由t_m-1时刻导航地理位置p_m-1递推t_m时刻位置p_m的数值算法如下

在式(9)和(12)中，表示相应量在t_m-1/2时刻的计算值，均可通过t_m-2和t_m-1时刻的导航参数进行线性外推估计，在此不再详述。

S2、利用高精度GPS与捷联惯导构成组合导航系统，进行Kalman滤波，估计捷联惯导误差。

采用高精度载波相位GNSS与SINS构成组合导航系统，GNSS可达厘米级的定位精度并且载机的机动大、飞行速度高，因此必需考虑到GNSS和SINS之间的安装杆臂误差δl以及两者之间的时间不同步误差δt影响。这里建立19维的GNSS/SINS组合Kalman滤波器，状态选取及滤波器构造如下

z＝p_SINS-p_GNSS＝Hx+v (15)

此步骤为现有的算法，上式中各符号的含义，详细系统参数以及估计过程具体可参考文献《严恭敏.车载定位定向系统研究[D].西安：西北工业大学博士论文，2006.》以及《赵春莲.航空摄影测量中高精度POS导航算法研究[D].西安：西北工业大学硕士论文，2014.》。

姿态中涉及的参数为和分别表示陀螺测量的角速率和加速度计测量的比力；是n系下的惯导速度矢量，表示沿“东-北-天”方向的投影分量；p＝[λ L h]^T是SINS位置矢量，λ，L，h分别表示地理经度、纬度和海拔高度。故Kalman滤波估计的捷联惯导误差包括姿态中的陀螺测量的角速率误差和加速度计测量的比力误差，速度中的沿“东-北-天”方向的投影分量误差；位置中的经度误差，纬度误差以及海拔高度的误差。

S3、根据Kalman滤波估计的捷联惯导误差，使用部分反馈修正方法修正所述当前时刻的惯导姿态、速度和位置信息，得到精确并且平滑的姿态、速度和位置组合导航参数。

为了保持惯导系统的导航误差始终为小量，即保证惯导误差方程的线性，提高滤波器模型精度，在组合导航过程中需将Kalman滤波的状态估计值不断反馈给惯导系统，进行惯导修正。传统的反馈方法是：选定某些状态参与反馈，将Kalman滤波器中的这些状态的估计值一次性全部反馈给惯导系统，修正惯导参数，再将滤波器中所有参与反馈的状态清零。但是，上述全反馈方法容易造成惯导导航输出剧烈的锯齿状波动，从而影响后续SINS反演算法的效果，在惯性器件模拟器上产生很大的间歇性脉冲失真。为了避免全反馈的不良影响，下面提出部分反馈修正原理，以经度误差的状态反馈修正为例，表示为

以惯导经度误差的状态反馈修正为例，部分反馈修正方法为：

其中，为捷联惯导算法的经度，λ_m为部分反馈修正后的经度输出；为Kalman滤波器的经度误差估计值，为经过部分反馈修正后的残余估计值；α∈[0，1]为计算修正权重的系数因子。

同理，姿态、速度和位置中的各个参数都可以根据Kalman滤波估计的捷联惯导误差按照公式(16)和(17)进行修正，最终获得精确并且平滑的姿态、速度和位置组合导航参数。

虽然GNSS/SINS组合导航的量测更新周期一般远大于SINS更新周期，但如果选择反馈修正周期与SINS更新周期T_m一致，并利用上述部分反馈修正方法，则可得到比较平滑的导航参数输出，为惯性模拟器的反演算法提供更加逼真的参考轨迹。当然，如欲进一步提高参考轨迹参数的求解精度，还可采用双向滤波或平滑等算法。

S4、对姿态和位置组合导航参数作三次样条函数拟合，获得分段连续的轨迹函数，即姿态函数A(t)和位置函数p(t)，由位置函数p(t)微分并乘以相应系数得到速度函数vⁿ(t)。

以位置信息中的经度为例，对组合导航输出的经度序列L，λ_m-1，λ_m，λ_m+1，L进行分段三次样条函数拟合，假设在时间区间t∈[t_m-1，t_m]上的拟合三次多项式函数为

λ(t)＝a_m0+a_m1t+a_m2t²+a_m3t³ (18)

式中，a_m0，a_m1，a_m2，a_m3为样条拟合系数。

经度与惯导东向速度之间的关系为

一般区间[t_m-1，t_m]很小，上式中不妨将L(t)和R_Nh(t)取成t_m-1/2时刻的值。由于位置函数已知，这里L_m-1/2可通过样条函数插值精确获得，而不像常规SINS更新算法中的那样需通过线性外推进行估计。

由式(19)移项，再将式(18)代入，即可求得东向速度函数

同理，对纬度和高度序列进行处理，可求得北向速度和天向速度，从而获得任意时刻的位置函数p(t)和速度函数vⁿ(t)；若对欧拉角序列进行同样处理，可获得欧拉角分段拟合函数，记为姿态函数A(t)。值得注意的是，如果欧拉角表示姿态存在奇异点，则需做特殊处理，这里不再详述。

S5、对轨迹函数A(t)、vⁿ(t)和p(t)作等间隔插值，得到离散的轨迹序列，即姿态序列A_k、速度序列和位置序列p_k。

假设SINS反演算法生成惯性器件的解算周期为T_k＝t_k-t_k-1，它可以不同于T_m，一般情况下小于T_m，T_k越小则解算精度越高。

以T_k为等时间间隔，对姿态函数A(t)作等间隔插值可获得姿态序列A_k，对速度函数vⁿ(t)作等间隔插值可获得速度序列对位置函数p(t)作等间隔插值可获得位置序列p_k。

S6、根据联惯导航反演算法进行惯性器件模拟，即陀螺角增量Δθ_k和加速度计速度增量Δv_k模拟采样序列。

以T_k为等时间间隔，对姿态函数A(t)作等间隔插值可获得姿态序列A_k，这等效于已知姿态变换矩阵序列信息由式(4)移项并将时间下标m换成k，可得

其中，分别为k-1和k时刻的姿态矩阵，均可根据姿态序列计算；可根据计算。由于已知速度和位置函数，这里中的相关参数可通过样条函数插值精确计算。

由变换矩阵和式(6)的逆运算可求得等效旋转矢量Φ_k，再根据式(5)求得陀螺角增量

Δθ_k＝(I_3×3+1/12·Δθ_k-1×)^-1Φ_k

≈(I_3×3-1/12·Δθ_k-1×)Φ_k≈Φ_k-1/12·Δθ_k-1×Φ_k (22)

在反演算法初始化时，可假设Δθ₀＝0。

其中，Φ_k是变换矩阵的等效旋转矢量表示方式。

同样以T_k为时间间隔，对速度三次样条函数vⁿ(t)插值可获得速度序列由式(7)移项得

上式中，分别为k-1和k时刻的导航速度；为惯导有害加速度补偿量；为k-1至k-1/2时刻的导航系旋转。和的计算并不复杂，也无需详述。

另外，将式(10)和(11)代入式(8)，得

对式(24)移项整理，得

这里同样可假设初值Δv₀＝0。至此，根据式(23)和(25)即可求解得加速度计的速度增量，完成惯性器件的模拟采样仿真。

下面将本发明实施例提供的方法应用至具体实施例中：

现有一组机载导航实验数据，其SINS中陀螺的随机常值漂移约为0.03°/h，加速度计的逐次启动随机常值偏值约为200ug，原始数据输出频率200Hz；载波相位GPS的定位精度为5cm，数据输出频率1Hz。载机飞行轨迹包括静止、滑跑起飞、转弯、巡航和在作业区域实施作业等阶段，图2给出了1小时飞行轨迹的水平投影图，其中左下角“☆”为轨迹起始点。

对实验数据进行处理，大致过程如下：

(1)采用二子样姿态圆锥误差和速度划船误差补偿算法进行SINS导航更新，同时进行GPS/SINS组合导航Kalman滤波，使用部分反馈修正方法修正惯导误差，这些算法均基于PSINS工具箱实现；

(2)对组合导航输出的100Hz姿态和位置数据作三次样条函数拟合，获得分段连续的轨迹函数A(t)、vⁿ(t)和p(t)；

(3)对轨迹函数A(t)、vⁿ(t)和p(t)作200Hz等间隔插值，得到序列A_k、和p_k；

(4)根据SINS反演算法生成惯性器件模拟采样值Δθ_k和Δv_k，比较IMU原始采样数据与模拟采样数据的功率谱，参见图3，图中显示的是x轴陀螺和x轴加速度计在1000s-1100s的结果，其它轴和时间段的效果类似，不再给出；

(5)选用二子样误差补偿算法，由模拟采样Δθ_k和Δv_k进行纯惯导SINS解算，但对惯导的高度通道进行了阻尼，纯惯导解算结果与(1)中的组合导航结果比较，误差参见图4。

从图3可看出，原始采样数据与模拟采样数据的功率谱在低频段吻合得很好，说明了本发明实施例提供的方法设计的惯性器件SINS模拟器能够模拟出载机的低频运动特性。图4表明，本发明设计的SINS模拟器的模拟采样数据具有很高的导航精度，在1小时内纯惯导的姿态误差为0.001arcsec角秒″量级、位置误差为0.1m米量级。

本发明实施例提供的设计方法根据传统的高精度SINS数值更新算法，它由惯性器件输出求解姿态、速度和位置等导航信息，提出了SINS反演算法，即如果已知姿态和位置轨迹信息，反向模拟求解惯性器件采样值，实现惯性器件模拟器的仿真。针对实际载机的飞行数据，需要结合GNSS进行组合导航和使用部分反馈修正技术处理，提高导航精度和获得平滑的轨迹参数，才能作为惯性器件模拟器的理想输入。实验数据处理结果表明，所提惯性模拟器具有在1小时内纯惯导位置误差0.1m量级的精度，完全满足高精度惯导系统对数据源仿真精度的要求。

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

Claims (1)

1.一种高精度捷联惯导系统SINS模拟器的设计方法，其特征在于，包括以下步骤：

S1、由实际捷联惯导原始数据，即陀螺角增量Δθ_m和加速度计速度增量Δv_m，进行捷联惯导更新算法，推算获得当前时刻的惯导姿态、速度和位置信息；

S2、利用高精度GPS与捷联惯导构成组合导航系统，进行Kalman滤波，估计捷联惯导误差；

S3、根据Kalman滤波估计的捷联惯导误差，使用部分反馈修正方法修正所述当前时刻的惯导姿态、速度和位置信息，得到精确并且平滑的姿态、速度和位置组合导航参数；

所述部分反馈修正方法为：

其中，为捷联惯导算法的经度，λ_m为部分反馈修正后的经度输出；为Kalman滤波器的经度误差估计值，为经过部分反馈修正后的残余估计值；α∈[0，1]为计算修正权重的系数因子；

S4、对姿态和位置组合导航参数作三次样条函数拟合，获得分段连续的轨迹函数，即姿态函数A(t)和位置函数p(t)，由位置函数p(t)微分并乘以相应系数得到速度函数vⁿ(t)；

S5、对轨迹函数A(t)、vⁿ(t)和p(t)作等间隔插值，得到离散的轨迹序列，即姿态序列A_k、速度序列和位置序列p_k；

S6、根据联惯导航反演算法进行惯性器件模拟，即获得陀螺角增量Δθ_k和加速度计速度增量Δv_k模拟采样序列。