第3章 矩阵旋转
一、概述
-
旋转变换的核心思想
在不同坐标系下，虽然坐标不同，但是同一个向量还是一样的。这句话有点儿怪怪的，但是可以用数学公式表出：\(\beta_1^T\cdot\alpha_1=\beta_2^T\cdot\alpha_2\)，其中\(\beta\)是不同坐标系的标准正交基（行分块），\(\alpha\)是不同坐标系下的坐标（列向量）。
-
旋转变换的五种表述
- 旋转矩阵；
- 欧式矩阵；
- 旋转向量；
- 欧拉角；
- 四元数；
-
旋转变换表述的演替
- 旋转矩阵和平移矩阵：有小尾巴累积（非线性）
- 欧式矩阵：n+1维方阵要\((n+1)^2\)个自由度，太多了（线性，但不紧凑且不直观）
- 旋转向量：这是什么呀这一堆数？！看不懂！（紧凑但不直观）
- 欧拉角：这会看懂了…等等，这转个90\(^\circ\)咋就膈屁了呢？！（紧凑直观但奇异）
- 四元数：爱咋转咋转…等等不对！咋1个\(R\)冒俩\(q\)呢？\(q\)咋还内讧了呢？（紧凑非奇异，但不唯一且不稳定）
-
在Eigen库中它们四个大哥（欧式矩阵对不起，现在我们只考虑旋转）的转换关系
旋转向量和四元数先初始化（默认定义为‘单位阵’，不能赋值为nullptr或者直接使用）！！！
-
旋转矩阵
-
初始化旋转矩阵
Eigen::Matrix3d rotation_matrix; // 通过标准输入设备（标准输入流）键入赋值 rotation_matrix << x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22; -
旋转矩阵 \(\Longrightarrow\) 旋转向量
// 第一种：通过构造函数（传入一个旋转矩阵） Eigen::AngleAxisd rotation_vector(rotation_matrix); // 第二种：首先初始化，然后通过旋转矩阵直接赋值（重载了赋值运算符） Eigen::AngleAxisd rotation_vector; rotation_vector = rotation_matrix; // 第三种：首先初始化，然后from函数直接作用于this对象（rotation_vector） Eigen::AngleAxisd rotation_vector; rotation_vector.fromRotationMatrix(rotation_matrix); -
旋转矩阵 \(\Longrightarrow\) 欧拉角
// (2, 1, 0)表示旋转顺序ZYX，数字越小表示优先级越高 Eigen::Vector3d euler_angle = rotation_matrix.eulerAngles(2, 1, 0); -
旋转矩阵 \(\Longrightarrow\) 四元数
// 第一种：通过构造函数（传入一个旋转矩阵） Eigen::Quaterniond quaternion(rotation_matrix); // 第二种：首先初始化，然后通过旋转矩阵直接赋值（重载了赋值运算符） Eigen::Quaterniond quaternion; quaternion = rotation_matrix;
-
-
旋转向量
-
初始化旋转向量
// 通过构造函数 Eigen::AngleAxisd rotation_vector(alpha, Vector3d(x,y,z)); -
旋转向量 \(\Longrightarrow\) 旋转矩阵
// 第一种方法：通过构造方法传入旋转向量 Eigen::Matrix3d rotation_matrix(rotation_vector); // 第二种方法：首先初始化，然后通过旋转向量直接赋值（重载了赋值运算符） Eigen::Matrix3d rotation_matrix; rotation_matrix = rotation_vector; // 第三种方法：通过matrix方法 Eigen::Matrix3d rotation_matrix = rotation_vector.matrix(); // 第四种方法：通过toRotationMatrix方法 Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix(); -
旋转向量 \(\Longrightarrow\) 欧拉角
// 不能直接转换，需要通过旋转矩阵搭桥 Eigen::Vector3d euler_angles = rotation_vector.matrix().eulerAngles(2, 1, 0); -
旋转向量 \(\Longrightarrow\) 四元数
// 第一种方法：通过构造函数传入旋转向量 Eigen::Quaterniond quaterniond(rotation_vector); // 第二种方法：首先初始化，然后用旋转向量赋值 Eigen::Quaterniond quaterniond; quaterniond = rotation_vector;
-
-
欧拉角
-
初始化欧拉角
Eigen::Vector3d euler_angles(yaw, pitch, roll); -
欧拉角 \(\Longrightarrow\) 旋转矩阵
// 初始化三个旋转角的旋转向量 Eigen::AngleAxisd rollAngle(AngleAxisd(euler_angles(2),Eigen::Vector3d::UnitX())); Eigen::AngleAxisd pitchAngle(AngleAxisd(euler_angles(1),Eigen::Vector3d::UnitY())); Eigen::AngleAxisd yawAngle(AngleAxisd(euler_angles(0),Eigen::Vector3d::UnitZ())); // 先初始化旋转矩阵为单位矩阵，然后这三个旋转向量相乘得到旋转矩阵（运算符重载） Eigen::Matrix3d rotation_matrix; rotation_matrix = yawAngle * pitchAngle * rollAngle; -
欧拉角 \(\Longrightarrow\) 旋转向量
// 初始化三个旋转角的旋转向量 Eigen::AngleAxisd rollAngle(AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitX())); Eigen::AngleAxisd pitchAngle(AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY())); Eigen::AngleAxisd yawAngle(AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitZ())); // 先初始化旋转向量，然后这三个旋转向量相乘得到旋转向量（运算符重载） Eigen::AngleAxisd rotation_vector; rotation_vector = yawAngle * pitchAngle * rollAngle; -
欧拉角 \(\Longrightarrow\) 四元数
// 初始化三个旋转角的旋转向量 Eigen::AngleAxisd rollAngle(AngleAxisd(euler_angles(2),Eigen::Vector3d::UnitX())); Eigen::AngleAxisd pitchAngle(AngleAxisd(euler_angles(1),Eigen::Vector3d::UnitY())); Eigen::AngleAxisd yawAngle(AngleAxisd(euler_angles(0),Eigen::Vector3d::UnitZ())); // 先初始化四元数，然后这三个旋转向量相乘得到旋转向量（运算符重载） Eigen::Quaterniond quaterniond; quaterniond = yawAngle * pitchAngle * rollAngle;
-
-
四元数
-
初始化四元数
Eigen::Quaterniond quaterniond(w, x, y, z); -
四元数 \(\Longrightarrow\) 旋转矩阵
// 第一种方法：通过构造方法传入四元数 Eigen::Matrix3d rotation_matrix(quaterniond); // 第二种方法：首先初始化，然后通过四元数直接赋值（重载了赋值运算符） Eigen::Matrix3d rotation_matrix; rotation_matrix = quaterniond; // 第三种方法：通过matrix方法 Eigen::Matrix3d rotation_matrix = quaterniond.matrix(); // 第四种方法：通过toRotationMatrix方法 Eigen::Matrix3d rotation_matrix = quaterniond.toRotationMatrix(); -
四元数 \(\Longrightarrow\) 旋转向量
// 第一种方法：通过构造函数传入一个四元数 Eigen::AngleAxisd rotation_vector(quaterniond); // 第二种方法：通过四元数直接赋值（运算符重载） Eigen::AngleAxisd rotation_vector; rotation_vector = quaterniond; -
四元数 \(\Longrightarrow\) 欧拉角
// 不能直接转换，需要靠旋转矩阵搭桥 Eigen::Vector3d euler_angles = quaterniond.matrix().eulerAngles(2, 1, 0);
-
-
在Eigen中的转换——总结篇
- 旋转矩阵到旋转向量的FRM()方法是fromRotationMatrix()；
- 四元数和旋转向量到旋转矩阵用的同一套体系，其中TRM()方法是toRotationMatrix()；
- 只有旋转矩阵才能直接转换为欧拉角，其EA()方法为eulerAngles()；
- 欧拉角转换成其他旋转表述形式用的同一套体系：RPY相乘。先初始化三个旋转角（RPY）的旋转向量，然后初始化所需旋转表述形式，最后这三个旋转向量相乘得到相应旋转表述形式（运算符重载）；
-
-
旋转表述的使用
-
旋转矩阵
Eigen::Vector3d v( 1,0,0 ); v_rotated = rotation_matrix * v; -
欧式矩阵
Eigen::Vector3d v( 1,0,0 ); Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 为欧式矩阵设置旋转矩阵 T.rotate(rotation_vector); // 为欧式矩阵设置平移矩阵 T.pretranslate(Eigen::Vector3d(1, 3, 4)); Eigen::Vector3d v_transformed = T * v; -
旋转向量
Eigen::Vector3d v( 1,0,0 ); Eigen::Vector3d v_rotated = rotation_vector * v; -
欧拉角
Eigen::Vector3d v( 1,0,0 ); Eigen::Vector3d euler_angles(M_PI / 4, M_PI / 4, M_PI / 4); // 通过上述转换：rotation_matrix !!! Eigen::Vector3d v_rotated = rotation_matrix * v; -
四元数
Eigen::Vector3d v( 1,0,0 ); Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector); // 注意数学上的表达式是：qvq^{-1} Eigen::Vector3d v_rotated = q * v;
-
二、详述
-
旋转矩阵
-
旋转矩阵的定义
\[\begin{aligned} &由旋转的本质方程：\beta_1^T\alpha_1=\beta_2^T\alpha_2， 又由于\beta是标准正交基，所以\beta\beta^T = E； \\ &所以两边同时乘上\beta_1，故而可得\alpha_1=\beta_1\beta_2^T\alpha_2，记旋转矩阵R=\beta_1\beta_2^T； \end{aligned} \] -
旋转矩阵各个参数的意义
\(\beta\)是标准正交基，\(\alpha\)是相应坐标系下的坐标。
-
旋转矩阵各个参数的计算
\(R=\beta_1\beta_2^T\)。
-
-
欧式矩阵
-
欧式矩阵的定义
\[T = \left[ \begin{matrix} R&t\\ \it{0}^T&1 \end{matrix} \right] \] -
欧式矩阵各个参数的意义
\(R\)是旋转矩阵，\(t\)是平移向量，\(\it{0}^T\)是0列向量。
-
欧式矩阵各个参数的计算
不用计算，直接就有！！！
-
-
旋转向量
-
旋转向量的定义
\[\overrightarrow{n}与旋角\theta \] -
旋转向量各个参数的意义
任何一个向量（或称为点）【1】的旋转都是绕着一个特定的轴来旋转，我们可以用这个轴的长度保存旋转角的大小\(\theta\)。故而旋转角被定义为：\(\theta\overrightarrow{n}\)。
【注】【1】：这里本来是坐标系的旋转，但是我们用相对的眼光看问题，我们如果聚焦于坐标系的话就相当与是向量在旋转。一个向量绕着一个轴在转可能比坐标系绕着一个轴在转好理解一点，这俩本质一样。
-
旋转向量各个参数的计算
-
旋转轴\(\overrightarrow{n}\)的计算
旋转轴在旋转的时候是不会变化的，所以有：\(R\overrightarrow{n}=\overrightarrow{n}\)，即有\(\overrightarrow{n}\)为\(R\)的特征值为1的特征向量。
-
旋转角\(\theta\)的计算
罗德格里斯指出了旋转向量到旋转矩阵的法则：\(R=\cos{\theta}I+(1-\cos{\theta})\overrightarrow{n}\overrightarrow{n}^T+\sin{\theta}\overrightarrow{n}^{\wedge}\)。
同时取迹可得：\(\mathbf{tr}(R)=1+2\cos{\theta}\)。所以就计算出了\(\theta=\arccos{\frac{\mathbf{tr}(R)-1}{2}}\)。
-
-
-
欧拉角
-
欧拉角的定义
每个轴旋转一个特定的角度，但是有顺序要求，我们一般使用ZYX的顺序（称为RPY）。
-
欧拉角各个参数的意义
-
R：Roll，偏航角
-
P：Pitch，翻滚角
-
Y：Yaw，俯仰角
-
-
欧拉角各个参数的计算
通过传感器或者人为给出。不是吧不是吧，不会真有人用欧拉角吧？！【1】
【注】【1】：万向锁问题（奇异性）问题——只要我们想用3个实数来表达3维旋转时，都会不可避免地碰到奇异性问题。所以很少用这样的旋转表述方式，一般用也只是用于人机交互中传入旋转角度，或者验证系统的算法，因为这样的表述对于人类来说是非常直观的。
-
-
四元数
-
四元数的定义
\[q=(s，\overrightarrow{v})^{T}=(s，x，y，z)^{T}=s+xi+yj+zk \] -
四元数各个参数的意义
-
实部\(s\)表示旋转程度：\(s=f(\theta)\)；
-
虚部\(\overrightarrow{v}\)表示旋转轴：\(\overrightarrow{v}=k\overrightarrow{n}\)；
虚部\(\overrightarrow{v}\)的定义为某个点在三维直角系下的坐标，由于四元数表示对一个向量（或称为点）的旋转，用数学公式可以严谨地证明，当对\(\overrightarrow{v}\)进行\(q=(s，\overrightarrow{v})^{T}\)旋转时不变，所以\(\overrightarrow{v}\)表示旋转轴。
-
-
四元数各个参数的计算（利用旋转向量）
-
实部\(s\)的计算
-
四元数 \(\Longrightarrow\) 旋转矩阵
\[\begin{aligned} R& = \overrightarrow{v}\overrightarrow{v}^{T}+s^2I+2s\overrightarrow{v}^{\wedge}+(\overrightarrow{v})^2 \\\\ \mathbf{tr}(R)&=4s^2-1 \end{aligned} \] -
旋转矩阵 \(\Longrightarrow\) 旋转向量
\[\begin{aligned} \theta& = \arccos(\frac{\mathbf{tr}(R)-1}{2})=\arccos(2s^2-1) \\\\ \theta& = 2\arccos{s} \\\\ s& = \cos{\frac{\theta}{2}} \end{aligned} \]
-
-
虚部\(\overrightarrow{v}\)的计算
-
得到旋转轴
旋转轴就是四元数的虚部\(\overrightarrow{v}\)。
-
将四元数单位化
我们已经知道了实部\(s=\cos{\frac{\theta}{2}}\)，所以虚部向量就只用除以一个\(\sin{\frac{\theta}{2}}\)就行了。
-
-
-
三、矩阵旋转的两种理解
我们先想象一个坐标系和其中的一个点（向量）。记这个坐标系为\(\Alpha\)；记这个向量和它的坐标分别为\(\overrightarrow{v}\)和\(\{p_i\}\)，其中\(\{p_i\}\)构成一个列向量\(\overrightarrow{p}\)。此时由于旋转因子\(R\)，使得\(\Alpha\)旋转至\(\Beta\)；\(\overrightarrow{v}\)旋转到了\(\overrightarrow{v_1}\)，\(\overrightarrow{v_1}\)的坐标\(\{p_j\}\)构成列向量\(\overrightarrow{p_1}\)，某个\(\overrightarrow{v_2}\)旋转到了\(\overrightarrow{v}\)，\(\overrightarrow{v_2}\)的坐标\(\{p_k\}\)构成列向量\(\overrightarrow{p_2}\)；\(\overrightarrow{v}\)在\(\Beta\)中的坐标\(\{p_{l}\}\)构成列向量\(\overrightarrow{p}^{’}\)。
描述点与基向量关系的坐标值不变（坐标系旋转）
由上述的描述，我们可以得到坐标系之间的表示关系
\[\begin{cases} \beta_1=a_1\alpha_1+a_2\alpha_2+a_3\alpha_3\\\\ \beta_2=b_1\alpha_1+b_2\alpha_2+b_3\alpha_3\\\\ \beta_3=c_1\alpha_1+c_2\alpha_2+c_3\alpha_3 \end{cases} \Longrightarrow \left[ \begin{matrix} \beta_1& \beta_2& \beta_3 \end{matrix} \right] = \left[ \begin{matrix} \alpha_1& \alpha_2& \alpha_3 \end{matrix} \right] \cdot \left[ \begin{matrix} a_1 & b_1 & c_1\\ a_2 & b_2 & c_2\\ a_3 & b_3 & c_3 \end{matrix} \right] \Longrightarrow \Beta=\Alpha\cdot{R} \]所以如果时坐标系的旋转，我们是对原坐标系右乘旋转矩阵从而得到新的坐标系。进一步的我们发现，当坐标系旋转的时候，向量\(\overrightarrow{v}\)相对坐标系静止地跟着旋转到向量\(\overrightarrow{v_1}\)，所以这一种旋转对于向量与坐标系的联系（坐标值）是固定的，它研究的是对整个系统的扰动，这个扰动不会影响坐标值。
描述向量的张成空间的基向量不变（向量的旋转）
这次旋转对于\(\overrightarrow{v}\)向量来说，在不同坐标系中的表示分别是\(\Alpha\cdot\overrightarrow{p}\)和\(\Beta\cdot\overrightarrow{p}^{’}\)，由上述结论可知坐标系之间的关系为\(\Beta=\Alpha\cdot{R}\)，所以有\(\Beta\cdot\overrightarrow{p}^{’}=\Alpha\cdot{R}\cdot\overrightarrow{p}^{’}\)。又由于是同一个向量，所以在同一个基向量下坐标应该是相同的，故而有\(\overrightarrow{p}={R}\cdot\overrightarrow{p}^{’}\)。
进一步的我们发现，在\(\Beta\)中的\(\overrightarrow{p}^{’}\)就是\(\Alpha\)中的\(\overrightarrow{p_2}\)，因为在坐标系旋转的时候，向量\(\overrightarrow{v_2}\)相对坐标系静止地跟着旋转到向量\(\overrightarrow{v}\)，所以\(\Beta\)与\(\overrightarrow{v}\)的相对位置就是\(\Alpha\)与\(\overrightarrow{v_2}\)的相对位置。故而有\(\overrightarrow{p}={R}\cdot\overrightarrow{p}^{’}={R}\cdot\overrightarrow{p_2}\)，这样都是在\(\Alpha\)中的表述，所以可以定义为：在\(\Alpha\)中向量\(\overrightarrow{v_2}\)经\(R\)作用旋转到向量\(\overrightarrow{v}\)。
所以当我们描述向量的旋转时，我们是对新坐标值左乘旋转矩阵从而得到原坐标值。
