unity 欧拉角求反 欧拉角不同旋转顺序

此文章旨在讲清楚欧拉角使用中的细节问题,让大家能够以专业的方式表达和交流欧拉角.

1欧拉角简介

欧拉角是由Leonhard Euler 提出的概念,用来描述刚体/移动坐标系在一个固定坐标系中的姿态.简单的说是使用XYZ三个轴的旋转分量,来描述一个6自由度的旋转.

欧拉角一般具有两大类表示方式,每类按照旋转次序的不同分为6小类:

Proper Euler angles (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)

Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z).

每个大类都使用了3个变量描述三次旋转过程中的旋转角度, 差别在于Proper Euler angles只涉及两个转轴.而Tait–Bryan angles涉及三个转轴.一般在SLAM中我们使用的是Tait–Bryan angles.

Tait–Bryan angles 也被称为Cardan angles, nautical angles, (heading, elevation, and bank),(yaw, pitch, and roll). 我们接触的比较多的是yaw(偏航), pitch(俯仰), roll(横滚).三个变量一般对应(车体,飞行器的)z,y,x三个坐标轴.

2欧拉角描述旋转

2.1一些欧拉角特性

一般对于旋转矩阵(3*3),旋转向量/角轴(3*1),四元数(4*1),我们给定一串数字,就能表示清楚一个姿态/旋转.比如这里给出一个旋转矩阵R:

表示刚体在A某坐标系下的姿态, 我们就可以确切的画出刚体A的姿态.

但如果我给出一组欧拉角(后面都是指Tait–Bryan angles),绕x,y,z三个轴的转角分别为(α,β,γ),我们不能能确定一个明确的姿态.需要再追加两个属性:(1)旋转顺序(2)内旋/外旋.才能确定的给出这组欧拉角对应的姿态.

2.2关于旋转顺序

旋转顺序就是我们上文提到的Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z).

我们指定绕x轴旋转α,绕y轴旋转β.但是可以有多个旋转顺序, 比如:

情况1:先绕x轴旋转α,再绕y轴旋转β.得到姿态

 情况2:先绕y轴旋转β,再绕x轴旋转α,得到姿态

 得到的

 一般是不等于

 的.

对于x,y,z三个轴的不同旋转顺序一共有(x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)六种组合.我们需要明确旋转顺序,才能确定欧拉角所指的姿态.

2.3关于内旋/外旋

首先列出几种等价的概念:

内旋(intrinsic rotations) = 旋转轴(rotated axis)

外旋(extrinsic rotations) = 固定轴(static/fixed axis)

我们后面直接用内旋/外旋来描述.

内旋/外旋的定义:

假设在世界坐标系中XYZ中存在物体,物体自身坐标系为xyz,假设初始状态物体相对XYZ的旋转为(0,0,0),即xyz与XYZ重合.我们定义旋转顺序为z->y->x,转角分别为γ,β,α.

先绕z轴旋转γ,旋转过后,物体的x,y轴的坐标系发生了改变,z轴不变,得到新的物体自身坐标系

,此时的坐标轴

,

不再与世界坐标系的坐标轴XY重合.

内旋,外旋的区别在于:

在转β(第二个转角)时:

内旋按照旋转后物体的坐标y轴,也就是

旋转.外旋按照世界坐标系中的Y轴旋转.

旋转最后一个角度时亦然.

因此, 增加了这两个概念(旋转顺序, 内外旋)后,我们描述一个能表示确定姿态/旋转的欧拉角,应该这样:

旋转角度(α,β,γ),旋转顺序(z->y->x),外旋.

或者:

旋转角度(α,β,γ),旋转顺序(x->y->z),内旋.

等等, 三个元素缺一不可.

旋转矩阵的表述方式:

上述的表述方式是否太罗嗦? 其实一般我们用欧拉角是为了方便主观理解,最终我们一般还是要转换为旋转矩阵,或者四元数来参与计算.

所以我们不如直接提供欧拉角与旋转矩阵的换算方式,一举两得.

比如上述: 旋转角度(α,β,γ),旋转顺序(z->y->x),外旋.

可以写为:

(公式a)

其中:

这个公式(a)根据旋转矩阵构成的定义即可得到.

而上述(α,β,γ),旋转顺序(x->y->z),内旋.

可以写为:

(公式b)

这个公式的推导稍微复杂些,这里只给出定义,推导过程不在本文的探讨范围内.

可以发现, 公式(a)与公式(b)是一样的.也就是说x->y->z内旋等价于z->y->x,外旋.

事实上,每种特定顺序的外旋等价于其相反顺序的内旋.

2.4 总结及示例代码

至此,我们得出提供一组能够精确描述旋转的欧拉角, 可以如下描述:

方法1:

旋转角度(α,β,γ),旋转顺序(x->y->z),内旋.

方法2:

见(公式b)

以下所有代码表示的都是:

1. Python tf lib:

from tf import transformations
import math
T = transformations.euler_matrix(x, y, z, "sxyz")

(2)python numpy:

import numpy as np
Rz = np.mat([[c1,-s1, 0],
[s1, c1, 0],
[0,0,1]])

Ry = np.mat([[c2, 0, s2],
[0, 1, 0],
[-s2, 0, c2]])

Rx = np.mat([[1,0, 0],
[0, c3, -s3],
[0,s3,c3]])

print(np.dot(Rz ,np.dot(Ry ,Rx)))

(3)c++ eigen

#include <Eigen/Core>
#include <Eigen/Geometry>

Eigen::Matrix3d R = Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitZ()) *
                    Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) * 
                    Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitX());

3欧拉角与旋转矩阵/四元数的转换

3.1欧拉角转旋转矩阵

见2.3

3.2旋转矩阵转欧拉角

参考wikipedia: https://en.wikipedia.org/wiki/Euler_angles

借用其中的这张表:

这张表描述了所有旋转的组合.以最后一行为例,我们可以把这里的

认为是Z->X->Y内旋 or Y->X->Z外旋.

从矩阵的角度就是Z*X*Y.

这里的1,2,3可以认为是各个转轴上的旋转角度.后面的c1表示cos(绕Z轴转角).s1表示sin(绕Z轴转角).

旋转矩阵转欧拉角可以结合这张表中的公式. 比如对一个给定的旋转矩阵R.我们需要把R分解为Z*X*Y形式的欧拉角.我们就找到最后一行的

.我们记Z,X,Y对应的角度分别为yaw,roll,pitch.而

sin(yaw) = s1, cos(yaw) = c1.

sin(roll) = s2, cos(roll) = c2.

sin(pitch) = s3, cos(pitch) = c3.

我们的计算方式是寻找每个角度对应的tan值.

我们看到R[0][1]/R[1][1] = (-c2s1)/(c1c2) = -s1/c1 = -tan1.

则yaw = -atan2(R[0][1]/R[1][1]).

我们知道了yaw, 就能计算出c1, s1, 代入R[1][1] = c1c2可以求出c2.

c2结合R[2][1]=s2可计算出tan2, 得到roll.

同样的方法,得到tan3, 计算出pitch.

至此可得yaw,roll,pitch.

一般情况下可以如此计算,不过在万向节思索状态下似乎会出现分母为0的情况.

3.3欧拉角转四元数

可以借用旋转矩阵:欧拉角->旋转矩阵->四元数

当然, 也有直接转换的方法.

3.4四元数转欧拉角

可以借用旋转矩阵:四元数->旋转矩阵->欧拉角

当然, 也有直接转换的方法.

3.5基于python tf库的转换代码

from tf import transformations
import math
import numpy a3s np

def euler_to_matrix_rad(x, y, z):
    T = transformations.euler_matrix(x, y, z, "sxyz")
    return T

def matrix_to_euler_rad(matrix):
    q = transformations.quaternion_from_matrix(matrix)
    eulers = transformations.euler_from_quaternion(q, axes='sxyz')
    return eulers

def matrix_to_quaternion(matrix):
    return transformations.quaternion_from_matrix(matrix)

#四元数是ijk3 也就是xyz的顺序
def quaternion_to_matrix(quat):
    return transformations.quaternion_matrix(quat)

def quaternion_to_euler_rad(quat):
    return transformations.euler_from_quaternion(quat, axes='sxyz')

def euler_to_quaternion_rad(x, y, z):
    return transformations.quaternion_from_euler(x, y, z, axes='sxyz')

def rad_to_degree(rad):
    return rad / math.pi * 180

def degree_to_euler(degree):
    return degree / 180 * math.pi

def inverse_matrix(matrix):
    return transformations.inverse_matrix(matrix)

#注意简单的a*b是按对应位置元素相乘
def dot_matrix(a, b):
  return np.dot(a, b)

对于库卡机器人，根据手册《库卡编程详解》如下所述，为Rabg

按照旋转的坐标系分为两种旋转方式：

内旋-intrinsic rotation（动态）:相对变换后的（当前的，自身的）坐标系做变换，以z-y′-z′′表示。′上标表达的是该旋转是以上一次旋转为参考。
外旋-extrinsic rotation（静态）:相对初始的（固定的）坐标系做变换，以z-x-z表示。

不考虑内旋与外旋时，经典欧拉角和泰特布莱恩角各有六种绕轴旋转方式;如果考虑内旋与外旋，则各有12种绕轴旋转方式。

机器人欧拉角

史陶比（Staubli）使用XY'Z''
爱得普（Adept）使用ZY'Z''
库卡（KUKA）使用ZY'X''
发那科（Fanuc）与安川（Motoman）则使用XYZ
ABB机器人使用四元数表达旋转
优傲（UR）机器人使用方向矢量