005四元数与姿态阵间的关系

学习惯导这么久，还是没有形成自己的体系，应该说还是没有入门（手动尴尬）。所以在此对看过的内容进行解释，加深印象。本文的内容针对秦永元老师《惯性导航（第二版）》中“9.2.2 四元数与姿态阵间的关系”进行备忘。

一、前提

如图所示（为了便于理解，我把原图擦去了一部分），假定有刚体金刚圈AB，坐标系R，注意这个坐标系不与金刚圈固联（无论金刚圈怎么转，我坐标系坚决不动，相当于这是一个导航坐标系）。当看到这个图，你肯定不知道坐标系R在哪了。没关系，这都不重要，可以这么认为，坐标系就假定是以O为原点的右手系（O隐藏的很深，仔细看）。
这里写图片描述
金刚圈跟坐标系R介绍完了，就该谈一谈其中的元素了。请看下面这个被我蹂躏得比较轻的图。连接OO’（O’为圆心）得矢量 $\vec{OO'}$ ，连接O’A得矢量 $\vec{O’A}$ ，连接O’B得矢量 $\vec{O'B}$ （ $\vec{O'A}$ 垂直于 $\vec{O'B}$ ），连接OA得矢量 $\vec{OA}$ 记为 $\vec{r}$ 。把这些元素定义好了，金刚圈就开始绕 $\vec{OO'}$ 转动，转动方向沿短弧沿短弧 $A\xrightarrow{沿短弧}B$ 。
这里写图片描述
圆圈的转动还是要作一下说明，如下图所示（这个图没有遭到我的毒手）。在 $\vec{OO}$ 上取单位矢量 $\vec{u}$ ，该矢量可以等效成刚体的旋转轴。 $\vec{O'A}$ 旋转角度 $\theta$ 得到 $\vec{O'A'}$ ，连接OA’得到 $\vec{OA'}$ 记为 $\vec{r}'$ 。
这里写图片描述
到此为止，把旋转的关系理清楚了。下面请看四元数与姿态阵关系的推导过程。

二、推导

推导的过程其实就是分析转动前后的矢量 $\vec{r}$ 与 $\vec{r}'$ 的关系。

根据前面的描述，首先可以得到：

\vec{O O^{'}} = (\vec{r} \cdot \vec{u}) \vec{u}

$\vec{OO'}=(\vec{r}·\vec{u})\vec{u}$

为什么呢？因为：

$\vec{O O^{'}} = (| \vec{r} | \cdot | \vec{u} | \cdot c o s < \vec{r}, \vec{u} >) \vec{u} = | \vec{O O^{'}} | \cdot \vec{u} = \vec{O O^{'}}$ $\vec{OO'} =(|\vec{r}|·|\vec{u}|·cos<\vec{r},\vec{u}>)\vec{u} \\ =|\vec{OO'}|·\vec{u}=\vec{OO'} \\$

那么可求：

\vec{O^{'} A} = \vec{r} - \vec{O O^{'}} = \vec{r} - (\vec{r} \cdot \vec{u}) \vec{u}

$\vec{O'A}=\vec{r}-\vec{OO'}=\vec{r}-(\vec{r}·\vec{u})\vec{u}$

\vec{O^{'} B} = \vec{u} \times \vec{O^{'} A} = \vec{u} \times \vec{r} - \vec{u} \times (\vec{r} \cdot \vec{u}) \vec{u} = \vec{u} \times \vec{r} - (\vec{r} \cdot \vec{u}) \vec{u} \times \vec{u} = \vec{u} \times \vec{r}

$\vec{O'B} =\vec{u}×\vec{O'A}\\ =\vec{u}×\vec{r}-\vec{u}×(\vec{r}·\vec{u})\vec{u}\\ =\vec{u}×\vec{r}-(\vec{r}·\vec{u})\vec{u}×\vec{u}\\ =\vec{u}×\vec{r}$

这一步，点乘是一个实数，所以会有 $\vec{u}×\vec{u}$ ，而 $\vec{u}×\vec{u}=0$ 。

说到这里又要再加一个图了，如下图，这个图的意思不用我多说了吧。
这里写图片描述
通过这个图求 $\vec{O'A'}$ :

\vec{O^{'} A^{'}} = \vec{O^{'} A} c o s θ + \vec{O^{'} B} s i n θ = \vec{r} c o s θ - (\vec{r} \cdot \vec{u}) \vec{u} c o s θ + \vec{u} \times \vec{r} s i n θ

$\vec{O'A'} =\vec{O'A}cos\theta+\vec{O'B}sin\theta\\ =\vec{r}cos\theta-(\vec{r}·\vec{u})\vec{u}cos\theta+\vec{u}×\vec{r}sin\theta$
这样，

{\vec{r}}^{'}

$\vec{r}'$ 就可以求得：

{\vec{r}}^{'} = \vec{O O^{'}} + \vec{O^{'} A^{'}} = \vec{r} c o s θ + (1 - c o s θ) (\vec{r} \cdot \vec{u}) \vec{u} + \vec{u} \times \vec{r} s i n θ

$\vec{r}' =\vec{OO'}+\vec{O'A'}\\ =\vec{r}cos\theta+(1-cos\theta)(\vec{r}·\vec{u})\vec{u}+\vec{u}×\vec{r}sin\theta$

另外有这样一个公式，在推导的过程中需要用到（公式来源：向量积-拉格朗日公式）：

$\vec{a} \times (\vec{b} \times \vec{c}) = \vec{b} (\vec{a} \cdot \vec{c}) - \vec{c} (\vec{a} \cdot \vec{b})$ $\vec{a}×(\vec{b}×\vec{c})=\vec{b}(\vec{a}·\vec{c})-\vec{c}(\vec{a}·\vec{b})$
我们知道向量的点乘满足交换律，所以可以写成：
$\vec{a} \times (\vec{b} \times \vec{c}) = \vec{b} (\vec{a} \cdot \vec{c}) - (\vec{a} \cdot \vec{b}) \vec{c}$ $\vec{a}×(\vec{b}×\vec{c})=\vec{b}(\vec{a}·\vec{c})-(\vec{a}·\vec{b})\vec{c}$
这时候令 $\vec{a}=\vec{b}=\vec{u}$ ， $\vec{c}=\vec{r}$ ，可得：
$\vec{u} \times (\vec{u} \times \vec{r}) = \vec{u} (\vec{u} \cdot \vec{r}) - (\vec{u} \cdot \vec{u}) \vec{r}$ $\vec{u}×(\vec{u}×\vec{r})=\vec{u}(\vec{u}·\vec{r})-(\vec{u}·\vec{u})\vec{r}$
现在就跟书上的对照起来啦。接下来还有 $\vec{u}·\vec{u}=1$ ，所以上式可表示为：
$\vec{u} \times (\vec{u} \times \vec{r}) = \vec{u} (\vec{u} \cdot \vec{r}) - \vec{r} = (\vec{r} \cdot \vec{u}) \vec{u} - \vec{r}$ $\vec{u}×(\vec{u}×\vec{r}) =\vec{u}(\vec{u}·\vec{r})-\vec{r}\\ =(\vec{r}·\vec{u})\vec{u}-\vec{r}$
所以可以得到：
$(\vec{r} \cdot \vec{u}) \vec{u} = \vec{r} + \vec{u} \times (\vec{u} \times \vec{r})$ $(\vec{r}·\vec{u})\vec{u}=\vec{r}+\vec{u}×(\vec{u}×\vec{r})$
够详细了吧，我这颗心那，就怕以后看不懂！！

所以可以得到：

{\vec{r}}^{'} = \vec{O O^{'}} + \vec{O^{'} A^{'}} = \vec{r} c o s θ + (1 - c o s θ) (\vec{r} \cdot \vec{u}) \vec{u} + \vec{u} \times \vec{r} s i n θ = \vec{r} c o s θ + (1 - c o s θ) [\vec{r} + \vec{u} \times (\vec{u} \times \vec{r})] + \vec{u} \times \vec{r} s i n θ = \vec{r} + \vec{u} \times \vec{r} s i n θ + (1 - c o s θ) \vec{u} \times (\vec{u} \times \vec{r})

$\vec{r}' =\vec{OO'}+\vec{O'A'} =\vec{r}cos\theta+(1-cos\theta)(\vec{r}·\vec{u})\vec{u}+\vec{u}×\vec{r}sin\theta\\ =\vec{r}cos\theta+(1-cos\theta)[\vec{r}+\vec{u}×(\vec{u}×\vec{r})]+\vec{u}×\vec{r}sin\theta\\ =\vec{r}+\vec{u}×\vec{r}sin\theta+(1-cos\theta)\vec{u}×(\vec{u}×\vec{r})$
下面就是将

{\vec{r}}^{'}

$\vec{r}'$ 向R系内投影，表示为：

{\vec{r}}^{' R} = {\vec{r}}^{R} + (\vec{u} \times \vec{r})^{R} s i n θ + (1 - c o s θ) [\vec{u} \times (\vec{u} \times \vec{r})]^{R}

$\vec{r}'^R =\vec{r}^R+(\vec{u}×\vec{r})^Rsin\theta+(1-cos\theta)[\vec{u}×(\vec{u}×\vec{r})]^R$
记

{\vec{r}}^{' R} = [\begin{matrix} r_{x}^{'} \\ r_{y}^{'} \\ r_{z}^{'} \end{matrix}], {\vec{r}}^{R} = [\begin{matrix} r_{x} \\ r_{y} \\ r_{z} \end{matrix}], {\vec{u}}^{R} = [\begin{matrix} l \\ m \\ n \end{matrix}]

$\vec{r}'^R= \left[ \begin{matrix} r'_x \\ r'_y \\ r'_z \\ \end{matrix} \right] , \space\space\space\space \vec{r}^R= \left[ \begin{matrix} r_x \\ r_y \\ r_z \\ \end{matrix} \right] , \space\space\space\space \vec{u}^R= \left[ \begin{matrix} l\\ m\\ n\\ \end{matrix} \right]$

看到这肯定又不懂了。其实就是将转动后得到的 $\vec{OA'}$ 投影到原来的R系中，然后在R系内将投影的向量用坐标表示出来了。

此时根据叉乘表达式可以得到：

(\vec{u} \times \vec{r})^{R} = [\begin{matrix} 0 & - n & m \\ n & 0 & - l \\ - m & l & 0 \end{matrix}] [\begin{matrix} {\vec{r}}_{x} \\ {\vec{r}}_{y} \\ {\vec{r}}_{z} \end{matrix}]

$(\vec{u}×\vec{r})^R= \left[ \begin{matrix} 0 & -n & m\\ n & 0 & -l\\ -m & l & 0\\ \end{matrix} \right] \left[ \begin{matrix} \vec{r}_x\\ \vec{r}_y\\ \vec{r}_z\\ \end{matrix} \right]$
记

\begin{matrix} (1) & U = [\begin{matrix} 0 & - n & m \\ n & 0 & - l \\ - m & l & 0 \end{matrix}] \end{matrix}

$U= \left[ \begin{matrix} 0 & -n & m\\ n & 0 & -l\\ -m & l & 0 \end{matrix} \right] \tag 1$

这又是什么鬼？不要慌，下面来拓展反对称阵。
设

$\vec{a} = [\begin{matrix} a_{1} \\ a_{2} \\ a_{3} \end{matrix}], \vec{b} = [\begin{matrix} b_{1} \\ b_{2} \\ b_{3} \end{matrix}]$ $\vec{a}= \left[ \begin{matrix} a_1\\ a_2\\ a_3\\ \end{matrix} \right] , \space\space\space \vec{b}= \left[ \begin{matrix} b_1\\ b_2\\ b_3\\ \end{matrix} \right]$
$\vec{a} \times \vec{b} = | \begin{matrix} \vec{i} & \vec{j} & \vec{k} \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \end{matrix} | = [\begin{matrix} a_{2} b_{3} - a_{3} b_{2} \\ a_{3} b_{1} - a_{1} b_{3} \\ a_{1} b_{2} - a_{2} b_{1} \end{matrix}]$ $\vec{a}×\vec{b}= \left| \begin{matrix} \vec{i} & \vec{j} & \vec{k}\\ a_1 & a_2 & a_3\\ b_1 & b_2 & b_3\\ \end{matrix} \right| = \left[ \begin{matrix} a_2b_3-a_3b_2\\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \\ \end{matrix} \right]$
而
$[\begin{matrix} 0 & - a_{3} & a_{2} \\ a_{3} & 0 & - a_{1} \\ - a_{2} & a_{1} & 0 \end{matrix}] [\begin{matrix} b_{1} \\ b_{2} \\ b_{3} \end{matrix}] = [\begin{matrix} a_{2} b_{3} - a_{3} b_{2} \\ a_{3} b_{1} - a_{1} b_{3} \\ a_{1} b_{2} - a_{2} b_{1} \end{matrix}]$ $\left[ \begin{matrix} 0 & -a_3 & a_2\\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{matrix} \right] \left[ \begin{matrix} b_1\\ b_2 \\ b_3 \\ \end{matrix} \right] =\left[ \begin{matrix} a_2b_3-a_3b_2\\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \\ \end{matrix} \right]$
则记
$(a \times) = [\begin{matrix} 0 & - a_{3} & a_{2} \\ a_{3} & 0 & - a_{1} \\ - a_{2} & a_{1} & 0 \end{matrix}] ， \vec{a} \times \vec{b} = (a \times) \vec{b}$ $(a×)=\left[\begin{matrix} 0 & -a_3 & a_2\\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{matrix} \right] ， \space\space\space \vec{a}×\vec{b}=(a×)\vec{b}$

因此可以继续往下推导：

(\vec{u} \times \vec{r})^{R} = U {\vec{r}}^{R}, [\vec{u} \times (\vec{u} \times \vec{r})]^{R} = U \cdot U {\vec{r}}^{R}

$(\vec{u}×\vec{r})^R=U\vec{r}^R , \space\space\space [\vec{u}×(\vec{u}×\vec{r})]^R=U·U\vec{r}^R$
所以：

\begin{matrix} (2) & {\vec{r}}^{' R} = {\vec{r}}^{R} + (\vec{u} \times \vec{r})^{R} s i n θ + (1 - c o s θ) [\vec{u} \times (\vec{u} \times \vec{r})]^{R} = {\vec{r}}^{R} + U {\vec{r}}^{R} s i n θ + (1 - c o s θ) U \cdot U {\vec{r}}^{R} = (I + 2 U s i n \frac{θ}{2} c o s \frac{θ}{2} + 2 s i n^{2} \frac{θ}{2} U \cdot U) {\vec{r}}^{R} \end{matrix}

$\vec{r}'^R =\vec{r}^R+(\vec{u}×\vec{r})^Rsin\theta+(1-cos\theta)[\vec{u}×(\vec{u}×\vec{r})]^R\\ =\vec{r}^R+U\vec{r}^Rsin\theta+(1-cos\theta)U·U\vec{r}^R\\ =(I+2Usin\frac{\theta}{2}cos\frac{\theta}{2}+2sin^2\frac{\theta}{2}U·U)\vec{r}^R \tag 2$
令：

\begin{matrix} (3) & D = I + 2 U s i n \frac{θ}{2} c o s \frac{θ}{2} + 2 s i n^{2} \frac{θ}{2} U \cdot U \end{matrix}

$D=I+2Usin\frac{\theta}{2}cos\frac{\theta}{2}+2sin^2\frac{\theta}{2}U·U \tag 3$
那么

\begin{matrix} (4) & {\vec{r}}^{' R} = D {\vec{r}}^{R} \end{matrix}

$\vec{r}'^R =D\vec{r}^R \tag 4$
之后的关系递推比较简单，假定刚体的固联坐标系为

b_{0}

$b_0$ ，刚体未转动时，刚体固联坐标系与参考坐标系 R重合，就是对向量

\vec{r}

$\vec{r}$ （注意，这是刚体没有转动的向量）而言，在两个坐标系中的矢量是相等的：

{\vec{r}}^{R} = {\vec{r}}^{b_{0}}

$\vec{r}^R=\vec{r}^{b_0}$
在刚体转动后，因为向量

\vec{r}

$\vec{r}$ 与坐标系

b_{0}

$b_0$ 都随着刚体一起转动，转动结束后得到向量

\vec{r}

$\vec{r}$ 和坐标系 b。向量

{\vec{r}}^{'}

$\vec{r}'$ 可以说相对于坐标系 b没动：

{\vec{r}}^{b_{0}} = {\vec{r}}^{' b}

$\vec{r}^{b_0}=\vec{r}’^b$
所以：

{\vec{r}}^{R} = {\vec{r}}^{' b}

$\vec{r}^{R}=\vec{r}’^b$
带入（4）可得：

{\vec{r}}^{' R} = D {\vec{r}}^{' b}

$\vec{r}'^R =D\vec{r}'^b$
那么 b系到 R系的旋转矩阵即为 D:

C_{b}^{R} = D = I + 2 U s i n \frac{θ}{2} c o s \frac{θ}{2} + 2 s i n^{2} \frac{θ}{2} U \cdot U

$C^R_b=D=I+2Usin\frac{\theta}{2}cos\frac{\theta}{2}+2sin^2\frac{\theta}{2}U·U$
后面的展开用四元数表示就不再赘述，但在此列出：

定义：

q_{0} = c o s \frac{θ}{2} q_{1} = l s i n \frac{θ}{2} q_{2} = m s i n \frac{θ}{2} q_{3} = n s i n \frac{θ}{2}

$q_0=cos\frac{\theta}{2}\\ q_1=lsin\frac{\theta}{2}\\ q_2=msin\frac{\theta}{2}\\ q_3=nsin\frac{\theta}{2}\\$
有：

C_{b}^{R} = [\begin{matrix} q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} - q_{0} q_{3}) & 2 (q_{1} q_{3} + q_{0} q_{2}) \\ 2 (q_{1} q_{2} + q_{0} q_{3}) & q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} - q_{0} q_{1}) \\ 2 (q_{1} q_{3} - q_{0} q_{2}) & 2 (q_{2} q_{3} + q_{0} q_{1}) & q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2} \end{matrix}]

$C^R_b= \left[ \begin{matrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2)\\ 2(q_1q_2+q_0q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2q_3-q_0q_1)\\ 2(q_1q_3-q_0q_2) & 2(q_2q_3+q_0q_1) & q_0^2-q_1^2-q_2^2+q_3^2\\ \end{matrix} \right]$

资源：

惯性导航-该资源仅供学术交流，如有侵权，请联系删除密码：iml9
学术不需要积分！