方向余弦矩阵与四元数

)

方向余弦矩阵

基本概念

方向余弦：
在坐标系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）,每个载体坐标轴都可以在参考坐标系上找到三个对应的方向余弦。最终得到九个方向余弦，把九个方向余弦写成矩阵形式就是方向余弦矩阵。
而方向余弦矩阵的写法有两种，一种是载体坐标系相对于参考坐标系，一种是参考坐标系相对于载体坐标系。
$$\begin{gathered} C_0^r=\left[\begin{array}{c} \\ {c}_{11}{c}_{12}{c}_{13}\\ c_{21}{c}_{22}{c}_{23}\\ c_{31}{c}_{32}{c}_{33}\end{array}\right]或C_r^0=\left[\begin{array}{c} \\ {c}_{11}{c}_{21}{c}_{31}\\ c_{12}{c}_{22}{c}_{32}\\ c_{13}{c}_{23}{c}_{33}\end{array}\right] \end{gathered}$$
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$$
$\left[\begin{array}{ccc}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{array}\right]$

已知方向余弦求欧拉角

方向余弦矩阵：$\left[\begin{array}{ccc}{T}_{11}& T_{21}& T_{31}\\ T_{12}& T_{22}& T_{32}\\ T_{13}& T_{23}& T_{33}\end{array}\right]$
推出：
横滚角：$$\theta =arcsin{T}_{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(xyz)]$$
$\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})$ $\overrightarrow{n_x}$ $.$ sin$(\frac{\theta }{2})$$\overrightarrow{n_y}$ $.$ sin$(\frac{\theta }{2})$$\overrightarrow{n_z}$ ]
(其中.为分隔用)

其中

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

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

$$\overrightarrow{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四元数的逆

四元数的逆就是他的共轭除以他的模
$$\overrightarrow{q^{-}1}=\frac{(\vec{q}\ast )}{(|\vec{q}|)}$$
一般使用单位四元数,所以他的逆和共轭是相等的
因为公式中涉及到四元数的乘法，在表示旋转时用叉乘，计算公式如下
$$[w_1(x_1{y}_1{z}_1)][w_2(x_2{y}_2{z}_2)]=\left[\begin{array}{c}{w}_1{w}_2-x_1{x}_2-y_1{y}_2-z_1{z}_2\\ \left(\begin{array}{c}{w}_1{x}_2+x_1{w}_2+z_1{y}_2-y_1{z}_2\\ w_1{y}_2+y_1{w}_2+x_1{z}_2-z_1{x}_2\\ w_1{z}_2+z_1{w}_2+y_1{x}_2-x_1{y}_2\end{array}\right)\end{array}\right]$$
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\ast 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)$$

由四元数叉乘计算公式 $\overrightarrow{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\ast sin\frac{\theta }{2}\ast i+y\ast sin\frac{\theta }{2}\ast j+z\ast sin\frac{\theta }{2}\ast 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_{1}i+q_{2}j+q_{3}k$四元数旋转的方向余弦矩阵公式如下：
$$\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=\left[\begin{array}{ccc}{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_0{q}_1+q_2{q}_3)\\ 2(q_1{q}_2-q_0{q}_3)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\cdot \left[\begin{array}{c}{x}_n\\ y_n\\ z_n\end{array}\right]$$
$$四元数表示的旋转矩阵公式$$

姿态表示的三种方法

欧拉角

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

方向余弦矩阵

$\left[\begin{array}{ccc}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{array}\right]$

四元数

$$\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]=\left[\begin{array}{ccc}{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_0{q}_1+q_2{q}_3)\\ 2(q_1{q}_2-q_0{q}_3)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\cdot \left[\begin{array}{c}{x}_n\\ y_n\\ z_n\end{array}\right]$$

$$spark-1$$