[J]基于四元数姿态确定的扩展卡尔曼滤波方法

Ｊ１．一ｉｉａｎｇ，Ｅ—ｍａｉｌ：ｌｊｌｉａｎｇ＠ｂｉｔ．ｅｄｕ．ｃｎ）
Ａｂｓｔｒａｃｔ：Ａｎ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｈｅｒｍｇ ｔｅｃｈｎｉｑｕｅ ｉｏｒ ｔｈｅ ｌａｒｇｅ ａｎｇｕｌａｒ ｍａｎｅｕｖｅｒｉｎｇ ａｎｄ ｎｏｒｍａｌ ｍｏｄｅ ａｔ：ｉｔｕｄｅ
ｄｅｔｅｒｎａｉｎａｄｏｎ。ｆ Ｅ ｓａｔｅｌｌｉｔｅ ｉｓ ｐｒｅｓｅｎｔｅｄ ｂｙ ｕｓｉｎｇ ｇｙｒｃ，ｓｕｎ Ｓｅｘｑｓｏｒｓ ａｎｄ ｉｎｆｒａｒｅｄ ｈｏｒｉｚｏｎ ｓｃａｌａｎｅＴｅｏ．Ｔｈｅ ａｔｔｉｔｕｄｅ ｑｕａｔｅｒｍｏｎ ｒｅｐｒｅｓｅｎｔａｔｉｏｎ ｉｓ ａｄａｐｔｅｄ ｔＯ ｄｅｓＮｎ ａＤ ａｔｔｉｔｕｄｅ ｅｓｔｉｍａｔｏｒ ａｎｄ ｉｎｔｅｇｒａｔｉｏｎ ｌｉｎｅａｒ ｅｒｒｏｒ ｅｑｕａｔｉｏｎｓ ａｒｅ ｅｓｚａｈｌｉｓｈｅｄ．Ｔｈｅ ｒｅｓｕｌｔｓ ｏｆ ｃｈｅ ｓｉｍｕｌａｔｉｏｎ ｓｈｏｗ ｔｈａｌ ａｔｕｔｕｄｅ ｄｅｔｅｒｍｉｎａｔｉｏｎ ｉｓ ａｃｃｕｒａｔｅｌｙ ａｃｈｉｅｖｅｄ ｂｙ ｕｓｍｇ ｔｈｅ
叠三 乏、‘．，
１００
３００
ｊ‘
ⅣＳ
（ａ） 误差四元数三矢量误差
Ｅ： ｌ，ｏ乏 一三、 ４
兰ｅ－－．４——一—— 甚≮‘ｒ’？１一ｉ４：”、“，“。。ｉ。扑。巾蚪‘母ｐＨｌ守‘
１“·
ｊ“·
５嘎
Ｄ、 ……。…
‘
岂‘５——～——ｊ 《 妻，’ｊ；＾拼叫咐蜥带咿＊Fra bibliotek蚺》山ｌ趣唧妒‰
】ｏ。
３ Ｌ）（Ｉ
Ｔ一，
（１９）
０（￡。）一
ｄｉａｇ（Ｏ＂；，口；，口；．酲，口：，《．一，一，ｄ：·吼，盯盖泌）．（２０）
当＾一，时，根据太阳敏感器和红外地平仪测量结果
给出ｙ值，计算观测阵Ｈ，得到更新值如下：
丘（“州）一Ｐ一（＾叫）Ｈ１（＾＋１）×
［Ｈ（＾＋，）Ｐ （￡…）Ｈ１（＾十】）＋
Ｒ（＾“）］～，
（２１）
Ｐ＋（＾＋】）＝
（１ｊ）
其中：曲为采样时间．
西（山。．西）≈
ｋ。一去西ｉ？ 一．哆’Ｔ１］
（１６）
一 一删。 一。“。×ｊ一‘ｘ。
为壮志转化矩阵．
误差协方差Ｐ的预报｝｝算如下：
Ｐ—ｏ！…）一虹断）Ｐ４ ｉｔ。）ｆ Ｃａｔ）＋
Ｐ（ａｔ）Ｑ（ｔ＾），（ａｔ），
（１ ７）
其中
≠（出）＝ｅ”１“≈Ｉ十Ｆ（ｆ。），（１８）
ｄｒ（髓）一Ｉ≠（＝）Ｇ（ｒ）ｄｒ，
影响确定系统的精度．在常规的确定系统中，通常采
用陀螺作为基准元件测量连续的姿态角速度，使用
太阳敏感器、红外地平仪或磁强计＝耐量姿态位置．目
前，已有很多姿态确定算法，诸如宜适应滤波、预测
滤波、卡尔曼滤波、自适应卡尔曼滤渡、扩展卡尔曼
滤波等，用于减小敏感器的测量误差和提高确定系
统的测量精度．本文使用扩展卡尔曼滤波方法研究
ｗ’一）； 。ａ 。， 口舳］Ｌ。：， Ｖ一一Ｆ－。ｋ ｑ］＆：．
通过各个观测溅量值．根据先前设定的估计状
态变量墨和误差协方差初始矩阵Ｐｃ，可计算出每 步的置和Ｐ。，利用新的测量值ｙＨ：得出估计值墨．
令五一和丑一分别表示状志变量更新前后的值．
根据敏感器的测量值．计算预报值如下：
ｇ一池。。）＝垂（∞．ａｔ）ｑ＋（￡．）．
［Ｊ—Ｋ（ｆｋ＋】）Ｈ（ｚ＾＋１）］Ｐ一（ｆ，，１）［ｊ一
片（“＋：）Ｈ（“＋１）］７＋丘（“＋１）Ｒ（ｒⅢ）酽（‘，１），
（２２）
ｘ＋（ｔ，叫）＝Ｘ一（“一】）＋Ｋ（ｚ＾．，）［ＹｉｔＨ】）一
Ｈ（＾。，）ｘ一（ｚＨ）］，
Ｒ（ｆＩ，】）＝ｄｉａｇ（靠，口：，ｄ；，砰）．
获得ｆ㈩的状态变量ｘ＋（“＋：）后，依据对四元
５∞
（ｂ） 陀螺常值漂移误差
暗１ 太姿态位置的仿真螬暴
图１是太姿态位置（初始姿态角为５０。）的仿真
结果，图２是在正常运行模式下的仿真结果．通过仿
真结果可以看出，对姿态四元数误差、陀螺常值漂移
能够进行准确校正和估计，收敛也很快．红外地平仪
收敛较慢，初始值的设定对初始收敛性有较大影响．
２９Ｓ
２００４中国垄制与头策学术蔓台论文集
为姿态角速度，“。为陀螺测量惯性角速度．‰＝ Ｚｃ，一“。ｏ］，“。为轨道角速度．
滤波的目的是估计四元数误差而不是估计四 元数本身．因为所有测量元件都存在误差，定义误差 四元数吼为真实四元数与估计四元数之差，误差四 元数很小，但非零其表示如下：
目，一曲。 叮，， 口。： ｇ。；］：一曲。 口。］１．
＼▲，
相应四元数描述的动力学方程为
１
ｊ－一÷口。＠Ａ（ｎｋ）， ●
ｃ‰＝Ⅲ－一７乙（‰）吣．
（２）
式中：ｏ表示四元数乘法，Ａ（‰）一［ｏ‰１］。，％
收稿日期：２００３一ｌｏ－１２．
作者简介：李金良（１§７７一），男，北京人，硬±生，从事卫星姿志控制的研究
２９ ６
２００４中匿控制与冼第学术丘会论支集
Ｉ＾＿三
，
＾
１ｗ
３哦·
ｎ、
一… 一
“，
Ｅ姜：Ｉ， ｒ…～～～……４…～
篓、．２．．一一．．
】００
．。
３００
¨
一
、１－
（ｚ） 误差四元敷三矢量误差
（ｂ） 陀螺常值漂移谩差
臣２ 正常运行楼式的仿真结果
６结
论
本文使用误差四元鼓建立了动力学误差方程．
采用车文所提到的滤渡方法，无论在大角度还是正
常模式下都是适用的，仿真结果证明■方法的可行
．．
．
，
·目：＋
Ｉ警是叫一ｃ》Ｓ 。
ｓ毛
ｓ姆
、ｂ’Ｊ
口，＝＾。·ｇ，一。，．
（９）
在确定系统中两轴红外地平仪直接测量的是
俯仰角以和滚动角‰．它可表示为
！泞！；：＋［茇：札． ㈣，
其中：‰；和Ｅ。为常值钡ｆ量偏差，ＴＪ“为方差是吼零
均值的高斯白噪声．定义真实姿态位置（Ｐ口）与估
昔姿态位置（Ｐ目）的误差为
数、陀螺漂移和红外地平仪测量偏差进行校正，校正 后将状态变量ｘ重新设置为零．
５ 仿真结果
表１给出了各个敏感器的基本参数和滤渡初始 值，轨道太阳倾角为９８。，太阳赤纬为一２３。．升交点 地方时是上午９：００，采样步长为１ ｓ，时间常数设置
为ｌ Ｓ．
Ｅ
一
■’
￣＾：
兰、．：
ｌ ００
３０Ｃ
…
｝＾
■。 ’一ｋ。Ｈ“；。，“。·｝融：¨ｐ’ｔ耕ｉ｛＾－一？十计、一∞
体坐标系相对于当地轨道坐标系的卫星姿态，它的
矩阵可用四元数表示如下：
Ｔ。（‰）＝
方向余弦矩阵的四元数表示
Ｔ ＝ －Ｔ转置
ｒ２（酣＋爵）一１ ２（吼ｇｌ＋口。舶） ｌ ２曲：ｑ ２一ｇｏ吼）２（毹＋一）一ｌ ｌ ２（目，目。＋ｇｏｑ ２） ２（口ｚ啦一口。口】）
２（口妇１一ｇ。ｇ：）一
２（口２吼＋ｑ。ｑ１）｜ ２（矗＋一）一１Ｊ
值高斯白噪声驱动．
４姿态确定算法
状态估计向量为
ｘ。：＝［；，ｊ砰：西．‰‰。］一
测量向量为
ｙ。＝［仁以；Ｄｔ％］７．
姿态估计模型可表示为
Ｘ（ｆ）＝Ｆ（Ｏｘ（ｆ）＋Ｇ（ｒ）Ｗ，
Ｙ＝ＨＸ＋Ｖ．
（１４）
其中
ｒ一：：×］一号ｋｓ一÷ｋ ｓ咄ｚ］
Ｆ“）一０３ｘ ３ ｏａ×，
ｏ…
ｏ…
ｏ｜ｘ ２ｊ
，
。… 一７‘Ｊｓ×，ｏ…｜
Ｃｏｌｏｒａｄｏ，１９９Ｓ，（１）：６：一６７．
参考文献（Ｒｅｆｅｒｅｎｃｅｓ）： ［１：Ｃｈａｎｇ Ｙ Ｔ，ｗｕ Ｔ Ｆ，Ｃｈａｎｇ Ｆ Ｒ Ｆ－ｔｅ ｒ ｄｅｓｘｇｒ．ｉｏｒ
ａｔｔｉｔｕｄｅ ｄｅｔｅｒｒａｉｎａｔｉｎｍ ｕｓｉｎｇ ＧＰＳ ａｎｄ Ｇ３”ｒｏ：Ｆｒｅｑｕｅｎｃｙ
ｄｏｍａｉｎ ａｐｐｒｏａｃｈ［Ａ：．ＳＩＣ￡ｉｃ］．Ｎａｇｏｙａ，２００１．２４０＿
Ｊ。，：一２［９，×：．
（ｉ）
其中［ｉ，×］是由吼－Ｅｌｉ；。构成的反对称阵．
结合式（２一芋三将式（４）微分，可得误差匹元熬方
程为
｛。一ｉ１吼ｏ＾４（‰）一｛一（五。）ｓ“＋
去ｉ，＠Ａ（ｑ），
Ｃａ／，，ｈ一［丁。咄）一Ｌ。。］丁０（五）吣．
（６）
其中：二。为惯性角速度的测量值，ｆｏｒ＆；为姿态角速度
；，一［号印丢羽吾嘲．
（。）
其中ｄＰ，甜和蛳劳别表示滚动角、俯仰角和偏航角
误差．
ｏ和ｇ。之间的关系，可用四元数乘法表示为
口一；ｏ ｑ，，
（４）
其中ｇ为真实值ｑ的估计值．玑相应转化姿态矩阵的
误差７１。（ｑ。）为
３了１。ｉ目。）＝７乙（吼）一
一
１
２９ｄ
—日。２］
一２ｑ。：
Ｉ
２９“＝
。２目，：
一２ｑｄ
１一
了太阳同步轨道卫星的姿态确定，该卫星采用了陀
螺、太阳敏感器、红外地平仪构成的测量系统．为各
敏感器建立了更加详细的姿态估计器．考虑在大姿
态角度下Ｅｕｌｅｒ角描述容易引起奇异性，本文采用
了四元数描述建立卫星运动学方程．仿真结果表明，
该方法有效地提高了卫星的测量精度．
２姿态运动学方程
使用四元数ｑ＊＝ｂ。目，口：ｇａｊ７描述在星
事盎良等：基于西元鼓姿寿确定的扩晨卡尔曼靖渡方法
２９７
Ｇ０）一
『一托，：。。ｏ。。：×：１
『０３ ｘ ３
０３，３ —０２。；
日＝
，Ⅲ０３ｘ ３
０ ｓ×ａ １３×３
ＯⅢ０ ２ｘ ３
０３ｘ ２ ．
０３ｘ ２
Ｊ２ｘ ２ｊ】１×】１
｝ｌ ０
２
３