SLAM基础知识记录--数学知识--方向余弦矩阵与等效旋转变量

        在惯导的使用中，我们时常要使用到姿态的更新，这里复习一些关于姿态变化的相关知识。

一、方向余弦阵 

两个坐标系之间的关系，可以通过方向余弦矩阵描述，即我们通常熟悉的旋转矩阵SO(3)。根据欧拉转动定理，刚体从一个位置到另一个角位置的任意转动总能够等效于绕某一固定轴的一次转动，这一固定轴和转角就构成了等效旋转矢量的概念。

 我们将记为从b系到i系的坐标变换矩阵，也就是从i系到b系得坐标系变换矩阵，这里可以总结：i系为参考坐标系，放到上方，b系为动坐标系，放在下方。

P矩阵为单位正交阵，如P的第一行向量，它表示i系参考矩阵的坐标轴x单位矢量在b系的投影，也就是i系坐标轴x在b系的余弦值，因此称为方向余弦阵。

二、等效旋转矢量

三维空间中某个矢量r绕着另一个单位矢量u转动角，设,得到新的矢量。不妨假设矢量r和单位矢量u的起始点相同。

 

 

 

式(2.2.17) 称为罗德里格斯旋转公式，它建立了转动前后两个矢量的线性变换的关系，此变换为旋转轴和旋转角的函数。

矩阵D刚好是从参考坐标系i系到动坐标系b系得过渡矩阵，从b系到i系得坐标变换矩阵，也就是方向余弦矩阵(旋转矩阵R)：

然后用大phi和小phi来代替u，大phi即称为等效旋转矢量，从转动的物理含义上看，转角为360°周期的，表示相同的转动，这可通过将其带入上式进行验证，即

 矩阵与k取值无关，若限定转角的取值范围为,那么等效旋转矢量与方向余弦矩阵之间存在一一对应的关系。

附录的三重矢积公式：

 以基准轴做旋转的旋转矩阵为：

 上三式称为Givens矩阵或初等旋转矩阵，空间的任意转动都可以由三次基本转动合成。

由旋转矩阵求解出等效旋转矢量的方法：利用旋转矩阵的迹求角度的大小

2.3 方向余弦阵微分方程及其求解

        假设动坐标系(b系)和参考坐标系(i系)具有共同的原点, b系相对于i系转动的角速度为,从i系到b系得坐标系矩阵记作,它是时变矩阵，举个例子，相当于机器人相对于原始静态坐标系的，固定矢量在两个坐标系投影为：

 因此，对应在i系中任意固定的矢量r均成立，任选三个不共面的非零矢量，有：

上式即为方向余弦的微分方程，它建立了动坐标系相对于参考坐标系的方向余弦阵和动坐标系运动角速度之间的动态关系， 的含义是指动坐标系b系相对于i系的旋转角速度，表达在动坐标系下的值。

以下四种余弦矩阵是等价的：

 

方向余弦微分方程的求解:为了书写简便，略去各量的上下角标，记反对称阵为

这是 典型的变系数齐次微分方程，采用皮卡迭代法(Picard)。

参考文献：严恭敏 翁浚《捷联惯导算法与组合导航原理》西北工业大学