03_三维空间刚体运动（下）
旋转向量
-
自由度：在统计学中，自由度（英语：degree of freedom, df）是指当以样本的统计量来估计总体的参数时，样本中独立或能自由变化的数据的个数，称为该统计量的自由度。
- 举例：若存在两个变量a、b，而 a+b=6那么他的自由度为1。因为其实只有a才能真正的自由变化，b会被a选值的不同所限制。
- 矩阵的自由度是矩阵所包含的可变参数的个数，即它做变换的角度参量个数。
-
矩阵表示旋转存在的问题
- 旋转矩阵有9个量，但一次旋转只有三个自由度。
- 旋转矩阵自身带有约束，旋转矩阵必须是正交矩阵且行列式为1。
-
由于旋转矩阵存在以上问题，因此这里我们引出了旋转向量。任意旋转都可以用一个旋转轴和一个旋转角来刻画。我们来举个例子，如图所示：
\(a\)旋转到向量\(b\)，\(a\)是以w为轴，旋转了一定的角度，得到向量\(b\)。
-
旋转向量和旋转矩阵的关系
-
旋转向量->旋转矩阵（罗德里格斯公式，Rodrigues's Formula）
\[\boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{\mathrm{T}}+\sin \theta \boldsymbol{n}^{\wedge} \] -
旋转矩阵->旋转向量
\[\begin{aligned} \operatorname{tr}(\boldsymbol{R}) &=\cos \theta \operatorname{tr}(I)+(1-\cos \theta) \operatorname{tr}\left(n n^{\mathrm{T}}\right)+\sin \theta \operatorname{tr}\left(n^{\wedge}\right) \\ &=3 \cos \theta+(1-\cos \theta) \\ &=1+2 \cos \theta \end{aligned} \]则有
\[\theta=\arccos \frac{\operatorname{tr}({R})-1}{2} \]
-
-
欧拉角
-
欧拉角对于我们来说描述旋转非常直观，它是用了三个分离的转角，把一个旋转分解成3次不同轴的旋转。我们还是用上面的例子
-
\(a\)绕w轴旋转了 \(\theta\)度，根据欧拉角的方式，那么则\(a\)先绕\(x\)轴旋转了 \(\theta_{x}\)度，然后绕\(y\)轴旋转了 \(\theta_{y}\)度，再绕\(z\)轴旋转了 \(\theta_{z}\)度，欧拉角中用 \(\left[\theta_{x}, \theta_{y}, \theta_{z}\right]\)表示这次旋转。
-
常用的旋转一般为“偏航-俯仰-滚转”角(yaw-pitch-roll)，对应的坐标轴为\(z\)\(y\)\(x\)。
-
-
欧拉角万向锁问题（Gimbal Lock）：在俯仰角为士90°时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了2次旋转)。
四元数
-
四元数
- 优点：既紧凑，又没有奇异性。
- 缺点：四元数不够直观，运算复杂一些
-
定义：
其中\(q_{0}\)为实部，\(i\)，\(j\)，\(k\)为虚部，三个虚轴维度和实轴相互垂直。三个虚部满足关系如下：
- 四元数求模
- 四元数相乘的几何意义：旋转 + 缩放，我们还是举个例子：
\(q_{2}\)会以\(q_{1}\)的大小放缩，然后在施加一种特殊的旋转。
总结： 当你看到表达式\(i \cdot j\)时，将\(i\)看作一个函数，然后扭曲了整个空间
-
四元数和旋转矩阵
-
已经四元数\(q=q_{0}+q_{1} i+q_{2} j+q_{3} k\)，求旋转矩阵\(R\)
\[\boldsymbol{R}=\left[\begin{array}{ccc} 1-2 q_{2}^{2}-2 q_{3}^{2} & 2 q_{1} q_{2}+2 q_{0} q_{3} & 2 q_{1} q_{3}-2 q_{0} q_{2} \\ 2 q_{1} q_{2}-2 q_{0} q_{3} & 1-2 q_{1}^{2}-2 q_{3}^{2} & 2 q_{2} q_{3}+2 q_{0} q_{1} \\ 2 q_{1} q_{3}+2 q_{0} q_{2} & 2 q_{2} q_{3}-2 q_{0} q_{1} & 1-2 q_{1}^{2}-2 q_{2}^{2} \end{array}\right] \] -
已知旋转矩阵\(R=m_{i j}, i, j \in[1,2,3]\)，求四元数\(q=q_{0}+q_{1} i+q_{2} j+q_{3} k\)
\[q_{0}=\frac{\sqrt{\operatorname{tr}(R)+1}}{2}, q_{1}=\frac{m_{23}-m_{32}}{4 q_{0}}, q_{2}=\frac{m_{31}-m_{13}}{4 q_{0}}, q_{3}=\frac{m_{12}-m_{21}}{4 q_{0}} . \]
-
编程实践
- 主要针对本节内容调用库中现成的接口
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
// 本程序演示了 Eigen 几何模块的使用方法
void RotationVectorAndQuaterniond()
{
Matrix3d rotation_matrix = Matrix3d::Identity();
//旋转向量
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));
//设置精度值
cout.precision(3);
//旋转向量转换成矩阵
cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;
//坐标变换
rotation_matrix = rotation_vector.toRotationMatrix();
Vector3d v(1, 0, 0);
Vector3d v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
//旋转矩阵转成欧拉角
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
cout << "(1,0,0) after rotation (by matrix) = " << euler_angles.transpose() << endl;
//欧式变换矩阵使用
Isometry3d T = Isometry3d::Identity();
T.rotate(rotation_vector);
// 把平移向量设成(1,3,4)
T.pretranslate(Vector3d(1, 3, 4));
cout << "Transform matrix = \n" << T.matrix() << endl;
//四元数
Quaterniond q = Quaterniond(rotation_vector);
// 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl;
// 旋转矩阵变换四元数
q = Quaterniond(rotation_matrix);
cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
// 注意数学上是qvq^{-1}
v_rotated = q * v;
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
// 用常规向量乘法表示，则应该如下计算
cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;
}
int main(void)
{
RotationVectorAndQuaterniond();
return 0;
}
