600字范文 > 四元数欧拉角旋转矩阵旋转向量之间的转换

四元数欧拉角旋转矩阵旋转向量之间的转换

时间：2023-06-21 17:28:18

相关推荐

四元数欧拉角旋转矩阵旋转向量之间的转换

Eigen 中四元数、欧拉角、旋转矩阵、旋转向量之间的转换

1. 欧拉旋转定理2. 轴角表示2.1 旋转性质3. 罗德里格斯公式4. pitch yaw roll方向5. 内旋和外旋6. 使用哪种表示方法？3. 四元数、欧拉角、旋转矩阵、旋转向量之间的转换3.1 旋转向量3.2 旋转矩阵3.3 欧拉角3.4 四元数

1. 欧拉旋转定理

在运动学里，欧拉旋转定理(Euler’s rotation theorem)表明，在三维空间里，假设一个刚体在做一个旋转的时候，刚体内部至少有一点固定不动，则此位移等价于一个绕着包含那固定点的固定轴的旋转。

2. 轴角表示

旋转一个向量从 aaa 到 bbb，轴角为 (u,θ)(u,\theta)(u,θ)，如下图所示：

罗德里格斯公式：

b=acosθ+(u×a)sinθ+u(u⋅a)(1−cosθ)b=acos\theta+(u\times a)sin\theta+u(u\cdot a)(1-cos\theta)b=acosθ+(u×a)sinθ+u(u⋅a)(1−cosθ)

2.1 旋转性质

(u,θ)(u,\theta)(u,θ) 和 (−u,−θ)(-u,-\theta)(−u,−θ) 为相同旋转。(u,−θ)(u,-\theta)(u,−θ) 为 (u,θ)(u,\theta)(u,θ) 的逆旋转。旋转运算是不能交换的，如：

a=[300]⊤(u1,θ1)=([100]⊤,90∘)(u2,θ2)=([010]⊤,90∘)a=\left[\begin{array}{lll}3 & 0 & 0\end{array}\right]^{\top} \quad\left(u_{1}, \theta_{1}\right)=\left(\left[\begin{array}{lll}1 & 0 & 0\end{array}\right]^{\top}, 90^{\circ}\right) \quad\left(u_{2}, \theta_{2}\right)=\left(\left[\begin{array}{lll}0 & 1 & 0\end{array}\right]^{\top}, 90^{\circ}\right)a=[300]⊤(u1,θ1)=([100]⊤,90∘)(u2,θ2)=([010]⊤,90∘)

情况一：

旋转 aaa (u1,θ1)(u_1,\theta_1)(u1,θ1)，得到 b=a=[300]⊤b=a=\left[\begin{array}{lll}3 & 0 & 0\end{array}\right]^{\top}b=a=[300]⊤

旋转 bbb (u2,θ2)(u_2,\theta_2)(u2,θ2)，得到 c=[003]⊤c=\left[\begin{array}{lll}0 & 0 & 3\end{array}\right]^{\top}c=[003]⊤

情况二：

旋转 aaa (u2,θ2)(u_2,\theta_2)(u2,θ2)，得到 b=a=[003]⊤b=a=\left[\begin{array}{lll}0 & 0 & 3\end{array}\right]^{\top}b=a=[003]⊤

旋转 bbb (u1,θ1)(u_1,\theta_1)(u1,θ1)，得到 c=[030]⊤c=\left[\begin{array}{lll}0 & 3 & 0\end{array}\right]^{\top}c=[030]⊤

3. 罗德里格斯公式

向量 a,ba, ba,b 可以被分解成相对于 uuu 的正交 (a⊥,b⊥)(a_{\perp}, b_{\perp})(a⊥,b⊥) 和平行 (a∥,b∥)(a_{\|}, b_{\|})(a∥,b∥) 分量。

此时以下关系是成立的：

a∥=b∥a_{\|}=b_{\|}a∥=b∥

b⊥=a⊥cos⁡θ+ksin⁡θb_{\perp}=a_{\perp} \cos \theta+k \sin \thetab⊥=a⊥cosθ+ksinθ

因此：

b=b⊥+b∥=b⊥cos⁡θ+ksin⁡θ+u(u⋅a)=acos⁡θ−u(u⋅a)cos⁡θ+(u×a)sin⁡θ+u(u⋅a)=acos⁡θ+(u×a)sin⁡θ+u(u⋅a)(1−cos⁡θ)\begin{aligned} b &=b_{\perp}+b_{\|}=b_{\perp} \cos \theta+k \sin \theta+u(u \cdot a) \\ &=a \cos \theta-u(u \cdot a) \cos \theta+(u \times a) \sin \theta+u(u \cdot a) \\ &=a \cos \theta+(u \times a) \sin \theta+u(u \cdot a)(1-\cos \theta) \end{aligned}b=b⊥+b∥=b⊥cosθ+ksinθ+u(u⋅a)=acosθ−u(u⋅a)cosθ+(u×a)sinθ+u(u⋅a)=acosθ+(u×a)sinθ+u(u⋅a)(1−cosθ)

4. pitch yaw roll方向

roll: 翻滚角，顺时针翻滚即正值，逆时针翻滚为负值(图左)。

yaw(heading): 航向角(图中)。

pitch: 俯视角，俯仰是从局部 xyxyxy 平面的旋转(图右)。

5. 内旋和外旋

按旋转的坐标系分为内旋(intrinsic rotation)和外旋(extrinsic rotation)。

内旋: 绕物体自身的坐标系(object-space)旋转，举个例子，一个 (ϕ,θ,ψ)(\phi, \theta, \psi)(ϕ,θ,ψ) (xyz,intrinsic)(xyz, intrinsic)(xyz,intrinsic) 的欧拉角，指绕物体的 xxx 轴转 ϕ\phiϕ 后，再绕物体的 y′y'y′ 轴(这里用 y′y'y′ 表示这个新的 yyy 轴已经和一开始世界坐标系下的那个物体的 yyy 轴不一样了)旋转 θ\thetaθ，最后绕 z′z'z′ 轴旋转 ψ\psiψ，每一次旋转都会改变下一次旋转的轴。这种情况下旋转的轴是动态(moving axis)的。

外旋: 绕惯性系(upright-space)旋转(upright space 指基向量平行于 world-space 或 parent-space，原点与 object-space 的原点重合的空间)。也就是说，无论是三步旋转中的哪一步，轴都是固定不会动的。

内旋与外旋的转换关系：互换第一次和第三次旋转的位置则两者结果相同。例如 Z−Y−XZ-Y-XZ−Y−X 旋转的内部旋转和 X−Y−ZX-Y-ZX−Y−Z 旋转的外部旋转的旋转矩阵相同。

证明：

假设绕 XYZXYZXYZ 三个轴旋转的角度分别为 α\alphaα β\betaβ γ\gammaγ，则三次旋转的旋转矩阵计算方法如下：

Rx(α)=[1000cos⁡α−sin⁡α0sin⁡αcos⁡α]Ry(β)=[cos⁡β0sin⁡β010−sin⁡β0cos⁡β]Rz(γ)=[cos⁡γ−sin⁡γ0sin⁡γcos⁡γ001]\begin{array}{l} R_{x}(\alpha)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \alpha & -\sin \alpha \\ 0 & \sin \alpha & \cos \alpha \end{array}\right] \\ R_{y}(\beta)=\left[\begin{array}{ccc} \cos \beta & 0 & \sin \beta \\ 0 & 1 & 0 \\ -\sin \beta & 0 & \cos \beta \end{array}\right] \\ R_{z}(\gamma)=\left[\begin{array}{ccc} \cos \gamma & -\sin \gamma & 0 \\ \sin \gamma & \cos \gamma & \\ 0 & 0 & 1 \end{array}\right] \end{array} Rx(α)=⎣⎡1000cosαsinα0−sinαcosα⎦⎤Ry(β)=⎣⎡cosβ0−sinβ010sinβ0cosβ⎦⎤Rz(γ)=⎣⎡cosγsinγ0−sinγcosγ001⎦⎤

按照内旋方式，Z−Y−XZ-Y-XZ−Y−X 旋转顺序（指先绕自身轴ZZZ，再绕自身轴YYY，最后绕自身轴XXX），可得旋转矩阵(内旋是右乘)：

R1=Rz(γ)∗Ry(β)∗Rx(α)R 1=R_{z}(\gamma) * R_{y}(\beta) * R_{x}(\alpha) R1=Rz(γ)∗Ry(β)∗Rx(α)

按照外旋方式，X−Y−ZX-Y-ZX−Y−Z 旋转顺序（指先绕固定轴XXX，再绕固定轴YYY，最后绕固定轴ZZZ），可得旋转矩阵(外旋是左乘)：

R2=Rz(γ)∗Ry(β)∗Rx(α)R 2=R_{z}(\gamma) * R_{y}(\beta) * R_{x}(\alpha) R2=Rz(γ)∗Ry(β)∗Rx(α)

因此 R1=R2R1=R2R1=R2，ZYXZYXZYX 顺序的内旋等价于 XYZXYZXYZ 顺序的外旋。

SLAM十四讲中提到的常用旋转顺序是 Z−Y−XZ-Y-XZ−Y−X，对应 RPY(Roll-Pitch-Yaw) 角，指的就是内旋（绕自身轴）Z−Y−XZ-Y-XZ−Y−X 顺序。而欧拉角转换成旋转矩阵（相对于世界坐标系的旋转矩阵）通常是按外旋方式（绕固定轴），即 X−Y−ZX-Y-ZX−Y−Z 顺序，所以旋转矩阵为：

R=R2=Rz(γ)∗Ry(β)∗Rx(α)R = R 2=R_{z}(\gamma) * R_{y}(\beta) * R_{x}(\alpha) R=R2=Rz(γ)∗Ry(β)∗Rx(α)

6. 使用哪种表示方法？

奇异性和不连续性: 用很少的参数表示可能导致数值问题：

欧拉角(3)：

–奇异性：万向锁配置

–不连续性：从 0° 跳变到 360°

旋转向量(3)：

–不连续性：从 0° 跳变到 360°

旋转矩阵(9)/四元数(4)：

– 没有奇异性或不连续性

几何解释: 有些表示法更容易用几何解释。它们更接近原始的传感器测量，也更容易与人类用户交互(显示或输入)

欧拉角：

– 参数是接近几何解释的角度和轴

旋转矩阵/四元数：

– 参数是正弦/余弦函数，其几何解释有些无关紧要

3. 四元数、欧拉角、旋转矩阵、旋转向量之间的转换

3.1 旋转向量

初始化旋转向量：旋转角为 alpha，旋转轴为 (x,y,z)(x,y,z)(x,y,z)

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))；

旋转向量转旋转矩阵

Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.matrix();Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.toRotationMatrix();

旋转向量转欧拉角(Z-Y-X内旋，即 RPY)

//(2,1,0)表示旋转顺序ZYX，数字越小表示优先级越高Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);

旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);

3.2 旋转矩阵

初始化旋转矩阵

Eigen::Matrix3d rotation_matrix;rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

旋转矩阵转旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);Eigen::AngleAxisd rotation_vector;rotation_vector=rotation_matrix;Eigen::AngleAxisd rotation_vector;rotation_vector.fromRotationMatrix(rotation_matrix);

旋转矩阵转欧拉角(Z-Y-X内旋，即 RPY)

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);

旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_matrix);Eigen::Quaterniond quaternion;quaternion=rotation_matrix;

3.3 欧拉角

初始化欧拉角(Z-Y-X，即RPY)

Eigen::Vector3d eulerAngle(yaw,pitch,roll);

欧拉角转旋转向量

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));Eigen::AngleAxisd rotation_vector;rotation_vector=yawAngle*pitchAngle*rollAngle;

欧拉角转旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));Eigen::Matrix3d rotation_matrix;rotation_matrix=yawAngle*pitchAngle*rollAngle;

欧拉角转四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ()));Eigen::Quaterniond quaternion;quaternion=yawAngle*pitchAngle*rollAngle;