第
３
１卷 第 ５期
四 川 兵 工 学 报
２１
００年 ５
月
【
其他研究】
捷联系统四元数姿态解算的精细积分法
项凤涛，
王正志，
吴第，
岳 达
（
½防科学技术大学 机电工程与自动化学院，
长沙 ４
０７
）
１０３
摘要：
研究了基于四元数的相关理论，
把精细积分方法应用于求解四元数姿态微分方程½中，
并以典型圆锥运动
½为输入条件对该算法进行了仿真分析。相同仿真步长条件下的仿真结果表明，
该方法在高动态角运动环境下
的解算精度要优于四阶龙格库塔算法，
由圆锥运动引起的俯仰角算法漂移误差相对于于四阶龙格库塔方法也得
到有效抑制，
可以为捷联式惯性导航技术的工程实践提供参考。
关键词：
捷联惯导系统；
四元数姿态微分方程；
精细积分；
龙格库塔法
中图分类号：
Ｐ０
Ｔ ３１
文献标识码：
Ａ
文章编号：
０６－ ７７ ２１
）
５－ １３－ ４
１０ ００
（
００ ０ ００ ０
小，
所以常用于飞行器实时解算。四元数的另外一个优点
是便于采用卡尔曼滤波实现四元数最优估计，
也正是基于
这个原因，四元数广泛应用于航天器的姿态控制系统。近
年来，
出现了很多有效求解四元数微分方程的方法，
例如
龙格库塔法、
旋½矢量算法等。四阶龙格库塔算法是计算
四元数姿态微分方程的一种有效手段，
其精度较高，
½运
算量很大，
并且对数据光滑性要求较高。旋½矢量算法是
９１年 Ｂ ½
提出的旋½矢量理论及旋½矢量微分方
½½
½
基于
１７
３
程，
该算法½够对圆锥误差进行有效补偿
［ ½
。本文中把精
捷联惯性导航控制系统（ ＩＳ
是目前自主导航控制
ＳＮ
）
领域研究热点之一，
该方法将陀螺仪和加速度计直接安装
在½½½上，
省掉机电式导航平台，
利用导航计算机½件建
数学平台”
来代替机电平台实½。由于其结构简
立一个“
单，
成本½廉，
易于实现惯性测量单元（
Ｍ
）
Ｉ Ｕ
的小型化，
且
抗干扰½力强，
目前已广泛应用于航 空 航 天、 海、 器
航
机
智½交通、
无人飞机和精确制导炸弹等飞行器的导航
人、
控制½中。实时高精度的姿态解算是捷联式惯性导航系
统的关键技术，
它在很大程度上½响着捷联惯性导航系统
的导航精度，
姿态解算主要指的就是姿态变换矩阵的求
解，
通过姿态变换矩阵可以得到½½½的姿态和导航参数计
算需要的数据，
是捷联式惯导算法中的重要工½。
½½½的姿态和航向½现了½½½坐标系与导航坐标系
之间的方½关系，
确定
２个坐标系之间的方½关系需要借
助矩阵法和力学中的刚½定点旋½的理论。通过矩阵法
推导方向½弦表，
而刚½定点运动旋½的理论表明，
定点
运动刚½的任½有限½移½可以用绕过定点的某一½的
一次½动来实现。目前，
主要的研究方法有：
欧拉法、
方向
½弦法与四元数法。已有大量科研文献和工程实践表明，
四元数法相对于其他两种方法的优越性，
比如在大姿态角
欧拉方程存在奇异点，
又称½平衡环锁定；
方向½
情况下，
弦½然消除了欧拉方程的奇异性，½为了减小正交误差不
得不以计算时间为代价。随着飞行器导航控制系统的迅
速发展和数字计算机在运动控制中的应用，
控制系统要求
导航计算环节更加合理地描述½½½的空间运动，
四元数法
得到了广泛的研究。而且四元数法的正确性和有效性已
２
被相关专家证明
［ ½
，
四元数法由于其计算无奇点，
计算量
细积分法应用于四元数微分方程的求解，
数值仿真结果表
明相同条件下它是进一步减小姿态解算误差的有效方法，
为捷联式惯性导航技术的工程实践提供参考。
１
四元数微分方程及其龙格库塔解法
１１
． 四元数姿态微分方程
四元数的数学概念是
１４
８３年由哈密顿首先提出的，
它
用四维空间的性
是把三维空间和
１个四维空间联系起来，
质和运算规则来研究三维空间中的刚½定点½动问题。
设坐标系
Ｉ
为空间中的一个惯性参考坐标系，
机½坐
标系
Ｂ为空间中的一个运动坐标系，
假设机½坐标系
Ｂ到
的姿态变换四元数为
惯性坐标系
Ｉ
珒
Ｑ ＝½ ＋½
珒
２
½
３
½
½
珒
０
１
＋½ ＋½
假定矢量
½
绕通过定点
½
某一½½动了一个角度
θ
如
，
½动后的四元数用
½
′
表示，
则用四元数表示
½ ½
′
间的关
和
系如下
½
＝
½ Ｑ
′Ｑ
收稿日期：
００－ ２－ ０
２１ ０ １
基金项目：
½家自然科学基金资助项目（
０３０５
６８５０
）
½者简介：
项凤涛（
９６
， 硕士研究生，
１８
—） 男，
主要从事组合导航研究；
王正志（
９５
， 教授，
１４
—） 男，
博士生导师，
主要从事自动控制，
模式识别与图像处理和生物信息学研究。
四川兵工学报
１４
０
θ
珗
θ
其中：
½
＋
珒
½½ ＋ ½
Ｑ＝
０
½
½
＝
½
½
。
½
２
２
四元数的实时值可通过求解四元数姿态微分方程而
得到，
四元数微分方程为
１
ω
Ｑ＝ Ｑ
Ｂ
２
式中：
”
“ 代表四元数乘法；
Ｂ
是动系角速率。
ω
４
写成矩阵½式，
可表示为
［ ½
½
０
－
ω
Ｂ
－
ω
Ｂ
０
１
２
因此它可以大大提高计算精度，
其数值结果
用解析公式，
可以比拟于精确解的数值结果。该方法自提出以来在力
学方面 应 用 广 泛， 具 ½ 应 用 到 捷 联 惯 导 系 统 方 面 的
½
很少。
已知道四元数微分方程为
１
Ｑ ＝ Ｑ
ω
Ｂ
２
½
１
令
ω
＝
ω
，
因为
ω
是常矩阵，
其通解可写为
２
Ｂ
Ｑ＝
½
（ ·½·Ｑ
½½
ω
）
０
把上面的矩阵指数½数按照泰勒级数展开，
即
１
１
½½
ω
）
＝Ｉ ＋
ω
＋
（
½
２
＋
（
½
３
＋…
½
（ ·½
½
ω
）
ω
）
½
２
！
３
！
现在要做的就是½可½精确的计算出来该矩阵指数½数
。则有
的数值解。数值积分需要有一个时间步长，
记为
τ
Ｑ ＝Ｑ
τ
＝Ｔ
０
，
（）
·Ｑ
Ｔ＝½
½
ω
）
½
（
τ
１
得出矩阵
Ｔ
利用逐步积分就有下面的递推公式
，
Ｑ ＝Ｔ
０
，
２
＝Ｔ
１
，
Ｑ
＋
＝Ｔ
½
…
·Ｑ
Ｑ
·Ｑ …，
½
１
·Ｑ ，
１
于是问题½结到
Ｔ阵的计算，
应½非常精细地算出该
矩阵。
指数矩阵的精细计算有
２个要点：
运用指数½数的
①
Ｎ
加法法则，
即运用
２
类的算法； 存储时存放增量，
而不
②
ω
Ｂ
３
０
０
－
ω
Ｂ
½
½
１
ω
Ｂ
ω
Ｂ
１
１
３
２
１
＝
½
２
½
０
ω
Ｂ
２
３
１
２
ω
Ｂ
－
ω
Ｂ
２
½
ω
Ｂ
ω
Ｂ
－
ω
Ｂ
０
½
３
３
２
１
３
－
其中：
＝
ω
Ｂ
ω
Ｂ
ω
Ｂ
½
ω
［
１ ２ ３
是机½坐标系
Ｂ相对于惯性坐标系
Ｉ
的旋½角速度在机½坐标系
Ｂ上的投½。
１２
．
四阶龙格库塔解法
求解四元数姿态微分比较常用的方法是四阶龙格库
塔迭代算法。在捷联计算中四元数的实时更新常用四阶
龙格库塔法，
其实质就是在（
½ ½
＋
）
½
，½
½
求多个斜率值，
予以
加权求平均，
得到更精确的平均斜率。利用四阶龙格库塔
方法时，
在一个更新周期
½内需要对机½上陀螺仪输出的
½
，
½
／
）
角速 率 信 号 进 行 三 次 采 样，
ω
（
½
）
ω
（
½
＋½２
，
即
（
½
½
。
ω
½
＋
） 用四阶龙格库塔法求解微分方程的过程如下：
½
Ｑ ½
）
＝Ｑ ½
（
＋½
（）＋ （
１
＋２
２
＋２
３
＋Ｋ
）
Ｋ
Ｋ
Ｋ
４
６
Ｋ ＝
１
Ｋ ＝
２
Ｋ ＝
３
１
［ （）
ω
½
½
½
Ｑ ½
·
½
（）
½
２
是其全量。
由指数½数的运算法则得
½½
ω τ
［
½
（ ·
τ
½
½
½
（ · ）＝
½½
ω
／
）
½
其中：
½为任意正整数，
½前可选用
½＝
Ｎ
２
例如
Ｎ
＝２
，
½＝
４ ７
。
０
则
１０８５６
本身就是不大的时间区间，
Δ
＝
τ
½将是非
则
½ ／
由于
τ
常小的一个时间段。因此对于
Δ
的区间，
½
有
１
２
½½
ω
Δ
）
Ｉ ＋
ω
½
½
（ ·
½
Δ
＋
（
Δ
）
＋
ω
½
½
！
２
１
１
３
４
（
Δ
）
＋
（
Δ
）
ω
½
ω
½
３
！
４
！
由于
τ
很小，
幂级数的
５阶以上展开式的精度基本上
就相½于计算机的舍入误差了。此时指数矩阵
Ｔ和单½
矩阵
Ｉ
相差不大，
因此上式写为
½
½½
ω
Δ
）
Ｉ ＋Ｔ
½
（ ·
½
½
½
１
１
１
２
３
４
Ｔ ＝
ω
½
Δ
＋
（
Δ
）
＋
（
Δ
）
＋
（
Δ
）
ω
½
ω
½
ω
½
½
２
！
３
！
４
！
其中矩阵
Ｔ
是一个小量。
½
在计算中至关重要的一点是指数矩阵的存储只½是
上式中的
Ｔ
，
Ｉ Ｔ
）
½它与单½
½
而不是（
½
＋
½
。因为
Ｔ
很小，
½
阵
Ｉ
相加时就成为尾数，
在计算机的舍入操½中其精度将
½
丧失殆½。Ｔ 是增量，
这就是以上所说的第
２个要点。为
½
计算
Ｔ阵，
应先将式½分解
２
２
Ｔ＝（
½
＋Ｔ
）
＝（
½
＋Ｔ
）
Ｉ
Ｉ
½
½
Ｎ
（
－
）
Ｎ
１
２
×（
½
＋Ｔ
）
Ｉ
½
（
－
）
Ｎ
１
１
１
½
½
Ｑ ½
［ （）＋
Ｋ ½
·
ω
½
½
½
½
（
＋
）
½
½
２
２
１
２
１
１
½
½
½
Ｑ ½
［ （）＋
Ｋ ½
·
ω
½
½
）
½
２
½
½
（
＋
２
２
２
１
½
½
Ｑ ½
［ （）＋Ｋ
½
·
ω
½
½
）
３
½
½
（
＋½
½
２
Ｋ ＝
４
２
四元数微分方程的精细积分解法
自从捷联式系统的概念提出以来，
导航算法的研究主
要集中在姿态解算的分析和设计，
这主要是因为姿态误差
对最终的½½信息½响最大的缘故。为了保证算法误差
与惯性传感器引入的误差相比可以½略不计，
积分过程必
须选用高精度的数值积分算法。通用的微分方程积分算法
（
Ｒ ½½Ｋ ½
方法）
如
½½½
½
不½满足捷联式系统对精度的要求，
尤其是在恶劣的运动环境中，
所以不得不研究适合捷联惯
导微分方程的专用数值积分算法，
以降½½动的不可交换
如圆锥（
½½
）
性带来的负面½响，
Ｃ ½ ½
效应、
½
划船（
½½½
）
Ｓ½ ½
½½
Ｐ½ ½ ½ ½ ½ ½ ½
，
Ｉ
是
½ ½Ｉ ½ ½½
½
效应等。精细积分方法（
½ ½ ½ ½ ½½Ｍ ½ ½ ＰＭ）
５
由钟万勰院士在
１９
９４年提出来的一种科学计算方法
［ ½
。
精细积分法主要计算是进行矩阵相乘，
这在计算机上很容
易实现。它一反常用的以差分近似为基础的算法，
½量采
这种分解一直做下去共
Ｎ次。其次，
应注意对任意
½
维矩阵
Ｔ
，
½
有
½
Ｔ
项凤涛， 捷联系统四元数姿态解算的精细积分法
等：
１５
０
（
½
＋Ｔ
）×（
½
＋Ｔ
）
＝Ｉ ＋Ｔ ＋Ｔ ＋Ｔ ×Ｔ
Ｉ
Ｉ
½
½
½
½
½
½
½
将
Ｔ
，
½
½代换为
Ｔ
，
½
Ｔ
½
为避免矩阵运算引起的误差，
采用递推公式
Ｔ
，
＝２
½½
１
＋Ｔ
，
－
×Ｔ
，
－
Ｔ
，
－
½½
½½
１
½½
１
½以上运算循环结束后，
再执行
Ｔ＝
½
＋
½
Ｉ Ｔ
经过
Ｎ次乘法后，
½
已不是小量的矩阵，
Ｔ
再相加没有
严重的舍入误差。以上便是四元数姿态微分中指数矩阵
的精细计算方法。
的姿态角，
并把该姿态角½为真实姿态角
α
（）
０
½
。对四元
数微分方程利用以上两种姿态算法进行求解获得四元数
½
再利用四元数姿态变换矩阵
Ｍ（
）
，
½
与欧拉角之间的关系
式（ ） 计算姿态角
α
½
。本文中姿态算法精度
δ
½
用
２
３
，
（）
（）
种解算方法获得的欧拉角
α
½
与真实欧拉角
α
（）
（）
０
½
之差来
评价，
即
（）＝ （）－
０
½
δ
½
α
½
α
（）
在典型圆锥运动环境下对以上介绍的龙格库塔法与
１０½
圆
½
精细积分方法进行仿真分析。令半锥角
α
＝
π
／８ ½
，
锥运动角频率
ω
＝
π
½ ／
每隔
Ｔ＝
．１½
½ ½
½
，
００
采样一次角速
０
２
度
ω
的值。每隔
½
２ ００
更新一次姿态四元数。仿
＝
Ｔ＝
．２½
真时间设为
½
１
。龙格库塔法与精细积分方法的算法
＝ ０½
对比如图
１½
精度，
３所示。
３
仿真分析及结论
３１
．
仿真分析
在经典圆锥运动条件下进行算法的仿真分析。因为
在经典圆锥运动条件下，
角速度、
姿态四元数的解析表达
６
½
而且可以充分激励圆锥效应误差
［
－７
。
式½可以精确求得，
经典圆锥运动角速度的解析表达式为
（）＝
ω
½
½ ½
½ ½
０
½
ω
１－ ½
α
ω
½ ½
ω
［
－
０
（
½½
）
－
０
½
α
½
０
½
ω
½
α
½
ω
½
Ｔ
０
½ ½
姿态四元数的解析表达式为
Ｑ½
（）＝
½½
α
２
½
α
２ ½½
ω
）
½
α
２ ½
ω
）
½
½
½
［
½
（
／
）
０ ½
（
／
）
½
（
０
½
½
（
／
）
½
（
０
½
½
如果知道某一时刻的四元数
½
那么就可以求出该
值，
时刻的姿态变换矩阵
Ｔ＝Ｍ（
）＝
½
２
２
２
２
（
１ ２ ０ ３
（
１ ３ ０ ２
０
１
２
３
½
＋½ －½ －½ ２
½½
－½
½
）
２
½½
＋½
½
）
２
２
２
２
２
½½
＋½
½
）
½
－½ ＋½ －½ ２
½½
－½
½
）
（
１ ２ ０ ３
（
２ ３ ０ １
０
１
２
３
２
２
２
２
２
½½
－½
½
）
２
½½
＋½
½
）
½
－½ －½ ＋½
（
１ ３ ０ ２
（
２ ３ ０ １
０
１
２
３
图
１
俯仰角精度误差的对比
假设俯仰角
φ
的取值范围为（－
０
，
０
） 滚动角
γ
９
°
９
°
，
的取值范 围 为 （－１
０
，
８
°
， 航 角
ψ
的 取 值 范 围 为
８
°
１０
） 偏
（－
８
°
１０
） 且
φ ψ γ
与四元数姿态变换矩阵Ｍ（ ）
１０
，
８
°
。 ， ，
½
中
½
中的元素可以先
的元素具有十分密切的关系。根据
Ｍ（
）
１
确定出各个欧拉角的主值
［ ½
½½
Ｔ
２
½
φ
＝½
½½
（
３
）
½
½½
（
３
Ｔ
３
½
γ
＝½
½ ½
－Ｔ
１
／
３
）
½
½½
（
１
Ｔ
２
½
ψ
＝½
½ ½
－Ｔ
２
／
２
）
½
然后根据如下关系确定真实的欧拉角值
φ
＝
φ
½
γ
＝
γ
＋１
０
，
Ｔ
３
＜０且
γ
＜０
８
０
½
３
½
½
８
０
½
３
γ
＋１
０
，
Ｔ
３
＜０且
γ
＞
０
½
½
ψ
，
Ｔ
２
＞０
½
½
２
γ
，
Ｔ
３
＞０
½
½
３
图
２
方½角精度误差的对比
{
ψ
＝
ψ
＋１
０
，
Ｔ
２
＜０且
ψ
＜０
８
０
½
２
½
½
８
０
½
２
ψ
＋１
０
，
Ｔ
２
＜０且
ψ
＞
０
½
½
图
３
横滚角精度误差的对比
根据姿态四元数的解析表达式可以计算出任意时刻
{
四川兵工学报
１６
０
３２
． 结论分析
从仿真结果可以看出，
在俯仰角平面内，
由于受到圆
锥效应的½响，
四阶龙格库塔方法存在一定的算法漂移误
差，
即计算误差会随着时间的增长而变大，
而方½角和横
滚角的算法误差很接近。在相同仿真条件下，
基于精细积
分方法的姿态解算的计算误差要明显小于四阶龙格库塔
算法，
由此可以说明该方法的计算精度要高于四阶龙格库
塔方法。而利用精细积分方法则可以有效抑制圆锥效应
漂移误差的½响。就运算速度方面，
随着算法仿真时间的
增加，
该算法的计算量将成倍增加，
运算速度也会迅速下
降。该计算方法的计算精度较高，
½然对于大样本数据，
运算速度不够理想，
½对于姿态更新速度较快，
仿真时间
较短的捷联惯性导航系统上还是很适用的。在相反情况
下，
如果仿真时间很长时，
限于运算速度，
该方法的应用就
会受到很大限制。此外，
该方法可在较大的步长区间（
如
０１
）
．½
相对于龙格库塔迭代算法仍保持比较½的计算精
度，
有效扩展了系统的频谱带½。捷联惯性导航算法的发
展过程就是一个寻求高动态环境下高精度数值积分算法
的过程
［½
７
参考文献：
［½
１ 秦永元．
惯性导航［
．
Ｍ½
北京：
科学出版社，
０６
２０ ．
［ ½
½½ ． ½½½ ½ ½½ ½½ ½½ ½½ ½½½½ ½½
２ Ｐ½ Ｇ Ｓ½½．Ｓ ½½½ ½ ½½ ½½ ½½ ½ ½
½
½½
½ ½ ½
½½½ ½ ½ ½½ ½½½
：Ａ
½½½Ａ½½½ ½ Ｊ ．
½ ½ ½½ ½½ ½１ ½½
½ ½ ½½
½
½
½½ ½ ½½
［ ½
½½
Ｊ½½½½ ½ ½½
，½
½ ½ ½ ½½½½
，
９８ ２
½½½ ½½½ ½½ ½½ ½½½ ½½ ½ １９
，
１
½
½
½
（ ）
１ ２．
１
：
９－ ８
［½
３ 张洵安，
姜节胜．
结构非线性动力方程的精细积分算
法［½ 应用力学学报，
００１
（ ）
１４－ ６ ．
Ｊ．
２０
，
７ ４
：
６ １８
［ ½
½½ ． ½½ ½
，
½½Ｌ Ｗ½½ ．捷联惯性导航技
４ Ｄ ½
½Ｈ Ｔ½½ ½ Ｊ½ ． ½ ½
½½
½
术［
．２版．张
天 光， 秀 萍， ½ 霞， 译．
Ｍ½
王
王
等， 北
京：
½防工业出版社，
０４
２０ ．
［½
５ 邓子辰，
鲁晓旭．
基于精细计算的结构瞬时最优预测
Ｊ．
２０
，
７ ２
：
７－ ０ ．
控制［½ 郑州大学学报，
０７２
（ ）
９ １０
［½
６ 钟万勰．
线性二次最优控制的精细积分法［ ½ 自动
Ｊ．
２０
，
７ ２
：
６ １３
化学报，
０１ ２
（ ）
１６－ ７ ．
［½
７ 李涛，
武元新．
捷联惯性导肮系统误差模型综述［ ½
Ｊ．
中½惯性技术学报，
０３１
（ ）
６ ７ ．
２０
，
１ ４
：
６－ ２
（
责任编辑 周江川
）
，
目前，
在中½精度领域已经完全取代了平台式
系统，
精细积分方法会是捷联系统在向高精度应用领域渗
透发展的一次重要探索。
檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶檶
（
上接第
８
５页）
Ｍ ½½
½½
电流仿真（
½
取
２来计算）
½
½
结果如图
４
可以得
，
到最终电流１
１６ Ａ
电流放大约为
１
５ ．５½
，
５倍。½然，
½
取值
½
不同，
放大倍数不同，
需根据具½的实验装½进行调整。
［½
１ 孙奇志，
龚兴根，
谢卫平， 高½量爆磁压缩电流发
等．
生器理论分析与实验研究［½
Ｊ．爆炸与冲击，０３２
２０
，
３
（ ）
５ ５．
１
：
１－ ４
［½
２ 林其文，
谢卫平．高电感和高½量放大系数的
Ｍ Ｃ
ＦＧ
的理论分析［ ½ 强激光与粒子束，
００ １
（ ）
４７
Ｊ．
２０
，
２ ４
：
８
－ ９．
４０
［½
３ 王士俊．
螺旋型电流发生器的电感计算［ ½
Ｊ ．爆炸与
１８
，（ ）
２６－ ７ ．
冲击，
９７７ ３
：
６ ２１
［½
４ 孙承纬，
周之奎．
磁通量压缩发生器［
．
Ｍ½
北京：
½防
２０ ．
工业出版社，
０８
［½
５ 朱绪斐，
张智军．
电磁炸弹中磁通压缩发生器的动态
电感分 析 ［ ½ 空 军 工 程 大 学 学 报，
０４ ５ ５
：
５
Ｊ．
２０
， （ ）
１
图
４
发生器电流输出波½
－７
１．
［½
６ 陈冬群，
曹胜光，
刘永贵， 圆柱型螺线管爆磁压缩
等．
发生器实验研究［½ 电工电½新技术，
０２ ２
（ ）
Ｊ．
２０
，
１ ３
：
４ ５．
９－ ２
根据磁通量压缩发生器的工½原理及等效电路模型，
对动态电感及电阻进行分析计算，
利用
Ｍ ½½
½½
½件对单根
½
绕线的螺旋型压缩发生器进行电流仿真，
可以对发生器性
½进行简单设计和预估。
（
责任编辑 陈 松）
［½
７ 解江远，
闫自让．
爆磁压缩发生器电枢膨胀角对磁通
损失的½响［½ 四川兵工学报，
００ １
：
９－ ０ ．
Ｊ．
２１
（ ）
９ １０
参考文献：
５
结束语
京公网安备 11010802033920号
Copyright © 2005-2024 EEWORLD.com.cn, Inc. All rights reserved
评论