三维空间中刚体运动

    封面是艾泽拉•洛伊女神,本文是总结<视觉SLAM14讲>所做的笔记。在前面的博文ROS-坐标转换(TF)介绍了ROS中的坐标变换通信的机制,但是没有提到坐标变换本身,因此这篇博文补充这些基本概念。

    两个坐标系之间的运动由一个旋转加上一个平移组成,若同一个向量在各个坐标系下的长度和夹角都不会发生变化,则这种运动称为刚体运动。刚体运动前后的两个坐标系相差一个欧式变换。除了欧式变换之外,还有其它的变换,汇总如下:

    欧式变换:保持向量的长度和夹角,包括旋转和平移。

    相似变换:比欧式变换多了一个缩放因子s,可以在旋转之后进行缩放。

    仿射变换:仿射变换要求A是是一个可逆矩阵,而不一定是正交矩阵。经过放射变换后,立方体可能会变成斜的,但是各个面仍然是平行四边形。

    射影变换:射影变换是最一般的变换,左上角为可逆矩阵A,右上角为平移t,左下角为缩放a。

各个变换对比:

40.jpg

    一般说的坐标变换就是欧式变换。


旋转矩阵和变换矩阵

旋转矩阵

    对于坐标变换,向量本身没有发生变换,只是坐标系变了,根据坐标的定义有:

1.jpg

    对上式左右两边同时左乘[e1,e2,e3]T,得:

2.jpg

    得到旋转矩阵R, 该矩阵各分量是两个坐标系基的内积,由于基向量的长度为 1,所以实际上是各基向量的夹角之余弦,所以也叫方向余弦矩阵(Direction Cosine matrix)。旋转矩阵是行列式为1的正交矩阵,反之行列式为1的正交矩阵也是旋转矩阵。旋转矩阵集合:

3.jpg

    SO(n)称为特殊正交群(special Orthogonal Group)。旋转矩阵的逆描述了一个相反的旋转,且R-1=RT


平移向量

    欧式变换由旋转变换和平移变换组成,旋转变换由旋转矩阵R描述,平移变换由平移向量t描述,即坐标变换A’=RA+t


变换矩阵

    使用旋转矩阵和平移向量描述坐标系变换,多次变换后公式过于复杂,因此引入齐次坐标变换矩阵。齐次坐标:在三维向量末尾添加1,变成四维向量。变换矩阵T;将旋转和平移写在一个矩阵里。坐标变换可表示如下:

4.jpg

    变换矩阵集合:

5.jpg

    SE(3)的意思是特殊欧式群(Special Euclidean Group)。变换矩阵的逆也表示一个反向的变换:

6.jpg


旋转向量和欧拉角

旋转向量

    用旋转矩阵描述旋转存在以下缺点:

    1.SO(3)的旋转矩阵有9个量,但是一次旋转只有三个自由度,因此这种方式存在冗余。变换矩阵同理。

    2.旋转矩阵自身带有约束:必须是正交矩阵,而且行列式为1。当想要估计或优化一个旋转矩阵/变换矩阵时,这些约束会使得求解变得更加困难。

    因此需要一种更加紧凑方式的描述旋转和平移。任意旋转都可以用一个旋转轴和一个旋转角来刻画,使用一个向量,其方向和旋转轴一致,长度等于旋转角,这种向量称为旋转向量(或轴角Axis-Angle)。这种表示法仅需要一个三维向量即可描述旋转。使用旋转向量加平移向量,只有六维,即可描述一次变换。


旋转向量和旋转矩阵之间的转换

    假设有一个旋转轴为n,角度为θ的旋转,则其对应的旋转向量为θ*n。从旋转向量到旋转矩阵的转换使用罗德里格斯公式

7.jpg

    符号^表示向量到反对称的转换符,如下:

10.jpg

    反之从旋转矩阵转换到旋转向量,对于转角θ,有:

11.jpg

    其中tr(R)表示矩阵R的迹。对于转轴n,由于旋转轴上的向量在旋转后不发生改变,即Rn=n,因此转轴n是旋转矩阵R特征值1对应的特征向量。求解此方程,再归一化,就得到了旋转轴。


欧拉角

    旋转矩阵和旋转向量虽然能描述旋转,但是非常不直观。欧拉角提供了非常直观的方式描述旋转,将一个旋转分解为3次绕不同轴的旋转。比如先绕X轴,再绕Y轴,最后绕Z轴就得到XYZ轴的旋转。同理得ZYX,ZXY等方式的旋转。这里使用ZYX分解,假设一个刚体的前方(朝向我们的方向)为X轴,右侧为Y轴,上方为Z轴。

1.绕物体的Z轴旋转,得到偏航角yaw

2.绕旋转之后的Y轴旋转,得到俯仰角pitch

3.绕旋转之后的X轴旋转,得到滚转角roll

9.jpg

    欧拉角和旋转向量的一个重大缺点是万向锁问题(Gimbal Lock):在俯仰角为+-90度时,第一次旋转和第三次旋转将使用同一个轴,这使得系统丢失了一个自由度,这被称为奇异性问题。三个实数表示旋转一定会碰到奇异性问题,因为三维旋转是一个三维流形,想要无奇异的表达它,三个量是不够的。因此欧拉角往往只用于人际交互。


四元数

概念

    旋转矩阵具有冗余性,而欧拉角和旋转向量虽然紧凑,但是具有奇异性。四元数(Quaternion)是一种扩展的复数,用它来表示旋转既紧凑,又没有奇异性。 一个四元数q有一个实部和三个虚部,如q = q0 + q1*i + q2*j + q3*k ,其中i,j,k为四元数的三个虚部。三个虚部满足如下关系式:

12.jpg

    也可以用一个标量和一个向量表示四元数:q=[s,v],s=q0,v=[q1,q2,q3],s称为实部,v称为虚部。若虚部为0,则该四元数称为实四元数,反之称之为虚四元数


四元数的运算

1.加减法

13.jpg

2.乘法

14.jpg

15.jpg

3.模长

16.jpg

两个四元数乘积的模即为模的乘积

17.jpg

4.共轭

18.jpg

四元数共轭与其本身相乘,得到一个实四元数,其实部为模长的平方:

19.jpg

5.逆

一个四元数的逆为:

20.jpg

且:

21.jpg

22.jpg

6.数乘

23.jpg


用四元数表示旋转

    可以用一个单位四元数q表示旋转。假设空间点p为[x,y,z],旋转后的点为p’。用一个虚四元数描述p点p=[0,x,y,z],则p’=q*p*q-1,结果p’的虚部就是旋转后的坐标。


将四元数乘法写成矩阵形式

    q=[s,v],定义如下符号:

27.jpg

    则可以将四元数的乘法写成矩阵形式:

28.jpg

29.jpg

    则用四元数矩阵乘法表示旋转:

30.jpg

31.jpg


四元数和旋转矩阵的转换

    四元数转换为旋转矩阵:

32.jpg

    设四元数q=q0+q1*i+q2*j+q3*k,对应的旋转矩阵R为:

33.jpg

旋转矩阵到四元数的转换:已知旋转矩阵为R={m_ij},i,j属于[1,2,3],其对应的四元数为:

34.jpg


四元数和旋转向量的转换

35.jpg


    



Eigen实现

    Eigen是一个开源的C++线性代数库。坐标变换的Eigen数据类型:

41.jpg


CMakeLists.txt

cmake_minimum_required(VERSION 2.6)
project(eigengeometrytest)
include_directories("/usr/include/eigen3")
add_executable(eigengeometrytest main.cpp)
install(TARGETS eigengeometrytest RUNTIME DESTINATION bin)

main.cpp

#include <iostream>
#include<ctime>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include<Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
    std::cout << "Hello, world!" << std::endl;
      
    //旋转矩阵使用Matrix3d
    Eigen::Matrix3d rotation_m=Eigen::Matrix3d::Identity();
    //旋转向量 转角,转轴
    Eigen::AngleAxisd rotation_v(M_PI/4,Eigen::Vector3d(0,0,1));
    
    cout.precision(3);
    //将旋转向量转换为旋转矩阵
    cout<<"rotation_v.matrix:\n"<<rotation_v.matrix()<<endl<<endl;
    rotation_m=rotation_v.toRotationMatrix();
    cout<<"rotation_m:\n"<<rotation_m<<endl<<endl;
    
    //使用旋转向量进行旋转变换
    Eigen::Vector3d v(1,0,0);
    Eigen::Vector3d v_r=rotation_v*v;
    cout<<v<<endl<<"after v rotated:"<<v_r<<endl<<endl;
    
    //使用旋转矩阵进行旋转变换
    v_r=rotation_m*v;
    cout<<v<<endl<<"after m rotated:"<<v_r<<endl<<endl;
    
    //将旋转矩阵转换为欧拉角ZYX
    Eigen::Vector3d euler_a=rotation_m.eulerAngles(2,1,0);
    cout<<"yaw pitch roll="<<euler_a.transpose()<<endl<<endl;
    
    //欧式变换矩阵
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//实质是4x4矩阵
    //设置旋转
    T.rotate(rotation_v);
    //设置平移向量
    T.pretranslate(Eigen::Vector3d(1,3,4));
    cout<<"Transform m:\n"<<T.matrix()<<endl<<endl;
    
    
    //旋转向量转换为四元数
    Eigen::Quaterniond q=Eigen::Quaterniond(rotation_v);
    //输出为q1 q2 q3 q0,最后一个是实部
    cout<<"q:\n"<<q.coeffs()<<endl<<endl;
    
    //旋转矩阵转换为旋转向量
    q=Eigen::Quaterniond(rotation_m);
    cout<<"q\n"<<q.coeffs()<<endl<<endl;
    
    
    //使用四元数旋转
    v_r=q*v; //乘法是重载的,数学上是qvq^-1
    cout<<"v_q_r:"<<v_r<<endl;
    
    
    return 0;
}




下一篇:

首页 所有文章 机器人 计算机视觉 自然语言处理 机器学习 编程随笔 关于