一种基于地磁梯度张量测量的水下载体全姿态确定方法

摘要

本发明属于水下地磁辅助导航领域，具体涉及到一种基于地磁梯度张量测量的水下载体全姿态确定方法。本发明包括：在载体系下建立三维直角坐标系得到磁梯度张量的5个分量；从预先存储的地磁梯度张量数据库中提取地理系下地磁梯度张量的5个独立分量；代入关于q₀、q₁、q₂和q₃的非线性方程组；四元数和代入载体系与地理系的变换矩阵，得到俯仰角θ的估计值。本发明提出的基于地磁异常反演的水下载体定位技术具有成本低、灵敏度高、抗干扰能力强及无积累误差等特性，解决单个磁强计无法依赖地磁场自主定姿的问题，为水下载体姿态估计提供一种自主的、隐蔽的途径。

主权项

一种基于地磁梯度张量测量的水下载体全姿态确定方法，其特征在于：（1）在载体系下建立三维直角坐标系x_b、y_b、z_b，两个三轴正交的磁力计分别放置在x_b轴以原点为中心对称的A、B两点，分别测量磁场分量（h₄、h₅、h₆）和（h₁、h₂、h₃）；另一对二轴正交磁力计分别置于y_b轴以原点为中心对称的C、D两点，分别测量磁场分量（h₉、h₁₀）和（h₇、h₈），所有磁力计的敏感轴方向一致，根据地磁梯度张量G^b的5个分量及与磁场分量h₁～h₁₀的关系式，$\left\{\begin{array}{c}{g}_{\mathrm{xx}}^b=\frac{h_4-h_1}{L_x},g_{\mathrm{yy}}^b=\frac{h_9-h_7}{L_y},g_{\mathrm{yx}}^b=\frac{h_5-h_2}{L_x},\\ g_{\mathrm{zy}}^b=\frac{h_{10}-h_8}{L_y},g_{\mathrm{zx}}^b=\frac{h_6-h_3}{L_x}\end{array}\right.$得到磁梯度张量的5个分量，L_x和L_y分别为x_b和y_b方向上梯度测量基线长；（2）根据惯性/地磁组合导航系统的指示位置，从预先存储的地磁梯度张量数据库中提取地理系下地磁梯度张量Gⁿ的5个独立分量及（3）将步骤1、2得到的载体系下地磁梯度张量分量和地理系下地磁梯度张量分量及代入关于q₀、q₁、q₂和q₃的非线性方程组:$\begin{array}{c}{g}_{\mathrm{xx}}^b=4g_{\mathrm{xz}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3+q_0{q}_2)+8g_{\mathrm{zy}}^n(q_1{q}_2-q_0{q}_3)(q_1{q}_3+q_0{q}_2)+4g_{\mathrm{yy}}^n{(q_1{q}_2-q_0{q}_3)}^2\\ +g_{\mathrm{xx}}^n{(q_0^2+q_1^2-q_2^2-q_3^2)}^2+4g_{\mathrm{xy}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_2-q_0{q}_3)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n){(q_1{q}_3+q_0{q}_2)}^2\end{array}$$\begin{array}{c}{g}_{\mathrm{yy}}^b=4g_{\mathrm{xx}}^n{(q_1{q}_2+q_0{q}_3)}^2+4g_{\mathrm{yx}}^n(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2+q_2^2-q_3^2)+8g_{\mathrm{zx}}^n(q_1{q}_2+q_0{q}_3)(q_2{q}_3-q_0{q}_1)\\ +g_{\mathrm{yy}}^n{(q_0^2-q_1^2+q_2^2-q_3^2)}^2+4g_{\mathrm{zy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3-q_0{q}_1)-4(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)\end{array}$$\begin{array}{c}{g}_{\mathrm{zy}}^b=4g_{\mathrm{xx}}^n(q_1{q}_2+q_0{q}_3)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[2(q_1{q}_2+q_0{q}_3)(q_2{q}_3+q_0{q}_1)+(q_0^2-q_1^2+q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)]\\ +2g_{\mathrm{zx}}^n[(q_1{q}_2+q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_2{q}_3-q_0{q}_1)(q_1{q}_3-q_0{q}_2)]+2g_{\mathrm{yy}}^n(q_0^2-q_1^2+q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)\\ {+g}_{\mathrm{zy}}^n[(q_0^2-q_1^2+q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_2{q}_3-q_0{q}_1)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_2{q}_3-q_0{q}_1)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}$$\begin{array}{c}{g}_{\mathrm{zx}}^b=2g_{\mathrm{xx}}^n(q_0^2+q_1^2-q_2^2-q_3^2)(q_1{q}_3-q_0{q}_2)+2g_{\mathrm{yx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_2{q}_3+q_0{q}_1)+2(q_1{q}_2-q_0{q}_3)(q_1{q}_3-q_0{q}_2)]\\ +g_{\mathrm{zx}}^n[(q_0^2+q_1^2-q_2^2-q_3^2)(q_0^2-q_1^2-q_2^2+q_3^2)+4(q_1{q}_3+q_0{q}_2)(q_1{q}_3-q_0{q}_2)]+4g_{\mathrm{yy}}^n(q_1{q}_2-q_0{q}_3)(q_2{q}_3+q_0{q}_1)\\ {+2g}_{\mathrm{zy}}^n[(q_1{q}_2-q_0{q}_3)(q_0^2-q_1^2-q_2^2+q_3^2)+2(q_1{q}_3+q_0{q}_2)(q_2{q}_3+q_0{q}_1)]\\ -2(g_{\mathrm{xx}}^n+g_{\mathrm{yy}}^n)(q_1{q}_3+q_0{q}_2)(q_0^2-q_1^2-q_2^2+q_3^2)\end{array}$关于q₀、q₁、q₂和q₃的非线性方程组可由Gⁿ和G^b的关系式得到，其中T表示矩阵转置,是地理系到载体系的变换矩阵，且是载体系与地理系的变换矩阵，且结合q₀、q₁、q₂和q₃的约束关系利用牛顿下山法求解关于q₀、q₁、q₂和q₃的非线性方程组；（4）将步骤（3）中得到的满足迭代条件的四元数和代入载体系与地理系的变换矩阵，$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_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\left(q_2{q}_3{-q}_0{q}_1\right)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right],$得到的矩阵元素（i,j=1,2,3），得到俯仰角θ的估计值；航向角的估计值分以下几种情况：当${\hat{c}}_{22}\to 0,{\hat{c}}_{12}>0$时,$\hat{\psi }={90}^0;$当${\hat{c}}_{22}\to 0,{\hat{c}}_{12}<0$时,$\hat{\psi }=-{90}^0;$当${\hat{c}}_{22}>0,{\hat{c}}_{12}>0$时,当${\hat{c}}_{22}>0,{\hat{c}}_{12}<0$时，当${\hat{c}}_{22}<0,{\hat{c}}_{12}>0$时，当${\hat{c}}_{22}<0,{\hat{c}}_{12}<0$时，横滚角的估计值分3种情况：当c₃₃＞0时，当c₃₃＜0,时，当时，