本发明涉及全球卫星导航系统(gnss)领域,具体涉及到一种多模接收机联合定位解算的方法。
背景技术:
目前全球卫星导航系统(gnss)主要包括中国的bds、美国的gps、俄罗斯的glonass、欧洲的galileo四大系统。gnss在军事和民用领域都扮演者极其重要的地位,在军事用途中,它可以提高导弹的目标命中率;在民用领域,gnss可为海、陆、空各个层面的物体进行定位、导航,包括船舶远洋航行和进港引水,汽车自主导航以及飞机航路引导和进场降落等。此外,各种地面车辆跟踪和城市智能交通管理的最核心技术就是gnss定位;它还可以为电力、邮电和通信等网络系统授时和校频,包括产生同步时间、精确授时与校频等;大地测量、地壳运动监测、工程测量、工程变形监测和资源勘探等各种测量任务与gnss密不可分。随着高精度技术应用的成熟,智能机器人、无人机、自动驾驶等新兴领域对gnss接收机的需求越来越多,对产品的精度和稳定性要求也越来越高。且从行业发展方向看,gnss接收机产品的发展趋势是多模兼容、高性能、低功耗,小型化,所以在工程实现上对数据处理算法提出了更高的要求。
gnss进行接收机定位的基本原理是利用卫星星历参数解算出卫星在空间坐标系中的位置,然后根据伪距和载波相位测量值信息,求解以下n元非线性方程组:
其中,(x,y,z)代表接收机在wgs-84坐标系中的位置,
由卫星定位原理可知,解算之前需要计算每颗卫星的位置、速度和钟差。传统方式gps、bds、galileo通过卫星导航电文播发星历参数,其中包含6个开普勒轨道参数,9个摄动修正参数,经过一系列复杂运算,包括求解超定方程、迭代收敛、反三角函数等,解算出精确的卫星空间位置坐标和速度坐标,运算量大,算法复杂度高,不能满足高频率定位导航的需求。
接收机定位解算中,按照解算数据来源分类,通常有快照算法和滤波算法。快照算法包括最小二乘法、加权最小二乘法等;滤波算法包括扩展卡尔曼滤波,无迹卡尔曼滤波等。快照算法只考虑了每个定位历元时刻信息,并没有融合过去时刻的接收机信息,通常情况下接收机位置是渐变的,所以常采用滤波以平滑定位结果。卡尔曼滤波用一套数学递推公式对系统状态进行最优估计,使系统状态的估计值具有最小均方误差。现有的接收机定位算法模型,常采用卡尔曼滤波进行定位解算,常规的卡尔曼更新以一种并行处理的方式得到卡尔曼增益和状态修正值。这种常规的并行处理中,矩阵的逆运算是必不可少的。当卫星数目为n,需要求逆的矩阵维度nxn,目前四大系统同时能跟踪的卫星数目多达40颗,所以在嵌入式系统上实现时会带来非常繁重的运算量,工程实现很困难。
技术实现要素:
为了解决上述不足的缺陷,本发明提供了一种多模接收机联合定位解算的方法,该方法通过卫星星历参数计算卫星空间位置坐标边界值,在滑动窗时间区间内,通过多项式拟合插值算法进行卫星轨道估计,降低求解卫星空间位置的运算复杂度,并采用最小二乘和卡尔曼滤波组合的方式进行接收机定位解算,卡尔曼滤波采用串行更新的方法,避免大维度矩阵求逆运算,降低运算复杂度,从而可提供高频率定位结果,满足高动态场景的应用;以及根据gps定位中卫星位置、速度和钟差随时间的连续性的特点和对多项式插值算法的研究,提出了在嵌入式系统中运用多项式插值算法计算卫星位置、速度和钟差,可以简化复杂的数学运算。针对卡尔曼滤波更新大矩阵求逆的问题,可以将多个观测量看成“依次顺序”到来的,从而能以一种串行的方式来处理,对每个观测量y,假设其噪声方差为r,可以计算这一个观测量导致的卡尔曼增益k、p和x的更新值,从而不需要进行大维度矩阵求逆的运算。
本发明提供了一种多模接收机联合定位解算的方法,包括以下步骤:
步骤(1):星历计算卫星6个时刻的位置、速度、钟差;
步骤(2):计算系数矩阵a;
步骤(3):插值计算t时刻卫星pvt;
步骤(4):最小二乘定位解算;
步骤(5):卡尔曼滤波解算。
上述的方法,其中,所述步骤(1)具体包括:6个时刻分别为(t1,t2,t3,t4,t5,t6),每个时刻间隔时间t取60s,也即时间跨度为60s。
上述的方法,其中,所述步骤(2)具体包括:在(t1,t2,t3,t4,t5,t6)时刻卫星位置、钟差、速度分别为:
pi=[pixpiypizδtivixviyviz]
于是可以列出方程式
其中τi=i*t,则:
上述的方法,其中,所述步骤(3)具体包括:在[t1,t6]时间之间的任意感兴趣的时刻t的卫星位置速度钟差坐标由下式可得:
[pxpypzδtvxvxvx]=[1dtdt2dt3dt4dt5]a
由于插值点位于区间末端时误差会变大,所以当时间t>t5时,更新时间窗口。
上述的方法,其中,所述步骤(4)具体包括:
步骤(4.1):设置初始位置和钟差,一般设置历史位置,如果没有历史位置,可以初始置0;
步骤(4.2):使用方程(2)计算伪距ρi,计算伪距测量值和计算值之间的差δρi
步骤(4.3):将计算值ρi代入方程式(3),求出ai1,ai2,ai3;
步骤(4.4):计算矩阵a,通过方程式(5)求出δx,δy,δz,δδtu;
步骤(4.5):判定是否迭代结束。由δx,δy,δz,δδtu的绝对值和方程(6)计算出δv,若小于门限值则迭代结束,若大于门限值继续迭代;
步骤(4.6):将δx,δy,δz,δδtu和初始x,y,z,δtu相加,得到新的状态,这些值作为下次迭代的初值;
步骤(4.7):重复步骤(4.1)-步骤(4.6),直到δv小于门限值,一般迭代次数不超过10次,若10次还未收敛,则解算失败,可能是观测量存在异常。
上述的方法,其中,所述步骤(5)具体包括:
步骤(5.1):ekf模型
状态方程:
其中x=[x,y,z,bgps,df,δbgb,δbgr,δbge]t,bgps为接收机本地时间和gps卫星系统时间差,δbgb为北斗和gps系统时间差,δbgr为glonass和gps系统时间差,δbge为伽利略和gps系统时间差。
观测方程:
步骤(5.2):状态时间更新
由时间差更新矩阵φk+1,由7式预测k+1时刻的状态;
步骤(5.3):计算观测量残差
根据上一步预测的状态
进而计算实际观测量与预测观测量之间的观测量残差:
步骤(5.4):更新p和q矩阵
每个状态量的处理噪声相互独立,且为白噪声,各自的噪声功率谱密度分别为diag{σp,σp,σp,σv,σv,σv,σb,σdf,σδb,σδb,σδb},则
然后进行状态协方差矩阵p的更新:
步骤(5.5):更新r矩阵
r的计算可以根据每颗星的cn0和仰角拟合,一般r值与cn0和仰角成反比关系;
步骤(5.6):观测量串行更新
将m个观测量看作“依次顺序”到来的,从而以一种串行的方式来处理,对每一个观测量yi,假设其噪声方程为ri,可以计算这一个观测量得出的卡尔曼增益ki,状态误差pi和xk+1的更新值;更新具体计算如下式;
对观测方程在k+1时刻进行线性化,计算h阵;
每颗星串行计算公式如下:
pi=[i-kihi]pi-1(14)
步骤(5.7):更新系统状态x
由观测量更新后的误差状态δxk+1|k+1和第一步得到的状态时间更新,可以对x进行观测量更新:
这一步更新后,
上述步骤中步骤(5.2)-步骤(5.4)为时间更新,e-g为观测量更新,每次处理的结果pk+1|k+1和xk+1|k+1被保存下来,作为下一次迭代的起始条件。
本发明提供了一种多模接收机联合定位解算的方法具有以下有益效果:1、根据gps定位中卫星位置、速度和钟差随时间的连续性的特点和对多项式插值算法的研究,提出了在嵌入式系统中运用多项式插值算法计算卫星位置、速度和钟差的方法。该方法通过卫星星历参数计算卫星空间位置坐标边界值,在滑动窗时间区间内,通过多项式拟合插值算法进行卫星轨道估计,降低求解卫星空间位置的运算复杂度。;2、采用最小二乘和卡尔曼滤波组合的方式进行接收机定位解算,针对卡尔曼滤波更新大矩阵求逆的问题,可以将多个观测量看成“依次顺序”到来的,从而能以一种串行的方式来处理,对每个观测量y,假设其噪声方差为r,可以计算这一个观测量导致的卡尔曼增益k、p和x的更新值,卡尔曼滤波采用串行更新的方法,避免大维度矩阵求逆运算,降低运算复杂度,从而可提供高频率定位结果,满足高动态场景的应用。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述,本发明及其特征、外形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例绘制附图,重点在于示出本发明的主旨。
图1为本发明提供的多模接收机联合定位解算流程图。
图2a、图2b、图2c分别为插值法计算卫星位置、速度、钟差精度对比图。
具体实施方式
在下文的描述中,给出了大量具体的细节以便提供对本发明更为彻底的理解。然而,对于本领域技术人员而言显而易见的是,本发明可以无需一个或多个这些细节而得以实施。在其他的例子中,为了避免与本发明发生混淆,对于本领域公知的一些技术特征未进行描述。
为了彻底理解本发明,将在下列的描述中提出详细的步骤以及详细的结构,以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下,然而除了这些详细描述外,本发明还可以具有其他实施方式。
参照图1-图2c所示,本发明提供的一种多模接收机联合定位解算的方法,该方法包括以下实施步骤:
星历计算卫星6个时刻的位置、速度、钟差;具体为6个时刻分别为(t1,t2,t3,t4,t5,t6),每个时刻间隔时间t取60s,也即时间跨度为60s。
计算系数矩阵a
在(t1,t2,t3,t4,t5,t6)时刻卫星位置、钟差、速度分别为:
pi=[pixpiypizδtivixviyviz]
于是可以列出方程式
其中τi=i*t。则:
插值计算t时刻卫星pvt
在[t1,t6]时间之间的任意感兴趣的时刻t的卫星位置速度钟差坐标由下式可得:
[pxpypzδtvxvxvx]=[1dtdt2dt3dt4dt5]a
由于插值点位于区间末端时误差会变大,所以当时间t>t5时,更新时间窗口。
和常规的用星历计算卫星位置速度的方法比较,能大大减少运算量。
最小二乘定位解算
设置初始位置和钟差,一般设置历史位置,如果没有历史位置,可以初始置0。
使用方程(2)计算伪距ρi,计算伪距测量值和计算值之间的差δρi
将计算值ρi代入方程式(3),求出ai1,ai2,ai3。
计算矩阵a,通过方程式(5)求出δx,δy,δz,δδtu。
判定是否迭代结束。由δx,δy,δz,δδtu的绝对值和方程(6)计算出δv,若小于门限值则迭代结束,若大于门限值继续迭代。
将δx,δy,δz,δδtu和初始x,y,z,δtu相加,得到新的状态,这些值作为下次迭代的初值。
重复上述步骤,直到δv小于门限值。一般迭代次数不超过10次,若10次还未收敛,则解算失败,可能是观测量存在异常。
卡尔曼滤波解算
ekf模型
状态方程:
其中x=[x,y,z,bgps,df,δbgb,δbgr,δbge]t,bgps为接收机本地时间和gps卫星系统时间差,δbgb为北斗和gps系统时间差,δbgr为glonass和gps系统时间差,δbge为伽利略和gps系统时间差,df为接收机钟漂。
观测方程:
状态时间更新
由时间差更新矩阵φk+1,由7式预测k+1时刻的状态。
计算观测量残差
根据上一步预测的状态
进而计算实际观测量与预测观测量之间的观测量残差:
更新p和q矩阵
每个状态量的处理噪声相互独立,且为白噪声,各自的噪声功率谱密度分别为
diag{σp,σp,σp,σv,σv,σv,σb,σdf,σδb,σδb,σδb},则
然后进行状态协方差矩阵p的更新:
更新r矩阵
r的计算可以根据每颗星的cn0和仰角拟合,一般r值与cn0和仰角成反比关系。
观测量串行更新
将m个观测量看作“依次顺序”到来的,从而以一种串行的方式来处理,对每一个观测量yi,假设其噪声方程为ri,可以计算这一个观测量得出的卡尔曼增益ki,状态误差pi和xk+1的更新值。更新具体计算如下式。
对观测方程在k+1时刻进行线性化,计算h阵。
每颗星串行计算公式如下:
pi=[i-kihi]pi-1(14)
更新系统状态x
由观测量更新后的误差状态δxk+1|k+1和第一步得到的状态时间更新,可以对x进行观测量更新:
这一步更新后,
上述步骤中b-d为时间更新,e-g为观测量更新,每次处理的结果pk+1|k+1和xk+1|k+1被保存下来,作为下一次迭代的起始条件。
在本发明中,实现了利用插值法对卫星位置、速度、钟差的计算,并用最小二乘和扩展卡尔曼滤波组合的方法对多模数据进行联合解算。该发明根据导航卫星运行轨道具有平滑性的特性,运用多项式拟合插值法进行卫星轨道插值估计,在轨道估计误差为10-6米前提下,且尽量避免了传统方法中计算卫星轨道位置时的迭代、反三角函数和解超定方程等运算。定位解算采用最小二乘和卡尔曼滤波组合的方法,提高系统的鲁棒性;卡尔曼滤波状态更新中采用串行更新的方式,避免了大矩阵求逆运算,极大的降低了运算复杂度,满足用户对接收机定位精确性、实时性和鲁棒性的要求。该方法已经成功应用于北极星云的多模多频基带板卡产品中,定位性能优良。
在本发明中,接收机信号处理部分首先进行卫星信号捕获,然后跟踪环路实现对码相位和载波相位跟踪,同步电文bit后,实现帧同步,并提取星历参数信息;定位解算部分判定星历有效性,并初始计算卫星轨道位置、速度和钟差分量,选取滑动窗时间区间边界点,用多项式插值法进行卫星位置、速度、钟差插值预测,利用伪距观测量进行最小二乘定位解算,利用多普勒观测量进行最小二乘速度解算,设置系统状态向量及过程噪声、观测噪声协方差矩阵,利用卡尔曼滤波器平滑定位解算结果,最后更新滑动窗时间区间,重新进行接收机快速定位技术细节。
以上对本发明的较佳实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施;任何熟悉本领域的技术人员,在不脱离本发明技术方案范围情况下,都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰,或修改为等同变化的等效实施例,这并不影响本发明的实质内容。因此,凡是未脱离本发明技术方案的内容,依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰,均仍属于本发明技术方案保护的范围内。