一种基于GNSS的含约束EKF相对定位方法与流程

本发明属于卫星导航技术领域，更具体的是涉及一种基于卫星导航的相对定位方法

背景技术：

全球卫星导航系统(gnss，globalnavigationsatellitesystem)是一种利用人造地球卫星发射的无线电信号进行导航的综合系统，能在全球范围内提供导航服务的卫星导航系统，可为用户提供全球、全天候、实时的三维导航服务。基于gnss的相对定位是将两套gnss接收机和天线设备分别固定安装在两个载体上，同时接收gnss测量信息，通过通信数据链将一个载体接收到的gnss测量信息实时传输到另一个载体接收机设备中，利用相对定位方法处理同时接收到的测量信息，即可获得两个载体之间相对位置信息。相比其他导航技术而言，gnss相对定位技术具有低成本、高精度、无漂移、覆盖范围广、高实时性等技术优势，其应用可为地面静止和运动载体，也可为空间低速、高速运动的飞行器提供分米级到厘米级相对定位精度。

高精度相对定位技术是基于载波相位测量信息解算精确的相对位置。然而载波相位测量存在整周模糊度未知的问题，一周整周模糊度估计偏差就可引起0.2m左右的相对定位误差，所以要实现高精度相对定位，必须正确求解整周模糊度，而模糊度浮点解精度是影响整周模糊度能否正确解算的关键因素之一。

模糊度浮点解精度主要取决于相对定位最优估计算法和测量模型。目前，常用的相对定位最优估计算法是最小二乘法和扩展卡尔曼滤波(extendkalmanfilter，ekf)，ekf滤波能够有效利用动力学模型信息提高参数估计精度，因而得到更广泛的应用。然而，目前常用的相对定位方法尚存在如下不足：

(1)常用的方法采用lambda法全部模糊固定的方法，由于不同观测卫星的测量误差不同，新的卫星不断接收到，这些卫星的高度角比较低、噪声比较大，卫星整周模糊度之间存在相关性，需经过很长时间的初始化才能固定所有整周模糊度；新的卫星加入会破坏原有模糊度浮点解方差协方差阵的信息，导致采用lambda法固定所有模糊度解低下。

(2)常用的相对定位方法仅采用ekf一次迭代方法，没有对已部分固定整周模糊度和解算出的精密相对位置解充分利用，这些信息重新构造含约束测量模型，强化当前历元时刻的测量模型，以获得高精度的模糊度浮点解，进而提高所有模糊度固定的成功率和可靠性。

技术实现要素：

本发明的发明目的在于提出一种基于gnss的含约束ekf相对定位方法，该方法采用含约束的ekf滤波和部分模糊度固定算法进行模糊度解算和相对位置解算；首先通过伪距测量和载波相位测量构建传统的测量模型，然后利用ekf进行状态更新和测量更新，获得高精度的模糊度浮点解，采用部分模糊度固定算法固定部分模糊度，并用部分模糊度解算出高精度相对位置解，将部分固定的模糊度向量和解算出的高精度相对位置解作为约束，重新构造ekf测量模型，进行ekf二次状态更新和测量更新，获得更高精度的模糊度浮点解，再次采用部分模糊度固定算法进行整周模糊度解算；本方法在传统的ekf相对定位方法解算的基础上，构建含约束的ekf滤波，并采用部分模糊度固定算法，更好地强化了测量模型，提高了模糊度浮点解的精度，加快整周模糊度快速固定，缩短了相对定位的初始化时间，增强了相对定位的实时性和可靠性。

本发明的发明目的通过以下技术方案实现：

3、一种基于gnss的含约束ekf相对定位方法，包含以下步骤：

步骤1：获取ekf状态向量的初始值令进入步骤2；

步骤2：计算当前历元时刻的一步状态预测和一步状态预测方差协方差阵pk,k-1：

其中，fk,k-1为状态转移矩阵，qk为噪声阵，为上一历元时刻得到的状态向量，pk-1为第k-1历元时刻的方差协方差阵；

步骤3：计算当前历元的状态向量和方差协方差阵pk并代入步骤4中：

pk＝(i-kkhk，1)pk,k-1

其中，kk，1为ekf滤波测量模型的一次增益矩阵，

hk，1为ekf滤波测量模型的一次设计矩阵，当历元时刻k＝1时，当历元时刻k>1时，

rk，1为ekf滤波测量模型的一次测量方差协方差阵，

zk，1为ekf滤波测量模型一次滤波测量向量，当历元时刻k＝1时，当历元时刻k>1时，为k-1历元时刻的部分固定模糊度向量。

步骤4：找出满足模糊度固定成功率所需的整周模糊度个数l；

如果l≥3,则从状态向量和方差协方差阵pk中解出精密相对位置解和部分整周模糊度向量并传入到步骤5-1进行当前历元时刻的再次滤波；

如果0<l<3，则从状态向量和方差协方差阵pk中解出部分整周模糊数向量并传入的步骤5-2进行当前历元时刻的再次滤波；

如果l＝0，则令pk-1＝pk代入步骤2，进行下一历元时刻导航解算；

如果l＝n，则进入步骤七，解算出精密相对位置解；

步骤5-1：利用精密相对位置解、部分整周模糊度向量、双差伪距测量模型和双差载波相位测量模型构建含约束的ekf滤波测量模型的再次滤波测量向量zk，2、再次设计矩阵hk，2和再次方差协方差阵rk，2；

其中mm×(n-1)为对应于部分模糊度固定解向量的稀缺矩阵，为部分固定的

模糊度解，为精密相对位置解，为精密相对位置解的方差协方差阵；

步骤5-2：利用部分固定模糊解向量、双差伪距测量模型和双差载波相位测量模型构建含约束的ekf滤波测量模型的再次滤波测量向量zk，2、再次设计矩阵hk，2和再次方差与协方差阵rk，2；

其中mm×(n-1)为对应于部分固定模糊解向量的稀缺矩阵；

步骤6：对状态向量和方差协方差阵pk进行更新并代入步骤4中：

pk＝(i-kk，2，hk1)pk,k-1

其中，再次增益矩阵

步骤7：求解精密相对位置

采用步骤4已经部分固定整数模糊度z变换向量求解精密相对位置解和方差协方差矩阵；将固定的部分模糊度解传送至步骤3做测量信息。

其中，

优选地，步骤1包含以下步骤：

步骤1.1、利用移动站接收机r和基准站接收机m的单点定位解给出ekf状态向量中的相对位置解的初始值：

其中，表示移动站接收机的位置向量，表示基准站接收机的位置向量；

步骤1.2、利用移动站接收机r和基准站接收机m接收到的卫星s和卫星p的伪距测量信息和载波相位测量信息构建双差伪距测量方程和双差载波相位测量方程

其中：表示双差几何距离，表示双差整周模糊度，表示伪矩的测量噪声，表示载波相位的测量噪声，λ为载波l波长；

步骤1.3、利用双差伪距测量方程和双差载波相位测量方程给出ekf状态向量中双差整周模糊度的初始值：

得到

本发明的有益效果在于：本方法在传统的ekf相对定位方法解算的基础上，构建含约束的ekf滤波，并采用部分模糊度固定算法，更好地强化了测量模型，提高了模糊度浮点解的精度，加快整周模糊度快速固定，缩短了相对定位的初始化时间，增强了相对定位的实时性和可靠性。该方法适用于采用单频、多频、单星座以及多星座gnss系统的各类飞行器和地面运动载体的相对定位，具有较高工程应用价值。

附图说明

图1是本发明所述方法流程图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

下面将结合附图1和技术方案对本发明的具体实施过程做进一步的详细说明。

步骤一：ekf状态初始化

通过接收的到卫星伪距和载波相位信息构建伪距和载波相位测量方程；

式中：为伪距观测量(米)；为载波相位观测量(米)；s表示不同卫星号；r为移动站接收机编号；为信号发射时刻的卫星位置到信号接收时刻的接收机天线位置间的几何距离(米)，dt^s和dtr分别为卫星和接收机的钟差(秒)；λ为载波l波长(米)；为整周模糊度；c＝2.99792458×10⁸m/s为光速；为电离层的延迟量(米)；为对流层的延迟量(米)；为卫星和接收机的初始相位(<1周)；和为载波相位的观测噪声(米)

对基准站接收机m和移动站接收机r同时观测到某颗卫星s做站际差分,可得单差伪距和载波相位方程：

式中，△为单差算子，△(·)rm＝(·)r-(·)m，

由式(3)和(4)可知，单差消去了与卫星相关的误差，如卫星钟差，如果两个接收机距离较短也削弱了电离层和对流层误差影响。

对观测得到两个不同的卫星s和卫星p做星间单差，可得到站际-星间双差方程：

通过上式可知，双差测量模型又消去了接收钟差和卫星钟差，此时为整数，对于两个接收机相对基线(相对位置)小于15km情况下，双差电离层和对流层延迟误差非常小可以忽略不计，本发明考虑接收机距离小于15km情况，上述双差测量模型可简化为：

采用ekf滤波算法，状态向量包括相对位置向量、双差整周模糊度向量

利用公式(7)和(8)可对观测卫星s的模糊度进行初始化：

对于两个接收机共同观测到n颗卫星，其中参考卫星为p(一般取具有最大高度角的卫星为参考卫星),则双差模糊度向量初始化解可通过公式(9)获得

利用两个接收解单点定位解可对相对位置解进行初始化，

利用上述求解的双差模糊度和相对位置解，可对ekf滤波状态量进行初始化。初始化的状态噪声阵q0为

i3×3为三维单位矩阵，0n-1×n-1为n-1零矩阵。

步骤2：ekf滤波状态更新

采用步骤1中由公式(9)和(10)初始化的ekf状态向量进行状态更新，其状态转移矩阵fk,k-1和噪声阵qk为：

其中ξ设定为100。

其滤波状态更新为:

式中，为滤波一步预测值，pk,k-1为一步预测方差协方差阵。

步骤3：ekf滤波测量更新，求解模糊度浮点解

对式(3)和(4)在移动站接收机r位置rr下进行线性化，移动站接收机r的位置信息可通过伪距单点定位解算得到。由于相对两个接收机天线相对位置rrm相对于卫星高度(22000km)较小，则对在移动站接收机r线性化的结果为：

式中，卫星到接收机的方向向量；卫星位置可通过卫星广播星历实时解算得到。将式(15)分别代入式(3)和(4)，整理得到：

令上述伪距和载波相位站际单差方程，对观测得到两个不同的卫星s和p做星间单差，公式(7)和(8)的线性化形式为：

式中，

对于两个接收机共同观测到n颗卫星，其中参考卫星为p(一般取具有最大高度角的卫星为参考卫星),相对定位双差测量模型可简写成如下形式：

式中，zk为测量向量包含双差伪距向量和双差载波相位设计矩阵为：待估计相对位置和模糊度参数为：测量噪声为：

每一历元时刻的方差协方差阵为：

式中，双差伪距噪声阵为：双差载波相位噪声阵为：和分别为伪距和载波相位测量噪声，可通过接收机给据的伪距噪声和载波相位噪声设定。

如果历元时刻k＝1，或者历元k-1时刻无整周模糊度固定解，或者历元k-1时刻已整周模糊度固定解都发生周跳时，则采用以下测量模型：

如果历元k-1时刻固定模糊解个数大于1，则采用相应的含约束的测量模型：

其中mm×(n-1)为对应于部分模糊度固定解的稀缺矩阵；

该测量模型所对应测量模型的方差与协方差阵为：

利用步骤2中一步预测方差协方差矩阵pk,k-1和测量模型设计矩阵hk，计算滤波增益矩阵kk：

通过一步状态预测值滤波增益阵kk，测量值zk和测量模型设计矩阵hk，可计算得到当前滤波测量状态估计值：

滤波测量更新方差协方差矩阵为：

pk＝(i-kkhk)pk,k-1(27)

注意，ekf滤波解属于浮点解。

步骤4：采用部分模糊度法求解模糊度固定解

为了有效利用载波相位测量信息，需要对步骤六求解的模糊度浮点解进行整数固定，才可实现高精度厘米级相对定位。经过步骤3解算后浮点解和方差协方差矩阵pk，写成如下形式：

式中，和分别为第k历元时刻相对位置和模糊度浮点解；为相对位置解；为模糊度浮点解方差阵。

将上式整周模糊度解算部分写成整数最小二乘形式：

式中，为任意整数模糊度为解算出最优整周模糊度解向量；

对上述最小二乘求解，采用目前比较成熟的模糊度解算算法-最小二乘降相关法(least-squaresambiguitydecorrelationadjustment,lambda)算法中z变换对模糊度方差-协方差矩阵进行变换，假设l个模糊度成功率较高

其中p0为预先设定的可接受成功率0.995，为模糊度条件概率，i＝{i+1,i+2,…,l}，则

z＝[zfloatzpar](33)

式中，为可部分模糊度固定z变换向量，为部分模糊度固定z变换向量，该向量通过lambda算法中搜索法进行确定，zfloatzpar分别为浮点解和固定解的z变换矩阵，为第k历元时刻部分固定模糊度向量。

随着数据积累，浮点解的精度会进一步提高，模糊度固定需要的时间也将更加少，相对定位解的精度也会进一步得到提高

如果l>2,则采用修正法公式(35)求解得到精密相对位置解，

则得到更新后的状态向量为

其中，

并将求解得到的精密相对位置解和已固定的整周模糊度向量传入到步骤5进行当前历元时刻二次滤波；

如果0<l<3，则直接已固定的部分整周模糊数向量传入的步骤5进行当前历元时刻二次滤波；

如果l＝0，则直接进入步骤2，进行下一历元时刻导航解算。

如果l＝n，则进入步骤七，解算出精密相对位置解；

步骤5：采用含约束ekf法重新求解模糊度浮点解

作为进一步优选，本发明采用步骤4获得部分整周模糊度向量和精密相对位置解，重新构造ekf测量模型；

如果步骤4得到精密相对位置解和部分整周模糊度向量，则含约束的ekf测量模型变为：

其中为对应于部分模糊度固定解向量的稀缺矩阵，为部分固定的模糊度解，为精密相对位置解；

该测量模型所对应测量模型的方差与协方差阵rk为：

为精密相对位置解的方差协方差阵。

如果采用步骤5只能获得部分固定模糊解向量时，则相应的含约束的测量模型变为：

其中mm×(n-1)为对应于部分模糊度固定解向量的稀缺矩阵；

该测量模型所对应测量模型的方差与协方差阵为：

采用步骤三经过ekf算法测量更新后可获得二次滤波的模糊度浮点解和相对位置解；

步骤6：再次求解整周模糊度

对状态向量和方差协方差阵pk进行更新并代入步骤4中：

pk＝(i-kk，2，hk1)pk,k-1

其中，再次增益矩阵

步骤7：求解精密相对位置

采用步骤4已经固定整数模糊度求解精密相对位置解和方差协方差矩阵；相对定位解为

其中，

并输出相对位置解和方差协方差矩阵。将固定的模糊度解传送至步骤2进入下一个历元解算。

上述伪距噪声方差、载波相位噪声方差、伪距单点定位、卫星位置计算、lambda算法、最小二乘法、矩阵变换等一系列指标的计算及处理公式属于本领域的公知常识且为本领域的技术人员所熟知，因此在此不再赘述。

可以理解的是，对本领域普通技术人员来说，可以根据本发明的技术方案及其发明构思加以等同替换或改变，而所有这些改变或替换都应属于本发明所附的权利要求的保护范围。