大角度范围内四元数转化为欧拉角的算法_辛岩

合集下载

欧拉角四元数转换

欧拉角四元数转换

欧拉角四元数转换欧拉角和四元数都是用于描述物体在三维空间中的旋转状态的数学表示方法。

欧拉角是一个由三个角度组成的向量,而四元数是一个由四个实数组成的向量。

在计算机图形学和机器人学等领域中,这两种方法都被广泛使用。

欧拉角和四元数之间的转换可以帮助我们在不同的数学表示方法中进行转换,从而更方便地进行计算和应用。

下面是欧拉角和四元数之间的转换公式:欧拉角转四元数:给定欧拉角向量 [α, β, γ],我们可以通过以下公式将其转换为对应的四元数 [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 欧拉角和四元数相互转化

欧拉角和四元数是在计算机图形学和游戏开发中常用的两种旋转表示方法,它们可以相互转化,用于描述物体的旋转姿态。

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⾓)的相互转换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对欧拉角的导数。

注意,这是一个相对复杂的过程,涉及到多个三角函数的求导和组合。

最终结果会是一系列与欧拉角相关的三角函数表达式。

大话多旋翼飞行器--欧拉角与四元数

大话多旋翼飞行器--欧拉角与四元数
的旋转。 B) 绕着 xyz 坐标轴旋转:最初,两个坐标系统 xyz 与 XYZ 的坐标轴都是重叠著的。开
始先绕着 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 结论四元数和欧拉角是两种不同的数学表达方式,但它们之间存在一定的关系,通过转换可以实现相互转换。

四元数转换为欧拉角的公式虽然有些复杂,但是在实际应用中可以轻松地进行计算,并且可以提高程序的效率和可读性。

四元数与欧拉角间的转换

四元数与欧拉角间的转换

四元数与欧拉角之间的转换早在1843年哈米尔顿就提出了四元数的基本概念[138],但当时仅停留在理论概念的讨论上。

20世纪以来,随着航天、航空工业的发展,四元数得到了广泛地实际应用[139-142]。

四元数的定义为[143]4q ⎛⎫≡ ⎪⎝⎭Q q(0-1)式中:123(,,)sin 2T q q q α≡=Q e (0-2)4cos2q α= (0-3)其中,e 是沿旋转轴的单位向量,α是旋转角度。

四元数满足约束条件1T =q q(0-4)四元数与姿态矩阵具有如下关系:224334222212341234132422221234123423142222132423141234()22[]()()2()2()2()2()2()2()T T q I q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q ⨯=-+-⨯=Ξψ⎛⎫--++-⎪=--+-++ ⎪⎪+---++⎝⎭A Q QQ Q q q q(0-5)其中,33⨯I 是33⨯单位矩阵,433[]()T q ⨯+⨯⎛⎫Ξ≡ ⎪-⎝⎭I Q q Q(0-6)433[]()T q ⨯-⨯⎛⎫ψ≡ ⎪-⎝⎭I Q q Q(0-7)3231210[]00q q q q q q -⎛⎫ ⎪⨯=- ⎪ ⎪-⎝⎭Q (0-8)地面坐标系Axyz 和弹体坐标系O 1x 1y 1z 1的关系可以用下式来表示=b Ar(0-9)其中,b 、r 分别为某向量在弹体坐标系和地面坐标系中的表示形式,A 的表达形式见错误!未找到引用源。

,具体为cos cos sin cos sin sin cos cos sin sin cos cos sin sin cos cos sin sin cos sin sin cos cos sin sin sin sin cos cos ϑψϑϑψϑψγψγϑγϑψγψγϑψγψγϑγϑψγψγ-⎛⎫⎪=-++ ⎪ ⎪+--+⎝⎭A(0-10)比较式(0-5)和式(0-10),得到姿态角的四元数表示131324222211123412123432231422222212342()arctan arctan arcsin arcsin 2()2()arctan arctan q q q q q q q q q q q q q q q q q q q q ψθφ⎧-=-=-⎪--+⎪⎪==+⎨⎪-⎪=-=-⎪-+-+⎩q q q q q A A A A A (0-11)也可以用姿态角来表示四元数q ,即(cossinj)(cossinj)(cos sin j)222222ψψθθφφ=+⊗+⊗+q(0-12)其中,符号⊗表示四元数乘法,其定义如下[144-145]41234123(i j k)(i j k)A A A A B B B B ⊗=+++⊗+++A B44123114322341232143B A A A A B A A A A A A A A B A A A A B ---⎛⎫⎛⎫ ⎪ ⎪- ⎪⎪= ⎪⎪- ⎪ ⎪ ⎪-⎝⎭⎝⎭44112233144132232431421334211243()()i ()j ()kA B A B A B A B A B A B A B A B A B A B A B A B A B A B A B A B =---++-++++-+-++(0-13)其中,4123i j k A A A A =+++A 和4123i j k B B B B +++B =均为四元数。

欧拉角与四元数

欧拉角与四元数

四元数与旋转一.四元组基础Q(x,y,z,w),其中x,y,z用来确定旋转轴,w为旋转的角度Q=w+xi+yj+zk,i,j,k为三个虚轴的单位分量I*j=kJ*k=i;K*i=j;叉乘:c=a × b=| i j k||a1 b1 c1||a2 b2 c2|=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)c也为一个向量,且c的长度为|a||b|sin(theta),垂直于a和b所在的平面,方向由右手法则来判定,用右手的四指先表示向量a的方向,然后手指朝着手心的方向摆动到向量b的方向,大拇指所指的方向就是向量c 的方向1.四元组相乘:Q1=w1+x1i+y1j+z1k=(w1,v1)Q2=w2+x2i+y2j+z2k=(w2,v2)Q1*Q2=(w1*w2-<v1,v2>,w1*v2+w2*v1+v1xv2)( w1+x1i+y1j+z1k)*( w2+x2i+y2j+z2k)=w1*w2-x1*x2-y1*y2-z1*z2+(W1*x2+x1*w2+y1*z2-z1-y2)i+(y1*w2+w1*y2+z1*x2-x1*z2)j+(w1*z2+z1*w2+x1*y2-y1*x2)k对于其中的轴部分,假如v1//v2,则有v1 x v2=0(平行向量的叉乘结果为0)2.四元组的点乘,点乘积为数值:Q1.*Q2=w1*w2+<v1,v2>=w1*w2+x1*x2+y1*y2+z1*z2;3.数乘s为一实数,q为四元组,则有sq=qs4.共轭p=(w,v),则p*=(w,-v)(pq)*=q*p*N(q)=w2+x2+y2+z2q-1=q*/N(q)---------------显然可得qq-1=(1,0)二.使用四元数旋转向量假如有一表示向量的四元组q=(w,v),对其应用旋转量p后的结果为:q’=pqp-1=(w,v’)从上可以看出,计算的结果q’的实部和q的实部是相等的,并且有N(v)=N(v’)如果N(q)=1,则可以令q=(cosa,usina),u也为一个单位向量,则q’是q绕u旋转2a个弧度的结果假如S(q)表示q的实部,则有2S(q)=q+q*2S(pqp-1)= pqp-1+( pqp-1)*=pqp*+(pqp*)*=pqp*+pq*p*=p(q+q*)p*=2S(q)(这里由于p是单位四元数,所以有p-1等于p*)欧拉角到四元数的转换定义pitch, yaw, roll分别为绕X轴、Y轴、Z轴的旋转弧度float p = pitch * PIOVER180 / 2.0;float y = yaw * PIOVER180 / 2.0;float r = roll * PIOVER180 / 2.0;float sinp = sin(p);float siny = sin(y);float sinr = sin(r);float cosp = cos(p);float cosy = cos(y);float cosr = cos(r);this->x = sinr * cosp * cosy - cosr * sinp * siny;this->y = cosr * sinp * cosy + sinr * cosp * siny;this->z = cosr * cosp * siny - sinr * sinp * cosy;this->w = cosr * cosp * cosy + sinr * sinp * siny;normalise();三.使用matlab进行相关计算计算两个向量v1和v2之间的旋转量四元数p,使得v1应用p后到达v2假如v1转到v2的旋转轴为v,旋转角为theta,则q=[v*cos(theta/2)sin(theta/2)]Matlab代码:function q=vector2q(v1,v2)%..normalize....len1=sqrt(v1*v1');len2=sqrt(v2*v2');v1=v1/len1;v2=v2/len2;angle=v1*v2';axis=cross(v1,v2);alen=sqrt(axis*axis');axis=axis/alen;t=acos(angle);t=t/2;q(1)=axis(1)*sin(t);q(2)=axis(2)*sin(t);q(3)=axis(3)*sin(t);q(4)=cos(t);end计算出了q之后,可以获得对应的旋转矩阵,旋转矩阵的计算Matlab里面的矩阵是以列为主顺序的function r=q2rot(q)w=q(4);x=q(1);y=q(2);z=q(3);r=zeros(3,3);r(1,1)=1-2*y*y-2*z*z;r(1,2)=2*x*y+2*w*z;r(1,3)=2*x*z-2*w*y;r(2,1)=2*x*y-2*w*z;r(2,2)=1-2*x*x-2*z*z;r(2,3)=2*z*y+2*w*x;r(3,1)=2*x*z+2*w*y;r(3,2)=2*y*z-2*w*x;r(3,3)=1-2*x*x-2*y*y;r=r';end同时,也可以根据四元数来计算欧拉角function R=q2euler(q)w=q(4);x=q(1);y=q(2);z=q(3);t11=2*(w*x+y*z);t12=1-2*(x*x+y*y);R(1)=atan2(t11,t12);t2=2*(w*y-z*x);R(2)=asin(t2);t31=2*(w*z+x*y);t32=1-2*(y*y+z*z);R(3)=atan2(t31,t32);end计算出来的欧拉角rx,ry,rz,分别为绕X轴、Y轴和Z轴的旋转角,假如有:Rotq=q2rot(q)R=q2euler(q)[rotx roty rotz]=Rotation(R)可以发现Rotq==rotz*roty*rotx从这里可以看出,上面使用四元数这样计算出来的旋转矩阵的旋转顺序分别是X轴、Y轴和Z轴的ra=pi/4;qz=[0 0 -sin(ra) cos(ra)] %绕z旋转-90度qy=[0 sin(ra) 0 cos(ra) ] %绕y旋转90度qyz=qmult(qy,qz)r=q2euler(qyz)上面的r得出的结果为r = -1.5708 0.0000 -1.5708也就是说其几何意义变成先绕X轴旋转-90度,再绕Z轴旋转-90度,而根据qy和qz的相乘我们实际进行的操作却是先绕Z轴旋转-90度,再绕Y轴旋转90度,但是结果却是这两种操作等价,这说明由四元数到欧拉角可以有多个解两个四元数,假如它们的方向是相反的,用它们作用于向量得到的新向量的值仍然相等q1=[0.024666 -0.023954 0.504727 0.862594];arm=[-8.881719 6.037597 -2.36776];q2=-q1;rot1=q2rot(q1);rot2=q2rot(q2);v1=rot1*arm'v2=rot2*arm'上面计算出来的v1等于v2四元数的余弦值为它们的内积假如余弦值小于0,则需要将其中的一个取反,因为上面我们知道一个四元数和它的反方向的四元数对一个向量起相同的作用四元数的相乘,代表旋转的累积pq=p*q;rotp=q2rot(p);rotq=q2rot(q);rotpq=q2rot(pq);rotmul=rotp*rotq;这里rotpq与rotmul相等四. OGRE中Quaternion类的几个函数1.四元数到旋转向量void Quaternion::ToRotationMatrix (Matrix3& kRot) const1 - 2*qy2 -2*qz22*qx*qy -2*qz*qw2*qx*qz +2*qy*qw2*qx*qy + 2*qz*qw 1 - 2*qx2 -2*qz22*qy*qz -2*qx*qw2*qx*qz -2*qy*qw 2*qy*qz +2*qx*qw1 - 2*qx2 -2*qy22.旋转量到四元数根据1中的表格,有:4 *(1-qx2-qy2-qz2) = 1 + m00 + m11 + m22又qw2=1-qx2-qy2-qz2,可得4 *qw2= 1 + m00 + m11 + m22这里解qw必须保证1 + m00 + m11 + m22>=0,如果不是的话,就构造其他的等式来计算,OGRE中分成两种情况,一种是m00 + m11 + m22>=0,就可以直接先解出qw,否则的采用另外的等式计算3.Local axisVector3 xAixs(void) const;取得旋转矩阵的第一列,旋转矩阵和一个向量相乘的话,第一列的数据均和向量的x分量相乘Vecotr3 yAxis(void) const;取得旋转矩阵的第二列,旋转矩阵和一个向量相乘的话,第二列的数据均和向量的y分量相乘Vecotr3 zAxis(void) const;取得旋转矩阵的第三列,旋转矩阵和一个向量相乘的话,第三列的数据均和向量的z分量相乘。

四元数转化成欧拉角

四元数转化成欧拉角

四元数转化成欧拉角笔者:奋斗 修改时间:2015.11.23一. 姿态解算(以匿名版程序为例)首先,程序中一般用了两种求解姿态的方法,一种为欧拉角法,一种为四元数法。

由四元数法将MPU6050的值融合成四元,再由欧拉角法把四元转换成欧拉角送入串级PID 控制器。

(1)欧拉角法静止状态,或者总加速度只是稍微大于g 时,由加计算出的值比较准确。

使用欧拉角表示姿态,令Φ,θ和Φ代表ZYX 欧拉角,分别称为偏航角、俯仰角和横滚角。

载体坐标系下的 加 速 度(a x B,a y B ,a z B )和参考坐标系下的加速度(a x N, a y N, a z N)之间的关系可表示为(1)。

其中 c 和 s 分别代表 cos 和 sin 。

axB,ayB,azB 就是mpu 读出来的三个值。

这个矩阵就是三个旋转矩阵相乘得到的,因为矩阵的乘法可以表示旋转。

axB c c c s s axN ayB c s s s c c c s s s s c ayN azB s s c s c s c c s s c c azN θψθψθφψφθψφψφθψφθφψφθψφψφθψφθ-⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥=-++⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥+-+⎣⎦⎣⎦⎣⎦ (1) 飞行器处于静止状态,此时参考系下的加速度等于重力加速度,即00xN yN zN a a g a ⎡⎤⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦(2)把(2)代入(1)可以解得arctg θ=(3)yB zB a arctg a φ⎛⎫= ⎪⎝⎭(4) 即为初始俯仰角和横滚角,通过加速度计得到载体坐标系下的加速度即可将其解出,偏航角可以通过电子罗盘求出。

(2)四元数法(通过处理单位采样时间内的角增量(mpu的陀螺仪得到的就是角增量),为了避免噪声的微分放大,应该直接用角增量)void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){float norm;// float hx, hy, hz, bx, bz;float vx, vy, vz; // wx, wy, wz;float ex, ey, ez;// 先把这些用得到的值算好float q0q0 = q0*q0;float q0q1 = q0*q1;float q0q2 = q0*q2;// float q0q3 = q0*q3;float q1q1 = q1*q1;// float q1q2 = q1*q2;float q1q3 = q1*q3;float q2q2 = q2*q2;float q2q3 = q2*q3;float q3q3 = q3*q3;if(ax*ay*az==0)return;norm = sqrt(ax*ax + ay*ay + az*az); //acc数据归一化ax = ax /norm;ay = ay / norm;az = az / norm;// estimated direction of gravity and flux (v and w)vx = 2*(q1q3 - q0q2); //四元素中xyzvy = 2*(q0q1 + q2q3);vz = q0q0 - q1q1 - q2q2 + q3q3 ;// error is sum of cross product between reference direction of fields and direction measured by sensorsex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差ey = (az*vx - ax*vz) ;ez = (ax*vy - ay*vx) ;exInt = exInt + ex * Ki; //对误差进行积分eyInt = eyInt + ey * Ki;ezInt = ezInt + ez * Ki;// adjusted gyroscope measurementsgx = gx + Kp*ex + exInt; //将误差PI 后补偿到陀螺仪,即补偿零点漂移 gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt; //这里的gz 由于没有观测者进行矫正会产生漂移,表现出来的就是积分自增或自减// integrate quaternion rate and normalise //四元素的微分方程 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;// normalise quaternionnorm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 / norm;q1 = q1 / norm;q2 = q2 / norm;q3 = q3 / norm;//Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yawQ_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitchQ_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll}下面对上面的程序逐条解释。

旋转矩阵 四元数 欧拉角

旋转矩阵 四元数 欧拉角

旋转矩阵四元数欧拉角旋转矩阵是在三维空间中表示旋转的一种方法,它通过一个三阶正交矩阵来描述。

这个矩阵可以用来表示一个点或向量绕某个轴旋转一定的角度。

在3D图形学、计算机视觉和机器人领域中,旋转矩阵被广泛应用。

旋转矩阵的大小为3×3,可以表示绕x轴、y轴、z轴旋转的三个角度。

例如,绕z轴旋转θ角度的旋转矩阵为:cosθ -sinθ 0sinθ cosθ 00 0 1四元数是另一种描述旋转的方法,它是一个具有四个实数的向量,通常被写作q = a + bi + cj + dk,其中a、b、c、d是实数。

四元数具有叠加和乘法运算,并且可以表示旋转和转动等复杂的运动。

四元数与旋转矩阵之间有一个简单的关系。

任何一个旋转矩阵都可以用一个四元数来表示,并且一个四元数也可以转换成一个旋转矩阵。

这个转换过程中,四元数和旋转矩阵的元素之间有一个固定的关系。

欧拉角是一种描述物体在三维空间中的方向的方法。

欧拉角通常由三个角度(yaw、pitch、roll)组成,用于描述物体相对于旋转参考系的旋转。

这些角度通常是绕物体的三个轴,通常是z、y、x,在类飞行器航空学中也称为偏转、俯仰和滚转。

欧拉角是最常见的描述物体方向的方法之一,但也存在一些问题。

一个主要的问题是欧拉角的旋转顺序,相同的三个角度可能会导致不同的旋转结果。

这个问题称为欧拉角万向锁问题。

总之,旋转矩阵、四元数和欧拉角都是用于描述物体在三维空间中的方向和位置的方法。

它们各有优缺点,并且在不同的领域和应用中具有不同的优势。

了解这些方法的特点和用途,有助于我们在设计和实现3D图形、机器人和计算机视觉应用时做出更合适的决策。

欧拉角与四元数

欧拉角与四元数

四元数与旋转一.四元组基础Q(x,y,z,w),其中x,y,z用来确定旋转轴,w为旋转的角度Q=w+xi+yj+zk,i,j,k为三个虚轴的单位分量I*j=kJ*k=i;K*i=j;叉乘:c=a × b=| i j k||a1 b1 c1||a2 b2 c2|=(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)c也为一个向量,且c的长度为|a||b|sin(theta),垂直于a和b所在的平面,方向由右手法则来判定,用右手的四指先表示向量a的方向,然后手指朝着手心的方向摆动到向量b的方向,大拇指所指的方向就是向量c的方向1.四元组相乘:Q1=w1+x1i+y1j+z1k=(w1,v1)Q2=w2+x2i+y2j+z2k=(w2,v2)Q1*Q2=(w1*w2-<v1,v2>,w1*v2+w2*v1+v1xv2)( w1+x1i+y1j+z1k)*( w2+x2i+y2j+z2k)=w1*w2-x1*x2-y1*y2-z1*z2+(W1*x2+x1*w2+y1*z2-z1-y2)i+(y1*w2+w1*y2+z1*x2-x1*z2)j+(w1*z2+z1*w2+x1*y2-y1*x2)k对于其中的轴部分,假如v1//v2,则有v1 x v2=0(平行向量的叉乘结果为0)2.四元组的点乘,点乘积为数值:Q1.*Q2=w1*w2+<v1,v2>=w1*w2+x1*x2+y1*y2+z1*z2;3.数乘s为一实数,q为四元组,则有sq=qs4.共轭p=(w,v),则p*=(w,-v)(pq)*=q*p*N(q)=w2+x2+y2+z2q-1=q*/N(q)--------------- 显然可得qq-1=(1,0)二.使用四元数旋转向量假如有一表示向量的四元组q=(w,v),对其应用旋转量p后的结果为:q’=pqp-1=(w,v’)从上可以看出,计算的结果q’的实部和q的实部是相等的,并且有N(v)=N(v’)如果N(q)=1,则可以令q=(cosa,usina),u也为一个单位向量,则q’是q绕u旋转2a个弧度的结果假如S(q)表示q的实部,则有2S(q)=q+q*2S(pqp-1)= pqp-1+( pqp-1)*=pqp*+(pqp*)*=pqp*+pq*p*=p(q+q*)p*=2S(q)(这里由于p是单位四元数,所以有p-1等于p*)欧拉角到四元数的转换定义pitch, yaw, roll分别为绕X轴、Y轴、Z轴的旋转弧度float p = pitch * PIOVER180 / 2.0;float y = yaw * PIOVER180 / 2.0;float r = roll * PIOVER180 / 2.0;float sinp = sin(p);float siny = sin(y);float sinr = sin(r);float cosp = cos(p);float cosy = cos(y);float cosr = cos(r);this->x = sinr * cosp * cosy - cosr * sinp * siny;this->y = cosr * sinp * cosy + sinr * cosp * siny;this->z = cosr * cosp * siny - sinr * sinp * cosy;this->w = cosr * cosp * cosy + sinr * sinp * siny;normalise();三.使用matlab进行相关计算计算两个向量v1和v2之间的旋转量四元数p,使得v1应用p后到达v2假如v1转到v2的旋转轴为v,旋转角为theta,则q=[v*cos(theta/2) sin(theta/2)] Matlab代码:function q=vector2q(v1,v2)%..normalize....len1=sqrt(v1*v1');len2=sqrt(v2*v2');v1=v1/len1;v2=v2/len2;angle=v1*v2';axis=cross(v1,v2);alen=sqrt(axis*axis');axis=axis/alen;t=acos(angle);t=t/2;q(1)=axis(1)*sin(t);q(2)=axis(2)*sin(t);q(3)=axis(3)*sin(t);q(4)=cos(t);end计算出了q之后,可以获得对应的旋转矩阵,旋转矩阵的计算Matlab里面的矩阵是以列为主顺序的function r=q2rot(q)w=q(4);x=q(1);y=q(2);z=q(3);r=zeros(3,3);r(1,1)=1-2*y*y-2*z*z;r(1,2)=2*x*y+2*w*z;r(1,3)=2*x*z-2*w*y;r(2,1)=2*x*y-2*w*z;r(2,2)=1-2*x*x-2*z*z;r(2,3)=2*z*y+2*w*x;r(3,1)=2*x*z+2*w*y;r(3,2)=2*y*z-2*w*x;r(3,3)=1-2*x*x-2*y*y;r=r';end同时,也可以根据四元数来计算欧拉角function R=q2euler(q)w=q(4);x=q(1);y=q(2);z=q(3);t11=2*(w*x+y*z);t12=1-2*(x*x+y*y);R(1)=atan2(t11,t12);t2=2*(w*y-z*x);R(2)=asin(t2);t31=2*(w*z+x*y);t32=1-2*(y*y+z*z);R(3)=atan2(t31,t32);end计算出来的欧拉角rx,ry,rz,分别为绕X轴、Y轴和Z轴的旋转角,假如有:Rotq=q2rot(q)R=q2euler(q)[rotx roty rotz]=Rotation(R)可以发现Rotq==rotz*roty*rotx从这里可以看出,上面使用四元数这样计算出来的旋转矩阵的旋转顺序分别是X轴、Y轴和Z轴的ra=pi/4;qz=[0 0 -sin(ra) cos(ra)] %绕z旋转-90度qy=[0 sin(ra) 0 cos(ra) ] %绕y旋转90度qyz=qmult(qy,qz)r=q2euler(qyz)上面的r得出的结果为r = -1.5708 0.0000 -1.5708也就是说其几何意义变成先绕X轴旋转-90度,再绕Z轴旋转-90度,而根据qy和qz的相乘我们实际进行的操作却是先绕Z轴旋转-90度,再绕Y轴旋转90度,但是结果却是这两种操作等价,这说明由四元数到欧拉角可以有多个解两个四元数,假如它们的方向是相反的,用它们作用于向量得到的新向量的值仍然相等q1=[0.024666 -0.023954 0.504727 0.862594];arm=[-8.881719 6.037597 -2.36776];q2=-q1;rot1=q2rot(q1);rot2=q2rot(q2);v1=rot1*arm'v2=rot2*arm'上面计算出来的v1等于v2四元数的余弦值为它们的内积假如余弦值小于0,则需要将其中的一个取反,因为上面我们知道一个四元数和它的反方向的四元数对一个向量起相同的作用四元数的相乘,代表旋转的累积pq=p*q;rotp=q2rot(p);rotq=q2rot(q);rotpq=q2rot(pq);rotmul=rotp*rotq;这里rotpq与rotmul相等四.OGRE中Quaternion类的几个函数1.四元数到旋转向量void Quaternion::ToRotationMatrix (Matrix3& kRot) const1 - 2*qy2 - 2*qz22*qx*qy - 2*qz*qw2*qx*qz + 2*qy*qw2*qx*qy + 2*qz*qw 1 - 2*qx2 - 2*qz22*qy*qz - 2*qx*qw2*qx*qz - 2*qy*qw2*qy*qz + 2*qx*qw 1 - 2*qx2 - 2*qy22.旋转量到四元数根据1中的表格,有:4 *(1-qx2-qy2-qz2) = 1 + m00 + m11 + m22又qw2=1-qx2-qy2-qz2,可得4 *qw2= 1 + m00 + m11 + m22这里解qw必须保证1 + m00 + m11 + m22>=0,如果不是的话,就构造其他的等式来计算,OGRE中分成两种情况,一种是m00 + m11 + m22>=0,就可以直接先解出qw,否则的采用另外的等式计算3.Local axisVector3 xAixs(void) const;取得旋转矩阵的第一列,旋转矩阵和一个向量相乘的话,第一列的数据均和向量的x分量相乘Vecotr3 yAxis(void) const;取得旋转矩阵的第二列,旋转矩阵和一个向量相乘的话,第二列的数据均和向量的y分量相乘Vecotr3 zAxis(void) const;取得旋转矩阵的第三列,旋转矩阵和一个向量相乘的话,第三列的数据均和向量的z分量相乘。

四元素与欧拉角

四元素与欧拉角

1.欧拉角在四元数出现之前先看下欧拉角:对于在三维空间里的一个参考系,任何坐标系的取向,都可以用三个欧拉角来表现。

为了后面的角度不混乱,我们要先区分参考系和坐标系的概念。

参考系即为大地参考系,是静止不动的。

而坐标系则固定于四轴飞行器,随着四轴飞行器的旋转而旋转。

按照右图所示。

设定 xyz-轴为四轴上的参考轴,XYZ-轴则是大地的参考轴。

右图即为四轴相对地面进行了一定旋转, xy-平面与XY-平面的相交线为交点线,用英文字母(N)代表。

我们可以这样定义欧拉角:α是 x-轴与交点线的夹角β是 z-轴与Z-轴的夹角γ是交点线与X-轴的夹角这样我们就可以用三个欧拉角:(α,β,γ)其取值为0-360来描述四轴飞行器相对于大地的参考系的姿态角度了。

三个欧拉角:(α,β,γ)。

蓝色的轴是 xyz-轴,红色的轴是 XYZ-坐标轴。

绿色的线是交点线 (N) 。

2.轴角欧拉角使用roll,pitch,yaw来表示这些分量的旋转值。

需要注意的是,这里的旋转是针对大地参考系说的,这意味着第一次的旋转不会影响第二、三次的转轴,简单的说,三角度系统无法表现任意轴的旋转,只要一开始旋转,物体本身就失去了任意轴的自主性,这也就导致了万向节锁(Gimbal Lock)的问题。

什么是Gimbal Lock?正如前面所说,因为欧拉描述中针对x,y,z的旋转描述是世界坐标系下的值,所以当任意一轴旋转90°的时候会导致该轴同其他轴重合,此时旋转被重合的轴可能没有任何效果,这就是Gimbal Lock,还有一种是轴角的描述方法,这种方法比欧拉描述要好,它避免了Gimbal Lock,它使用一个3维向量表示转轴和一个角度分量表示绕此转轴的旋转角度,即(x,y,z,angle),一般表示为(x,y,z,w)或者(v,w)。

(x,y,z)为旋转轴,w为旋转角度。

但这种描述法却不适合插值。

轴角的表示方法:那么轴、角的描述方法又有什么问题呢?虽然轴、角的描述解决了Gimbal Lock,但这样的描述方法会导致差值不平滑,差值结果可能跳跃,欧拉角描述同样有这样的问题。

欧拉角与四元数之间的转换数学公式

欧拉角与四元数之间的转换数学公式

欧拉角与四元数之间的转换数学公式欧拉角和四元数是两种广泛应用于机器人、航空、姿态控制等领域的旋转表示方法。

它们之间的转换是非常有用的,在实际应用中经常需要将一个旋转表示方法转换成另一个。

本文将介绍欧拉角和四元数之间的转换数学公式,以及它们的应用。

欧拉角是一种旋转表示方法,它将旋转分解成三个轴向旋转的组合。

欧拉角共有12种表示方式,其中最常用的是ZYZ方式。

它将一个旋转分解为绕Z轴旋转一个角度,绕新的Y轴旋转一个角度,再绕新的Z轴旋转一个角度。

欧拉角的转换公式如下:$$begin{bmatrix} alpha beta gamma end{bmatrix} =begin{bmatrix} atan2(r_{23},r_{33}) asin(-r_{13})atan2(r_{12},r_{11}) end{bmatrix}$$其中,r是旋转矩阵,α、β、γ是绕Z、Y、Z轴旋转的角度。

四元数是一种用四元组表示旋转的方法。

它比欧拉角更为简洁、稳定,并且没有万向锁问题。

四元数的转换公式如下:$$begin{bmatrix} q_w q_x q_y q_z end{bmatrix} =begin{bmatrix} cos(theta/2) sin(theta/2) * n_x sin(theta/2) * n_y sin(theta/2) * n_z end{bmatrix}$$其中,q是四元数,θ是旋转角度,n是旋转轴向量。

四元数的逆转换公式如下:$$begin{bmatrix} theta n_x n_y n_z end{bmatrix} = begin{bmatrix} 2 * acos(q_w) q_x / sqrt{1-q_w^2} q_y /sqrt{1-q_w^2} q_z / sqrt{1-q_w^2} end{bmatrix}$$ 欧拉角和四元数之间的转换可以通过以下公式实现:$$begin{bmatrix} q_w q_x q_y q_z end{bmatrix} = begin{bmatrix} cos(alpha/2) * cos(beta/2) * cos(gamma/2) + sin(alpha/2) * sin(beta/2) * sin(gamma/2) sin(alpha/2) * cos(beta/2) * cos(gamma/2) - cos(alpha/2) * sin(beta/2) * sin(gamma/2) cos(alpha/2) * sin(beta/2) * cos(gamma/2) + sin(alpha/2) * cos(beta/2) * sin(gamma/2) cos(alpha/2) * cos(beta/2) * sin(gamma/2) - sin(alpha/2) * sin(beta/2) * cos(gamma/2) end{bmatrix}$$$$begin{bmatrix} alpha beta gamma end{bmatrix} = begin{bmatrix} atan2(2(q_w*q_x+q_y*q_z),1-2(q_x^2+q_y^2)) asin(2(q_w*q_y-q_z*q_x))atan2(2(q_w*q_z+q_x*q_y),1-2(q_y^2+q_z^2)) end{bmatrix}$$ 通过欧拉角和四元数之间的转换,可以方便地在不同领域应用中进行旋转表示的转换。

四元数转欧拉角奇异值问题

四元数转欧拉角奇异值问题

四元数转欧拉角奇异值问题四元数和欧拉角作为常见的表示方法,在计算机图形学和机器人领域中广泛应用。

在四元数表示角度时,存在着一些奇异值问题,会使得欧拉角的转换结果出现异常,本文将对此进行详细的阐述。

一、四元数简介四元数是一种扩充了实数的数学结构,它包括一个实数部分和三个虚数部分,可以表示旋转和剪切等变换。

四元数形式上可以写成 $q = w + xi + yj + zk$,其中 $w, x, y, z$ 都是实数,$i, j, k$ 是三个虚数单位,它们的平方分别等于-1。

四元数通过旋转的方式来表示角度。

例如,单位四元数可以表示3D中的转动180度绕轴 $(1,1,1)$,则可以写成:$$q = [\cos(\theta/2),\sin(\theta/2)\frac{1}{\sqrt{3}},\sin(\theta/2)\frac{1}{\sqrt{3}},\sin(\theta/2)\frac{1}{\sqrt{3}}]$$其中,$\theta$ 是旋转的角度。

二、欧拉角简介欧拉角是一种表示旋转的方式,它通过描述旋转的三个角度来确定旋转的方向。

欧拉角可以通过任何航向、俯仰、翻转(yaw-pitch-roll)的组合来表示。

例如,将 3D 物体绕 X 轴旋转30度,然后绕 Y 轴旋转45度,再绕 Z 轴旋转 60 度,这个过程可以用以下的欧拉角表示:$$\alpha = 30^\circ,\beta = 45^\circ,\gamma = 60^\circ$$当然,在欧拉角的表示中也存在固有的问题,例如欧拉角的万向锁问题等。

但是,在本文中,我们主要探讨的问题是四元数转为欧拉角时可能会遇到的问题。

三、四元数转换为欧拉角在将四元数转换为欧拉角时,我们可以首先计算对应的旋转矩阵,然后通过该矩阵计算欧拉角。

假设四元数为$q = [w, x, y, z]$,则相应的旋转矩阵可以表示为:$$\left[\begin{matrix} 1 - 2(y^2+z^2)& 2(xy-wz)& 2(xz+wy)\\ 2(xy+wz)& 1-2(x^2+z^2)& 2(yz-wx)\\ 2(xz-wy)& 2(yz+wx)& 1 - 2(x^2+y^2)\end{matrix}\right]$$根据四元数到旋转矩阵的转换方法,我们可以通过这个矩阵计算出欧拉角,例如:$$\alpha = \tan^{-1}(\frac{-m_{23}}{m_{33}})$$$$\beta = \sin^{-1}(m_{13})$$$$\gamma = \tan^{-1}(\frac{-m_{12}}{m_{11}})$$其中 $m_{ij}$ 表示旋转矩阵中的第 $i$ 行第$j$ 列的元素。

四元数转欧拉角奇异值

四元数转欧拉角奇异值

四元数转欧拉角奇异值
四元数是一种表示旋转的方法,而欧拉角则是另一种。

在将四元数转换为欧拉角时,有时会遇到奇异值的问题,这时需要进行特殊处理。

奇异值出现的原因是欧拉角的定义域有限,而四元数的定义域无限。

当四元数的某些元素超出欧拉角的定义域时,就会出现奇异值。

为了解决奇异值问题,可以使用雅可比矩阵方法进行转换。

这种方法可以通过计算雅可比矩阵的特征值来确定是否存在奇异值,并进行相应的处理。

另外,还可以使用另一种方法来避免奇异值,即使用欧拉角的另一种定义方式。

这种方式可以将欧拉角的定义域扩展到无限,从而避免奇异值的出现。

总之,在将四元数转换为欧拉角时,需要注意奇异值的问题,并选择合适的方法进行处理。

- 1 -。

四元数与欧拉角之间的转换

四元数与欧拉角之间的转换

四元数与欧拉角之间的转换在3D图形学中,最常用的旋转表示方法便是四元数和欧拉角,比起矩阵来具有节省存储空间和方便插值的优点。

本文主要归纳了两种表达方式的转换,计算公式采用3D笛卡尔坐标系:图1 3D Cartesian coordinate System (from wikipedia)定义分别为绕Z轴、Y轴、X轴的旋转角度,如果用Tait-Bryan angle表示,分别为Yaw、Pitch、Roll。

图2 Tait-Bryan angles (from wikipedia)一、四元数的定义通过旋转轴和绕该轴旋转的角度可以构造一个四元数:其中是绕旋转轴旋转的角度,为旋转轴在x,y,z方向的分量(由此确定了旋转轴)。

二、欧拉角到四元数的转换三、四元数到欧拉角的转换arctan和arcsin的结果是,这并不能覆盖所有朝向(对于角的取值范围已经满足),因此需要用atan2来代替arctan。

四、在其他坐标系下使用在其他坐标系下,需根据坐标轴的定义,调整一下以上公式。

如在Direct3D中,笛卡尔坐标系的X轴变为Z轴,Y轴变为X轴,Z轴变为Y轴(无需考虑方向)。

五、示例代码/Files/heath/Euler2Quaternion.rarDemo渲染两个模型,左边使用欧拉角,右边使用四元数,方向键Up、Left、Right旋转模型。

参考文献:[1]/wiki/Conversion_between_quaternions_and _Euler_angles[2] Ken Shoemake, Animating Rotation with Quaternion Curves, 1985。

  1. 1、下载文档前请自行甄别文档内容的完整性,平台不提供额外的编辑、内容补充、找答案等附加服务。
  2. 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
  3. 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θ ⎤

⎥ (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


⎥ ⎥
⎢⎢−


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(−
相关文档
最新文档