旋转矩阵、欧拉角之间转换

842
0
2020年12月10日 09时10分

学习过程中涉及欧拉角和旋转矩阵的转换,索性整理学习一下欧拉角四元数和旋转矩阵的概念以及matlab中的互相转换
本文摘自各大课本,博客,自己学习整理使用,侵删
MATLAB矩阵乘法从左到右依次相乘
用R表示旋转矩阵。
yaw(偏航) pitch(俯仰) roll(横滚)分别表示Z Y X轴的转角。
q=[q0,q1,q2,q3]’表示单位四元数。

 

1旋转矩阵(方向余弦矩阵)

 

当确定一个点在空间中的位置后,要确定其姿态才能完全定义该点的位姿。所以采用坐标系{B}相对于坐标系{A}的描述来表示物体姿态,

 

1

 

通俗的讲,是坐标系{B}的各个轴分别与参考坐标系{A}的各个轴的余弦值构成的3×3矩阵,称为旋转矩阵。

 

2

 

欧拉角

 

1、内旋和外旋

 

3

 

4

 

5

 

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

 

一、绕定轴X-Y-Z旋转(RPY角)(外旋)

 

假设两个坐标系A和B,二者初始时完全重合。

 

过程如下:B绕A的X轴旋转γ角,再绕A的Y轴旋转β角,最后绕A的Z轴旋转α角,完成旋转。整个过程,A不动B动。

 

6

 

旋转矩阵的计算方法如下:R = Rz * Ry *Rx,乘法顺序:从右向左,依次旋转X轴Y轴Z轴

 

7

 

其中,cα = cosα,sα = sinα,矩阵相乘,结果如下:

 

8

 

二、绕动轴Z-Y-X旋转(Euler角)(内旋)

 

过程如下:B绕B的Z轴旋转α角,再绕B的Y轴旋转β角,最后绕B的X轴旋转γ角,完成旋转。整个过程,A不动B动。
旋转矩阵的计算方法如下:R = Rz * Ry *Rx。乘法顺序:从左向右

 

9

 

11

 

欧拉角的表示方式比较直观,但是有几个缺点:

 

(1) 欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向,即使给定yaw、pitch、roll的顺序,也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。比如,同样的yaw-pitch-roll顺序,(0,90,0)和(90,90,90)会将刚体转到相同的位置。这其实主要是由于万向锁(Gimbal Lock)引起的

 

(2) 欧拉角的插值比较难。

 

(3) 计算旋转变换时,一般需要转换成旋转矩阵,这时候需要计算很多sin, cos,计算量较大。

 

欧拉角转旋转矩阵

 

在计算坐标变换时,旋转更方便的表示形式是旋转矩阵(Rotation Matrix)。三维空间的旋转矩阵可以表示成3×3的矩阵,将欧拉角转换为旋转矩阵的计算方式如下,假设欧拉角yaw、pitch、roll的角度为alpha, beta, gamma,则旋转矩阵可以计算如下:

 

12

 

eul2rotm(eul)中,默认旋转顺序是ZYX,而矩阵相乘顺序是从右到左,先X再Y后Z,
即:若eul(Z Y X)=eul(angle1 angle2 angle3),则eul2rotm(eul,sequence)=Rot(Z,angle1)*Rot(Y,angle2)*Rot(X,angle3).

 

14

 

rotm2eul()
计算单位为弧度制,计算结果的顺序对应欧拉角旋转轴顺序

 

deg2rad()%MATLAB角度转弧度函数
%    Z   Y  X
eul=[0 pi/2 0];
yaw=0;
pitch=pi/2'
roll=0;
 R_x=[1 0 0;
     0 cos(roll) -sin(roll);
     0 sin(roll) cos(roll)];
 R_y=[cos(pitch) 0 sin(pitch);
     0 1 0;
     -sin(pitch) 0 cos(pitch)];
 R_z=[cos(yaw) -sin(yaw) 0;
     sin(yaw) cos(yaw) 0;
     0 0 1];
R=R_z*R_y*R_x;%欧拉角转旋转矩阵
R2=angle2dcm(eul,'ZYX');%由欧拉角转方向余弦矩阵
R2=eul2rotm(eul,'ZYX');%欧拉角转旋转矩阵

 

四元数(后续补充)

 

一篇介绍四元数的文章
在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

 

发表评论

后才能评论