四元数与欧拉角(Yaw、Pitch、Roll)的转换
目录
0、简介
一、四元数的定义
二、欧拉角到四元数的转换
2.1 公式:
2.2 code:
三、四元数到欧拉角的转换
3.1 公式
3.2 code:
3.3 四元素到旋转矩阵转换
四. 奇点
五. 矢量旋转
证明:
六 . 其他参考
七 python 转换
7.1 四元素欧拉角互相转换
7.2 旋转矩阵<->欧拉角 py
八 Eigen transform
8.1 欧拉角到四元素
四元素得到yaw
四元素到旋转向量
旋转轴向量到四元素
Eigen 转换函数
九 旋转矩阵与欧拉角
0、简介
四元数与欧拉角之间的转换
百度百科四元素
在3D图形学中,最常用的旋转表示方法便是四元数和欧拉角,比起矩阵来具有节省存储空间和方便插值的优点。
本文主要归纳了两种表达方式的转换,计算公式采用3D笛卡尔坐标系:
定义,,分别为绕Z轴、Y轴、X轴的旋转角度,如果用Tait-Bryan angle表示,分别为Yaw、Pitch、Roll。
一、四元数的定义
- 通过旋转轴和绕该轴旋转的角度可以构造一个四元数:
- 其中α是一个简单的旋转角(旋转角的弧度值),而是定位旋转轴的“方向余弦”(欧拉旋转定理)。
利用欧拉角也可以实现一个物体在空间的旋转,它按照既定的顺序,如依次绕z,y,x分别旋转一个固定角度,使用yaw,pitch,roll分别表示物体绕,x,y,z的旋转角度,记为,,,可以利用三个四元数依次表示这三次旋转,即:
二、欧拉角到四元数的转换
2.1 公式:
2.2 code:
struct Quaternion
{double w, x, y, z;
};Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{// Abbreviations for the various angular functionsdouble cy = cos(yaw * 0.5);double sy = sin(yaw * 0.5);double cp = cos(pitch * 0.5);double sp = sin(pitch * 0.5);double cr = cos(roll * 0.5);double sr = sin(roll * 0.5);Quaternion q;q.w = cy * cp * cr + sy * sp * sr;q.x = cy * cp * sr - sy * sp * cr;q.y = sy * cp * sr + cy * sp * cr;q.z = sy * cp * cr - cy * sp * sr;return q;
}
三、四元数到欧拉角的转换
3.1 公式
可以从四元数通过以下关系式获得欧拉角:
- arctan和arcsin的结果是,这并不能覆盖所有朝向(对于角的取值范围已经满足),因此需要用atan2来代替arctan。
3.2 code:
#define _USE_MATH_DEFINES
#include <cmath>struct Quaternion {double w, x, y, z;
};struct EulerAngles {double roll, pitch, yaw;
};EulerAngles ToEulerAngles(Quaternion q) {EulerAngles angles;// roll (x-axis rotation)double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);angles.roll = std::atan2(sinr_cosp, cosr_cosp);// pitch (y-axis rotation)double sinp = 2 * (q.w * q.y - q.z * q.x);if (std::abs(sinp) >= 1)angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of rangeelseangles.pitch = std::asin(sinp);// yaw (z-axis rotation)double siny_cosp = 2 * (q.w * q.z + q.x * q.y);double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);angles.yaw = std::atan2(siny_cosp, cosy_cosp);return angles;
}
3.3 四元素到旋转矩阵转换
或等效地,通过齐次表达式:
四. 奇点
当螺距接近±90°(南北极)时,必须意识到欧拉角参数化的奇异性。这些情况必须特别处理。这种情况的通用名称是万向节锁。
处理奇异点的代码可从以下网站获取:www.euclideanspace.com
五. 矢量旋转
定义四元素的尺度 和向量 ,有
请注意,通过定义欧拉旋转的四元数来旋转三维矢量的规范方法是通过公式:
这儿:是包含嵌入向量的四元数,是共轭四元数,
在计算实现中,这需要两个四元数乘法。一种替代方法是应用一对关系:
:表示三维矢量叉积。这涉及较少的乘法,因此计算速度更快。数值测试表明,对于矢量旋转,后一种方法可能比原始方法快30%[4]。
证明:
标量和矢量部分的四元数乘法的一般规则由下式给出:
利用这种关系可以找到:
并替换为三乘积:
可得到:
在定义时,可以按标量和矢量部分来表示:
六 . 其他参考
- Rotation operator (vector space)
- Quaternions and spatial rotation
- Euler Angles
- Rotation matrix
- Rotation formalisms in three dimensions
七 python 转换
7.1 四元素欧拉角互相转换
def EulerAndQuaternionTransform( intput_data):"""四元素与欧拉角互换"""data_len = len(intput_data)angle_is_not_rad = Falseif data_len == 3:r = 0p = 0y = 0if angle_is_not_rad: # 180 ->pir = math.radians(intput_data[0]) p = math.radians(intput_data[1])y = math.radians(intput_data[2])else:r = intput_data[0] p = intput_data[1]y = intput_data[2]sinp = math.sin(p/2)siny = math.sin(y/2)sinr = math.sin(r/2)cosp = math.cos(p/2)cosy = math.cos(y/2)cosr = math.cos(r/2)w = cosr*cosp*cosy + sinr*sinp*sinyx = sinr*cosp*cosy - cosr*sinp*sinyy = cosr*sinp*cosy + sinr*cosp*sinyz = cosr*cosp*siny - sinr*sinp*cosyreturn [w,x,y,z]elif data_len == 4:w = intput_data[0] x = intput_data[1]y = intput_data[2]z = intput_data[3]r = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))p = math.asin(2 * (w * y - z * x))y = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))if angle_is_not_rad : # pi -> 180r = math.degrees(r)p = math.degrees(p)y = math.degrees(y)return [r,p,y]
7.2 旋转矩阵<->欧拉角 py
import numpy as np
import math
import randomdef isRotationMatrix(R) :Rt = np.transpose(R)shouldBeIdentity = np.dot(Rt, R)I = np.identity(3, dtype = R.dtype)n = np.linalg.norm(I - shouldBeIdentity)return n < 1e-6def rotationMatrixToEulerAngles(R) :assert(isRotationMatrix(R))sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])singular = sy < 1e-6if not singular :x = math.atan2(R[2,1] , R[2,2])y = math.atan2(-R[2,0], sy)z = math.atan2(R[1,0], R[0,0])else :x = math.atan2(-R[1,2], R[1,1])y = math.atan2(-R[2,0], sy)z = 0return np.array([x, y, z])def eulerAnglesToRotationMatrix(theta) :R_x = np.array([[1, 0, 0 ],[0, math.cos(theta[0]), -math.sin(theta[0]) ],[0, math.sin(theta[0]), math.cos(theta[0]) ]])R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],[0, 1, 0 ],[-math.sin(theta[1]), 0, math.cos(theta[1]) ]])R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],[math.sin(theta[2]), math.cos(theta[2]), 0],[0, 0, 1]])R = np.dot(R_z, np.dot( R_y, R_x ))return R
c++:
static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R){Eigen::Vector3d n = R.col(0);Eigen::Vector3d o = R.col(1);Eigen::Vector3d a = R.col(2);Eigen::Vector3d ypr(3);double y = atan2(n(1), n(0));double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));ypr(0) = y;ypr(1) = p;ypr(2) = r;return ypr / M_PI * 180.0;}template <typename Derived>static Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr2R(const Eigen::MatrixBase<Derived> &ypr){typedef typename Derived::Scalar Scalar_t;Scalar_t y = ypr(0) / 180.0 * M_PI;Scalar_t p = ypr(1) / 180.0 * M_PI;Scalar_t r = ypr(2) / 180.0 * M_PI;Eigen::Matrix<Scalar_t, 3, 3> Rz;Rz << cos(y), -sin(y), 0,sin(y), cos(y), 0,0, 0, 1;Eigen::Matrix<Scalar_t, 3, 3> Ry;Ry << cos(p), 0., sin(p),0., 1., 0.,-sin(p), 0., cos(p);Eigen::Matrix<Scalar_t, 3, 3> Rx;Rx << 1., 0., 0.,0., cos(r), -sin(r),0., sin(r), cos(r);return Rz * Ry * Rx;}
八 Eigen transform
8.1 欧拉角到四元素
Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch,const double yaw) {const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());return yaw_angle * pitch_angle * roll_angle;}
四元素得到yaw
template <typename T>T GetYaw(const Eigen::Quaternion<T>& rotation) {const Eigen::Matrix<T, 3, 1> direction =rotation * Eigen::Matrix<T, 3, 1>::UnitX();return atan2(direction.y(), direction.x());}
四元素到旋转向量
template <typename T>Eigen::Matrix<T, 3, 1> RotationQuaternionToAngleAxisVector(const Eigen::Quaternion<T>& quaternion) {Eigen::Quaternion<T> normalized_quaternion = quaternion.normalized();// We choose the quaternion with positive 'w', i.e., the one with a smaller// angle that represents this orientation.if (normalized_quaternion.w() < 0.) {// Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560normalized_quaternion.w() = -1. * normalized_quaternion.w();normalized_quaternion.x() = -1. * normalized_quaternion.x();normalized_quaternion.y() = -1. * normalized_quaternion.y();normalized_quaternion.z() = -1. * normalized_quaternion.z();}// We convert the normalized_quaternion into a vector along the rotation axis// with length of the rotation angle.const T angle =2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w());constexpr double kCutoffAngle = 1e-7; // We linearize below this angle.const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / 2.);return Eigen::Matrix<T, 3, 1>(scale * normalized_quaternion.x(),scale * normalized_quaternion.y(),scale * normalized_quaternion.z());}
旋转轴向量到四元素
template <typename T>Eigen::Quaternion<T> AngleAxisVectorToRotationQuaternion(const Eigen::Matrix<T, 3, 1>& angle_axis) {T scale = T(0.5);T w = T(1.);constexpr double kCutoffAngle = 1e-8; // We linearize below this angle.if (angle_axis.squaredNorm() > kCutoffAngle) {const T norm = angle_axis.norm();scale = sin(norm / 2.) / norm;w = cos(norm / 2.);}const Eigen::Matrix<T, 3, 1> quaternion_xyz = scale * angle_axis;return Eigen::Quaternion<T>(w, quaternion_xyz.x(), quaternion_xyz.y(),quaternion_xyz.z());}
Eigen 转换函数
九 旋转矩阵与欧拉角
按旋转坐标系分 内旋(旋转的轴是动态的) 和 外旋(旋转轴是固定的,是不会动的)。
绕定轴 XYZ旋转(RPY)(外旋)
- 假设两个坐标系A和B,二者初始时完全重合。 过程如下:B绕A的X轴旋转γ角,再绕A的Y轴旋转β角,最后绕A的Z轴旋转α角,完成旋转。整个过程,A不动B动。旋转矩阵的计算方法如下:R = Rz * Ry *Rx,乘法顺序:从右向左,依次旋转X轴Y轴Z轴 。
绕动轴ZYX旋转(Euler角)(内旋)
- 过程如下:B绕B的Z轴旋转α角,再绕B的Y轴旋转β角,最后绕B的X轴旋转γ角,完成旋转。整个过程,A不动B动。 旋转矩阵的计算方法如下:。乘法顺序:从左向右
欧拉角的表示方式比较直观,但是有几个缺点:
- (1) 欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向,即使给定yaw、pitch、roll的顺序,也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。比如,同样的yaw-pitch-roll顺序,(0,90,0)和(90,90,90)会将刚体转到相同的位置。这其实主要是由于万向锁(Gimbal Lock)引起的 (2) 欧拉角的插值比较难。 (3) 计算旋转变换时,一般需要转换成旋转矩阵,这时候需要计算很多sin, cos,计算量较大。
四元数与欧拉角(Yaw、Pitch、Roll)的转换相关推荐
- yaw公式_坐标系和yaw, pitch, roll等基础概念
坐标系 右手坐标系:把右手拇指食指中指伸直并正交,拇指X,食指Y,中指Z.一般无特殊说明,都是右手坐标系. 右手坐标系的旋转正方向:从轴的正方向看向原点,逆时针方向即是旋转正向.或者,伸出右手,拇指指 ...
- 机器人中的yaw/pitch/roll
1.yaw(pan)/pitch(tilt)/roll 我认为,yaw/pitch/roll绕哪个轴旋转,是要看坐标系的朝向的,如果坐标系的轴如下图,则对应关系是yaw(z轴).pitch(x轴).r ...
- Yaw Pitch Roll的变换顺序问题
今天花了不少时间去理解Yaw Pitch Roll的变换顺序 比如给定一个旋转(15, 30, 60),那么物体应该先围绕哪个轴转呢?总得有个顺序吧? 解决这个问题的第一个天坑就是:网上的资料坐标系都 ...
- ROS中四元数、欧拉角、旋转矩阵等格式转换
未完- ROS接收到odometry格式消息: nav_msgs::Odometry pos_msg 具有: pos_msg.pose.pose.orientation.x; // xyzw pos_ ...
- 姿态角(Euler角):yaw pitch roll
姿态角和自由度相关联: 姿态角在控制,机械领域广泛使用: 这里,简单了解一下: 什么是姿态角(Euler角)? yaw,pitch,roll的识别? 步骤阅读 百度经验:jingyan.baidu.c ...
- YAW PITCH ROLL 定义
百度上居然搜不到俯仰滚转和偏航的定义,可见大家的浮躁了,没事摇一摇都不干正事了,整理了一下资料,公布给大家. 要了解飞机姿态,需要首先知道什么是地面坐标系和机体坐标系. ■地面坐标系( eart ...
- 航空摄影测量中yaw,pitch,roll
在航空摄影测量中,pitch, yaw, roll含义如下图: pitch是围绕X轴旋转,也叫做俯仰角: yaw是围绕Y轴旋转,也叫偏航角: roll是围绕Z轴旋转,也叫翻滚角: yaw 旋转 ...
- eigen 编译_头条 | 使用eigen实现四元数、欧拉角、旋转矩阵、旋转向量间的转换...
点击上方蓝字,关注本公众号,获得更多资源上一篇文章介绍了四元数.欧拉角.旋转矩阵.轴角如何相互转换,本篇文章介绍如何用eigen来实现. 旋转向量 1,初始化旋转向量:旋转角为alpha,旋转轴为(x ...
- yaw pitch row欧拉角理解
飞行中的飞机可以在三维空间中自由旋转 --------------------------------------------------------------------------------- ...
- 四元数和欧拉角的个人理解
欧垃角可以表示两个坐标之间的转换,但是存在一些弊端,比如万象锁,四元数也可以表述 问题分析: 比如,一个坐标系绕y轴旋转pi/2,那么我们就可以用欧拉角roll,pitch,yaw和四元数x,y,z, ...
最新文章
- ajax实现自动刷新页面实例
- java出现no XXX in java.library.path的解决办法及eclipse配置
- Linux创建线程读取双口数据,linux环境下读写一次双口ram尽然要十几个毫秒。(附驱动代码)...
- 云原生五大趋势预测,K8s 安卓化位列其一
- 【无标题】RestHighLevelClient工具类
- 微信小程序rpx作为高度单位时,在 ios 出现异常
- python回归分析实验_python线性回归实验
- 康纳的表情包(思维)
- Python 3 学习——函数扩展and迭代器生成器装饰器
- 开源压缩算法brotli_Brotli:一种新的压缩算法,可加快互联网速度
- ModuleNotFoundError: No module named ‘torchversion‘
- 儿童讲堂 - 学科分类
- java中文转英文_eclipse英文转中文怎么设置 eclipse中英文切换图文教程
- SumatraPDF 高级设置
- unity3D灯光烘焙(二)
- 848. Shifting Letters**
- 快应用上架时,你最关心的问题都在这里!
- 51单片机——读写AT24c64
- 计算机视觉中的MAP的理解(mean average precision)
- 物联网大数据存储利器IoTDB介绍