旋转矩阵、欧拉角之间转换 |
您所在的位置:网站首页 › 欧拉角旋转矩阵公式 › 旋转矩阵、欧拉角之间转换 |
学习过程中涉及欧拉角和旋转矩阵的转换,索性整理学习一下欧拉角四元数和旋转矩阵的概念以及matlab中的互相转换 本文摘自各大课本,博客,自己学习整理使用,侵删 MATLAB矩阵乘法从左到右依次相乘 用R表示旋转矩阵。 yaw(偏航) pitch(俯仰) roll(横滚)分别表示Z Y X轴的转角。 q=[q0,q1,q2,q3]'表示单位四元数。 1旋转矩阵(方向余弦矩阵)当确定一个点在空间中的位置后,要确定其姿态才能完全定义该点的位姿。所以采用坐标系{B}相对于坐标系{A}的描述来表示物体姿态,
假设两个坐标系A和B,二者初始时完全重合。 过程如下:B绕A的X轴旋转γ角,再绕A的Y轴旋转β角,最后绕A的Z轴旋转α角,完成旋转。整个过程,A不动B动。 过程如下:B绕B的Z轴旋转α角,再绕B的Y轴旋转β角,最后绕B的X轴旋转γ角,完成旋转。整个过程,A不动B动。 旋转矩阵的计算方法如下:R = Rz * Ry *Rx。乘法顺序:从左向右 (1) 欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向,即使给定yaw、pitch、roll的顺序,也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。比如,同样的yaw-pitch-roll顺序,(0,90,0)和(90,90,90)会将刚体转到相同的位置。这其实主要是由于万向锁(Gimbal Lock)引起的 (2) 欧拉角的插值比较难。 (3) 计算旋转变换时,一般需要转换成旋转矩阵,这时候需要计算很多sin, cos,计算量较大。 欧拉角转旋转矩阵在计算坐标变换时,旋转更方便的表示形式是旋转矩阵(Rotation Matrix)。三维空间的旋转矩阵可以表示成3x3的矩阵,将欧拉角转换为旋转矩阵的计算方式如下,假设欧拉角yaw、pitch、roll的角度为alpha, beta, gamma,则旋转矩阵可以计算如下: 一篇介绍四元数的文章 在Matlab里,可以用quatmultiply计算四元数乘法,用quatinv来计算四元数的逆,用quatconj来计算四元数的共轭。四元数的旋转和旋转矩阵的旋转可以由以下matlab代码验证: % Matlab code by MulinB, Aerospace Toolbox is needed pt = [10,20,30]; % point coordinate yaw = 45; pitch = 30; roll = 60; q = angle2quat(yaw/180*pi,pitch/180*pi,roll/180*pi); R = angle2dcm(yaw/180*pi,pitch/180*pi,roll/180*pi); pt1 = R*pt'; pt2 = quatmultiply(quatconj(q), quatmultiply([0,pt],q)); % NOTE the order disp(pt1');disp(pt2(2:4));从上述代码里也可以看到四元数和欧拉角和dcm的转换,在matlab里可以很方便的用quat, dcm, angle之间的转换来任意互转。另外,从四元数计算axis和angle,可以用以下代码计算: % Matlab code by MulinB, Compute the axis and angle from a quaternion function [axis, theta] = quat2axisangle(q) theta = acos(q(1)) * 2; axis = q(2:4)/sin(theta/2); 总结: 转欧拉角:旋转矩阵转欧拉角 [r2,r2,r3]=dcm2angle(R, S) eul=rotm2rul(R,S) 注:得到的结果为弧度,若需要角度需进一步转化 四元数转欧拉角 [r1,r2,r3]=quat2angle([q0 q1 q2 q3],S) 注:S 的选择有12种,【‘ZYX’,‘ZYZ’,‘ZXY’,‘ZXZ’,‘YXZ’,‘YXY’,‘YZX’,‘YZY’,‘XYZ’,‘XYX’,‘XZY’,‘XZX’】 S 默认 ‘ZYX’ 转旋转矩阵四元数转旋转矩阵 R=quat2dcm([q0 q1 q2 q3]) 欧拉角转旋转矩阵 R=angle2dcm(r1,r2,r3,S); R=angle2dcm(yaw/180pi,pitch/180pi,roll/180*pi) 注:根据欧拉角是弧度/角度,选择以上操作 转四元数旋转矩阵转四元数 q =dcm2quat®; 欧拉角转四元数 q=angle2quat(r1,r2,r3,S); clear all; close all; clc; %欧拉角 x = 0.5; y = 0.6; z = 0.7; Ang1 = [x y z]; %欧拉角转旋转矩阵 Rx = [1 0 0; cos(x) -sin(x); sin(x) cos(x)]; Ry = [cos(y) 0 sin(y); 1 0; -sin(y) 0 cos(y)]; Rz = [cos(z) -sin(z) 0; sin(z) cos(z) 0; 0 1]; R = Rz*Ry*Rx; R1 = R; %旋转矩阵转欧拉角 x = atan2(R(3,2),R(3,3)); y = atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2)); z = atan2(R(2,1),R(1,1)); Ang2 = [x y z]; %旋转矩阵转四元数 t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2; q=[t (R(3,2)-R(2,3))/(4*t) (R(1,3)-R(3,1))/(4*t) (R(2,1)-R(1,2))/(4*t)]; Q1 = q; %四元数转旋转矩阵 R=[ 2*q(1).^2-1+2*q(2)^2 2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3)); 2*(q(2)*q(3)+q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2 2*(q(3)*q(4)-q(1)*q(2)); 2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2]; R2 = R; %欧拉角转四元数 q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ... sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ... cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ... cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)]; Q2 = q; %四元数转欧拉角 x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2)); y = asin(2*(q(1)*q(3) - q(2)*q(4))); z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2)); Ang3 = [x y z]; Ang1 Ang2 Ang3 R1 R2 Q1 Q2 |
今日新闻 |
推荐新闻 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |