旋转与平移(从数学上分析万向锁)

Rotation and translation

根据SLAM十四讲里面的内容，旋转的表达方式有如下四种，并且分别描述其使用（一般在slam中都是使用Eigen来实现的，当然也可以自己来写矩阵运算的库）。

• 旋转矩阵
• 旋转向量
• 欧拉角(gimbal lock problem exists in internal rotation and external rotation | It is a limitation of the Euler angle representation, not the rotation method itself.)
• 四元数(单位四元数才能表示旋转 | 并且计算旋转之后的坐标可以不转换成旋转矩阵，可以使用四元数本身进行计算，opencv中自己会通过重载运算符进行计算)

$$q=\left[\cos \frac{\theta }{2},w\sin \frac{\theta }{2}\right]$$

Rotation matrix

旋转矩阵就是一个3X3的矩阵，是使用9个元素来描述3自由度的旋转，并且旋转矩阵的生成、物理意义、使用中有不少需要注意的部分。

1. 生成：旋转矩阵的生成可以使用基向量、欧拉角、旋转向量生成（个人认为旋转向量与欧拉角是旋转矩阵的其他表示形式——因为如果要计算变换后的向量坐标的话，还是要转换到旋转矩阵中进行计算）。

   • 基向量：基向量是使用旋转后以及旋转前的三个轴方向向量的坐标生成一个旋转矩阵(一般是吧旋转后坐标系的基向量定义在旋转前的坐标系中)。

   • 欧拉角：欧拉角旋转分为内旋以及外旋，分别是绕自身坐标系或者固定坐标系。旋转方式不同，那么旋转矩阵的生成方式就有所不同。绕固定坐标系依次左乘，绕自身坐标系旋转依次右乘 。

   • 旋转向量：单纯使用一个模值以及单位向量表示旋转(很讨厌的一点是使用旋转向量描述旋转不够直观，而且不知道这个旋转具体是怎么形成的 | 并且旋转向量定义在哪个坐标系下，生成的旋转矩阵应该就定义在该坐标系中)。—— 旋转向量是使用一个向量就表示出了欧拉角旋转三次的效果，所以其不直观，但是简洁(思考其空间形状没有意义)。

2. 定义坐标系

   • 我个人觉得旋转矩阵是有被定义的坐标系的，即一个坐标系相当对与另一个坐标系的旋转关系是被定义在第三个坐标系中的，当然绝大部分的情况是第三个坐标系选择为其中一个坐标系,即要么是变换前的坐标系要么是变换后的坐标系。

3. 物理意义

   • 物理意义：物理意义有两种，即在一个固定坐标系下的向量旋转，以及两个坐标系之间的向量变换(这两种情况下的R矩阵的生成方式是一样的)。

4. 计算方式

   • **从w系到c系之间的旋转矩阵R_wc计算方式为：w系如何变成c系的过程，依次按照左乘或者右乘的原则生成旋转矩阵。**R_wc可以将c系中定义的向量转换到w系中，实现一种类似于退回式的效果。如果求解的是变换矩阵T_wc，平移向量t为从w移动到c系的平移向量。

---

参考链接

• 旋转矩阵的物理意义以及左乘右乘(关于坐标变换中的左右乘不够全面)
• GTSAM中有关于旋转矩阵|变换矩阵的定义
• 关于欧拉角的左右乘

---

For example:

已知一个点在世界坐标系w下的坐标，该坐标系绕x轴旋转45度，再绕y轴旋转30度，再加上平移得到机器人系c。

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#define PI 3.1415926

int main() {
    // 定义一个点在世界坐标系中的坐标P_w
    Eigen::Vector4f P_w(1.0, 2.0, 3.0, 1.0);
    
    // 定义机器人在世界坐标系中的位置t_wr（即得到从世界系到机器人系的平移关系）
    Eigen::Vector3f t_wr(4.0, 5.0, 6.0);
    
    // 构建旋转矩阵R_wc
    Eigen::Matrix3f R_1;
    R_1 <<  1,0,0,
            0,cos(45*PI/180),-sin(45*PI/180),
            0,sin(45*PI/180),cos(45*PI/180);
    Eigen::Matrix3f R_2;
    R_2 <<  cos(30*PI/180),0,sin(30*PI/180),
            0,1,0,
            -sin(30*PI/180),0,cos(30*PI/180);
    Eigen::Matrix3f R_cw = R_2*R_1;
    
    // 构建变换矩阵T_wc(R矩阵与平移都是从世界系到机器人系的 | 变换矩阵也描述一种退回关系)
    Eigen::Matrix4f T_wc;
    T_wc.setIdentity();
    T_wc.block<3,3>(0,0) = R_cw;
    T_wc.topRightCorner(3,1) = t_wr; 

    std::cout << T_wc << std::endl;
    
    // 计算结果
    Eigen::Vector4f P_c = T_wc.inverse() * P_w;
    std::cout << P_c << std::endl;
    return 0;
}

// 使用旋转向量表示：
//int main()
//{
//    Eigen::Vector4f P_w(1.0, 2.0, 3.0, 1.0);
//    Eigen::Vector3f t_wr(4.0, 5.0, 6.0);
//
//
//    float angle_x = 45.0*M_PI/180.0;
//    float angle_y = 30.0*M_PI/180.0;
//
//    Eigen::Matrix3f rotation_x;
//    Eigen::Matrix3f rotation_y;
//
//    rotation_x = Eigen::AngleAxisf(angle_x, Eigen::Vector3f::UnitX());
//    rotation_y = Eigen::AngleAxisf(angle_y, Eigen::Vector3f::UnitY());
//
//    Eigen::Matrix3f R_wc = rotation_y*rotation_x;
//
//    Eigen::Matrix4f T_wc = Eigen::Matrix4f::Identity();
//    T_wc.block<3,3>(0,0) = R_wc;
//    T_wc.block<3,1>(0,3) = t_wr;
//    Eigen::Vector4f P_c = T_wc.inverse()*P_w;
//
//    std::cout  << rotation_x << std::endl;
//    std::cout  << rotation_y << std::endl;
//
//    std::cout << "The point in the robot coordinate system is: \n" << T_wc << std::endl;
//    std::cout<<P_c<<std::endl;
//    return 0;
//
//}

Transformation matrix

变换矩阵与旋转矩阵之间没有特别大地区别，只是把旋转与平移关系都结合到一个矩阵中(这样把关系转换为线性，不过需要把三维向量坐标转变为齐次坐标)。唯一一点需要注意地是变换矩阵的逆矩阵。

1. 变换矩阵

$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$

2. 变换矩阵的逆

$$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ 0^T& 1\end{array}\right]$$

我之前有一个误区在于，把从A到B系的关系为R*P+t，反过来的关系写成了R.inverse()*P-t。但t向量本身就是B系原点在A系中的定义(该向量定义在A系中)，所以如果想把关系转换成B系到A系，t向量也得定义到B系中，即为R.inverse()*t。

• 也可以写成 $P^r=R_w^r\cdot (P^w-t)$,并且t是定义在w系下的，与上面使用变换矩阵的逆表示的关系一样

**PS：**如果想描述平移关系的话，还是逃脱不了使用矩阵来表示，旋转的描述方式可能多一点。

---

Quaternion

1. 四元数可以在opencv中通过旋转向量直接生成，即(设旋转角度为$\theta$,旋转向量为w,w为单位向量，生成的四元数为单位四元数):

$$q=\left[\cos \frac{\theta }{2},w\sin \frac{\theta }{2}\right]$$

2. 四元数关于时间导数

​ 设存在一个旋转(旋转轴为$w$,模值为$\theta$),关于时间的导数：
$$\underset{\Delta t\to 0}{\lim }\frac{q\left(t+\Delta t\right)-q\left(t\right)}{\Delta t}=\underset{\theta \to 0}{\lim }\frac{q\otimes \Delta q-q}{\theta }$$

3. 其他
   • 四元数与复数都可以使用矩阵的形式进行表示，都可以表示旋转+缩放，所以表示旋转会使用单位四元数！！(缩放大小为1)

Gimbal lock

也许在几何上，我理解不了万向锁这个问题。但是可以在数学上发现，欧拉角在一些情况下，无法被正确解算，即出现了万向锁。因为欧拉角本质上还是一种表示旋转的东西，也需要计算旋转矩阵，即存在一定的可能出现万向锁。

数学推导万向锁

设有一个坐标系，设旋转顺序为xyz，那么对于后续的旋转矩阵：内旋(按右乘生成)，外旋(按左旋生成)，可以发现如果在绕y的旋转角度为90度时，生成的R矩阵中可以转换为:

• 内旋:内旋因为是绕自身坐标系(随着旋转改变)，几何上直观。可以通过最后生成的R矩阵找到其对应的Rx与Ry，即可以说明其缺少了一维(无法并且直接求解a,b)，说明了内旋也存在着Gimbal lock
  $$\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(a\right)& -\sin \left(a\right)\\ 0& \sin \left(a\right)& \cos \left(a\right)\end{array}\right)\left(\begin{array}{ccc}0& 0& 1\\ 0& 1& 0\\ -1& 0& 0\end{array}\right)\left(\begin{array}{ccc}\cos \left(b\right)& -\sin \left(b\right)& 0\\ \sin \left(b\right)& \cos \left(b\right)& 0\\ 0& 0& 1\end{array}\right)=\left(\begin{array}{ccc}0& 0& 1\\ sin(a+b)& cos(a+b)& 0\\ -cos(a+b)& sin(a+b)& 0\end{array}\right)$$
  即
  $$\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(a+b\right)& -\sin \left(a+b\right)\\ 0& \sin \left(a+b\right)& \cos \left(a+b\right)\end{array}\right)\left(\begin{array}{ccc}0& 0& 1\\ 0& 1& 0\\ -1& 0& 0\end{array}\right)=R_x\left(a+b\right)R_y\left(\frac{\pi }{2}\right)=R$$

  ---

• 外旋:外旋因为是绕固定坐标系，所以几何上不直观。但是仍与内旋一样，可以发现也是无法直接求解出来a,b两个角度值，缺少了一维，即认为外旋也存在着Gimbal lock这种问题。
  $$\left(\begin{array}{ccc}\cos \left(b\right)& -\sin \left(b\right)& 0\\ \sin \left(b\right)& \cos \left(b\right)& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}0& 0& 1\\ 0& 1& 0\\ -1& 0& 0\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(a\right)& -\sin \left(a\right)\\ 0& \sin \left(a\right)& \cos \left(a\right)\end{array}\right)=\left(\begin{array}{ccc}0& sin(a-b)& cos(a-b)\\ 0& cos(a-b)& -\sin \left(a-b\right)\\ -1& 0& 0\end{array}\right)$$
  即
  $$\left(\begin{array}{ccc}0& 0& 1\\ 0& 1& 0\\ -1& 0& 0\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(a-b\right)& -\sin \left(a-b\right)\\ 0& \sin \left(a-b\right)& \cos \left(a-b\right)\end{array}\right)=R_y\left(\frac{\pi }{2}\right)R_x\left(a-b\right)=R$$

参考链接（这位写的非常好，我四元数公式的推导也是跟着这位巨佬学的！）
https://krasjet.github.io/quaternion/bonus_gimbal_lock.pdf