一种基于惯性导航系统和双天线GPS的定位测姿方法与流程

本发明涉及利用gps卫星导航系统对载体进行姿态解算的方法，具体涉及到一种基于惯性导航系统和双天线gps的定位测姿方法。

背景技术：

随着科学技术的迅速发展，目前广泛应用于航空、航天、航海和地面载体的导航系统多种多样，存在各自的特点与优点，但同时也具有各自的缺陷。惯性导航系统(inertialnavigationsystem，ins)具有能够不依赖外界信息，完全独立自主地提供多种较高精度的导航参数的优点，具有抗电子辐射干扰、大机动飞行、隐蔽性好的特点。但是，惯性导航系统的检测精度主要取决于惯性测量器件(陀螺仪和加速度计)，导航参数的误差随时间而累积，不适合长时间导航。gps作为新型导航系统，能够进行全球、全天候、实时导航，其定位误差与时间无关，具有较高的定位和测速精度。但是，gps的载体在做高动态的运动时，常使gps接收机不易捕获和跟踪卫星载波信号，甚至产生周跳现象等。由此可见，gps和ins具有优势互补的特点。

技术实现要素：

为解决现有技术惯性导航系统和gps定位系统各自存在的检测精度主要取决于惯性测量器件，不适合长时间导航及做高动态的运动时不易捕获和跟踪卫星载波信号等问题，本发明提出一种基于惯性导航系统和双天线gps的定位测姿方法。

本发明基于惯性导航系统和双天线gps的定位测姿方法，在载体上固定设置惯性导航系统ins和呈几何分布的两个gps信号接收天线且对应设置两台接收机，使得两个gps信号接收天线构成了一个二维基线向量ab；将基线的端点a作为坐标原点且采用a点位置的gps原始数据解算结果作为端点a的位置坐标；将载波相位双差观测方程进行线性化处理，采用clambda算法利用约束条件求得整周模糊度a的最小二乘浮动解和基线向量b的估计值及其方差-协方差矩阵；同时，利用无约束的lambda算法寻找一个最小二乘法估计解alls，根据该估计解alls与浮点解计算初始搜索空间大小，求得整周模糊度a，进而求出端点b的位置坐标；通过端点a和端点b的位置坐标和方向向量之间的关系，求解出载体的二维姿态信息。

进一步的，所述将载波相位双差观测方程进行线性化处理，包括，在载波相位双差模式下相对定位的线性化观测方程可表示为：

y＝aa+bb+e(1)

式中，y为载波相位双差观测向量；a为整周模糊度向量；b为基线向量，属于3维实数向量r³；e为噪声及非建模误差项；a是整周模糊度向量的参数矩阵；b是基线向量的参数矩阵。

进一步的，所述采用clambda算法利用约束条件求得整周模糊度a的最小二乘浮动解和基线向量b的估计值及其方差-协方差矩阵；同时，利用无约束的lambda算法寻找一个最小二乘法估计解alls，根据该估计解alls与浮点解计算初始搜索空间大小，求得整周模糊度a，包括以下步骤：

s1、通过标准最小二乘法平差求基线向量估计值和模糊度最小二乘浮点解与及相应的方差-协方差矩阵，所述方差-协方差矩阵如下式(2)所示：

式中，为整周模糊度浮点解的方差矩阵；为最小二乘模糊度浮点解与基线向量估计值的协方差值；为基线向量估计值与模糊度最小二乘浮点解的协方差值；为基线向量估计值的方差矩阵；

s2、利用模糊度最小二乘浮点解基线向量估计值和方差-协方差矩阵通过使式(3)的目标函数最小作为约束条件求得模糊度向量的固定解，其计算式如式(3)所示：

式中：和分别为整周模糊度a的最小二乘浮点解和基线向量b的估计值；和分别为整周模糊度浮点解和基线向量估计值的方差矩阵；zⁿ表示整数域；其解计作为模糊度的整数最小二乘估计；

s3、用模糊度最小二乘浮点解与整数最小二乘模糊度估计值的差纠正基线向量的最小二乘估计值即可得到基线向量的确定值b：

s4、为了选择一个合适的搜索空间，可以将整周模糊度候选解的搜索空间ω定义为：

式中：和分别为整周模糊度a的浮点解和基线向量最小二乘估计值和分别为整周模糊度浮点解和基线向量最小二乘估计值的方差矩阵；zⁿ表示整数域；χ²表示搜索空间的大小；

同时，使用无约束的lambda算法寻找一个整数最小二乘法估计解alls，根据该估计解alls计算初始搜索的空间大小式中，为整周模糊度a的浮点解；为整周模糊度浮点解的方差矩阵；如果初始搜索空间内无解，则将搜索空间按照的方式不断扩大，直到模糊度搜索空间内有解，所求出的解即为整周模糊度a；其中，设定所述按照的方式不断扩大搜索空间，直到通过clambda解算出的整周模糊度候选解个数超过最大范围800。

进一步的，在计算整周模糊度a的浮点解时，还包括将kalman滤波算法应用于clambda方法中模糊度浮点解的实时计算；包括以下步骤：

选取天线b所在位置坐标增量以及解算位置所需要的双差整周模糊度作为kalman滤波器的状态参数：即，

式中：xk为状态参数矩阵；xk，yk，zk为端点b的位置坐标增量，为载波相位双差的整周模糊度，n为双差载波相位整周模糊度的个数；

系统的状态方程为：

式中：xk为系统状态参数；wk-1为系统噪声：e(wk-1)＝0，cov(wk，wj)＝qkδkj；状态转移矩阵为

设天线b的位置坐标为(x2，y2，z2)，系统测量方程为：

zk＝hkδxk+vk(8)

式中：hk为观测矩阵；δxk为待估参数向量，包括位置分量参数、电离层和对流层延迟残差以及双差模糊度；zk为观测量与计算量之差；vk为观测噪声，且满足e(vk-1)＝0，cov(vk，vj)＝rkδkj；

klaman滤波算法的递推公式如下：

初始值定义：

计算增益矩阵：

xk+1,k+1＝xk,k+1+kk+1[zk+1-hk+1δxk+1,k](11)

参数的更新估计：

计算更新参数的方差—协方差阵：

上述各式中：xk+1，k为k时刻预测的k+1时刻的转移矩阵；为k时刻预测的k+1时刻的状态转移矩阵；xk,k为k时刻的转移矩阵；pk+1,k为k时刻预测的k+1时刻的方差；pk,k为k时刻的方差；t为转置矩阵；qk为系统噪声方差；xk+1,k+1为k+1时刻的增益矩阵；xk,k+1为k+1时刻利用k时刻状态预测的结果；kk+1为k+1时刻的各状态量增益矩阵；zk+1为k+1时刻的测量方程；hk+1为k+1时刻的观测矩阵；δxk+1,k＝xk+1，k-xk，xk为状态参数矩阵；pk+1,k+1为k+1时刻的方差；i为1的矩阵；kk为k时刻的各状态量增益矩阵；rk为观测噪声方差；

采用上述系统状态方程、系统测量方程和klaman滤波算法的递推公式即可求得整周模糊度a的浮点解

进一步的，所述通过端点a和端点b的位置坐标和方向向量之间的关系，求解出载体的二维姿态信息；包括，设端点a和端点b的位置坐标所确定的基线向量ab的坐标为(x12，y12，z12)，则载体的二维坐标航向角α与俯仰角β就由下式确定：

进一步的，所述基于惯性导航系统和双天线gps的定位测姿方法，还包括，当按照的方式不断扩大搜索空间的次数达到设定次数时，模糊度搜索空间内仍然无解，即判定姿态解算错误；此时，直接采用ins测量得到的航向角α、俯仰角β作为载体的姿态参数辅助模糊度搜索结果输出，保证整个系统的实时输出；同时，采用自适应度函数f(β,α)对模糊度搜索获得的双差整周模糊度浮点解的适应性进行评价，并在浮点解满足自适应度函数f(β,α)时，利用该浮点解解算出整周模糊度，进而求得端点b的位置坐标，最终求解出载体的二维姿态信息；其中，所述自适应度函数f(β,α)如下式所示：

式中，f(β,α)为定义的适应度函数，航向角α，俯仰角β可由ins测量得到，为双差整周模糊度浮点解。

本发明基于惯性导航系统和双天线gps的定位测姿方法的有益技术效果是在很大程度上减少了数据存储空间，提高了整周模糊度的搜索效率，提高了系统的整体导航精度与性能。具有精度高、可靠性好和自主性强等显著优点。

附图说明

附图1为本发明基于惯性导航系统和双天线gps的定位测姿方法的系统框架图；

附图2为本发明基于惯性导航系统和双天线gps的定位测姿方法的算法框架图。

下面结合附图对本发明基于惯性导航系统和双天线gps的定位测姿方法作进一步的说明。

具体实施方式

为了更清楚地说明本发明实施例的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

附图1为本发明基于惯性导航系统和双天线gps的定位测姿方法的系统框架图,附图2为本发明基于惯性导航系统和双天线gps的定位测姿方法的算法框架图。由图可知，本发明基于惯性导航系统和双天线gps的定位测姿方法，在载体上固定设置惯性导航系统ins和呈几何分布的两个gps信号接收天线且对应设置两台接收机，使得两个gps信号接收天线构成了一个二维基线向量ab；将基线的端点a作为坐标原点且采用a点位置的gps原始数据解算结果作为端点a的位置坐标；将载波相位双差观测方程进行线性化处理，采用clambda算法利用约束条件求得整周模糊度a的最小二乘浮动解和基线向量b的估计值及其方差-协方差矩阵；同时，利用无约束的lambda算法寻找一个最小二乘法估计解alls，根据该估计解alls与浮点解计算初始搜索空间大小，求得整周模糊度a，进而求出端点b的位置坐标；通过端点a和端点b的位置坐标和方向向量之间的关系，求解出载体的二维姿态信息。

作为具体实施方式，所述将载波相位双差观测方程进行线性化处理，包括，在载波相位双差模式下相对定位的线性化观测方程可表示为：

y＝aa+bb+e(1)

式中，y为载波相位双差观测向量；a为整周模糊度向量；b为基线向量，属于3维实数向量r³；e为噪声及非建模误差项；a是整周模糊度向量的参数矩阵；b是基线向量的参数矩阵。

并且，所述采用clambda算法利用约束条件求得整周模糊度a的最小二乘浮动解和基线向量b的估计值及其方差-协方差矩阵；同时，利用无约束的lambda算法寻找一个最小二乘法估计解alls，根据该估计解alls与浮点解计算初始搜索空间大小，求得整周模糊度a，包括以下步骤：

s1、通过标准最小二乘法平差求基线向量估计值和模糊度最小二乘浮点解与及相应的方差-协方差矩阵，所述方差-协方差矩阵如下式(2)所示：

式中，为整周模糊度浮点解的方差矩阵；为最小二乘模糊度浮点解与基线向量估计值的协方差值；为基线向量估计值与模糊度最小二乘浮点解的协方差值；为基线向量估计值的方差矩阵；

s2、利用模糊度最小二乘浮点解基线向量估计值和方差-协方差矩阵通过使式(3)的目标函数最小作为约束条件求得模糊度向量的固定解，其计算式如式(3)所示：

式中：和分别为整周模糊度a的最小二乘浮点解和基线向量b的估计值；和分别为整周模糊度浮点解和基线向量估计值的方差矩阵；zⁿ表示整数域；其解计作为模糊度的整数最小二乘估计；

s3、用模糊度最小二乘浮点解与整数最小二乘模糊度估计值的差纠正基线向量的最小二乘估计值即可得到基线向量的确定值b：

s4、为了选择一个合适的搜索空间，可以将整周模糊度候选解的搜索空间ω定义为：

式中：和分别为整周模糊度a的浮点解和基线向量最小二乘估计值和分别为整周模糊度浮点解和基线向量最小二乘估计值的方差矩阵；zⁿ表示整数域；χ²表示搜索空间的大小；

同时，使用无约束的lambda算法寻找一个整数最小二乘法估计解alls，根据该估计解alls计算初始搜索的空间大小式中，为整周模糊度a的浮点解；为整周模糊度浮点解的方差矩阵；如果初始搜索空间内无解，则将搜索空间按照的方式不断扩大，直到模糊度搜索空间内有解，所求出的解即为整周模糊度a；其中，设定所述按照的方式不断扩大搜索空间，直到通过clambda解算出的整周模糊度候选解个数超过最大范围800。

由于最小二乘法在求浮点解过程中，要对所有观测值进行整体的平差，其数据处理量巨大，由于在处理超长时间的静态观测数据时，其运算速度的劣势会非常明显，而kalman滤波算法在进行计算时，只需要对前一次的信息进行运算和存储。为了实现运算的实时性，本发明在计算整周模糊度a的浮点解时，还包括将kalman滤波算法应用于clambda方法中模糊度浮点解的实时计算；包括以下步骤：

选取天线b所在位置坐标增量以及解算位置所需要的双差整周模糊度作为kalman滤波器的状态参数：即，

式中：xk为状态参数矩阵；xk，yk，zk为端点b的位置坐标增量，为载波相位双差的整周模糊度，n为双差载波相位整周模糊度的个数；

系统的状态方程为：

式中：xk为系统状态参数；wk-1为系统噪声：e(wk-1)＝0，cov(wk，wj)＝qkδkj；状态转移矩阵为

设天线b的位置坐标为(x2，y2，z2)，系统测量方程为：

zk＝hkδxk+vk(8)

式中：hk为观测矩阵；δxk为待估参数向量，包括位置分量参数、电离层和对流层延迟残差以及双差模糊度；zk为观测量与计算量之差；vk为观测噪声，且满足e(vk-1)＝0，cov(vk，vj)＝rkδkj；

klaman滤波算法的递推公式如下：

初始值定义：

计算增益矩阵：

xk+1,k+1＝xk,k+1+kk+1[zk+1-hk+1δxk+1,k](11)

参数的更新估计：

计算更新参数的方差—协方差阵：

上述各式中：xk+1，k为k时刻预测的k+1时刻的转移矩阵；为k时刻预测的k+1时刻的状态转移矩阵；xk,k为k时刻的转移矩阵；pk+1,k为k时刻预测的k+1时刻的方差；pk,k为k时刻的方差；t为转置矩阵；qk为系统噪声方差；xk+1,k+1为k+1时刻的增益矩阵；xk,k+1为k+1时刻利用k时刻状态预测的结果；kk+1为k+1时刻的各状态量增益矩阵；zk+1为k+1时刻的测量方程；hk+1为k+1时刻的观测矩阵；δxk+1,k＝xk+1，k-xk，xk为状态参数矩阵；pk+1,k+1为k+1时刻的方差；i为1的矩阵；kk为k时刻的各状态量增益矩阵；rk为观测噪声方差；

采用上述系统状态方程、系统测量方程和klaman滤波算法的递推公式即可求得整周模糊度a的浮点解

显然，kalman滤波器对初始值没有特别的要求，但如果滤波初始值精度提高，可以缩短滤波达到系统精度的时间，很大程度上减少了数据存储空间，提高了整周模糊度的搜索效率。利用kalman滤波方法求解整周模糊度解，克服了最小二乘法对大量数据处理速度慢的缺点，极大的减少了计算量，提高了算法解算效率。

作为具体实施方式，所述通过端点a和端点b的位置坐标和方向向量之间的关系，求解出载体的二维姿态信息；包括，设端点a和端点b的位置坐标所确定的基线向量ab的坐标为(x12，y12，z12)，则载体的二维坐标航向角α与俯仰角β就由下式确定：

本发明方法确实提高了姿态解算的成功率，但仍会有一部分姿态角解算错误。当得到的整周模糊度解的个数超过一定范围时，仍未找到满足约束条件的整周模糊度解即判定姿态解算错误。此时可利用ins信息辅助afm算法搜索解算姿态角。将所得姿态角作为接收机的输出姿态角，保证整个系统的实时输出。因此，本发明基于惯性导航系统和双天线gps的定位测姿方法，还包括，当按照的方式不断扩大搜索空间的次数达到设定次数时，模糊度搜索空间内仍然无解，即判定姿态解算错误；此时，直接采用ins测量得到的航向角α、俯仰角β作为载体的姿态参数辅助模糊度搜索结果输出，保证整个系统的实时输出；同时，采用自适应度函数f(β,α)对模糊度搜索获得的双差整周模糊度浮点解的适应性进行评价，并在浮点解满足自适应度函数f(β,α)时，利用该浮点解解算出整周模糊度，进而求得端点b的位置坐标，最终求解出载体的二维姿态信息；其中，所述自适应度函数f(β,α)如下式所示：

式中，f(β,α)为定义的适应度函数，航向角α，俯仰角β可由ins测量得到，为双差整周模糊度浮点解。

显然，通过利用ins信息辅助gps解算浮点解，增加求解浮点解方程的冗余度来提高浮点解的精度，进而提高gps定姿的成功率。同时，在ins辅助求解浮点解时，增加了权重的概念，可以根据ins的性能调整该参数。根据解算出的整周模糊度，求解出天线b的最佳位置。从而确定基线向量ab，载体的姿态角即可完全确定。

显然，本发明基于惯性导航系统和双天线gps的定位测姿方法的有益技术效果是在很大程度上减少了数据存储空间，提高了整周模糊度的搜索效率，提高了系统的整体导航精度与性能。具有精度高、可靠性好和自主性强等显著优点。