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;
}