欧拉角转换特殊情况(万向锁等)

问题说明

旋转的表示方式有：角轴，旋转矩阵、四元数、欧拉角。具体见旋转矩阵、欧拉角、四元数及四元数插值

欧拉角表示
优点：理解友好；3个变量3个自由度
缺点：奇异性(万向锁)；同一个旋转可由多组欧拉角来表示，即一个旋转对应多组欧拉角。
这篇文章从旋转矩阵转化欧拉角来分析这些情况。
主要说明同一个旋转解析出不同欧拉角的特殊情况。

首先提以下旋转矩阵概念：
旋转矩阵根据内外旋可以写成左右乘的形式，左右乘的旋转矩阵是互相的转置。旋转矩阵既可以表征旋转运动，也可以用来表征姿态角。同理欧拉角，四元数、角轴等能够表征旋转的，都可以用来表征一个物体旋转运动后的姿态。

这里以左乘，(左前上)坐标系为例。

欧拉角到旋转矩阵

三轴欧拉角：$rx，ry，rz$
使用Y->X->Z的顺序构造成的旋转矩阵：$crx$代表$cos(rx)$, $srx$代表$sin(rx)$，其他同理。
$$R_z{R}_x{R}_y=\left[\begin{array}{ccc}crz& srz& 0\\ -srz& crz& 0\\ 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{ccc}1& 0& 0\\ 0& crx& srx\\ 0& -srx& crx\end{array}\right]\cdot \left[\begin{array}{ccc}cry& 0& -sry\\ 0& 1& 0\\ sry& 0& cry\end{array}\right]$$
$$\left[\begin{array}{ccc}crzcry+srzsrxsry& srzcrx& -crzsry+srzsrxcry\\ -srzcry+crzsrxsry& crzcrx& srzsry+crzsrxcry\\ crxsry& -srx& crxcry\end{array}\right]$$

旋转矩阵到欧拉角

欧拉角将旋转按顺序分解到三个基轴上，便于人的直观理解。但是使用欧拉角表示旋转，或者在其他旋转表述方式中恢复欧拉角时会出现特殊情况。

计算公式详见：
旋转矩阵、欧拉角、四元数及四元数插值

这里主要说明旋转矩阵中解算欧拉角的特殊情况

同一旋转矩阵对应多组欧拉角：
1、万向锁(Gimbal lock):
当中间轴(本例中为x轴)转动$\pm \pi /2$时，会发生万向锁问题，此时$R$阵对应多组欧拉角
2、$(rx,ry,rz)$与$(\pm \pi -rx,ry\pm \pi ,rz\pm \pi )$对应同一个旋转矩阵。
3、$(rx,ry,rz)$与$(rx\pm 2\pi ,ry\pm 2\pi ,rz\pm 2\pi )$

万向锁

参考链接：
推荐：
https://krasjet.github.io/quaternion/bonus_gimbal_lock.pdf
https://wang-yimu.com/gimbal-lock/

万向锁是在使用欧拉角时出现的问题，由于欧拉角多使用动态轴来定义，即围绕自身轴的旋转。因此，欧拉角需要定义三轴的旋转次序。虽然角度相同，定义的次序不同，则代表的旋转不同。
万向锁问题的主要原因是由于固定的旋转次序而引起的。
实际的物体旋转，可以归结为绕三维空间角轴的旋转，要最后的旋转过程按顺序分解为三个正交基轴的3次旋转。

使用欧拉角表述旋转，会出现万向锁情况，当第二次序旋转轴旋转$\pm \pi /2$时，会将第一次序轴转到与第三次序轴所在的方向上。因此，使绕第一次序轴和第三次序轴的旋转均是在绕3维空间中的同一轴旋转，从而欧拉角可表示的空间维度减少一维。

具体看下图(借用第一个参考链接的图，因此旋转次序这里按x-y-z的顺序)：
绕x轴旋转任意角度：

绕y轴旋转90度:

此时我们发现，新z轴和原始的x轴位于同一个空间轴，也就是说第一次旋转和第三次旋转都是绕同一个空间轴旋转，因此，此时的欧拉角只能表征空间中两个方向的旋转，一个是y轴，一个原始x轴和新z轴的旋转。旋转维度减一。此时出现万向锁。
总结就是。第二次序轴旋转是$\pm \pi /2$时，欧拉角不能表征三维的旋转。
回到我们自己的例子，分析万向锁情况下的欧拉角表征的旋转矩阵：
$$rx=\pm \pi /2$$$$R=R_z{R}_x{R}_y=\left[\begin{array}{ccc}crzcry+srzsrxsry& srzcrx& -crzsry+srzsrxcry\\ -srzcry+crzsrxsry& crzcrx& srzsry+crzsrxcry\\ crxsry& -srx& crxcry\end{array}\right]$$$$=\left[\begin{array}{ccc}crzcry\pm srzsry& 0& -crzsry\pm srzcry\\ -srzcry\pm crzsry& 0& srzsry\pm crzcry\\ 0& \mp 1& 0\end{array}\right]$$$$=\left[\begin{array}{ccc}\cos \left(rz\mp ry\right)& 0& \pm \sin \left(rz\mp ry\right)\\ -\sin \left(rz\mp ry\right)& 0& \pm \cos \left(rz\mp ry\right)\\ 0& \mp 1& 0\end{array}\right]$$
令$rz\mp ry=d$
则：原式化为：

从公式也可以得出，当中间(第二)次序轴的转动角度为$\pm \pi /2$时，此时的欧拉角只能表征两轴旋转。
此时从旋转矩阵中恢复欧拉角时。满足$rx=(\pm \pi /2),ry,rz；s.t..rz\mp ry=d$均对应同一组旋转矩阵

代码测试：

Enter the rx: 90
Enter the ry: 30
Enter the rz: 50
catch the rpy value: 90 30 50
Start to test rotation to euler
R:
0.939693 0 0.34202
-0.34202 2.38419e-07 0.939692
0 -1 2.38419e-07
euler: rx, ry, rz: 90 -20 0

---

Enter the rx: -90
Enter the ry: 30
Enter the rz: 40
catch the rpy value: -90 30 40
Start to test rotation to euler
R:
0.34202 0 -0.939692
-0.939692 1.78814e-07 -0.34202
0 1 1.78814e-07
euler: rx, ry, rz: -90 70 0

与$\pi$结合

首先从旋转矩阵、欧拉角、四元数及四元数插值中可知，将3轴按顺序绕$\pm \pi$后，坐标系回到起始状态。是否存在将三个欧拉角与$\pi$结合，而与原欧拉角所代表的旋转相同的情况。

分析旋转矩阵，当rx，ry，rz结合$\pi$对应如下的三角函数符号变化，则对应同样的旋转。

rz	rx	ry
cos(rz) 变号	cos(rx) 变号	cos(ry) 变号
sin(rz) 变号	sin(rx) 不变号	sin(ry) 变号

以下一组满足上述变号情况：

$[rx,ry,rz]->[\pm \pi -rx,ry\pm \pi ,rz\pm \pi ]$对应一组相同旋转。
$$\sin \left(\pm \pi -rx\right)=\sin \left(\alpha \right),\cos \left(\pm \pi -rx\right)=-\cos \left(\alpha \right)$$$$\sin \left(\alpha \pm \pi \right)=-\sin \left(\alpha \right),\cos \left(\alpha \pm \pi \right)=-\cos \left(\alpha \right)$$

代码测试：

Enter the rx: 90
Enter the ry: 30
Enter the rz: 50
catch the rpy value: 90 30 50
Start to test rotation to euler
R:
0.939693 0 0.34202
-0.34202 2.38419e-07 0.939692
0 -1 2.38419e-07
euler: rx, ry, rz: 90 -20 0
rx: pi-rx; ry ± pi; rz ± pi
rpy value turn to be: 180 180 -3.4565e+258
after R:
0.939693 0 0.34202
-0.34202 2.38419e-07 0.939692
0 -1 2.38419e-07
euler: rx, ry, rz: 90 -20 0

测试代码

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <math.h>

using namespace std;

//# define M_PI 3.141592653
double deg2rad = M_PI/180;
int main()
{
  double rx_orig = 0;
  double ry_orig = 0;
  double rz_orig = 0;

  cout << "Enter the rx: ";
  cin >> rx_orig;
  cout << "Enter the ry: ";
  cin >> ry_orig;
  cout << "Enter the rz: ";
  cin >> rz_orig;

  cout << "catch the rpy value: " << rx_orig << " " << ry_orig << " " << rz_orig << endl;

  cout << "******Start to test rotation to euler******" << endl;

  Eigen::AngleAxisf rxA(-rx_orig*deg2rad, Eigen::Vector3f::UnitX());
  Eigen::AngleAxisf ryA(-ry_orig*deg2rad, Eigen::Vector3f::UnitY());
  Eigen::AngleAxisf rzA(-rz_orig*deg2rad, Eigen::Vector3f::UnitZ());
  Eigen::Matrix3f R = (rzA * rxA * ryA).matrix();

  cout << "R: " << endl << R << endl;

  Eigen::Vector3f Nqr = R.eulerAngles(2,0,1);
  cout << "euler: rx, ry, rz: " << -Nqr[1]/deg2rad  << " " << -Nqr[2]/deg2rad << " "   << -Nqr[0]/deg2rad<< endl;


  cout << "rx: pi-rx; ry +- pi; rz +- pi" << endl;
  double rx = 180 - rx;
  double ry = ry + 180;
  double rz = rz - 180;

  cout << "rpy value turn to be: " << rx << " " << ry << " " << rz << endl;
  Eigen::AngleAxisf rxB(-rx_orig*deg2rad, Eigen::Vector3f::UnitX());
  Eigen::AngleAxisf ryB(-ry_orig*deg2rad, Eigen::Vector3f::UnitY());
  Eigen::AngleAxisf rzB(-rz_orig*deg2rad, Eigen::Vector3f::UnitZ());
  R = (rzB * rxB * ryB).matrix();

  cout << "after R: " << endl << R << endl;

  Nqr = R.eulerAngles(2,0,1);
  cout << "euler: rx, ry, rz: " << -Nqr[1]/deg2rad  << " " << -Nqr[2]/deg2rad << " "   << -Nqr[0]/deg2rad<< endl;


  return 0;
}

//g++ quaternion2euler.cpp `pkg-config eigen3 --libs --cflags`