姿态解算

方向余弦矩阵

基本概念
已知欧拉角求方向余弦
已知方向余弦求欧拉角

四元数

基本概念
四元数表征姿态

<1四元数的共轭
<2四元数的逆

四元数与方向余弦

姿态表示的三种方法

欧拉角
方向余弦矩阵
四元数
$$spark-1$$

)

方向余弦矩阵

基本概念

方向余弦：
在坐标系R中设v为一个空间向量，在R坐标系下的投影为（vx,vy,vz）
与其x y z 轴分别成 $\alpha \beta \gamma$ 角度，则 $cos\alpha ,cos\beta,cos\gamma$ 分别为v在三轴的方向余弦，大小分别等于|vx|,|vy|,|vz|。

方向余弦矩阵（direction cosine matrix）
在一个平面内对向量进行旋转相当于对坐标进行旋转，初始状态下令载体坐标系和参考坐标系完全重合。
方向余弦矩阵便是存放着一系列方向余弦，设r(x,y,z)为参考坐标系，O(X1，Y1，Z1)为载体坐标系，载体转动时，载体坐标系会相对于参考坐标系转动，把载体坐标系的三个轴当做三个单位向量（vx,vy,vz）,每个载体坐标轴都可以在参考坐标系上找到三个对应的方向余弦。最终得到九个方向余弦，把九个方向余弦写成矩阵形式就是方向余弦矩阵。
而方向余弦矩阵的写法有两种，一种是载体坐标系相对于参考坐标系，一种是参考坐标系相对于载体坐标系。
$C_0^r=\begin{bmatrix}\\ c_{11} c_{12}c_{13} \\ c_{21} c_{22} c_{23} \\ c_{31}c_{32}c_{33} \end{bmatrix}或C_r^0=\begin{bmatrix}\\ c_{11} c_{21}c_{31} \\ c_{12} c_{22} c_{32} \\ c_{13}c_{23}c_{33} \end{bmatrix}$
C_0^r表称O系对于r系的方向余弦矩阵,这两个矩阵互为转置矩阵。
$C_{12}=cos(x_r.x_0)$ 载体坐标系上的$x_0相对于参考坐标系上的x_r的方向余弦，
$C_{12}=cos(x_r,y_0)同理$ $y_0相对于参考坐标系上x_r的方向余弦$

已知欧拉角求方向余弦

$\psi \theta \gamma$
$\begin{bmatrix} cos \gamma cos\psi +sin\gamma sin\psi sin \theta& -cos\gamma sin\psi sin\theta & -sin\gamma cos\theta \\ sin\psi cos\theta&cos\psi cos\theta &sin\theta \\sin\gamma cos\psi -cos\gamma sin\psi sin\theta &-sin\gamma sin\psi -cos\gamma cos\psi sin\theta&cos\gamma cos\theta \end{bmatrix}$

已知方向余弦求欧拉角

方向余弦矩阵： $\begin{bmatrix} T_{11}&T_{21}&T_{31}\\T_{12}&T_{22}&T_{32} \\T_{13}&T_{23}&T_{33}\end{bmatrix}$
推出：
横滚角： $\theta=arcsinT_{32}$
俯仰角：
$\gamma=arctan(-\frac{T_{32}}{T_{33}})$
偏航角：
$\psi =arctan(\frac {T_{12}}{T_{22}})$

四元数

基本概念

四元数是由实数和虚数组成的一组超复数，假设Q是一个四元数。

                      Q =  w  +  xi +  yj +  zj

其中w表示四元数的实数大小，x,y,z表示虚数大小,实部单位为1，虚部单为为i,j,k。

四元数一般表示：
$\vec{q}=[w \vec{v}]=[w (xyz)]$

四元数表征姿态

其中w为实数部分， $\vec{v}$ 为矢量部分，首先讨论如何把一个空间向量转换到四元数。
假设在三维空间中有一个点（x,y,z),拓展到四元数空间有
$\vec{p}=[0(x y z)]$
$\vec{p}$ 就是三维空间中一点四元数的表示，而旋转是通过旋转四元数来实现的。现在假设空间中有一个向量 $\vec{n}$ , $(n_x,n_y,n_z)$ ,若用四元数来表示点p(x,y,z)绕向量 $\vec{n}$ 旋转 $\theta$ 角度后 $p^,$
定义旋转四元数为 $\vec{p}$ 则对于本次旋转有：

$\vec{p}=[cos$ $(\frac{\theta}{2})$ sin $(\frac{\theta}{2})$ $\vec{n}$ ]=[cos $(\frac{\theta}{2})$ $.$ sin $(\frac{\theta}{2})$ $\vec{n_x}$ $.$ sin $(\frac{\theta}{2})$ $\vec{n_y}$ $.$ sin $(\frac{\theta}{2})$ $\vec{n_z}$ ]
(其中.为分隔用)

其中

$\vec{n_x}$	$\vec{n_y}$	$\vec{n_z}$
$\vec{n_x}$ = $n_x$ *i	$\vec{n_y}$ = $n_y$ *j	$\vec{n_z}$ = $n_z$ *k

若想得到旋转后的矩阵只需要执行以下式子

$\vec{p^,}=\vec{q}\vec{p} \vec{q}^-1$

$(\vec{q}^-1为\vec{q}的逆矩阵)$

$想要推导下面的公式需要了解两个概念$

<1四元数的共轭

四元数的共轭就是让四元数的向量部分取负，记作
$\vec{p}$ =[w $\vec{v}$ ]=[w - $\vec{v}$ ]=[w (-x -y -z)]
四元数与他的共轭代表反向的角位移，因为相当于旋转轴反向

<2四元数的逆

四元数的逆就是他的共轭除以他的模
$\vec{q^-1} = \frac{(\vec{q}*)}{(|\vec{q}|)}$
一般使用单位四元数,所以他的逆和共轭是相等的
因为公式中涉及到四元数的乘法，在表示旋转时用叉乘，计算公式如下
$[w_1 (x_1 y_1 z_1)][w_2 ( x_2 y_2 z_2)]=\begin{bmatrix} w_1w_2 -x_1x_2 -y_1y_2-z_1z_2\\ \begin{pmatrix}w_1x_2 + x_1w_2 +z_1y_2-y_1z_2 \\w_1y_2+y_1w_2+x_1z_2-z_1x_2 \\w_1z_2+z_1w_2+y_1x_2-x_1y_2 \end{pmatrix} \end{bmatrix}$
example：

假设空间中有一个点p(0,1,0)饶z轴逆时针旋转90度，求旋转后的 $p^,$ 点。理论上 $p^,$ 点为（-1,0,0）。
首先把p(0,1,0)拓展成一个四元数 $\vec{p}$ =(0,0,j,0),然后定义旋转四元数，旋转轴为z轴化为单位向量（0,0,1）,因为旋转角度为90度所以------

$\vec{q}=(cos45^。,0,0,1*sin45^。k)=(\frac{\sqrt 2}{2},0,0,\frac{\sqrt 2}{2}k)$
单位四元数的逆的共轭是相等的

$\vec{q}^-1=(\frac{\sqrt 2}{2},0,0,-\frac{\sqrt 2}{2}k)$

由四元数叉乘计算公式 $\vec{p^,}=\vec{q}\vec{p} \vec{q}^-1$ 用上述乘法公式计算
$\vec{q}\vec{p}=(0,\frac{\sqrt 2}{2}kj,\frac{\sqrt 2}{2}j,0)$
k*j=-i再乘以 $\vec{q}^{-1}得到\vec{p}^{,}=[0,-i,0,0]$ ,带回去得到点[-1,0,0]即为旋转之后的坐标。

四元数与方向余弦

由前面的介绍知道了旋转四元数 $q=cos\frac {\theta}{2}+x*sin\frac{\theta}{2}*i+y*sin\frac {\theta}{2}*j+z*sin\frac{\theta}{2}*k$
这里令 $q_0=cos\frac{\theta}{2},q_1=sin\frac{\theta} {2},q_2=sin\frac{\theta}{2},q_3=sin\frac{\theta}{2},$
q= $q_0+q_1i+q_2j+q_3k$ 四元数旋转的方向余弦矩阵公式如下：
$\begin{bmatrix} x_b\\y_b\\z_b\end{bmatrix}=\begin{bmatrix}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_0q_1+q_2q_3) \\2(q_1q_2-q_0q_3)&2(q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \\ \end{bmatrix}\cdot\begin{bmatrix}x_n \\y_n \\z_n\end{bmatrix}$
$四元数表示的旋转矩阵公式$

姿态表示的三种方法

欧拉角

直观的欧拉角横滚角roll 俯仰角pitch 偏航角 yaw