05、姿态解算---四元素求姿态变换矩阵2

最新推荐文章于 2024-07-24 16:20:01 发布

keaishamao

最新推荐文章于 2024-07-24 16:20:01 发布

阅读量648

点赞数 19

分类专栏：姿态解算文章标签：矩阵线性代数算法 c语言嵌入式硬件

本文链接：https://blog.csdn.net/keaishamao/article/details/137781538

版权

姿态解算专栏收录该内容

7 篇文章 1 订阅

订阅专栏

继续上一节，对四元素旋转矩阵的推导2

公式14：

将公式12、13代入公式9中，得出：

$\vec{{r}'}^{E}=\vec{r}^{E}+(1-cos\theta )U*U\vec{r}^{E}+U\vec{r}^{E}sin\theta$

$=[I+(1-cos\theta )U*U+Usin\theta ]\vec{r}^{E}$

其中I为单位矩阵： $I=\begin{bmatrix} 1 &0 &0 \\ 0 &1 & 0\\ 0& 0 & 1 \end{bmatrix}$

公式15：

三角函数半角公式、倍角公式：

$1-cos\theta =2sin^{2}\frac{\theta}{2}$

$sin\theta =2sin\frac{\theta }{2}cos\frac{\theta }{2}$

公式16：

将公式15代入公式14得：

$\vec{{r}'}^{E}=[I+2sin^{2}\frac{\theta }{2}U*U+2Usin\frac{\theta }{2}cos\frac{\theta }{2}]\vec{r}^{E}$

公式17：

令 $D=[I+2sin^{2}\frac{\theta }{2}U*U+2Usin\frac{\theta }{2}cos\frac{\theta }{2}]$

则有： $\vec{{r}'}^{E}=D\vec{r}^{E}$

公式18：

我们做的所有推导都是求旋转之后的 ${R}'$ 和旋转之前的 $R$ 之间的关系。D的公式包含了旋转轴和旋转角度的信息，这样旋转矩阵实际上就是求D的旋转矩阵。

由于在没有旋转之前，物体坐标系(b系)与地理坐标系重合，则有：

$\vec{r}^{E}=\vec{r}^{b0}$

公式19：

对于物体来说，在转动过程中，位置向量与物体的坐标系之间的相对角度位置始终不变，即有：

$r^{b0}=\vec{{r}'}^{b}$

公式20：

将公式19代入公式18，则有：

$\vec{r}^{E}=\vec{{r}'}^{b}$

公式21：

将公式20代入公式17，得出：

$\vec{{r}'}^{E}=D\vec{{r}'}^{b}$

公式22：

记D为物体坐标系至地址坐标系的坐标变换矩阵，记为：

$c_{b}^{E}=D=[I+2Usin\frac{\theta }{2}cos\frac{\theta }{2}+2sin^{2}\frac{\theta }{2}U*U]$

在数学中四元素定义为： $\vec{Q}(q0,q1,q2,q3)=q0+q1\vec{i}+q2\vec{j}+q3\vec{k}$

令: $U=\begin{bmatrix} 0 & -n & m\\ n&0 &-l \\ -m &l & 0 \end{bmatrix}$

则有：

$D = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0& 1 \end{bmatrix}+2cos\frac{\theta }{2}\begin{bmatrix} 0 & -nsin\frac{\theta }{2} &msin\frac{\theta }{2} \\ nsin\frac{\theta }{2} & 0 &-lsin\frac{\theta }{2} \\ -msin\frac{\theta }{2} & lsin\frac{\theta }{2}& 0 \end{bmatrix}+2\begin{bmatrix} -(m^{2}+n^{2})sin^{2}\frac{\theta }{2} & lmsin^{2}\frac{\theta }{2} & nlsin^{2}\frac{\theta }{2}\\ lmsin^{2}\frac{\theta }{2}& -(l^{2}+n^{2})sin^{2}\frac{\theta }{2} & mnsin^{2}\frac{\theta }{2}\\ nlsin^{2}\frac{\theta }{2}&mnsin^{2}\frac{\theta }{2} & -(m^{2}+l^{2})sin^{2}\frac{\theta }{2} \end{bmatrix}$

公式23：

①、四元素的三角形式为： $cos\frac{\theta }{2}+(l\vec{i}+m\vec{j}+n\vec{k})sin\frac{\theta }{2}=cos\frac{\theta }{2}+\vec{u}sin\frac{\theta }{2}$

可以记为： $q0=cos\frac{\theta }{2},q1=lsin\frac{\theta }{2},q2=msin\frac{\theta }{2},q3=nsin\frac{\theta }{2}$

②、用 $q0,q1,q2,q3$ 表示的四元素和三角形式表示的四元素联系起来：

$\vec{Q}=q0+q1\vec{i}+q2\vec{j}+q3\vec{k}=cos\frac{\theta }{2}+(l\vec{i}+m\vec{j}+n\vec{k})sin\frac{\theta }{2}=cos\frac{\theta }{2}+\vec{u^{E}}sin\frac{\theta }{2}$

从以上推导，我们可以得出，从一个坐标系变换到另一个坐标系的变换矩阵，通过构造四元素可进一步得到以四元素表示的变换矩阵。构造的四元素描述了物体的顶点转动问题，当我们只关心物体系相对于地理系的角度位置时，可以认为物体系是经过无中间过程的一次性等效旋转形成的。四元素包含了这种等效信息， $\vec{u}^{E}$ 为旋转轴和旋转方向，θ为旋转角度：

将 $q0=cos\frac{\theta }{2},q1=lsin\frac{\theta }{2},q2=msin\frac{\theta }{2},q3=nsin\frac{\theta }{2}$ 代入公式22得出：

$c_{b}^{E}=\begin{bmatrix} 1 & 0 &0 \\ 0 &1 &0 \\ 0& 0 &1 \end{bmatrix}+\begin{bmatrix} 0 &-2q0q3 & 2q0q2\\ 2q0q3 & 0&-2q0q1 \\ -2q0q2 & 2q0q1 & 0 \end{bmatrix}+\begin{bmatrix} -2(q2^{2}+q3^{2}) & 2q1q2 &2q1q3 \\ 2q1q2&-2(q1^{2}+q3^{2}) & 2q2q3\\ 2q1q3& 2q2q3& -2(q2^{2}+q1^{2}) \end{bmatrix}=\begin{bmatrix} 1-2(q2^{2}+q3^{2}) &2(q1q2-q0q3) &2(q0q2+q1q3) \\ 2(q0q3+q1q2)& 1-2(q1^{2}+q3^{2}) & 2(q2q3-q0q1)\\ 2(q1q3-q0q2) &2(q0q1+q2q3) &1 -2(q2^{2}+q1^{2}) \end{bmatrix}$

公式24：

根据四元素的范数
，以及四元素的三角表达式，见《03、姿态解算---四元素》，可求得：

$\left \| \vec{Q} \right \|=q0^{2}+q1^{2}+q2^{2}+q3^{2}=cos^{2}\frac{\theta }{2}+(l^{2}+m^{2}+n^{2})sin^{2}\frac{\theta }{2}=1$

其中 $\vec{u}$ 为旋转轴，也为单位向量，其模为1，也即有 $l^{2}+m^{2}+n^{2} = 1$ ，又有 $sin^{2}\frac{\theta }{2}+cos^{2}\frac{\theta }{2}=1$ ，故有 $\left \| \vec{Q} \right \|=1$ 。

所以可以得出描述物体转动的四元素为规范化的四元素。

将 $\left \| \vec{Q} \right \|=q0^{2}+q1^{2}+q2^{2}+q3^{2}=1$ 代入公式23中，可以得出：

$c_{b}^{E}=\begin{bmatrix} q0^{2}+q1^{2}+q2^{2}+q3^{2}&2(q1q2-q0q3) &2(q1q3+q0q2) \\ 2(q1q2+q0q3) & q0^{2}-q1^{2}+q2^{2}-q3^{2} &2(q2q3-q0q1) \\ 2(q1q3-q0q2)& 2(q2q3+q0q1) & q0^{2}-q1^{2}-q2^{2}+q3^{2} \end{bmatrix}$