大角度范围内四元数转化为欧拉角的算法_辛岩
欧拉角四元数转换
欧拉角四元数转换欧拉角和四元数都是用于描述物体在三维空间中的旋转状态的数学表示方法。
欧拉角是一个由三个角度组成的向量，而四元数是一个由四个实数组成的向量。
在计算机图形学和机器人学等领域中，这两种方法都被广泛使用。
欧拉角和四元数之间的转换可以帮助我们在不同的数学表示方法中进行转换，从而更方便地进行计算和应用。
下面是欧拉角和四元数之间的转换公式：欧拉角转四元数：给定欧拉角向量 [α, β, γ]，我们可以通过以下公式将其转换为对应的四元数 [q0, q1, q2, q3]：q0 = cos(α/2)cos(β/2)cos(γ/2) + sin(α/2)sin(β/2)sin(γ/2)q1 = sin(α/2)cos(β/2)cos(γ/2) - cos(α/2)sin(β/2)sin(γ/2)q2 = cos(α/2)sin(β/2)cos(γ/2) + sin(α/2)cos(β/2)sin(γ/2)q3 = cos(α/2)cos(β/2)sin(γ/2) - sin(α/2)sin(β/2)cos(γ/2)四元数转欧拉角：给定四元数 [q0, q1, q2, q3]，我们可以通过以下公式将其转换为对应的欧拉角向量 [α, β, γ]：α = atan2(2(q0q1 + q2q3), 1 - 2(q1^2 + q2^2))β = asin(2(q0q2 - q3q1))γ = atan2(2(q0q3 + q1q2), 1 - 2(q2^2 + q3^2))其中，atan2() 是一个带有两个参数的反正切函数，它可以根据两个参数的正负关系来确定结果的正负号。
通过以上公式，我们可以方便地在欧拉角和四元数之间进行转换，从而更加灵活地描述物体在三维空间中的旋转状态。
旋转矩阵、欧拉角、四元数理论及其转换关系
旋转矩阵、欧拉⾓、四元数理论及其转换关系1. 概述旋转矩阵、欧拉⾓、四元数主要⽤于表⽰坐标系中的旋转关系，它们之间的转换关系可以减⼩⼀些算法的复杂度。
本⽂主要介绍了旋转矩阵、欧拉⾓、四元数的基本理论及其之间的转换关系。
2、原理2.1 旋转矩阵对于两个三维点p1(x1,y1,z1)，p2(x2,y2,z2)，由点 p1 经过旋转矩阵 R 旋转到 p2，则有注：旋转矩阵为正交矩阵RR^T=E任意旋转矩阵：任何⼀个旋转可以表⽰为依次绕着三个旋转轴旋三个⾓度的组合。
这三个⾓度称为欧拉⾓。
三个轴可以指固定的世界坐标系轴，也可以指被旋转的物体坐标系的轴。
三个旋转轴次序不同，会导致结果不同。
2.2 欧拉⾓欧拉⾓有两种：静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静⽌，所以称为静态。
动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态。
使⽤动态欧拉⾓会出现万向锁现象；静态欧拉⾓不存在万向锁的问题。
对于在三维空间⾥的⼀个参考系，任何坐标系的取向，都可以⽤三个欧拉⾓来表现。
参考系⼜称为实验室参考系，是静⽌不动的。
⽽坐标系则固定于刚体，随着刚体的旋转⽽旋转。
如图1，设定xyz-轴为参考系的参考轴。
称xy-平⾯与XY-平⾯的相交为交点线，⽤英⽂字母（N）代表。
zxz顺规的欧拉⾓可以静态地这样定义：α是x-轴与交点线的夹⾓，β是z-轴与Z-轴的夹⾓，γ是交点线与X-轴的夹⾓。
图中三个欧拉⾓分别为：(α,β,γ)；蓝⾊的轴为：xyz轴红⾊的轴为：XYZ轴绿⾊的线为交线：Nα∈[0,2π],β∈[0,π],γ∈[0,2π]很可惜地，对于夹⾓的顺序和标记，夹⾓的两个轴的指定，并没有任何常规。
科学家对此从未达成共识。
每当⽤到欧拉⾓时，我们必须明确的表⽰出夹⾓的顺序，指定其参考轴。
实际上，有许多⽅法可以设定两个坐标系的相对取向。
欧拉⾓⽅法只是其中的⼀种。
此外，不同的作者会⽤不同组合的欧拉⾓来描述，或⽤不同的名字表⽰同样的欧拉⾓。
gl-matrix 欧拉角和四元数相互转化
欧拉角和四元数是在计算机图形学和游戏开发中常用的两种旋转表示方法，它们可以相互转化，用于描述物体的旋转姿态。
gl-matrix 是一种专门用于线性代数的JavaScript 库，提供了便利的函数来处理矩阵、向量、旋转等操作。
在 gl-matrix 中，也提供了欧拉角和四元数相互转化的函数，方便开发者在使用这两种表示方法时进行转换。
本文将介绍 gl-matrix 中欧拉角和四元数的相互转化方法，并结合实际案例进行详细讲解。
1. 欧拉角和四元数的概念在计算机图形学和游戏开发中，欧拉角和四元数是用来描述物体旋转的常用方法。
欧拉角是通过旋转顺序和旋转角度来描述物体在三维空间中的旋转姿态，一般包括绕 x 轴、y 轴和 z 轴的旋转角度。
而四元数是一种复数的扩展，可以用来表示三维空间中的旋转，其具有较好的数学性质和计算性能。
2. gl-matrix 中的欧拉角和四元数表示gl-matrix 是一种快速、轻量级且又开放源代码的数学库，用于矢量和矩阵运算。
在 gl-matrix 中，欧拉角和四元数都是以数组的形式来表示的。
欧拉角可以用一个包含三个旋转角度的数组来表示，而四元数则可以用包含四个元素的数组来表示。
3. 欧拉角和四元数相互转化的原理欧拉角和四元数是两种不同的旋转表示方法，它们之间存在着一定的转换关系。
在 gl-matrix 中，提供了方便的函数来实现欧拉角和四元数的相互转化。
欧拉角转四元数的原理是先将欧拉角转化为旋转矩阵，再将旋转矩阵转化为四元数；而四元数转欧拉角的原理是先将四元数转化为旋转矩阵，再将旋转矩阵转化为欧拉角。
4. 在 gl-matrix 中实现欧拉角和四元数的相互转化接下来，我们将通过一个具体的案例来演示在 gl-matrix 中如何实现欧拉角和四元数的相互转化。
假设我们有一个物体，其初始的欧拉角为 [0, 0, 0]，我们需要将其转化为四元数表示。
我们可以使用 gl-matrix 提供的函数 'quat.create()' 来创建一个四元数：```javascriptvar quaternion = quat.create();```我们可以使用 gl-matrix 提供的函数 'quat.fromEuler()' 来将欧拉角转化为四元数：```javascriptvar euler = [0, 0, 0];quat.fromEuler(quaternion, euler[0], euler[1], euler[2]);```通过上述操作，我们就可以将初始的欧拉角 [0, 0, 0] 转化为对应的四元数表示。
四元数与欧拉角（RPY角）的相互转换
四元数与欧拉⾓（RPY⾓）的相互转换RPY⾓与Z-Y-X欧拉⾓ 描述坐标系{B}相对于参考坐标系{A}的姿态有两种⽅式。
第⼀种是绕固定（参考）坐标轴旋转：假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转γ，然后绕{A}的Y轴旋转β，最后绕{A}的Z轴旋转α，就能旋转到当前姿态。
可以称其为X-Y-Z fixed angles或RPY⾓(Roll, Pitch, Yaw)。
Roll:横滚 Pitch: 俯仰Yaw: 偏航（航向） 由于是绕固定坐标系旋转，则旋转矩阵为（cα is shorthand for cosα, sα is shorthand for sinα,and so on.）R XYZ(γ,β,α)=R Z(α)R Y(β)R X(γ)=cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ 另⼀种姿态描述⽅式是绕⾃⾝坐标轴旋转：假设开始两个坐标系重合，先将{B}绕⾃⾝的Z轴旋转α，然后绕Y轴旋转β，最后绕X轴旋转γ，就能旋转到当前姿态。
称其为Z-Y-X欧拉⾓，由于是绕⾃⾝坐标轴进⾏旋转，则旋转矩阵为：R Z′Y′X′(α,β,γ)=R Z(α)R Y(β)R X(γ)=cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαcβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ 可以发现这两种描述⽅式得到的旋转矩阵是⼀样的，即绕固定坐标轴X-Y-Z旋转(γ,β,α)和绕⾃⾝坐标轴Z-Y-X旋转(α,β,γ)的最终结果⼀样，只是描述的⽅法有差别⽽已。
In gerenal: three rotations taken about fixed axes yield the same final orientation as the same three rotations taken in opposite order about the axes of the moving frame.Axis-Angle与四元数 绕坐标轴的多次旋转可以等效为绕某⼀转轴旋转⼀定的⾓度。
四元数和欧拉角
四元数和欧拉角
四元数与欧拉角：
1、四元数
四元数是一种数学物体，它由ONE个实部和THREE个虚部组成。
实部是非负实数，其余的THREE个部分是虚数。
四元数的表达式可以表示为：Q=a+bi+cj+dk (a、b、c、d是实实部）。
它们有助于以三维空间中的方式表示方向和旋转。
离散空间中表示四元数，当旋转发生时，它们保持一致。
2、欧拉角：
欧拉角是一种三维旋转矢量，可以用来表示不同方向之间的关系。
它将旋转分成THREE个不同的轴，分别为X轴、Y轴和Z轴。
每个轴都有自己的旋转角度，比如X轴有X角度，Y轴有Y角度，Z轴有Z角度。
欧拉角的表达式可以写作：[X,Y,Z]，其中X、Y、Z分别表示每个轴的旋转角度。
欧拉角与四元数之间的差别在于欧拉角表示的是不同方向间的关系，而四元数表示的是不同方向的空间旋转。
可以说四元数和欧拉角均是用来表示旋转有关信息的，但是又是有所差别的。
下面我们来总结下它们两者之间的区别：
1）表示方式不同：欧拉角表示的是不同方向间的关系，而四元数是表
示不同方向的空间旋转；
2）用途不同：欧拉角被广泛应用于3D图形学处理领域，可以很方便
地实现3D模型的几何变换；而四元数方便地表示空间任意的旋转变换，是用来表达机器人的运动控制中的基本角度表示方式；
3）值域不同：欧拉角的值域为[0,2π)，而四元数的值域为[-1,1]；
4）叠加角度表示方法不同：四元数是可以用乘法表达累加旋转；而欧
拉角只能使用加法来表达叠加旋转。
欧拉角对四元数求导
我们要找出欧拉角（Euler angles）对四元数（quaternion）的导数。
首先，我们需要了解欧拉角和四元数的基本定义和关系。
欧拉角是用来描述三维空间中物体方向的一组角度，通常表示为(φ, θ, ψ)。
四元数是一种复数扩展，用于表示三维空间中的旋转，通常表示为(q0, q1, q2, q3)。
欧拉角和四元数之间的关系可以通过一系列的转换公式来表示。
为了简化问题，我们假设使用Z-Y-X欧拉角。
根据Z-Y-X欧拉角到四元数的转换公式，我们有：q0 = cos(ψ/2) * cos(θ/2) * cos(φ/2) + sin(ψ/2) * sin(θ/2) * sin(φ/2)q1 = sin(ψ/2) * cos(θ/2) * cos(φ/2) - cos(ψ/2) * sin(θ/2) * sin(φ/2)q2 = cos(ψ/2) * sin(θ/2) * cos(φ/2) + sin(ψ/2) * cos(θ/2) * sin(φ/2)q3 = cos(ψ/2) * cos(θ/2) * sin(φ/2) - sin(ψ/2) * sin(θ/2) * cos(φ/2)为了找到欧拉角对四元数的导数，我们需要对上述每个四元数组件分别求导。
这将涉及到对cos和sin函数的求导，并使用链式法则。
针对q0的求导示例：δq0/δφ = -1/2 * sin(ψ/2) * cos(θ/2) * sin(φ/2) - 1/2 * cos(ψ/2) * sin(θ/2) * cos(φ/2)同理，我们可以求出δq0/δθ和δq0/δψ，以及其他q1, q2, q3对欧拉角的导数。
注意，这是一个相对复杂的过程，涉及到多个三角函数的求导和组合。
最终结果会是一系列与欧拉角相关的三角函数表达式。
大话多旋翼飞行器--欧拉角与四元数
始先绕着 z-轴旋转 角值。然后，绕着 x-轴旋转 角值。最后，绕着 z-轴作角值 的
旋转” 我们通常使用的欧拉角是服从描述A）的。也就是绕着固定于刚体的坐标轴的三个旋转的复 合。为什么呢？因为对物体施加的转矩通常都是相对于物体自身的坐标轴的，控制上更直观 更容易。 另一个问题，欧拉角的三次旋转的旋转轴和旋转顺序。根据维基百科的说法，“在经典力 学里，时常用 zxz 顺规来设定欧拉角；照着第二个转动轴的轴名，简称为 x 顺规。另外， 还有别种欧拉角组。合法的欧拉角组中，唯一的限制是，任何两个连续的旋转，必须绕着 不同的转动轴旋转。因此，一共有 12 种顺规。例如，y 顺规，第二个转动轴是 y‐轴，时 常用在量子力学，核子物理学，粒子物理学。另外，还有一种顺规，xyz 顺规，是用在航空 航天工程学”。
a2通常用 表示，代表升降或俯仰（elevation or pitch）
a3通常用 表示，代表倾斜或横滚（bank or roll）
注意：有些书上a3用φ表示，其实 和φ是同一个字母的两种写法，本质是一样的。
小常识：
根据第一部分的介绍，可以知道，经过三个欧拉角转动后，世界坐标系下的一个矢量 rW=(xW,yW,zW)与其对应的运载体坐标系下的矢量rB=(xB,yB,zB)之间的关系可以表示为
个多体系统提供一个统一的参照坐标系。在物体上要建立一个局部坐标系，称为 BCS（Body Coordinate System），一方面用来描述物体在 GCS 内的位置和姿态，另一方面，为物体上的 点或其它坐标系提供局部的确定位置和姿态的标准。此外，在物体上可以根据需要建立其它 的坐标系，例如，为描述物体上的约束及列写约束方程，需要建立约束的坐标系。 //==========================================================================
四元数转欧拉角公式
四元数转欧拉角公式1 四元数简介四元数是一种数学上的扩张，它可以用来表示旋转、仿射变换、坐标变换等多种数学表示。
四元数由一个标量和一个三维向量组成，其中标量称为实部，三维向量称为虚部。
2 四元数转欧拉角欧拉角是指绕三个坐标轴的旋转角度，分别称为滚动角、俯仰角和偏向角。
将四元数转换为欧拉角可以用以下公式：psi = atan2(2(q0*q1 + q2*q3), 1 - 2(q1*q1 + q2*q2))theta = asin(2(q0*q2 - q3*q1))phi = atan2(2(q0*q3 + q1*q2), 1 - 2(q2*q2 + q3*q3))其中psi、theta、phi分别对应欧拉角的偏向角、俯仰角和滚动角，q0、q1、q2、q3为四元数的四个分量。
3 四元数转欧拉角的原理通过四元数和欧拉角的定义，我们可以知道，欧拉角相当于描述了三个坐标轴的旋转，而四元数可以表示任意旋转。
因此，我们可以使用四元数来表示欧拉角相应的旋转，然后通过比较四元数和欧拉角的定义，推出它们之间的关系式。
4 四元数转欧拉角的应用四元数转欧拉角常常用于3D游戏、计算机图形学等领域。
在3D 游戏中，我们需要获取角色的坐标和旋转信息，通过四元数转换欧拉角可以获得相应的旋转角度；在计算机图形学中，我们需要将三维模型转换为投影到二维屏幕上的二维图像，通过四元数转换欧拉角可以对三维对象进行旋转变换。
5 结论四元数和欧拉角是两种不同的数学表达方式，但它们之间存在一定的关系，通过转换可以实现相互转换。
四元数转换为欧拉角的公式虽然有些复杂，但是在实际应用中可以轻松地进行计算，并且可以提高程序的效率和可读性。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
ψ = arcctg(− A22 ) A21
结束
图 4 ψ 的求取算法流程图
图 5 卫星绕俯仰轴机动 180 度
图 6 卫星绕偏航轴机动正 90 度
135
全国第十二届空间及运动体控制技术学术年会论文
4 结论
图 7 卫星绕偏航轴机动负 90 度
本文对已有的欧拉角和四元数之间的转化关系进行了分析，在此基础上提出了转化算法的值 域拓展问题，解决了大角度情况下欧拉角存在奇异性的问题。通过数学仿真可以看出，所设计的 算法可以实现大角度的姿态机动，而且没有出现异常。所以，设计的算法完全合理，可以应用到 各种情况。
第 n 列对应的元素，以下同。 但上述三式仅适用于ϕ、θ、ψ ∈ (− π ，π ) 的情况。当ϕ、θ、ψ ∈ (−π，π ) 时，如果还用 22
这三式求欧拉角，将造成仿真的失败。因此，必须考虑转化算法的ϕ、θ、ψ 值域拓展问题，以
便能适合大角度机动时的姿态控制系统的仿真。本文设计了一种大角度范围内将四元数转化为欧
2.1 算法设计
1°对于ϕ 角
a． A22 ≥ 0或者A33 ≥ 0，
即ϕ
∈ [－
π
π ，
]时
,
22
ϕ = arcsin A23＝arcsin B23 ；
b．
A22
<
0&
A33
<
0&
A23
≥
0，即ϕ
∈
π (
，π
]时，
2
ϕ = π − arcsin A23 = π − arcsin B23 ；
c．
A22
> −2& A22
< 0即ψ
∈(π
+ arctg(−2)，π]时，ψ＝π
+
arctg(−
A21 A22
)
；
g．
−
2
≥
−
A21 A22
&
A22
≥
0，即ψ
∈[−
π 2
，arctg(−2)]时，
ψ＝− π + arcctg(− A22 ) = − π − arctg(− A22 ) ；
A21
2
A21
h．
−2
2
A22 ) = A21
π 2
− arctg(−
A22 ) ；
A21
b． −
A21 A22
>
2&
A22
≤ 0，即ψ
∈[− π ，arctg2 − π）时，ψ＝− π
2
+
arcctg(−
A22 A21
)
=
−π 2
−
arctg(−
A22 A21
)；
c． 2
≥
−
A21 A22
≥
0&
A22
>
0，即ψ
∈[0，arctg 2]时， ψ＝arctg(−
A21 A22
)
；
d． 2 ≥ −
A21 A22
≥ 0 & A22
< 0，即ψ
∈[arctg2 −π，−π]时，ψ＝− π
+
arctg(−
A21 A22
)
；
e．
0≥−
A21 A22
> −2&A22
> 0，即ψ
∈[0，arctg(−2))时，ψ＝arctg(−
A21 A22
)
；
f． 0 ≥ −
A21 A22
≥
−
A21 A22
&
A22
≤
0，即ψ
∈[π 2
，π
+ arctg(−2)]时,ψ＝arcctg(−
A22 A21
)
=
π 2
−
arctg(−
A22 A21
)
。
i）对于θ 角，其规律同ψ 角，这里不再赘述。
2.2 算法流程
ϕ 和ψ 的算法的流程图如图 3、图 4 所示，θ 的算法的流程图与ψ 类似，这里不再给出。
132
全国第十二届空间及运动体控制技术学术年会论文
欧拉角的公式如下：
ϕ = arcsin A23 = arcsin B23 = arcsin(2q0q1 + 2q2q3 )
θ = arctg(− A13 ) = arctg(− B13 ) = arctg( 2q0q2 − 2q1q3 )
A33
B33
⎡cos γ
A312
(ψ
,φ,θ
)
=
⎢ ⎢
0
⎢⎣sin γ
sin γsign(sin ϕ) 0
− sign(sinϕ) cosγ
0⎤
sin
ϕ
⎥ ⎥
（6）
0 ⎥⎦
显然，满足条件的θ、ψ 有无穷多个。针对这种奇异情况，如果假设：在一个仿真步长内，
姿态角的变化不会太大，至多不会超过 90°，在仿真中，可让θ、ψ 保持前一时刻的值。
134
全国第十二届空间及运动体控制技术学术年会论文
开始
cosϕ = 0 N
ϕ 根据前一时刻的值确定， θ、ψ 维持前一时刻的值
ψ = arcctg(− A22 ) Y A21
A22 > 0
Y
A21 > 2 A22
N
N
ψ = −π +arcctg(− A22 )
A21
A21 > 0 Y A22 > 0 A22 Y
转（6 种），第二类：非对称旋转（6 种）。描述三轴稳定卫星一般采用 3-1-2 非对称旋转，本文
将统一采用 3-1-2 旋转式，如图 2。（这里的 3、1、2 分别代表被旋转坐标系的 z、x、y 轴）。
根据坐标转换矩阵的基础理论知识，得出采用 3-1-2 旋转顺序表示的姿态矩阵为：
A312 (ψ ,φ ,θ ) = R y (ϕ )R x (θ )R z (ψ )
sθsψ − sϕcθcψ
− cϕsθ ⎤
sϕ
⎥ （1） ⎥
cϕcθ ⎥⎦
式中，字符“ c ”，“ s ”分别为“ cos ”和“ sin ”的缩写形式。
1.2 四元数式
图 2 3-1-2 欧拉角转动
欧拉角有奇点位置存在，这将引起计算上的一系列问题。而利用四元数可避免奇点问题的出
现，而且转换概念清晰，应用灵活，被广泛地应用于卫星姿态确定的算法设计中。 定义四元数 q 为：
全国第十二届空间及运动体控制技术学术算法
辛岩 1，2 ，陈磊 1 ，王巍 2 （1.国防科学技术大学 航天与材料工程学院，湖南 长沙 410073；2.上海航天控制工程研究所，上海 200233）
摘要：提出了一种四元数转化为大范围欧拉角的新算法，与一般资料中介绍的算法不同的是， 该算法适用的欧拉角的范围可达到ϕ、θ、ψ ∈ (−π，π ) 。并采用数学仿真，分别在绕俯仰轴 机动 180 度、绕偏航轴机动正 90 度和绕偏航轴机动负 90 度的情况下，验证了算法的正确性。 关键词：大角度；四元数；欧拉角；转化算法；仿真
q
=
⎡q0 ⎢⎣ Q
⎤ ⎥⎦
=
[q0
q1
q2
q3 ]，
式中：
q0
=
ϕ cos( )
2
；Q
=
ϕ nsin( ) （
2
n
为单位欧拉轴； ϕ
为转动的欧拉角）。
同样，按 3-1-2 转序时四元数姿态矩阵 B(q) 可表示为：
B(q)
=
⎢⎢⎡q−02
+ q12 −q22 −q32 2q0q3 +2q1q2
⎢⎣ 2q0q2 + 2q1q3
0 引言
由于欧拉角可以由姿态敏感器直接测量，故在卫星姿态控制规律中得到普遍采用。但欧拉角 表示的姿态方程在大角度时会出现奇异问题，因此在进行姿态偏差较大或大角度机动的姿态控制 系统设计时，往往采用四元数表示的运动学方程。因而，把四元数转化为工程上常用的欧拉角是 十分必要的。一般的资料或参考书上所介绍的两者之间的转化公式仅仅适用于
姿态参数。姿态参数有多种形式，各种姿态参数之间可以相互转换，同时相对各种参考坐标的姿 态也可相互转换。工程中常用的姿态描述方法是欧拉角和四元数这两种姿态参数。所以下面仅对 这两种姿态参数进行介绍。
zr
zb
yb xb
yr
O
xr
图 1 卫星姿态的描述
131
全国第十二届空间及运动体控制技术学术年会论文
1.1 欧拉角式
⎡cθ
=
⎢ ⎢
0
⎢⎣sθ
0 − sθ ⎤ ⎡1 0 0 ⎤ ⎡ cψ sψ
1
0
⎥ ⎥
⎢⎢0
cϕ
sϕ
⎥ ⎥
⎢⎢−
sψ
cψ
0 cθ ⎥⎦ ⎢⎣0 − sϕ cϕ ⎥⎦ ⎢⎣ 0 0
⎡cθcψ − sφsθsψ
0⎤ 0⎥⎥
=
⎢ ⎢
− cϕsψ
⎢⎣sθcψ + sϕcθsψ
1⎥⎦
cθsψ + sϕsθcψ cϕcψ
2）当 cosϕ ≠ 0 时 i）对于ψ 角
133
全国第十二届空间及运动体控制技术学术年会论文
考虑到在ψ
π
＝
时， tgψ
→ ∞ ，于是在 − A21
> 2 时，切换为 ctgψ
= − A22 来计算。
2
A22
A21
a． −
A21 A22
>
2 & A22
≥ 0， 即ψ
∈ (arctg 2，π ] 时，ψ＝arcctg(−