四元数与欧拉角(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αcα is shorthand for cosαcos⁡α, sαsα is shorthand for sinαsin⁡α,and so on.)

 

RXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=⎡⎣⎢cαcβsαcβsβcαsβsγsαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγcαsγcβcγ⎤⎦⎥RXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=[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欧拉角,由于是绕自身坐标轴进行旋转,则旋转矩阵为:

 

RZYX(α,β,γ)=RZ(α)RY(β)RX(γ)=⎡⎣⎢cαcβsαcβsβcαsβsγsαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγcαsγcβcγ⎤⎦⎥RZ′Y′X′(α,β,γ)=RZ(α)RY(β)RX(γ)=[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与四元数

  绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设等效旋转轴方向向量为K=[kx,ky,kz]TK→=[kx,ky,kz]T,等效旋转角为θθ,则四元数q=(x,y,z,w)q=(x,y,z,w),其中:

 

xyzw=kxsinθ2=kysinθ2=kzsinθ2=cosθ2x=kx⋅sinθ2y=ky⋅sinθ2z=kz⋅sinθ2w=cosθ2

 

  且有x2+y2+z2+w2=1x2+y2+z2+w2=1

  即四元数存储了旋转轴和旋转角的信息,它能方便的描述刚体绕任意轴的旋转。

  四元数转换为旋转矩阵:

 

R=⎡⎣⎢12y22z22(xy+zw)2(xzyw)2(xyzw)12x22z22(yz+xw)2(xz+yw)2(yzxw)12x22y2⎤⎦⎥R=[1−2y2−2z22(xy−zw)2(xz+yw)2(xy+zw)1−2x2−2z22(yz−xw)2(xz−yw)2(yz+xw)1−2x2−2y2]

 

   已知旋转矩阵为:

  则对应的四元数为:

 


  • 四元数与欧拉角的相互转换

  定义两个四元数:

  
  
  其中 表示矢量 
 
 ;而 
 
 表示矢量 
 

四元数加法:

  跟复数、向量和矩阵一样,两个四元数之和需要将不同的元素加起来。
  加法遵循实数和复数的所有交换律和结合律。

四元数乘法:

  四元数的乘法的意义类似于矩阵的乘法,可以表示旋转的合成。当有多次旋转操作时,使用四元数可以获得更高的计算效率。

  由于四元数乘法的非可换性,pq并不等于qp,qp乘积的向量部分是:
  
  Mathematica中有四元数相关的程序包Quaternions Package,需要先导入才能使用。下面计算了三个四元数的乘积:
 

  计算结果为:Quaternion[-12, 4, 14, 2]

 
  那么将Z-Y-X欧拉角(或RPY角:绕固定坐标系的X-Y-Z依次旋转αα,ββ,γγ角)转换为四元数:

 

q=⎡⎣⎢⎢⎢⎢⎢cosγ200sinγ2⎤⎦⎥⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢⎢cosβ20sinβ20⎤⎦⎥⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢cosα2sinα200⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢cosα2cosβ2cosγ2+sinα2sinβ2sinγ2sinα2cosβ2cosγ2cosα2sinβ2sinγ2cosα2sinβ2cosγ2+sinα2cosβ2sinγ2cosα2cosβ2sinγ2sinα2sinβ2cosγ2⎤⎦⎥⎥⎥⎥⎥⎥⎥q=[cos⁡γ200sin⁡γ2][cos⁡β20sin⁡β20][cos⁡α2sin⁡α200]=[cos⁡α2cos⁡β2cos⁡γ2+sin⁡α2sin⁡β2sin⁡γ2sin⁡α2cos⁡β2cos⁡γ2−cos⁡α2sin⁡β2sin⁡γ2cos⁡α2sin⁡β2cos⁡γ2+sin⁡α2cos⁡β2sin⁡γ2cos⁡α2cos⁡β2sin⁡γ2−sin⁡α2sin⁡β2cos⁡γ2]

 

   根据上面的公式可以求出逆解,即由四元数q=(q0,q1,q2,q3)q=(q0,q1,q2,q3)或q=(w,x,y,z)q=(w,x,y,z)到欧拉角的转换为:

 

⎡⎣⎢αβγ⎤⎦⎥=⎡⎣⎢⎢⎢⎢arctan2(q0q1+q2q3)12(q21+q22)arcsin(2(q0q2q1q3))arctan2(q0q3+q1q2)12(q22+q23)⎤⎦⎥⎥⎥⎥[αβγ]=[arctan⁡2(q0q1+q2q3)1−2(q12+q22)arcsin⁡(2(q0q2−q1q3))arctan⁡2(q0q3+q1q2)1−2(q22+q32)]

 

  由于arctan和arcsin的取值范围在π2−π2和π2π2之间,只有180°,而绕某个轴旋转时范围是360°,因此要使用atan2函数代替arctan函数:

 

⎡⎣⎢αβγ⎤⎦⎥=⎡⎣⎢atan2(2(q0q1+q2q3),12(q21+q22))arcsin(2(q0q2q1q3))atan2(2(q0q3+q1q2),12(q22+q23))⎤⎦⎥[αβγ]=[atan2(2(q0q1+q2q3),1−2(q12+q22))arcsin⁡(2(q0q2−q1q3))atan2(2(q0q3+q1q2),1−2(q22+q32))]

 

对于tan(θ) = y / x :

  θ = ATan(y / x)求出的θ取值范围是[-PI/2, PI/2];

  θ = ATan2(y, x)求出的θ取值范围是[-PI,   PI]。

  • 当 (x, y) 在第一象限, 0 < θ < PI/2

  • 当 (x, y) 在第二象限 PI/2 < θPI

  • 当 (x, y) 在第三象限, -PI < θ < -PI/2

  • 当 (x, y) 在第四象限, -PI/2 < θ < 0

   将四元数转换为欧拉角可以参考下面的代码。需要注意欧拉角有12种旋转次序,而上面推导的公式是按照Z-Y-X顺序进行的,所以有时会在网上看到不同的转换公式(因为对应着不同的旋转次序),在使用时一定要注意旋转次序是什么。比如ADAMS软件里就默认Body 3-1-3次序,即Z-X-Z欧拉角,而VREP中则按照X-Y-Z欧拉角旋转。
 
复制代码

复制代码

   上面的代码存在一个问题,即奇异性没有考虑。下面看一种特殊的情况(参考Maths - Conversion Quaternion to Euler):假设一架飞机绕Y轴旋转了90°(俯仰角pitch=90),机头垂直向上,此时如何计算航向角和横滚角?

  这时会发生自由度丢失的情况,即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题:

  arcsin(2(q0q2q1q3))arcsin⁡(2(q0q2−q1q3))的定义域为[1,1][−1,1],因此(q0q2q1q3)[0.5,0.5](q0q2−q1q3)∈[−0.5,0.5],当q0q2q1q3=0.5q0q2−q1q3=0.5时(在程序中浮点数不能直接进行等于判断,要使用合理的阈值),俯仰角ββ为90°,将其带入正向公式计算出四元数(q0,q1,q2,q3)(q0,q1,q2,q3),然后可以发现逆向公式中atan2函数中的参数全部为0,即出现了0000的情况!无法计算。

  β=π/2β=π/2时,sinβ2=cosβ2=0.707sin⁡β2=cos⁡β2=0.707,将其带入公式中有

 

q=⎡⎣⎢⎢⎢wxyz⎤⎦⎥⎥⎥⎡⎣⎢⎢⎢⎢⎢⎢0.707(cosα2cosγ2+sinα2sinγ2)0.707(sinα2cosγ2cosα2sinγ2)0.707(cosα2cosγ2+sinα2sinγ2)0.707(cosα2sinγ2sinα2cosγ2)⎤⎦⎥⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢0.707cosαγ20.707sinαγ20.707cosαγ20.707sinαγ2⎤⎦⎥⎥⎥⎥⎥⎥q=[wxyz][0.707(cos⁡α2cos⁡γ2+sin⁡α2sin⁡γ2)0.707(sin⁡α2cos⁡γ2−cos⁡α2sin⁡γ2)0.707(cos⁡α2cos⁡γ2+sin⁡α2sin⁡γ2)0.707(cos⁡α2sin⁡γ2−sin⁡α2cos⁡γ2)]=[0.707cos⁡α−γ20.707sin⁡α−γ20.707cos⁡α−γ20.707sin⁡α−γ2]

 

  则xw=zy=tanαγ2xw=zy=tan⁡α−γ2,于是有

 

αγ=2atan2(x,w)α−γ=2⋅atan2(x,w)

 

   通常令α=0α=0,这时γ=2atan2(x,w)γ=−2⋅atan2(x,w)。可以进行验证:当四元数为(w,x,y,z)=(0.653,-0.271,0.653,0.271)时,根据这些规则计算出来的ZYX欧拉角为α=0°,β=90°,γ=45°

  当俯仰角为-90°,即机头竖直向下时的情况也与之类似,可以推导出奇异姿态时的计算公式。比较完整的四元数转欧拉角(Z-Y-X order)的代码如下:

复制代码

复制代码

 


  在DirectXMath Library中有许多与刚体姿态变换相关的函数可以直接调用:

  • 四元数乘法:XMQuaternionMultiply method --Computes the product of two quaternions.
  • 旋转矩阵转四元数:XMQuaternionRotationMatrix method --Computes a rotation quaternion from a rotation matrix.
  • 四元数转旋转矩阵:XMMatrixRotationQuaternion method -- Builds a rotation matrix from a quaternion.
  • 欧拉角转四元数:XMQuaternionRotationRollPitchYaw method --Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles).
  • 四元数转Axis-Angle:XMQuaternionToAxisAngle method --Computes an axis and angle of rotation about that axis for a given quaternion.
  • 欧拉角转旋转矩阵:XMMatrixRotationRollPitchYaw method --Builds a rotation matrix based on a given pitch, yaw, and roll (Euler angles).
  • Axis-Angle转旋转矩阵:XMMatrixRotationAxis method --Builds a matrix that rotates around an arbitrary axis.
  • 构造绕X/Y/Z轴的旋转矩阵:XMMatrixRotationX method --Builds a matrix that rotates around the x-axis.(Angles are measured clockwise when looking along the rotation axis toward the origin)

   下面的代码中坐标系绕X轴旋转90°(注意这里不是按照右手定则的方向,而是沿着坐标轴向原点看过去以顺时针方式旋转,因此与传统的右手定则刚好方向相反),来进行变换:

复制代码

复制代码

  结果如下图所示:

 

参考:

quaternions.online

DirectXMath Library Quaternion Functions

Convert quaternion to euler rotations

Conversion between quaternions and Euler angles

Maths - Conversion Quaternion to Euler

Coordinate Transformations in Robotics—MATLAB

Introduction to Robotics - Mechanics and Control. Chapter 2 Spatial descriptions and transformations

posted on 2018-09-20 15:51  无情风91  阅读(2031)  评论(0编辑  收藏  举报

导航