一种考虑偏差时引入伪测量的重力梯度运动学导航方法与流程

本发明提供一种考虑偏差时引入伪测量的重力梯度运动学导航方法，它涉及一种基于重力梯度测量的序贯运动学定位方法和偏差固定方法，即一种对无动力学约束的静止或者运动载体利用重力梯度测量进行位置估计和测量偏差估计的方法，属于导航技术领域。

背景技术：

全张量重力梯度匹配导航技术是一种新兴的自主导航方式。其利用地球本身的重力梯度信息，通过建模、匹配等方法对载体位置进行估计。重力梯度匹配导航技术具有实时、全自主、抗干扰等诸多优势，通过在载体平台上安装一个重力梯度仪作为测量仪器，得到六个输出量，如果不考虑仪器的测量偏差，则一个时刻的测量数据可以直接解算出位置信息。由于重力梯度匹配导航技术具有完全自主、抗干扰、无误差积累等优点，在未来有望成为新一代的主流导航系统。

重力梯度是重力势对位置的二阶导数，是一个3阶对称方阵，其中只有五个元素是相互独立的。在通常的重力梯度辅助导航系统或者作为单独的导航系统中，一般不考虑重力梯度仪的测量偏差。在重力梯度导航方式作为单独工作的导航系统中，目前已经有融合卫星动力学信息的导航方式出现。重力梯度定位精度依赖于重力梯度仪的测量精度和重力场模型的精度，同时依赖于是否有其它信息的约束，比如动力学信息。依据是否利用卫星动力学信息，可以将重力梯度导航方式分为动力学导航和运动学导航。其中运动学导航算法因为不需要动力学信息的约束，适用范围更广，可以为车辆、舰船、飞机、火箭和卫星等提供导航信息。在不考虑仪器测量偏差的情况下，运动学导航算法可以利用六个重力梯度测量量来估计三个位置信息，根据仪器测量数据和相对应的测量方差，利用迭代非线性最小二乘解算得到位置信息。此种方法相当于对于每一时刻进行单点定位解算。在考虑仪器测量偏差时，由于仪器测量偏差未知，因此需要将仪器测量偏差当作未知量进行估计。这样就增加了未知量的个数，有九个未知量，而观测方程还是只有六个，因此观测方程秩亏，无法解算。由于考虑重力梯度仪测量偏差时运动学解算方程秩亏的问题，目前重力梯度导航的研究主要有两方面，第一是作为辅助导航方式应用在舰船、潜艇等载体上，第二是结合卫星动力学信息对卫星进行导航。

本发明在运动学导航方法的基础上提出了一种考虑测量偏差的序贯运动学导航方法，该方法引入了伪测量，从而增加了每个历元的观测信息，解决了普通运动学导航观测方程秩亏的问题，而且可以同时对载体位置和仪器测量偏差进行估计。由于是运动学导航方法，因此该方法可以摆脱对卫星动力学信息的依赖，能够用于普通载体如汽车、火箭、飞机等的导航。

技术实现要素：

(一)发明目的：本发明以序贯运动学导航算法基础，针对考虑带有偏差的运动学导航中秩亏的问题，提出了一种引入伪测量的序贯运动学导航方法。

(二)技术方案

本发明提出一种考虑偏差时引入伪测量的重力梯度运动学导航方法，其步骤如下：

步骤一：准备工作

首先，重力梯度定义为重力势函数对位置的二阶导数，以u表示地球重力势，其表达形式如下：

式中gm表示地球的引力常数，表示地球半径，cnm和snm表示重力场模型中的球谐项系数，其值由重力场模型给出。m和n表示重力场模型的阶次数(order，degree)。vnm和wnm的计算可参见montenbruck所著《satelliteorbits》一书。现总结如下：

式中，r表示位置[x，y，z]处与地球球心之间的距离，根据上面式(2)和式(3)，可以分别迭代计算各个阶次的vnm和wnm，给出零阶零次的v00和w00，

其递推计算过程如下：

重力梯度张量是重力势对位置的二阶导数，单位为e(1e＝10^-9s^-2)，其张量矩阵在坐标系ecef下的表达式如下：

为vnm，wnm相对于位置的二阶偏导，式(7)为vnm对位置的二阶导数，式(8)为wnm对位置的二阶导数。ggt^e为ecef坐标系下的重力梯度张量矩阵。

由上面(2)式到(8)式即可计算出在地球固连坐标系ecef下某一位置处的重力梯度。重力梯度仪的的测量值是在重力梯度仪坐标系(grf)下表示的，在实际定位中涉及到重力梯度在不同坐标系下的表示，下面给出重力梯度在地球固连坐标系(ecef)和重力梯度仪坐标系(grf)之间的转换关系：

其中tecef2grf表示ecef到grf坐标系的转换矩阵，在实际中可以由其他手段如星敏感器获得。ggt^e表示ecef坐标系下的重力梯度张量矩阵。tecef2grf的表达形式如下：

cij表示矩阵第i行第j列的元素。考虑到重力梯度张量矩阵是一个对称矩阵，重力梯度仪实际输出是张量矩阵ggt中上三角的6个测量结果，组成一个包含6个元素的向量。其表达式为：

式中v^g表示重力梯度在重力梯度仪坐标系(grf)下的分量。

实际重力梯度仪测量中，包含较大偏差，给出考虑测量偏差时的重力梯度原始观测方程如下：

式中表示grf坐标系下的观测值，ht表示重力梯度从ecef到grf的转换矩阵，是一个6×6的矩阵，矩阵中的元素cij即是tecef2grf中第i行，第j列的元素。表示重力梯度在ecef坐标系位置r处的真实值，b表示重力梯度仪的测量偏差，w表示重力梯度仪的测量噪声。重力梯度与位置之间是强非线性关系，在利用重力梯度求解位置信息时需将原始观测方程线性化，为统一计算将测量方程在[r0，b0]处线性化，过程如下：

将上式整理得到考虑测量偏差的重力梯度线性化观测方程如下：

其中表示在位置r＝r0处重力梯度向量对位置向量的偏导阵。

步骤二：初始参数设置

给出历元k时的位置初值rk0和测量偏差初值bk0，可将上一历元的估计结果作为本历元的位置初值rk0和偏差初值bk0：

以及实际观测测量方差阵rrk和偏差观测测量方差阵rbk。实际观测测量方差取值为goce星载重力梯度仪测量方差：

rrk＝diag([15151535015500])²(17)

且实际测量方差不随时间变化。由于测量偏差是伪测量，不存在实际的测量，也就不存在测量方差，利用测量偏差不随时间变化的特性，由于将k-1历元的估计值作为本历元的测量值，故可将k-1历元的偏差估计的协方差pb(k-1)作为本历元的测量方差rbk：

rbk＝pb(k-1)(18)

上面的位置初值、偏差初值和偏差测量方差阵都牵涉到上一历元的信息，对于第一个历元，由于没有上一历元信息，需要给出先验信息，假定位置初值r10、偏差初值b10，以及偏差的测量方差rb1。第一历元的信息是任意给出的，存在很大不确定性，因此第一时刻的测量方差阵需要给出一个稍大一些的值，例如第一时刻六个偏差的初值误差都是10⁴me，则取测量噪声都为10⁴me量级，即：

rb1＝diag([10⁴10⁴10⁴10⁴10⁴10⁴])(19)

然后，将历元k时ecef坐标系与grf坐标系之间的转换矩阵tecef2grf提取出来，并根据式(13)构造出相应的htk，在实际测量中，tecef2grf可以由其它手段获得，如星敏等设备。

步骤三：计算位置rk0处的重力梯度值ggv(rk0)和偏导阵hg(rk0)

由式(2)到式(8)，可以求出在ecef坐标系下位置rk0处的重力梯度张量矩阵，由于矩阵是对称的，因此，只需取下(上)三角元素即可，得到ggv(rk0)如下：

ggv(rk0)＝ggt^e[1，5，9，2，3，6]＝[ggvxx，ggvyy，ggvzz，ggvxy，ggvxx，ggvyz](20)

同时，还需要求出在位置rk0处的重力梯度ggv(rk0)对位置的偏导阵hg(rk0)，经推导，ggv第一个元素对位置的偏导表达形式如下：

其中，

同理，对于重力梯度向量的第二个元素ggvyy，将其对位置求偏导，我们得到：

其中，

对于重力梯度向量的第三个元素，将其对位置求偏导，我们得到：

其中，

对于重力梯度的其余元素，其对位置的偏导有以下关系：

其中，

至此，我们已经求得重力梯度对位置的偏导阵hg，其表达形式如下：

式中所有的元素都已在上面求出。

步骤四：引入伪测量，构建运动学导航观测模型

重力梯度只是位置的函数，对于考虑偏差时的运动学导航，其原始观测方程与其线性化形式都已经在步骤一中给出，见式(12)及式(15)。

由于方程只有六个观测量，分别为重力梯度仪的六个输出量，而待估量有9个，分别为三个位置待估量x，y，z，六个偏差待估量bxx，byy，bzz，bxy，bxz，byz，因此，观测量的个数少于待估量的个数，观测方程秩亏，无法解算。为解决方程秩亏的问题，本方法在重力梯度运动学导航方法中引入了伪测量的概念，考虑到每个历元的测量偏差都相等，因此可将上一历元的估计结果作为本历元的观测值，其具体表达式如下：

其中bk为本历元k的偏差真值，w为测量噪声。上式本就已经是线性方程，为计算上的统一性，将其考虑成非线性方程，对其在bk＝bk0处进行线性化，可以得到伪观测方程线性化后的形式如下：

在步骤二中已经说明，由于测量偏差bk不随时间变化，因此上一历元的估计协方差pb(k-1)即可作为本历元的观测方差rbk：

rbk＝pb(k-1)(30)

由于每个历元偏差的测量方差都是由上一历元的估计协方差传递而来，因此，每个历元偏差的测量方差都是不断变化的。结合式(15)和式(29)，可以得到考虑测量偏差的重力梯度序贯运动学导航的观测方程如下：

其中，令

得到线性化表达式为：

zk-zk＝hk·dx+w(32)

步骤五：非线性最小二乘方法迭代求解

根据线性化之后的观测方程式(32)，由线性最小二乘理论可以直接得到每次迭代修正量的大小dx，

得到修正量之后，更新待估量，表达式如下：

xk＝xk0+dx(34)

此时需要判断修正量的模是否超过预定阈值，此处我们设定阈值为1，即：

norm(dx)＞1？(35)

如果判断为是，即修正量大于阈值，则更新后的位置和偏差作为初值rk0和bk0，即：

然后重新进入步骤三。

如果判断为否，即修正量足够小，则可以结束本历元的解算，然后保存本历元数据，作为下一历元的初值，进入步骤二进行下一历元的解算。解算结果的协方差矩阵为：

此处需要指出的是，步骤二中对于偏差伪测量的测量方差，需要取前一历元的估计方差，此处所选协方差方法如下：

rk＝pk-1(4：9，4：9)(38)

通过以上步骤，引入伪测量，增加了观测信息，然后构建考虑测量偏差的重力梯度序贯运动学导航观测方程。摆脱了传统卫星定轨应用中对卫星动力学约束的依赖，将重力梯度导航的应用扩展到一般物体，从而大大提高了重力梯度定位导航的应用潜力和应用价值。

(三)优点

本发明提供的一种考虑测量偏差时引入伪测量的重力梯度序贯运动学导航方法的优点在于：

①本发明对于不需要卫星动力学信息，仅仅依靠重力梯度仪的测量输出就能进行定位，尤其适用于普通物体的定位导航。

②本发明在解决了传统运动学定位无法解决测量偏差和位置同时估计的问题。

附图说明

说明书附图说明了本发明一种考虑测量偏差时引入伪测量的重力梯度序贯运动学导航方法的具体实施过程。

具体实施方式

本发明一种考虑测量偏差时引入伪测量的重力梯度序贯运动学导航方法，见附图中的图1所示，其具体实施步骤如下：

步骤一：初始参数设置

给出历元k时的位置初值rk0和测量偏差初值bk0，可将上一历元的估计结果作为本历元的位置初值rk0和偏差初值bk0：

以及实际观测测量方差阵rrk和偏差观测测量方差阵rbk。实际观测测量方差取值为goce星载重力梯度仪测量方差：

rrk＝diag([15151535015500])²(40)

且实际测量方差不随时间变化。由于测量偏差是伪测量，不存在实际的测量，也就不存在测量方差，利用测量偏差不随时间变化的特性，由于将k-1历元的估计值作为本历元的测量值，故可将k-1历元的偏差估计的协方差pb(k-1)作为本历元的测量方差rbk：

rbk＝pb(k-1)(41)

上面的位置初值、偏差初值和偏差测量方差阵都牵涉到上一历元的信息，对于第一个历元，由于没有上一历元信息，需要给出先验信息，假定位置初值r10、偏差初值b10，以及偏差的测量方差rb1。第一历元的信息是任意给出的，存在很大不确定性，因此第一时刻的测量方差阵需要给出一个稍大一些的值，例如第一时刻六个偏差的初值误差都是10⁴me，则取测量噪声都为10⁴me量级，即：

rb1＝diag([10⁴10⁴10⁴10⁴10⁴10⁴])(42)

然后，将历元k时ecef坐标系与grf坐标系之间的转换矩阵tecef2grf提取出来，并根据式(51)构造出相应的htk，在实际测量中，tecef2grf可以由其它手段获得，如星敏等设备。此部分对应于说明书附图中的1～2.

步骤二：计算位置rk0处的重力梯度值ggv(rk0)和偏导阵hg(rk0)

由式(2)到式(8)，可以求出在ecef坐标系下位置rk0处的重力梯度张量矩阵，由于矩阵是对称的，因此，只需取下(上)三角元素即可，得到ggv(rk0)如下：

ggv(rk0)＝ggt^e[1，5，9，2，3，6]＝[ggvxx，ggvyy，ggvzz，ggvxy，ggvxz，ggvyz](43)

同时，还需要求出在位置rk0处的重力梯度ggv(rk0)对位置的偏导阵hg(rk0)，经推导，ggv第一个元素对位置的偏导表达形式如下：

其中，

同理，对于重力梯度向量的第二个元素ggvyy，将其对位置求偏导，我们得到：

其中，

对于重力梯度向量的第三个元素，将其对位置求偏导，我们得到：

其中，

对于重力梯度的其余元素，其对位置的偏导有以下关系：

其中，

至此，我们已经求得重力梯度对位置的偏导阵hg，其表达形式如下：

式中所有的元素都已在上面求出。此部分对应于说明书附图中的框图3.

步骤三：引入伪测量，构建运动学导航观测模型

重力梯度只是位置的函数，对于考虑偏差时的运动学导航，其原始观测方程与其线性化形式都已经在步骤一中给出，见式(12)及式(15)。

由于方程只有六个观测量，分别为重力梯度仪的六个输出量，而待估量有9个，分别为三个位置待估量x，y，z，六个偏差待估量bxx，byy，bzz，bxy，bxz，byz，因此，观测量的个数少于待估量的个数，观测方程秩亏，无法解算。为解决方程秩亏的问题，本方法在重力梯度运动学导航方法中引入了伪测量的概念，考虑到每个历元的测量偏差都相等，因此可将上一历元的估计结果作为本历元的观测值，其具体表达式如下：

其中bk为本历元k的偏差真值，w为测量噪声。上式本就已经是线性方程，为计算上的统一性，将其考虑成非线性方程，对其在bk＝bk0处进行线性化，可以得到伪观测方程线性化后的形式如下：

在步骤二中已经说明，由于测量偏差bk不随时间变化，因此上一历元的估计协方差pb(k-1)即可作为本历元的观测方差rbk：

rbk＝pb(k-1)(53)

由于每个历元偏差的测量方差都是由上一历元的估计协方差传递而来，因此，每个历元偏差的测量方差都是不断变化的。结合式(15)和式(29)，可以得到考虑测量偏差的重力梯度序贯运动学导航的观测方程如下：

其中，令

得到线性化表达式为：

zk-zk＝hk·dx+w(55)

此部分对应于说明书附图中的框图4.

步骤五：非线性最小二乘方法迭代求解

根据线性化之后的观测方程式(55)，由线性最小二乘理论可以直接得到每次迭代修正量的大小dx，

得到修正量之后，更新待估量，表达式如下：

xk＝xk0+dx(57)

此时需要判断修正量的模是否超过预定阈值，即：

norm(dx)＞threshold？(58)

如果判断为是，即修正量大于阈值，则更新后的位置和偏差作为初值rk0和bk0，即：

然后重新进入步骤三。

如果判断为否，即修正量足够小，则可以结束本历元的解算，然后保存本历元数据，作为下一历元的初值，进入步骤二进行下一历元的解算。解算结果的协方差矩阵为：

此处需要指出的是，步骤二中对于偏差伪测量的测量方差，需要取前一历元的估计方差，此处所选协方差方法如下：

rk＝pk-1(4：9，4：9)(61)

此部分对应于说明书附图中的5～8.

通过上述步骤，提出了一种考虑偏差时引入伪测量的重力梯度运动学导航方法。该方法基于重力梯度，考虑重力梯度仪的测量偏差，通过引入伪观测，解决了普通运动学导航方法中的方程秩亏问题，能够同时估计观测偏差和位置。该方法为全自主运动学导航方法，不受外界干扰，可对地面、海面以及空中静止或者运动的载体进行定位导航。