技术领域
[0001] 本
发明属于惯性导航领域，可用于快速应急发射时运载火箭发射前的快速
定位定向。技术背景
[0002] 初始对准技术是捷联惯导系统进行实时导航定位的第一步，只有保证初始对准高
精度，后续导航才能精确，初始对准可以分为粗对准和精对准两个过程。粗对准主要是通过双矢量定姿的方法，即空间中不共线的两个矢量确定载体初始时刻
姿态的过程，当载体在静止条件下，一般采用重
力矢量和地球自转
角速度矢量；当载体在晃动基座条件下，地球自转角速度矢量会受到很大的干扰，无法进行初始定姿对准。西北工业大学秦永元等在文献《摇摆基座上基于信息的捷联惯导粗对准研究》提出了一种
凝固坐标系的粗对准方法，该方法利用
跟踪重力矢量的方法，即不同时刻重力矢量在地球坐标系和载体坐标系下的分量，来实现双矢量定姿，该方法能够有效屏蔽晃动对于粗对准的影响。精对准主要是利用卡尔曼滤波技术，通过建立误差方程，实现对方位失准角和
水平失准角的估计的过程，在此
基础上发展了容积卡尔曼滤波、
扩展卡尔曼滤波以及渐消因子卡尔曼滤波等。
[0003] 颜开思等在文献《晃动基座下的SINS初始对准方法研究》中采用凝固坐标系的粗对准和卡尔曼滤波的精对准方法，解决了晃动基座初始对准问题，但是对于惯性元件的常值漂移未作出补偿，这样会导致惯性元件的输出精度不高，对准误差大。针对惯性元件常值漂移影响对准精度的问题，提出了一种旋转调制型捷联惯导系统在晃动基座初始对准
算法，该方法能够通过旋转调制技术补偿惯性元件常值漂移，实现晃动基座下旋转调制捷联惯导系统精确对准。
发明内容
[0004] 本发明的技术解决问题是：提供了一种旋转调制型捷联惯导系统晃动基座初始对准算法，实现了晃动基座条件下旋转调制型捷联惯导系统精确初始对准。该方法不仅可以通过旋转调制技术补偿惯性元件常值漂移，还能通过凝固坐标系的方法实现晃动基座粗对准过程，最后利用卡尔曼滤波方法实现精对准，为晃动基座下旋转调制型捷联惯导系统初始对准提供了一种全新的算法。
[0005] 本发明的技术解决方案是：建立旋转调制型捷联惯导系统误差方程，通过旋转调制技术实现对惯性元件常值漂移的补偿，利用凝固坐标系的方法实现晃动基座条件下粗对准，最后采用卡尔曼滤波技术对失准角进行估计，完成精对准。具体包括以下步骤：
[0006] (1)旋转调制技术及旋转调制型捷联惯导误差模型：
[0007] 连续旋转调制的基本原理为：
[0008] 若惯性元件在旋转台的带动下绕Z轴以恒定角速度ω旋转，则在任意时刻t
陀螺仪的输出为式(1)，转化为载体坐标系下的分量为式(2)
[0009]
[0010]
[0011] ωs——陀螺仪的输出；
[0012] ωn——地球自转角速度在导航坐标系下的分量；
[0013] ωZ——转台的自转在角速度在惯性元件坐标系下的分量；
[0014] εc——为陀螺仪的常值漂移；
[0015] εr——陀螺仪的随即漂移；
[0016] ——为载体坐标系到惯性元件坐标系的转换矩阵；
[0017] ——为导航坐标系到载体坐标系的转换矩阵；
[0018] ——为惯性元件坐标系到载体坐标系的转换矩阵。
[0019] 常值漂移在载体系下的分量可以表示为
[0020]
[0021] εc——为陀螺仪的常值漂移；
[0022] εb——为陀螺仪的常值漂移在载体系下的分量；
[0023] ——为惯性元件坐标系到载体坐标系的转换矩阵；
[0024] x,y,z——表示坐标轴。
[0025] 旋转调制型捷联惯导系统误差模型：
[0026] 当载体仅存在晃动而没有位移的情况下，忽略水平交叉耦合项，可以得到如下误差模型：
[0027]
[0028]
[0029]
[0030]
[0031]
[0032]
[0033] 因初始对准时间较短，所以可以假定
加速度误差和陀螺漂移为随机常数，即惯性器件模型为:
[0034]
[0035]
[0036] 对于旋转调制型捷联惯导系统有
[0037]
[0038]
[0039] δVN——北向速度误差；
[0040] δVE——东向速度误差；
[0041] ——北向失准角；
[0042] ——东向失准角；
[0043] ——方位向失准角；
[0044] ωie——地球自转角速度；
[0046] L——当地地理纬度；
[0047] R——为地球半径；
[0049] ε——为陀螺仪随机常值漂移；
[0050] x,y,z——分别表示惯性元件坐标轴。
[0051] (2)基于凝固坐标系的粗对准
[0052] 晃动基座条件下，陀螺仪的输出信息会受到影响，而加速度计的输出几乎没有影响，所以凝固坐标系粗对准方法，主要是根据不同时刻重力矢量在载体坐标系和导航坐标系下的分量，再通过双矢量定姿的方法得到对准矩阵，具体方法如下：
[0053] 根据链式法则可以得到
[0054]
[0055]
[0056]
[0057]
[0058] ——为载体坐标系到导航坐标系的转换矩阵；
[0059] ——为地球坐标系到导航坐标系的转换矩阵；
[0060] ——为凝固惯性地球坐标系到地球坐标系的转换矩阵；
[0061] ——为凝固惯性载体坐标系到凝固惯性地球坐标系的转换矩阵；
[0062] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵；
[0063] L——为当地的地理纬度；
[0064] ωie——为地球自转角速度；
[0065] Δt——为时间间隔；
[0066] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵；
[0067] ——为陀螺仪的输出在凝固惯性载体坐标系下的分量；
[0068] ×——表示矩阵的叉乘计算。
[0069] 其中 是载体坐标系下陀螺仪测得的载体自转角速率。 可以通过等效旋转矢量的方法求取：
[0070] 而 是通过陀螺仪测量得到的，是一个随时间变化的量，其在时间间隔为T的情况下等效旋转矢量可以近似表示为
[0071]
[0072]
[0073]
[0074] ——为
采样间隔T时间内的等效旋转矢量；
[0075] Δθ1——为0～T/2时间内 积分结果；
[0076] Δθ2——为T/2～T时间内 积分结果；
[0077] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵；
[0078] ×——表示矩阵的叉乘计算。
[0079] 为初始时刻载体坐标系到地球坐标系的转换矩阵，该矩阵为常值，可以采用双矢量定姿的方法求取，但是传统的双矢量定姿的方法由于基座晃动的原因，导致陀螺仪的输出误差较大，所以采用跟踪重力矢量，即选取不同时刻重力加速度在初始时刻载体坐标系和初始时刻地球坐标系下的分量作为双矢量进行定姿，该方法能够有效屏蔽角晃动对初始对准精度的影响；
[0080] 根据惯性导航坐标系的定义可知
[0081]
[0082]
[0083] g——为地球重力加速度；
[0084] gn——为重力加速度在导航坐标系下的分量；
[0085] ——为重力加速度在地球坐标下的分量；
[0086] ——为导航坐标系到地球坐标系的转换矩阵；
[0087] ——为地球坐标系到凝固惯性地球坐标系的转换矩阵；
[0088] L——为当地的地理纬度；
[0089] ωie——为地球自转角速度；
[0090] Δt——为时间间隔。
[0091] 重力加速度g在载体坐标系下的分量可以通过加速度计获得，将其转化为凝固惯性载体坐标系下的分量可以得到
[0092]
[0093] 式中
[0094] ——为加速度计的输出在凝固惯性载体坐标系下的分量；
[0095] fb——为加速度计的输出在载体坐标系下的分量；
[0096] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵。
[0097] 根据比力方程，当忽略载体的线性干扰可以获得
[0098]
[0099] ——为加速度计的输出在凝固惯性载体坐标系下的分量；
[0100] ——为重力加速度在地球坐标下的分量；
[0101] ——为凝固惯性地球坐标系到凝固惯性载体坐标系的转换矩阵。
[0102] 取不同时刻，t1时刻和t2时刻后，作叉乘转置移项组合可以得到式(25)。通过上诉方法可以实现对 的求解，但是在实际捷联惯导初始对准过程中，测量数据都是通过惯性元件获得，在测量的过程中难免有噪声干扰，为了有效屏蔽噪声又不丢失信息，分别对和 在时间[0，t]积分并同理取两个不同时刻，t1时刻和t2时刻， 矩阵可以如下求解[0103]
[0104]
[0105]
[0106]
[0107]
[0108] ——为加速度计输出在凝固惯性载体坐标系下分量积分结果；
[0109] ——为重力加速度在凝固惯性地球坐标系下分量积分结果；
[0110] g——为地球重力加速度；
[0111] Δt——为时间间隔；
[0112] ωie——为地球自转角速度；
[0113] L——为当地的地理纬度；
[0114] ——为加速度计的输出在凝固惯性载体坐标系下的分量；
[0115] fb——为加速度计的输出在载体坐标系下的分量；
[0116] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵。
[0117] (3)基于卡尔曼滤波技术精对准
[0118] 基于卡尔曼滤波的精对准方法
[0119] 通过粗对准得到初始姿态角有较大误差，再利用卡尔曼滤波技术对水平失准角和方失准角进行估计。
[0120] 离散型卡尔曼滤波过程如下：
[0121]
[0122]
[0123]
[0124]
[0125] Pk＝(I-KkHk)Pk/k-1(34)
[0126] ——状态一步预测；
[0127] ——状态估计；
[0128] Kk——滤波增益；
[0129] Pk/k-1——一步预测均方误差；
[0130] Pk——估计均方误差；
[0132] Zk——观测矢量；
[0133] Φk,k-1——一步转移矩阵；
[0134] Γk-1——系统噪声驱动矩阵。
[0135] 旋转调制型捷联惯导系统初始对准的误差
状态方程：
[0136]
[0137]
[0138]
[0139]
[0140]
[0141] δVN——北向速度误差；
[0142] δVE——东向速度误差；
[0143] ——北向失准角；
[0144] ——东向失准角；
[0145] ——方位向失准角；
[0146] ωie——地球自转角速度；
[0147] g——当地重力加速度；
[0148] L——当地地理纬度；
[0149] R——为地球半径；
[0150] ——为加速度计的随机常值偏置；
[0151] ε——为陀螺仪随机常值漂移；
[0152] x,y,z——分别表示惯性元件坐标轴；
[0153] Cij——为 中所对应的元素；
[0154] W——为N(0,Q)的高斯白噪声。
[0155] 选取两个水平速度误差作为观测量，则观测方程为：
[0156] Z＝HX+V (40)
[0157] X——为状态矢量；
[0158] Z——为观测矢量；
[0159] H——为系统观测矩阵；
[0160] V——系统观测噪声。
[0161] 为了验证该方法的效果，载体所在地理
位置为北纬45.7°，东经129.7°设旋转调制转速为10°/s；加速度计的常值偏差为1×10-4g，随机偏差为0.5×10-4g；陀螺仪常值漂移为0.02°/h，随机漂移为0.01°/h，安装误差为Kgx＝Kgy＝Kgz＝10×10-6，标度因数误差为Kgxy＝-6
Kgxz＝Kgyx＝Kgyz＝Kgzx＝Kgzy＝1.5×10 rad；晃动引起的姿态角变化为：
[0162] 航向角：
[0165] 仿真结果如图2、图3、图4所示，其中图2为经过旋转调制后与未经过旋转调制惯性元件的输出误差比较，其中灰色的虚线是经过旋转调制的结果，黑色实线是未经过旋转调制的结果。可以明显的看出经过旋转调制后，常值漂移呈现出在零附近周期性变化，因为本实验方案为绕Z轴旋转，所以Z轴的误差无法进行补偿，所以对比结果无明显变化。而这种周期性变化的误差在后续初始对准过程中会被积分、滤波等过程进一步补偿而趋近于零，而常值误差难以被补偿。
[0166] 图3为利用未旋转调制和旋转调制后的惯性元件输出作为输入后，经过凝固坐标系粗对准后得到的初始姿态角。其中虚线为利用旋转调制技术后粗对准结果，点画线表示未采用任何处理的粗对准结果，实线表示理论结果。从图中可以看出航向角和横滚角结果变化不能明显，俯仰角结果较明显。因为惯性元件Z轴的结果未被调制，而航向角和横滚角结算过程中与Z轴的数据有关，所以效果不明显。但是从俯仰角结果可以清楚的看出经过旋转调制后的结果更加接近理论结果，说明采用旋转调制技术可以提高初始对准精度。
[0167] 图4为利用粗对准得到的初始姿态角，经过卡尔曼滤波后对失准角进行估计，可以看出失准角大部分都在1×10-4°左右
波动，满足精对准对精度的要求。
[0168] 本发明的方案与现有方案相比，主要优点在于：针对惯性元件常值漂移会引起捷联惯导系统初始对准精度低的问题，提出了一种基于旋转调制技术的捷联惯导系统在晃动基座条件下初始对准方法，实现了对惯性元件常值漂移的补偿，同时可以解决晃动基座高精度初始对准问题，为晃动基座条件下捷联惯惯导系统高精度对准提供了一种全新的技术途径。
附图说明
[0169] 图1为整体初始对准设计方案图；
[0171] 图3为惯性元件误差图；
[0172] 图4为凝固坐标系粗对准仿真结果；
[0173] 图5为卡尔曼滤波精对准仿真结果。具体实施方案
[0174] 本发明的实施方案如图1所示，首先利用旋转调制技术对惯性元件常值漂移进行误差补偿，将补偿后的高精度惯性元件输出作为捷联惯导系统的输入，接着利用凝固坐标系的方法实现晃动基座条件下粗对准，最后利用卡尔曼滤波技术实现对失准角的估计，完成初始对准过程。
[0175] (1)利用旋转调制技术实现对惯性元件常值漂移的误差补偿，并建立旋转调制型捷联惯导系统误差方程，对系统进行误差分析；
[0176] 连续旋转调制的基本原理为：
[0177] 若惯性元件在旋转台的带动下绕Z轴以恒定角速度ω旋转，则在任意时刻t陀螺仪的输出为式(1)，转化为载体坐标系下的分量为式(2)
[0178]
[0179]
[0180] ωs——陀螺仪的输出；
[0181] ωn——地球自转角速度在导航坐标系下的分量；
[0182] ωZ——转台的自转在角速度在惯性元件坐标系下的分量；
[0183] εc——为陀螺仪的常值漂移；
[0184] εr——陀螺仪的随即漂移；
[0185] ——为载体坐标系到惯性元件坐标系的转换矩阵；
[0186] ——为导航坐标系到载体坐标系的转换矩阵；
[0187] ——为惯性元件坐标系到载体坐标系的转换矩阵。
[0188] 常值漂移在载体系下的分量可以表示为
[0189]
[0190] εc——为陀螺仪的常值漂移；
[0191] εb——为陀螺仪的常值漂移在载体系下的分量；
[0192] ——为惯性元件坐标系到载体坐标系的转换矩阵；
[0193] x,y,z——表示坐标轴。
[0194] 旋转调制型捷联惯导系统误差模型：
[0195] 当载体仅存在晃动而没有位移的情况下，忽略水平交叉耦合项，可以得到如下误差模型：
[0196]
[0197]
[0198]
[0199]
[0200]
[0201]
[0202] 因初始对准时间较短，所以可以假定加速度误差和陀螺漂移为随机常数，即惯性器件模型为:
[0203]
[0204]
[0205] 对于旋转调制型捷联惯导系统有
[0206]
[0207]
[0208] δVN——北向速度误差；
[0209] δVE——东向速度误差；
[0210] ——北向失准角；
[0211] ——东向失准角；
[0212] ——方位向失准角；
[0213] ωie——地球自转角速度；
[0214] g——当地重力加速度；
[0215] L——当地地理纬度；
[0216] R——为地球半径；
[0217] ——为加速度计的随机常值偏置；
[0218] ε——为陀螺仪随机常值漂移；
[0219] x,y,z——分别表示惯性元件坐标轴。
[0220] (2)将旋转调制后的高精度惯性元件输出作为输入，通过凝固坐标系的粗对准方法，实现晃动基座条件下高精度粗对准；
[0221] 晃动基座条件下，陀螺仪的输出信息会受到影响，而加速度计的输出几乎没有影响，所以凝固坐标系粗对准方法，主要是根据不同时刻重力矢量在载体坐标系和导航坐标系下的分量，再通过双矢量定姿的方法得到对准矩阵，具体方法如下：
[0222] 根据链式法则可以得到
[0223]
[0224]
[0225]
[0226]
[0227] ——为载体坐标系到导航坐标系的转换矩阵；
[0228] ——为地球坐标系到导航坐标系的转换矩阵；
[0229] ——为凝固惯性地球坐标系到地球坐标系的转换矩阵；
[0230] ——为凝固惯性载体坐标系到凝固惯性地球坐标系的转换矩阵；
[0231] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵；
[0232] L——为当地的地理纬度；
[0233] ωie——为地球自转角速度；
[0234] Δt——为时间间隔；
[0235] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵；
[0236] ——为陀螺仪的输出在凝固惯性载体坐标系下的分量；
[0237] ×——表示矩阵的叉乘计算。
[0238] 其中 是载体坐标系下陀螺仪测得的载体自转角速率。 可以通过等效旋转矢量的方法求取：
[0239] 而 是通过陀螺仪测量得到的，是一个随时间变化的量，其在时间间隔为T的情况下等效旋转矢量可以近似表示为
[0240]
[0241]
[0242]
[0243] ——为采样间隔T时间内的等效旋转矢量；
[0244] Δθ1——为0～T/2时间内 积分结果；
[0245] Δθ2——为T/2～T时间内 积分结果；
[0246] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵；
[0247] ×——表示矩阵的叉乘计算。
[0248] 为初始时刻载体坐标系到地球坐标系的转换矩阵，该矩阵为常值，可以采用双矢量定姿的方法求取，但是传统的双矢量定姿的方法由于基座晃动的原因，导致陀螺仪的输出误差较大，所以采用跟踪重力矢量，即选取不同时刻重力加速度在初始时刻载体坐标系和初始时刻地球坐标系下的分量作为双矢量进行定姿，该方法能够有效屏蔽角晃动对初始对准精度的影响；
[0249] 根据惯性导航坐标系的定义可知
[0250]
[0251]
[0252] g——为地球重力加速度；
[0253] gn——为重力加速度在导航坐标系下的分量；
[0254] ——为重力加速度在地球坐标下的分量；
[0255] ——为导航坐标系到地球坐标系的转换矩阵；
[0256] ——为地球坐标系到凝固惯性地球坐标系的转换矩阵；
[0257] L——为当地的地理纬度；
[0258] ωie——为地球自转角速度；
[0259] Δt——为时间间隔。
[0260] 重力加速度g在载体坐标系下的分量可以通过加速度计获得，将其转化为凝固惯性载体坐标系下的分量可以得到
[0261]
[0262] 式中
[0263] ——为加速度计的输出在凝固惯性载体坐标系下的分量；
[0264] fb——为加速度计的输出在载体坐标系下的分量；
[0265] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵。
[0266] 根据比力方程，当忽略载体的线性干扰可以获得
[0267]
[0268] ——为加速度计的输出在凝固惯性载体坐标系下的分量；
[0269] ——为重力加速度在地球坐标下的分量；
[0270] ——为凝固惯性地球坐标系到凝固惯性载体坐标系的转换矩阵。
[0271] 取不同时刻，t1时刻和t2时刻，作叉乘转置移项组合可以得到式(25)，通过上诉方法可以实现对 的求解，但是在实际捷联惯导初始对准过程中，测量数据都是通过惯性元件获得，在测量的过程中难免有噪声干扰，为了有效屏蔽噪声又不丢失信息，分别对 和在时间[0，t]积分并同理取两个不同时刻，t1时刻和t2时刻， 矩阵可以如下求解[0272]
[0273]
[0274]
[0275]
[0276]
[0277] ——为加速度计输出在凝固惯性载体坐标系下分量积分结果；
[0278] ——为重力加速度在凝固惯性地球坐标系下分量积分结果；
[0279] g——为地球重力加速度；
[0280] Δt——为时间间隔；
[0281] ωie——为地球自转角速度；
[0282] L——为当地的地理纬度；
[0283] ——为加速度计的输出在凝固惯性载体坐标系下的分量；
[0284] fb——为加速度计的输出在载体坐标系下的分量；
[0285] ——为载体坐标系到凝固惯性载体坐标系的转换矩阵。
[0286] (3)最后利用粗对准得到的姿态角，进行卡尔曼滤波得到失准角估计值，对姿态角进行修正，完成精对准基于卡尔曼滤波的精对准方法。
[0287] 通过粗对准得到初始姿态角有较大误差，再利用卡尔曼滤波技术对水平失准角和方失准角进行估计。
[0288] 离散型卡尔曼滤波过程如下：
[0289]
[0290]
[0291]
[0292]
[0293] Pk＝(I-KkHk)Pk/k-1 (34)
[0294] ——状态一步预测；
[0295] ——状态估计；
[0296] Kk——滤波增益；
[0297] Pk/k-1——一步预测均方误差；
[0298] Pk——估计均方误差；
[0299] Hk——观测矩阵；
[0300] Zk——观测矢量；
[0301] Φk,k-1——一步转移矩阵；
[0302] Γk-1——系统噪声驱动矩阵。
[0303] 旋转调制型捷联惯导系统初始对准的误差状态方程：
[0304]
[0305]
[0306]
[0307]
[0308]
[0309] δVN——北向速度误差；
[0310] δVE——东向速度误差；
[0311] ——北向失准角；
[0312] ——东向失准角；
[0313] ——方位向失准角；
[0314] ωie——地球自转角速度；
[0315] g——当地重力加速度；
[0316] L——当地地理纬度；
[0317] R——为地球半径；
[0318] ——为加速度计的随机常值偏置；
[0319] ε——为陀螺仪随机常值漂移；
[0320] x,y,z——分别表示惯性元件坐标轴；
[0321] Cij——为 中所对应的元素；
[0322] W——为N(0,Q)的高斯白噪声。
[0323] 选取两个水平速度误差作为观测量，则观测方程为：
[0324] Z＝HX+V (40)
[0325] X——为状态矢量；
[0326] Z——为观测矢量；
[0327] H——为系统观测矩阵；
[0328] V——系统观测噪声。
[0329] 通过上述典型仿真实验结果可以看出采用本发明提出的初始对准方法，可以将常值漂移调制成周期性误差，再通过后续的滤波手段实现补偿。仿真结果表明，该方法能有效提高粗对准俯仰角精度，且利用粗对准得到的结果进行精对准，得到的估计失准角误差满足精度要求，该方法操作简单，工程性强。
[0330] 本发明书中未作详细描述的内容属于本领域专业技术人员公知的
现有技术。
