SLAM 之四元数转欧拉角再理解

   我们知道在SLAM中，经常会用到旋转，平移等坐标变换，平移的话，相对来说比较好理解，但是旋转就会比较复杂，通常旋转可以有很多种表示方法，像是旋转矩阵，四元数，欧拉角，欧拉轴角，李代数(SO3)等，但是万变不离其宗，都是为了表达两个坐标系之间的相对角度关系。最近重新回顾欧拉角这些基础知识的时候有了一些新的体会，这篇博文主要是为了记录一下最近的总结。首先，参考了网上很多博客和资源，以前在脑子里记得的欧拉角都是Z-Y-X这种常用的欧拉角转换，但是都知道欧拉角可以有12种组合，就是不知道到底是怎么个意思，所以这里记录一下。以下内容仅自己总结，如需转载请著名出处，如有错误，欢迎指出，谢谢！若文中缺少了对相关文章或链接的引用，请指出，我会添加上去的，尊重原创！

欧拉角定义

   总结之前还是要回顾一下，欧拉角的定义，欧拉角定义分为两种，一种是静态欧拉角，也称外旋/固定轴，另一种是动态欧拉角，也称内旋/动态轴，参考weiki百科的内容，静态和动态可以从下面两幅图来理解

静态欧拉角，每次都绕着固定参考系的某一个轴转动

动态欧拉角，每次都绕着自身参考系的某一个轴转动

总之一句话，静态欧拉角是绕着固定坐标系旋转的角度，动态欧拉角是绕着刚体自身坐标系的旋转，且内旋是右乘，外旋是左乘变换
在SLAM中其实经常用的是内旋的形式，所以下面的内容也是按照内旋来计算说明的。

四元数到旋转矩阵

   都知道四元数是可以转换为旋转矩阵的，但实际上怎么转换的呢，网上也有很多直接给公式的，但是只要花点时间去推，很快就能推出来的：

假设有点p1和p2，满足对应的旋转关系，p1=R∗p2p1 = R*p2p1=R∗p2，其中RRR表示旋转矩阵
R=[r00r01r02r10r11r12r20r21r22]R=\left[\begin{matrix} r00 & r01 & r02 \\ r10 & r11 & r12 \\ r20 & r21 & r22\end{matrix} \right] R=⎣⎡r00r10r20r01r11r21r02r12r22⎦⎤
而对四元数q=[qw,qx,qy,qz]T=[qw,qv]\bold q = [qw,qx,qy,qz]^T = [qw,\bold {qv}]q=[qw,qx,qy,qz]T=[qw,qv]而言，有
[0p1xp1yp1z]=q∗[0p2xp2yp2z]∗q∗\left[\begin{matrix}0\\ p1_x \\ p1_y \\ p1_z\end{matrix}\right] = \bold q *\left[\begin{matrix}0\\ p2_x \\ p2_y \\ p2_z\end{matrix}\right]*\bold q^* ⎣⎢⎢⎡0p1xp1yp1z⎦⎥⎥⎤=q∗⎣⎢⎢⎡0p2xp2yp2z⎦⎥⎥⎤∗q∗
其中q∗\bold q^*q∗表示共扼四元数q∗=[q0,−qx,−qy,−qz]T\bold q^* = [q0,-qx,-qy,-qz]^Tq∗=[q0,−qx,−qy,−qz]T。
另外，四元数的乘法也可以写成矩阵的形式，上面的内容可以转换为：
[0p1xp1yp1z]=[q]L∗[q∗]R[0p2xp2yp2z]其中，[q]L=qwI+[0−qvTqv[qv]×],表示左乘四元数q然后，[q∗]R=qwI+[0−q∗vTq∗v−[q∗v]×],表示乘右乘四元数q∗[qv]×=[0−qzqyqz0−qx−qyqx0]表示反对称矩阵\left[\begin{matrix}0\\ p1_x \\ p1_y \\ p1_z\end{matrix}\right] = \bold [\bold q]_L *[\bold q^*]_R\left[\begin{matrix}0\\ p2_x \\ p2_y \\ p2_z\end{matrix}\right] \\ 其中，[\bold q]_L = qw\bold I + \left[\begin{matrix}0 & -\bold {qv}^T \\ \bold {qv} & [\bold {qv}]_\times\end{matrix}\right],表示左乘四元数\bold q \\ 然后，[\bold q^*]_R = qw\bold I + \left[\begin{matrix}0 & -\bold {q^*v}^T \\ \bold {q^*v} & -[\bold {q^*v}]_\times\end{matrix}\right],表示乘右乘四元数\bold q^* \\ [\bold {qv}]_\times=\left[\begin{matrix} 0 & -qz & qy \\ qz & 0 & -qx \\ -qy & qx & 0 \end{matrix}\right]表示反对称矩阵 ⎣⎢⎢⎡0p1xp1yp1z⎦⎥⎥⎤=[q]L∗[q∗]R⎣⎢⎢⎡0p2xp2yp2z⎦⎥⎥⎤其中，[q]L=qwI+[0qv−qvT[qv]×],表示左乘四元数q然后，[q∗]R=qwI+[0q∗v−q∗vT−[q∗v]×],表示乘右乘四元数q∗[qv]×=⎣⎡0qz−qy−qz0qxqy−qx0⎦⎤表示反对称矩阵
所以，这就可以计算出对应的转换关系：
[q]L∗[q∗]R=qw2∗I+[0002∗qw∗[qv]×]+[qvT∗qv−qvT∗[qv]×−[qv]×∗qvqv∗qvT+[qv]×∗[qv]×]又因为，对向量而言，[a]×∗b=a×b，所以[qv]×∗qv=qv×qv=0T则[q]L∗[q∗]R=[∣∣q∣∣20T0qv∗qvT+qw2I+2∗qw∗[qv]×+[qv]×∗[qv]×]\bold [\bold q]_L *[\bold q^*]_R= qw^2*\bold I + \left[\begin{matrix} 0 & 0 \\ 0 & 2*qw* [\bold {qv}]_\times \end{matrix}\right] + \left[\begin{matrix} \bold {qv}^T*\bold {qv} & -\bold {qv}^T*[\bold {qv}]_\times \\ -[\bold {qv}]_\times*\bold {qv} & \bold {qv}*\bold {qv}^T + [\bold {qv}]_\times*[\bold {qv}]_\times \end{matrix}\right] \\又因为，对向量而言，[\bold {a}]_\times*\bold {b} = \bold a \times\bold b，\\所以[\bold {qv}]_\times*\bold {qv} = \bold {qv} \times\bold {qv} = \bold 0^T \\则 \\\bold [\bold q]_L *[\bold q^*]_R=\left[\begin{matrix} ||\bold {q}||^2 & \bold {0}^T \\ \bold {0} & \bold {qv}*\bold {qv}^T + qw^2\bold I + 2*qw*[\bold {qv}]_\times + [\bold {qv}]_\times*[\bold {qv}]_\times\end{matrix}\right] [q]L∗[q∗]R=qw2∗I+[0002∗qw∗[qv]×]+[qvT∗qv−[qv]×∗qv−qvT∗[qv]×qv∗qvT+[qv]×∗[qv]×]又因为，对向量而言，[a]×∗b=a×b，所以[qv]×∗qv=qv×qv=0T则[q]L∗[q∗]R=[∣∣q∣∣200Tqv∗qvT+qw2I+2∗qw∗[qv]×+[qv]×∗[qv]×]
所以，综上所述，可以得到:
[0p1]=[∣∣q∣∣20T0qv∗qvT+qw2I+2∗qw∗[qv]×+[qv]×∗[qv]×]∗[0p2]即:p1=(qv∗qvT+qw2I+2∗qw∗[qv]×+[qv]×∗[qv]×)∗p2=[qx2+qw2−qy2−qz22∗qx∗qy−2qw∗qz2∗qx∗qz+2∗qw∗qy2∗qx∗qy+2∗qw∗qzqw2+qy2−qx2−qz22∗qy∗qz−2∗qw∗qx2∗qx∗qz−2∗qw∗qy2∗qy∗qz+2∗qw∗qxqw2+qz2−qx2−qy2]\left[\begin{matrix}0\\ \bold p1 \end{matrix}\right] = \left[\begin{matrix} ||\bold {q}||^2 & \bold {0}^T \\ \bold {0} & \bold {qv}*\bold {qv}^T + qw^2\bold I + 2*qw*[\bold {qv}]_\times + [\bold {qv}]_\times*[\bold {qv}]_\times\end{matrix}\right]*\left[\begin{matrix}0\\ \bold p2 \end{matrix}\right] \\ 即: \bold p1 = ( \bold {qv}*\bold {qv}^T + qw^2\bold I + 2*qw*[\bold {qv}]_\times + [\bold {qv}]_\times*[\bold {qv}]_\times)*\bold p2 \\ =\left[\begin{matrix} qx^2 + qw^2-qy^2 - qz^2 & 2*qx*qy-2qw*qz & 2*qx*qz + 2*qw*qy \\ 2*qx*qy+2*qw*qz & qw^2+qy^2-qx^2-qz^2 & 2*qy*qz - 2*qw*qx \\ 2*qx*qz-2*qw*qy & 2*qy*qz+2*qw*qx & qw^2+qz^2-qx^2-qy^2\end{matrix}\right] [0p1]=[∣∣q∣∣200Tqv∗qvT+qw2I+2∗qw∗[qv]×+[qv]×∗[qv]×]∗[0p2]即:p1=(qv∗qvT+qw2I+2∗qw∗[qv]×+[qv]×∗[qv]×)∗p2=⎣⎡qx2+qw2−qy2−qz22∗qx∗qy+2∗qw∗qz2∗qx∗qz−2∗qw∗qy2∗qx∗qy−2qw∗qzqw2+qy2−qx2−qz22∗qy∗qz+2∗qw∗qx2∗qx∗qz+2∗qw∗qy2∗qy∗qz−2∗qw∗qxqw2+qz2−qx2−qy2⎦⎤
完成了上面的推导之后，就可以根据四元数转换到旋转矩阵了。

旋转矩阵到欧拉角

讲解完了上面的内容，接下来就是怎么根据旋转矩阵转换出欧拉角了，正如前面所言，SLAM中常用的其实是内旋的形式，正如我们对旋转矩阵求偏导的时候会采用右扰动的方式一样，也就是说，假如相机/Lidar坐标系经过三次旋转之后，其方位(不考虑平移)能够转到和世界坐标系重合，这三次旋转分别为Rz(α),Ry(β),Rx(γ)Rz(\alpha),Ry(\beta),Rx(\gamma)Rz(α),Ry(β),Rx(γ)，则旋转矩阵表示的形式为：
R=[cos(α)−sin(α)0sin(α)cos(α)0001]∗[cos(β)0sin(β)010−sin(β)0cos(β)]∗[1000cos(γ)−sin(γ)0sin(γ)cos(γ)]=[cos(α)cos(β)−sin(α)cos(γ)+cos(α)sin(β)sin(γ)sin(α)sin(γ)+cos(α)sin(β)cos(γ)sin(α)cos(β)cos(α)cos(γ)+sin(α)sin(β)sin(γ)−cos(α)sin(γ)+sin(α)sin(β)cos(γ)−sin(β)cos(β)sin(γ)cos(β)cos(γ)]=R=[r00r01r02r10r11r12r20r21r22]R = \left[\begin{matrix} cos(\alpha) & -sin(\alpha) & 0 \\ sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 1 \end{matrix}\right]* \left[\begin{matrix} cos(\beta) & 0 & sin(\beta) \\ 0 & 1 & 0 \\ -sin(\beta) & 0 & cos(\beta) \end{matrix}\right]* \left[\begin{matrix} 1 & 0 & 0 \\ 0 & cos(\gamma) & -sin(\gamma)\\ 0 & sin(\gamma) & cos(\gamma) \end{matrix}\right] \\ = \left[\begin{matrix} cos(\alpha)cos(\beta) & -sin(\alpha)cos(\gamma)+cos(\alpha)sin(\beta)sin(\gamma) & sin(\alpha)sin(\gamma)+cos(\alpha)sin(\beta)cos(\gamma) \\ sin(\alpha)cos(\beta) & cos(\alpha)cos(\gamma)+sin(\alpha)sin(\beta)sin(\gamma) & -cos(\alpha)sin(\gamma) + sin(\alpha)sin(\beta)cos(\gamma)\\ -sin(\beta) & cos(\beta)sin(\gamma) & cos(\beta)cos(\gamma)\end{matrix}\right] \\=R=\left[\begin{matrix} r00 & r01 & r02 \\ r10 & r11 & r12 \\ r20 & r21 & r22\end{matrix} \right] R=⎣⎡cos(α)sin(α)0−sin(α)cos(α)0001⎦⎤∗⎣⎡cos(β)0−sin(β)010sin(β)0cos(β)⎦⎤∗⎣⎡1000cos(γ)sin(γ)0−sin(γ)cos(γ)⎦⎤=⎣⎡cos(α)cos(β)sin(α)cos(β)−sin(β)−sin(α)cos(γ)+cos(α)sin(β)sin(γ)cos(α)cos(γ)+sin(α)sin(β)sin(γ)cos(β)sin(γ)sin(α)sin(γ)+cos(α)sin(β)cos(γ)−cos(α)sin(γ)+sin(α)sin(β)cos(γ)cos(β)cos(γ)⎦⎤=R=⎣⎡r00r10r20r01r11r21r02r12r22⎦⎤
由此可得到，在内旋，Z-Y-X的旋转顺序下对应欧拉角所构成的旋转矩阵是上面的这个样子，要想反解出对应的角度，只要对矩阵做一定的操作即可：
sy=sqrt(r21∗r21+r22∗r22)γ=atan2(r21,r22)β=atan2(−r20,sy)α=atan2(r10,r00)sy=sqrt(r21*r21+r22*r22) \\ \gamma = atan2(r21,r22) \\ \beta = atan2(-r20,sy) \\ \alpha = atan2(r10,r00) sy=sqrt(r21∗r21+r22∗r22)γ=atan2(r21,r22)β=atan2(−r20,sy)α=atan2(r10,r00)
这样，就可以得到三轴的欧拉角为(γ,β,α)(\gamma,\beta,\alpha)(γ,β,α)
但是，上面的计算是不考虑了Gimbol Lock(万向锁)的情况的，具体Gimbol Lock是啥这里不做详细的解释，一句话概括，就是假如第二次旋转的角度为+90度或-90度的时候，会出现绕第三个轴旋转和绕第一个轴旋转的效果是一样的情况，也就是少了一个自由度，这个大家可以用自己的右手做一个测试即可，设食指朝前是x轴，中指朝左是y轴，大拇指朝上是z轴，当先绕z轴旋转一定水平角度α\alphaα之后，再绕此时的中指旋转90度，会惊奇的发现，食指和原来的大拇指方向重合了，这个时候，再去绕食指转动γ\gammaγ，实际上相当于一开始就绕z轴旋转α+γ\alpha + \gammaα+γ角度，然后再绕此时的中指旋转90度，所以说是少了一个自由度。
从矩阵上看，还是以Z-Y-X的旋转顺序，就是第二角度β=90\beta = 90β=90，那么cos⁡β=0,sinβ=1\cos\beta = 0,sin\beta = 1cosβ=0,sinβ=1，这个时候，再去反解算欧拉角的时候，就会出现0/0的情况，这是明显不对的，所以需要再做个判断，
由于在计算机中，都是浮点数，所以基本上不会完全等于0，一般认为a<ϵa<\epsilona<ϵ的时候就认为是0了，所以假如前面的sy < 1e-6之类的，就可以认为是遇到了万向锁了，这个时候，其实对角度解算就没有太大意义了，因为有两个轴α,γ\alpha,\gammaα,γ在同一个方向上了，可以假定其中一个为0，然后再带回去算就好了，完整的判断如下：
sy=sqrt(r21∗r21+r22∗r22)ifsy<1e−6:γ=atan2(r01,r02)β=atan2(−r20,sy)α=0.0esleγ=atan2(r21,r22)β=atan2(−r20,sy)α=atan2(r10,r00)sy=sqrt(r21*r21+r22*r22) \\ if \quad sy <1e-6: \\ \quad \quad \quad \gamma = atan2(r01,r02) \\ \quad \quad \quad \beta = atan2(-r20,sy) \\ \quad \quad \quad \alpha = 0.0 \\esle \quad \quad \quad\quad \quad \quad \\ \quad \quad \quad \gamma = atan2(r21,r22) \\ \quad \quad \quad \beta = atan2(-r20,sy) \\ \quad \quad \quad \alpha = atan2(r10,r00) sy=sqrt(r21∗r21+r22∗r22)ifsy<1e−6:γ=atan2(r01,r02)β=atan2(−r20,sy)α=0.0esleγ=atan2(r21,r22)β=atan2(−r20,sy)α=atan2(r10,r00)
最后得到的结果就是上面所述的情况了，至于其它的旋转顺序，同样可以这么分解，先通过内旋的形式把对应欧拉角的旋转矩阵乘起来，然后再和四元数推出来的旋转矩阵对比，注意一下Gimbol Lock的情况，就可以解出来了，大家可以用在线转换工具测试一下

需要注意的是：以上解法只适用于内旋的反解，目前好像大多的库比如Eigen,Ros tf 都是这么解的，根据旋转矩阵解外旋欧拉角还不清楚有哪些，但是外旋和内旋满足：
(Riz(α)∗Riy(β)∗Rix(γ))−1=(Rex(γ)∗Rey(β)∗Rez(α))其中Ri表示内旋，Re表示外旋，且:Riz(α)=[cos(α)−sin(α)0sin(α)cos(α)0001]Rez(α)=[cos(α)sin(α)0−sin(α)cos(α)0001](Riz(\alpha)*Riy(\beta)*Rix(\gamma))^{-1} =(Rex(\gamma)*Rey(\beta)*Rez(\alpha)) \\其中Ri表示内旋，Re表示外旋，且: \\ Riz(\alpha)=\left[\begin{matrix} cos(\alpha) & -sin(\alpha) & 0 \\ sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 1 \end{matrix}\right] \\Rez(\alpha)=\left[\begin{matrix} cos(\alpha) & sin(\alpha) & 0 \\ -sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 1 \end{matrix}\right] (Riz(α)∗Riy(β)∗Rix(γ))−1=(Rex(γ)∗Rey(β)∗Rez(α))其中Ri表示内旋，Re表示外旋，且:Riz(α)=⎣⎡cos(α)sin(α)0−sin(α)cos(α)0001⎦⎤Rez(α)=⎣⎡cos(α)−sin(α)0sin(α)cos(α)0001⎦⎤

参考

特别感谢以下参考，其实还看了很多，记不全了，如有遗漏，也请指出！
<< Quaternion kinematics for the error-state Kalman filter >>
https://en.wikipedia.org/wiki/Euler_angles
https://blog.csdn.net/qq_38288618/article/details/77195271
https://quaternions.online