旋转矩阵与欧拉角之间的转换

旋转矩阵与欧拉角之间的转换_第1张图片

简 介: 对于欧拉角与旋转矩阵之间的转换公式和程序实现进行了测试。也显示了这其中的转换关系的复杂性,来自于欧拉角的方向、范围、转换顺序。这在实际应用中需要特别的关注。

关键词 欧拉角旋转矩阵

欧拉角
目 录
Contents
基本概念
旋转顺序
旋转矩阵
由欧拉角到旋转矩阵
任意轴旋转
相互转换
转换公式
转换代码
对比代码
总 结
  • 欧拉角和旋转矩阵之间的转换

  在机器人视觉应用中,经常会遇到旋转矩阵、旋转向量、四元素、欧拉角之间的相互转换。其中最容易出错的是旋转矩阵与欧拉角之间的相互转换。

旋转矩阵与欧拉角之间的转换_第2张图片

▲ 图1 坐标系旋转变换

 

§01 拉角


1.1 基本概念

1.1.1 欧拉角的名称

  • 欧拉角的叫法不固定,跟坐标轴的定义强相关。
  • 在图1中,假设X是车头,Y是车左方,Z是车上方,那么绕X轴旋转得到的是roll,绕Y轴旋转得到的是pitch,绕Z轴得到的是yaw。
  • 在图1中,假设Y是车头,X是车右方,Z是车上方,那么绕X轴旋转得到的是pitch,绕Y轴旋转得到的是roll,绕Z轴得到的是yaw。

1.1.2 欧拉角方向

  • 如果是右手系,旋转轴正方向面对观察者时,逆时针方向的旋转是正、顺时针方向的旋转是负。
  • 亦可这样描述:使用右手的大拇指指向旋转轴正方向,其他4个手指在握拳过程中的指向便是正方向。
  • 如图1中的三次旋转都是正向旋转。

旋转矩阵与欧拉角之间的转换_第3张图片

▲ 图1.1.1 三次旋转的过程

1.1.3 欧拉角范围

  • 这个要具体问题具体对待。
  • 假如是车体坐标系(x-前,y-左,z-上),那么rollpitch应该定义在(-90°,+90°),yaw应该定义在(-180°,+180°)。
  • 假如是飞机坐标系,那么rollpitchyaw都应该定义在(-180°,+180°)。
  • Eigen中的默认范围rollpitchyaw都是(-180°,+180°)。

1.2 旋转顺序

1.2.1 旋转顺序和旋转轴

  • 对于x,y,z三个轴的不同旋转顺序一共有(x-y-z,y-z-x,z-x-y,x-z-y,z-y-x,y-x-z)六种组合,在旋转相同的角度的情况下不同的旋转顺序得到的姿态是不一样的。
  • 比如,先绕x轴旋转alpha,再绕y轴旋转beta;先绕y轴旋转beta,再绕x轴旋转alpha。这两种顺序得到的姿态是不一样的。### 内旋和外旋

1.2.2 内旋和外旋

  • 每次旋转是绕固定轴(一个固定参考系,比如世界坐标系)旋转,称为外旋。
  • 每次旋转是绕自身旋转之后的轴旋转,称为内旋。
  • 下图说明了内旋和外旋的区别。

旋转矩阵与欧拉角之间的转换_第4张图片

▲ 图1.2.1 内在旋转

旋转矩阵与欧拉角之间的转换_第5张图片

▲ 图1.2.2 外在旋转

  按照内旋方式,Z-Y-X旋转顺序(指先绕自身轴Z,再绕自身轴Y,最后绕自身轴X),可得旋转矩阵(内旋是右乘)

R 1 = R z ( γ ) ⋅ R y ( β ) ⋅ R x ( α ) R_1 = R_z \left( \gamma \right) \cdot R_y \left( \beta \right) \cdot R_x \left( \alpha \right) R1=Rz(γ)Ry(β)Rx(α)

  按照外旋方式,X-Y-Z旋转顺序(指先绕固定轴X,再绕固定轴Y,最后绕固定轴Z),可得旋转矩阵(外旋是左乘):

R 2 = R z ( γ ) ⋅ R y ( β ) ⋅ R x ( α ) R_2 = R_z \left( \gamma \right) \cdot R_y \left( \beta \right) \cdot R_x \left( \alpha \right) R2=Rz(γ)Ry(β)Rx(α)

  故R1=R2,具体不在此证明,记住即可。这个结论说明ZYX顺序的内旋等价于XYZ顺序的外旋。

 

§02 转矩阵


2.1 由欧拉角到旋转矩阵

  假设绕XYZ三个轴旋转的角度分别为 α , β , γ \alpha ,\beta ,\gamma α,β,γ,则三次旋转的旋转矩阵计算方法如下:

▲ 图2.1 三个旋转矩阵

▲ 图2.1 三个旋转矩阵

R x ( θ ) = [ 1 0 0 0 cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ ] R_x \left( \theta \right) = \begin{bmatrix} \begin{matrix} 1 & 0 & 0\\0 & {\cos \theta } & { - \sin \theta }\\0 & {\sin \theta } & {\cos \theta }\\\end{matrix} \end{bmatrix} Rx(θ)=1000cosθsinθ0sinθcosθ

R y ( θ ) = [ cos ⁡ θ 0 sin ⁡ θ 0 1 0 − sin ⁡ θ 0 cos ⁡ θ ] R_y \left( \theta \right) = \begin{bmatrix} \begin{matrix} {\cos \theta } & 0 & {\sin \theta }\\0 & 1 & 0\\{ - \sin \theta } & 0 & {\cos \theta }\\\end{matrix} \end{bmatrix} Ry(θ)=cosθ0sinθ010sinθ0cosθ

R z ( θ ) = [ cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ 0 0 0 1 ] R_z \left( \theta \right) = \begin{bmatrix} \begin{matrix} {\cos \theta } & { - \sin \theta } & 0\\{\sin \theta } & {\cos \theta } & 0\\0 & 0 & 1\\\end{matrix} \end{bmatrix} Rz(θ)=cosθsinθ0sinθcosθ0001

2.2 任意轴旋转

  如果沿着一个起始于原点,长度为1的向量 U = ( U x , U y , U z ) U = \left( {U_x ,U_y ,U_z } \right) U=(Ux,Uy,Uz),旋转 θ \theta θ角度,那么对应的旋转矩阵为:
R U ( θ ) = I + ( sin ⁡ θ ) ⋅ S + ( 1 − cos ⁡ θ ) ⋅ S 2 R_U \left( \theta \right) = I + \left( {\sin \theta } \right) \cdot S + \left( {1 - \cos \theta } \right) \cdot S^2 RU(θ)=I+(sinθ)S+(1cosθ)S2

  其中: I I I是单位矩阵;

S = [ 0 − U z U y U z 0 − U x − U y U x 0 ] S = \begin{bmatrix} \begin{matrix} 0 & { - U_z } & {U_y }\\{U_z } & 0 & { - U_x }\\{ - U_y } & {U_x } & 0\\\end{matrix} \end{bmatrix} S=0UzUyUz0UxUyUx0

旋转矩阵与欧拉角之间的转换_第6张图片

▲ 图2.2 旋转角与旋转矩阵

 

§03 互转换


3.1 转换公式

旋转矩阵与欧拉角之间的转换_第7张图片

▲ 图3.1.1 欧拉角与转换过程

3.1.1 从欧拉角到旋转矩阵

  根据 Euler Angle Formulas 给出了不同参数下测转换矩阵。

旋转矩阵与欧拉角之间的转换_第8张图片

▲ 图3.2.1 从欧拉角到旋转矩阵

旋转矩阵与欧拉角之间的转换_第9张图片

▲ 图3.2.2 欧拉角

3.2 转换代码

3.2.1 Python代码

  Rotation matrix to Euler angles Python code example

(1)旋转拒转→欧拉角
def 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-6

def rotationMatrixToEulerAngles(R) :
    assert(isRotationMatrix(R))
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    singular = sy < 1e-6
    if  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 = 0
    return np.array([x, y, z])
(2)欧拉角→旋转矩阵
def euler_to_rotVec(yaw, pitch, roll):
    Rmat = euler_to_rotMat(yaw, pitch, roll)

    theta = math.acos(((Rmat[0, 0] + Rmat[1, 1] + Rmat[2, 2]) - 1) / 2)
    sin_theta = math.sin(theta)
    if sin_theta == 0:
        rx, ry, rz = 0.0, 0.0, 0.0
    else:
        multi = 1 / (2 * math.sin(theta))
        rx = multi * (Rmat[2, 1] - Rmat[1, 2]) * theta
        ry = multi * (Rmat[0, 2] - Rmat[2, 0]) * theta
        rz = multi * (Rmat[1, 0] - Rmat[0, 1]) * theta
    return rx, ry, rz

def euler_to_rotMat(yaw, pitch, roll):
    Rz_yaw = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw),  np.cos(yaw), 0],
        [          0,            0, 1]])
    Ry_pitch = np.array([
        [ np.cos(pitch), 0, np.sin(pitch)],
        [             0, 1,             0],
        [-np.sin(pitch), 0, np.cos(pitch)]])
    Rx_roll = np.array([
        [1,            0,             0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll),  np.cos(roll)]])
    rotMat = np.dot(Rz_yaw, np.dot(Ry_pitch, Rx_roll))
    return rotMat

  通过测试可以知道:

  • yaw: z 轴
  • pitch: y 轴
  • roll: x 轴

3.2.2 SciPy

  scipy.spatial.transform.Rotation.from_euler

import sys,os,math,time
import matplotlib.pyplot as plt
from numpy import *
from scipy.spatial.transform import Rotation as R

r = R.from_euler('x', 90, degrees=True)
print("r.as_quat(): {}".format(r.as_quat()))
r.as_quat(): [0.70710678 0.         0.         0.70710678]

  R产生的对象包含有以下对象:

'apply',
'as_dcm',
'as_euler',
'as_quat',
'as_rotvec',
'from_dcm',
'from_euler',
'from_quat',
'from_rotvec',
'inv',
'match_vectors',

3.3 对比代码

x = 30
y = 45
z = 60

r = R.from_euler('zyx', [-z, -y, -x], degrees=True)
print("r.as_dcm():\n{}".format(r.as_dcm()))

rm = euler_to_rotMat(z*pi/180, y*pi/180, x*pi/180)
print("rm.T:\n{}".format(rm.T))
r.as_dcm():
[[ 0.35355339  0.61237244 -0.70710678]
 [-0.5732233   0.73919892  0.35355339]
 [ 0.73919892  0.28033009  0.61237244]]
rm.T:
[[ 0.35355339  0.61237244 -0.70710678]
 [-0.5732233   0.73919892  0.35355339]
 [ 0.73919892  0.28033009  0.61237244]]

  可以看到, 在上述;定义中,关于方向的角度正负定义,在两种函数中时有区别的。输出的矩阵也呈现转置的情况。

rm = euler_to_rotMat(z*pi/180, y*pi/180, x*pi/180)
print("rm.T:\n{}".format(rm.T))

r = R.from_dcm(rm.T)
print("r.as_euler('zyx')*180/pi: {}".format(r.as_euler('zyx')*180/pi))
rm.T:
[[ 0.35355339  0.61237244 -0.70710678]
 [-0.5732233   0.73919892  0.35355339]
 [ 0.73919892  0.28033009  0.61237244]]
r.as_euler('zyx')*180/pi: [-60. -45. -30.]

 

  结 ※


  于欧拉角与旋转矩阵之间的转换公式和程序实现进行了测试。也显示了这其中的转换关系的复杂性,来自于欧拉角的方向、范围、转换顺序。这在实际应用中需要特别的关注。

#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST4.PY                     -- by Dr. ZhuoQing 2021-12-31
#
# Note:
#============================================================

from headm import *                 # =
from scipy.spatial.transform import Rotation as R
import numpy as np

#------------------------------------------------------------
def euler_to_rotVec(yaw, pitch, roll):
    Rmat = euler_to_rotMat(yaw, pitch, roll)

    theta = math.acos(((Rmat[0, 0] + Rmat[1, 1] + Rmat[2, 2]) - 1) / 2)
    sin_theta = math.sin(theta)
    if sin_theta == 0:
        rx, ry, rz = 0.0, 0.0, 0.0
    else:
        multi = 1 / (2 * math.sin(theta))
        rx = multi * (Rmat[2, 1] - Rmat[1, 2]) * theta
        ry = multi * (Rmat[0, 2] - Rmat[2, 0]) * theta
        rz = multi * (Rmat[1, 0] - Rmat[0, 1]) * theta
    return rx, ry, rz

def euler_to_rotMat(yaw, pitch, roll):
    Rz_yaw = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw),  np.cos(yaw), 0],
        [          0,            0, 1]])
    Ry_pitch = np.array([
        [ np.cos(pitch), 0, np.sin(pitch)],
        [             0, 1,             0],
        [-np.sin(pitch), 0, np.cos(pitch)]])
    Rx_roll = np.array([
        [1,            0,             0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll),  np.cos(roll)]])
    rotMat = np.dot(Rz_yaw, np.dot(Ry_pitch, Rx_roll))
    return rotMat

#------------------------------------------------------------
x = 30
y = 45
z = 60

r = R.from_euler('zyx', [-z, -y, -x], degrees=True)
printt(r.as_dcm()\)

rm = euler_to_rotMat(z*pi/180, y*pi/180, x*pi/180)
printt(rm.T\)

#------------------------------------------------------------
rm = euler_to_rotMat(z*pi/180, y*pi/180, x*pi/180)
printt(rm.T\)

r = R.from_dcm(rm.T)
printt(r.as_euler('zyx')*180/pi:)






#------------------------------------------------------------
#        END OF FILE : TEST4.PY
#============================================================


■ 相关文献链接:

  • 欧拉角和旋转矩阵之间的转换
  • Euler Angle Formulas
  • Rotation matrix to Euler angles Python code example
  • scipy.spatial.transform.Rotation.from_euler

● 相关图表链接:

  • 图1 坐标系旋转变换
  • 图1.1.1 三次旋转的过程
  • 图1.2.1 内在旋转
  • 图1.2.2 外在旋转
  • 图2.1 三个旋转矩阵
  • 图2.2 旋转角与旋转矩阵
  • 图3.1.1 欧拉角与转换过程
  • 图3.2.1 从欧拉角到旋转矩阵
  • 图3.2.2 欧拉角

你可能感兴趣的:(教学-交流-科普,科学-技术-探究,矩阵,线性代数,计算机视觉)