单轴旋转捷联惯导系统回溯式自对准方法
【专利摘要】本发明涉及一种单轴旋转捷联惯导系统回溯式自对准方法，采用惯性凝固粗对准方法对陀螺和加速度计采样数据进行实时处理，求解初始时刻的姿态矩阵，在此过程中每1s同步压缩存储关键数据；待粗对准结束后利用存储数据进行回溯卡尔曼滤波精对准，正向解算至结束时刻，并在卡尔曼滤波器中将安装杆臂列为误差状态进行在线估计。本发明方法在保证对准精度和对准时间的同时，避免了繁琐的逆向导航解算，降低了存储空间和计算量，提高了工程实用性。
【专利说明】
单轴旋转捷联惯导系统回溯式自对准方法
技术领域
[0001] 本发明属惯性导航领域，为捷联惯导系统初始对准技术领域，具体涉及一种单轴 旋转捷联惯导系统回溯式自对准方法。
【背景技术】
[0002] 单轴旋转方式能够调制陀螺和加速度计的低频误差，在器件精度一定的条件下提 高捷联惯导系统(SINS)的自对准精度。但在旋转和晃动环境下，需要较长的粗对准时间来 建立初始姿态矩阵，以满足后续精对准线性化模型的要求。
[0003] 文献"一种单轴旋转捷联惯导系统高精度快速对准方法，火力与指挥控制，2015， V〇140(7)，p79-83"公开了一种利用逆向导航技术进行旋转自对准的方法。方法首先进行实 时粗对准，同时将所有传感器采样数据存储下来，在粗对准结束后利用存储数据进行逆向 导航和逆向卡尔曼滤波计算，当逆向精对准至数据起点后再顺序进行姿态更新直到对准结 束。这种方法在粗对准和精对准阶段都有效利用了所有测试数据，在有限的对准时间内最 大程度保证了对准精度，较好地解决了对准精度和对准时间的矛盾。但是按照文献所述方 法，导航计算机需要足够大的存储空间和足够强的计算能力来完成嵌入式计算。以采样周 期5ms、对准时间300s为例，方法需要每周期存储陀螺和加速度计数据共6个双精度浮点数， 共计占用6 X 200 X 300 X 8~2.75MB的存储空间，同时要在短时间内完成200 X 300 X 2 = 120,000次捷联解算的突发计算量，一般的嵌入式导航计算机难以实现，导致方法的工程实 用性较低;另外方法对安装杆臂采用人工测量补偿的方法，操作流程复杂。
【发明内容】
[0004] 要解决的技术问题
[0005] 为了避免现有技术的不足之处，本发明提出一种单轴旋转捷联惯导系统回溯式自 对准方法，克服现有的单轴旋转捷联惯导系统逆向对准方法难以工程实现的不足。
[0006] 技术方案
[0007] -种单轴旋转捷联惯导系统回溯式自对准方法，其特征在于步骤如下：
[0008] 步骤1:启动单轴旋转捷联惯导系统，按指定角速率co绕方位轴旋转；
[0009] 步骤2:旋转过程中，根据陀螺和加速度计每周期采样数据进行惯性系下的粗对准 计算，实时求解初始时刻的姿态矩阵:
[0011]式中下标。32为两个不同时刻，且^ =以2，乜=七4为粗对准时间；<°、<°为根 据理论加速度计算的no系速度;$为根据惯性传感器输出计算的bo系速度；
[0012] nQ-初始导航惯性坐标系，在对准起始时刻将导航坐标系n相对惯性空间凝固所 得的惯性坐标系；
[0013] bo -初始载体惯性坐标系，在对准起始时刻将载体坐标系b相对惯性空间凝固所 得的惯性坐标系；
[0014] n-导航坐标系，坐标轴指向按当地地理位置的东-北-天方向确定；
[0015] b-载体坐标系，坐标轴指向按惯性测量组件的右-前-上方向确定；
[0016] 在整个对准过程中，同步压缩存储每Is周期的关键数据，包括:姿态四元数gg，速 度增量和瞬时角速率;
[0017] 步骤3:利用粗对准结束获得的初始姿态阵和存储的关键数据，每Is周期按顺 序进行回溯速度更新和姿态更新；
[0018] 步骤4:每Is周期进行卡尔曼滤波时间更新和量测更新，直至精对准结束。
[0019] 所述步骤3利用粗对准结束获得的初始姿态阵和存储数据，每Is周期按顺序进 行回溯速度更新为：
[0021] 式中V^、<分别为第k-1和k秒的导航速度;下标k-1/2表示时间区间[k_l，k]的 中间时刻;转换矩阵qt根据地球自转角速率武、表面运动角速率<、当地炜度 和对准时间获得;为粗对准获得的初始姿态阵；为粗对准过程中压缩存储的速度增 量;容为重力加速度矢量;Tk= 1为更新周期；
[0022] 姿态更新方法为其中矩阵cj为第k秒的姿态矩阵，矩阵cj由粗 对准阶段存储的四元数转换获得，矩阵:根据地球自转角速率<、当地炜度和对准时 间计算。
[0023]有益效果
[0024] 本发明提出的一种单轴旋转捷联惯导系统回溯式自对准方法，采用惯性凝固粗对 准方法对陀螺和加速度计采样数据进行实时处理，求解初始时刻的姿态矩阵，在此过程中 每Is同步压缩存储关键数据;待粗对准结束后利用存储数据进行回溯卡尔曼滤波精对准， 正向解算至结束时刻，并在卡尔曼滤波器中将安装杆臂列为误差状态进行在线估计。本发 明方法在保证对准精度和对准时间的同时，避免了繁琐的逆向导航解算，降低了存储空间 和计算量，提高了工程实用性。
[0025] 本方法利用采集数据实时粗对准得到起始时刻的姿态矩阵，同时每Is周期压缩存 储关键数据，待粗对准结束后利用压缩数据正向精对准解算至结束时刻，并将安装杆臂列 为误差状态进行自动估计。方法在保证对准精度和对准时间的同时，避免了繁琐的逆向导 航解算，降低了存储空间和计算量，提高了工程实用性。
[0026] 本技术方案在旋转对准过程中，利用每Is周期压缩存储的关键数据进行回溯导航 和精对准解算，在保证对准精度和对准时间的同时，避免了繁琐的逆向导航解算，降低了存 储空间和计算量，提高了工程实用性;将安装杆臂列为误差状态进行在线估计，简化了操作 流程。仍以采样周期5ms、对准时间300s为例，通过实施技术方案，所需存储空间由2.75MB减 少为10X300 X 8~23KB，减少120倍；捷联解算计算量由120,000次减小为300次，减小400 倍，并且能够分散执行以满足嵌入式解算要求。
【附图说明】
[0027] 图1是本发明单轴捷联惯导系统回溯式自对准方法的流程图
[0028] 图2是本发明实施例粗对准阶段的姿态误差曲线
[0029] 图3是【背景技术】和本发明实施例精对准阶段的方位失准角对比曲线
[0030] 图4是本发明实施例精对准阶段对安装杆臂的估计结果
【具体实施方式】
[0031] 现结合实施例、附图对本发明作进一步描述：
[0032] 本发明共包含4幅附图，下面结合图1~图4对本发明的【具体实施方式】作详细说明。
[0033] 1、转位机构旋转。
[0034]参照图1，在to时刻惯导系统获得初始位置并进入对准状态后，控制转位机构绕方 位轴旋转。旋转角速率固定为《，其大小在6~60/s之间取值，方位轴每旋转1周交替改变co 的方向。
[0035] 2、惯性系粗对准。
[0036]参照图1，在[to，t。]时间区间进行惯性系粗对准实时解算。首先定义解算所需的参 考坐标系：
[0037] n-导航坐标系，坐标轴指向按当地地理位置的东-北-天方向确定；
[0038] b -载体坐标系，坐标轴指向按惯性测量组件(MU)的右-前-上方向确定；
[0039] no -初始导航惯性坐标系，在对准起始时刻将导航坐标系n相对惯性空间凝固所 得的惯性坐标系；
[0040] bo -初始载体惯性坐标系，在对准起始时刻将载体坐标系b相对惯性空间凝固所 得的惯性坐标系。
[0041] 求解微分方程获得当前t时刻的转换矩阵
[0043] 其中积分初值为￡?^)二/;€为13系运动角速率，由每周期采样的陀螺数据获 得。
[0044] 积分获得t时刻的bo系速度
[0047] 式中@f为两个不同时刻ti、t2的bo系速度，且ti = t/2，t2 = t，t为当前对准时 间；M Sb系比力，由每周期采样的加速度计数据获得。
[0048]计算t时刻的转换矩阵
[0050] 其中Lo为当地炜度，地球自转角速率《ie = 7.292115X 10-5。
[0051]积分获得t时刻的no系速度
[0054]式中gn=[0 0 -g]T，g为当地重力加速度。
[0055]采用TRIAD算法求解初始姿态矩阵_
[0057]对进行标准正交化处理即获得初始时刻的姿态矩阵。
[0058] 3、数据压缩存储。
[0059] 在整个对准[tQ，tf]过程中，每Is周期存储四元数0$、速度增量和瞬时角速率 ，共10个浮点数参数。各参数的计算方法如下：
[0060] ①为了减小存储量，采用经典变换方法将公式（1)解算的第k秒姿态矩阵转换 为四元数@后进行存储。
[0061 ]②第k秒的速度增量按下式计算
[0063] 式中为根据公式（1)获得的姿态矩阵，/ 乂 r)为根据加速度计输出获得的比 力。
[0064] ③瞬时角速率藏^的计算方法为
[0065] mlk-A0jTm (9)
[0066] 其中为本周期陀螺输出的角增量，Tm为姿态解算周期。
[0067] 4、回溯姿态速度更新。
[0068]参照图1，粗对准结束后，惯导系统继续实时采集存储[tc^tf]区间的数据，并利用 该段空闲时间处理[to，tf]区间的存储数据，进行周期为Is的正向姿态和速度更新。
[0069]①回溯速度更新
[0071]式中V丨分别为第k_l和k秒的导航速度;下标k-1/2表示时间区间[k_l，k]的 中间时刻;转换矩阵Ct1根据式(4)计算;Aif为每秒存储的速度增量;_为地球自转角速 率；at为绕地球表面运动角速率;^为粗对准获得的姿态矩阵；乂Ui为重力加速度矢量； T k= 1，为更新周期;2的计算方法为
[0074] ②回溯姿态更新
[0075] 将每秒存储的四元数gf转换为<^，并计算姿态矩阵
[0077]其中矩阵<^为第k秒更新的姿态矩阵，矩阵巧按式(4)计算。
[0078] 5、回溯卡尔曼滤波精对准。
[0079] 在第4步的回溯导航解算过程中，同步进行卡尔曼滤波精对准，卡尔曼滤波器的时 间更新和量测更新周期均为Is。
[0080] 选择惯导解算相关误差状态包括速度误差6V；1、平台失准角巾'陀螺零偏0和加 速度计零位Vb;考虑MU敏感中心与转位机构旋转中心之间的安装关系，将安装杆臂L b也扩 充为状态变量。建立15维卡尔曼滤波的状态变量
[0082]在此基础上建立卡尔曼滤波器状态方程
[0084] 式中<,、Vt为导航坐标系n下等效的陀螺和加速度计零位噪声为根据经典捷 联惯导误差方程建立的12 X 12维状态转移矩阵。
[0085] 选取导航解算速度误差Jv〗作为量测值，建立卡尔曼滤波器量测方程
[0087] 式中为公式（12)周期计算结果，^为粗对准存储的角速率，wv为速度量测噪 声。
[0088] 惯导系统利用[tc，tf]区间的空闲计算能力对[t0，tf]区间的存储数据进行处理， 分散完成回溯导航计算和卡尔曼滤波精对准，直到t f时刻整个对准过程结束。
[0089] 下面通过仿真试验对本发明的【具体实施方式】和实施效果做进一步说明。
[0090] 仿真条件:设定载体处于摇摆状态，俯仰、横滚和航向轴分别存在0.3° sin(0.l3i t)、0.5°sin(0.063it)、0.1°sin(0.083Tt)的角运动干扰，存在 0.005sin(0.2Jit)m/s、 0.005sin(0.3Jit)m/s、0.005sin(0. lJit)m/s的线振动干扰;对准过程中转位机构的旋转角 速率《 = 20° /s，转位机构和頂U之间存在安装杆臂Lb = [-0 ? 04m; 0 ? 05m; 0 ? lm]; MU中陀螺 漂移误差为0.01°/h、随机游走噪声为
，加速度计零位为50ug、随机噪声为
;惯导系统对准时间[to, tf]共计300s，采样周期为5ms。
[0091] 图2给出了粗对准阶段的姿态误差曲线。可以看出，受载体摇摆环境和杆臂速度的 影响，粗对准需要100s以上才能获得满足线性要求的方位精度。本实施例中取t c = 200s，并 将粗对准阶段的数据压缩存储，有效利用了粗对准时间。
[0092] 图3为【背景技术】和本发明技术在精对准阶段的方位失准角对比曲线。【背景技术】需 要先进行逆向解算，然后再进行正向更新，计算量大且流程复杂;本发明技术在减少存储空 间和计算量的同时，能够获得与【背景技术】相同的对准精度。
[0093] 图4为本发明实施例在精对准阶段估计的安装杆臂值。对比预先设定的仿真条件， 表明本方法能够对X、Y轴安装杆臂进行有效估计，无需进行手工测量，简化了操作流程。由 于仅存在绕方位轴的转动，图中Z轴杆臂不能得到估计。
【主权项】
1. 一种单轴旋转捷联惯导系统回溯式自对准方法，其特征在于步骤如下： 步骤1:启动单轴旋转捷联惯导系统，按指定角速率u绕方位轴旋转； 步骤2:旋转过程中，根据陀螺和加速度计每周期采样数据进行惯性系下的粗对准计 算，实时求解初始时刻的姿态矩阵:式中下标乜山为两个不同时刻，且乜=以2山=七八为粗对准时间；<>、<为根据理 论加速度计算的no系速度;、^为根据惯性传感器输出计算的bQ系速度； no -初始导航惯性坐标系，在对准起始时刻将导航坐标系n相对惯性空间凝固所得的惯 性坐标系； bo -初始载体惯性坐标系，在对准起始时刻将载体坐标系b相对惯性空间凝固所得的惯 性坐标系； n-导航坐标系，坐标轴指向按当地地理位置的东-北-天方向确定； b-载体坐标系，坐标轴指向按惯性测量组件的右-前-上方向确定； 在整个对准过程中，同步压缩存储每Is周期的关键数据，包括:姿态四元数!，速度增 量八咛和瞬时角速率 步骤3:利用粗对准结束获得的初始姿态阵Q和存储的关键数据，每Is周期按顺序进 行回溯速度更新和姿态更新； 步骤4:每1 s周期进行卡尔曼滤波时间更新和量测更新，直至精对准结束。2. 根据权利要求1所述单轴旋转捷联惯导系统回溯式自对准方法，其特征在于:所述步 骤3利用粗对准结束获得的初始姿态阵和存储数据，每is周期按顺序进行回溯速度更新 为：式中V。、<分别为第k-1和k秒的导航速度；下标k-1/2表示时间区间[k-l，k]的中间 时刻;转换矩阵根据地球自转角速率武、表面运动角速率、当地炜度和对 准时间获得；q为粗对准获得的初始姿态阵；Ag3为粗对准过程中压缩存储的速度增量； ^ 1/2为重力加速度矢量;Tk= 1为更新周期； 姿态更新方法为:，其中矩阵cf为第以少的姿态矩阵，矩阵ct由粗对准 阶段存储的四元数fit转换获得，矩阵c：：根据地球自转角速率乾、当地炜度和对准时间计 算。
【文档编号】G01C25/00GK106052715SQ201610344731
【公开日】2016年10月26日
【申请日】2016年5月23日
【发明人】付强文, 李四海, 刘洋, 沈彦超, 严恭敏, 秦永元, 翁浚, 崔潇
【申请人】西北工业大学