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

一、四元数解法

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

  用四元数$Q={\left[\begin{array}{cccc}q1& q2& q3& q4\end{array}\right]}^T$来描述b系统相对于n系统的旋转。定义如下：
Q = [ q 1 q 2 q 3 q 4 ] = [ ( Θ x / Θ ) s i n ( Θ / 2 ) ( Θ y / Θ ) s i n ( Θ / 2 ) ( Θ z / Θ ) s i n ( Θ / 2 ) c o s ( Θ / 2 ) ] Q = \begin{bmatrix} q1 \\ q2 \\q3\\q4\end{bmatrix} = \begin{bmatrix} (Θ_{x}/Θ)sin(Θ/2) \\ (Θ_{y}/Θ)sin(Θ/2) \\(Θ_{z}/Θ)sin(Θ/2)\\cos(Θ/2)\end{bmatrix} Q= ​q1q2q3q4​ ​= ​(Θx​/Θ)sin(Θ/2)(Θy​/Θ)sin(Θ/2)(Θz​/Θ)sin(Θ/2)cos(Θ/2)​ ​
  其中，$\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 = [ R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 ] R^n_{b} = \begin{bmatrix} R_{11} &R_{12}&R_{13}\\ R_{21} &R_{22}&R_{23}\\ R_{31} &R_{32}&R_{33} \end{bmatrix} Rbn​= ​R11​R21​R31​​R12​R22​R32​​R13​R23​R33​​ ​
= [ 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 ] =\begin{bmatrix} q^2_{1}-q^2_{2}-q^2_{3}+q^2_{4} &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^2_{1}+q^2_{2}-q^2_{3}+q^2_{4}&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^2_{1}-q^2_{2}+q^2_{3}+q^2_{4} \end{bmatrix} = ​q12​−q22​−q32​+q42​2(q1​q2​+q3​q4​)2(q1​q3​−q2​q4​)​2(q1​q2​−q3​q4​)−q12​+q22​−q32​+q42​2(q2​q3​+q1​q4​)​2(q1​q2​+q3​q4​)2(q2​q3​−q1​q4​)−q12​−q22​+q32​+q42​​ ​
  得到四元数的更新方程为:

  得到倾角θ，工具面φ和方位ψ如下
$$\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公式吧，最好整理个表格，要不然一个个的公式太麻烦了。

二、往期回顾

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