欧拉角因为其奇异性,虽然在优化和插值的不会使用,但是当我们对别人描述一个旋转的过程是怎么样的时候,欧拉角还是很有用的,比如,做无人机姿态控制的时候使用的就是欧拉角,但是搞明白欧拉角与旋转矩阵的转换确实是一件头疼的事,所以就写下了这篇总结,希望对大家理解欧拉角有所帮助
需要区分每次旋转是绕固定轴旋转的,还是绕旋转之后的轴旋转的,如果不特殊指明,下面的讨论都是指:绕旋转之后的轴旋转的。不要局限于表示旋转的旋转顺序是什么样的,使用ZYX
(先绕Z
再绕Y
最后绕X
)仅仅是因为习惯而已。仅仅绕单个轴旋转一个角度得到的也是旋转矩阵!
定义一个欧拉角,需要明确下面5条:
1.1 表示旋转的欧拉角旋转顺序有12种
z-x-z
,x-y-x
,y-z-y
,z-y-z
, x-z-x
,y-x-y
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 主动旋转和被动旋转
主动旋转是指将向量逆时针围绕旋转轴旋转,被动旋转是对坐标轴进行的逆时针旋转,相当于主动旋转的逆操作
给出逆时针旋转的角度为正时(与右手系旋转方向相同的为旋转正方向),绕不同轴的旋转结果:
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中源坐标系)所用的欧拉角!
从旋转矩阵转换欧拉角时,将欧拉角转换成旋转矩阵使用的什么样的公式,那么从旋转矩阵转换成欧拉角的时候也要使用同一套公式,这样才不容易乱套
坐标系 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
之所以 α ′ α' α′ 和 α α α 能合并是因为他俩的旋转轴相同!
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
中的验证
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旋转顺序
是等价的!!!
Z-Y-X
)得到的是 R w b R_{wb} Rwb,而不是 R b w R_{bw} RbwEigen::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