捷联惯导静基座初始对准技术仿真

Ztbb Z~ibb (Zibe Zebt ) Zcb Z~ibb H b Zibb Zibe Zebt GZibe GZebt
（4） （5）
式中，Cbp —要计算的捷联矩阵；
Z~ibb —陀螺仪对载体相对惯性空间运动角速度量 测值；
εb —陀螺漂移；
Zibb —载体摇摆产生的干扰角速度在载体系上的 分量；
» »
««Zie cos L»» ；
«¬R»¼ «¬Zie sin L »¼
ª0 g 0º
F
« ¬
g
0
0»¼ 。
选取东向速度误差 δVE 和北向速度误差 δVN 做为 外观测量，则可以建立模型量测方程为：
Z(t)=HX(t)+v(t)
（22）
式中，v(t)—量测方程的随机噪声向量，v(t)=[vE vN]T， 服从零均值高斯正态分布的量测噪声向量，即 v(t)~N(0,R)
（23）
xˆ(k 1k) I(k 1)xˆ(k)
（24）
K (k 1) P(k 1k)H T (k 1)[H (k 1) P(k 1k)H T (k 1) R(k 1)]1
（25）
P(k 1k) I(k 1, k)P(k)IT (k 1, k) * (k 1, k)Q(k)* T (k 1, k)
Cbt 0 3u3
» » » »
«¬ 03u2 03u3 03u2 03u3 »¼
（21）
式中，A'
ª0 «¬ K
K 0
º » ¼
；
K=2ωiesinL；
S
ªC11 «¬C 21
C12 C 22
º » ¼
；
ª 0 R Tº
A
«« R 0
P
» »
；
«¬ T P 0 »¼
ªPº ª 0 º
««T
设计初始对准方案所采用的方法通常分为两大类： 一类是基于经典控制理论的对准方法，称为频域法或 经典法；另一类是基于现代控制理论的状态空间法， 也称最优估计方法或卡尔曼滤波方法。随着初始对准 技术的研究，对准方案也由原来的频域法逐渐向基于 卡尔曼滤波的最优估计方法转变。用卡尔曼滤波方法 对惯导系统进行估计时，对可观状态能够得到很好的 估计。另外，惯导系统初始对准的研究是在惯导系统 误差分析的基础上进行的，因此，要弄清惯导系统初 始对准原理，必须从惯导误差方程着手，在此基础上， 再讨论初始对准原理。
（19）
w(t)~N(0,Q)
（20）
其中，B—单位矩阵，矩阵的维数与 A 相同 [5]。
由式（1）、（13）、（16）、（17）可得静基座
捷联惯导系统的误差模型的状态方程中的矩阵 A 为：
ª A'2u2 F2u3 S 0 2u3 º
A
«
« 03u2
« «
0 2u2
A 3u3 0 2u3
0 3u2 0 2u3
关键词：捷联惯导；静基座；初始对准；卡尔曼滤波
中图分类号：U666.1
文献标识码：A
文章编号：1006-883X（2014）08-0027-04
收稿日期：2014-05-08
捷联惯导静基座初始对准技术仿真
郭勇
中北大学仪器与电子学院 山西太原 030051
一、引言
捷联 惯 导 系 统（Strapdown Inertial Navigation System，简称 SINS）的导航解算的关键问题就 是在导航积分开始时获得姿态、速度和位置变量的初 始值，这就是初始对准问题。时间和精度是对准技术 主要的技术指标，在尽可能短的时间内达到最高的对 准精度，是捷联惯导初始对准技术追求的目标。
（12）
在静基座初始对准的过程中，纬度误差 δL、天向
速度分量误差 δVU 变化很小，可以忽略不计 [4]，所以 式（12）可以变换为：
°¯°®­GGVVEN
f NMU fUMN 2Zie sin LGVN /E fUME fEMU 2Zie sin LGVE /N
（13）
H －常值矩阵，H >I 0@。
静基座捷联惯导系统初始对准误差模型的状态方
程和量测方程分别为式（18）和（22）所示。
五、基于卡尔曼滤波的静基座初始对准仿真
根据上述系统方程和观测方程，可建立离散卡尔
曼滤波方程如下：
xˆ(k 1) xˆ(k 1k) K(k 1)[z(k 1) H (k 1)xˆ(k)
实际的地理系和理想的地理系之间存在失准角，造成
加速度计测量的比力经过捷联矩阵转换后在地理坐标
系的水平方向有分量。初始对准就是应用这个信息来
不断地估计水平失准角 φE，φN 的 [2]。当数学平台有方 位失准角 φU 时，则地球自转角速率分量 ωiecosLsinφU 就耦合到解析平台保持水平，必须在东西轴上加一个
取 Zc
«ª ¬
GVN R
GVE R
ºT 0»
，式（14）可表示为：
¼
°­M E ° °®MN °
GVN R
HE
ZieMN sin L ZieMU cos L
GVE R
HN
ZieME sin L
（15）
°°¯MU HU ZieME cos L
式（13）和（15）就是静基座初始对准速度和失 准角误差方程。陀螺的刻度系数设为 δKgi (i=x, y, z)， 陀螺的常值漂移、随机漂移和白噪声误差分别为 εbi， εri，Wgi(i=x, y, z)，载体的角速度为 ωb=[ωx ωy ωz]T，则 陀螺在地理坐标系内的等效漂移为：
­M °®M
E N
°¯MU
ZcE H E ZieM N sin L ZieMU cos L
ZcN H N ZieM E sin L ZcU HU ZieM E cos L
（14）
28
传感器世界 2014.08
Vol.20 NO.08 Total 230
Technology & Application 技术与应用
控制信号，从而使数学解析平台稳定在地理坐标系上。
其稳定过程是如下数学公式进行的： Cbp Cbp:btb
（1）
ª «
0
ZibbU
ZibbN
º »
:
b tb
« «
ZibbU
0

ZibbE
» »
«¬ ZibbU ZibbE
0 »¼
（2）
> @ Zibb ZibbE ZibbN ZibbU T
（3）
ªH ««H
E N
º » »
«¬HU »¼
Cbt
ªH «
bx
«H by
H rx H ry
GK gxZx GK gyZ y
Wgx Wgy
º » »
««¬H bz
H rz
GK gzZz
Wgz
» »¼
（16）
同理，设 fi (i=x, y, z) 为载体的理想比力在载体坐
计和陀螺仪的量测值，产生修正角速度 ωc 以供捷联矩 阵的更新，并不断减小失准角使之趋于 0[3]。
四、静基座初始对准模型的建立 惯性导航的速度和姿态方程为：
GV )f t (2Zite Zett ) u GV (2GZie Zet ) uV t g t /t
（6）
M Zc H t )Zt
Technology & Application 技术与应用
摘要：捷联惯导在初始对准时，按载体的运行状态来分，可以分为静基座和动基座对准。从静基座捷联
惯导初始对准的原理出发，推导了捷联惯性导航系统静基座初始对准的误差动态方程和量测方程，构成
了卡尔曼滤波模型。最后将卡尔曼滤波模型应用于静基座初始对准，并进行了仿真。
Zibe —定义为C bpZipe ； Zebt —定义为C bpZept ； Zcb —加速度计和陀螺仪的量测值经处理后形成 的投影；
GZibe —偏开理想值Zibe 的偏差，由地理纬度误差 引起的；
GZebt —偏开理想值Zebt 的偏差，由速度引起的。 捷联惯导初始对准的基本思路是，通过对加速度
GVE f N MU fU M N 2Zie sin LGVN
2Zie cos LGVU /E GVN fU M E f EMU 2Zie sin LGVE /N GVU f EM N f N M E 2Zie cos LGVE /u
（11）
ME ZcE H E ZieMN sin L ZieMU cos L MN ZcN H N ZieGL sin L ZieME sin L MU ZcU HU ZieGL cos L ZieME cos L
（26）
P(k 1) (Imx K (k 1)H (k 1))P(k 1k) （27） 其中，x ∈ Rn—系统的状态向量；
z ∈ Rm—系统的观测向量； φ (k+1,k) ∈ Rn×n —离散化的状态转移矩阵（系统
矩阵）；
传感器世界 2014.08
Vol.20 NO.08 Total 230
» ¼
取状态变量：
（17）
X
[GVE
GVN
M E
M N
M U
/E
/N
HE
HN
HU ]T
这样就可以建立系统对准误差的动态方程如下：
X (t) AX (t) Bw(t)
（18）
式中，B—单位矩阵，矩阵的维数与 A 相同；
w(t)—状态噪声，
w(t) [W/E W/N WME WMN WMU 0 0 0 0 0]T 它服从于零均值高斯正态分布，即有：
二、捷联惯导主要误差来源分析 影响惯导系统性能的误差因素，大致可以分为以
下几种：元件误差、安装误差、初始条件误差、计算误差、
原理误差和外干扰误差 [1]。理论和实践证明，对惯导 系统的工作性能影响较大的还是元件误差、安装误差 和初始条件误差。以下针对这三种误差源进行分析。
（1）元件误差 主要是指来自于陀螺产生的漂移和加速度计的零 位偏差以及两者的刻度因数误差。 （2）安装误差 在理性情况下，捷联惯导的加速度计三个敏感轴 要严格保持相互垂直；而陀螺仪的三个敏感轴不仅要 求相互垂直，并且应当分别与加速度计的敏感轴保持 平行。但是实际上，在惯性元件安装过程中，总会出 现一定的偏差，这种偏差就是安装误差。 （3）初始条件误差 在惯导系统进入导航工作之前，给计算机引入初 始经、纬度的操作误差 Δλ0 和 Δφ0；以及系统初始对准 后，接入舒勒回路一瞬间平台所具有的水平姿态误差 M x0 、M y0和方位误差Mz0 。
三、初始对准的原理 捷联系统初始对准首先利用外部信息或由惯性测
量组件提供的信息粗略地确定出捷联矩阵。由于没有 考虑到惯性测量元件的测量误差等的影响，这时给出
传感器世界 2014.08
Vol.20 NO.08 Total 230
27
技术与应用 Technology & Application
的捷联矩阵与真实的捷联矩阵之间存在误差，相应于
29
技术与应用 Technology & Application
Q—系统噪声协方差矩阵； R—观测噪声的协方差矩阵； Γ ∈ Rn×q—系统噪声阵； H ∈ Rm×n—系统观测阵。 具体每项求法可参考文献 [3]。 选 取 状 态 矢 量 X 的 初 始 值 X(0) 均 为 0，P(0)、 Q、R 均取中等精度陀螺的对应值，初始失准角 φN、 φE、φU 均取 1°，陀螺常值漂移为 0.02°/h，随机漂移为 0.01°/h；加速度初始偏差均取为 1×10-4g；随机偏差 为 0.5×10-4g；惯导系统所处位置的地理纬度 L=37°。 则 P(0)、Q、R 可表示为： P(0)=diag{(0.1m/s)2,(0.1m/s)2,(1°)2,(1°)2, (1°)2, (100μg)2, (0.02°/h)2, (0.02°/h)2, (0.02°/h)2}； Q=diag{(50μg)2,(50μg)2,(0.1°/h)2,(0.1°/h)2,(0.1°/h)2, 0,0,0,0,0}； R={(0.1m/s)2, (0.1m/s)2}； 通过 Matlab 仿真得到三个失准角误差如图 1 所示。 在以上初始条件下，使用 Matlab 软件对三个失准 角误差做出了仿真，仿真时间为 300s。仿真结果验证 了卡尔曼滤波在捷联惯导系统静基座对准过程中可以 进行很好的滤波，三个失准角经过卡尔曼滤波在较短 时间内都基本收敛至较高精度，达到了捷联惯导系统 初始对准时间和精度要求。
（7）
其中，φ —平台系相对于地理系的误差角，φ=[φE φN
φU]T；
ωc—对准修正加速度在载体系上的投影；
gt—重力加速度误差在地理系上的投影；
f t—比力误差在地理系上的投影；
εt —陀螺仪误差在地理系上的投影；
Λt —加速度计误差在地理系上的投影；
Φ —φ 的反对称矩阵
ª 0 MU MN º
标系内的投影，Λbi，δKai ，Wai (i=x, y, z) 分别为加速度
计偏置量、刻度系数误差及白噪声误差。则加速度计
的等效偏置为：
««ª//EN
º » »
«¬/U »¼
Cbt
««ª//bbyx
GKax GKay
fx fy
Wax wenku.baidu.comWay
º » »
«¬/bz
GKaz
fz
Waz
)
« «
MU
0
ME
» »
«¬ MN ME 0 »¼
（8）
由于是静基座，VE = VN = VU = 0，所以： ª 0º
Zite ««Zie cos L»» «¬Zie sin L »¼
（9）
Zett >0 0 0@T
（10）
将式（9）和式（10）带回式（6）和式（7）得速
度和姿态误差方程，展开如下：