微分几何在机器人领域的应用（二）深入理解三维空间变换

空间几何变换

空间中的几何变换分为多类，从最简单，到逐渐复杂的变换，分别有如下几种。

1. 等距变换（Isometries）。等距变换下点到点的欧式距离保持不变。刚体变换是典型的等距变换。
2. 相似变换（Similarity）。在等距变换的基础上加上一个各向同性的缩放。矩阵表示上需要在旋转矩阵部分乘以一个系数s。
3. 仿射变换（Affine）。是一个非奇异的线性变换加上一个平移向量组成的变换。
4. 投影变换（Projective）。任意非奇异的4×4矩阵所构成的变换。

变换的分类和特征如下图所示:

三维刚体的空间变换属于第一种情况。如果问题不变形，那么刚体变换涵盖物理世界中的所有情况。刚体变换包含三个平移自由度和三个旋转自由度，总共6个自由度。应用刚体变换，点到点的距离保持不变，同时矢量的点积和叉积保持不变。平移自由度易于理解，故本文重点讨论旋转分量，即旋转矩阵R。

旋转矩阵：

 

 

    在理解高维理论时，我们一般采用降维的方式理解，由易到难。首先回到二维空间的变换。二维平面中，刚体变换有三个自由度，x, y 和旋转角θ。用矩阵的形式表示：

 

其中

 

　　分别为旋转矩阵和平移向量。可以看到旋转矩阵只有一个自由度，因其只有一个变量θ。

 

旋转矩阵R的性质：

 

1. 旋转矩阵的逆矩阵是它的转置矩阵，故旋转矩阵是正交矩阵。（如果不理解逆矩阵和转置矩阵，请首先恶补线性代数）。
2. 一个矩阵是旋转矩阵，当且仅当它是正交矩阵，且它的行列式是1。正交矩阵的行列式是±1。读者可思考行列式为-1的情况对应什么变换。

 

二维旋转矩阵可用旋转角唯一表示。正角表示逆时针旋转。

 

 

 

　　如下图表示的是当θ=20°的情况。

 

 

 

　　 二位旋转矩阵的许多性质在三维空间中同样满足。

 

让我们回到三维空间。旋转可以有三个旋转组合而成。在右手（笛卡尔）坐标系下分别绕x, y, z轴旋转。其旋转矩阵分别对应为

 

 

 

 

 

 

 

 

 

 

任意旋转矩阵可写作一定角度下的三个矩阵的乘积。

 

注意：矩阵乘法不符合交换律！故顺序不同，得到的旋转矩阵并不相同。

欧拉角

 

航空领域，一般定义飞机前后轴为x轴，沿x轴旋转的角度一般称为Roll，中文称作翻滚角；两翼方向称作Pitch，中文称作俯仰角；垂直地面的方向是航向角（Yaw），如下图所示。个人觉得中文翻译很符合愿意，更易于理解。可以记住在驾驶飞机时，如何操纵翻滚角，俯仰角，航向角。Roll，Pitch，Yaw，又称作欧拉角。习惯上，三个欧拉角的方向是z-y-x，使用时需要特别重要，欧拉角顺序错了，旋转矩阵也会发生变换。

 

 

程序实现：

 

程序使用基于C++的Eigen库[3]。注意，Eigen库是一个仅包含头文件的基础矩阵库，没有静态或动态库。使用时仅需要把相关的目录include就可以了。

 

再次注意：三个欧拉角的顺序！

 

#define _USE_MATH_DEFINES
#include <math.h>

#include <Eigen/Core>
#include <Eigen/Dense>
typedef Eigen::Vector3f            Geo3d;
typedef Eigen::Matrix3f            GeoMat3;
typedef Eigen::Matrix4f            GeoMat4;
typedef GeoMat3                    RotMat;

GeoMat3 RotationMatrixFromEulers(float rx, float ry, float rz)
{
　　Eigen::AngleAxisf quat = Eigen::AngleAxisf(rz, Geo3d::UnitZ()) * Eigen::AngleAxisf(ry, Geo3d::UnitY()) * Eigen::AngleAxisf(rx, Geo3d::UnitX());
14　　 return quat.matrix();
}

int main(int argc, char *argv[])
{
    auto rot = RotationMatrixFromEulers(M_PI/12, -M_PI/3, M_PI/2);
    std::cout << “Rotation Matrix: “ << rot << std::endl;

    // to euler angles. (2, 1, 0) means, rz, ry, rx
    Geo3d euler_angles = rot. eulerAngles(2, 1, 0);
    std::cout <<”Euler angles: “ << euler_angles.transpose() << std::endl;
    return 1;
}

 李群和李代数

 

三维旋转矩阵是最直观的表示方法，但旋转矩阵有9个变量，只有3个自由度，故信息是冗余的。旋转矩阵在工程使用更好的表达方法。根据定义，所有的刚体变换属于一个群（李群，Lie Group）。刚体变换又称作特殊欧式变换（special Euclidean transformation），通常写作SE（3）。李群中的变换满足如下特性。详细性质可参见李群和李代数的资料。如果只限于3D视觉或机器人学，只需记住其主要特性：

• 封闭性

 

• 相关性

 

•  单位矩阵
• 可逆

 

刚体变换的组合和逆变换均属于刚体变换。

 

单纯的旋转变换称作特殊正角变换（special orthogonal transformation), 通常写作SO（3）。旋转矩阵都是正交矩阵。

 

李代数通过指数映射，将旋转矩阵的9个变量转换为3个变量，结合三个平移向量，总共6个变量，对应6个自由度。李代数表示法在三维重建（SFM）、VR、SLAM等位姿估计领域应用的较多。李代数有基于Eigen的Sophus库[4]可使用，方便完成指数映射。

 

罗德里格斯旋转公式（Rodriguez’s Rotation Formula）

 

旋转矩阵有一个更有效的表达方法，即由一个单位向量和一个旋转角生成。每一个旋转矩阵均可转化为向量和角（又称轴-角）的表达方式。根据公式，单位向量用表示，旋转的角度是θ，那么相应的旋转矩阵是

 

 

 

此矩阵可用简化为如下公式，

 

 

 

　　具体点符号定义可参见相关文献。单纯环绕x，y或z轴旋转而成的旋转矩阵是罗德里格斯公式的特殊形式。读者可以把上式中的单位向量替换为（0,0,1）进行验证。虽然公式复杂，但程序实践比较方便。利用Eigen库中的Eigen::AngleAxisf（旋转向量）可以直接获得。

 

四元数

    四元素可看作一种特殊的复数，由一个实部和三个虚部构成。四元素的表示方法同旋转矩阵、欧拉角表示方法是等价的。根据罗德里格斯旋转公式，任何一个旋转都可以表达成轴角的表达法。四元素可以更方便的表达出旋转轴和旋转角。单位欧拉向量可表示为

 

根据欧拉公式的扩展，四元素可表示为

 

 

 

 

四元素分为实部和虚部，实部只跟旋转角有关。虚部有单位向量和旋转角共同计算得来。

四元数的求逆可采用复数的共轭（即虚部取反）方式求得

同时，四元数更易于做线性插值（Slerp）。实际实验中，使用四元素做旋转矩阵的计算更加方便。使用Eigen库时，四元素的使用更为方便。

 

 

总结

 

• 　　刚体的空间变换由平移和旋转两部分组成。平移部分易于理解，旋转部分一般由直观的3×3矩阵表示。

 

•  　　  旋转矩阵有很多特性（正交矩阵、单位矩阵），但其由9个元素，但只有3个自由度，故数学上的表示是冗余的。

 

•  　　  在机器人领域，使用最多的除旋转矩阵外，还有旋转向量、欧拉角、四元素等。

 

•  　　 本文的几乎所有变换都容易实现，可直接使用三方库如Eigen[3]，类似的还要OpenCV等。但如要深入理解，最好自己实战。

 

•  　　  思考：二维空间刚体变换有3个自由度，三维有6个自由度，四维空间呢？n维空间呢？

 

参考文献：

 

1. Multiple View Geometry in Computer Vision (2^nd Edition), Richard Hartley and Andrew Zisserman.

 

2. An Invitation to 3-D Vision From Images to Models, Yi Ma, Jana Kosecka, Stefano Soatto and Shankar Sastry.

 

3. Eigen, http://eigen.tuxfamily.org/

 

4. Sophus, https://github.com/strasdat/Sophus