课题学习(四)----四元数解法

一、四元数解法

  为了求解惯性导航的力学方程，姿态矩阵$R_b^b$可以有姿态微分方程得到。其中，四元数是常用的方法，如下图所示，假设刚体在原点旋转，根据欧拉定理，运动坐标系(b系列)相对于导航坐标系(n系列)的方向，相当于b系绕等效轴旋转一个角度Θ。

  用四元数$Q={\left[\begin{array}{cccc}q1& q2& q3& q4\end{array}\right]}^T$来描述b系统相对于n系统的旋转。定义如下：
$$Q=\left[\begin{array}{c}q1\\ q2\\ q3\\ q4\end{array}\right]=\left[\begin{array}{c}({\Theta }_x/\Theta )sin(\Theta /2)\\ ({\Theta }_y/\Theta )sin(\Theta /2)\\ ({\Theta }_z/\Theta )sin(\Theta /2)\\ cos(\Theta /2)\end{array}\right]$$
  其中，$\Theta =\sqrt{{\Theta }_x^2+{\Theta }_y^2+{\Theta }_z^2}$,${\Theta }_x/\Theta ,{\Theta }_y/\Theta ,{\Theta }_z/\Theta$是n坐标系中的方向余弦。
  从四元数的定义来看，$q_1^2+q_2^2+q_3^2+q_4^2=1$，四元数的分量不是相互独立的，只需要三个独立的四元数分量来描述坐标轴的旋转。然而，通常存在计算误差，定义为$\Delta =1-q_1^2+q_2^2+q_3^2+q_4^2$。为了纠正这一错误，每次计算后都需要将向量形式的四元数Q更新为以下公式:

  用一阶微分方程描述四元数的时域变化:

  其中,$w_x,w_y,w_z$是载体的角速度。
  求解一阶微分方程，根据tk时刻的Qk，求出tk + 1时刻的Qk + 1，如下所示:
  当时刻tk，四元数确定时，$R_b^n$可直接由下式确定:
$$R_b^n=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]$$
$$=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2-q_3{q}_4)& 2(q_1{q}_2+q_3{q}_4)\\ 2(q_1{q}_2+q_3{q}_4)& -q_1^2+q_2^2-q_3^2+q_4^2& 2(q_2{q}_3-q_1{q}_4)\\ 2(q_1{q}_3-q_2{q}_4)& 2(q_2{q}_3+q_1{q}_4)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right]$$
  得到四元数的更新方程为:

  得到倾角θ，工具面φ和方位ψ如下
$$\theta =arctan(\frac{R_{32}}{\sqrt{R_{12}^2+R_{22}^2}})$$
$$\varphi =arctan(\frac{-R_{31}}{R_{33}})$$
$$\psi =arctan(\frac{-R_{12}}{R_{22}})$$
  进而可得到井斜角$I=90-\theta$.
  在捷联导航系统中，由于GPS对准，在建立误差模型后，采用卡尔曼滤波方法可以获得准确的载体姿态和位置信息。但在随钻测量中，无法使用GPS信号校正，需要重新建立卡尔曼滤波模型。随钻陀螺测量系统的卡尔曼滤波模型，由于采用了惯性陀螺仪和加速度传感器，其误差模型与航空航天领域捷联导航的卡尔曼滤波误差模型一致。从可靠性的角度来看，目前在钻井测量领域，基于磁的测量系统比陀螺仪系统更具应用优势。然而，由于磁干扰等因素，磁基系统不可能完全完善。未来的发展方向必然是复合模式测量系统，即磁传感器与陀螺仪系统相结合的随钻测量系统。

  剧透一下，后续有时间更新一下LaTex公式吧，最好整理个表格，要不然一个个的公式太麻烦了。

二、往期回顾

课题学习(一)----静态测量
课题学习(二)----倾角和方位角的动态测量方法（基于磁场的测量系统）
课题学习(三)----倾角和方位角的动态测量方法（基于陀螺仪的测量系统）