基于INS辅助CLAMBDA与AFM的GPS／INS组合导航测姿方法

丛 丽，李二翠，张立杨，秦红磊，薛 瑞

（北京航空航天大学电子信息工程学院，北京100191）

基于INS辅助CLAMBDA与AFM的GPS／INS组合导航测姿方法

丛 丽，李二翠，张立杨，秦红磊，薛 瑞

（北京航空航天大学电子信息工程学院，北京100191）

为了提高全球定位系统（global positioning system，GPS）定姿的性能及成功率，将带约束的最小二乘降相关平差法（constrained least squares ambiguity decorrelation adjustment，CLAMBDA）与模糊度函数法（ambiguity function method，AFM）两种算法相结合，同时利用惯性导航系统（inertial navigation system，INS）姿态信息进行辅助，提出了一种GPS／INS组合定姿方法。在CLAMBDA解算过程中，将INS姿态信息与GPS载波相位和码伪距联合解算浮点解，然后再进行固定解搜索，如果姿态解算错误，则用AFM算法得到的姿态角进行替换。其间，用INS姿态信息缩小AFM的搜索范围。之后，GPS／INS进行组合滤波。通过实际系统跑车实验表明，相比于原算法，该算法能有效提高GPS定姿的成功率。

全球定位系统；惯性导航系统；带约束最小二乘降相关平差；模糊度函数法

0 引 言

如今，许多导航应用领域不但需要导航系统提供准确的位置、速度，还要提供准确的姿态。传统的测姿方法使用惯性导航输出的姿态角，但存在误差随时间不断累加、长时间工作时姿态角精度较差的问题。由于利用全球定位系统（global positioning system，GPS）信号测姿具有体积小、成本低、无累积误差、精度高等优势，已成为当前姿态测量的重要手段。然而，GPS定姿也存在自身缺点，如易受外界环境影响，在高动态时易于丢星等。因此，利用惯性导航对GPS进行辅助，可提高姿态测量的可靠性及准确性。

GPS测姿利用载波相位作为观测量，虽然它的精度高，但存在相位的整周模糊。如何准确地确定整周模糊度，一直是载波相位高精度定姿的关键问题。在过去的十几年中，人们对整周模糊度的求解进行了很多研究。如位置域的搜索，以模糊度函数法（ambiguity function method， AFM）［13］为代表，庞大的计算量是该类算法的瓶颈。模糊度域的搜索，有最小二乘搜索算法［4］，优化Cholesky分解［5］，最小二乘降相关平差（least squares ambiguity decorrelation adjustment，LAMBDA）算法［6］等。当前最流行的是LAMBDA算法，它已经被证明是性能较好、理论体系最为完善的算法之一［7］。在LAMBDA的基础上，继而发展成熟的带约束的最小二乘降相关平差算法（constrained LAMBDA，CLAMBDA）［8-10］进一步提高了姿态解算成功率。然而CLAMBDA算法，只能将高精度的浮点解映射到正确的整数解，在浮点解精度较低时，姿态解算常会出错［11］。不少相关研究者在GPS／惯性导航系统（inertial navigation system，INS）组合测姿方面展开研究［12-15］，但大多数GPS／INS组合测姿主要是利用组合滤波方法，没有解决整周模糊度浮点解精度较低时，双差整周模糊度搜索成功率低的问题。对此，本文提出了一种基于INS辅助GPS单频单历元姿态解算算法，将INS姿态观测量和GPS的载波以及码伪距观测量联合解算浮点解，再进行固定解搜索，解算姿态角。若姿态角解算错误，则用AFM解算的姿态角进行替换。然而，AFM算法搜索的范围大，计算效率较低。因此，可以利用惯导输出的姿态角来缩小AFM的搜索范围，提高其计算效率。

1 CLAMBDA算法简介

线性化的载波相位观测方程可概括为

式中，y是观测向量；a是整周模糊度向量；b是基线向量，属于3维实数向量R3；A是整周模糊度向量的参数矩阵；B是基线向量的参数矩阵。

CLAMBDA算法通过使式（2）的目标函数最小来求得模糊度固定解。

式中

整周模糊度候选解的搜索空间为

式中，χ2表示搜索空间的大小。为了选择一个合适的搜索空间，可以使用无约束LAMBDA算法寻找一个整数最小二乘解aILS，根据该解计算初始的空间大小如果空间内无解，则将空间按照不断扩大。此过程不断进行，直到空间内有解。

2 INS辅助单频单历元GPS解算浮点解

以A、B两个天线为端点的短基线，对于第i颗卫星的单差载波相位观测方程和单差码伪距观测方程分别为

式中，λ是载波波长；ΔφiAB和ΔNiAB分别是单差小数部分载波相位和整周模糊度；si是第i颗卫星的单位矢量。

式（4）矩阵模型为

为削减钟差项βφAB，需进行Householder变换，式（6）两边左乘矩阵，得

式中，¯e＝（1，1，…，1）T（m－1）×1；m是可见卫星数目。将不含钟差项的部分提取，有

式中，NSD和NDD分别表示单差和双差整周模糊度向量。

则式（9）等价于

与式（10）类似，得到

定义σ1≡σφ／σρ，则有

原始的GPS测姿方法，是通过式（12），用最小二乘方法求得浮点解。为了提高浮点解的精度，本文提出了利用INS进行辅助求算浮点解，下面将进行具体介绍。

式中，ε为地理坐标系下基线矢量的观测噪声。

定义σ2≡σφ／σI，将式（13）两边同时乘以再和式（12）联立可得

得到式（14）后，同样采用最小二乘方法求得浮点解。也就是说，通过利用INS信息，增加了求浮点解的方程的冗余度来提高浮点解的精度，进而提高GPS定姿的成功率。同时，在INS辅助求算浮点解时，增加了权重的概念，在式（13）中乘以系数σ2，可以根据INS的性能调整该参数，当INS性能较差时，减小其在方程中的权重，当INS性能较好时，增大其在方程中的权重。

3 INS辅助AFM及CLMBDA与AFM的结合

虽然利用惯导信息与GPS的载波相位和码伪距观测量联合，通过增加方程对未知数的冗余度，一定程度上提高了整周模糊度浮点解的解算精度。经过实际验证，此方法确实提高了姿态解算的成功率，但仍会有一部分姿态角解算错误。此时，我们可利用其他搜索方法解算姿态角。AFM的基本思想是对于正确的姿态角，双差方程中的模糊度最接近整数。对于以A、B两个天线为端点的短基线，其双差载波相位观测方程表述为

整周模糊度的浮点解为

对于N个双差整周模糊度，AFM算法将观测值适应度函数定义为

在搜索过程中，需要解决的一个关键问题是如何有效识别出高精度的整周模糊度浮点解。这就需要利用更多已知条件进行约束。其实，单独使用AFM算法时，为了提高姿态角解算精度，将搜索到的姿态角代入式（17），可得到整周模糊度的浮点解，将浮点解进行四舍五入可得到模糊度的整数解，已知全部卫星的整周模糊度后，就可代入双差观测方程式（15）中，采用最小二乘方法，计算高精度的基线向量，然后求得高精度的姿态角。设式（15）的线性形式为

则最小二乘的残差为

由最小二乘法得到的残差，可作为一个约束条件。这样，对于正确的解，用其求得的基线向量应满足如下约束条件：

（1）基线长度与真值的误差很小；

（2）对于地面载体，基线向量的俯仰角通常一般在－10°～10°；

（3）最小二乘残差最小或接近最小。

因此，可根据约束条件对模糊度候选解进行筛选，逐步缩小候选解个数。另外，对于约束条件（2），如果将其去掉，姿态解算的正确率只会稍有下降，影响不大。因此，本文提出的方法，同样适用于非地面载体。

这样，先用由惯导信息辅助的CLAMBDA算法解算载体姿态，CLAMBDA根据其适应度函数及基线长度约束条件选择出正确的整周模糊度。同时增加了另外的约束条件，利用INS的短期高精度，CLAMBDA解算出的当前时刻的基线坐标与上一时刻GPS得到的基线坐标之间的夹角d1，与INS当前时刻得到的基线坐标与上一时刻INS得到的基线坐标之间的夹角d2，两者相比较，如果d1与d2的差值相差太大，则舍弃候选解，如果相差很小，则保留。但当采用膨胀法时，得到的CLAMBDA候选解个数超出一定范围，例如800时，仍未找到满足CLAMBDA约束条件的整周模糊度，此时，即判断姿态解算错误，然后再用由INS信息辅助的AFM得到的姿态角作为GPS接收机的输出姿态角。

由于当浮点解精度较高时，用CLAMBDA搜索固定解时，找到正确解的时间会很短，效率高。因此，本文利用INS辅助提高浮点解精度，缩短了CLAMBDA的搜索时间。另外，降低了CLAMBDA终止条件的候选解个数，使得消耗在用质量差的浮点解寻求固定解的时间减少，使其尽早进入INS辅助的AFM算法。原始的AFM算法的一个主要问题是计算量较大，搜索效率低。所以，使用INS缩小AFM搜索范围，提高了AFM搜索效率。因此，整个算法的实时性可以得到保证。整体设计方案的框图如图1所示。

图1 总体方案设计框图

4 实验结果及分析

在低成本GPS接收机与惯导构成的组合导航系统中，接收机采用Novatel的SUPERSTAR II，其测码精度为0．75 m，差分载波相位精度为0．01 m，输出频率为1 Hz；惯导为Nov Atel生产的IMU-CPT，陀螺零偏稳定性是0．03°／s，加速度计零偏稳定性为2 m g，输出频率为100 Hz。为验证GPS／INS测姿方法的性能，将组合导航系统固定于汽车载体上进行动态的跑车实验，实验地点为北京市的大屯路，时间为十分钟左右。图2为GPS各天线安装方式示意图，沿汽车纵向安装的两个天线确定的基线为主基线，其长度测量值为1．0 m；沿汽车横向安装的两个天线确定的基线为辅基线，其长度测量值为0．9 m。图3为本次跑车实验路线图，其与实际的跑车路线是相符合的。

图2 GPS天线安装示意图

图3 跑车实验路线图

为验证GPS／INS组合测姿方法的性能，首先分析INS辅助GPS定姿方法的性能。基于本次跑车实验采集的数据，分别使用单纯的GPS单频单历元算法和INS辅助的GPS单频单历元算法解算主基线和辅基线矢量，其俯仰角、横滚角、航向角如图4～图6所示。

图4 两种姿态解算方法的俯仰角

图5 两种姿态解算方法的横滚角

图6 两种姿态解算方法的航向角

在实验过程中，汽车均在较为平坦的路面上进行静止或圆周运动，在一定的误差允许范围内，两个基线的俯仰角可以认为是恒定不变的。静止情况下INS初始对准得出的载体俯仰角和横滚角分别为2．5°和0．5°，以此作为主基线和辅基线的俯仰角真值的绝对值。通常情况下，基线长度为1 m左右，整周模糊度的搜索成功时，基线解算的姿态角误差约为2°以内，因此，本文以主基线俯仰角绝对值不大于4．5°，辅基线的俯仰角绝对值不大于2．5°为依据，来统计无惯导辅助的CLAMBDA、有INS辅助的AFM及基于INS辅助的CLAMBDA与AFM相结合的3种姿态解算方法所对应的载波相位整周模糊度搜索成功率，如表1所示。由于CLAMBDA算法只能将高精度的浮点解映射到正确的固定解，当浮点解精度不高时，CLAMBDA并不能很好的找出正确解。此次实验用到的是低成本GPS接收机，性能很差。当利用它的载波和码伪距进行定姿时，求得的浮点解精度是很差的。而且，此次实验是动态实验，更加影响CLAMBDA解算的成功率。因此CLAMBDA解算出的姿态成功率并没有很高。

表1 3种姿态解算方法的载波相位整周模糊度搜索成功率

由图4～图6和表1可知，提出的基于INS辅助的CLAMBDA与AFM相结合的定姿算法，相对于单纯的CLAMBDA算法和INS辅助的AFM算法在解算成功率上有显著提高。当GPS接收机性能较差时，由于码伪距的精度较低，导致GPS采用CLAMBDA单独定姿时，成功率很低。当采用了INS辅助及与AFM结合的方法时，使得我们能够有效找到正确的整周模糊度，解算出正确的姿态角，从而大大提高了姿态解算的成功率，这与理论分析相符。

为了进一步验证组合定姿的性能，在同样的地点，进行第2组实验。通常情况下，高精度的惯导成本比较高，因此，此组实验主要是采用更低成本的惯导，测试组合定姿的效果。接收机采用的是Novatel生产的OEMStar，其对于GPS的测码精度为0．05 m，差分载波相位精度为0．006 m，输出频率为1 Hz；惯导为北京星网宇达生产的XWIMU5220，陀螺零偏稳定性是0．02°／s，加速度计零偏稳定性为8 mg，输出频率为100 Hz。

主基线的长度是0．98 m，辅基线的长度0．93 m，实验时间同样为10分钟。我们分别使用单纯的GPS单频单历元算法和INS辅助的GPS单频单历元算法解算主基线和辅基线矢量，其俯仰角、横滚角和航向角分别如图7～图9所示。

图7 两种姿态解算方法所得俯仰角

图9 两种姿态解算方法所得航向角

接下来统计了有无INS辅助时GPS姿态解算的成功率，如表2所示。

表2 两种姿态解算的成功率

由图7～图9以及表2可以看出，有INS辅助的GPS定姿比无INS辅助的GPS定姿，成功率有了大幅度的提高。

5 结 论

本文提出了一种GPS／INS紧密组合测姿方法。该方法首先将INS姿态信息和GPS的载波相位和码伪距观测量联合，增加方程对未知数的冗余度，解算浮点解；同时利用CLAMBDA与AFM相结合进行搜索。对于用CLAMBDA解算失败的情况下，用AFM进行姿态角的解算，并用INS姿态信息缩小AFM的搜索范围，减小计算量，提高效率。经过实际系统跑车实验证明了由于INS姿态信息的辅助，GPS单频单历元姿态解算方法的整周模糊度搜索成功率显著提高。在后续工作中，需要做的是进一步提高姿态角的精度。这可以通过设计滤波器，增加对陀螺和加速度计的误差量进行滤波来实现，当出现丢星，GPS数据不可用时，载体姿态仍会维持的较好。

［1］Chen W T，Qin H L，Cong L，et al．Error analysis and improve of the ambiguity function method based on the intersection point on sphere［J］．Journal of Electronics and Information Technology，2010，32（9）：2220- 2225．（陈万通，秦红磊，丛丽，等．基于球面交点的AFM算法的误差分析与改进方法［J］．电子与信息学报，2010，32（9）：2220- 2225．）

［2］Wang Y，Zhan X，Zhang Y．Improved ambiguity function method based on analytical resolution for GPS attitude determination［J］．Measurement Science and Technology，2007，18（9）：2985- 2990．

［3］Eling C，Zeimetz P，Kuhlmann H．Development of an instantaneous GNSS／MEMSattitude determination system［J］．GPSSolutions，2013，17（1）：129- 138．

［4］Wang C．Development of a low-cost GPS-based attitude determination system［D］．Calgary：University of Calgary，2003．

［5］Liu N，Xiong Y L，Feng W，et al．An algorithm for rapid integer ambiguity resolution in single frequency GPS kinematical positioning［J］．Acta Geodaetica et Cartographica Sinica，2013，42（2）：211- 217．（刘宁，熊永良，冯威，等．单频GPS动态定位中整周模糊度的一种快速解算方法［J］．测绘学报，2013，42（2）：211- 217．）

［6］Teunissen P J G．Integer least-squares theory for the GNSS compass［J］．Journal of Geodesy，2010，84（7）：433- 447．

［7］Verhagen S，Teunissen P J G．New global navigation satellite system ambiguity resolution method compared to existing approaches［J］．Guidance，Control，and Dynamics，2006，29（4）：981 -991．

［8］Teunissen P J G，Giorgi G，Buist P J．Testing a new single-frequency GNSS carrier phase determination method：land，ship and aircraft experiments［J］．GPS Solutions，2011，15（1）：15- 28．

［9］Chansik P，Teunissen P J G．Integer least squares with quadratic equality constraints and its application to GPS attitude determination systems［J］．International Journal of Control，Auto-mation，and Systems，2009，7（4）：566- 576．

［10］Wang B，Miao L J，Wang S T．A constrained LAMBDA method for GPS attitude determination［J］．GPS Solutions，2009，13（2）：97- 107．

［11］Hofmann W．GPS-global navigation satellite systems GPS，GLONASS，Galileo，and more［M］．New York：Springer Wien，2008：227- 234．

［12］Li Y，Efatmaneshnik M，Dempster A G．Attitude determination by integration of MEMS inertial sensors and GPS for autonomous agriculture applications［J］．GPS Solutions，2012，16（1）：41- 52．

［13］Heon O S，Hwang D H，Park C，et al．Attitude determination GPS／INS integration system design using triple difference technique［J］．Journal of Electrical Engineering and Technology，2012，7（4）：615- 625．

［14］Chiang K W，Lin Y C，Huang Y W，et al．An ANN-RTS smoother scheme for accurate INS／GPS integrated attitude determination［J］．GPS Solutions，2009，13（3）：199- 208．

［15］Lai Y C，Jan S S．Attitude estimation based on fusion of gyroscopes and single antenna GPS for small UAVs under the influence of vibration［J］．GPS Solutions，2011，15（1）：67- 77．

GPS／INSintegrated navigation attitude determination method based on CLAMBDA and AFM aided by INS

CONG Li，LI Er-cui，ZHANG Li-yang，QIN Hong-lei，XUE Rui
（School of Electronic and Information Engineering，Beihang University，Beijing 100191，China）

To improve the success rate and performance of global positioning system（GPS）attitude determination，an attitude determination method is proposed，which combines the constrained least squares ambiguity decorrelation adjustment（CLAMBDA）and ambiguity function method（AFM），and makes use of the inertial navigation system（INS）attitude information．In the process of CLAMBDA，the INS information is combined with the GPScarrier phase and code measurements，increasing the redundancy of equations to solve the float solution，and then searching the fixed solution．If the true ambiguity solutions are not found and the attitude is false，the false attitude is replaced by the attitude using AFM．Meanwhile，the INS information is used to shrink the search space of AFM．After that，the integrated filter of GPS／INSis made．Through the land vehicle experiments，compared with the original algorithm，the proposed algorithm can effectively improve the success rate of attitude determination．

global positioning system（GPS）；inertial navigation system（INS）；constrained least squares ambiguity decorrelation adjustment（CLAMBDA）；ambiguity function method（AFM）

TN 967．2

A

10．3969／j．issn．1001-506X．2015．04．24

丛 丽（1981 ），女，讲师，博士，主要研究方向为卫星导航、组合导航和战术导航。E-mail：congli_hlj＠163．com

李二翠（1989-），通讯作者，女，硕士，主要研究方向为INS／GNSS组合导航定姿。E-mail：buaa_liercui＠163．com

张立杨（1986-），男，硕士，主要研究方向为GNSS姿态测量。E-mail：liyang_zhang＠126．com

秦红磊（1975 ），男，教授，博士，主要研究方向为卫星导航、组合导航和自动测试系统E-mail：qhlmmm＠sina．com

1001-506X（2015）04-0882-06

2014- 04- 28；

2014- 09- 26；网络优先出版日期：2014- 10- 30。

网络优先出版地址：http：／／w ww．cnki．net／kcms／detail／11．2422．TN．20141030．0942．005．html

国家重点基础研究发展计划（973计划）（2011CB707004）；国家高技术研究发展计划（863计划）（2012AA121801）；国家科技支撑计划（2011BAH24B02）；国家自然科学基金（61101077）资助课题