一种低成本AHRS辅助GPS确定整周模糊度装置及方法
技术领域
本发明涉及一种低成本AHRS辅助GPS确定整周模糊度装置及方法,属于卫星导航领域。
背景技术
GPS精确定位和定向是近年来国内外研究的热点,而要想得到载体的姿态角,必须快速准确的求得载波相位的整周模糊度,求整周模糊度的方法有很多,如双频相位的线性组合、双频相位与伪距的线性组合、模糊度函数搜索法、模糊度协方差法。GPS单独的数据处理,需要对整周模糊度进行搜索,搜索范围很大故而耗时且容易受到周跳或系统异常等的影响。
AHRS是微型航姿参考系统,融合了三轴陀螺仪、三轴加速度计、三轴磁力计的数据,并可以对这些数据进行处理以得到高精度的姿态数据。该系统近年来应用比较广泛,其与GPS的联合导航也是近来的研究热点。
发明内容
本发明的目的是为了解决GPS单独的数据处理耗时且容易受到周跳或系统异常等影响的问题,提出了一种低成本AHRS辅助GPS确定整周模糊度装置及方法。本发明可以缩小整周模糊度的搜索范围加大搜索效率,故可以更快的确定整周模糊度,进而快速解算出载体姿态角度,同时可以在GPS发生周跳或异常时系统输出AHRS的姿态角,保证系统有效无间断运行,不会因为GPS系统失常而影响装备的可用性。
为达到上述目的,本发明的构思是:
本发明采用的求整周模糊度的方法是基于卡尔曼滤波的最小二乘去相关法,就是将整周模糊度的解算过程分为初值估计和模糊度确定两个环节。本发明采用基于KALMAN滤波的初值估计和基于LAMBDA的模糊度确定方法。LAMBDA方法包括两个步骤,一是模糊度的去相关处理,二是模糊度的搜索确定。
根据上述发明构思,本发明采用下述技术方案:
一种低成本AHRS辅助GPS确定整周模糊度装置由AHRS系统和GPS模块构成,其中AHRS由MEMS三轴陀螺仪、MEMS三轴加速度计、MEMS三轴磁力计构成,GPS模块由两个标准GPS天线和单频GPS接收机构成。
优选的,所述三轴陀螺仪为低成本MEMS芯片。
优选的,所述三轴加速度计为低成本MEMS芯片。
优选的,所述三轴磁力计为低成本MEMS芯片。
优选的,所述A天线和B天线为标准的GPS天线。
优选的,单频GPS接收机,只接收卫星的L1载波信号,处理卫星的星历,载波相位等数据。
优选的,数据处理与控制单元模块,由单片机,通信接口构成,单片机可选择DSP、ARM、C8051或C51等微处理器。通信接口为串口,I2C或者SPI,把数据从传感器和GPS接收机传给主控制单片机。
一种AHRS辅助GPS确定整周模糊度方法,采用上述装置进行操作,操作步骤如下:
(1).GPS数据采集:单频GPS接收机通过天线采集卫星的L1载波信号,信号内容包括卫星星历、载波相位,然后接收机对星历进行处理,得出接收机的在WGS-84坐标系下的坐标;
(2).卡尔曼滤波:卡尔曼滤波初始的基线向量由AHRS确定的姿态角反算出来作为初始值,这样基线向量的初始值越接近真值,则可以缩短滤波时间,滤波方程如下:
(1)
(2)
(3)
(4)
(5)
其中,为状态向量,,是预测的K+1时刻的状态向量,是k+1时刻的状态向量,系统噪声的方差矩阵为;为一步转移矩阵,,观测噪声的方差矩阵为,是K时刻系统的方差,是K时刻预测的K+1时刻的方差,是K+1时刻系统的方差,是单位矩阵,为双差载波相位观测量,
其中,是k卫星和n卫星的载波相位的双差值;
(6)
其中,、、是接收机(16)到、卫星的单位向量双差值的三维分量。而,为接收机(16)到卫星的单位向量。
(7)
其中,、、为卫星在WGS-84坐标系下的坐标;、、为接收机(16)在WGS-84坐标系下的坐标;为载波的波长,长度为0.190293673;
(3).整周模糊度去相关:采用整数高斯变换法去相关,把整周模糊度的误差协方差矩阵进行上三角变换,然后确定整周模糊度的搜索空间;
(4).基线约束:通过增加约束找到正确的整周模糊度,由载波相位的双差观测原理得到:
(8)
式(8)中:为双差载波相位观测量,为基线向量;为整周模糊度组合;为观测噪声;
其中,、、是接收机(16)到、卫星的单位向量双差值的三维分量;
由式(8)可得基线矢量:
(9)
可求出基线的长度为:
(10)
求出这个基线长度以后,可进行基线约束检验:
(11)
式(9)、(10)、(11)中:为基线向量,是中与对应的部分,是的转置矩阵,是载波相位的双差观测量,为整周模糊度组合,是基线的长度,为基线向量,是的转置矩阵,为事先测好的基线长度;为基线约束的门限值,可根据具体要求选取;
在基线约束的过程中,剔除不满足基线约束的整周模糊度组合;
(5).姿态角约束:AHRS系统在GPS对姿态角没有精确解出之前已经给出了载体的航向角及俯仰角,因此可继续对整周模糊度组合进行校验,从满足基线约束的多组备选值中,逐个求解载体航向及俯仰角,把不符合姿态角约束的剔除,假设基线的安装月载体的纵轴重合,那么可求出航向角及俯仰角:
(12)
(13)
式(12)、(13)中:、、为基线在地理坐标系下的东北天三个方向的坐标;为航向角;为俯仰角;姿态约束检验:
(14)
(15)
式中:为AHRS解算出的载体航向角,为AHRS解算出的载体俯仰角,为姿态角约束的门限值,可以根据具体要求选取。
(6).基线矢量结算,在求解出整周模糊度以后就可以带入公式(9)得到载体在WGS-84坐标系下的姿态角,经过坐标转换矩阵(假设本系统基线的安装与载体的纵轴重合,则载体坐标系和地理坐标系重合):
(16)
得出基线在载体坐标系下的坐标,式(16)中:、、为基线在地理坐标系下的东北天三个方向的坐标,为载体所在位置的纬度,为载体所在位置的经度,、、为基线在WGS-84坐标系下的坐标;
(7).姿态解算,根据式(16)得出基线矢量的坐标,然后根据式(12)、(13)可求出基线在载体坐标系下的航向角和俯仰角;
当由于周跳或系统失常导致搜索整周模糊度失败时,AHRS输出的载体姿态角暂时作为载体姿态角,直到GPS的整周模糊度正确解算出来为止。另外,GPS在刚上电搜索时,GPS输出的载体姿态角会有延时,这个时候也暂时用AHRS的输出的姿态角作为载体姿态角,这样则保证系统有效无间断运行,不会因为GPS系统失常而影响装备的可用性。
本发明一种低成本AHRS辅助GPS确定整周模糊度方法及装置具有如下特点及优点:在GPS求解整周模糊度的过程中,搜索整周模糊度的范围比较大,而且由于没有约束条件也使得求解的精度也有所影响,用AHRS辅助GPS求解整周模糊度可以提供稳定的姿态角约束,同时在系统出现周跳或卫星信号中断时也可以稳定输出载体姿态。
附图说明
图1是本发明的的装置结构框图。
图2是本发明低成本AHRS辅助GPS确定整周模糊度的流程图。
具体实施方式
下面将结合附图对本发明优选实施例作详细说明:
实施例一:
1.参见图1,低成本AHRS辅助GPS确定整周模糊度装置,由AHRS系统和GPS模块组成一种低成本AHRS辅助GPS确定整周模糊度装置,该装置由AHRS系统和GPS模块构成,其中AHRS由MEMS三轴陀螺仪(12)MEMS三轴加速度计(11)和MEMS三轴磁力计(13)构成,GPS模块由两个标准GPS天线(14、15)和单频GPS接收机(16)构成,所述MEMS三轴陀螺仪(12)、MEMS三轴加速度计(11)、MEMS三轴磁力计(13)和单频GPS接收机(16)与一个数据处理与控制单元(17)相连,所述两个标准GPS天线(14、15)与单频GPS接收机(16)相连。
实施例二:
参见图2,本低成本AHRS辅助GPS确定整周模糊度方法,采用上述装置进行操作,步骤S23为GPS数据采集,单频GPS接收机(16)通过天线(14、15)采集卫星的L1载波信号,信号内容包括卫星星历、载波相位,然后接收机对(16)星历进行处理,这个处理过程很常见,这里不再赘述,得出接收机(16)在WGS-84坐标系下的坐标。
GPS原始数据都得出以后,进行步骤S24,进行卡尔曼滤波,初始的基线向量由AHRS确定的姿态角反算出来作为初始值,这样基线向量的初始值越接近真值,则可缩短滤波时间,滤波方程如下:
(1)
(2)
(3)
(4)
(5)
其中,为状态向量,,是预测的K+1时刻的状态向量,是k+1时刻的状态向量,系统噪声的方差矩阵为;为一步转移矩阵,,观测噪声的方差矩阵为,是K时刻系统的方差,是K时刻预测的K+1时刻的方差,是K+1时刻系统的方差,为单位矩阵,为双差载波相位观测量;
其中,是k卫星和n卫星的载波相位的双差值;
(6)
其中,、、是接收机到、卫星的单位向量双差值的三维分量。而,为接收机到卫星的单位向量。
(7)
其中,、、为卫星在WGS-84坐标系下的坐标;、、为接收机在WGS-84坐标系下的坐标;为载波的波长,长度为0.190293673。
在卡尔曼滤波结束后对整周模糊度进行去相关处理(步骤S25),缩小模糊度的搜索空间,可以大大缩短搜索时间,去相关的方法是整数高斯变换法,把整周模糊度的误差协方差矩阵进行上三角变换(这个在诸多论文里都详细叙述过,在此不再阐述),然后确定整周模糊度的搜索空间。找到正确的那组整周模糊度采取的是增加约束的方案,一共用两个约束结合起来用,第一个是基线约束,第二个则是姿态角约束。
步骤S26,基线约束:
由载波相位的双差观测原理可得到:
(8)
式(8)中:是双差载波相位观测量,是中与对应的部分,为基线向量;为整周模糊度组合;为观测噪声;
、、是接收机到、卫星的单位向量双差值的三维分量;
由式(8)可得基线矢量:
(9)
我们可以求出基线的长度为:
(10)
求出这个基线长度以后我们就可以进行基线约束检验了:
(11)
式(9)、(10)、(11)中:为基线向量,是中与对应的部分,是的转置矩阵,是载波相位的双差观测量,为整周模糊度组合,是基线的长度,为基线向量,是的转置矩阵,为事先测好的基线长度;为基线约束的门限值,可根据具体要求选取;
在基线约束的过程中,剔除不满足基线约束的整周模糊度组合。
步骤S27,姿态角约束:
由于本实施例采用AHRS系统,故在GPS对姿态角没有精确解出之前,我们已经知道载体的大致姿态(航向角及俯仰角),可继续对整周模糊度组合进行校验,从满足基线约束的多组备选值中,逐个求解载体航向及俯仰角,把不符合姿态角约束的剔除,我们假设基线的安装月载体的纵轴重合,那么可求出航向角及俯仰角:
(12)
(13)
式(25)、(26)中:、、为基线在地理坐标系下的东北天三个方向的坐标;为航向角;为俯仰角;姿态约束检验:
(14)
(15)
式中:为AHRS解算出的载体航向角,为AHRS解算出的载体俯仰角,为姿态角约束的门限值,可根据具体要求选取。
最后进行最小二乘检验,很多文章都有提出,这里也不再赘述。最终根据约束条件筛选出的整周模糊度求解基线向量,进而求出精确的载体姿态。
在求解出整周模糊度以后就可以带入公式(9)得到载体在WGS-84坐标系下的姿态角,步骤S28进行基线矢量解算,经过坐标转换矩阵,本系统基线的安装与载体的纵轴重合,则载体坐标系和地理坐标系重合:
(16)
式(16)中:、、为基线在地理坐标系下的东北天三个方向的坐标,为载体所在位置的纬度,为载体所在位置的经度,、、为基线在WGS-84坐标系下的坐标;
得出基线在载体坐标系下的坐标,这时根据式(12)、(13)可求出基线在载体坐标系下的航向角和俯仰角,进行步骤S29接触姿态角。
当由于周跳或系统失常导致搜索整周模糊度失败,则AHRS输出的载体姿态角暂时作为载体姿态角,直到GPS的整周模糊度正确解算出来为止,还有一种情况是GPS在刚上电搜索时,GPS输出的载体姿态角会有延时,这个时候也暂时用AHRS的输出的姿态角作为载体姿态角,这样则保证系统有效无间断运行,不会因为GPS系统失常而影响装备的可用性。