捷联系统航姿算法分析

为
Υ=Δθ+0 .45·(Δθ2 ×Δθ3)+0 .675·Δθ2
×(Δθ3 -Δθ1)
(12)
式中
,
n
Δθ= i
∑=1Δθi ;Δθi =
∫tt
+i·T/ n +(i-1)·T/
nω(t
+τ)
dτ;n =3 。
若认为在一个计算周期内 ω=a +b·t +
c·t2 +dt3 , 可导出高效旋转矢量四子样计算
0 .9045
0 .0101
-0 .08 54
算法五
2 .2733e -008
-0 .9767
-0 .0034
0 .0199
表 4 采用输出为角速 度形式的陀螺 ,α=2°, Χ=1rad/ sec 。
漂移误差(rad)
i
j
k
算法一
5 .8166e -005
0 .0791
-1 .8987e -005
修正后 , 双子样法的角增量由以下公式 角速度输出的陀螺 , 在圆锥运动下进行了数
提取
字仿真 。 不同算法的时间复杂度取捷联矩
Δθ1 =[ 8ω(t -T2/ 2)-ω(t)+5ω(t +T2/ 阵一次递推所需运算次数 , 如下表所示
表 1 不同算法的时间复杂度
算法
加减法 乘除法 开根号
算法一 四元数四阶龙格 ——— 库塔法
η1 =736/ 945 , η2 =334/945 , η3 =526/
的角增量 。
945 ,η4 =654/945 。
3 .2 旋转矢量法
当采用角速度输出陀螺 , 旋转矢量法中
旋转 矢 量法 是 波 尔兹 (Bortz)和 约 当 的角增量可由以下公式提取
(Jordan)用 旋转矢量 的概念构 造的一 种算
漂移误差(rad)
i
j
k
算法一
1 .4557e -004
0 .9721
0 .0071
-0 .02 08
算法二
3 .3731e -006
0 .0012
-0 .9161
0 .0828
算法三
2 .2726e -008
-0 .9568
-5 .6152e -004
0 .0427
算法四
4 .5505e -008
四元数乘法 , 即有
对于三子样法
Q(T +h)={d1·I +d2·[ Δ ] }Q(h) (9)
Δθ1 =[ ω(t)+ω(t +T3/ 3)] ·T3/ 6 +Δ
d1 、d2 取四阶近似
Δθ2 =[ ω(t +T3/ 3)+ω(t +2T3/ 3)] ·T3/ 6 +
2
4
2
d1 ≈1 -80 +ζ·3804 , d2 ≈0 .5 -408
(13)
ω(t +T1/ 2)=(Δθ1 +Δθ2)/T1 ω(t +T1)=(3Δθ2 -Δθ1)/T1
(8)
式中
,
n
Δθ= i
∑=1Δθi ,
Δθi =
∫tt
+i·T/ n +(i-1)·T/
nω(t
+τ)
式中 , Δθ1 为陀螺从 t 到 t +T1/2 时输出的角 dτ;n =4 ;
增量 ;Δθ2 为陀螺从 t +T1/ 2 到 t +T1 时输出
-4 .7489e -006
0 .0791
算法二
7 .5943e -008
0 .9103
-0 .0099
0 .0798
算法三
1 .8180e -010
0 .1192
9.4903e -006
0 .8808
算法四
3 .6359e -010
-0 .7100
-1 .2802e -004
-0 .28 99
算法五
速度输出 。
库塔法的四元数递推公式为[ 8]
自动化技术与应用 Vol .19 ,No .1 2000 年第 1 期 7
q(t +T1)=q(t)+(k1 +k2 +k3 +k4)/ 6
若认为在一个计算周期内 ω=a +b·t +
(7) c·t2 , 可导出等效旋转矢量三子样计算公式
0 =[ Υ·Υ] 1/2 。
b
=
1 2
·
ωx ωy
0 -ωz
ωz 0
-ωy 是
ωx
3 航姿算法实现
ωz ωy -ωx 0
3 .1 四元数数值积分法
飞行器坐标系相对定坐标系的旋转角速度
数值积分法通常最常用的是四阶龙格
的斜对称矩阵 ;ω=[ ωx ωy ωz] T 是陀螺角 一库塔法 。以式(2)为基础导出四阶龙格 —
对于双子样法
法 , 目的是用来补偿在数值积分法中对角速
Δθ1 =[ ω(t)+ω(t +T2/ 2)] ·T2/ 4 ;
度矢量积分造成的不可交换误差[ 2 , 3, 4, 5] .以
Δθ2 =[ ω(t +T2/ 2)+ω(t +T2)] ·T2/4
式(6)为基础建立起以旋转矢量来矫正四元
(14)
数递推算子的航姿算法 。 式(6)中的乘法为 式中 , T2 为双子样法计算周期 。
系 , 可得出捷联矩阵 C 的公式
C= q20 +q21 -q22 -q23 2(q1·q2 -q0·q3) 2(q1·q3 +q0·q2) 2(q1·q2 +q0·q3) q20 -q21 +q22 -q23 2(q2·q3 -q0·q1)
1 引言
2(q1·q3 -q0·q2) 2(q2·q3 +q0·q1) q20 -q21 -q22 +q23
航姿算法是用来实时计算飞行器的捷 联矩阵的算法 , 它是飞行器姿态控制中的计 算机程序核心 。 由于捷联矩阵的计算精度 将直接影响航姿角的提取和导航计算的精 度 , 所以提出一种高精度的航姿算法就显得 很有必要 。圆锥运动是最恶劣的环境 , 最能 体现算法的精度高低 , 因此 , 在本文中采用 圆锥运动作为输入进行仿真 。
20ms , T5 =10ms .
当采用输出为角速度形式的陀螺 , α=
因为运算结果呈周期性变化 , 所以仿真 1°, Χ=1rad/ sec 时 , 五种算法的漂移误差随
计算时间取 Time =36s , 漂移误差取仿真运 时间变化图如下所示
图 1 算法一漂移误差随时间变化图 图 2 算法二漂移误差随时间变化图 图 3 算法三漂移误差随时间变化图 表 3 采用输出为角速 度形式的陀螺 ;α=1°, Χ=1rad/ sec 。
为了在相同的采样周期下进行比较 , 仿
自动化技术与应用 Vol .19 ,No .1 2000 年第 1 期 9
真中五种算法的计算周期分别取
算中的最大值 ;i , j , k 分别表示漂移方向在
T1 =10ms , T2 =10ms , T3 =15ms , T4 = x ,y , z 方向上的方向余弦 。
1 .8180e -010
0 .9205
5.4184e -006
-0 .07 95
4 .1 圆锥运动及仿真结果
圆锥运动是一种较为恶劣的飞行情况 ,
在这种情况下对角速度矢量积分造成的刚
体有限转动不可交换性误差表现的最为突
出。
取圆锥运动 :
ωx
-2 Χsin2(a/ 2)
ωy = -ΧsinasinΧt ,
(3)
2 .2 旋转矢量 旋转矢量表示为 :Υ=[ x , y , z] T , 它可 唯一确定飞行器在 t 时刻的位置 , 即以 Υ(t)
为轴 , 转动角度大小等于旋转 矢量 Υ(t)的 幅值 。当以旋转矢量描述飞行器姿态时 , 旋
转矢量微分方程为
·
Υ= ω +
1 2
Υ ×ω+
1
2
·[
1
-
0
2 航姿算法理论
0sin 0 2·(1 -cos
0)] Υ×(Υ×ω)
(4)
2 .1 转动四元数
式(4)可近似表达为
所谓转动四元数 , 是指由一个实数单位 1 和三个虚数单位 i 、j 、k 组成并具有下列实
Υ· =ω+12 Υ×ω+112·Υ×(Υ×ω) (5)
元的数
用旋转矢量来修正四元数的递推公式
Q =q0 +q1i +q2j +q2k
T3/ 36
(15)
式中 , T3 为三子样法计算 周期 ;Δ 为修正
项。
Υ=Δθ1 +Δθ2 +23 Δθ1 ×Δθ2
(11)
对于四子样法
式中
,
Δθ1
=∫tt
+T/
2ω(t +τ)dτ;Δθ2 =
∫tt
+T +T/
2ω
Δθi =[ ω(t +(i -1)·T4/4)+ω(t +i·T4/
(t +τ)dτ。
(10)
Δ Δθ3 =[ ω(t +2T3/ 3)+ω(t +T3)] ·T3/6 +Δ
式中 ,ζ=0 .8 , 目的是为了减小漂移误差 。 若认为在式(5)中 , 一个计算周期内 ω
=a +b·t , 并作一定的近似 , 可推导出等效 旋转矢量双子样计算公式为
Δ =[ 2ω(t +T3/ 3)-ω(t)-ω(t +2T3/3)] ·
-0 .0794
2.1669e -005
0 .9205
图 4 算法四漂移误差随时间变化图 图 5 算法五漂移误差随时间变化图
4 .2 算法分析
(1)随着圆锥角 α的增大和频率的提高 , 五 种算法的漂移误差均有增大的趋势 , 并且漂 移方向主要发生 在 X 轴方向 , 这是因为选 择绕 X 轴旋转的圆锥运动做仿真的结果 。
式中 ,T1 为计算周期 ; k1 =0 .5·T1[ ωb(t)] ·q(t); k2 =0 .5·T1[ ωb(t +T1/ 2)] ·(q(t)+k1/
2); k3 =0 .5·T1[ ωb(t +T1/ 2)] ·(q(t)+k2/
2); k4 =0 .5·T1[ ωb(t +T1)] ·(q(t)+k3)。
-0 .92 09
算法二
1 .6976e -007
0 .8834
-0 .0388
0 .0777
算法三
3 .6354e -010
0 .1212
5.4653e -005
0 .8787
算法四
7 .2708e -010
-0 .7097
-5 .1211e -004
-0 .28 97
算法五
3 .6354e -010
6
捷联系统航姿算法分析
捷联系统航姿算法分析
徐丽娜 谢 青 (哈尔滨工业大学)
[ 摘要] 本文对几种 捷联系 统航姿算 法进 行了分
根据转动四元数与转动方向余弦的关
析和比较 , 提出了在采用角速率输出陀螺的情 况下 的一种高精度算法 , 并给出了在圆锥运动环境 下的 仿真结果 。 关键词 惯性导航 捷联系统 航姿算法
式中 , T4 为四 子样法计算周期 ;Δ 为 修正 24
(17)
项。
式中 , ω(t -T2/ 2)为上一计算周期中角速度
当采用输出为角速度形式的陀螺时 , 我 采样值 。
们可以利用前一时刻的角速度采样值对旋 转矢量双子样法中的角增量进行修正 , 由此
4 数字仿真
提出双子样改进算法 。
我们用 MATLAB 编写了仿真程序 , 采用
将式(7)代入式(3)可得捷联矩阵 C 。
公式为[ 5]
当采用输出为角增量形式的陀螺 , 此法
Υ=Δθ+η1·(Δθ1ຫໍສະໝຸດ Baidu×Δθ2 +Δθ3 ×Δθ4)+η2
中的角速度由以下公式提取[ 1]
·(Δθ1 ×Δθ3 +Δθ2 ×Δθ4)+η3·(Δθ1 ×Δθ4)+
ω(t)=(3Δθ1 -Δθ2)/T1
η4·(Δθ2 ×Δθ3)
ωz ΧsinacosΧt
由旋转矢量和转动四元数的定义可知
刚体绕定点作圆锥运动用四元数可表示为
Q =[ cos(α2 )0 sin(α2 )·cos(Χt) sin (α2 )sin(Χt)] T 式中 , α为圆锥角 , Χ为圆锥运动角频率 。
在仿真中 , t 时刻载体的真实位置可用 上述四元数来表示 。
(1) 为
当用四元数描述飞行器姿态时 , 四元数微分
Q(T +h)=Q(T)＊q(h)
(6)
方程式的表达式为
式中 , q(h)=[ d1 d2· x d2· y d2· z] T ;
·
d1 =cos(0/2);
Q = bQ 式中 ,
(2)
d2 =sin(0/ 2);
0
0 -ωx -ωy -ωz
(2)算法二 ～ 五比算法一精度有了明显 提高 , 这是由于算法二 ～ 五(下接第 12 页)
12
动调陀螺再平衡回路的设计
G(s)=k
pkTG1(s) As2
(14)
G2(s)=ωsnG1(s)
4)] ·T4/ 8 +Δ (i =1 , 2 , 3 , 4)
8
捷联系统航姿算法分析
Δ =[ 3ω(t)-11ω(t +T4/ 4)+13ω(t +T4/ 2) 2)] ·T2/ 24
-5ω(t +3T4/ 4)] ·T4/ 96
Δθ2 =[ 8ω(t)-ω(t +T2/ 2)+5ω(t +T2)] ·T2/
82
106
1
算法二 旋转矢量双子样法
70
98
2
算法三 旋转矢量三子样法
94
126
2
算法四 旋转矢量四子样法
124
192
2
算法五 旋转矢量双子样改进算法
76
110
2
表 2 采用输出为角速 度形式的陀螺 ,α=1°, Χ=1rad/ sec 。
漂移误差(rad)
i
j
k
算法一
2 .9087e -005
-0 .9209