第 21 卷第 6 期 系 统 仿 真 学 报© Vol. 21 No. 6 2009 年 3 月 Journal of System Simulation Mar., 2009 全角度姿态角解算方法研究与仿真 周 亢，闫建国，屈耀红 （西北工业大学自动化学院，西安 710072） 摘 要：针对传统四元数法在求解捷联惯性导航姿态角受限的问题，提出了一种基于欧拉角在转动 过程中的变化特性的新的计算方法。该方法利用四元数到欧拉角转换的特点，并结合三轴姿态角 在不同区间的变化特点，进而作出准确的区间转移的判断，最后实现全角度范围变化条件下姿态 角的转换计算。仿真结果表明，该方法可以很好的应用于大机动飞行条件下。 关键词：四元数；全角度；姿态；区间转移 中图分类号：TP391.9 文献标识码：A 文章编号：1004-731X (2009) 06-1697-04 Research and Simulation on Method of Calculating Full-scale Attitude ZHOU Kang, YAN Jian-guo, QU Yao-hong (Automation College of Northwestern Polytechnical University, Xi’an 710072, China) Abstract: Based on the problem of limitation in calculating attitude by quaternion way in strapdown INS, a new calculation way which based on transformation of Euler scale in rolling process was proposed. This method utilized the characteristic of transformation of Euler angle, and combined the characteristic of transformation of three-axis attitude in different fields, then made the correct judgment of field’s transformation; in the end the attitude’s transform calculation under full-scale transform condition could be actualized. Simulation results show that this method can be used in high maneuver flight condition effectively. Key words: quaternion; full-scale; attitude; field’s transformation 引 言1 数十年来，四元数及其解法成功地应用于捷联惯性导航 和制导系统中，成为经典的算法,它定义了从导航坐标系到 达机体坐标系的四元数，然后给出四元数更新方程，再根据 实时确定的四元数求出体系到导航坐标系的方向余弦矩阵， 以便将测得的体系的视速度增量转换到导航系[1]。四元数法 已成为飞行姿态解算中的主流方法。但是，通过研究飞行姿 态矩阵就可以发现，从四元数到欧拉角之间的转换只适用俯 [-90 ,90 ] 的情况下，一旦出现俯仰角在整个坐标系 仰角在 0 范围内变化幅度也达到全角度的360度的范围，则由于姿态 矩阵中没有一个数值可以预先确定正负性而无法判断各个 0 姿态角的范围。而这种情况在现在的很多领域都有比较大的 需求，例如小型战斗机做大机动、全角度的翻转运动，飞行 器救生设施（弹射座椅，降落伞等）的工作控制，航天器在 外太空的复杂运动等等，均要求三轴欧拉角都在全空间内发 生变化，因此，实现全角度条件下姿态角的计算问题，在当 前具有较大的实用意义。 在上述问题的研究上，文献[2]中提出了一种扩展四 元数表示欧拉角范围的方法，但是仅仅着眼于单个四元数 的转换，没有分析到转换的实质以及飞行器实际飞行过程 复杂性[3]，仅限于单数值的理论推导。并且，计算方法过 于繁琐，对于每一个四元数都需要计算两次，然后进行比 较，大大增加了计算量。文献[4]中提到利用欧拉角导数 判断转换的方向，但是由于在实际过程中，欧拉角导数的 计算需要其它一些实时变量，因此很难适用于简单系统的 计算。 针对上述不足，本文利用三轴欧拉角在全角度范围内变 化的特性，根据飞行器做大范围姿态转换时三轴欧拉角的变 化特点，提出了一种新的快捷的判断方法。通过仿真示例验 算，该方法是可行的，满足实际工程的应用要求。 1 飞行姿态矩阵与全角度条件下欧拉角的 四元数表示方法 取地理坐标系的形式为欧美式坐标系，机体转动按照 绕偏航轴（Z 轴）转动ψ，俯仰轴（Y 轴）转动θ，滚转 轴（X 轴）转动γ的顺序转动时，飞机姿态矩阵 A 可表示 为（1）式[5]：若用四元数法表示[6]则为（2）其中用 q0 , q1 ,q2 , q3 表 示 四 元 数 ， 均 为 实 数 ， 根 据 定 义 ， 四 元 数 满 足 q q = 。这一条件可作为四元数计算过程中 2 2 0 3 的检验修正条件。 + q 2 1 + + q 1 2 2 收稿日期：2007-08-03 修回日期：2009-01-07 作者简介：周亢(1983-)男, 河南南阳人, 硕士生,. 研究方向为控制理论 与控制工程等；闫建国(1956-)男, 上海人, 教授, 研究方向为智能控制, 导航制导与控制等；屈耀红(1971-)男, 陕西合阳人, 博士, 研究方向为导 航制导与控制、航迹规划等 = ⎛ ⎜ ⎜ ⎜ ⎝ • 1697 • A = 1 0 0 ⎛ ⎜ ⎜ ⎜ ⎝ 0 cos γ sin γ − 0 sin γ cos γ ⎞⎛ ⎟⎜ ⎟⎜ ⎟⎜ ⎠⎝ cos θ 0 sin θ 0 1 0 − sin 0 cos θ θ ψ ψ ψ ψ cos sin − 0 ⎞⎛ ⎟⎜ ⎟⎜ ⎟⎜ ⎠⎝ sin cos 0 0 0 1 ⎞ ⎟ ⎟ ⎟ ⎠ θ ψ ⎞ ⎟ sin sin sin γ θ ψ γ ψ γ θ ψ γ ψ γ θ ⎟ ⎟ cos sin s in ψ γ ψ γ θ γ θ ψ γ ψ γ θ ⎠ θ sin cos cos cos cos cos − + cos sin + − sin sin cos cos sin cos cos cos sin cos cos sin sin sin θ ψ − sin (1) 

第 21 卷第 6 期 Vol. 21 No. 6 2009 年 3 月 系 统 仿 真 学 报 Mar., 2009 A = q 2 3 ) ) q q q − − + 2 2 2 2 1 0 q q q q 2( − 0 3 1 2 q q q q 2( + 0 2 1 3 A A A 11 12 13 A A A 23 22 21 A A A 31 32 33 ⎛ ⎜ ⎜ ⎜ ⎝ ⎡ ⎢ = ⎢ ⎢ ⎣ ⎤ ⎥ ⎥ ⎥ ⎦ q q 2( 1 2 q q − 2 2 0 1 q q 2( 2 3 q q + 0 3 q − + 2 2 q q − 0 1 ) q 2 3 ) q q q q 2( − 1 3 0 2 q q q q 2( + 0 1 2 3 q q q − + − 2 2 2 0 1 2 ) ) q 2 3 ⎞ ⎟ ⎟ ⎟ ⎠ (2) 由上面的分析可以看出，在俯仰角处于全角度变化，即 [-180 ,180 ] 范围内时，由(5)，(6)两种表示方式，单个 0 0 位于 四元数可以对应于两个不同的欧拉角。 1.2 奇异点表示方法 四元数到欧拉角的转换是通过超复数的映像概念求出， 可表示三个四元数相乘仍是一个四元数，所得表示坐标连续 三次旋转后的四元数： q = = q q q γ θ ψ γ ) 2 (cos( − i sin( ))(cos( − j sin( ))(cos( γ 2 θ ) 2 θ 2 − k sin( ψ )) 2 (3) ψ ) 2 将(3)式展开，可得到姿态角与四元数之间的转换关系： 0 sin( ⎡ ⎢ ⎢ ⎢ ⎢ ⎣ cos( )cos( )cos( q q 1 q q ⎡ ⎢ ⎢ ⎤ ⎢ ⎥ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ ⎢ ⎦ ⎢ ⎢ ⎢ ⎣ γ 2 γ )cos( 2 γ cos( 2 γ 2 θ ψ ) 2 2 θ ψ ) )cos( 2 2 θ ψ ) 2 2 θ ψ ) )cos( 2 2 1.1 欧拉角基本表示方法 )cos( )sin( )sin( cos( 2 3 + sin( − sin( + sin( − sin( )sin( )sin( γ 2 γ )sin( 2 γ 2 γ 2 θ ψ ) 2 2 θ ψ ) )sin( 2 2 θ ψ ) )cos( 2 2 θ ψ ) )cos( 2 2 )sin( )sin( ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥⎦ (4)式表示四元数和欧拉角的之间的关系，可以将四元数 转化成为相应的欧拉角： (5) 主 ) ) = ＝ 主 arcsin( arctan( A − 13 A 23 A 33 A 12 A 11 θ ⎧ ⎪ ⎪⎪ γ ⎨ ⎪ ⎪ ψ ⎪⎩ 由(1)的表示方式可以看出，在俯仰角在 0 arctan( ＝ 主 ) (-90 ,90 ) 的情 况下，由于 cosθ的值可以确定为正值，则可以实现在常规 [-90 ,90 ] ，滚 条件下欧拉角的范围转换（俯仰角的范围为 0 [-180 ,180 ] ，偏航角的范围为 0 [0 ,360 ] ）当 转角的范围为 A33 和 A11 均不为零时，表示方法如下： 0 0 0 0 0 0 0 主 23 23 33 33 = 0 0 > < > < < 0 ∩ 0 A ∩ 0 A A 33 主 180 A ＋ 主 180 A － 主 ⎧ γ ⎧ ⎪ ⎪= γ γ ⎨ ⎪ ⎪ ⎪ γ ⎩ ⎪⎪ θ θ ⎨ ⎪ ψ ⎧ ⎪ ⎪ ⎪ ψ ψ ⎨ ⎪ ⎪ ψ ⎪ ⎩ ⎩ 为了实现全姿态四元数的转换，文献[2]结合 cosθ为负 值时的情形，即俯仰角在 II，III 象限内时，欧拉角的转换可 表示为： A 11 主 180 A ＋ 11 主 360 A ＋ 0 11 主 ∩ 0 A 0 12 0 ∩ 0 A 0 12 > < > > = < 0 (6) 0 0 33 A ⎧ γ ⎧ 33 主 ⎪ ⎪= 180 A γ γ ＋ ⎨ ⎪ 主 ⎪ ⎪ 180 A γ － ⎩ 主 ⎪⎪ 180 * θ θ = − 0 ⎨ ⎪ A ψ ⎧ ⎪ 11 主 ⎪ ⎪ 180 A ψ ψ ＋ ⎨ 11 主 ⎪ ⎪ 360 A ψ 主＋ ⎪ ⎩ ⎩ 11 sign A 13 − = ( ) 主 33 0 0 < > > 0 ∩ 0 A ∩ 0 A < > 0 0 23 23 < > < < ∩ 0 0 A 12 0 ∩ 0 0 A 12 > • 1698 • 由上面的分析可以看出，在姿态角两种表示法中，都存 在着奇异点的情况。即当 A33 或 A11 为零时，上述式子中的 滚转角和偏航角便不能正确的表达。这些点主要出现在转换 边界点的附近[7]。 由(2)，(5)式可以看出，当 A33 为零时，即 cos cosγ θ=0， 0θ≠ ，则γ主 可能得到两个值，在其定义域范围内， 假设 cos 可能为 90 度或是-90 度。同理，当 A11 为零时，假设 cos 0θ≠ , 则ψ主 可能为 90 度或是 270 度。因为在实际工程应用中，不 可能出现相邻两拍出现较大的跳变，且飞行器初始状态不可 (4) 能位于这些点上，因此可以这两个可能值与前一拍的差值来 判断（取采样两拍之间的合理差值低于 180 度）。取差值在 合理范围内的那个值作为准确值。 当 A33 或 A11 为零且 cos θ= ± 时，情况比 较复杂，因为此时飞机三个姿态角自由度已退化为两个姿态 0θ= ，即 090 角自由度，只能判断航向角与滚转角之和或差而不能判断它 们各自的值[8]。因为θ不存在奇异点，此时 sin 1θ= ± ，则 )γ ψ∓ 的三角函数表达式，可通 A21，A22，A31，A32 均可作为 ( 过反三角函数求出，真值范围可由前一时刻两角所处范围确 定。为了保持计算的连续性，可假设一个值与前一时刻值相 等而求出另一个值，保证在大机动条件下不会出现大的误 差。 2 全角度飞行条件下的姿态角解算算法分析 由上面的分析可知，单个四元数可以对应于两个不同的 欧拉角，为叙述方便，下面将 cosθ为正值的状态称之为状 态一，cosθ为负值的状态称之为状态二（ cosθ为零时因为 处在俯仰角计算的合理范围内，所以它的状态应处在和它的 前一拍相同的状态）。文献[2]仅仅给定四元数验证了计算的 准确性，而在实际应用中，每一时刻的姿态角数值应是变化 的，在实际应用中，必须从姿态角本身的性质来进行判断。 2.1 姿态角状态判断 经过上述分析可知，不同状态间的切换主要是通过俯仰 角位于不同象限，即 cosθ的正负性产生的。对于俯仰角本 身而言，如果原来位于状态一中，当它的值从 89.5 度增加 1 度的话，如果不进行状态转变，得出的值应是减少 1 度的值， 即 88.5 度，而不是应该得出的准确解 90.5 度。此后，如果 一直持续在状态一下进行计算，则计算结果会将连续上升的 (7) 结果计算为连续下降，反之亦然。但是，可以看出，无论是 否进行状态转变，在状态切换点，俯仰角的值的变化始终是 连续的，不会因为错误的状态选择而产生大的跳变。但是， 同样四元数对应在状态一与状态二下的滚转角和偏航角的 

第 21 卷第 6 期 Vol. 21 No. 6 2009 年 3 月 周 亢，等：全角度姿态角解算方法研究与仿真 Mar., 2009 值彼此处于相互对应的象限（如第一象限对应第三象限，第 二象限对应第四象限），正好相差 180 度，这是因为两种状 态的不同选择算法决定的。例如四元数[-0.4451，0.5416， 0.4545，0.5496]，对应状态一下的欧拉角值为[-89.0000， 89.0000，170.0000]，对应状态二下的欧拉角值为[-91.0000， -91.0000，350.0000]。因此，只能通过对滚转角和偏航角在 不同状态下的值进行选择而进行不同状态间的切换。在实际 工程应用中，可使用两拍之间的差值是否合理来判断是否应 进行状态切换。为了保证在判断过程中不出现错误的状态判 断，应保证两拍之间的合理角度差低于相邻两拍间的最大角 度变化值 p,为了不与另一种状态发生混淆，应保证 p<90.例 360 / s ，采样频率为 f，则 如当角速率传感器的反应速度为 0 应保证 f>4，p 值越小，越能与由于四元数在不同状态下计 算所产生的差值相区分。这样，由于飞行器一拍而产生的单 个欧拉角差值为 p± ，在另一种计算状态下产生的差值为 p± ，两者之间不会产生混淆.在实际应用中，应根据实 180 际的频率值和角速率传感器在应用中存在的各种内在的或 是外界的误差或干扰，从而作出合理的分析判断。 另外，在姿态角解算过程中，因为每一拍的计算中都要 将得到的角度主值转化到相应的完整定义域中，这样在各个 定义域的边缘就不可避免会出现大的跳变。例如，当航向角 由 0.1 度再减小 1 度时，根据航向角的定义，会得到 359.1 度，事实上，这是一个连续状态，但是依照计算过程中的数 值来判断，就会得出错误的值。基于飞机只能在连续状态下 变化的事实，如果差值位于 360 度附近，则应视为连续状态。 在程序设计过程中，应充分考虑这种情况，对于位于 180 度 和 360 度之间的差值，应全部取靠近 360 度的差值为真实差 值。例如，若滚转角或偏航角与前一拍的差值为 m,则应依 照下式修正： − sign m m 360 (8) m = ( ) * 这样可以在消除正常状况下由于定义域转化引起的差值的 情况下而不影响由于采用不同状态算法所产生的非正常差 值。鉴于不同状态之间的差值在 180 度附近，可以用这种方 式区别不同情况下的差值。 依照对上述过程的分析，可以使用滚转角和偏航角分别 与前一拍的绝对差值的和来进行判断，如果在一种状态下的 计算过程中绝对差值和超出 180 度（两倍允许的最大 p 值）， 即可则应视为发生了状态切换。即本应该是依另外一种状态 计算的值，错误的在当前状态下计算。此时，应切换至另一 种状态对当前拍进行重新计算，并且在下一拍计算时根据转 换后的状态进行计算。 2.2 初始状态设定 0 位于 0 飞行器在初始飞行时，应处于状态一条件下，即俯仰角 [-90 ,90 ] ，从第二拍的计算过程中开始根据上述条件 判断状态值，对每种状态设置不同的标志位，根据标志位的 不同选择决定进行在不同状态下的欧拉角的计算。 2.3 软件设计流程 根据以上的分析，可得出软件设计的具体流程： 开始 初始状态设置为状态一 解四元数微分方程 继续下一时刻的计算 根据状态标志实现四元数 到欧拉角的转变 求滚转角，偏航角与 上一时刻的差值 N N 差值在180与 360之间 差值转换 绝对差值和>180 状态标志切换 依新状态重新计算 输出欧拉角 结束 图 1 软件设计流程图 四元数更新过程针对前一组四元数进行更新运算，与解 算出的欧拉角无关. 3 算法仿真 为了验证上述算法，利用 MATLAB 中提供的六自由度 模型进行仿真验证，该模型是一个封装好的模拟器，可以给 定飞行力和飞行力矩，输出量为三个欧拉角和三轴角速度， 可以很好的模拟飞行器的飞行状态。为了达到精确的仿真结 果，对此模型设置为定步长模式，步长为 0.02s，即模拟采 样的频率为 50Hz，通过调节初始输入条件，可以实现欧拉 角在全角度范围内的变动，且符合不在极短时间内出现大的 4500 / s ，则在 跳变（实际工程中的角速率传感器一般低于 采样频率 50Hz 条件下一拍内的变化幅度一般不会超过 90 度）。为符合实际情况及方便作出对比，将六自由度模型的 欧拉角输出调整至标准的定义域。 0 由图 2 可见，该模型输出的欧拉角变化范围确实是在大 机 动 全 角 度 范 围 内 ， 且 变 化 幅 度 足 够 大 （ 俯 仰 角 为 [-178.8758，179.3037]，滚转角为[-0.9699，89.7614]，偏航 • 1699 • 

第 21 卷第 6 期 Vol. 21 No. 6 2009 年 3 月 系 统 仿 真 学 报 Mar., 2009 : 度 位 单 200 0 -200 100 0 0 -100 400 0 200 0 0 100 200 300 400 500 600 100 200 300 400 500 600 100 200 300 400 500 600 仿真次数 图 2 在正常定义域下的三轴欧拉角输出理论图形 （从上到下,依次为俯仰角,滚转角,偏航角,下同） : 度 位 单 200 100 0 -100 -200 : 度 位 单 角为[0，359.9801]）。由于作了定义域的修正，在某些点出 现较强烈的变化（实际上应视为连续点，如将航向角由零下 降的点通过定义域修正为接近 360 的值，超出 360 的值修正 为接近 0 的值），可满足大机动飞行条件下的飞行姿态模拟。 在四元数微分方程更新求解过程中，采用精确度较高的 三阶泰勒展开法，得到的三轴欧拉角的误差曲线如图 3 所 示： : 度 位 单 0.2 0 -0.2 1 0 100 200 300 400 500 600 秒（20000 次）下俯仰角值的变化曲线以及在这种变化条件 下理论值与计算值之间的误差曲线。 俯仰角值变化曲线 0 2 1 0 -1 -2 0 0.5 1 1.5 俯仰角值误差曲线 2 仿真次数 2.5 x104 0.5 1 1.5 2 2.5 仿真次数x104 图 4 俯仰角多次大范围变化角的比较曲线 可以看出，在多次连续剧烈变化的过程中，误差值始终 在很小的范围内，跟踪效果良好。 4 结论 由上面的分析和仿真示例可见，此算法很好的解决了俯 仰角在全角度范围内变化的跟踪解算问题，跟踪敏感及时， 且误差扩散范围不大，可以很好运用于大机动飞行条件下。 参考文献： [1] 李连仲，王小虎，蔡述江. 捷联惯性导航、制导系统中方向余弦 矩阵的递推算法[J]. 宇航学报, 2006. [2] 张帆，曹喜滨，邹经湘. 一种新的全角度四元数与欧拉角的转换 [3] 封文春，林贵平. 四元数在弹射座椅性能仿真中的应用[J]. 北京 航空航天大学学报，2006. [4] 左玲，张新国. 用于大机动飞行仿真的四元数改进算法[J]. 系统 仿真学报，2006, 18(11): 3018-3020. [5] 陈哲. 捷联惯导系统原理[M]. 北京: 宇航出版社, 1986. [6] 陈万春，肖业伦. 矢量与四元数的关系及其在飞行力学中的应用 [J]. 宇航学报, 1997. [7] 刘忠，梁晓庚，曹秉刚，贾晓洪. 基于四元数的空间全方位算法 0 100 200 300 400 500 600 算法[J]. 南京理工大学学报, 2002. 0 -1 1 0 -1 0 100 200 300 400 图 3 三轴欧拉角的误差曲线 500 600 仿真次数 三轴欧拉角的误差在仿真过程中的最大偏差值为： [0.1421，0.6947，0.7038]. 为了进一步验证算法对于俯仰角变化的跟踪敏感性以 研究[J]. 西安交通大学学报, 2006. 及计算的稳定性，进一步设计了只在俯仰角大范围连续变 化，而滚转角和偏航角均为 0 度下的算法仿真，图 4 为 400 [8] 陈哲. 全姿态飞机捷联式系统姿态角的计算[J]. 航空学报, 1984. • 1700 •