三维刚体运动，即三维空间下的刚体的运动。刚体，是指在运动中和受力作用后,形状和大小不变,而且内部各点的相对位置不变的物体。在运动过程中，机器人或者飞机和汽车的形变很小，可以近似看作刚体。三维刚体运动就是研究如何描述和表示一个刚体在三维空间下的位姿变换过程。比如装甲板的位姿描述，云台的位姿描述等。

　　描述刚体的位姿变换，可以将其运动分解为两个过程，旋转以及平移。旋转调整其姿态，平移调整其位置。

　　世界坐标选取的是一系列真实世界中的坐标系，其可以是人为规定的坐标系。其三维坐标可以使用\((X_W, Y_W, Z_W)\)来表示(W = World)。

　　相机坐标系则是以相机焦点为坐标系中心来规定的坐标系。使用\((X_C, Y_C, Z_C)\)来进行表示.(C = Camera)

　　在RM比赛中，视觉组一项重要的工作就是如何仅依靠摄像头的图片(也就是纯视觉)进行装甲板测距，测量装甲板相对于相机坐标系的坐标。这一步便是装甲板的世界坐标向相机坐标系下的坐标变换。

　　明确了以上几个概念，我们就可以开始展开描述三维姿态的第一种方法了，那就是旋转矩阵。

　　不妨假设有另外一组单位正交基为\({\vec{e_1},\vec{e_2},\vec{e_3}}\),向量a在该组基底的坐标为\((a_1,a_2,a_3)\),则根据向量不变性(在不同坐标系下向量的长度以及与其他向量的夹角不变)可以得出如下等式

　　可以看出，我们可以将a‘到空间点a的变换用矩阵R来描述。这里的R我们称之为旋转矩阵，它描述了同一向量在不同坐标系下的坐标变换。

　　值得注意的是，这里的基底向量以及变换的向量都是抽象意义上的向量，并不依赖于某个坐标系下的坐标，都保持自身的不变性。不过在我看来或者计算来看，可以取一个独立于二者之外的坐标系来进行参考，更加容易理解。

　　上述描述称为特殊正交群，满足此条件即为旋转矩阵，同理旋转矩阵也都满足此条件。

　　至此由坐标系1中的坐标变换到坐标系2的这一过程我们就可以使用旋转矩阵来进行描述了。值得注意的是旋转矩阵更为强调的是坐标系之间的变换，变的是坐标系而非向量，通过旋转矩阵可以知道变换之后向量在新坐标系下的坐标与位置。

　　需要注意的一点是，上述式子应该从右向左看，代表从右边的坐标系转换到左边的坐标系。

　　开始说道，描述坐标系的变换，不仅要进行旋转来调整姿态上的开云体育官网不同，还要进行平移来调整位置上的不同。平移的操作很简单，直接对系1原点进行平移至与系2原点重合即可。我们使用一个向量\(t\)来描述平移的过程，t就被称为平移向量

　　当变换的坐标越多，使用旋转矩阵和平移向量来描述过程就越加繁琐，出现括号套括号的现象。为了表示更加整洁，可以采用以下方式。

　　使用这种表示法之后，原先使用旋转矩阵和旋转向量的坐标变换便可以转化为如下表示。

　　由上述计算结果来看，无论是使用旋转矩阵还是变换矩阵，结果都是一样的，只是使用变换矩阵来描述会更加简洁。

　　值得注意的是，我们将a化为\(\tilde{a}\)的目的是为了使之可以与变换矩阵相乘。变换矩阵实际上是一个4*4的矩阵，a变换之前是3*1,不可乘

　　可以知道旋转矩阵是一个\(3\times3\)的矩阵，有着九个元素，而且必须满足单位正交群的约束。描述一次旋转需要用到9个量，看起来有点多。这里给出另一种描述坐标旋转的方式，叫做旋转向量或者轴角或者角轴。

　　使用旋转向量来描述坐标系的旋转过程只需要一个描述旋转的向量即可，该向量的方向指向转轴方向，大小或者说系数为转动角度大小。其描述形式如下

　　其中的\(\theta\)即为转动角度，而\(\vec{n}\)为表征转轴方向的单位向量。

　　注意这里使用轴角来描述旋转时，指的是绕给定轴一步到位的旋转，而非分解到几个轴的旋转。

　　不难看出，轴角只需要一个旋转向量就可以描述这次旋转，且\(\vec{w}\in R^3\)。本来需要\(3\times3\)规模旋转矩阵的描述变为了使用一个三维向量来描述，更加简洁。不过旋转矩阵与旋转向量描述的变换终究是一回事，因此两者可以相互转化，对同一种旋转都是等价的。

　　上面的旋转矩阵以及旋转向量的描述虽然都用数学语言描述了三维空间坐标下的旋转，但是对于人类来讲还是太抽象了。但是使用欧拉角可以直观感受到坐标系或者物体的旋转。

　　在如图坐标中我们将飞机绕不同的轴转动，从而得到不同的欧拉角定义(都是作为垂直轴)

　　不难体会出对于欧拉角的定义都是很直观的，左右摆动就是偏航，上下摆动就是俯仰，绕轴滚动就是滚转。这里要注意一点，欧拉角的定义和转轴名称并不绑定，即并非绕y轴转动就是yaw角，而是与转动方式相关。比如俯仰的上下变换就是yaw角，而非绕y轴转动就是yaw。

　　，如yaw-pitch-roll或者roll-pitch-yaw，并且

　　在使用欧拉角描述姿态变换时，有两种选择，一种是变轴的欧拉角，即每次旋转都会带动其他轴的旋转，下一次旋转会在此基础上接着转。而另一种是定轴的欧拉角，转轴在一开始就已经确定不变，每次旋转都是参考选定转轴旋转。

　　如图所示，万向锁是动轴的欧拉角会出现的一种现象。当三组角的中间角(这里假设为图中的y轴即yaw角，假设顺序为x_pitch - y_yaw - z_roll)转动为\(90^o\)时，会导致第三轴与第一轴重合，导致绕z轴旋转等同与绕最初的x轴旋转了。本来应该描述三个方向的旋转变成了两个方向。丢失了一个方向的信息。万向锁的出现是因为动轴的欧拉角的动轴以及欧拉角旋转的顺序性共同导致的。

　　这里贴上一个讲的很好的视频，帮助理解欧拉角产生的万向锁现象是如何产生的。万向锁

　　这里强调一点，欧拉角的顺序性。如以pyr为顺序，(10,20,30)为pyr的转动角度，如果我们将(10,20,30)变为(12,20,30)时，发生的过程并非为在当前变换后的坐标系下的x轴转动,而是依据顺序性，重新从未旋转的状态开始进行pyr的旋转刚体运动的三维世界。因为当第一次旋转角度改变后，第二次旋转的轴也会有所不同。

　　我们知道在二维平面上，可以使用复数来实现旋转如\(\vec{n} * e^{\pi i}\)角度相加，相当于向量n转动了180度。那么，对于三维空间，是否有类似于复数的体系来通过运算描述三维空间中的转动呢？

　　Eigen库是c++的一个功能强大，性能优越的开源线性代数计算库，简单好用。大多数复杂的计算在Eigen中都有相应的接口，可以大大简化我们写代码的方式。Eigen是一个纯头文件实现的模板库，因此在使用时不需要额外链接动态或者静态库。

　　其所有接口封装在命名空间Eigen中，最主要的就是一个Matrix模板类，Eigen中几乎所有的类都是Matrix的typedef或者宏,给出Matrix类的构造函数