四元数、欧拉角、旋转矩阵、旋转向量之间的转换
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;
3.4 四元数
- 初始化四元数
Eigen::Quaterniond quaternion(w,x,y,z);
- 四元数转旋转向量
Eigen::AngleAxisd rotation_vector(quaternion);Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;
- 四元数转旋转矩阵
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();
- 四元数转欧拉角(Z-Y-X,即RPY)
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
四元数、欧拉角、旋转矩阵、旋转向量之间的转换相关推荐
- 【自动驾驶】30.c++实现基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换(附代码)
矩阵的使用可参考系列博客:点击此处 原文链接:基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换. 也可以参考另一篇博客:eigen 中四元数.欧拉角.旋转矩阵.旋转向量. ...
- eigen 编译_头条 | 使用eigen实现四元数、欧拉角、旋转矩阵、旋转向量间的转换...
点击上方蓝字,关注本公众号,获得更多资源上一篇文章介绍了四元数.欧拉角.旋转矩阵.轴角如何相互转换,本篇文章介绍如何用eigen来实现. 旋转向量 1,初始化旋转向量:旋转角为alpha,旋转轴为(x ...
- 罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化
罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格斯公式推导 2 cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格 ...
- 【自动驾驶】24.欧拉角、旋转向量、四元数、万向锁
原文链接:https://blog.csdn.net/shenxiaolu1984/article/details/50639298 原文链接:https://blog.csdn.net/lql071 ...
- [转] 罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化
罗德里格斯公式推导,以及如何使用cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格斯公式推导 2 cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化 1 罗德里格 ...
- Sophus降维、升维与欧拉角、旋转向量的爱恨情仇
0. 简介 在面对二维与三维之间的转换时,我们常常会困惑该如何去转换,在G2O中存在有理想的坐标转换工具,但是在Sophus中却缺乏这样的手段.之前在Sophus处简要的介绍了一下SE(2)与SE(3 ...
- 三维坐标变换(旋转矩阵旋转向量)
矩阵运算显然是计算机三维坐标变换最简单方便的计算方法,因此在 opencv.opengl.工业机器人等开发中,提到位姿旋转变换,多半用的是旋转矩阵.然而,用矩阵来表示一个旋转关系有两个缺点: 首先,通 ...
- python二郎成长笔记(三)(matlab标定工具箱详解,旋转矩阵旋转向量,matlab标定数据传入opencv)
这一章由来:二郎之前研究matlab的双目立体视觉,已经得到了需要的信息,可是,二郎想要对代码进行修改使其更适宜自己的应用目的. 修改:标定不用修改--matlab内置的已经很强大了,而且没有必要用p ...
- 旋转向量与欧拉角 罗德里格斯公式(Rodrigues's Formula)
旋转向量 旋转矩阵表达方式 旋转矩阵描述旋转,变换矩阵描述一个6自由度的三维刚体运动.但存在如下缺点: SO(3)的旋转矩阵有9个量,但一次旋转只有3个自由度.因此这种表达方式是冗余的.同理,变换矩阵 ...
- 旋转矩阵、旋转向量(轴角)、四元数、欧拉角之间相互转换的代码实现(利用Eigen实现)...
1 #include <iostream> 2 #include <cmath> 3 using namespace std; 4 5 #include <Eigen/C ...
最新文章
- LNMP环境搭建(一)搭建PHP环境
- C# dynamic使用
- 3级联动 ajax java_java+ajax(三级连动下拉框)
- 第七章 Web开发实战2——商品详情页
- Apache JMeter 下载
- RabbitMQ交换器Exchange介绍与实践
- 程序员都长得丑?颜值底线是程序员?
- 在Ubuntu上安装使用深度影音深度音乐
- [Win/Mac]下载工具推荐及使用
- 【优化算法】可变步长LMS算法(VSS-LMS)【含Matlab源码 317期】
- 禁用使用未签名驱动导致的windows 7自动修复
- c语言符号意思大全,c语言符号意思
- python富翁与穷人_穷人和富人最根本的区别
- Java Eclipse internal error Polling news feeds 解决方案
- 阿里云超级码力第二场记录
- Z字形变换(Java)
- cad中怎么随意移动图形_CAD中需要挪动图形到准确位置?试试这几种方法
- 计算机保研统计方向,北京大学统计科学中心保研申请全解析
- InPaint V7.0
- 工厂控制灯光系统小结(观小蜜蜂老师教学视频有感)
热门文章
- 工业企业成本费用的核算方法
- 计算机导师问读研计划和后续计划,考研面试,问“研究生时期的规划”怎么回答急...
- 反安装冰盾抗DDOS防火墙后出现启动报错的解决方法
- 视频会议十大开源项目排行
- diskpart 删除磁盘OEM分区 及设置活动分区
- gsm在linux下工作原理,GSM模块详解 从工作原理到应用案例
- linux进程终止命令,Linux kill命令:终止进程
- 一个有趣的二维码生成库Qart
- linux 磁盘隔离,Linux 磁盘坏道故障修复
- HTML5期末大作业:海美食网站设计——上海美食(8页) 酒水网页设计作业,甜品美食网页制作作业, 学生零食网页作业