欧拉角因为其奇异性,虽然在优化和插值的不会使用,但是当我们对别人描述一个旋转的过程是怎么样的时候,欧拉角还是很有用的,比如,做无人机姿态控制的时候使用的就是欧拉角,但是搞明白欧拉角与旋转矩阵的转换确实是一件头疼的事,所以就写下了这篇总结,希望对大家理解欧拉角有所帮助
文章目录
需要区分每次旋转是绕固定轴旋转的,还是绕旋转之后的轴旋转的,如果不特殊指明,下面的讨论都是指:绕旋转之后的轴旋转的。不要局限于表示旋转的旋转顺序是什么样的,使用ZYX
(先绕Z
再绕Y
最后绕X
)仅仅是因为习惯而已。仅仅绕单个轴旋转一个角度得到的也是旋转矩阵!
- 基础旋转矩阵:绕某单一坐标轴进行旋转对应的矩阵
- 组合旋转矩阵:多次旋转组合对应的矩阵
1、欧拉角的定义
定义一个欧拉角,需要明确下面5条:
- 三个旋转角的组合方式
- 旋转角度的参考坐标系统(旋转是相对于固定的坐标系还是相对于自身的坐标系)
- 使用旋转角度是左手系还是右手系
- 三个旋转角的记法
- 主动旋转还是被动旋转
1.1 表示旋转的欧拉角旋转顺序有12种
- Proper/classic Euler angle
z-x-z
,x-y-x
,y-z-y
,z-y-z
,x-z-x
,y-x-y
- Tait-Bryan angle(也称作:Cardan angles; nautical angles; heading、elevation、bank; yaw、pitch、rooll)
x-y-z
,y-z-x
,z-x-y
,x-z-y
,z-y-x
,y-x-z
Proper/classic Euler angle说明这些角度并不是独立的,例如当下面的旋转组合:先绕x
轴旋转90
度,再绕y
轴旋转90
度,最后绕x
轴旋转-90
度,这一些列组合得到的效果与只绕z
轴旋转-90
度是一样的。也就是说我们仅仅在2
个平面上进行旋转(其中一个平面上必须进行两次旋转)就可以得到任意的三维旋转!
1.2 内在旋转(intrinsic rotations)和外在旋转(extrinsic rotations)
内在旋转每次旋转围绕的轴是上次旋转之后坐标系的某个轴,外在旋转每次旋转的轴是固定坐标系中的轴。内在旋转与外在旋转的转换关系:互换第一次和第三次旋转的位置则两者结果相同。例如Z-Y-X
旋转的内部旋转和X-Y-Z
旋转的外部旋转的旋转矩阵相同
1.3 使用旋转角度是左手系还是右手系
使用右手的大拇指指向旋转轴,其他4个手指在握拳过程中的指向便是正的角度
- 右手系是逆时针
- 左手系是顺时针
1.4 主动旋转和被动旋转
主动旋转是指将向量逆时针围绕旋转轴旋转,被动旋转是对坐标轴进行的逆时针旋转,相当于主动旋转的逆操作
2、不同轴的欧拉角转换成旋转矩阵
给出逆时针旋转的角度为正时(与右手系旋转方向相同的为旋转正方向),绕不同轴的旋转结果:
R x = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] , R y = [ cos ϕ 0 sin ϕ 0 1 0 − sin ϕ 0 cos ϕ ] , R z = [ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 ] R_x=\left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {-\sin \theta} \\ {0} & {\sin \theta} & {\cos \theta}\end{array}\right], R_y=\left[\begin{array}{ccc}{\cos \phi} & {0} & {\sin \phi} \\ {0} & {1} & {0} \\ {-\sin \phi} & {0} & {\cos \phi}\end{array}\right], R_z=\left[\begin{array}{ccc}{\cos \psi} & {-\sin \psi} & {0} \\ {\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1}\end{array}\right] Rx= 1000cosθsinθ0−sinθcosθ ,Ry= cosϕ0−sinϕ010sinϕ0cosϕ ,Rz= cosψsinψ0−sinψcosψ0001
假设有一个坐标系
b
b
b(其上有一个点
p
p
p(点和向量是空间中一样东西,只有当选取坐标系时才讨论它的的坐标),坐标也用
p
p
p 表示),它要绕X
轴逆时针转
α
α
α 角度,那么在旋转之后的坐标系下点
p
p
p 的坐标(记为,
p
′
p'
p′)变成了多少?做一下数学转换得到:
R
x
∗
p
′
=
p
(1)
R_x*p'=p \tag{1}
Rx∗p′=p(1)
转换一下得到,
p
′
=
R
x
T
∗
p
(2)
p'=R_x^T * p \tag{2}
p′=RxT∗p(2)
当从w
系依次进行ZYX
顺序的旋转得到b
系,那么根据式(2)可得
R
b
w
R_{bw}
Rbw:
R
b
w
=
R
x
T
∗
R
y
T
∗
R
z
T
=
[
1
0
0
0
cos
θ
−
sin
θ
0
sin
θ
cos
θ
]
T
∗
[
cos
ϕ
0
sin
ϕ
0
1
0
−
sin
ϕ
0
cos
ϕ
]
T
∗
[
cos
ψ
−
sin
ψ
0
sin
ψ
cos
ψ
0
0
0
1
]
T
=
[
1
0
0
0
cos
θ
sin
θ
0
−
sin
θ
cos
θ
]
∗
[
cos
ϕ
0
−
sin
ϕ
0
1
0
sin
ϕ
0
cos
ϕ
]
∗
[
cos
ψ
sin
ψ
0
−
sin
ψ
cos
ψ
0
0
0
1
]
\begin{aligned} R_{bw} &= R_x^T * R_y^T * R_z^T \\ &= \left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {-\sin \theta} \\ {0} & {\sin \theta} & {\cos \theta}\end{array}\right]^T * \left[\begin{array}{ccc}{\cos \phi} & {0} & {\sin \phi} \\ {0} & {1} & {0} \\ {-\sin \phi} & {0} & {\cos \phi}\end{array}\right]^T * \left[\begin{array}{ccc}{\cos \psi} & {-\sin \psi} & {0} \\ {\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1}\end{array}\right]^T \\ &= \left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {\sin \theta} \\ {0} & {-\sin \theta} & {\cos \theta}\end{array}\right] * \left[\begin{array}{ccc}{\cos \phi} & {0} & {-\sin \phi} \\ {0} & {1} & {0} \\ {\sin \phi} & {0} & {\cos \phi}\end{array}\right] * \left[\begin{array}{ccc}{\cos \psi} & {\sin \psi} & {0} \\ {-\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1}\end{array}\right] \end{aligned}
Rbw=RxT∗RyT∗RzT=
1000cosθsinθ0−sinθcosθ
T∗
cosϕ0−sinϕ010sinϕ0cosϕ
T∗
cosψsinψ0−sinψcosψ0001
T=
1000cosθ−sinθ0sinθcosθ
∗
cosϕ0sinϕ010−sinϕ0cosϕ
∗
cosψ−sinψ0sinψcosψ0001
从w
系变换到b
系所进行的ZYX
顺序的旋转的含义是什么呢?我们知道
t
w
b
t_{wb}
twb 是b
系的坐标原点在w
系下的坐标,那么此处的ZYX
顺序的旋转就是指:b
系在w
系下的欧拉角,表示w
系按照此欧拉角顺序旋转即可得到b
系!既然是b
系在w
系下的欧拉角,那么我们求解的应该是
R
w
b
R_{wb}
Rwb,所以对
R
b
w
R_{bw}
Rbw做转置,得到
R
w
b
R_{wb}
Rwb,如下:
R
w
b
=
R
b
w
T
=
R
z
∗
R
y
∗
R
x
=
[
cos
ψ
−
sin
ψ
0
sin
ψ
cos
ψ
0
0
0
1
]
∗
[
cos
ϕ
0
sin
ϕ
0
1
0
−
sin
ϕ
0
cos
ϕ
]
∗
[
1
0
0
0
cos
θ
−
sin
θ
0
sin
θ
cos
θ
]
\begin{aligned} R_{wb}=R_{bw}^T &=R_z * R_y * R_x\\ &=\left[\begin{array}{ccc}{\cos \psi} & {-\sin \psi} & {0} \\ {\sin \psi} & {\cos \psi} & {0} \\ {0} & {0} & {1}\end{array}\right] * \left[\begin{array}{ccc}{\cos \phi} & {0} & {\sin \phi} \\ {0} & {1} & {0} \\ {-\sin \phi} & {0} & {\cos \phi}\end{array}\right]* \left[\begin{array}{ccc}{1} & {0} & {0} \\ {0} & {\cos \theta} & {-\sin \theta} \\ {0} & {\sin \theta} & {\cos \theta}\end{array}\right] \end{aligned}
Rwb=RbwT=Rz∗Ry∗Rx=
cosψsinψ0−sinψcosψ0001
∗
cosϕ0−sinϕ010sinϕ0cosϕ
∗
1000cosθsinθ0−sinθcosθ
注意,此时的左乘顺序跟旋转顺序是相反的,这也是VINS代码里的写法。同样,当从旋转矩阵
R
w
b
R_{wb}
Rwb转换成ZYX
顺序的欧拉角时,此欧拉角也表示:b
系在w
系下的欧拉角,即从w
系(
R
w
b
R_{wb}
Rwb中目的坐标系)变换到b系(
R
w
b
R_{wb}
Rwb中源坐标系)所用的欧拉角!
从旋转矩阵转换欧拉角时,将欧拉角转换成旋转矩阵使用的什么样的公式,那么从旋转矩阵转换成欧拉角的时候也要使用同一套公式,这样才不容易乱套
3、旋转的本质
坐标系
b
1
b_1
b1(其上有一个点
p
p
p,坐标也用
p
p
p 表示),先绕X
轴转
α
α
α 角度,再绕Y
轴转
β
β
β,再绕Z
轴转
γ
γ
γ,得到
b
2
b_2
b2 坐标系,计算
R
b
2
b
1
R_{b_2b_1}
Rb2b1?将这个过程分为下面三步进行:
1)坐标系
b
1
b_1
b1 先绕X
轴转
α
α
α,在新坐标系
b
1
′
b1'
b1′ 下
p
p
p 的坐标是:
p
′
=
R
x
T
∗
p
p'=R_x^T * p
p′=RxT∗p
你也可以写成:
R
x
∗
p
′
=
p
R_x*p'=p
Rx∗p′=p,但是因为后面接下来还有别的旋转,后面的旋转可以看成在本次旋转的结果的叠加,所以为了可持续发展,当然要使用在当前这个坐标系下
p
p
p 点的坐标
p
′
p'
p′ 了
2)坐标系
b
1
′
b1'
b1′ 再绕Y
轴转
β
β
β ,在新坐标系
b
1
′
′
b_1''
b1′′ 下
p
p
p 的坐标是:
p
′
′
=
R
y
T
∗
p
′
p''=R_y^T * p'
p′′=RyT∗p′
3)坐标系
b
1
′
′
b_1''
b1′′ 再绕Z
轴转
γ
γ
γ,在新坐标系
b
1
′
′
′
b_1'''
b1′′′ 下
p
p
p 的坐标是:
p
′
′
′
=
R
z
T
∗
p
′
′
p'''=R_z^T * p''
p′′′=RzT∗p′′
将上面的过程合并(组合)得,
p
′
′
′
=
R
z
T
∗
R
y
T
p
′
=
R
z
T
∗
R
y
T
∗
R
x
T
∗
p
p'''=R_z^T * R_y^Tp' = R_z^T * R_y^T * R_x^T * p
p′′′=RzT∗RyTp′=RzT∗RyT∗RxT∗p
所以
R
b
2
b
1
=
R
z
T
∗
R
y
T
∗
R
x
T
R_{b_2b_1} = R_z^T * R_y^T * R_x^T
Rb2b1=RzT∗RyT∗RxT
如果明白了上面的推导过程,以后所有的旋转都能这么推导,因为旋转是叠加的(在上次的结果上继续左乘)。假设存在这样的旋转,先绕X
轴转
α
α
α 角度,再还是绕X
轴转
α
′
α'
α′,再绕Y
轴转
β
β
β,然后还是绕X
轴转
α
′
′
α''
α′′,整个旋转下来组合的旋转矩阵就是:
R
=
R
(
α
′
′
)
T
∗
R
(
β
)
T
∗
R
(
α
′
)
T
∗
R
(
α
)
T
=
R
(
α
′
′
)
T
∗
R
(
β
)
T
∗
R
(
α
′
+
α
)
T
R=R(α'')^T * R(β)^T * R(α')^T * R(α)^T = R(α'')^T * R(β)^T * R(α'+α)^T
R=R(α′′)T∗R(β)T∗R(α′)T∗R(α)T=R(α′′)T∗R(β)T∗R(α′+α)T
之所以 α ′ α' α′ 和 α α α 能合并是因为他俩的旋转轴相同!
4、内部旋转(Z-Y-X
)对应的旋转矩阵
上面按照XYZ
的旋转顺序,推导得到结果是:
R
b
2
b
1
=
R
z
T
∗
R
y
T
∗
R
x
T
R_{b_2b_1} = R_z^T * R_y^T * R_x^T
Rb2b1=RzT∗RyT∗RxT
如果将
b
2
b_2
b2 换成
b
b
b,
b
1
b_1
b1 换成
w
w
w,即得,
R
b
w
=
R
z
T
∗
R
y
T
∗
R
x
T
R_{bw} = R_z^T * R_y^T * R_x^T
Rbw=RzT∗RyT∗RxT,再次转置得到,
R
w
b
=
R
x
∗
R
y
∗
R
z
R_{wb} = R_x * R_y * R_z
Rwb=Rx∗Ry∗Rz
如果我们换成ZYX
顺序旋转(从w
系到b
系),就自然可以得到:
R
w
b
=
R
z
∗
R
y
∗
R
x
R_{wb} = R_z * R_y * R_x
Rwb=Rz∗Ry∗Rx
这个结果就和我们之前的推导是一样的了!注意这是
R
w
b
R_{wb}
Rwb,而不是
R
b
w
R_{bw}
Rbw!见6
中的验证
5、为什么内部旋转(Z-Y-X
)和外部旋转(X-Y-Z
)对应的旋转矩阵是相同的
因为每次都是绕固定的坐标系(记为,
w
w
w)进行旋转,那么不管旋转之后的
b
1
b_1
b1 系在哪里,都可以认为
w
w
w 和
b
1
b_1
b1 系是刚性连接的(第一次旋转之前,可认为
w
w
w 和
b
1
b_1
b1 是重合的),当发生了旋转之后,那么它们之间就会相差了一个旋转矩阵外参:
假设先对
w
w
w 做了一次绕 X
轴的旋转,得到
b
1
′
b_1'
b1′,
R
b
1
′
w
=
R
w
′
w
=
R
x
T
R_{b_1'w} = R_{w'w} = R_x^T
Rb1′w=Rw′w=RxT,这就是此时的外参
如果接着再对
w
w
w 做一次绕Y
轴的旋转,得到
b
1
′
′
b_1''
b1′′,显然
R
w
′
w
=
R
y
T
R_{w'w}=R_y^T
Rw′w=RyT,使用外参进行转换,
R
b
1
′
′
w
=
(
R
b
1
′
w
∗
R
w
′
w
∗
R
b
1
′
w
T
)
∗
R
b
1
′
w
=
R
x
T
∗
R
y
T
∗
R
x
∗
R
x
T
=
R
x
T
∗
R
y
T
R_{b_1''w}= (R_{b_1'w} * R_{w'w} * R_{b_1'w}^T) * R_{b_1'w} = R_x^T * R_y^T * R_x * R_x^T= R_x^T * R_y^T
Rb1′′w=(Rb1′w∗Rw′w∗Rb1′wT)∗Rb1′w=RxT∗RyT∗Rx∗RxT=RxT∗RyT
如果绕
w
w
w 做一次绕Z轴的旋转,得到
b
1
′
′
′
b_1'''
b1′′′,即,
R
b
1
′
′
′
w
=
R
x
T
∗
R
y
T
∗
R
z
T
,
R
w
b
1
′
′
′
=
R
z
∗
R
y
∗
R
x
R_{b_1'''w}=R_x^T * R_y^T * R_z^T,R_{wb_1'''}=R_z * R_y * R_x
Rb1′′′w=RxT∗RyT∗RzT,Rwb1′′′=Rz∗Ry∗Rx
所以就可以得到,绕固定轴旋转的XYZ旋转顺序
和绕旋转之后的轴的ZYX旋转顺序
是等价的!!!
6、验证内部旋转(Z-Y-X
)得到的是
R
w
b
R_{wb}
Rwb,而不是
R
b
w
R_{bw}
Rbw
Eigen::Matrix3d zyxToRotationMatrix(Eigen::Vector3d zyx)
{
// 计算旋转矩阵的X分量
Eigen::Matrix3d R_x;
R_x << 1, 0, 0,
0, cos(zyx[2]), -sin(zyx[2]),
0, sin(zyx[2]), cos(zyx[2]);
// 计算旋转矩阵的Y分量
Eigen::Matrix3d R_y;
R_y << cos(zyx[1]), 0, sin(zyx[1]),
0, 1, 0,
-sin(zyx[1]), 0, cos(zyx[1]);
// 计算旋转矩阵的Z分量
Eigen::Matrix3d R_z;
R_z << cos(zyx[0]), -sin(zyx[0]), 0,
sin(zyx[0]), cos(zyx[0]), 0,
0, 0, 1;
// 依次左乘,合并
Eigen::Matrix3d R = R_z*R_y*R_x;
return R;
}
int main()
{
Eigen::AngleAxisd r_z ( 0, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转
Eigen::AngleAxisd r_y ( M_PI/6, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转
Eigen::AngleAxisd r_x ( M_PI/6, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转
Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序(绕旋转后的轴接着旋转)
//【1-2】: 四元数-->>旋转矩阵
Eigen::Matrix3d rotation_matrix = q_zyx.toRotationMatrix(); //如果是从w系按照ZYX旋转得到b系,那么此旋转矩阵是Rwb,不是Rbw
cout << (rotation_matrix * Eigen::Vector3d(1,0,0)).transpose() << endl;
cout << (rotation_matrix.transpose() * Eigen::Vector3d(1,0,0)).transpose() << endl;
//【2-1】: 欧拉角(机体坐标系旋转)
Eigen::Vector3d euler_zyx(0, M_PI/6, M_PI/6);
//【2-2】: 欧拉角-->>旋转矩阵
rotation_matrix = zyxToRotationMatrix(euler_zyx); //此转换结果与【1-1】相同
cout << (rotation_matrix * Eigen::Vector3d(1,0,0)).transpose() << endl;
//......
return 0;
}
输出结果是:
0.866025 -1.38778e-17 -0.5
0.866025 0.25 0.433013
0.866025 0 -0.5
将上面代码里欧拉角显示出来,如下图,
测试代码地址:https://github.com/LeatherWang/slam_car
<完>
@leatherwang