本发明公开了一种基于重力梯度信息测距的协同导航方法，采用主从式协同导航结构进行，主AUV通过自身导航设备获得自身的位置信息，主AUV上的重力梯度仪通过从AUV引起的重力梯度异常来获取主AUV和从AUV之间的相对距离：主AUV和从AUV运动过程中不断进行信息交互，从AUV持续采用扩展卡尔曼滤波进行位置误差修正以进行实时定位。实现了无源测距，在测量时具有实时性，减少了协同导航过程中信号的发射，增强了隐蔽性、耗时较少。
1.一种基于重力梯度信息测距的协同导航方法，其特征在于：
采用主从式协同导航结构进行，所述主从式协同导航结构包括至少一个从AUV，主AUV携带导航设备和重力梯度仪，从AUV上配置比主AUV导航设备精度低的导航设备，主AUV和从AUV上携带的导航设备至少有压力深度计、测速仪、惯性单元和陀螺仪；主AUV通过自身导航设备获得自身的位置信息，主AUV上的重力梯度仪通过从AUV引起的重力梯度异常来获取主AUV和从AUV之间的相对距离：主AUV和从AUV运动过程中不断进行信息交互，从AUV持续采用扩展卡尔曼滤波进行位置误差修正以进行实时定位；包括如下具体步骤：S1：在行驶过程中两个连续时刻，主AUV分别测量自身与从AUV之间的相对距离，并保存各时刻时主AUV与从AUV之间的相对距离、以及主AUV的位置信息作为导航信息；主AUV和从AUV之间的相对距离R根据下列公式 计算得到：其中，G为万有引力常量，M为从AUV的质量，Γxx、Γyy、Γxy、Γyz、Γxz为重力梯度的五个分量； 是重力位Φ分别在i方向和j方向的二阶偏导数，i＝x，y，z；j＝x，y，z；x，y，z为从AUV在导航坐标系下的位置坐标；R为主AUV和从AUV的相对距离，单位为米；
S3：主AUV通过水声隐蔽通信向从AUV发送所保存的导航信息；
S4：从AUV接收主AUV发送的导航信息，建立从AUV的测量方程；
S5：利用S2中建立的从AUV的运动状态方程、和S4中建立的从AUV的测量方程，从AUV通过扩展卡尔曼滤波融合自身定位信息和主AUV发送的导航信息，通过协同导航提高自身导航定位精度。
2.根据权利要求1所述的基于重力梯度信息测距的协同导航方法，其特征在于步骤S1中，主AUV测量两个连续时刻自身与从AUV之间的相对距离具体过程如下：主AUV测量tk-1时刻自身与从AUV之间的相对距离R(k-1)，并保存R(k-1)和tk-1时刻主AUV的位置信息(xM(k-1),yM(k-1),zM(k-1))，其中xM(k-1)，yM(k-1)为tk-1时刻主AUV在水平面内的位置，zM(k-1)为tk-1时刻主AUV的深度；同理，主AUV测量tk时刻自身与从AUV之间的相对距离R(k)，并保存R(k)和tk时刻主AUV的位置信息(xM(k),yM(k),zM(k))，其中xM(k)，yM(k)为tk时刻主AUV在水平面内的位置，zM(k)为tk时刻主AUV的深度。
3.根据权利要求1所述的基于重力梯度信息测距的协同导航方法，其特征在于步骤S1中，需要预先存储从AUV所活动水域的全张量重力梯度基准图，利用主AUV上的重力梯度传感器对活动水域进行实时测量，获得从AUV的实测重力梯度信号，并将获得的实测重力梯度信号与全张量重力梯度基准图进行对比，获得由从AUV引起的全张量重力梯度异常；并根据主AUV和从AUV之间梯度张量的关系获取两者相对距离。
4.根据权利要求1所述的基于重力梯度信息测距的协同导航方法，其特征在于步骤S2建立从AUV的运动状态方程时，将三维空间简化为二维协同导航，得到在两个连续时刻的较晚时刻tk时，从AUV的运动状态方程为：其中，x(k)、y(k)和ψ(k)分别表示从AUV在tk时刻的横坐标、纵坐标和偏航角，从AUV运动状态向量为X(k)＝[x(k),y(k),ψ(k)]T；V(k-1)和ω(k-1)表示在前一时刻tk-1的前向速度T和偏航角速度，从AUV测量输入向量为u(k-1)＝[V(k-1) ω(k-1)]；T为采样周期。
5.根据权利要求1所述的基于重力梯度信息测距的协同导航方法，其特征在于步骤S4中根据主AUV和从AUV的各自深度信息，将三维位置关系转到二维平面，从而得到从AUV的测量方程Y(k)＝[D(k-1) D(k)]T：其中，D(k-1)表示在tk-1时刻主AUV和从AUV的二维平面的位置关系；D(k)表示在tk时刻的主AUV和从AUV的二维平面的位置关系；e(k-1,k)为从AUV在x方向tk-1时刻到tk时刻的运动距离；s(k-1,k)为从AUV在y方向tk-1时刻到tk时刻的运动距离。
6.根据权利要求1所述的基于重力梯度信息测距的协同导航方法，其特征在于步骤S5中，从AUV通过扩展卡尔曼滤波提高自身导航定位精度的具体步骤如下：首先初始化从AUV滤波器，然后计算从AUV在两个连续时刻中的后一时刻tk的位置状态预测、以及该后一时刻tk的状态预测协方差矩阵；从AUV利用测量方程对自身的位置状态进行滤波；之后计算AUV在该后一时刻tk的位置状态滤波值。
一种基于重力梯度信息测距的协同导航方法
技术领域
[0001] 本发明涉及水下导航技术领域，特别是涉及一种基于重力梯度信息测距的协同导航方法。
背景技术
[0002] 自主水下航行器(Autonomous Underwater Vehicle，AUV)是探索和研究海洋的重要装备，具有灵活、可靠、隐蔽等特性，在民用和军事方面都有广泛的应用。随着对海洋领域的研究不断扩大和深入，单个航行器很难完成复杂的水下任务，多自主水下航行协作系统被提出。协同导航是多自主水下航行器协作执行水下任务的基础。多自主水下航行器通过信息共享来进行协同导航，有利于提高系统导航的整体精度和鲁棒性，增强了系统的协作能力。
[0003] 在协同导航过程中，导航精度低的水下航行器可以融合导航精度高的水下航行器的导航信息，来提升它的导航精度。其中导航精度高的为主AUV，导航精度低的为从AUV。主AUV将它的高精度位置信息和测量信息(相对距离和方位)发送给从AUV。从AUV利用接收到的主AUV导航信息修正自身的导航定位误差。
[0004] 获取水下航行器间的观测信息是协同导航的关键。通过水下声呐测距、测角实现相互观测是常用的方法，但需要向外发射声脉冲信号，利用声信号的传播时间进行测量，这不利于隐蔽性。同时，水声信号的传播有延时性，会受到水下环境的干扰，影响相互观测。重力是物体固有属性，重力梯度是重力的导数。水下航行器在自身周围会引起重力梯度异常。在协同导航中，装备有重力梯度仪的水下航行器在一定范围能测量到另一水下航行器引起的重力梯度异常，通过相关算法实现测距、测向，作为协同导航的观测信息。
[0005] 目前也有现有技术是关于组合导航，但重力梯度辅助GGI、卫星定位GPS、惯性导航INS三种模块都在同一载体上，不涉及多个载体的协同导航。对信息的处理也是在这个硬件框架基础上进行三种模块得到的导航信息进行信息融合，GGI和GPS根据自身的测量周期校正INS的导航位置、速度，并无法获取多个载体的协同导航数据；由于组合导航模型本身的限制，会影响最终的定位精度，同时由于利用多种模块进行有源测距，隐蔽性有待提高。
发明内容
[0006] 本发明要解决的技术问题是，为了克服上述现有技术的不足，提供一种基于重力梯度信息测距的协同导航方法，实现了无源测距，在测量时具有实时性，减少了协同导航过程中信号的发射，增强了隐蔽性、耗时较少。
[0007] 为解决上述技术问题，本发明采用如下技术方案：
[0008] 一种基于重力梯度信息测距的协同导航方法，其特征在于：
[0009] 采用主从式协同导航结构进行，所述主从式协同导航结构包括至少一个从AUV，主AUV携带导航设备和重力梯度仪，从AUV上配置比主AUV导航设备精度低的导航设备，主AUV和从AUV上携带的导航设备至少有压力深度计、测速仪、惯性单元和陀螺仪；主AUV通过自身导航设备获得自身的位置信息，主AUV上的重力梯度仪通过从AUV引起的重力梯度异常来获取主AUV和从AUV之间的相对距离：主AUV和从AUV运动过程中不断进行信息交互，从AUV持续采用扩展卡尔曼滤波进行位置误差修正以进行实时定位。
[0010] 进一步的，本发明包括如下具体步骤：
[0011] S1：在行驶过程中两个连续时刻，主AUV分别测量自身与从AUV之间的相对距离，并保存各时刻时主AUV与从AUV之间的相对距离、以及主AUV的位置信息作为导航信息；
[0012] S2：建立从AUV的运动状态方程；
[0013] S3：主AUV通过水声隐蔽通信向从AUV发送所保存的导航信息；
[0014] S4：从AUV接收主AUV发送的的导航信息，建立从AUV的测量方程；
[0015] S5：利用S2中建立的从AUV的运动状态方程、和S4中建立的从AUV的测量方程，从AUV通过扩展卡尔曼滤波融合自身定位信息和主AUV发送的导航信息，通过协同导航提高自身导航定位精度。
[0016] 进一步的，步骤S1中，主AUV测量两个连续时刻自身与从AUV之间的相对距离具体过程如下：
[0017] 主AUV测量tk-1时刻自身与从AUV之间的相对距离R(k-1)，并保存R(k-1)和tk-1时刻主AUV的位置信息(xM(k-1),yM(k-1),zM(k-1))，其中xM(k-1)，yM(k-1)为tk-1时刻主AUV在水平面内的位置，zM(k-1)为tk-1时刻主AUV的深度；同理，主AUV测量tk时刻自身与从AUV之间的相对距离R(k)，并保存R(k)和tk时刻主AUV的位置信息(xM(k),yM(k),zM(k))，其中xM(k)，yM(k)为tk时刻主AUV在水平面内的位置，zM(k)为tk时刻主AUV的深度。
[0018] 进一步的，步骤S1中，需要预先存储从AUV所活动水域的全张量重力梯度基准图，利用主AUV上的重力梯度传感器对活动水域进行实时测量，获得从AUV的实测重力梯度信号，并将获得的实测重力梯度信号与全张量重力梯度基准图进行对比，获得由从AUV引起的全张量重力梯度异常；并根据主AUV和从AUV之间梯度张量的关系获取两者相对距离。
[0019] 进一步的，步骤S1中主AUV和从AUV之间的相对距离根据下列公示计算得到：
[0020]
[0021] 其中，G为万有引力常量，M为从AUV的质量，Γxx、Γyy、Γxy、Γyz、Γxz为重力梯度的五个分量； 是重力位Φ分别在i方向和j方向的二阶偏导数，i,j＝x,y,z，x，y，z为从AUV在导航坐标系下的位置坐标；R为主AUV和从AUV的相对距离，单位为米。
[0022] 进一步的，步骤S2建立从AUV的运动状态方程时，将三维空间简化为二维协同导航，得到在两个连续时刻的较晚时刻tk时，从AUV的运动状态方程为：
[0023]
[0024] 其中，x(k)、y(k)和ψ(k)分别表示从AUV在tk时刻的横坐标、纵坐标和偏航角，从AUV运动状态向量为X(k)＝[x(k),y(k),ψ(k)]T；V(k-1)和ω(k-1)表示在前一时刻tk-1的前向速度和偏航角速度，从AUV测量输入向量为u(k-1)＝[V(k-1) ω(k-1)]T；T为采样周期。
[0025] 进一步的，步骤S4中根据主AUV和从AUV的各自深度信息，将三维位置关系转到二维平面，从而得到从AUV的测量方程Y(k)＝[D(k-1) D(k)]T：
[0026]
[0027] 其中，D(k-1)表示在tk-1时刻主AUV和从AUV的二维平面的位置关系；D(k)表示在在tk时刻的主AUV和从AUV的二维平面的位置关系；e(k-1,k)为从AUV在x方向tk-1时刻到tk时刻的运动距离；s(k-1,k)为从AUV在y方向tk-1时刻到tk时刻的运动距离。
[0028] 进一步的，步骤S5中，从AUV通过扩展卡尔曼滤波提高自身导航定位精度的具体步骤如下：首先初始化从AUV滤波器，然后计算从AUV在两个连续时刻的后一时刻时的位置状态预测、以及该时刻的状态预测协方差矩阵；从AUV利用测量方程对自身的位置状态进行滤波；之后计算AUV在tk时刻的位置状态滤波值。
[0029] 由此，本发明通过重力梯度信息测距来实现协同导航，与其它测距方法相比，重力梯度是质量体固有的属性，该方法是通过实时地测量从AUV引起的重力梯度异常，计算得到主、从AUV的相对距离，不用向外发射信号，具有无源的特性，可以增强协同导航的隐蔽性；在测量时具有实时性，不需要通过水声的传播时间来计算得到观测量。
附图说明
[0030] 图1是本发明基于重力梯度信息测距的协同导航方法流程图；
[0031] 图2基于本发明的主从AUV相对距离原理图；
[0032] 图3本发明基于重力梯度信息测距的协同导航过程。
具体实施方式
[0033] 本发明基于重力梯度信息测距的协同导航方法流程如图1所示。本发明采用主从式协同导航结构。主AUV携带高精度的导航设备和重力梯度仪，从AUV上配置的是低精度的导航设备。主AUV通过重力梯度反演获得与从AUV的相对距离，从AUV接收主AUV发出的导航信息，通过融合内外传感器信息提高自身导航定位精度。包括如下步骤：
[0034] S1：基于重力梯度信息测得相对距离。利用主AUV上的重力梯度传感器对活动水域进行实时测量，获得实测重力梯度信号，将获得的实测重力梯度值与活动水域的全张量重力梯度基准图进行对比，获得由从AUV引起的全张量重力梯度异常，从而获取主AUV和从AUV之间的相对距离；
[0035] 利用重力梯度仪测量相对距离的原理为：
[0036] 将海平面当作水平面，x轴和y轴在水平面内，z轴的方向垂直于水平面向下建立坐标系。在忽略离心力的情况下，假设某一目标物体的质量为m，a,b,c为该目标物体在坐标系上的坐标；该目标物体对位于(x,y,z)处测量点产生的重力位为：
[0037]
[0038] 式中，G为万有引力常量。对Φ求二阶导数得重力梯度张量：
[0039]
[0040] 是重力位Φ分别在i方向和j方向的二阶偏导数，i,j＝x,y,z。
[0041] 重力梯度张量遵守Laplace方程，即Γxx+Γyy+Γzz＝0，同时重力梯度张量也遵守重力场的无旋转性，即Γxy＝Γyx,Γxz＝Γzx,Γyz＝Γzy，Γ为对称矩阵。由测量目标物体引起的重力梯度分量的计算如下：
[0042]
[0043]
[0044]
[0045]
[0046]
[0047]
[0048] 式中，δn为第n个质量单元的密度。若测量点与目标物体距离足够远，则可以忽略目标物体本身尺寸的大小，此时式(3)～(8)可近似表示为
[0049]
[0050]
[0051]
[0052]
[0053]
[0054]
[0055] 式中，M为测量目标的剩余质量，Δx＝a-x为测量点与目标物体水平面内x轴方向上的距离差，Δy＝b-y为测量点与目标物体水平面内y轴方向上的距离差，Δz＝c-z为测量点与目标物体在垂直方向z轴上的距离差，R为重力梯度仪与测量目标的相对距离，单位为米。Δx、Δy、Δz分别为R在三个坐标轴方向上的投影，即 已知万有引力常量G，测量目标剩余质量M，重力梯度分量值，可以推导相对距离R为
[0056]
[0057] 具体到本发明的主从协同导航机构中，主AUV上携带重力梯度仪，在测得从AUV引起的重力梯度异常后，根据式(5)计算得到主AUV与测量目标从AUV的相对距离R。x，y，z为从AUV作为测量点在导航坐标系下的位置坐标。
[0058] S2：建立从AUV的运动状态方程。从AUV在水下的工作环境是三维空间，实际深度信息可由压力传感器测量，对于从AUV的定位误差不起积累作用，因此运动模型中不考虑深度项，进一步转化为二维协同导航。在tk时刻，从AUV的二维运动状态方程为：
[0059]
[0060] 其中，x(k)、y(k)和ψ(k)分别表示从AUV在tk时刻的横坐标、纵坐标和偏航角，从AUV运动状态向量为X(k)＝[x(k),y(k),ψ(k)]T。V(k-1)和ω(k-1)表示在tk-1时刻的前向速度和偏航角速度，从AUV测量输入向量为u(k-1)＝[V(k-1) ω(k-1)]T。T为采样周期。
[0061] 实际中，传感器的测量带有噪声干扰。前向速度和偏航角速度在tk-1时刻的噪声向量为ξ(k-1)＝[ξV(k-1) ξω(k-1)]T，它们的噪声协方差为
[0062]
[0063] 从AUV的二维运动状态方程可写为
[0064]
[0065] S3：主AUV通过S1中介绍的方法，测量tk-1时刻与从AUV的距离R(k-1)，并保存R(k-1)和tk-1时刻主AUV的位置信息(xM(k-1),yM(k-1),zM(k-1))。同理，主AUV测量tk时刻与从AUV的距离R(k)，并保存R(k)和tk时刻主AUV的位置信息(xM(k),yM(k),zM(k))。为了增强协同导航的隐蔽性，主AUV通过水声隐蔽通信向从AUV发送tk-1和tk时刻保存的导航信息，导航信息包括主AUV的位置和主AUV与从AUV的相对距离信息。
[0066] S4：从AUV接收到主AUV发送的导航信息，建立从AUV的测量方程。tk-1时刻主、从AUV的距离为R(k-1)，主AUV的位置信息为(xM(k-1),yM(k-1),zM(k-1))。设tk-1时刻从AUV的位置为(x(k-1),y(k-1),z(k-1))。根据主AUV、从AUV的深度信息将三维位置关系转到二维平面，如图2所示。
[0067]
[0068] 在tk-1时刻，主、从AUV的二维平面的位置关系为
[0069]
[0070] tk时刻主、从AUV的距离为R(k)，主AUV的位置信息为(xM(k),yM(k),zM(k))。设tk时刻，从AUV的位置为(x(k),y(k),z(k))。同理得tk时刻，主AUV、从AUV的二维平面的位置关系为
[0071]
[0072] 从AUV根据公式(16)可以得到x(k-1)和x(k)，y(k-1)和y(k)的关系，设e(k-1,k)＝T·V(k-1)·cos(ψ(k-1))、s(k-1,k)＝T·V(k-1)·sin(ψ(k-1))，e(k-1,k)为从AUV根据运动状态方程，在x方向tk-1时刻到tk时刻的运动距离；s(k-1,k)为从AUV根据运动状态方程，在y方向tk-1时刻到tk时刻的运动距离。则可以得到
[0073]
[0074] 公式(20)改写为
[0075]
[0076] 联立公式(21)和(23)，得到从AUV在tk时刻的测量方程
[0077]
[0078] S5：利用在S2中建立从AUV的运动状态方程，和S4中建立的从AUV的测量方程，从AUV通过扩展卡尔曼滤波融合自身定位信息和主AUV发送的导航信息，来提高从AUV的导航定位精度。扩展卡尔曼滤波来进行位置估计中主要问题是线性化，在状态估计时，对系统方程在前一状态估计值处做实时的线性泰勒近似；在预测步骤中，对测量方程在相应的预测位置也进行线性泰勒近似。协同导航提高导航定位精度过程如图3所示。
[0079] 图3中，在tk-1时刻，主AUV测量与从AUV的距离R(k-1)，并保存R(k-1)和tk-1时刻主AUV的位置信息(xM(k-1),yM(k-1),zM(k-1))；在tk时刻，主AUV测量与从AUV的距离R(k)，并保存R(k)和tk时刻主AUV的位置信息(xM(k),yM(k),zM(k))，将主AUV的导航信息发送给从AUV，从AUV进行扩展卡尔曼滤波，提高导航定位精度。
[0080] 从AUV基于扩展卡尔曼滤波进行状态预测的具体步骤如下：
[0081] 1)从AUV初始化滤波器如下
[0082]
[0083] 2)计算从AUV在tk时刻的状态预测为
[0084]
[0085] 计算从AUV在tk时刻的状态预测协方差矩阵为
[0086] P(k|k-1)＝F(k-1)P(k-1|k-1)FT(k-1)+L(k-1)Q(k-1)LT(k-1) (27)
[0087] 其中
[0088]
[0089]
[0090] 3)从AUV利用测量方程对它的状态进行滤波。测量方程为Y(k)＝[D(k-1) D(k)]T，距离测量也会带有噪声干扰，量测噪声协方差矩阵为
[0091]
[0092] 计算从AUV在tk时刻的卡尔曼增益矩阵为
[0093] K(k)＝P(k|k-1)HT(k)[H(k)P(k|k-1)HT(k)+N(k)]-1 (31)
[0094] 其中
[0095]
[0096]
[0097] 4)计算AUV在tk时刻的状态滤波值
[0098]
[0099] 其中
[0100]
[0101] 计算在tk时刻状态滤波协方差矩阵，用于下一时刻滤波
[0102] P(k|k)＝P(k|k-1)-K(k)H(k)P(k|k-1) (36)
[0103] 式(34)中的 就是从AUV利用扩展卡尔曼滤波对其自身位置状态的预测估计。
[0104] 综上所述，本发明是利用重力梯度反演方法来测量AUV之间的距离。在协同导航过程中，导航精度较低的从AUV接收主AUV发送的位置信息和距离信息通过扩展卡尔曼滤波估计自身的位置，提高从AUV的导航定位精度。
