(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 时输出
修正后 , 双子样法的角增量由以下公式 角速度输出的陀螺 , 在圆锥运动下进行了数
提取
字仿真 。 不同算法的时间复杂度取捷联矩
Δθ1 =[ 8ω(t -T2/ 2)-ω(t)+5ω(t +T2/ 阵一次递推所需运算次数 , 如下表所示
表 1 不同算法的时间复杂度
算法
加减法 乘除法 开根号
算法一 四元数四阶龙格 ——— 库塔法
系 , 可得出捷联矩阵 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
(10)
Δ Δθ3 =[ ω(t +2T3/ 3)+ω(t +T3)] ·T3/6 +Δ
式中 ,ζ=0 .8 , 目的是为了减小漂移误差 。 若认为在式(5)中 , 一个计算周期内 ω
=a +b·t , 并作一定的近似 , 可推导出等效 旋转矢量双子样计算公式为
Δ =[ 2ω(t +T3/ 3)-ω(t)-ω(t +2T3/3)] ·
(2)算法二 ～ 五比算法一精度有了明显 提高 , 这是由于算法二 ～ 五(下接第 12 页)
12
动调陀螺再平衡回路的设计
G(s)=k
pkTG1(s) As2
(14)
G2(s)=ωsnG1(s)
航姿算法是用来实时计算飞行器的捷 联矩阵的算法 , 它是飞行器姿态控制中的计 算机程序核心 。 由于捷联矩阵的计算精度 将直接影响航姿角的提取和导航计算的精 度 , 所以提出一种高精度的航姿算法就显得 很有必要 。圆锥运动是最恶劣的环境 , 最能 体现算法的精度高低 , 因此 , 在本文中采用 圆锥运动作为输入进行仿真 。
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
η1 =736/ 945 , η2 =334/945 , η3 =526/
的角增量 。
945 ,η4 =654/945 。
3 .2 旋转矢量法
当采用角速度输出陀螺 , 旋转矢量法中
旋转 矢 量法 是 波 尔兹 (Bortz)和 约 当 的角增量可由以下公式提取
(Jordan)用 旋转矢量 的概念构 造的一 种算
20ms , T5 =10ms .
当采用输出为角速度形式的陀螺 , α=
因为运算结果呈周期性变化 , 所以仿真 1°, Χ=1rad/ sec 时 , 五种算法的漂移误差随
计算时间取 Time =36s , 漂移误差取仿真运 时间变化图如下所示
图 1 算法一漂移误差随时间变化图 图 2 算法二漂移误差随时间变化图 图 3 算法三漂移误差随时间变化图 表 3 采用输出为角速 度形式的陀螺 ;α=1°, Χ=1rad/ sec 。
四元数乘法 , 即有
对于三子样法
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
为了在相同的采样周期下进行比较 , 仿
自动化技术与应用 Vol .19 ,No .1 2000 年第 1 期 9
真中五种算法的计算周期分别取
算中的最大值 ;i , j , k 分别表示漂移方向在
T1 =10ms , T2 =10ms , T3 =15ms , T4 = x ,y , z 方向上的方向余弦 。
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τ。
6
捷联系统航姿算法分析
捷联系统航姿算法分析
徐丽娜 谢 青 (哈尔滨工业大学)
[ 摘要] 本文对几种 捷联系 统航姿算 法进 行了分
根据转动四元数与转动方向余弦的关
析和比较 , 提出了在采用角速率输出陀螺的情 况下 的一种高精度算法 , 并给出了在圆锥运动环境 下的 仿真结果 。 关键词 惯性导航 捷联系统 航姿算法
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)为基础导出四阶龙格 —
式中 , T4 为四 子样法计算周期 ;Δ 为 修正 24
(17)
项。
式中 , ω(t -T2/ 2)为上一计算周期中角速度
当采用输出为角速度形式的陀螺时 , 我 采样值 。
们可以利用前一时刻的角速度采样值对旋 转矢量双子样法中的角增量进行修正 , 由此
4 数字仿真
提出双子样改进算法 。
我们用 MATLAB 编写了仿真程序 , 采用
(3)
2 .2 旋转矢量 旋转矢量表示为 :Υ=[ x , y , z] T , 它可 唯一确定飞行器在 t 时刻的位置 , 即以 Υ(t)
为轴 , 转动角度大小等于旋转 矢量 Υ(t)的 幅值 。当以旋转矢量描述飞行器姿态时 , 旋
转矢量微分方程为
·
Υ= ω +
1 2
Υ ×ω+
1
2
·[
1
-
0
2 航姿算法理论
漂移误差(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
对于双子样法
法 , 目的是用来补偿在数值积分法中对角速
Δθ1 =[ ω(t)+ω(t +T2/ 2)] ·T2/ 4 ;
度矢量积分造成的不可交换误差[ 2 , 3, 4, 5] .以
Δθ2 =[ ω(t +T2/ 2)+ω(t +T2)] ·T2/4
式(6)为基础建立起以旋转矢量来矫正四元
(14)
数递推算子的航姿算法 。 式(6)中的乘法为 式中 , T2 为双子样法计算周期 。
0sin 0 2·(1 -cos
0)] Υ×(Υ×ω)
(4)
2 .1 转动四元数
式(4)可近似表达为
所谓转动四元数 , 是指由一个实数单位 1 和三个虚数单位 i 、j 、k 组成并具有下列实
Υ· =ω+12 Υ×ω+112·Υ×(Υ×ω) (5)
元的数
用旋转矢量来修正四元数的递推公式
Q =q0 +q1i +q2j +q2k
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
ωz ΧsinacosΧt
由旋转矢量和转动四元数的定义可知
刚体绕定点作圆锥运动用四元数可表示为