math: 四元数与欧拉角(RPY角)的相互转换
2018-03-20 17:18
399 查看
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欧拉角,由于是绕自身坐标轴进行旋转,则旋转矩阵为:
RZ′Y′X′(α,β,γ)=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=kx⋅sinθ2=ky⋅sinθ2=kz⋅sinθ2=cosθ2x=kx⋅sinθ2y=ky⋅sinθ2z=kz⋅sinθ2w=cosθ2 且有x2+y2+z2+w2=1x2+y2+z2+w2=1
即四元数存储了旋转轴和旋转角的信息,它能方便的描述刚体绕任意轴的旋转。 四元数转换为旋转矩阵:
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⎤⎦⎥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,需要先导入才能使用。下面计算了三个四元数的乘积:
根据上面的公式可以求出逆解,即由四元数q=(q0,q1,q2,q3)q=(q0,q1,q2,q3)或q=(w,x,y,z)q=(w,x,y,z)到欧拉角的转换为:⎡⎣⎢αβγ⎤⎦⎥=⎡⎣⎢⎢⎢⎢arctan2(q0q1+q2q3)1−2(q21+q22)arcsin(2(q0q2−q1q3))arctan2(q0q3+q1q2)1−2(q22+q23)⎤⎦⎥⎥⎥⎥[αβγ]=[arctan2(q0q1+q2q3)1−2(q12+q22)arcsin(2(q0q2−q1q3))arctan2(q0q3+q1q2)1−2(q22+q32)]
由于arctan和arcsin的取值范围在−π2−π2和π2π2之间,只有180°,而绕某个轴旋转时范围是360°,因此要使用atan2函数代替arctan函数:⎡⎣⎢αβγ⎤⎦⎥=⎡⎣⎢atan2(2(q0q1+q2q3),1−2(q21+q22))arcsin(2(q0q2−q1q3))atan2(2(q0q3+q1q2),1−2(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欧拉角旋转。
这时会发生自由度丢失的情况,即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题: arcsin(2(q0q2−q1q3))arcsin(2(q0q2−q1q3))的定义域为[−1,1][−1,1],因此(q0q2−q1q3)∈[−0.5,0.5](q0q2−q1q3)∈[−0.5,0.5],当q0q2−q1q3=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γ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⎤⎦⎥⎥⎥⎥⎥⎥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,于是有
α−γ=2⋅atan2(x,w)α−γ=2⋅atan2(x,w) 通常令α=0α=0,这时γ=−2⋅atan2(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)的代码如下:
旋转矩阵转四元数: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.onlineDirectXMath Library Quaternion FunctionsConvert quaternion to euler rotationsConversion between quaternions and Euler anglesMaths - Conversion Quaternion to EulerCoordinate Transformations in Robotics—MATLABIntroduction to Robotics - Mechanics and Control. Chapter 2 Spatial descriptions and transformations
描述坐标系{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欧拉角,由于是绕自身坐标轴进行旋转,则旋转矩阵为:
RZ′Y′X′(α,β,γ)=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=kx⋅sinθ2=ky⋅sinθ2=kz⋅sinθ2=cosθ2x=kx⋅sinθ2y=ky⋅sinθ2z=kz⋅sinθ2w=cosθ2 且有x2+y2+z2+w2=1x2+y2+z2+w2=1
即四元数存储了旋转轴和旋转角的信息,它能方便的描述刚体绕任意轴的旋转。 四元数转换为旋转矩阵:
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⎤⎦⎥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,需要先导入才能使用。下面计算了三个四元数的乘积:
<<Quaternions` (* This loads the package *) Quaternion[2, 1, 1, 3] ** Quaternion[2, 1, 1, 0] ** Quaternion[1, 1, 1, 1] (* Be sure to use ** rather than * when multiplying quaternions *)计算结果为: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γ2−cosα2sinβ2sinγ2cosα2sinβ2cosγ2+sinα2cosβ2sinγ2cosα2cosβ2sinγ2−sinα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)1−2(q21+q22)arcsin(2(q0q2−q1q3))arctan2(q0q3+q1q2)1−2(q22+q23)⎤⎦⎥⎥⎥⎥[αβγ]=[arctan2(q0q1+q2q3)1−2(q12+q22)arcsin(2(q0q2−q1q3))arctan2(q0q3+q1q2)1−2(q22+q32)]
由于arctan和arcsin的取值范围在−π2−π2和π2π2之间,只有180°,而绕某个轴旋转时范围是360°,因此要使用atan2函数代替arctan函数:⎡⎣⎢αβγ⎤⎦⎥=⎡⎣⎢atan2(2(q0q1+q2q3),1−2(q21+q22))arcsin(2(q0q2−q1q3))atan2(2(q0q3+q1q2),1−2(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欧拉角旋转。
enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx};
// COMPILE: g++ -o quat2EulerTest quat2EulerTest.cpp #include <iostream> #include <cmath> using namespace std; /////////////////////////////// // Quaternion struct // Simple incomplete quaternion struct for demo purpose /////////////////////////////// struct Quaternion{ Quaternion():x(0), y(0), z(0), w(1){}; Quaternion(double x, double y, double z, double w):x(x), y(y), z(z), w(w){}; void normalize(){ double norm = std::sqrt(x*x + y*y + z*z + w*w); x /= norm; y /= norm; z /= norm; w /= norm; } double norm(){ return std::sqrt(x*x + y*y + z*z + w*w); } double x; double y; double z; double w; }; /////////////////////////////// // Quaternion to Euler /////////////////////////////// enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx}; void twoaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){ res[0] = atan2( r11, r12 ); res[1] = acos ( r21 ); res[2] = atan2( r31, r32 ); } void threeaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){ res[0] = atan2( r31, r32 ); res[1] = asin ( r21 ); res[2] = atan2( r11, r12 ); } void quaternion2Euler(const Quaternion& q, double res[], RotSeq rotSeq) { switch(rotSeq){ case zyx: threeaxisrot( 2*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z, -2*(q.x*q.z - q.w*q.y), 2*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z, res); break; case zyz: twoaxisrot( 2*(q.y*q.z - q.w*q.x), 2*(q.x*q.z + q.w*q.y), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z, 2*(q.y*q.z + q.w*q.x), -2*(q.x*q.z - q.w*q.y), res); break; case zxy: threeaxisrot( -2*(q.x*q.y - q.w*q.z), q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z, 2*(q.y*q.z + q.w*q.x), -2*(q.x*q.z - q.w*q.y), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z, res); break; case zxz: twoaxisrot( 2*(q.x*q.z + q.w*q.y), -2*(q.y*q.z - q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z, 2*(q.x*q.z - q.w*q.y), 2*(q.y*q.z + q.w*q.x), res); break; case yxz: threeaxisrot( 2*(q.x*q.z + q.w*q.y), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z, -2*(q.y*q.z - q.w*q.x), 2*(q.x*q.y + q.w*q.z), q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z, res); break; case yxy: twoaxisrot( 2*(q.x*q.y - q.w*q.z), 2*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z, 2*(q.x*q.y + q.w*q.z), -2*(q.y*q.z - q.w*q.x), res); break; case yzx: threeaxisrot( -2*(q.x*q.z - q.w*q.y), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z, 2*(q.x*q.y + q.w*q.z), -2*(q.y*q.z - q.w*q.x), q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z, res); break; case yzy: twoaxisrot( 2*(q.y*q.z + q.w*q.x), -2*(q.x*q.y - q.w*q.z), q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z, 2*(q.y*q.z - q.w*q.x), 2*(q.x*q.y + q.w*q.z), res); break; case xyz: threeaxisrot( -2*(q.y*q.z - q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z, 2*(q.x*q.z + q.w*q.y), -2*(q.x*q.y - q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z, res); break; case xyx: twoaxisrot( 2*(q.x*q.y + q.w*q.z), -2*(q.x*q.z - q.w*q.y), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z, 2*(q.x*q.y - q.w*q.z), 2*(q.x*q.z + q.w*q.y), res); break; case xzy: threeaxisrot( 2*(q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z, -2*(q.x*q.y - q.w*q.z), 2*(q.x*q.z + q.w*q.y), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z, res); break; case xzx: twoaxisrot( 2*(q.x*q.z - q.w*q.y), 2*(q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z, 2*(q.x*q.z + q.w*q.y), -2*(q.x*q.y - q.w*q.z), res); break; default: std::cout << "Unknown rotation sequence" << std::endl; break; } } /////////////////////////////// // Helper functions /////////////////////////////// Quaternion operator*(Quaternion& q1, Quaternion& q2){ Quaternion q; q.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z; q.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y; q.y = q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x; q.z = q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w; return q; } ostream& operator <<(std::ostream& stream, const Quaternion& q) { cout << q.w << " "<< showpos << q.x << "i " << q.y << "j " << q.z << "k"; cout << noshowpos; } double rad2deg(double rad){ return rad*180.0/M_PI; } /////////////////////////////// // Main /////////////////////////////// int main(){ Quaternion q; // x,y,z,w Quaternion qx45(sin(M_PI/8), 0,0, cos(M_PI/8) ); Quaternion qy45(0, sin(M_PI/8), 0, cos(M_PI/8)); Quaternion qz45(0, 0, sin(M_PI/8), cos(M_PI/8)); Quaternion qx90(sin(M_PI/4), 0,0, cos(M_PI/4) ); Quaternion qy90(0, sin(M_PI/4), 0, cos(M_PI/4)); Quaternion qz90(0, 0, sin(M_PI/4), cos(M_PI/4)); double res[3]; q = qz45*qx45; q.normalize(); quaternion2Euler(q, res, zyx); cout << "Rotation sequence: X->Y->Z" << endl; cout << "x45 -> z45" << endl; cout << "q: " << q << endl; cout << "x: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " z: " << rad2deg(res[2]) << endl << endl; q = qz90*qx90; q.normalize(); quaternion2Euler(q, res, zyx); cout << "Rotation sequence: X->Y->Z" << endl; cout << "x90 -> z90" << endl; cout << "q: " << q << endl; cout << "x: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " z: " << rad2deg(res[2]) << endl << endl; q = qx90*qz90; q.normalize(); quaternion2Euler(q, res, xyz); cout << "Rotation sequence: Z->Y->X" << endl; cout << "z90 -> x90" << endl; cout << "q: " << q << endl; cout << "x: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " z: " << rad2deg(res[2]) << endl; }上面的代码存在一个问题,即奇异性没有考虑。下面看一种特殊的情况(参考Maths - Conversion Quaternion to Euler):假设一架飞机绕Y轴旋转了90°(俯仰角pitch=90),机头垂直向上,此时如何计算航向角和横滚角?
这时会发生自由度丢失的情况,即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题: arcsin(2(q0q2−q1q3))arcsin(2(q0q2−q1q3))的定义域为[−1,1][−1,1],因此(q0q2−q1q3)∈[−0.5,0.5](q0q2−q1q3)∈[−0.5,0.5],当q0q2−q1q3=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γ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⎤⎦⎥⎥⎥⎥⎥⎥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,于是有
α−γ=2⋅atan2(x,w)α−γ=2⋅atan2(x,w) 通常令α=0α=0,这时γ=−2⋅atan2(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)的代码如下:
CameraSpacePoint QuaternionToEuler(Vector4 q) // Z-Y-X Euler angles { CameraSpacePoint euler = { 0 }; const double Epsilon = 0.0009765625f; const double Threshold = 0.5f - Epsilon; double TEST = q.w*q.y - q.x*q.z; if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90° { int sign = Sign(TEST); euler.Z = -2 * sign * (double)atan2(q.x, q.w); // yaw euler.Y = sign * (PI / 2.0); // pitch euler.X = 0; // roll } else { euler.X = atan2(2 * (q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z); euler.Y = asin(-2 * (q.x*q.z - q.w*q.y)); euler.Z = atan2(2 * (q.x*q.y + q.w*q.z), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z); } return euler; }在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°(注意这里不是按照右手定则的方向,而是沿着坐标轴向原点看过去以顺时针方式旋转,因此与传统的右手定则刚好方向相反),来进行变换:
#include "stdafx.h" #include<iostream> #include <DirectXMath.h> using namespace DirectX; #define PI 3.1415926 int _tmain(int argc, _TCHAR* argv[]) { //-------------------Computes the product of two quaternions. XMVECTOR q1 = XMVectorSet(1, 1, 3, 2); XMVECTOR q2 = XMVectorSet(1, 1, 0, 2); XMVECTOR q3 = XMVectorSet(1, 1, 1, 1); XMVECTOR result = XMQuaternionMultiply(XMQuaternionMultiply(q3, q2), q1); // Returns the product of two quaternions as q1*q2*q3 std::cout << "Quaternion Multiply:" << std::endl; std::cout << XMVectorGetX(result) << "," << XMVectorGetY(result) << "," << XMVectorGetZ(result) << "," << XMVectorGetW(result) << std::endl << std::endl; //------------------Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles). float pitch = 90.0 * PI / 180.0; // Angle of rotation around the x-axis, in radians. float yaw = 0; // Angle of rotation around the y-axis, in radians. float roll = 0; // Angle of rotation around the z - axis, in radians. result = XMQuaternionRotationRollPitchYaw(pitch, yaw, roll); std::cout << "RPY/Euler angles to Quaternion:" << std::endl; std::cout << XMVectorGetX(result) << "," << XMVectorGetY(result) << "," << XMVectorGetZ(result) << "," << XMVectorGetW(result) << std::endl << std::endl; //-----------------Computes a rotation quaternion from a rotation matrix. float matrix[16] = { 1, 0, 0, 0, 0, 0, 1, 0, 0, -1, 0, 0, 0, 0, 0, 1 }; XMMATRIX trans(matrix); // Initializes a new instance of the XMMATRIX structure from a sixteen element float array. result = XMQuaternionRotationMatrix(trans); // This function only uses the upper 3x3 portion of the XMMATRIX. std::cout << "Matrix to Quaternion:" << std::endl; std::cout << XMVectorGetX(result) << "," << XMVectorGetY(result) << "," << XMVectorGetZ(result) << "," << XMVectorGetW(result) << std::endl << std::endl; //-----------------Builds a rotation matrix from a quaternion. trans = XMMatrixRotationQuaternion(result); XMFLOAT3X3 fView; XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3 std::cout << "Quaternion to Matrix:" << std::endl; std::cout << fView._11 << "," << fView._12 << "," << fView._13 << std::endl << fView._21 << "," << fView._22 << "," << fView._23 << std::endl << fView._31 << "," << fView._32 << "," << fView._33 << std::endl << std::endl; //-----------------Computes an axis and angle of rotation about that axis for a given quaternion. float Angle = 0; XMVECTOR Axis; XMQuaternionToAxisAngle(&Axis, &Angle, result); Axis = XMVector3Normalize(Axis); // Returns the normalized version of a 3D vector std::cout << "Quaternion to Axis-Angle:" << std::endl; std::cout << "Axis: " << XMVectorGetX(Axis) << "," << XMVectorGetY(Axis) << "," << XMVectorGetZ(Axis) << std::endl; std::cout << "Angle: " << Angle*180.0 / PI << std::endl << std::endl; //-----------------Builds a matrix that rotates around an arbitrary axis. Angle = 90.0 * PI / 180.0; trans = XMMatrixRotationAxis(Axis, Angle); XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3 std::cout << "Axis-Angle to Matrix:" << std::endl; std::cout << fView._11 << "," << fView._12 << "," << fView._13 << std::endl << fView._21 << "," << fView._22 << "," << fView._23 << std::endl << fView._31 << "," << fView._32 << "," << fView._33 << std::endl << std::endl; //-----------------Builds a rotation matrix based on a given pitch, yaw, and roll(Euler angles). trans = XMMatrixRotationRollPitchYaw(pitch, yaw, roll); XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3 std::cout << "RPY/Euler angles to Matrix:" << std::endl; std::cout << fView._11 << "," << fView._12 << "," << fView._13 << std::endl << fView._21 << "," << fView._22 << "," << fView._23 << std::endl << fView._31 << "," << fView._32 << "," << fView._33 << std::endl << std::endl; //-----------------Builds a matrix that rotates around the x - axis. trans = XMMatrixRotationX(Angle); // Angles are measured clockwise when looking along the rotation axis toward the origin. XMStoreFloat3x3(&fView, trans); // Stores an XMMATRIX in an XMFLOAT3X3 std::cout << "Builds a matrix that rotates around the x-axis.:" << std::endl; std::cout << fView._11 << "," << fView._12 << "," << fView._13 << std::endl << fView._21 << "," << fView._22 << "," << fView._23 << std::endl << fView._31 << "," << fView._32 << "," << fView._33 << std::endl << std::endl; return 0; }结果如下图所示:
参考:quaternions.onlineDirectXMath Library Quaternion FunctionsConvert quaternion to euler rotationsConversion between quaternions and Euler anglesMaths - Conversion Quaternion to EulerCoordinate Transformations in Robotics—MATLABIntroduction to Robotics - Mechanics and Control. Chapter 2 Spatial descriptions and transformations
相关文章推荐
- 四元数与欧拉角(RPY角)的相互转换
- 三维空间 旋转矩阵 四元数 欧拉角 相互转换 组合旋转
- Unity 四元数与欧拉角的相互转换及推导
- 四元数、欧拉角及方向余弦矩阵的相互转换公式
- 四元数、欧拉角及方向余弦矩阵的相互转换公式
- 四元数、欧拉角及方向余弦矩阵的相互转换公式
- 四元数,欧拉角,矩阵的相互转换
- 四元数与欧拉角之间的转换
- 四元数与欧拉角(Yaw、Pitch、Roll)的转换
- 飞控姿态解算中,欧拉角与四元数之间的转换
- 四元数与欧拉角之间的转换
- 欧拉角、四元数和方向余弦的转换关系
- 四元数与欧拉角(roll,pitch,yaw)的转换
- 四元数与欧拉角(Yaw、Pitch、Roll)的转换
- 四元数与欧拉角---wii 的坐标系转换
- 坐标系转换之三:欧拉角、四元数、旋转矩阵、方向余弦矩阵、旋转向量、轴角表示
- 旋转矩阵、欧拉角、四元数、轴/角之间的转换
- crazyflie2.0中欧拉角和四元数之间转换的问题
- math: 四元数转欧拉角
- CSharpGL(32)矩阵与四元数与角度旋转轴的相互转换