四元数与姿态阵间的关系式推导

版权声明：本文为博主原创文章。 https://blog.csdn.net/u012763833/article/details/52583350

—–下文摘自秦永元的《惯性导航》第二版

　　设有参考坐标系$R$，坐标轴$x_0 、y_0 、z_0$，坐标轴方向的单位向量为$i_0 、j_0 、k_0$。刚体相对$R$系做定点转动，定点为$O$。取坐标系$b$与刚体固联，$b$系的坐标轴为$x、y、z$，坐标轴方向的单位向量为$i、j、k$。假设初始时刻$b$系与$R$系重合。为了便于分析刚体的空间位置，刚体上取一点$A$，转动点$O$至该点引位置向量$OA$，如下图所示：

　　设刚体以 $ω=ω_x i+ω_y j+ω_z k$相对 $R$ 系旋转，初始时刻位置向量处于 $OA=r$经过时间 t 后位置向量处于 $OA'=r'$。根据欧拉定理，仅考虑刚体在 0 时刻和 t 时刻的角位置时，刚体从 $A$位置转到 $A'$位置的转动可等效成绕轴 $u$（单位向量）转过 $θ$角一次完成这样，位置向量做圆锥运动， $A$和 $A'$位于同一圆上， $r$和 $r'$位于同一圆锥上。
　　下面分析 $r$和 $r'$的关系。在圆上取一点 $B$使 $∠AO' B=90°$，有上图可得：
　　 $OO'=(r∙u)u$
　　 $O' A=r- OO'=r-(r∙u)u$
　　 $O' B=u×O' A=u×r-(r∙u)u=u×r$
　　 $O' A'=O' A cos⁡θ+O' B sin⁡θ=r cos⁡θ-(r∙u)u cos⁡θ+u×r sin⁡θ$
所以
　　 $r'= OO'+O' A'=r cos⁡θ+(1-cos⁡θ )×(r∙u)u+u×r sin⁡θ$
由矢量三重积计算公式:
　　 $u×(u×r)=u(u∙r)-(u∙u)r=(r∙u)u-r$
即
　　 $(r∙u)u=r+u×(u×r)$
所以
　　 $r'=r cos⁡θ+(1-cos⁡θ )[r+u×(u×r)]+u×r sin⁡θ=r+u×r sin⁡θ+(1-cos⁡θ )u×(u×r)$
将上式向 $R$系内投影：
　　 $r'^R=r^R+(u×r)^R sin⁡θ+(1-cos⁡θ ) [u×(u×r)]^R$
记

又根据叉乘关系表达式

记

则
　　 $(u×r)^R=Ur^R$
　　 $[u×(u×r)]^R=U∙Ur^R$
所以
　　 $r'^R=r^R+Ur^R sin⁡θ+(1-cos⁡θ )U∙Ur^R=(I+2Usin\frac{θ}{2}cos⁡\frac{θ}{2}+2sin^2\frac{θ}{2} U∙U)r^R$
令
　　 $D=I+2U sin⁡\frac{θ}{2} cos⁡\frac{θ}{2}+2sin^2\frac{θ}{2} U∙U$
则有：
$r'^R=Dr^R$
　　记初始时刻的刚体固联坐标系为 $b_0$，由于初始时刻刚体固联坐标系与参考坐标系重合所以 $r^R=r^{b_0}$而在转动过程中，位置向量和 $b$系都同刚体固联，所以位置向量和 $b$系的相对角位置始终不变，即有:
　　 $r^R=r'^R=Dr'^b$
该式说明 $D$即为 $b$系至 $R$系的坐标变换矩阵。
　　 $C_b^R=I+2U sin⁡\frac{θ}{2} cos⁡\frac{θ}{2}+2sin^2\frac{θ}{2}U∙U$
即

令

并以 $q_0 q_1 q_2 q_3$构造四元数：
$Q=q_0+q_1 i_0+q_2 j_0+q_3 k_0=cos⁡\frac{θ}{2}+(li_0+mj_0+nk_0 ) sin⁡\frac{θ}{2}=cos⁡\frac{θ}{2}+u^R sin⁡\frac{θ}{2}$
则可得如下结论：

1. 四元数$Q=cos⁡\frac{θ}{2}+u^R sin⁡\frac{θ}{2}$描述了刚体的定点转动，即当只关心$b$系相对$R$系的角位置时，可认为$b$系是由$R$系经过无中间的一次性等效旋转形成的，$Q$包含了这种等效旋转的全部信息：$u^R$为旋转瞬时轴和坐标变换矩阵，$θ$为转过的角度。
2. 四元数可以确定$b$系至$R$系的坐标变换矩阵。
   
   由于$‖q‖=q_0^2+q_1^2+q_2^2+q_3^2=cos^2\frac{θ}{2}+(l+m+n)sin^2\frac{θ}{2}=1$，所以可以进一步推得如下结论：
   (1)描述刚体旋转的四元数是规范化四元数。
   (2)
   (3)如果将向量$r^R$和$r^b$看作零标量的四元数，则$r^R$和$r^b$间的变换关系可采用四元数乘法表示：$r^R=Q⊗r^b⊗Q^*$该式称为坐标变换的四元数表示法，其中$Q$为$R$至$b$系的旋转四元数。坐标变换的矩阵表示法为：$r^R=C_b^R r^b$