CN104061932B - 一种利用引力矢量和梯度张量进行导航定位的方法 - Google Patents

Abstract

一种利用引力矢量和梯度张量进行导航定位的方法，首先根据事先观测建立目标区域的背景场三维信息数据库，基本元素为各点的位置坐标及引力矢量不变量和引力梯度张量不变量；然后通过引力及引力梯度实时观测计算得到待定位点的三个不变量；接着通过判断解算矩阵是否满秩来判定是否可直接进行导航定位。若能，则通过最小二乘算法迭代计算出待定位点的位置；若不能，则结合惯性导航定位技术进行匹配搜索，通过多次迭代搜索得到待定位点的位置。专利最大的创新之处是采用了引力矢量不变量和引力梯度张量不变量，不仅顾及了引力场信息所有分量的贡献，且上述量均与姿态无关，因此相对于传统算法将大大降低姿态误差的影响。

Description

一种利用引力矢量和梯度张量进行导航定位的方法

技术领域

本发明涉及一种利用引力矢量和梯度张量进行导航定位的方法，特别适合于姿态误差观测较大情况下的重力辅助导航定位。

背景技术

相对于当前流行的导航方式(如卫星导航、无线电导航等)而言，重力辅助惯性导航具有自主性和无源性的特征，这决定了该导航方式在特定情况下具有一定的优势。重力辅助惯性导航的概念自1975年提出至今，受到许多学者的关注，例如：Metzger和Jircitano(Journal of Spacecraft and Rockets,1976.13(6):323-324)，Affeck和Jircitano(Proceedings of IEEE's Position Location and Navigation Symposium(PLANS),1990,60-66)均研究了如何利用重力梯度来提高导航定位的精度；Jircitano和Dosch(Institute of Navigation:Proceedings of the Forty-Seventh Annual Meeting,1991,221-229)设计了一个为水下潜艇进行重力辅助导航的系统GAINS，器件主要由一个重力梯度仪和垂直重力仪组成；Gleason(Journal of Guidance Control and Dynamics,1995,18(6):1450-1458)讨论了梯度导航中的各种实际问题；Jekeli(Journal ofGuidance,Control and Dynamics,2006,29(3):704-713)讨论了重力梯度仪对惯性导航系统的误差补偿问题。在实际应用中，美国BELL实验室研发了重力梯度仪导航系统；同时美国海军和空军也进行过相应实验，前者主要用于潜艇导航，后者则主要用于机载辅助导航。在国内利用重力辅助惯性导航也受到了广泛重视。近些年来，许大欣等(大地测量与地球动力学,2011,31(1):127-131)提出了利用重力垂直梯度进行匹配辅助导航的方法，并指出利用垂直梯度由于分辨率更高，因此优于基于重力异常的匹配导航；蒋东方等(武汉大学学报信息科学版，2012,37(10):1203-1206)研究了ICCP重力匹配算法在局部连续背景场中的实现，采用BFGS拟牛顿方法实现置信范围内最近等值点精确定位；袁赣南等(华中科技大学学报(自然科学版)，2013,41(1)：36-40)利用加密的改进ICCP算法提高了传统算法的精度，削弱了初始点位误差较大情况下的误差传播；哈尔滨工程大学专利201310690254.6公开了一种基于改进MSD的重力匹配方法，创新点是在传统算法的基础上，通过引进位置误差向量和SOR迭代算法，简化了计算，提高了匹配速度；辽宁工程大学专利201210194633.1公开了一种人工物理优化粒子滤波的重力梯度辅助定位方法。该发明通过引入人工物理优化，克服了粒子滤波的粒子退化问题，减少了计算量和迭代次数，通过不断的更新和递推，使重力梯度辅助惯导系统的位置误差逐渐趋于零；中国人民解放军海军工程大学专利201210259903.2公开了一种基于局部重力场逼近的匹配导航方法，该发明通过获取惯性导航系统指示航迹点处重力异常值，依次采用地形匹配算法和BFGS寻优实现了匹配定位。上述专利一般均采用重力异常或重力梯度，讨论的重点主要为匹配算法或背景场模型的建立，对姿态误差的问题较少关注。

概括而言，要实现重力自主或辅助惯性导航必须具备三个条件：首先是与重力观测的相关仪器的研制；其次是高精度的先验重力背景场模型的建立；最后就是重力导航算法的建立与研制。其中第一个因素，当前已得到快速发展，引力梯度的观测已在舰载、机载及星载实现；第二个因素得益于各类重力观测技术的发展，全球已经建立了2000多阶的引力场模型，分辨达到10km，而部分局部重力场模型的分辨率更高；第三个即主要研究如何利用重力观测来为导航进行服务，当前讨论的重点是导航算法的建立，一般主要利用重力异常和梯度张量分量来进行匹配辅助惯性导航。利用重力观测来辅助惯性导航的根本原因是惯性导航由于仪器漂移因子的存在，仅靠惯性手段，导航误差会随时间积累。而究其原因，惯性仪器的误差根本来源有两个：一是来自加速度计的误差，该仪器主要用以测量重力以外的加速度；另一个是陀螺仪的误差，该仪器主要用于确定运动及加速度计坐标轴的方向。易知：方向误差对定位的精度影响极大。

由于惯性导航误差的根本来源为仪器的观测误差，因此方向误差必是其中的重要来源之一。而当前所广泛采用的利用重力或重力梯度来辅助惯性导航的方法中，所采用的匹配量同样需要方向信息，因为引力矢量、引力梯度张量在不同的坐标系下，其各分量值会有显著差异。因此，利用传统的重力或重力梯度来辅助惯性导航的方法将不能有效消除惯性导航定位误差中源自姿态误差的那部分影响。

发明内容

本发明解决的技术问题是：克服姿态观测误差较大时的惯性导航定位瓶颈，提出了一种利用引力矢量和梯度张量进行导航定位的方法，该方法具有姿态观测误差零影响的特点，有很好的可行性，定位精度高，可为特殊情况下的导航定位提供方便。

本发明的技术方案是：一种利用引力矢量和梯度张量进行导航定位的方法，步骤如下：

1)通过事先已有观测，建立目标区域的背景场三维信息；所述的背景场三维信息包括三个不变量，即引力矢量不变量、两个引力梯度张量不变量；目标区域中每个点均拥有上述三个不变量；建立目标区域的背景场三维信息的具体形式为：

其中：g_i，v_i,j(i,j＝x,y,z)为事先观测得到的目标区域各点的引力矢量和引力梯度张量各分量，可为任意坐标系下的值，利用背景场数据也可构造出相应的数学模型。最终的基础数据库的元素为：(x,y,z)或(r,θ,λ)、(g,B,C)

2)通过实时观测得到待定位点的引力矢量和引力梯度张量，并计算出待定位点所对应的三个不变量；

3)对步骤1)获得的背景场三维信息模型函数求偏导，得到解算矩阵，检验该解算矩阵是否满秩；当解算矩阵满秩时，表明目标区域可直接定位，则进入步骤4)；当解算矩阵不满秩时，表明目标区域不可直接定位，则跳转至步骤5)；

4)利用实时重力和梯度张量观测信息、背景场三维信息及解算矩阵，进行迭代计算，直至得到待定位点的位置；重复步骤2)至步骤4)得到多个待定位点的位置，进而得到连续的导航轨迹；

5)结合惯性导航，利用观测获得的三个不变量在航行轨迹附近与背景场三维信息进行搜索匹配，获得新的航行轨迹，并在该新的航行轨迹附近进行重新搜索，直至利用新的航行轨迹与背景场三维信息确定出的三个不变量，与观测获得的三个不变量之间的差异满足阈值时，结束并退出。

步骤4)中待定位点的位置确定的具体方法为：

41)给待定位点赋初值并对公式(1)进行线性化，即：

其中：(x₀,y₀,z₀)为待定位点的初值；(g₀,B,C)为待定位点初值的不变量值，可由背景场模型计算得到；dx、dy、dz为坐标修正值；

42)利用公式(3)进行最小二乘解算，初步确定待定位点的位置

43)计算判断当l满足收敛阈值时，结束并退出；否则令：

并跳回至步骤41)重新计算，直至l满足要求后，结束并退出。

步骤5)中结合惯性导航获得新的航行轨迹的具体方法为：

51)利用重力观测按公式(1)计算待定位点的三个不变量；

52)利用惯导提供的航行轨迹，按下式计算匹配函数：

其中Δθ、Δλ为搜索点与起始点的余纬和经度之差，Δg、ΔB、ΔC则分别对应搜索点的不变量与实际观测计算得到的不变量之差；P_θ、P_λ、P_g、P_B、P_C是为了统一量纲所采用的归一化因子，其中P_θ、P_λ取搜索区域的余纬、经度最大差值；P_g、P_B、P_C取引力信息背景场对应元素的标准差；ω_θ、ω_λ、ω_g、ω_B、ω_C为权函数；所述的搜索点指航行轨迹附近的某点；所述起始点的初值第一次由惯导提供确定，此后由确定出的新的航行轨迹提供确定；

53)根据步骤52)计算得到惯导提供的航行轨迹上各点的匹配函数d，找出各点最小匹配函数d对应的搜索点；利用找出的搜索点确定出新的轨迹，通过二次多项式拟合排除新的轨迹中的异常点，该异常点的定位由二次多项式拟合计算得到；然后将排除异常点后新的轨迹作为新的航行轨迹，并跳回步骤52)重新进行搜索，直至确定出的航行轨迹各点的最小匹配函数d与上一次所得航行轨迹对应点的最小匹配函数d之差满足收敛阈值时，结束并退出。

本发明与现有技术相比的优点在于：

(1)本发明提出利用引力矢量和梯度张量不变量来进行导航定位，主要创新点在于：利用引力及引力梯度观测来辅助导航定位的传统方法中，所采用的量一般为某个或其中的几个分量，而各分量的描述需要精确的姿态信息，这在很多情况下难以满足，因此易受姿态观测误差的影响。而本专利提出利用引力矢量和梯度张量不变量来进行导航定位，所采用的量均与姿态无关，因此可大大削弱姿态误差的影响；

(2)本发明利用了三个不变量，并据此提出了直接定位和匹配导航定位两种思路。对于前者，由于有三个量，未知数也仅三个，因此在某些特定区域，可进行直接定位，相对于已有方法具有更好的自主性；对于后者，由于匹配量与传统方法不同，姿态误差的影响将会得到控制；

(3)本发明不仅利用了引力信息，也利用了引力梯度信息，所采用的三个不变量是各分量的组合量，顾及了所有分量信号的贡献，因此相对于传统方法仅利用部分分量的情况，本发明有利于发挥引力场信息不同频段的优势，更加有利于导航定位。

附图说明

图1为基于引力矢量和梯度张量不变量的导航定位系统构成图；

图2为基于不变量的直接导航定位流程图；

图3为基于不变量的匹配导航定位流程图；

图4为本发明中基于不变量的直接导航定位仿真结果图；

图5a为本发明中不含姿态误差时的不变量匹配导航定位仿真结果图；

图5b为本发明中含姿态误差时的不变量匹配导航定位仿真结果图。

具体实施方式

如图1所示，本发明的主要实施方式如下：

(1)首先根据已有观测建立目标区域的背景场三维信息，采用的量主要包括

如下的三组不变量：

其中：g_i,v_i,j(i,j＝x,y,z)即为事先已有观测得到的目标区域各点的引力矢量和引力梯度张量各分量，可为任意坐标系下的值，利用背景场数据也可尝试构造出相应的数学模型。最终的基础数据库的元素为：(x,y,z)或(r,θ,λ)、(g,B,C)。

(2)利用实时观测数据，采用公式(1)计算待定位点的引力矢量不变量和两个梯度张量不变量；

(3)判断进行直接定位的可行性，主要通过讨论如下解算矩阵的稳定性来进行判断：

矩阵G中的各分量可由公式(1)求偏导得到。通过检测该矩阵是否满秩来判断直接定位的可行性。当解算矩阵满秩时，表明目标区域可直接定位，则进入步骤4)；当解算矩阵不满秩时，表明目标区域不可直接定位，则跳转至步骤5)；

(4)根据步骤(3)的结果，如果可以进行直接导航定位，则采用如下的步骤来进行迭代计算，如图2所示：

a)给待定位点赋初值并对公式(1)进行线性化，即：

其中：(x₀,y₀,z₀)为待定位点的初值；(g₀,B₀,C₀)为待定位点初值的不变量值，可由背景场模型计算得到；dx、dy、dz为坐标修正值。

b)利用公式(3)进行最小二乘解算，令：

最终的解和定位结果为：

m＝(G^TG)^-1G^Td (5)

c)计算判断当l满足收敛阈值时，结束并退出；否则令：

并跳回至a重新计算，直至l满足要求。

重复步骤2)至步骤4)得到多个待定位点的位置，进而得到连续的导航轨迹，并通过二次多项式拟合排除导航轨迹中的异常点，该异常点的定位由二次多项式拟合计算得到。

(5)根据步骤(3)的结果，如果不能进行直接的导航定位，可结合惯性导航技术进行辅助惯性导航定位，如图3。下面利用球坐标对二维情况下的匹配导航定位步骤进行描述。

a)利用重力观测按公式(1)计算待定位点的三个不变量；

b)利用惯导提供的航行轨迹，按下式计算匹配函数：

其中Δθ、Δλ指搜索点与起始点(第一次由惯导提供)的余纬和经度之差，Δg、ΔB、ΔC则分别对应搜索点的不变量与实际观测计算得到的不变量之差；P_θ、P_λ、P_g、P_B、P_C是为了统一量纲所做的归一化处理，其中P_θ、P_λ取搜索区域的余纬、经度最大差值；P_g、P_B、P_C取引力信息背景场对应元素的标准差；ω_θ、ω_λ、ω_g、ω_B、ω_C为权函数，本文全取为1。公式(9)中，与θ、λ有关的量主要用来对距离进行约束，即保证匹配点不至于偏离起始点较远；与g、B、C有关的项主要用以约束与引力信息的匹配程度，即差异越小越优。所述的搜索点指航行轨迹附近的某点。

c)根据步骤b计算出惯导轨提供的航行轨迹上各点的匹配函数d，找出各点最小匹配函数d对应的搜索点；利用找出的搜索点确定出新的轨迹，通过二次多项式拟合排除新的轨迹中的异常点，该异常点的定位由二次多项式拟合计算得到；然后将排除异常点后新的轨迹作为新的航行轨迹，并跳回b重新进行搜索，直至确定出的航行轨迹各点的最小匹配函数d与上一次所得航行轨迹对应点的最小匹配函数d之差满足收敛阈值时，结束并退出。

算例

图4所示的实验为：背景场信息用引力场模型EGM08的前300完整阶次来表示。按纬度方向选择不同的点，然后在这些点上用模型计算出三个不变量作为观测值，最后利用上述迭代方法从这些不变量的值去反解点位坐标。图4所示的算例起始点坐标用(r,θ,λ)表示分别为：(6375000.071,99.890,279.117)、(6386000.071,46.790,119.783)、(6376000.071,6.623,45.450)，偏离真实位置距离分别为:17km、32km、42km。依次经过6、8、15次的迭代计算，最终的定位精度均收敛到了8cm以内。这个实例说明在某些特定的条件下(重力特征变化明显)，借助于三个不变量函数可以达到定位与导航的目的。也就是说，可以将三个不变量函数看成是空间点的曲线坐标。

图5所示的实验为：假定EGM08为引力场模型真值，利用该模型的前360阶完整阶次在θ和λ分别为71.35284°～72.495849°、130.098054°～131.983554°的区域以500m等间隔模拟出引力矢量和梯度张量观测值，其中r为常数，等于6378136.30m。以θ＝71.80172°，λ＝130.531082°，为起点， 为加速度，以8s为间隔连续模拟出50个点作为惯导技术提供的初始轨迹；然后以θ₀＝71.8°，λ₀＝130.554°， 为起点，作同样的计算，所产生的结果作为真实轨道。最后利用惯性导航技术提供的轨迹和引力观测值(真实轨迹处的值)按实施步骤5进行匹配定位，结果如图5a所示。为了验证姿态误差的影响，假定上述引力矢量和梯度张量观测的坐标系绕z轴逆时针存在0.1°的方向偏差，重复如上的实验，结果如图5b所示。结果表明：无论是否存在姿态观测误差，最终的定位误差大约为216m，这与背景场的分辨率基本一致(格网的一半)。这表明：基于不变量的匹配算法作为惯性导航的有效补充和备份，可以削弱姿态误差的影响。

本发明未公开技术属本领域技术人员公知常识。

Claims (1)

1.一种利用引力矢量和梯度张量进行导航定位的方法，其特征在于步骤如下：

1)通过事先已有观测，建立目标区域的背景场三维信息；所述的背景场三维信息包括三个不变量，即引力矢量不变量、两个引力梯度张量不变量；目标区域中每个点均拥有上述三个不变量，具体形式为：

$\left\{\begin{array}{c}g=\sqrt{g_x^2+g_y^2+g_z^2}\\ B=v_{xx}{y}_{yy}+v_{yy}{v}_{zz}+v_{xx}{v}_{zz}-(v_{xy}^2+v_{yz}^2+v_{xz}^2)\\ C=v_{xy}^2{v}_{zz}+v_{yz}^2{v}_{xx}+v_{xz}^2{v}_{yy}-v_{xx}{v}_{yy}{v}_{zz}-2v_{xy}{v}_{yz}{v}_{xz}\end{array}\right.$

其中：g_i，v_i,j(i,j＝x,y,z)为事先观测得到的目标区域各点的引力矢量和引力梯度张量各分量，可为任意坐标系下的值，利用背景场数据也可构造出相应的数学模型；最终的基础数据库的元素为：(x,y,z)或(r,θ,λ)、(g,B,C)；

2)通过实时观测得到待定位点的引力矢量和引力梯度张量，并计算出待定位点所对应的三个不变量；

3)对步骤1)获得的背景场三维信息模型函数求偏导，得到解算矩阵，检验该解算矩阵是否满秩；当解算矩阵满秩时，表明目标区域可直接定位，则进入步骤4)；当解算矩阵不满秩时，表明目标区域不可直接定位，则跳转至步骤5)；

4)利用实时重力和梯度张量观测信息、背景场三维信息及解算矩阵，进行迭代计算，直至得到待定位点的位置；重复步骤2)至步骤4)得到多个待定位点的位置，进而得到连续的导航轨迹；待定位点的位置确定的具体方法为：

41)给待定位点赋初值并进行线性化，即：

$\left\{\begin{array}{c}g=g_0+\frac{\∂g}{\∂x}{|}_{x_0,y_0,z_0}dx+\frac{\∂g}{\∂y}{|}_{x_0,y_0,z_0}dy+\frac{\∂g}{\∂z}{|}_{x_0,y_0,z_0}dz\\ B=B_0+\frac{\∂B}{\∂x}{|}_{x_0,y_0,z_0}dx+\frac{\∂B}{\∂y}{|}_{x_0,y_0,z_0}dy+\frac{\∂B}{\∂z}{|}_{x_0,y_0,z_0}dz\\ C=C_0+\frac{\∂C}{\∂x}{|}_{x_0,y_0,z_0}dx+\frac{\∂C}{\∂y}{|}_{x_0,y_0,z_0}dy+\frac{\∂C}{\∂z}{|}_{x_0,y_0,z_0}dz\end{array}\right.---\left(3\right)$

其中：(x₀,y₀,z₀)为待定位点的初值；(g₀,B,C)为待定位点初值的不变量值，可由背景场模型计算得到；dx、dy、dz为坐标修正值；

42)利用公式(3)进行最小二乘解算，初步确定待定位点的位置

$\left\{\begin{array}{c}x=x_0+dx\\ y=y_0+dy\\ z=z_0+dz\end{array}\right.$

43)计算判断当l满足收敛阈值时，结束并退出；否则令：

$\left\{\begin{array}{c}{x}_0=x\\ y_0=y\\ z_0=z\end{array}\right.$

并跳回至步骤41)重新计算，直至l满足要求后，结束并退出；

5)结合惯性导航，利用观测获得的三个不变量在航行轨迹附近与背景场三维信息进行搜索匹配，获得新的航行轨迹，并在该新的航行轨迹附近进行重新搜索，直至利用新的航行轨迹与背景场三维信息确定出的三个不变量，与观测获得的三个不变量之间的差异满足阈值时，结束并退出；具体方法为：

51)利用重力观测按公式(1)计算待定位点的三个不变量；

52)利用惯导提供的航行轨迹，按下式计算匹配函数：

$d=w_{\mathrm{\θ}}{\left(\frac{\mathrm{\Δ}\mathrm{\θ}}{P_{\mathrm{\θ}}}\right)}^2+w_{\mathrm{\λ}}{\left(\frac{\mathrm{\Δ}\mathrm{\λ}}{P_{\mathrm{\λ}}}\right)}^2+w_g{\left(\frac{\mathrm{\Δ}g}{P_g}\right)}^2+w_B{\left(\frac{\mathrm{\Δ}B}{P_B}\right)}^2+w_C{\left(\frac{\mathrm{\Δ}C}{P_C}\right)}^2---\left(9\right)$

其中Δθ、Δλ为搜索点与起始点的余纬和经度之差，Δg、ΔB、ΔC则分别对应搜索点的不变量与实际观测计算得到的不变量之差；P_θ、P_λ、P_g、P_B、P_C是为了统一量纲所采用的归一化因子，其中P_θ、P_λ取搜索区域的余纬、经度最大差值；P_g、P_B、P_C取引力信息背景场对应元素的标准差；ω_θ、ω_λ、ω_g、ω_B、ω_C为权函数；所述的搜索点指航行轨迹附近的某点；所述起始点的初值第一次由惯导提供确定，此后由确定出的新的航行轨迹提供确定；

53)根据步骤52)计算得到惯导提供的航行轨迹上各点的匹配函数d，找出各点最小匹配函数d对应的搜索点；利用找出的搜索点确定出新的轨迹，通过二次多项式拟合排除新的轨迹中的异常点，该异常点的定位由二次多项式拟合计算得到；然后将排除异常点后新的轨迹作为新的航行轨迹，并跳回步骤52)重新进行搜索，直至确定出的航行轨迹各点的最小匹配函数d与上一次所得航行轨迹对应点的最小匹配函数d之差满足收敛阈值时，结束并退出。