课题学习(十七)----姿态更新的四元数算法总结

   声明：因为接触本课题时间不长，对于四元数解法一直没太懂什么意思，本篇博客就对这几天的学习进行总结，肯定会有错误，希望读者能够帮忙指正。本篇博客主要参考秦永元老师《惯性导航》第九章第二小节以及几篇论文。

一、 四元数

1.1 四元数定义

   四元数就是由四个元构成的数：$$Q(q_0,q_1,q_2,q_3)=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$$
  其中，$q_0,q_1,q_2,q_3$是实数，在一些文献或者相关书籍里也会写作$q_1,q_2,q_3,q_4$或者$w,x,y,z$，$\mathbf{i},\mathbf{j},\mathbf{k}$即是互相正交的向量，又是虚单位$\sqrt{-1}$，具体规定体现在四元数乘法关系中：
$$\mathbf{i}⨂\mathbf{i}=-1,\mathbf{j}⨂\mathbf{j}=-1,\mathbf{k}⨂\mathbf{k}=-1$$
$$\mathbf{i}⨂\mathbf{j}=\mathbf{k},\mathbf{j}⨂\mathbf{k}=\mathbf{i},\mathbf{k}⨂\mathbf{i}=\mathbf{j}$$
$$\mathbf{j}⨂\mathbf{i}=-\mathbf{k},\mathbf{k}⨂\mathbf{j}=-\mathbf{i},\mathbf{i}⨂\mathbf{k}=-\mathbf{j}$$
   上述公式符合 右手螺旋定则，可以绘制一个如下所示的三维图，用右手螺旋定则判断：

1.2 四元数的表示方法

   （1）矢量式：$$Q=q_0+\mathbf{q}$$
  （2）复数式：$$Q=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$$
  （3）三角式：$$Q=cos\frac{\theta }{2}+\mathbf{u}sin\frac{\theta }{2}$$
  （4）指数式：$$Q=e^{\mathbf{u}\frac{\theta }{2}}$$
  （5）矩阵式：$$Q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$$

1.3 四元数大小----范数

   四元数的范数：$$||Q||=q_0^2+q_1^2+q_2^2+q_3^2$$
  若 $||Q||=1$，则称为规范四元数。

1.4 四元数的加减乘除

   （1）加法和减法：设$$\mathbf{Q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$$
$$\mathbf{P}=p_0+p_1\mathbf{i}+p_2\mathbf{j}+p_3\mathbf{k}$$
  则$$\mathbf{Q}\pm \mathbf{P}=(q_0\pm p_0)+(q_1\pm p_1)\mathbf{i}+(q_2\pm p_2)\mathbf{j}+(q_3\pm p_3)\mathbf{k}$$
  （2）乘法：$$a\mathbf{Q}=a{q}_0+a{q}_1\mathbf{i}+a{q}_2\mathbf{j}+a{q}_3\mathbf{k}$$
$$\begin{gathered} \mathbf{P}⨂\mathbf{Q}=(p_0+p_1\mathbf{i}+p_2\mathbf{j}+p_3\mathbf{k})⨂(q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}) \\ =(p_0{q}_0-p_1{q}_1-p_2{q}_2-p_3{q}_3)+(p_0{q}_1+p_1{q}_0+p_2{q}_3-p_3{q}_2)\mathbf{i}+ \\ (p_0{q}_2+p_2{q}_0+p_3{q}_1-p_1{q}_3)\mathbf{j}+(p_0{q}_3+p_3{q}_0+p_1{q}_2-p_2{q}_1)\mathbf{k} \\ =r_0+r_1\mathbf{i}+r_2\mathbf{j}+r_3\mathbf{k} \end{gathered}$$
  上式写成矩阵：

  公式比较麻烦，就不敲了，记住一点就好， 四元数乘法不满足交换律。
  （3）求逆：$$P^{-1}=\frac{P^{\ast }}{||P||}$$

二、四元数与姿态矩阵之间的关系

   秦老师的《惯性导航》在推导这部分时比较详细，推导部分这里就不再详细介绍了，主要讲一下几个重要的公式：
  《惯性导航》一书中取${\mathbf{u}}^R=\left[\begin{array}{c}l\\ m\\ n\end{array}\right]$ ，在薛启龙老师《Data Analytics for Drilling Engineering》的第三章“Dynamic Measurement of Spatial Attitude at the Bottom Rotating Drillstring”中，选择了${\mathbf{u}}^R=\left[\begin{array}{c}({\Theta }_x/\Theta )\\ ({\Theta }_y/\Theta )\\ ({\Theta }_z/\Theta )\end{array}\right]$ ，${\Theta }_x/\Theta 、{\Theta }_y/\Theta 、{\Theta }_z/\Theta$是导航系中的方向余弦。
   姿态转换矩阵：
$$\begin{gathered} C_b^R=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]+2cos\frac{\theta }{2}\left[\begin{array}{ccc}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{array}\right] \\ +2\left[\begin{array}{ccc}-(m^2+n^2)si{n}^2\frac{\theta }{2}& lmsi{n}^2\frac{\theta }{2}& lnsi{n}^2\frac{\theta }{2}\\ lmsi{n}^2\frac{\theta }{2}& -(l^2+n^2)si{n}^2\frac{\theta }{2}& mnsi{n}^2\frac{\theta }{2}\\ lnsi{n}^2\frac{\theta }{2}& mnsi{n}^2\frac{\theta }{2}& -(m^2+n^2)si{n}^2\frac{\theta }{2}\end{array}\right] \end{gathered}$$
  简化上式，令：$$\{\begin{array}{l}{q}_0=cos\frac{\theta }{2}\\ q_1=lsin\frac{\theta }{2}\\ q_2=msin\frac{\theta }{2}\\ q_3=nsin\frac{\theta }{2}\end{array}$$
  $||Q||=q_0^2+q_1^2+q_2^2+q_3^2=1$，为规范四元数，并且以$q_0,q_1,q_2,q_3$构造四元数：
$$Q=cos\frac{\theta }{2}+{\mathbf{u}}^{R}sin\frac{\theta }{2}$$
  （1）物理意义： 绕参考坐标系R内的一个的单位向量$\vec{u}$转动角度$\theta$，注意：不是转动$\frac{\theta }{2}$！！！
  （2）四元数可以确定b系至R（R是参考坐标系，一般选择导航坐标系n）系的姿态转换矩阵:
$$C_b^R=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2）& 2(q_1{q}_2-q_3{q}_0)& 2(q_1{q}_3+q_2{q}_0)\\ 2(q_1{q}_2+q_3{q}_0)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_1{q}_0)\\ 2(q_1{q}_3-q_2{q}_0)& 2(q_2{q}_3+q_1{q}_0)& 1-(q_1^2+q_3^2)\end{array}\right]$$
   由于是规范四元数，也可以写作下式
$$C_b^R=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_0^2& 2(q_1{q}_2-q_3{q}_0)& 2(q_1{q}_3+q_2{q}_0)\\ 2(q_1{q}_2+q_3{q}_0)& -q_1^2+q_2^2-q_3^2+q_0^2& 2(q_2{q}_3-q_1{q}_0)\\ 2(q_1{q}_3-q_2{q}_0)& 2(q_2{q}_3+q_1{q}_0)& -q_1^2-q_2^2+q_3^2+q_0^2\end{array}\right]$$
  （3）如果把${\mathbf{r}}^R、{\mathbf{r}}^b$看做是零标量的四元数，那么他们之间的变换关系可以采用四元数乘法表示：$${\mathbf{r}}^R=\mathbf{Q}⨂{\mathbf{r}}^b⨂{\mathbf{Q}}^{\ast }$$
  该式称为坐标变换的四元数乘表示法
  设参考坐标系为导航坐标系n，设航向角为$\Psi$，俯仰角为$\theta$，横滚角为$\gamma$:
$$\begin{gathered} C_b^n=\left[\begin{array}{ccc}cos\Psi cos\gamma +sin\Psi sin\theta sin\gamma & sin\Psi cos\theta & cos\Psi cos\gamma -sin\Psi sin\theta cos\gamma \\ -sin\Psi cos\gamma +cos\Psi sin\theta sin\gamma & cos\Psi cos\theta & -sin\Psi sin\gamma -cos\Psi sin\theta cos\gamma \\ -cos\theta sin\gamma & sin\theta & cos\theta cos\gamma \end{array}\right] \\ =\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right] \end{gathered}$$
  下图为姿态角的真值表：

  经过上述的分析：如果旋转四元数$\mathbf{Q}$已经确定，那么就可以表示出$C_b^n$:
$$\begin{gathered} C_b^n=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_0^2& 2(q_1{q}_2-q_3{q}_0)& 2(q_1{q}_3+q_2{q}_0)\\ 2(q_1{q}_2+q_3{q}_0)& -q_1^2+q_2^2-q_3^2+q_0^2& 2(q_2{q}_3-q_1{q}_0)\\ 2(q_1{q}_3-q_2{q}_0)& 2(q_2{q}_3+q_1{q}_0)& -q_1^2-q_2^2+q_3^2+q_0^2\end{array}\right] \\ =\left[\begin{array}{ccc}{M}_{11}& M_{12}& M_{13}\\ M_{21}& M_{22}& M_{23}\\ M_{31}& M_{32}& M_{33}\end{array}\right] \end{gathered}$$
  与基本旋转之后的转换矩阵对比，即可解算出姿态：
$$\begin{gathered} C_b^n=\left[\begin{array}{ccc}cos\Psi cos\gamma +sin\Psi sin\theta sin\gamma & sin\Psi cos\theta & cos\Psi cos\gamma -sin\Psi sin\theta cos\gamma \\ -sin\Psi cos\gamma +cos\Psi sin\theta sin\gamma & cos\Psi cos\theta & -sin\Psi sin\gamma -cos\Psi sin\theta cos\gamma \\ -cos\theta sin\gamma & sin\theta & cos\theta cos\gamma \end{array}\right] \\ =\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right] \end{gathered}$$
   一定注意的是，$C_b^n、C_n^b$矩阵存在着转置的关系！！！计算时一定要注意！！！
  例如，求解$\theta$时可以用$C_n^b$中可以第二列求解，即$$\theta =arctan(\frac{sin\theta }{\sqrt{(sin\Psi cos\theta {)}^2+(cos\Psi cos\theta {)}^2}})$$，也就是$$\theta =arctan(\frac{T_{32}}{\sqrt{T_{12}^2+T_{22}^2}})$$，由于知道了四元素构成的$C_b^n$，那么$$\theta =arctan(\frac{M_{32}}{\sqrt{M_{12}^2+M_{22}^2}})$$ ，这就是最终的计算公式。
  下面是我的代码中使用的姿态解算公式：

gyro_var->euler.pit = (asin(2*q2q3 + 2*q0q1))*57.29578f;
gyro_var->euler.roll = (atan2(-2*q1q3 + 2*q0q2, -2*q1q1-2*q2q2 + 1))*57.29578f;
gyro_var->euler.yaw = (atan2(2*q1q2 - 2*q0q3, -2*q2q2-2*q3q3+1))*57.29578f;

三、四元数微分方程

   令${\omega }_{Rb}^b=\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$，$$\frac{d\mathbf{Q}}{dt}=\frac{1}{2}{\mathbf{M}}^{\prime }({\omega }_{Rb}^b)\mathbf{Q}$$
  即$$\left[\begin{array}{c}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$$
  ${\omega }_{nb}^b$可以使用${\omega }_{nb}^b={\omega }_{ib}^b-C_n^b({\omega }_{ie}^n+{\omega }_{en}^n)$公式计算，${\omega }_{ib}^b$是陀螺仪的输出（对陀螺仪必须经过动、静态误差的补偿），${\omega }_{ie}^n、{\omega }_{en}^n$分别是 位置速率和地球自转速率：
$${\omega }_{ie}^n+{\omega }_{en}^n=\left(\begin{array}{c}\frac{-V_N}{R_M}\\ \frac{V_E}{R_N}+{\omega }_{ie}cosL\\ \frac{V_{E}tanL}{R_N}+{\omega }_{ie}sinL\end{array}\right)$$
  秦老师在第七章介绍了上式的一些定义：

  $L$为地理纬度，$R_M,R_N$分别是沿子午圈和沿卯酉圈的曲率半径：
$$R_M=R_e(1-f{)}^2[1-(2-f)si{n}^{2}L{]}^{-\frac{3}{2}}$$
$$R_N=R_e[1-(2-f)si{n}^{2}L{]}^{-\frac{1}{2}}$$
  $R_e$是地球(椭圆)的长轴，$R_p$是地球(椭圆)的短轴，$R_p=(1-f)R_e$ 。

四、四元数微分方程的毕卡求解法

   这里只介绍一下定时采样增量法，主要是通过对$Q$进行泰勒展开，对四元数进行各阶的近似：

  一般通常使用一阶近似算法即可，四元数计算如下：
  当选取：$$\{\begin{array}{l}{q}_0=cos\frac{\theta }{2}\\ q_1=lsin\frac{\theta }{2}\\ q_2=msin\frac{\theta }{2}\\ q_3=nsin\frac{\theta }{2}\end{array}$$
  $Q(k+1)$计算如下
$$Q(k+1)=Q(k)+\frac{1}{2}\left(\begin{array}{cccc}0& -\Delta {\theta }_x& -\Delta {\theta }_y& -\Delta {\theta }_z\\ \Delta {\theta }_x& 0& \Delta {\theta }_z& -\Delta {\theta }_y\\ \Delta {\theta }_y& -\Delta {\theta }_z& 0& \Delta {\theta }_x\\ \Delta {\theta }_z& \Delta {\theta }_y& -\Delta {\theta }_x& 0\end{array}\right)Q(k)$$
   注意，在一些论文或者书籍中，$q_0,q_1,q_2,q_3$与上面的选取不同，例如选取，当选取如下时：$$\{\begin{array}{l}{q}_0=lsin\frac{\theta }{2}\\ q_1=msin\frac{\theta }{2}\\ q_2=nsin\frac{\theta }{2}\\ q_3=cos\frac{\theta }{2}\end{array}$$
  $Q(k+1)$计算如下
$$Q(k+1)=Q(k)+\frac{1}{2}\left(\begin{array}{cccc}0& \Delta {\theta }_z& -\Delta {\theta }_y& \Delta {\theta }_x\\ -\Delta {\theta }_z& 0& \Delta {\theta }_x& \Delta {\theta }_y\\ \Delta {\theta }_y& -\Delta {\theta }_x& 0& \Delta {\theta }_z\\ -\Delta {\theta }_x& -\Delta {\theta }_y& -\Delta {\theta }_z& 0\end{array}\right)Q(k)$$

五、往期回顾

课题学习(一)----静态测量
课题学习(二)----倾角和方位角的动态测量方法（基于磁场的测量系统）
课题学习(三)----倾角和方位角的动态测量方法（基于陀螺仪的测量系统）
课题学习(四)----四元数解法
课题学习(五)----阅读论文《抗差自适应滤波的导向钻具动态姿态测量方法》
课题学习(六)----安装误差校准、实验方法
课题学习(七)----粘滑运动的动态算法
课题学习(八)----卡尔曼滤波动态求解倾角、方位角
课题学习(九)----阅读《导向钻井工具姿态动态测量的自适应滤波方法》论文笔记
课题学习(十)----阅读《基于数据融合的近钻头井眼轨迹参数动态测量方法》论文笔记
课题学习(十一)----阅读《Attitude Determination with Magnetometers and Accelerometers to Use in Satellite》
课题学习(十二)----阅读《Extension of a Two-Step Calibration Methodology to Include Nonorthogonal Sensor Axes》
课题学习(十三)----阅读《Calibration of Strapdown Magnetometers in Magnetic Field Domain》论文笔记
课题学习(十四)----三轴加速度计+三轴陀螺仪传感器-ICM20602
课题学习(十五)----阅读《测斜仪旋转姿态测量信号处理方法》论文
课题学习(十六)----阅读《Continuous Wellbore Surveying While Drilling Utilizing MEMS Gyroscopes Based…》论文