当前位置：文档之家 › 基于陀螺和四元数的EKF卫星姿态确定算法

基于陀螺和四元数的EKF卫星姿态确定算法

  • 格式：pdf
  • 大小：259.80 KB
  • 文档页数：6

下载文档原格式

  / 6

合集下载

基于姿态参数切换的四元数快速传递对准算法

基于姿态参数切换的四元数快速传递对准算法

基于姿态参数切换的四元数快速传递对准算法周卫东;吉宇人;乔相伟【摘要】In order to solve the problem of large misalignment angle in rapid transfer alignment, a quaternion unscented filter algorithm making the use of transformation of attitude parameters is investigated based on building nonlinear quaternion error model. Firstly, the transform relation between quaternion and modified Rodriguez parameters was utilized to derive the quaternion formula of weighted sum, which made the calculation more directly. Then the normalization problem of quaternion part in disturb sigma points was solved by the method which obtained the vector part by calculating the sigma points firstly, and got the scalar part according to the normalizing condition afterwards; meanwhile, the error variance matrix containing the attitude quaternion was derived and corresponding formulas were presented on this basis. The simulation experiment shows that the estimation accuracy and rapidity of this algorithm for large misalignment angle can be satisfied for transfer alignment.%针对快速传递对准过程中的大失准角情况,在建立非线性四元数误差模型的基础上,对利用姿态参数转换的四元数无迹卡尔曼滤波(QUKF)进行研究.通过四元数和修正罗德里格斯参数间的相互转换关系对四元数加权求和公式进行推导,使计算更为直观.针对扰动sigma点中四元数部分的规范性问题,通过先计算sigma 点得到其向量部分,再利用单位化约束得到标量部分的方法进行解决,并在此基础上对含有姿态四元数的误差方差阵进行推导并给出相应公式.仿真结果表明,该算法对大失准角的估计准确度和快速性可以满足传递对准的要求.【期刊名称】《电机与控制学报》【年(卷),期】2012(016)012【总页数】6页(P72-77)【关键词】快速传递对准;修正罗德里格斯参数;四元数;无迹卡尔曼滤波;规范性【作者】周卫东;吉宇人;乔相伟【作者单位】哈尔滨工程大学自动化学院,黑龙江哈尔滨150001;哈尔滨工程大学自动化学院,黑龙江哈尔滨150001;哈尔滨工程大学自动化学院,黑龙江哈尔滨150001【正文语种】中文【中图分类】U666.10 引言Kain J E提出的速度+姿态匹配算法是目前最为常用的传递对准算法之一［1］，其推导的快速传递对准模型在小角条件下进行了合理的线性化处理。

（转）一种常见的四轴飞行器姿态解算方法分析

（转）一种常见的四轴飞行器姿态解算方法分析

（转）一种常见的四轴飞行器姿态解算方法分析全国各地已经陆续开放低空管制，北京也将在2015年全面开放低空领域，这对低空飞行器将是一个十分重大的好消息!低空飞行器也将迎来一个新的发展春天。

实际上，近年四轴飞行器发展相当迅速，国内的航拍水平越来越高，顺丰及亚马逊已在尝试将无人机用于快递行业。

越来越多的人开始关注并研究四轴飞行器。

本文将分析一种常见的四轴飞行器姿态解算方法，Mahony的互补滤波法。

此法简单有效，希望能给学习四轴飞行器的朋友们带来帮助。

关于姿态解算和滤波的理论知识，推荐秦永元的两本书，一是《惯性导航》，目前已出到第二版了;二是《卡尔曼滤波与组合导航原理》。

程序中的理论基础，可在书中寻找。

下面开始进入正题：先定义Kp，Ki，以及halfT 。

Kp，Ki，控制加速度计修正陀螺仪积分姿态的速度halfT ，姿态解算时间的一半。

此处解算姿态速度为500HZ，因此halfT 为0.001#define Kp 2.0f#define Ki 0.002f#define halfT 0.001f初始化四元数float q0 = 1, q1 = 0, q2 = 0, q3 = 0;定义姿态解算误差的积分float exInt = 0, eyInt = 0, ezInt = 0;以下为姿态解算函数。

参数gx，gy，gz分别对应三个轴的角速度，单位是弧度/秒;参数ax，ay，az分别对应三个轴的加速度原始数据由于加速度的噪声较大，此处应采用滤波后的数据void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){float norm;float vx, vy, vz;float ex, ey, ez;将加速度的原始数据，归一化，得到单位加速度norm = sqrt(ax*ax + ay*ay + az*az);ax = ax / norm;ay = ay / norm;az = az / norm;把四元数换算成“方向余弦矩阵”中的第三列的三个元素。

航天器姿态确定(研究现状)

航天器姿态确定(研究现状)

链接地址 /xiaozu/257088?ref=minifeed&sfet=211&fin=1&ff_id=71996187
法优于TRIAD法[3]。此后，Shuster又基于QUEST测量模型证明了：1) Wahba问题 等价于最大似然估计问题[18]，并进一步提出了广义Wahba问题[19]；2) TRIAD法是 一个最大似然估计器[20]； 3)该测量模型的方差阵在EKF公式中可以等效地用非奇异 阵 2 I 33 代替[16]， 该模型也是Shuster教授一生中最引以为自豪的[21]。 针对大视场敏 感器情形，Cheng利用一阶泰勒近似进一步扩展了QUEST测量模型[22]。对于连续 旋转理论， Shuster在文献[23]中正式提出并将该方法应用于解决一般性的姿态奇异 问题，该方法后来在FOAM法[4]、ESOQ2 法[8]中均得到应用。 近年来，虽然没有新的确定性算法出现，但随着Wahba问题本质的探索[19]， 现有算法与最大似然估计关系的揭示[19,20,24]以及方差分析的完善[13]等文献出现， 让 科研工作者对确定性算法有了更深刻的了解，并可进一步掌握方差分析这一有力 工具[25]。 (2) 状态估计法 单纯依靠矢量观测进行姿态解算的确定性方法要求参考矢量足够精确，且易 受敏感器的失准误差、测量误差等因素影响，往往难以满足高精度的定姿要求。 与这类方法相反，状态估计法中的状态量并不仅限于姿态参数，还包括矢量观测 中的一些不确定性参数；另外，现代航天器上的姿态确定系统往往采用多个姿态 敏感器进行组合测量，由于不同敏感器在测量精度、数据更新率上具有较大差异， 一般也需要采用状态估计法进行信息融合。根据姿态角速度信息的获取方式可将 姿态确定方案分为有陀螺方案和无陀螺方案，前者的姿态角速度由速率积分陀螺 测量得到，而后者的姿态角速度一般通过姿态动力学传播得到。 常用的姿态描述参数有方向余弦阵(Direction Cosine Matrix, DCM)、欧拉角 (Euler Angles)、旋转矢量(Rotation Vector)、姿态四元数(Quaternion)或欧拉对称参 数(Euler Symmetric Parameters)、罗德里格参数(Rodrigues Parameters)或吉布斯向量 (Gibbs Vector)、修正罗德里格参数(Modified Rodrigues Parameters, MRPs)、凯莱克莱参数(Cayley-Klein Parameters)等，目前航天器上最常用的姿态参数是四元数， 其优点主要在于用其表示的姿态运动学方程为线性形式，计算量小，且不存在奇 异性。在 1964 年，Stuelpnagel从数学上证明了三维参数用来表示姿态不可能是全 局且非奇异的[26]，因此，虽然旋转矢量[27]、MRPs[28]作为姿态描述参数也有一定应 用，但就描述航天器姿态而言始终不如四元数流行。不过，在航姿系统中常采用 旋转矢量进行快速姿态解算[29]，而欧拉角由于其明显的物理意义也常被用于描述 火箭或导弹的姿态，至于欧拉运动学方程中的奇异问题，可采用双欧拉角法进行 有效解决。另外，文献[30]对姿态描述参数及其运动学方程进行了系统的综述。 扩展卡尔曼滤波(extended Kalman filter, EKF)技术[31-34]常被用于航天器实时姿 态确定，根据姿态参数的选取不同和观测量的不同形式，常见的实现方式有乘性 扩展卡尔曼滤波[34,35](multiplicative ex-tended Kalman filter, MEKF)和加性扩展卡尔

基于四元数和航天器姿轨信息的相对位姿确定算法

基于四元数和航天器姿轨信息的相对位姿确定算法

摘 要 ： 天 器 间的相 对 位姿 确 定是航 天 器编 队飞行 、 航 交会 与对接 、 获 与 维 护 等 重 大航 天 任务 的 捕
关键技 术 。 于图像信 息的相 对位 姿确 定是 目前 国 内外研 究 的热点 ， 解决 航 天器相 对位 姿确 定 问 基 是 题 的有 效方 法。 文 中在 航 天 器姿 态动 力学 、 器视 觉理 论基础 上 ， 出基 于四元数 和航 天 器姿轨信 机 提
Ｑ 一 （ｏ口 口 ，）一 ｑ ＋ ｑ ｏ ｌ ｑＪ＋ ｑｋ ＋ ２ ３
Ｑ ＝ Ｑ 。 ‘ Ｑ 。 一 Ｑ 。Ｑ Ｑ Ｑ一 。
（） ３
（） ４
式 中 ， Ｑ ’ Ｑ ， ， 为 四元数 ， 。 Ｑ均 Ｑ＿， 别为 四元数 Ｑ 耍分
的逆 和共 轭 四元 数 。 Ｑ表 示 旋 转 变换 矩 阵 所对 应
第 ２卷 第 ３ ６ 期
Ｖｏ． １ ２６ Ｎｏ．３
基 于 四 元 数 和 航 天 器 姿 轨 信 息 的 相 对 位 姿 确 定 算 法
李 克 昭ｈ ，袁 建平 ，岳 晓奎
（ ． 北 工 业 大学 航 天 学 院 ， 西 西 安 １西 陕 ７０ ７ ；２ 河南 理 工 大 学 ，焦 作 １０２ ． ４４０） ５０ ３
关 键 词 ： 元 数 ， 天 器 姿 轨 信 息 ， 对 位 姿 确 定 四 航 相
中图分类号 ： ４ ． ２ Ｖ２ ９ ３
文 献标 识码 ： Ａ
文章编 号 ：０ ０ ２ ５ （０ ８ ０ —２ — ５ １ ０ — ７ ８ ２ ０ ） ３３ １０
２０ ０ ５年 ７ ３日深 夜 （ 月 太平 洋 时 间 ） 美 宇航 局 ， （ ＮＡｓ 进行 的“ 度撞 击 ” Ａ） 深 彗星 坦普 尔 １号科 学 实 验 ， 取 了大 量 宝 贵 的 科 学 数 据 ［ ； ０ ６年 ９月 ３ 获 １ ２０ 日， 欧洲 第一 个 月球 探 测器 “ 能 １号” 智 成功 撞 击 月 球 ， 于研究 月球 的成份 具有 重 要意 义 ［ 。 对 ２ ］

基于陀螺和四元数的EKF卫星姿态确定算法

基于陀螺和四元数的EKF卫星姿态确定算法

b^ ( t ) , b#^ = 0。定义滤波误差 $x 为
$x = x - x^ ,
( 19)
则由式( 17) ~ ( 18) 可得
d dt
$x (
t
)
=
F( t ) $x( t ) +
G( t) w( t) ,
( 20)
式中:
F( t) =
1 2
8(
X^ )
03@ 4
-
1 2
.
(
q^ )
;
03@ 3
G( t ) =
-
1 2
.
(
q^ )
04@ 3 .
03@ 3
03@ 3
式( 20) 忽略了二阶小量 8 ( $ X) $q。上述推导
采用了 EKF 法。因为不可能获知真实姿态四元数,
实际上无法得到式( 19) 表示的偏差量。此时, 误差
四元数由陀螺和星敏感器联合产生, 可认为星敏感
器的量测为真实四元数的最优估计值。
是陀螺常值漂移, 与时间相关部分的建模方法有两
种, 即
#
b= n 2;
( 5)
#
b= - b/ S+ n2,
( 6)
式中: n2 为白噪声, 且
E [ n2( t) ] = 0;
( 7)
E [ n2( t ) nT2 ( t ) ] = Q2( t ) D( t - S) ; ( 8)
E [ n1( t ) nT2 ( S) ] = 0,
确定结果。 为此, 本文采用 EKF 算法, 对基于陀螺和四元
数的卫星姿态确定方法进行了研究。
1 四元数及其运动学方程
定义四元数 q 为

单目视觉下基于对偶四元数的运动目标位姿确定

单目视觉下基于对偶四元数的运动目标位姿确定

第35卷第10期2010年10月武汉大学学报 信息科学版Geo matics and Informat ion Science of W uhan U niver sity Vo l.35N o.10Oct.2010收稿日期:2010 07 21。

项目来源:国家教育部高等学校博士学科点专项科研基金资助项目(20069998009)。

文章编号:1671 8860(2010)10 1147 04文献标志码:A单目视觉下基于对偶四元数的运动目标位姿确定冯国虎1 章大勇1 吴文启1(1 国防科技大学机电工程与自动化学院,长沙市砚瓦池街47号,410073)摘 要:利用对偶四元数研究运动载体和目标之间相对运动的单目算法大多是仿真验证,缺乏实验验证。

针对这些不足,在James 方法的基础上,扩展了状态量,推导了相应的系统模型和观测模型,组成新的滤波方程。

匀速旋转的转台实验验证了改进算法的优越性。

关键词:单目视觉;对偶四元数;扩展卡尔曼滤波;位姿确定中图法分类号:P228.41基于视觉信息的飞行器精密着陆、巡航导弹末端制导、无人驾驶汽车导航控制等领域都需要解决运动载体与目标之间相对位姿确定这一基本问题。

传统思路将目标特征提取和位姿确定作为两个独立的问题进行解决,导致了系统间的互不匹配。

Mukundan 等[1]利用3个非共线特征点,在适当假设下,基于四元数法推导出一个四次方程,通过求解此方程确定目标相对位姿的解析解,但存在多解问题。

Alonso 等[2]利用测量得到的目标的视线方向矢量,推导了编队飞行航天器相对导航和相对姿态最优估计的鲁棒算法,并利用相对动力学模型和相对运动学模型设计了最优观测器,实现了对目标位姿的最优估计。

Quan 等[3]研究了从若干对应点线性确定目标位姿的方法。

Shakhnarov ich 等[4]提出了一种 参数敏感散裂法!用于快速确定位姿。

Jam es [5]利用对偶四元数描述观测目标直线特征来确定目标相对相机坐标系的位置、姿态,该算法将位姿计算统一到对偶四元数体系中。

基于陀螺和星敏感器的卫星姿态确定算法_边志强

T
( 3)
2
陀螺的测量模型
陀螺模型中考虑陀螺的随机漂移 d 、 常值漂移 b
T gi 2
上式中, 按照四元数相乘的定义, 很容易得到下式 ^ 1 bo 1 ^ bo $ q ª Xbo X ª $ qbo = 2 2
和测量噪声 ng , 有 E{ ngi ( t) n ( S ) } = Rg D ( t- S) ( i= x , y , z ) , Rg 为 测量噪声 均方差阵 , D ( t) 是方差 强 度。模型如下
[1 - 2]
Xg = X+ d + b + ng
( 1)
U [1 0
0 0] , 所以有
T
其中 , Xg 为陀螺角速度在本体系上坐标 , X 为 卫星相对惯性空间的角速度在本体系上的坐标。 陀螺随机漂移通常被描述为一阶马尔 柯夫过 程
[ 8]
1 $ qbo ª $ Xbo = 1 $ Xbo + $( | $ qbo | | $ Xbo | ) 2 2 1 Abo ( $ qbo ) = - 2 $q 3 2 $q 2 2 $q 3 1 2 $q 1 - 2 $q 2 2 $q 1 1 ( 5)
- 0 1 5I 3@ 3 - DS 03@ 3
- 0 1 5I 3@ 3 03@ 3 03@ 3
9@ 9
相同, 测量精度为 v( 3R ) , 则可以得到 Rk = v2 I3@ 3 。ຫໍສະໝຸດ 6姿态确定算法过程
1) 在没有测量值时, 进行预报计算[ 2- 3, 6, 9] 在 k - 1 时刻 , 即使在没有星敏感器的测量输出
= I 3@ 3 - 2[ $q @ ]
将式( 4) 、 ( 5) 代入式( 3) 中, 忽略 ( 5) 中的二阶小 量, 有 $Û q0 = 0 $Û q = $q @ ^ Xbo + 1 $Xbo 2 =- [^ Xbo @ ] $q + 1 $Xbo 2 根 据 Xbo = X - Abo ( qbo ) Xoi , ^ Xbo = X ^ ^ Abo ( qbo ) Xoi , 真实姿态角速度 X 和陀螺测量角速度 X 之差为: $X= X- ^ ^ X= - $b- $d - ng , 则有 $Xbo = Xbo - ^ Xbo = $X+ 2[ $q @ ] Abo ( qbo ) Xoi 。 结合以上各式 , 可得关于 $q 的线性状态方程 $Û q=- [^ X @ ] $q - 1 $b - 1 $d - 1 ng 2 2 2 $Û q0 = 0 ( 7) ( 6)

基于四元数互补滤波的无人机姿态解算_吕印新

第 38 卷 第 2 期 2014 年 3 月燕山大学学报 Journal of Yanshan UniversityVol. 38 No. 2 Mar. 2014文章编号：1007-791X (2014) 02-0175-06基于四元数互补滤波的无人机姿态解算吕印新 1，肖前贵 2, *，胡寿松 12. 南京航空航天大学 无人机研究院， （1. 南京航空航天大学 自动化学院， 江苏 南京 210016； 江苏 南京 210016） 摘 要：针对无人机低成本姿态解算这一基本问题，考虑到传统姿态算法运算量大、不易调试，采用微惯性单元 （MEMS） 测量无人机原始姿态数据，采用基于四元数的互补滤波算法，有效降低姿态解算的运算量，实现 MEMS 各传感器的信息融合。

从理论上证明了基于四元数的互补滤波器的稳定性，分析了滤波器的性能。

采用 无人机真实数据验证了算法的有效性，解算得到的俯仰角、滚转角精度小于 1°，航向角精度小于 2°。

与传统姿 态算法比较，本算法简单有效、运算量小、易于调试。

关键词：姿态；四元数；互补滤波 ；稳定性分析 中图分类号：V243.5 文献标识码：A DOI：10.3969/j.issn.1007-791X.2014.02.0150引言微小型无人机具有成本低、隐蔽性好、生存能EKF 算法。

然而 EKF 存在 3 大缺陷：1） 在一般 情况下计算雅可比矩阵是不容易实现的过程， 而且 2) 其计算量很大； 当线性化假设不成立时，线性 化会导致滤波器极度不稳定；3) 实际应用中，噪 声难以符合白噪声的要求 [2-3]。

文献 [4-5] 利用粒 子滤波解决了系统非线性、 非白噪声对姿态解算的 影响，然而此方法计算量较大，不适合低成本航姿 系统的应用。

互补滤波器算法简单可靠，对惯性器 件的精度要求较低， 在飞行器姿态解算中的应用愈 加广泛。

文献 [6-7] 分别给出了欧拉角、方向余弦 矩阵形式下的互补滤波， 然而在飞行器存在运动加 速度的时候，姿态解算的误差较大。

四轴飞行器姿态控制算法

姿态解算姿态解算(attitude algorithm), 是指把陀螺仪，加速度计, 罗盘等的数据融合在一起，得出飞行器的空中姿态，飞行器从陀螺仪器的三轴角速度通过四元数法得到俯仰，航偏，滚转角，这是快速解算，结合三轴地磁和三周加速度得到漂移补偿和深度解算。

姿态的数学模型坐标系姿态解算需要解决的是四轴飞行器和地球的相对姿态问题。

地理坐标系是固定不变的，正北，正东，正上构成了坐标系的X，Y ，Z 轴用坐标系R 表示，飞行器上固定一个坐标系用r 表示，那么我们就可以适用欧姿态的数学表示姿态有多种数学表示方式，常见的是四元数，欧拉角，矩阵和轴角。

在四轴飞行器中使用到了四元数和欧拉角,姿态解算的核心在于旋转。

姿态解算中使用四元数来保存飞行器的姿态，包括旋转和方位。

在获得四元数之后，会将其转化为欧拉角，然后输入到姿态控制算法中。

姿态控制算法的输入参数必须要是欧拉角。

AD 值是指MPU6050 的陀螺仪和加速度值，3个维度的陀螺仪值和 3 个维度的加速度值，每个值为16位精度。

AD 值必须先转化为四元数，然后通过四元数转化为欧拉角。

在四轴上控制流程如下图：面是用四元数表示飞行姿态的数学公式，从MPU6050 中采集的数据经过下面的公式计算就可以转换成欧拉角，传给姿态PID 控制器中进行姿态控制.PID 控制算法先简单说明下四轴飞行器是如何飞行的,四轴飞行器的螺旋桨与空气发生相对运动，产生了向上的升力，当升力大于四轴的重力时四轴就可以起飞了。

四轴飞行器飞行过程中如何保持水平：我们先假设一种理想状况：四个电机的转速是完全相同的是不是我们控制四轴飞行器的四个电机保持同样的转速，当转速超过一个临界点时（升力刚好抵消重力）四轴就可以平稳的飞起来了呢？答案是否定的，由于四个电机转向相同，四轴会发生旋转。

我们控制四轴电机1和电机3同向，电机2电机4反向，刚好抵消反扭矩，巧妙的实现了平衡, 但是实际上由于电机和螺旋浆本身的差异，造成我们无法做到四个电机产生相同的升力，这样飞行器起飞之后就会失去平衡。

基于扩展卡尔曼滤波的四旋翼无人机姿态估计方法

基于扩展卡尔曼滤波的四旋翼无人机姿态估计方法作者：段敏赵凌周莹来源：《现代信息科技》2022年第04期摘要：為提高四旋翼无人机姿态参数获取的准确性，确保后续姿态控制精度，采用STM32F407微控制器以及多传感器构成姿态测量系统。

对各传感器原始误差进行校准，应用扩展卡尔曼滤波（EKF）进行基于陀螺仪的状态预测和基于加速度计/磁力计的测量校正，融合信息并估计出3姿态角，与3自由度姿态算法验证系统测量出的姿态角真实值对比，3个角度的平均误差为0.7°，相对于基于单一陀螺仪积分和基于加速度计/磁力计的姿态解算，误差分别下降了3.034°和0.174°，该方法可有效提高EKF估计精度。

关键词：扩展卡尔曼滤波;四旋翼无人机;姿态估计中图分类号：TP368;V279 文献标识码：A文章编号：2096-4706（2022）04-0007-05Attitude Estimation Method for Quad-rotor UAV Based on Extended Kalman FilterDUAN Min， ZHAO Ling， ZHOU Ying（The College of Post and Telecommunication of WIT， Wuhan 430073， China）Abstract： In order to improve the accuracy of attitude parameters acquisition of quad-rotor UAV and ensure the subsequent attitude control accuracy， STM32F407 microcontroller and multi-sensor are used to form an attitude measurement system. The original error of each sensor is calibrated. The extended Kalman filter （EKF） is used for gyro-based state prediction andaccelerometer/magnetometer-based measurement correction. The information is fused and the three attitude angles are estimated. Compared with the real value of the attitude angle measured by the 3-DOF attitude algorithm verification system， the average error of the three angles is 0.7°， compared with the attitude solution based on single gyro integral and the attitude solution based on accelerometer/magnetometer， the errors are reduced by 3.034° and 0.174° respectively. This method can effectively improve the accuracy of EKF estimation.Keywords： extended Kalman filter; quad-rotor UAV; attitude estimation0 引言四旋翼无人机因其成本低廉、维护操作简单、可替代人力完成特殊任务等优点，近年来被广泛应用于军用和民用领域[1，2]。

  1. 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
  2. 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
  3. 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

确定结果。 为此, 本文采用 EKF 算法, 对基于陀螺和四元
数的卫星姿态确定方法进行了研究。
1 四元数及其运动学方程
定义四元数 q 为
q0
q=
= [ q 0 q1 q 2 q 3] ,
( 1)
Q
式中: q 0= cos ( H2 ) ; Q= nsin ( H2 ) ( 此处, n 为单元 欧拉轴; H为转动的欧拉角) [ 2] 。
G( t ) =
-
1 2
.
(
q^ )
04@ 3 .
03@ 3
03@ 3
式( 20) 忽略了二阶小量 8 ( $ X) $q。上述推导
采用了 EKF 法。因为不可能获知真实姿态四元数,
实际上无法得到式( 19) 表示的偏差量。此时, 误差
四元数由陀螺和星敏感器联合产生, 可认为星敏感
器的量测为真实四元数的最优估计值。
式( 5) 表示陀螺漂移的时间相关部分是一随机 游走, 而式( 6) 是一阶马尔可夫过程( 即指数相关) ,
两者均为典型的有色噪声。在受噪声干扰的控制系
统估计算法中, 通常用成型滤波器将有色噪声转换 为由白噪声驱动的模型[ 3] 。
对随机游走过程, 有
t
S
Q Q E [ b( t ) b( S) ] = E [ n2( L) d L n2( K) d K] =
b^ ( t ) , b#^ = 0。定义滤波误差 $x 为
$x = x - x^ ,
( 19)
则由式( 17) ~ ( 18) 可得
d dt
$x (
t
)
=
F( t ) $x( t ) +
G( t) w( t) ,
( 20)
式中:
F( t) =
1 2
8(
X^ )
03@ 4
-
1 2
.
(
q^ )
;
03@ 3
根据式( 20) 构造的滤波器状态方程会使误差协
方差阵产生奇异, 其误差协方差阵可表示为
P 1( t ) 04@ 3
P ( t ) = 03@ 4 P2( t ) ,
( 21)
式中:
P1 ( t) =
1 4
[
8(
X^ ) $q$qT 8 T (
X^ ) -
. ( q^ ) $b$qT 8 T ( X^ ) - 8 ( X^ ) $q$bT . T ( q^ ) +
将式( 16) 代入式( 15) 的第一式, 可得
q# ( t ) =
1 2
8 ( u( t) -
b( t ) ) q( t ) -
1 2
.
(
q( t) ) n1( t ) .
( 17)
其预测方程为
#
q^ =
1 2
8(
X^ )
q^ ( t ) ,
( 18)
式中: q^ , X^ , b^ 分别为 q, X, b 的估值, 且 X^ = u( t ) -
. ( q^ ) $b$bT . T ( q^ ) + . ( q^ ) n1 nT1 . ( q^ ) ] .
( 22) 根据式( 22) 以及矩阵秩的性质, 可知 P1( t ) 必 然不 会满 秩。实 际 上, 因 存 在 舍入 误 差, 会出 现 P1( t ) 负定的情况。
陀螺漂移和安装误差的补偿效果取决于星敏感 器量测精度。星敏感器自身同样存在安装和热变形 等误差, 这种在短时间内可视为常值的偏差不影响 陀螺漂移等角速度的补偿, 但会被引入最终姿态角
收稿日期: 2005-01-29; 修回日期: 2005- 03-09 作者简介: 朱庆华( 1979 ) ) , 男, 硕士生, 主要从事卫星姿态测量 技术研究; 李英波( 1963 ) ) , 男, 研 究员, 主要 研究方 向为 航天 器制 导、导航和控制。
q# ( t ) = 8 ( Xb) q( t ) / 2,
( 3)
上海航天
2
A ERO SPA CE SHAN GHA I
2005 年第 4 期
0 - Xbx - Xby - Xbz
式中: 8 ( Xb ) =
Xbx Xby
0 - Xbz
Xbz - Xby
0
Xbx
Xbz Xby - Xbx
0
星惯性系角速度在本体系中的投影。
2. 2 星敏感器数学模型 设空间某一矢量在卫星本体系和星敏感器量测
系中分别为 rb, rs, 则量测模型可表示为 r b = TCbs # ( r s + ns) ,
式中: Cbs 为星敏感器量测 系至本体系的转换矩阵 ( 即安装矩阵) ; T 为安装误差矩阵; ns 为量测噪声。 实际上, CCD 星敏感器 CCD 敏感面的 量测精度与 其法线方向并不相同。
0
0
tS
QQqwD( L- K) d LdK= qw # [ t , S] min, 00
式中: qw 为方差强度。由此可得随机游走的方差为 E [ b2( t ) ] = q wt 。其成型滤波器如图 1 所示。
图 1 随机游走成型滤波器 Fig. 1 Forming f ilter of random floating
n 2( t ) = R 2/ S # m( t ) ;
( 11)
E [ n2( t ) ] = E [ R 2/ S# m( t ) ] = 0; ( 12) E [ n2( t ) nT2 ( t ) ] = 2( R2/ S) # D( t - S) . ( 13)
式( 11) ~ ( 13) 描述的一阶马氏过程适于机械陀 螺。时间相关项的漂移取决于时间常数 S、驱动噪 声参数及其初始值 b( 0) 。因时间常数值较大, 当 S 趋于无穷时就变为随机游走过程, 故可认为陀螺输 出为真值、常值漂移及量测噪声之和。
; Xb 为卫
2 陀螺及星敏感器数学模型
2. 1 陀螺数学模型 一般, 陀螺的输出 X 可分 为角速度真实 值 u、
陀螺漂移 b 和量测噪声 n 1 三部分, 即
X = u - b - n1,
( 4)
且
E
[
n
1(
t)
n
T 1
(
S)
]
=
Q1 ( t ) D( t -
S) 。其中, 陀螺
漂移 b 包括 常值和与时间相关部分。常值 部分就
根据 姿 态 四 元 数, 可 得 卫 星 姿 态 旋 转 矩 阵
A( q) 为
A( q) =
(
q
2 0
-
QT Q) I 3@ 3 + 2 QQT - 2 q0 Q@ ,
( 2)
0 - q3 q 2 式中: Q@ = q 3 0 - q1 ; I 为单位阵。
- q2 q 1 0
姿态四元数随时间的运动方程可表示为
关键词: 姿态确定; 四元数; 陀螺漂移; 扩展卡尔曼滤波 中图分类号: V 448. 21 文献标识码: A
Extended Kalman Filter for Attitude Determination Using Gyros and Quaternion
ZHU Qing- hua, L I Ying- bo
( Shanghai Aerospace Control Eng ineering Institute, Shanghai 200233, China)
Abstract: Aiming at deter mining the satellite att itude precisely, the g yros bias and misalignment w ere compensated in t ime by using the quaternion of star- sensor and extended Kalman filt er. T he related module of the filter w as der ived. T he t heoretic accuracy of t he algorithm was acquired from the numerical simulatio n. T he effects o f the star- sensor misalignment on attitude determinat ion w er e analyzed at last.
一阶 马 氏 过 程 是 自 相 关 函 数 Rx ( t ) = R2exp[ - | t | / S] 的零均 值正态 分布过程 x ( t ) , 其 中, 均方值 R2= R2x ( 0) ; S 为时间相关常数; t 为两时 间点之间的间隔。其功率谱可表示为
]
Q 7 x ( X) =
R x ( K) e- j XKdK=
是陀螺常值漂移, 与时间相关部分的建模方法有两
种, 即
#
b= n 2;
( 5)
#
b= - b/ S+ n2,
( 6)
式中: n2 为白噪声, 且
E [ n2( t) ] = 0;
( 7)
E [ n2( t ) nT2 ( t ) ] = Q2( t ) D( t - S) ; ( 8)
E [ n1( t ) nT2 ( S) ] = 0,
式中: nout U [ 1 ( nbs ) T / 2] T 。
当考虑安装误差时, 所得星敏感器输出四元数
qout 为
qout = ( q á nout ) á qerror,
式中: qerror为安装误差的等效四元数。
3 滤波器数学模型
根据星敏感器输出四元数估计和陀螺漂移, 选 取七维滤波器状态 x = [ qT ( t ) bT ( t ) ] T , 当陀螺 相关噪声为随机游走型时, 由式( 3) ~ ( 5) 可得系统