基于MEMS辅助的单基线北斗融合测姿算法

蒋 青，刁春帆，田增山，周 牧

(重庆邮电大学 移动通信重点实验室 重庆 400065)

基于MEMS辅助的单基线北斗融合测姿算法

蒋 青，刁春帆，田增山，周 牧

(重庆邮电大学 移动通信重点实验室 重庆 400065)

载体的姿态信息是导航的重要参数，随着北斗卫星导航系统(Beidou navigation satellite system, BDS)和微机电系统 (micro-electro-mechanical systems，MEMS)惯性传感器的发展与完善，高精度、低成本、自主化的融合测姿技术具有广阔的应用前景，因此，提出MEMS辅助单基线北斗融合测姿算法。根据MEMS惯性传感器解算出的姿态信息确定基线向量的搜索范围，从而辅助模糊度函数法(ambiguity function method, AFM)减小整周模糊度搜索空间，提高整周模糊度快速求解的成功率和计算效率。将BDS输出的姿态角信息作为观测信息，对MEMS陀螺仪解算出的姿态信息进行实时校正，实现BDS和MEMS传感器二者的数据融合算法。通过实测数据仿真验证，该算法能够解决信号失锁带来的整周模糊度求解困难的问题，并且测姿系统能在遮挡和动态等复杂环境下提供高质量姿态测量结果。

北斗卫星导航系统/微机电系统(BDS/MEMS)测姿；整周模糊度；模糊度函数法；扩展卡尔曼滤波器

0 引 言

如何充分利用各系统的优点，实现北斗卫星导航系统(Beidou navigation satellite system, BDS)和微机电系统 (micro-electro-mechanical systems，MEMS)惯性传感器信息融合，使姿态测量系统能在高动态环境下保证高可靠性和准确性已成为近年来的研究热点。一个单基线的BDS测姿系统需要高效、可靠的整周模糊度求解算法，主要有基于模糊度域和基于位置域的搜索算法。模糊度域的搜索算法主要有最小二乘搜索算法[1]，最小二乘降相关平差(least squares ambiguity decor-relation adjustment, LAMBDA)算法[2]，带约束的LAMBDA算法[3]等。然而这些算法主要是在高精度浮点解的基础上提高姿态解算的正确率，并没有解决在低精度浮点解情况下姿态解算错误的问题。基于位置域的搜索以模糊度函数法(ambiguity function method, AFM)[4]为代表，AFM算法虽然不受周跳影响，但其包含整个角度搜索空间，导致运算复杂，无法保证算法的实时性。近年来，利用惯性传感器提高卫星整周模糊度求解成功率的想法被提出，例如信息工程大学甘雨博士[5]对单全球定位系统(global position system，GPS)天线与三轴陀螺仪组合测姿进行了研究，但该方法受到陀螺漂移的制约，无法长时间保持系统的稳定性。Roth等[6]提出了一种低成本惯性传感器与磁力计信息用于提高卫星的姿态测量性能的算法，但磁力计容易受到周围磁场环境的影响，会导致利用磁力计输出的测量值计算出的姿态角误差增大，从而降低了系统测量精度。空军工程大学谢志成[7]对高超声速飞行器全球定位系统/惯性导航系统(global positioning system /inertial navigation system，GPS/INS)组合测姿原理进行了理论推导。

传统的BDS测姿算法在BDS信号受到建筑物遮挡，信号失锁的情况下，不能保证整周模糊度求解成功率，即BDS系统无法保证稳定提供高精度的姿态信息。同样基于MEMS传感器的测姿算法也未能解决传感器长期使用累积误差越来越严重的问题，也无法长期稳定提供高精度姿态角。因此，本文提出了基于MEMS辅助的单基线北斗融合测姿算法，测试结果表明，整周模糊度的求解成功率能提升到85%以上，数据融合算法也提升了测姿系统的稳定性，在短时BDS信号恶劣的条件下，仍然能够持续、稳定地提供高精度姿态信息。

1 算法总体结构

本文主要有2个模块，第1个模块利用传感器测量得到的姿态信息辅助AFM算法，减小搜索空间，从而大大降低运算的复杂度，提高整周模糊度的求解成功率；第2个模块提出了一种基于扩展卡尔曼滤波的BDS/MEMS惯性传感器融合姿态测量算法，克服BDS测姿系统因有效卫星颗数不足，无法提供稳定的高精度姿态角问题。算法框架如图1所示。

图1 系统算法框架图Fig.1 Frame diagram of whole method

2 MEMS辅助AFM 算法

AFM是一种在坐标域进行搜索的算法，其原理是越接近正确的姿态角反推出的模糊度带入适应度函数后其值越接近1[8]。MEMS姿态信息可以确定载体姿态角的搜索范围，从而减小整周模糊度的搜索空间，大大减少计算量，提高整周模糊度的求解效率和有效性[9]。根据MEMS的姿态角(φI,θI)确定AFM算法搜索空间，其相应的误差分别为Δφ和Δθ(均大于0)，即(φI-Δφ,φI+Δφ)为航向角的搜索空间，(θI-Δθ,θI+Δθ)为俯仰角的搜索空间，选取(τφ,τθ)作为搜索步长。搜索空间中根据搜索步长确定的任一姿态角组合(φ,θ)代入(1)式，求得对应的基线向量b为[10]

b=[LcosθsinφLcosθcosφLsinθ]T

(1)

(1)式中：L表示基线长度；(φ,θ)表示搜索空间中一组姿态角组合。双差载波相位观测方程为

(2)

(3)

将双差整周模糊度的浮点解代入AFM算法中的适应度函数，其表示为

(4)

(4)式中，n为观测到的卫星颗数。

尽管减小了搜索空间，但是仍然会有几组相似的适应度函数值接近最大值1，因此，需要通过几个约束条件进行筛选才能确定最精确的整周模糊度。选取适应度函数取值大于0.9所对应的浮点解，再将这些浮点解取整后代入(5)式，通过最小二乘计算出相应的基线向量bLS，进而求出姿态角(φBDS,θBDS)。

(5)

则最小二乘的残差为

(6)

综上归纳出以下2个约束条件。

1)基线长度与真值的误差小，即‖bLS‖与真值L的误差不超过0.02L；

2)最小二乘残差最小的x个候选解中选出适应度函数值最大的固定解。

候选解数量通过约束条件1)的筛选逐渐减少，再根据条件2)最终确定模糊度固定解。

利用确定的固定解代入(2)式，通过最小二乘算法求出姿态角。之后利用此姿态信息对应的四元数建立扩展卡尔曼滤波器的观测方程。

3 基于EKF的BDS/MEMS融合测姿

3.1 BDS/MEMS融合测姿滤波器设计

扩展卡尔曼滤波器估计最优姿态角所对应的四元数[11]，将旋转四元数更新姿态的离散时间模型作为状态方程

(7)

(7)式中：ωk为k时刻载体旋转角速率；Qk-1为k-1时刻最优姿态角估计值所对应的四元数；Ω(ωk)为4×4的反对称矩阵；T为传感器数据采样间隔；wk为系统噪声。

(8)

(9)

(10)

(11)

(12)

3.2 自适应协方差构建

3.2.1 加速度协方差构建

当载体处于静止状态时，其加速度等于重力加速度，即

(13)

(13)式中：‖·‖为取模运算；g≈9.8 m/s2。

(14)

(14)式中：ka1和ka2为权重因子；var()为求解方差函数；D为方差求解窗口大小。ka1，ka2和D的取值均由实验得到。

3.2.2 四元数协方差构建

Ratio值是模糊度检验和确认的重要参数。根据第2节的分析可知，每组候选姿态角对应一组模糊度浮点解，因此，姿态角搜索空间中的多组候选姿态角可以求出多组浮点解。选取一组浮点解使目标函数J取得最小值Jmin，同时，也可找到一组模糊度组合使Jsub取次小值。

(15)

(16)

3.3 根据四元数求对应的姿态角

根据四元数理论，求扩展卡尔曼滤波估计出的最优四元数对应的姿态角计算公式为

θ=arcsin(-2(q0q1+q2q3))

(17)

(17)式中：γ为横滚角；θ为俯仰角；φ为航向角。

4 实验验证及结果分析

2015年6月9日在重庆邮电大学逸夫科技楼顶(北纬29.5°，东经106.5°)搭建实验平台分别进行静态和动态测试验证如上算法的可行性。让载体坐标系下的y轴与2个BDS天线组成的基线方向重合，固定在铁杆上，放置在水平桌面，如图2所示。MEMS惯性测量单元包含三轴加速度计感器(STLIS3DH)和三轴陀螺仪(STL3G4200D)。测试过程中，基线长度为1.108 m，BDS接收机数据输出频率为1 Hz，MEMS传感器的原始数据采样频率为20 Hz。

4.1 静态测试

模糊度求解成功率的高低是决定BDS测姿算法能否解算出正确姿态角的关键因数之一。在静态条件下，保存BDS接收机和MEMS传回的数据，并通过时间戳进行数据同步。2种算法整周模糊度求解成功率如表1所示，对5次仿真结果的统计发现，MEMS辅助AFM算法能够在很大程度上提升整周模糊度的求解成功率，成功率一般在85%以上，大大高于LAMBDA算法。

图2 融合测姿硬件Fig.2 Fusion attitude measurement hardware

次数测试算法总历元数求解成功历元数求解成功率/%1LAMBDAMEMS⁃AFM辅助77542862664436．9185．682LAMBDAMEMS⁃AFM辅助1827 341182618．6599．93LAMBDAMEMS⁃AFM辅助28961361289346．9999．94LAMBDAMEMS⁃AFM辅助20701492207072．08100 5LAMBDAMEMS⁃AFM辅助1534 875153357．0499．93

4.2 动态测试

仿真验证动态情况下，EKF融合算法与BDS测姿算法的动态性能如图3所示。

从图3a中可以看出，由于受到接收信号不稳定影响，整周求解的成功率不高，导致解算出来的姿态角精度不高，特别是在载体处于运动状态时，姿态解算出现了错误。并且由于BDS接收机的输出频率为1 Hz，不能很好展现载体的运动状态，具体表现为在83历元时，载体的航向角跨过360°，其姿态角没有回到0°。从图3b可以看出，在单独用BDS进行姿态解算出现的错误的历元，经过数据融合后解算出正确姿态角，解决了因为BDS信号质量不佳导致的姿态解算不正确的问题。并且由于数据融合后输出频率提高到20 Hz，系统的动态性能得到了很大的提升，基本能展现载体整个运动过程。

图3 动态情况下BDS和融合算法解算出来的航向角Fig.3 Heading of BDS and fusion algorithm under dynamic circumstance

接下来主要分析数据融合后的姿态精度。在动态环境下分别采用BDS单独测姿和MEMS辅助单基线BDS融合测姿，统计俯仰角和横滚角均方根误差如表2所示。

表2 BDS及MEMS辅助BDS测姿的俯仰角和横滚角均方根误差

从表2可以看出，通过MEMS辅助单基线BDS测姿后，航向角和俯仰角的精度都有明显提高。辅助后的BDS/MEMS融合测姿算法更能满足人们在动态环境下对高精度的需求。

4.3 遮挡测试

遮挡实验是在静态试验的基础上使用隔离板对其中一个BDS天线进行遮挡持续大约100 s，比较融合算法和BDS测姿算法姿态解算的结果，如图4所示。

图4 遮挡情况下融合算法前后航向角和俯仰角对比Fig.4 Comparison of heading and pitch under blocking circumstance

从图4a和图4c可以看出，在0～400历元，载体处于静止状态，也没有对天线进行遮挡，但采用单独BDS算法进行姿态测量时，解算出的航向角和俯仰角均出现了错误的情况。而在第700历元附近时，因人为对某个天线进行了遮挡，北斗信号质量非常差，甚至出现无法接收到北斗卫星信号的情况，导致可观测到的卫星颗数不足，BDS测姿系统无法正常工作。当遮挡结束后，双天线观测到的有效卫星颗数超过4颗，BDS测姿模块重新开始正常工作，但整周模糊度未能快速锁定，导致输出姿态角精度不高，甚至错误。从图4b和图4d可以看出，通过EKF算法将MEMS数据与BDS卫星数据融合后，由于MEMS能在较短时间不受累计误差影响，仍能输出高精度姿态角，并且还可以利用该姿态角信息辅助整周模糊度的快速锁定。在0～400历元间均解算出正确的航向角和俯仰角，没有出现在某个历元解算错误的情况。并且在第700历元附近时，BDS测姿系统无法正常工作的情况下，仍然能稳定地提供正确姿态信息。

5 结束语

本文提出了一种利用MEMS信息辅助BDS求解整周模糊度和BDS/MEMS融合测姿的算法，利用MEMS传感器系统解算出来的姿态角缩小了整周模糊度的搜索空间，大大减少了AFM算法的计算量。基于EKF的融合算法，姿态角的输出频率高于单一的BDS测姿系统，所以动态性能好，同时，不仅提高了测姿系统在静态和动态情况下的精度，而且也能在BDS信号受到外界条件干扰的情况下持续提供较为精确的姿态角信息。未来的工作将在以下2个方面进行展开：① 研究在船载、森林等更加复杂的环境下提高算法测姿精度与稳定性；②研究GPS/BDS/GLONASS/MEMS多系统的融合算法，利用各个系统优势互补，向高可靠性、高精度发展。

[1] ZHAO Z, GAO Q, HU W. An efficient method for carrier phase ambiguity resolution in GPS attitude determination[C]//IEEE.Wireless, Mobile and Multimedia Networks,2006 IET International Conference on IET. Piscataway, NJ：IEEE Press，2006:1-3.

[2] TEUNISSEN P J G.The least-squares ambiguity decorrelation adjustment:a method for fast GPS integer ambiguity estimation[J].Journal of Geodesy,1995,70(1-2):65-82.

[3] NADARAJAH N, TEUNISSEN P J G, RAZIQ N.Instantaneous GPS-Galileo attitude determination: single-frequency performance in satellite-deprived environments[J].Vehicular Technology, IEEE Transactions on, 2013, 62(7): 2963-2976.

[4] 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): 2895-2990.

[5] 甘雨,隋立芬,戚国宾,等.陆地导航中GNSS/陀螺仪组合实时测姿方法[J].武汉大学学报:信息科学版, 2015, 40(3):379-383. GAN Yu,SUI Lifen,QI Guobing,et al.A Real-Time Attitude Determination Approach for Land Navigation Based on GPS/Gyroscope Integration[J].Geomatics and Information Science of Wuhan University,2015,40(3):379-383.

[6] ROTH J, KASCHWICH C, TROMMER G F. Improving GNSS attitude determination using inertial and magnetic field sensors[J].Inside GNSS, 2013, 9(10): 54-62.

[7] 谢志成,雷虎民,张大元,等.高超声速飞行器双GPS/INS松组合导航姿态估计[J].飞行力学,2014,32(6):12-15. XIE Zhicheng，LEI Humin，ZHANG dayuan，et al.Estimation of loosely-coupled dual GPS /INS integrated navigation attitude for hypersonic flight vehicles[J]. Flight Dynamic, 2014,32(6):12-15.

[8] CHEN W, QIN H, ZHANG Y, et al. Accuracy assessment of single and double difference models for the single epoch GPS compass[J]. Advances in Space Research, 2012, 49(4): 725-738.

[9] CAPORALI A,DALLA T A,PRATICELLI N.Interferometric attitude and direction sensor using GPS carrier phase data[J].Reports on Geodesy,2003,65(2):173-193.

[10] ELING C, ZEIMETZ P, KUHLMANN H. Development of an instantaneous GPS/MEMS attitude determination system[J].GPS Solut，2013,17(1)：129-138.

[11] 罗磊,田增山,陈俊亚.EKF定位跟踪算法研究[J].重庆邮电大学学报：自然科学版, 2009，21(1):50-52. LUO Lei,TIAN Zengshan,CHEN Junya. Algorithm of EKF positioning and tracking[J]. Journal of Chongqing University of Posts and Telecommunications:Natural Science Edition, 2009,21(1):50-52.

蒋 青(1965-),女，重庆人，教授，研究生导师，主要研究方向为宽带网络技术。E-mail:jiangq@cqupt.edu.cn。

刁春帆(1991-)，女，重庆人，硕士研究生，主要研究方向为无线定位与惯性导航。E-mail：409105807@qq.com。

田增山(1968-)，男，河南固始人，教授，博士，主要研究方向为移动通信、个人通信、GPS及蜂窝网定位系统及其应用技术研究。E-mail:tianzs@cqupt.edu.cn。

周 牧(1984-)，男，重庆人，教授，博士，主要研究方向为无线定位技术、机器学习与人工智能、凸优化理论。

(编辑：王敏琦)

Single baseline Beidou fusion attitude determination algorithm based on MEMS inertial sensors

JIANG Qing, DIAO Chunfan, TIAN Zengshan, ZHOU Mu

(Chongqing Key Lab of Mobile Communication Technology, Chongqing University of Posts and Telecommunications,Chongqing 400065, P. R. China)

The attitude information of a platform is an important parameter for modern navigation. With the development and improvement of the Beidou navigation satellite system(BDS)and micro-electro-mechanical systems(MEMS)inertial sensor, high-precision, low-cost and independent fusion attitude determination technology has broad application prospects. Based on this,a single baseline Beidou fusion attitude determination algorithm aided by MEMS Inertial sensors has been proposed to solve the real-time attitude determination problem. Firstly, using the MEMS attitude information deduce the range of baseline vector in order to aide ambiguity function method (AFM), the success rate of fast ambiguity solution is increased. Secondly the BDS attitude is taken as the observation information to eliminate the MEMS gyroscope drift during long-time attitude measurement. The results of simulation test experiments on measured data demonstrate the algorithm can solve ambiguity resolution problem when the signal is unlock and can offer high precision attitude information under complex circumstance.

Beidou navigation satellite system/micro-electro-mechanical systems(BDS/MEMS) attitude determination; ambiguity resolution; ambiguity function method; extended Kalman filter

10.3979/j.issn.1673-825X.2016.06.009

2015-12-24

2016-10-08

刁春帆 409105807@qq.com

国家自然科学基金(61471077)

Foundation Item：The National Natural Science Foundation of China(61471077)

TN914.53

A

1673-825X(2016)06-0804-06