课题学习(二十一)----姿态更新的四元数算法推导

   声明：本人水平有限，博客可能存在部分错误的地方，请广大读者谅解并向本人反馈错误。
   最近需要使用AEKF对姿态进行结算，所以又对四元数进了深入的学习，本篇博客仅对四元数进行推导，后续会对基于四元数的AEKF算法进行实现。

一、 坐标变化

1.1 基本概念

  首先，我对参考坐标系进行了定义，即选择“东-北-天”（或者叫做“右-前-上”），如下图：

  并且记：

1. p(pitch):俯仰角（绕X轴旋转）；
2. r(roll):横滚角（绕Y轴旋转）；
3. y(yaw):航向角（绕Z轴旋转）；

  并且设角度逆时针旋转为正。

1.2 绕X轴旋转

  由上图可以得出由X-Y-Z–>X’-Y’-Z’的变换矩阵：
$$\left[\begin{array}{ccc}1& 0& 0\\ 0& cosp& sinp\\ 0& -sinp& cosp\end{array}\right]$$

1.3 绕Y轴旋转

  变换矩阵：
$$\left[\begin{array}{ccc}cosr& 0& -sinr\\ 0& 1& 0\\ sinr& 0& cosr\end{array}\right]$$

1.4 绕Z轴旋转

  变换矩阵：
$$\left[\begin{array}{ccc}cosy& siny& 0\\ -siny& cosy& 0\\ 0& 0& 1\end{array}\right]$$

1.5 坐标变换矩阵

  在此假设旋转的顺序为：Z–>X–>Y，那么最后的坐标变换矩阵为（注意：一定要把矩阵相乘的顺序搞对）：
$$C_n^b=C_2^b(绕Y轴旋转矩阵)\ast C_1^2(绕X轴旋转矩阵)\ast C_n^1(绕Z轴旋转矩阵)$$
$$=\left[\begin{array}{ccc}cosr& 0& -sinr\\ 0& 1& 0\\ sinr& 0& cosr\end{array}\right]\ast \left[\begin{array}{ccc}1& 0& 0\\ 0& cosp& sinp\\ 0& -sinp& cosp\end{array}\right]\ast \left[\begin{array}{ccc}cosy& siny& 0\\ -siny& cosy& 0\\ 0& 0& 1\end{array}\right]$$
$$=\left[\begin{array}{ccc}cosr\ast cosy-sinp\ast sinr\ast siny& cosy\ast siny+sinp\ast sinr\ast cosy& -cosp\ast sinr\\ -cosp\ast siny& cosp\ast cosy& sinp\\ sinr\ast cosy+sinp\ast cosr\ast siny& siny\ast sinr-sinp\ast cosr\ast cosy& cosrcosp\end{array}\right]$$
  注意：$C_n^b$是n系到b系的坐标变换矩阵，而在求解时，是将b系的姿态变换到n系上求解，所以最后要对$C_n^b$转置变为$C_b^n$ 。
$$C_b^n=(C_n^b{)}^T$$
$$=\left[\begin{array}{ccc}cosr\ast cosy-sinp\ast sinr\ast siny& -cosp\ast siny& sinr\ast cosy+sinp\ast cosr\ast siny\\ cosy\ast siny+sinp\ast sinr\ast cosy& cosp\ast cosy& siny\ast sinr-sinp\ast cosr\ast cosy\\ -cosp\ast sinr& sinp& cosr\ast cosp\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]$$

二、 四元数

2.1 基础概念回忆

  在课题学习(十七)----姿态更新的四元数算法总结博客中，已经对四元数进行了比较详细的讲解，在这里只对四元数的物理意义重述一次：

  其中，${\overrightarrow{u}}^R=[l,m,n]$，且四元数定义为：
$$\{\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}$$

2.2 绕X轴旋转

  把${\overrightarrow{u}}^R=[l,m,n]=[1,0,0]$代入$\{\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}$,那么就可以得到绕X轴的四元数$$q_x=[cos\frac{p}{2},sin\frac{p}{2},0,0]$$

2.3 绕Y、Z轴旋转

  绕Y轴旋转：把${\overrightarrow{u}}^R=[l,m,n]=[0,1,0]$代入$\{\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}$,那么就可以得到绕Y轴的四元数$$q_y=[cos\frac{r}{2},0,sin\frac{r}{2},0]$$
  绕Y轴旋转：把${\overrightarrow{u}}^R=[l,m,n]=[0,0,1]$代入$\{\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}$,那么就可以得到绕Z轴的四元数$$q_z=[cos\frac{y}{2},0,0,sin\frac{y}{2}]$$

2.4 用四元数表示旋转矩阵(坐标变换矩阵)

  在第一节中，我们规定了旋转顺序为：Z–>X–>Y，那么最后四元数的旋转矩阵为（参考博客：《欧拉角和四元数之间转换公式推导》）：$$q=q_z⨂q_x⨂q_y$$
  "$⨂$"运算在《课题学习(十七)----姿态更新的四元数算法总结 》博客中也有介绍，大家可以参考：

  在这部分，我有一点未搞清楚，就是四元数的乘法不满足交换律，即“$\mathbf{P}⨂\mathbf{Q}\ne \mathbf{Q}⨂\mathbf{P}$” ,那么公式$q=q_z⨂q_x⨂q_y$不能更换相乘的顺序，所以…我搞不懂这个顺序是否正确。
  反正最后按照$q=q_z⨂q_x⨂q_y$得到的四元数为：

三、四元数与坐标变换矩阵的关系

  在秦永元老师的《惯性导航（第二版）》9.2.2节（P248-P253）中对四元数与坐标变换矩阵的关系进行了推导，大家可以参考学习。
  最后得到的四元数表示的坐标变换矩阵为：
$$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_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)& q_0^2-q_1^2+q_2^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)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$$
  同时，大家可以对本博客2.4节推导出来的四元数进行反推，比如我计算了$2(q_2{q}_3+q_1{q}_0)$，最后得到的结果就是$sinp$：

四、往期回顾

课题学习(一)----静态测量
课题学习(二)----倾角和方位角的动态测量方法（基于磁场的测量系统）
课题学习(三)----倾角和方位角的动态测量方法（基于陀螺仪的测量系统）
课题学习(四)----四元数解法
课题学习(五)----阅读论文《抗差自适应滤波的导向钻具动态姿态测量方法》
课题学习(六)----安装误差校准、实验方法
课题学习(七)----粘滑运动的动态算法
课题学习(八)----卡尔曼滤波动态求解倾角、方位角
课题学习(九)----阅读《导向钻井工具姿态动态测量的自适应滤波方法》论文笔记
课题学习(十)----阅读《基于数据融合的近钻头井眼轨迹参数动态测量方法》论文笔记
课题学习(十一)----阅读《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…》论文
课题学习(十七)----姿态更新的四元数算法总结
课题学习(十八)----捷联测试电路设计与代码实现(基于MPU6050和QMC5883L)
课题学习(十九)----Allan方差：陀螺仪噪声分析
课题学习(二十)----阅读《近钻头井斜动态测量重力加速度信号提取方法研究》论文