三维空间刚体运动3：欧拉角表示旋转（全面理解万向锁、RPY角和欧拉角）

1. 欧拉角
- 1.1 定义
- 2.2 RPY角与Z-Y-X欧拉角
2. 欧拉角到旋转矩阵
3. 旋转矩阵到欧拉角
4. 万向锁
- 4.1 定义
- 4.2 顺规ZYX的万向锁
- 4.3 解决方法
5. 实践：Eigen几何模块

序：本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》，讲解三维空间刚体运动，为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解，其中四元数部分较多较复杂，又分为四部分。如果读者急于实践，可直接阅读第五部分的机器人运动轨迹，此部分详细讲解了安装准备工作。此系列总体目录如下：

旋转矩阵和变换矩阵；
旋转向量表示旋转；
欧拉角表示旋转；
四元数包括以下部分：
4-1. 四元数表示变换；
4-2. 四元数线性插值方法：LinEuler/LinMat/Lerp/Nlerp/Slerp；
4-3. 四元数多点插值方法：Squad；
4-4. 四元数多点连续解析解插值方法：Spicv；
4-5. 四元数多点离散数值解插值方法：Sping。
实践：SLAM中显示机器人运动轨迹及相机位姿。

1. 欧拉角

1.1 定义

无论是旋转矩阵还是旋转向量，它们虽然能描述旋转，但对人类来说非常不直观，而欧拉角则提供了一种非常直观的方式来描述旋转----它使用了3个分离的转角，把一个旋转分解成3次绕不同轴的旋转。

欧拉角：欧拉角是在空间中，描述从一个用于表示某个固定的参考系的、已知的方向，经过一系列基本旋转得到新的、代表另一个参考系的方向的方式。因为只有旋转，所以原点位置并没有发生变化。

在讨论欧拉角的具体形式之前，需要明确几个概念：

左手坐标系与右手坐标系：既然是在坐标系中进行变换，首先就要了解坐标系的类别。同样的数据在左手坐标系和右手坐标系会有不同的呈现结果。本文在右手坐标系中讨论。
旋转的具体形式：物体旋转有多种，比如绕某个定点进行旋转，绕坐标轴进行旋转，绕任意轴进行旋转。不同的旋转需要用不同的数学方式来表达。注意，本文讨论的是三维中绕坐标轴的旋转。
欧拉角顺规：欧拉角定义了一组物体的旋转次序，称为顺规，可以理解为欧拉角旋转顺序的规定。(α,β,γ)(\alpha ,\beta, \gamma)(α,β,γ)在不同的旋转顺序下会有不同的结果，先绕X轴旋转α\alphaα，还是先绕y轴旋转β\betaβ，最后的结果是不一样的。本文根据不同旋转方式会采用不同的顺规。
静态动态欧拉角：从参考坐标系上区分，将欧拉角分为静态和动态，其中静态欧拉角以绝对坐标系为参考，不随转动改变，一般用小写的x-y-z来表示静态坐标系；动态欧拉角以刚体自身的物体坐标系为参考，一般用大写的X-Y-Z表示。

2.2 RPY角与Z-Y-X欧拉角

描述坐标系BBB相对于参考坐标系AAA的姿态有两种方式：RPY角与Z-Y-X欧拉角。
RPY角：RPY角是绕固定（参考）坐标系AAA旋转：假设开始两个坐标系重合，先将BBB绕AAA的X轴旋转α\alphaα，然后绕AAA的Y轴旋转β\betaβ，最后绕AAA的Z轴旋转γ\gammaγ，就能旋转到当前姿态。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。由于是绕固定坐标系旋转，则旋转矩阵为：RXYZ(α,β,γ)=RZ(γ)RY(β)RX(α).R_{XYZ}(\alpha,\beta ,\gamma) = R_{Z}(\gamma)R_{Y}(\beta)R_{X}(\alpha).RXYZ(α,β,γ)=RZ(γ)RY(β)RX(α).其乘法顺序为：从右到左，即XYZ。展开式为式（3.1）。
Z-Y-X欧拉角：另一种姿态描述方式是绕自身坐标轴BBB旋转：假设开始两个坐标系重合，先将BBB绕自身的Z轴旋转γ\gammaγ，然后绕Y轴旋转β\betaβ，最后绕X轴旋转α\alphaα，就能旋转到当前姿态。称其为Z-Y-X欧拉角，由于是绕自身坐标轴进行旋转，则旋转矩阵为：RX′Y′Z′(α,β,γ)=RZ′(γ)RY′(β)RX′(α)R_{X^{'}Y^{'}Z^{'}}(\alpha ,\beta ,\gamma) = R_{Z^{'}}(\gamma)R_{Y^{'}}(\beta)R_{X^{'}}(\alpha )RX′Y′Z′(α,β,γ)=RZ′(γ)RY′(β)RX′(α)其乘法顺序为：从左到右，即Z′Y′X′Z^{'}Y^{'}X^{'}Z′Y′X′。其展开式为(3.1)。
由于展开式相等，所以绕定轴X-Y-Z旋转（RPY角）等价于绕动轴Z-Y-X旋转（Euler角）。
此处高翔博士对RPY角和欧拉角的解释与其它版本不同，经反复确认后，决定采用熊有伦等编著的《机器人学》，也是网上流传最多的版本，相信，读者明白这一个知识点后，后边关于欧拉角及万向锁理解的误区会迎刃而解。
此处为方便理解，根据航空中的使用的RPY角定义再对此进行阐释。欧拉角定义方式上的不确定性带来了很多实际当中的困难，所幸在特定领域内，欧拉角通常有统一的定义方式。当中常用的一种是航空领域的，用“偏航-仰俯-滚转”（yaw-pitch-row）3个角度来描述的旋转，即RPY角，它是绕定轴x-y-z轴（从右至左）的旋转。RPY角相当于Z-Y-X欧拉角（从左至右）。那么，RPY角相当于把任意旋转分解成以下3个轴的转角，如图：
图1.1 Oxyz坐标轴旋转

绕物体的Z轴旋转γ\gammaγ，得到偏航角yaw。
绕旋转之后的Y轴旋转β\betaβ，得到仰俯角pitch。
绕旋转之后的X轴旋转α\alphaα，得到滚转角roll。

此时，可以使用[r,p,y]T[r,p,y]^{T}[r,p,y]T(即欧拉角)这样一个三维的向量描述任意旋转。rpy角的旋转顺序（从右到左）是ZYX。此外，为规避万向锁现象，常用的顺序是ZXY，但是无法完全消除万向锁，下面会解释原因。下面讲解欧拉角与旋转矩阵转换的推导，然后引出万向锁问题。

2. 欧拉角到旋转矩阵

一个旋转矩阵具有三个自由度，可以与欧拉角进行转换。下边推导欧拉角到旋转矩阵的转换。欧拉角旋转每次只绕一个坐标轴旋转，因此推导可分解为三次二维情况下的坐标变换，再延伸到三维。二维下称之为基本旋转矩阵，由基本旋转组成三维下的组合旋转矩阵。先讨论二维坐标下的情景。如下图所示：
图2.1 二维坐标旋转

点P(x,y)P(x,y)P(x,y)在原坐标系中与X轴夹角为α\alphaα，绕Z轴旋转角θ\thetaθ后的坐标为P′(x′,y′)P'(x',y')P′(x′,y′)，现在来推导(x′,y′)(x',y')(x′,y′)关于(x,y)(x,y)(x,y)的表示法。
推导方法有两种，第一种是利用三角恒等式的两角和差公式，第二种是利用三角形的性质推导。本文采用第一种，对第二种感兴趣的童鞋可祥阅《旋转矩阵（Rotation Matrix）的推导及其应用》。
由图可知，OP=OP′=x2+y2=dOP=OP'=\sqrt{x^{2} + y^{2}}=dOP=OP′=x2+y2=d，由正余弦定理可得到两组方程：{sin⁡α=ydcos⁡α=xd(2.1)\left\{\begin{matrix} \sin\alpha = \frac{y}{d}\\ \cos\alpha = \frac{x}{d} \end{matrix}\right. \tag{2.1}{sinα=dycosα=dx(2.1)和{sin⁡(α+θ)=y′dcos⁡(α+θ)=x′d(2.2)\left\{\begin{matrix} \sin(\alpha + \theta )= \frac{y'}{d}\\ \cos(\alpha + \theta )= \frac{x'}{d} \end{matrix}\right. \tag{2.2}{sin(α+θ)=dy′cos(α+θ)=dx′(2.2)
由两角和差公式可知：{sin⁡(α+θ)=sin⁡αcos⁡θ+cos⁡αsin⁡θcos⁡(α+θ)=cos⁡αcos⁡θ−sin⁡αsin⁡θ(2.3)\left\{\begin{matrix} \sin(\alpha + \theta )= \sin\alpha\cos\theta+\cos\alpha\sin\theta\\ \cos(\alpha + \theta )= \cos\alpha\cos\theta-\sin\alpha\sin\theta \end{matrix}\right. \tag{2.3}{sin(α+θ)=sinαcosθ+cosαsinθcos(α+θ)=cosαcosθ−sinαsinθ(2.3)将式（2.1）和（2.3）代入（2.2），消掉ddd得到：{x′=xcos⁡θ−ysin⁡θy′=ycos⁡θ+xsin⁡θ(2.4)\left\{\begin{matrix} x' = x\cos\theta-y\sin\theta\\ y' = y\cos\theta+x\sin\theta \end{matrix}\right.\tag{2.4}{x′=xcosθ−ysinθy′=ycosθ+xsinθ(2.4)因此有如下推导，设P′P'P′的齐次坐标为P′~\tilde{P'}P′~：P′~=[x′,y′,1]T=[cosθ−sin⁡θ0sin⁡θcos⁡θ0001][xy1]=RZ(θ)[xy1](2.5)\tilde{P'}=[x',y',1]^{T}=\begin{bmatrix} cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} x\\ y\\ 1 \end{bmatrix}=R_Z(\theta )\begin{bmatrix} x\\ y\\ 1 \end{bmatrix}\tag{2.5}P′~=[x′,y′,1]T=⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤⎣⎡xy1⎦⎤=RZ(θ)⎣⎡xy1⎦⎤(2.5)因此，绕Z轴的旋转矩阵为：RZ(θ)=[cosθ−sin⁡θ0sin⁡θcos⁡θ0001](2.6)R_Z(\theta )=\begin{bmatrix} cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix}\tag{2.6}RZ(θ)=⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤(2.6)同理可推得绕Y轴的旋转矩阵为：RY(θ)=[cosθ0sin⁡θ010−sin⁡θ0cos⁡θ](2.7)R_Y(\theta )=\begin{bmatrix} cos\theta & 0 & \sin\theta\\ 0 & 1 & 0\\ -\sin\theta & 0 & \cos\theta \end{bmatrix}\tag{2.7}RY(θ)=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤(2.7)绕X轴的旋转矩阵为：RX(θ)=[1000cos⁡θ−sin⁡θ0sin⁡θcos⁡θ](2.8)R_X(\theta )=\begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\theta & -\sin\theta\\ 0 & \sin\theta & \cos\theta \end{bmatrix}\tag{2.8}RX(θ)=⎣⎡1000cosθsinθ0−sinθcosθ⎦⎤(2.8)组合旋转矩阵等于基本旋转矩阵的连乘，连乘顺序依基本旋转的先后次序由右向左排列，因此顺规为ZYX的组合旋转矩阵为：RZYX=RX(α)RY(β)RZ(γ)(2.9)R_{ZYX}=R_X(\alpha)R_Y(\beta)R_Z(\gamma)\tag{2.9}RZYX=RX(α)RY(β)RZ(γ)(2.9)

3. 旋转矩阵到欧拉角

已知旋转矩阵，如何求欧拉角呢？只需要对应到旋转矩阵，求反弦值即可。将式（2.9）两侧展开得：[R11R12R13R21R22R23R31R32R33]=[cos⁡βcos⁡γ−cos⁡βsin⁡γsin⁡βsin⁡αsin⁡βcos⁡γ+cos⁡αsin⁡γ−sin⁡αsin⁡βsin⁡γ+cos⁡αcos⁡γ−sin⁡αcos⁡β−cos⁡αsin⁡βcos⁡γ+sin⁡αsin⁡γcos⁡αsin⁡βsin⁡γ+sin⁡αcos⁡γcos⁡αcos⁡β](3.1)\begin{bmatrix} R_{11} & R_{12} & R_{13}\\ R_{21} & R_{22} & R_{23}\\ R_{31} & R_{32} & R_{33} \end{bmatrix}= \begin{bmatrix} \cos \beta\cos \gamma & -\cos \beta\sin \gamma & \sin \beta \\ \sin \alpha\sin \beta\cos \gamma +\cos \alpha\sin \gamma & -\sin \alpha\sin \beta\sin \gamma +\cos \alpha\cos \gamma & -\sin \alpha\cos \beta\\ -\cos \alpha\sin \beta\cos \gamma +\sin \alpha\sin \gamma & \cos \alpha\sin \beta\sin \gamma +\sin \alpha\cos \gamma & \cos \alpha\cos \beta \end{bmatrix}\tag{3.1}⎣⎡R11R21R31R12R22R32R13R23R33⎦⎤=⎣⎡cosβcosγsinαsinβcosγ+cosαsinγ−cosαsinβcosγ+sinαsinγ−cosβsinγ−sinαsinβsinγ+cosαcosγcosαsinβsinγ+sinαcosγsinβ−sinαcosβcosαcosβ⎦⎤(3.1)
因此推得：{α=arctan⁡(−R23R33)β=arcsin⁡R13γ=arctan⁡(−R12R11)(3.2)\left\{\begin{matrix} \alpha=\arctan (-\frac{R_{23}}{R_{33}})\\ \beta=\arcsin R_{13}\\ \gamma=\arctan (-\frac{R_{12}}{R_{11}}) \end{matrix}\right.\tag{3.2}⎩⎨⎧α=arctan(−R33R23)β=arcsinR13γ=arctan(−R11R12)(3.2)由上面推导，引出万向锁问题。

4. 万向锁

4.1 定义

万向锁问题：欧拉角的一个重大缺点就是会碰到著名的万向锁问题（Gimbal Lock）,比如对于rpy，当仰俯角为±90∘\pm 90^{\circ }±90∘时，第一次旋转与第三次旋转使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了两次旋转），这被称为奇异性问题，或万向锁，其它顺规形式的欧拉角也同样存在奇异性问题。

理论上可以证明，只要想用3个实数来表达三维旋转，都不可避免的碰到奇异性问题。由于这种原理，欧拉角不适用于插值和迭代，往往只用于人机交互中，因此我们很少在SLAM程序、滤波以及优化中使用欧拉角表达旋转。

4.2 顺规ZYX的万向锁

万向锁理论性描述讲完了，相信大部分人都没理解，网文解释的也比较模糊。以外国小哥的视频：欧拉旋转，配合截图，试讲解如下：

以欧拉角的顺规ZYX为例，如下图所示：图中蓝色为Z轴，绿色为Y轴，红色为X轴，它们有一定的层级关系，Z为Y的父级，Y为X的父级。当绕Z轴旋转时，它的子级Y轴和X轴也会跟着移动；当绕Y轴旋转时，X轴会随之移动，Z轴不发生改变；当绕X轴旋转时，Z轴和Y轴都不会发生改变。层级关系是理解万向节锁问题的关键。初始状态如图所示：
图4.1 ZYX欧拉旋转-1

箭头绕Z轴旋转，此时Y轴和X轴也跟着变化，如图所示：
图4.2 ZYX欧拉旋转-2

箭头继续绕Y轴旋转，此时Z轴固定不变，X轴随之变化，如图所示：
图4.3 ZYX欧拉旋转-3
当Y轴旋转角度为±90∘\pm 90^{\circ }±90∘时，X轴和Z轴重合共面，丢失了一个自由度。三次旋转变换仅仅覆盖了两个外部轴的旋转，一个自由度就这样丢失了，这也就导致了 Gimbal Lock 的现象。Gimbal Lock 问题的核心还是在于我们采用了固定的旋转顺序。对于其它5种组合的顺规，当中间的轴正好旋转±90∘\pm 90^{\circ }±90∘或两侧轴重合时，同样会产生万向锁现象。
这个变换用公式来理解的话，则是这样（你可以自己来代入验证一下），将(α,π2,γ)(\alpha,\frac{\pi }{2} ,\gamma )(α,2π,γ)代入式（3.1）得：RZYX(α,π2,γ)=RX(γ)RY(π2)RZ(α)=[001sin⁡αcos⁡γ+cos⁡αsin⁡γ−sin⁡αsin⁡γ+cos⁡αcos⁡γ0−cos⁡αcos⁡γ+sin⁡αsin⁡γcos⁡αsin⁡γ+sin⁡αcos⁡γ0]=[001sin⁡(α+γ)cos⁡(α+γ)0−cos⁡(α+γ)sin⁡(α+γ)0]=RY(π2)RZ(α+γ)(4.1)\begin{aligned} R_{ZYX}(\alpha,\frac{\pi }{2} ,\gamma ) &= R_{X}(\gamma )R_{Y}(\frac{\pi }{2})R_{Z}(\alpha)\\ &= \begin{bmatrix} 0 & 0 & 1\\ \sin \alpha\cos \gamma +\cos \alpha\sin \gamma & -\sin \alpha\sin \gamma +\cos\alpha\cos \gamma & 0\\ -\cos \alpha\cos \gamma +\sin \alpha\sin \gamma & \cos \alpha\sin \gamma +\sin \alpha\cos \gamma & 0 \end{bmatrix}\\&= \begin{bmatrix} 0 & 0 & 1\\ \sin (\alpha+ \gamma ) & \cos (\alpha+ \gamma ) & 0\\ -\cos (\alpha+ \gamma ) & \sin (\alpha+ \gamma ) & 0 \end{bmatrix}\\&=R_{Y}(\frac{\pi }{2})R_{Z}(\alpha+ \gamma ) \end{aligned}\tag{4.1}RZYX(α,2π,γ)=RX(γ)RY(2π)RZ(α)=⎣⎡0sinαcosγ+cosαsinγ−cosαcosγ+sinαsinγ0−sinαsinγ+cosαcosγcosαsinγ+sinαcosγ100⎦⎤=⎣⎡0sin(α+γ)−cos(α+γ)0cos(α+γ)sin(α+γ)100⎦⎤=RY(2π)RZ(α+γ)(4.1)
利用一些三角恒等式，将原本由三个旋转矩阵所组成的变换化简成了两个变换矩阵。即便我们分别对Z−Y−XZ-Y-XZ−Y−X三轴进行了旋转，实际上这个矩阵仅仅旋转了Z−YZ-YZ−Y两轴，它并没有对初始的X 轴进行变换。RZ(γ)R_{Z}(\gamma )RZ(γ) 与RX(α)R_{X}(\alpha)RX(α)这两个变换被合并为单独的一个RZ(γ+α)R_{Z}(\gamma + \alpha )RZ(γ+α)变换（因为化简完之后变换顺序不一样了，严格来说并不是合并，只不过是能够使用一步来完成）。
注意，我们在这里化简的并不是单独的一个变换，而是一系列变换。因为它对任意 α\alphaα 和γ\gammaγ角都是成立的。也就是说，一旦

三维空间刚体运动3：欧拉角表示旋转（全面理解万向锁、RPY角和欧拉角）相关推荐

三维空间刚体运动4-1：四元数表示旋转（各形式相互转换加代码）
三维空间刚体运动4-1:四元数表示变换(各形式相互转换加代码) 1. 四元数的定义 1.1 为什么使用四元数 1.2 复数与四元数 1.3 四元数的形式 2. 四元数的运算 2.1 基础运算 2.2 ...
三维空间刚体运动2：旋转向量与罗德里格斯公式（最详细推导）
三维空间刚体运动2:旋转向量与罗德里格斯公式(最详推导) 1.旋转向量定义 2.罗德里格斯公式-向量转换为矩阵 2.1 定义 2.2 推导 2.2.1 推导一 2.2.2 推导二 2.2.3 推导向量 ...
三维空间刚体运动4-5：四元数多点离散数值解插值方法：Sping
三维空间刚体运动4-5:四元数多点离散数值解插值方法:Sping 1. 正切曲率κ(γ,t)\kappa(\gamma, t)κ(γ,t)在H1H_{1}H1上的离散数值解--Sping 1.1 离 ...
三维空间刚体运动4-4：四元数多点连续解析解插值方法：Spicv
三维空间刚体运动4-4:四元数多点连续解析解插值方法:Spicv 1. 总述:多点旋转插值的数学方法 2. 插值曲线及其连续性 2.1 插值曲线定义 2.2 插值曲线连续性的讨论 3. 最优插值曲线 ...
三维空间刚体运动5：详解SLAM中显示机器人运动轨迹及相机位姿（原理流程）
三维空间刚体运动5:详解SLAM中显示机器人运动轨迹及相机位姿(原理流程) 一.显示运动轨迹原理讲解二.前期准备三.git管理子模块及克隆源代码 1.学习使用Git Submodule 2.克隆源 ...
三维空间刚体运动4-3：四元数线性插值方法：Squad
三维空间刚体运动4-3:四元数线性插值方法:Squad Squad的引出 B e ˊ z i e r c u r v e B\acute{e}zier \space curveB e ˊ zier c ...
视觉SLAM十四讲：第3讲三维空间刚体运动
第3讲:三维空间刚体运动三维空间中刚体运动的描述方式:旋转矩阵.变换矩阵.四元数和欧拉角 3.1 旋转矩阵 3.1.1 点和向量,坐标系三维空间中,给定线性空间基(e1,e2,e3)(\mathb ...
高博SLAM十四讲书本程序学习——第3讲三维空间刚体运动
小白高博SLAM十四讲书本程序学习_1 第3讲三维空间刚体运动在高博原始注释上,针对我自己不明白的部分,做额外注释如果有错误的地方,请大家指点指点博文目录一.P.48 eigenMatrix ...
高博14讲--第三讲三维空间刚体运动
高博14讲--第三讲三维空间刚体运动旋转矩阵点和向量.坐标系坐标系间的欧式变换变换矩阵与齐次坐标旋转向量和欧拉角旋转向量欧拉角四元数四元数的定义四元数的运算用四元数表示旋转四 ...

三维空间刚体运动3：欧拉角表示旋转（全面理解万向锁、RPY角和欧拉角）

三维空间刚体运动3：欧拉角表示旋转（全面理解万向锁、RPY角和欧拉角）

1. 欧拉角

1.1 定义

2.2 RPY角与Z-Y-X欧拉角

2. 欧拉角到旋转矩阵

3. 旋转矩阵到欧拉角

4. 万向锁

4.1 定义

4.2 顺规ZYX的万向锁

三维空间刚体运动3：欧拉角表示旋转（全面理解万向锁、RPY角和欧拉角）相关推荐

最新文章

热门文章