第4讲：圆锥误差(2-1)

( ) Φˆ (h)
=
N
∑θi
+
N −1
∑
N
∑
Kij
θi
×θ
j
i =1
i=1 j >i
式中， N 为子样数，θi 为陀螺输出的角增量。
（2）优化准则
对捷联惯导系统来说，锥运动是最恶劣的工作环境条件，它会诱发 数学平台的严重漂移，所以对旋转矢量算法作优化处理时常以锥运动作 为环境条件。这就是说，如果能确保锥运动环境条件下的算法漂移最小， 就一定能确保在其余环境条件下的算法漂移最小。
方程中存在三角函数，给实时计算带来困难；且当φ = 90o时，方 程中出现奇点，使θ 、γ 的解变得不确定，因而使其使用受到限制，不
能用于全姿态飞行器上。
（2）方向余弦阵
由参考系OXYZ 至载体系OX bYbZb的姿态矩阵为
C(γ ,φ,θ ) = Cy (θ )Cx (φ )Cz (γ )
第 1 次转动：
显然，通过增加对陀螺的采样次数 N ，可以提高旋转矢量的估算精
刚体在三维空间作任意转动时，其等效旋转矢量Φ(t)在i 轴的分量
i 与刚体运动角速度在 轴上的分量之间有如下关系式：
Φi = ∫tt0 ωidt + Ai
式中，Ai 为由于测量的不可交换性而造成的不可测量项。对圆锥误差的 补偿，实际上就是求 Ai的大小，旋转矢量法能够对圆锥误差进行有效的
补偿。
3.2 旋转矢量微分方程
圆锥误差与刚体有限转动产生的不可交换误差具有相同性质。换言 之，圆锥误差是在三维角振动环境下刚体有限转动产生的不可交换误 差。因此，可以在姿态算法中用一切解决不可交换性误差的方法来减少 圆锥误差的影响。
3．旋转矢量算法
3.1 旋转矢量的定义
由刚体定点转动的欧拉定 理，参考坐标系可通过绕欧拉 轴旋转特定的角度与固结于刚 体的动坐标系重合。设欧拉轴
实数 q0 为四元数的标量部分， q13为四元数的矢量部分。
q0
=
cos⎜⎛ ⎝
ϕ
2
⎟⎞ ⎠
q13
=
⎡ q1 ⎢⎢q2
⎤ ⎥ ⎥
=
e
sin⎜⎛ ⎝
ϕ
2
⎟⎞ ⎠
⎣⎢q3 ⎥⎦
式中，e 是表示旋转轴方向的单位向量，ϕ 是旋转角。
四个元素满足正交约束方程
q02 + q12 + q22 + q32 = 1
视在的测量误差，即圆锥误差。
以上所讨论的两个角振动的相位差为90°，当相位差为ϕ 时，同样
可以导出 xb轴的常值角速度为
ω xb
=
−2ψ
sin 2
α
2
⋅ sin ϕ
由于角振动速度的幅值、频率和相位一般是随机变量，由上式给出 的圆锥误差表达式不能用于实时的修正计算，只能用来说明圆锥误差的 存在和对误差量级的估计。
旋转矢量微分方程为计算捷联惯性系统的姿态矩阵建立了全面的 理论基础。
根据 Euler 理论，旋转矢量是姿态矩阵大小为+1 的特征值所对应的 特征向量，即
(C − I )Φ = 0
且有
C
=
I
+
sin φ φ
[Φ×]
+
(1
− cos
φ2
φ
)
[Φ×]2
[ ] 式中，Φ = Φ x Φ y Φ z T 表示旋转矢量，C 为机体坐标系与参考坐
标系之间的方向余弦矩阵， I 表示单位矩阵，φ = (ΦT Φ)1/ 2。
旋转矢量微分方程为
Φ&
=
ω
+
1 2
Φ
×
ω
+
1
φ2
⋅
⎢⎣⎡1
−
2
⋅
φ
(1
sin φ − cosφ
)
⎥⎦⎤Φ
×
(Φ
×
ω
)
[ ] 式中，ω = ωx ω y ωz T 为机体角速度向量，方程右边第二项与第
三项之和就是有限转动引起的不可交换项，即圆锥误差。
有相同时间间隔的两个角增量向量的叉乘对圆锥误差的贡献是相等的， 可以不考虑它们与绝对时间的关系。
利用这个性质，可以将圆锥误差补偿公式简化为
Φˆ
=
N
∑θi
i =1
+
⎜⎜⎝⎛
N −1
∑ biθi
i =1
⎟⎟⎠⎞ ×θ N
可以看出，简化后的算法计算量大大减小。
3.5 改进旋转矢量优化算法的一般形式
（1） 利用前一周期陀螺角增量输出的优化算法
依次绕不同坐标轴的三次连
3-1-2 欧拉角转动
续转动来定义。
姿态欧拉角的运动学方程：
⎡φ& ⎤ ⎢⎢θ&⎥⎥ ⎢⎣γ&⎥⎦
=
1
cosφ
⎡
⎢⎢ω
⎢⎣
y
(ωx cosθ + ωz sinθ )cosφ cosφ + (ωx sinθ − ωz cosθ )
ωz cosθ − ωx sinθ
sin
φ
⎤ ⎥ ⎥ ⎥⎦
况。
3.3 旋转矢量微分方程的求解
在实际应用中，为保证实时性并考虑运算方便，仅取前两项，得
Φ& = ω + 1 ⋅ Φ ×ω
2
在姿态更新周期h = tk −t k −1内，通过对陀螺的角增量进行等间隔
采样，可求得等效旋转矢量的估值。且根据等间隔采样次数，求解方法 分为单子样法、双子样法、三子样法与四子样法。其中，单子样法就是 四元数法。
3.4 简化形式的旋转矢量优化算法
假设在[t, t + h]间隔内，对陀螺输出信号进行 N 次采样，θi 表示第i次
采样的陀螺输出角增量信号，有
θi
=
∫
t +(i N )h t +[(i−1) N ]h
ω(τ
)dτ
i = 1,2,L, N
=
⎡ ⎢ ⎢ ⎢− ⎢
2
sin
α
⋅
− 2 ψh ⋅ sin2 α
第二部分
高动态环境下捷联惯导系统 姿态算法研究
1．引言
在常规的捷联惯性导航 系统的姿态算法中，总是认为 载体坐标系和参考坐标系间 的转换是通过一系列的转动 来实现的，而这些转动间的次 序并不重要，这是基于无限小 转动是矢量的原理来得到的。
（1）欧拉角
由参考系OXYZ 至载体 系OX bYbZb的变换可以通过
记tk −1时刻导航系至机体系的姿态四元数为Q(tk −1)，tk 时刻导航系 至机体系的姿态四元数为Q(tk )。
设在姿态更新周期h = tk − tk −1内，机体系的姿态变化四元数为
q(h)，则姿态更新如下 Q(tk ) = Q(tk −1) ⊗ q(h)
且
q(h) = cos⎜⎝⎛φ(2h)⎟⎠⎞ + Φφ((hh))sin⎜⎝⎛φ(2h)⎟⎠⎞
方向余弦矩阵的微分方程为：
C& = CΩ
式中
⎡0
Ω
=
⎢ ⎢
ωz
− ωz
0
ωy − ωx
⎤ ⎥ ⎥
⎣⎢− ωy ωx 0 ⎥⎦
方程的解为：
Ck +1 = Ck exp ∫ttkk+1 Ωdt
（3）四元数
四元数是具有四个元素的超复数，它可以描述一个坐标系或一个矢
量相对于某一坐标系的旋转
q
=
⎡ q0 ⎤ ⎣⎢q13 ⎥⎦
上的单位矢量为nr ，旋转角度为
φ ，则旋转矢量定义为
Φ = φ ⋅ nr
[ ] = Φx Φ y Φz T ，
旋转矢量示意图
当刚体的定轴转动用旋转矢量表示为如下时，刚体的运动可称为圆 锥运动：
⎡0⎤
Φ = ⎢⎢α cos(ψt)⎥⎥ ⎣⎢α sin(ψt) ⎥⎦
其中α 表示圆锥运动的锥半角，ψ 表示圆锥运动的角频率。
根据欧拉旋转定理，飞行器的转动从任一给定方位到任一其它方位
可通过连续绕瞬时轴转动获得，而瞬时角速度方向在空间不断地改变，
对一个在空间方向随时间变换的角速度矢量进行积分是无意义的。
当不是定轴转动时，即ω(t )的方向在空间变化时，式
∆θ
=
∫
t t
+
∆h
ω(t)dt
是不成立的，即角改变量不是矢量。因此，在采用角速度矢量积分时，
⎝2N⎠ ⎝ 2N ⎠
⎤ ⎥ ⎥
⎢⎢A⎩⎨⎧cos⎢⎣⎡ψ⎜⎝⎛t + 22iN−1h⎟⎠⎞⎥⎦⎤ −cos⎢⎣⎡ψ⎜⎝⎛t + 22jN−1h⎟⎠⎞⎥⎦⎤⎭⎬⎫⎥⎥
⎢ ⎢ ⎢⎣
A⎩⎨⎧sin⎢⎣⎡ψ⎜⎝⎛t
+
22i N−1h⎟⎠⎞⎥⎦⎤
−
sin⎢⎣⎡ψ⎜⎝⎛t
+
22jN−1h⎟⎠⎞⎥⎦⎤⎭⎬⎫⎥⎥⎥⎦
但在实际情况中，载体运动的角速度ω 的方向是不断变化的，从而导 致某一时刻角速度ω 与当前时刻所积累的旋转矢量Φ方向不一致，上式 右边后两项不为零，倘若仍然直接采用角速度ω 直接进行积分解算就会
带来刚体转动的不可交换性误差。
而且，当载体作锥运动时，旋转矢量Φ和角速度ω 相互垂直，此时上
式右边的后两项最大，所以锥运动反映了不可交换误差最为恶劣的情
取围绕惯性坐标系 x轴旋转的圆锥运动：
⎡0⎤
Φ = ⎢⎢α cos(ψt)⎥⎥ ⎣⎢α sin(ψt) ⎥⎦
旋转矢量算法的优化设计的误差准则是使误差
达到最小。
Φε = Φx − Φˆ x
（3）姿态更新
在捷联系统的实际姿态解算中，为了补偿由于有限转动造成的不可 交换性误差，采用旋转矢量算法利用角增量计算姿态更新周期内的姿态 变化量，但进行姿态更新时是采用四元数实现。