旋转向量
从上一篇中已经知道,旋转可以用旋转矩阵来表示,变换可以用变换矩阵来表示,那么为什么还需要旋转向量呢?
仔细想一下,矩阵表示方式至少有以下几个缺点:
- 的旋转矩阵有9个量,但是一次旋转只有3个自由度,因此这种表达方式是冗余的。同理,变换矩阵用16个量来表示6自由度的变换也是冗余的。我们需要一种更紧凑的表达方式。
- 旋转矩阵自身带有约束:它必须是个正交矩阵,且行列式为1。变换矩阵也是如此。当想要估计或者优化一个旋转矩阵/变换矩阵时,这些约束会使得求解变得十分困难。
综合上面几点原因,我们希望找到一种更紧凑的方式来表达旋转和平移。
通过前面的学习,我们已经知道了可以用外积来表示两个向量间的旋转。
顺着这个思路下去,看一下如何用一个三维向量来表示旋转?
我们知道,任意一个旋转都可以用一个旋转轴和一个旋转角来刻画。于是,我们可以使用一个向量,其方向和旋转轴一致,长度等于旋转角。这种向量就称为旋转向量。通过这种表达方式,通过一个三维向量就可以表示旋转了。
同理,对于变换矩阵,我们使用一个旋转向量+平移向量即可表达一次变换。
思路有了,剩下的问题就是旋转矩阵和旋转向量之间是如何转换的?
从旋转向量到旋转矩阵的转换过程由Rodrigues's Formula表示:。(符号^是从向量到反对称矩阵的转换符,前面已经说明过这样的转换方式)。通过这个公式,由一个旋转向量和旋转角就可以得到旋转矩阵了。
反之,我们也可以计算从一个旋转矩阵到旋转向量的转换。
对于旋转角: |
对于旋转轴: 我们知道,旋转轴上的向量经过旋转之后不改变,说明 因此,转轴是矩阵R的关于特征值1的特征向量 求解矩阵方程,然后归一化,就得到了旋转轴。 |
欧拉角
无论是旋转矩阵还是旋转向量,它们虽然能够描述旋转,但是对我们人类是非常不直观的。当我们看到一个旋转矩阵或者旋转向量时,很难想象出这个旋转究竟是什么样的。当它们变换时,我们也不知道物体是朝哪个方向转动。
欧拉角提供了一种非常直观的方式来描述旋转---它使用了三个分离的转角,把一个旋转分解成3次绕不同轴的旋转。注:欧拉角的分解方式有很多种,因此欧拉角也会有不同的定义方法,但是思想都是一样的。
下面介绍一种比较常用的欧拉角:用 偏航角 - 俯仰角 - 翻滚角(yaw - pitch - roll)三个角度来描述一个旋转。
由于它等价于ZYX轴的旋转,因此就以ZYX为例。
假设一个刚体的前方(朝向我们的方向)为X轴,右侧为Y轴,上方为Z轴,如下图所示:
那么,ZYX转角相当于把任意旋转分解成以下三个轴上的转角:
1、绕物体的Z轴旋转,得到偏航角yaw
2、绕旋转之后的Y轴旋转,得到俯仰角pitch
3、绕旋转之后的X轴旋转,得到翻滚角roll
此时,可以使用这样一个三维的向量来描述任意旋转。这个向量是非常直观的,我们可以从这个向量中想象出旋转的过程。
其他定义的欧拉角也是通过这种方式,把旋转分解到3个轴上,得到一个三维向量,只是选用的轴和顺序不同。
欧拉角存在一个重大的缺点:著名的万向锁问题
可以看一下这里,帮助理解什么是万向锁。
可以证明:只要想用3个实数来表达三维旋转时,都会不可避免的碰到万向锁问题。因此很少在SLAM程序中直接使用欧拉角来表达姿态。
四元数
单位四元数(unit quaternion)可以用于表示三维空间里的旋转。它与常用的另外两种表示方式(三维正交矩阵和欧拉角)是等价的,但是避免了欧拉角表示法中的万向锁问题,比起三维正交矩阵表示,四元数表示能够更方便的给出旋转的转轴和旋转角。
暂时先不管四元数的含义,先来看看四元数的形式:
一个四元数拥有1个实部和3个虚部,像下面这样:,其中i、j、k为四元数的3个虚部,这三个虚部满足:
由于四元数的这种特殊的表示形式,有时也会有人用一个标量和一个向量来表达四元数:
如果一个四元数的实部为0,则将它称为虚四元数(纯四元数);如果一个四元数的虚部为0,则将它称为实四元数。
四元数的含义(关于四元数的更多资料可以看这里)
四元数可以用来表示三维空间里的点,也可以用来表示三维空间的旋转
1、四元数表示三维空间中的点
若三维空间中的一个点的笛卡尔坐标为,则用纯四元数表示为:
2、单位四元数表示一个三维空间旋转
设 q 为一个单位四元数,而 p 是一个纯四元数,定义,
则 Rq(p) 也是一个纯四元数,可以证明 Rq 确实表示一个旋转,这个旋转将空间的点 p 旋转为空间的另一个点 Rq(p)。
四元数表示旋转
用单元四元数表示旋转和用正交矩阵表示旋转是等价的,这可以通过直接的代数计算得到。
1、从旋转向量到四元数的转换
假设某个旋转是绕单位向量进行了角度为的旋转,那么这个旋转的四元数形式为
如果写成向量的形式就是:
2、从四元数到旋转轴、旋转角的转换
假设某个旋转用单位四元数表示,可以通过下列公式计算出旋转轴和夹角:
3、用四元数表示旋转
上面已经介绍了四元数和旋转轴、旋转角之间的相互转换,那么如果已经知道了点p,以及旋转q(用单位四元数表示的),如果计算旋转后得到的点的坐标呢?
假设一个空间三维点,以及一个由轴角指定的旋转,它们之间的关系可以用下列式子来表达:
- 首先,把三维空间点用一个纯四元数来描述:
- 然后,用四元数来表示旋转: (用上面的1中给出的公式计算得到)
- 旋转之后的点可以表示为:
可以验证,计算结果的实部为0,也就是一个纯四元数。虚部的3个分量表示旋转后的点的坐标。
四元数和旋转矩阵之间的转换
这一部分有较多的公式的推导,暂时先不看了。先对整体框架有个完整的了解之后在补把。
做个标记,书本的55页。
实践部分:Eigen几何模块
现在,我们通过编程在Eigen中使用四元数、欧拉角和旋转矩阵,演示它们之间的变换方式。
这里,要接触到Eigen中的一个新的模块Geometry,Eigen/Geometry模块提供了各种旋转和平移的表示。
#include <iostream>
#include "Eigen/Geometry"
using namespace std;
int main(int argc, char **argv)
{
//使用Matrix3d定义一个旋转矩阵
Eigen::Matrix3d rotation_matrix=Eigen::Matrix3d::Identity(); //Eigen::Matrix3d::Identity()返回一个单位矩阵,不一定是方阵
//使用AngleAxisd来定义一个旋转向量,它底层不直接是Matrix,但是可以当做矩阵来进行运算(因为重载了运算符)
Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1));
cout.precision(3); //输出时,小数点之后保留3位小数
//将旋转向量转换为旋转矩阵输出
cout<<"rotation_vector ---> rotation_matrix"<<endl;
cout<<rotation_vector.matrix()<<endl;
//也可以通过调用toRotationMatrix()方法转换为旋转矩阵直接赋值
rotation_matrix=rotation_vector.toRotationMatrix();
cout<<"rotation_vector ---> rotation_matrix"<<endl;
cout<<rotation_matrix<<endl;
//用旋转向量进行坐标转换
Eigen::Vector3d v(1, 0, 0);
Eigen::Vector3d v_ratated=rotation_vector*v;
cout<<"made a rotation by rotation_vector:"<<endl;
cout<<"(1, 0, 0) after rotation = "<<v_ratated.transpose()<<endl;
//用旋转矩阵来进行旋转
v_ratated=rotation_matrix*v;
cout<<"made a rotation by rotation_matrix:"<<endl;
cout<<"(1, 0, 0) after rotation = "<<v_ratated.transpose()<<endl;
//欧拉角:将旋转矩阵直接转换为欧拉角
Eigen::Vector3d euler_angles=rotation_matrix.eulerAngles(2,1,0); //参数2,1,0表示安装ZYX顺序,即yaw,pitch,roll顺序
cout<<"rotation_matrix ---> eulerAngles:"<<endl;
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
//欧式变换矩阵使用Eigen::Isometry
Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); //虽然是3d,实际上是4*4矩阵
cout<<"Transform matrix :"<<endl;
T.rotate(rotation_vector); //进行旋转
cout<<"Transform matrix = \n"<<T.matrix()<<endl;
T.pretranslate(Eigen::Vector3d(1, 3, 4)); //进行平移
cout<<"Transform matrix = \n"<<T.matrix()<<endl;
//用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v; //这里的变换包括了上面的两种:旋转和平移
cout<<"v transformed = "<<v_transformed.transpose()<<endl;
//四元数的使用:可以直接把旋转向量赋值给四元数,反之亦然
Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);
cout<<"quaternion = \n"<<q.coeffs()<<endl; //coeffs的顺序为(x,y,z,w),前三者为虚部,w为实部
//也可以把旋转矩阵赋值给四元数
q=Eigen::Quaterniond(rotation_matrix);
cout<<"quaternion = \n"<<q.coeffs()<<endl;
//使用四元数来旋转一个向量
v_ratated=q*v;
cout<<"(1, 0, 0) after rotation by quaternion = "<<v_ratated.transpose()<<endl;
return 0;
}
运行该程序可以发现:通过旋转向量、旋转矩阵、四元数来表示旋转的计算结果是相同的,也就印证了上面讲的:这三种方式是等价的。
第三讲学习到这里,明天开始学习第四讲:李群与李代数。