地固系GNSS／SINS组合导航正常重力间接算法

丁磊香，许厚泽，王 勇，柴 华，蔡小波

（1.中国科学院测量与地球物理研究所大地测量学与地球动力学国家重点实验室，武汉430077；2.中国科学院大学，北京100049)

地固系GNSS／SINS组合导航正常重力间接算法

丁磊香1，2，许厚泽1，王 勇1，柴 华1，蔡小波1，2

（1.中国科学院测量与地球物理研究所大地测量学与地球动力学国家重点实验室，武汉430077；2.中国科学院大学，北京100049)

相比导航系，GNSS／SINS组合导航在地固系计算，编排效率提高约40％，Kalman滤波效率提高约10％，即使正常重力与姿态矩阵的计算效率偏低，在地固系计算比导航系计算综合效率也提高约30％。针对地固系正常重力计算复杂的问题，提出了地固系正常重力间接算法，首先把三维直角坐标转换为大地坐标，然后在导航系计算正常重力，最后把正常重力转换到地固系，姿态矩阵也在此过程计算完毕。试验结果表明，间接法与传统方法计算精度相当，但间接法计算过程简单直观，计算效率可以提高3％～26％，尤其对于高速高精度的捷联惯导，效率提高尤为明显，具有重要的工程实用价值。

捷联惯导系统；正常重力；姿态矩阵；地固系；间接法

0 引言

GNSS／SINS组合导航具有采样率高、数据量大的特点，对于实时导航以及大数据量的后处理，计算效率是重要的考量。相比导航系，GNSS／SINS组合导航在地固系计算，编排效率提高约40％，Kalman滤波效率提高约10％，即使正常重力与姿态矩阵的计算效率偏低，在地固系计算比导航系计算综合效率也提高约30％［1⁃2］。另外，在GNSS／SINS组合导航中，GNSS的数据处理是在地固系进行的，选取地固系作为SINS的计算坐标系不需要GNSS导航结果的转换［3］，地固系特别适合于GNSS／SINS组合应用领域［2］。针对捷联惯导在地固系中的计算，文献［4］、文献［5］做了有益的研究。

GNSS／SINS组合导航在地固系计算中，姿态矩阵是通过地理坐标计算，地理坐标由地固系直角坐标转换得到，而正常重力是由地固系直角坐标计算［2⁃3］，计算公式和过程复杂，计算效率低。针对上述问题，本文提出了地固系正常重力的间接计算法，首先把地固系直角坐标转换为地理坐标，然后在导航系下计算正常重力，最后把正常重力转换到地固系，姿态矩阵也在此过程中计算。结果表明，在地固系中应用间接法计算正常重力，计算公式和过程简单直观，计算效率可以提高3％～26％。

1 坐标系的定义

地固系（e系)：原点在地球质心，x轴指向格林尼治平均子午圈，y轴与x轴、z轴正交，z轴平行于地球自转轴。

导航系又称当地水平坐标系（n系)：原点在载体中心，x轴沿参考椭球卯酉圈指向东，y轴沿参考椭球子午圈指向北，z轴沿椭球外法线指向天顶。

载体系（b系)：原点在载体中心，x轴指向载体的右方，y轴指向载体的前方，z轴指向载体的上方。

导航系向地固系的旋转矩阵由式（1)表示：

其中，B为地理纬度，L为地理经度。在本文中，把式（1)称为姿态矩阵。

姿态角根据式（2)矩阵的相应元素计算：

2 地固系正常重力与姿态矩阵的直接计算方法

地球的正常重力有严格的计算公式，由于公式复杂，计算量大，工程应用中一般使用近似值。在地固系中，正常重力可以根据直角坐标（x，y，z)得到，本文把此方法称为传统法或直接法。文献［2］推导了直接法的计算公式，在此直接给出：

其中，

在式（4)～式（9)中，J2、fM、a、ω为地球正常椭球的基本参数，参数的具体含义参见文献［6］，e为地球椭球的第一偏心率，可由基本参数求得。

由式（1)、式（2)可知，要得到姿态角，必须首先求得地理经纬度。地理经纬度可由地固系直角坐标求得，由直角坐标转换经纬度有许多不同解法［7⁃10］，一般分为3类：封闭解法、迭代法、近似算法，工程应用一般使用近似算法。不同于文献［2］的是，本文推荐使用文献［10］的近似算法，此种计算方法既简单，又有很高的计算精度，纬度的计算精度优于10-12（10-7″)［11］，详细的计算公式如下：

式中，B、L和H分别为大地坐标中的纬度、经度和大地高，a为地球正常椭球的长半轴，b为椭球的短半轴，e为椭球的第一偏心率，e′为椭球的第二偏心率。

把式（10)的值代入式（1)，即可求得姿态矩阵。

3 地固系正常重力的间接计算方法

间接法的计算过程如下：根据式（10)把地固系直角坐标转换为地理坐标，然后在导航系下根据式（11)计算正常重力，最后通过式（15)把正常重力转换到地固系。

根据索米里安公式可以计算椭球面任意一点的正常重力，再增加与大地高的有关项，可得到椭球面外任意一点的正常重力值。索米里安公式虽然是严密的，但并不适合于快速计算，一般展开成幂级数的形式，并省略高阶项。椭球外任意点正常重力的近似值可写为如下形式：

式中，B为纬度，H为大地高，系数k1～k5的值取决于椭球的参数。对于某一固定椭球，式（11)的系数是不变的，可直接应用，在此直接写出WGS⁃84椭球体对应的系数值。

式（12)求出的重力值是个标量，在导航系的正常重力可以表示为：

地理坐标由式（10)求出，可根据地理坐标直接求出式（1)，地固系的重力值为：

把式（1)代入式（14)，地固系的重力值可以表示为：

间接法正常重力的计算过程中，姿态矩阵已计算完毕，无需增加其他计算。

4 精度与效率比较

直观上，间接法比直接法公式简单，为客观反映两种方法的不同，对两种方法的精度与计算效率进行比较。

精度比较如下：两种姿态计算方法完全相同，姿态精度无需比较，只需要比较正常重力的计算精度，以北纬30°（不失一般性，经度设为零)椭球面一点的正常重力进行比较，参考值为索米里安公式的计算值，比较结果如表1所示。

表1 正常重力的近似误差比较Table 1 Approximate computation errors of normal gravity（unit：m／s2)

由表1可以得出，两种方法的计算精度相当，相比重力扰动和惯性传感器误差，近似误差可以忽略不计。

效率比较如下：和文献［1］方法相同，根据CPU时间比较两种方法的效率。假设SINS的采样频率为100Hz，分两种方式比较，第一种是只比较两种方法正常重力和姿态矩阵的计算效率；第二种是比较两种方法1s时长数据的捷联惯导系统编排效率。

表2为第一种方式的比较结果，通过表2可以看出，与直接法相比，应用间接法计算正常重力和姿态矩阵，效率提高约28％。

表2 正常重力与姿态矩阵计算的CPU时间Table 2 CPU time consumed by the computation of normal gravity and attitude matrix（unit：s)

对于低速的载体，重力的变化比较平缓，1Hz的正常重力与姿态矩阵计算频率就可以满足要求；但对于高速载体以及高精度捷联惯导，1Hz的正常重力与姿态矩阵计算频率精度不能满足要求。以速度50m／s载体为例，1Hz的计算频率引起的经纬度误差约为1.6″，通过式（15)可计算出引起的正常重力误差约为8mGal，与此相比，重力扰动的水平分量平均约为10mGal［12］，导航级加速度表零偏约为5μg～10μg。因此，1Hz的正常重力计算频率显然不能满足高速高精度的捷联惯导的需求。同样条件下，1Hz的姿态矩阵计算频率引起的姿态误差约为2″，同样不满足高速高精度的捷联惯导的需求。

第二种方式中，将比较正常重力和姿态矩阵不同计算频率时捷联惯导系统的编排效率，比较结果如表3所示。

表3 不同计算频率时编排的CPU时间Table 3 CPU time consumed by the computation of mechanization with different frequencies of normal gravity and attitude matrix（unit：s)

通过表3可以得出，不同计算频率时编排效率提高分别为：3％、7％、18％、26％，正常重力与姿态矩阵计算频率越高，间接法的效率越高，相比直接法，效率最大可以提高26％。

5 结论

捷联惯导在地固系中计算时，正常重力传统计算方法复杂、效率低，而且姿态矩阵需另行计算。本文提出了正常重力的间接计算法，计算方法简单，在计算过程中，姿态矩阵的计算得以完成。比较结果表明，在捷联惯导编排中，应用间接法计算正常重力与姿态矩阵，不仅精度可以得到保证，而且计算过程简单，相比直接法，效率可以提高3％～26％，尤其对于高速高精度的捷联惯导系统，计算效率提高尤为明显。

［1］Wei M，Schwarz K P.A strapdown inertial algorithm using an earth⁃fixed cartesian frame［J］.Navigation，1990，37（2)：153⁃167.

［2］董绪荣，张守信，华仲春.GPS／INS组合导航定位及其应用［M］.长沙：国防科技大学出版社，1998. DONG Xu⁃rong，ZHANG Shou⁃xin，HUA Zhong⁃chun. GPS／INS integrated navigation and positioning and its ap⁃plications［M］.Changsha：National University of Defense Technology Press，1998.

［3］柴艳菊.挖掘信息提高GPS／INS导航精度和理论与方法研究［D］.中国科学院测量与地球物理研究所，2008. CHAI Yan⁃ju.Theory and method for improving the navi⁃gation accuracy of GPS／INS integration by digging the hid⁃den information［D］.Institute of Geodesy and Geophysics，Chinese Academy of Sciences，2008.

［4］柴华，王勇，许大欣，等.地固系下四元数和卡尔曼滤波方法的惯导初始精对准研究［J］.武汉大学学报（信息科学版)，2012，37（1)：68⁃72. CHAI Hua，WANG Yong，XU Da⁃xin，et al.Inertial nav⁃igation system fine aliment under ECEF frame with quater⁃nion and Kalman filter［J］.Geomatics and Information Sciences of Wuhan University，2012，37（1)：68⁃72.

［5］张秋昭，张书毕，王坚，等.露天矿卡车低成本GPS／INS组合导航系统动态对准模型［J］.煤炭学报，2013，38（8)：1362⁃1367. ZHANG Qiu⁃zhao，ZHANG Shu⁃bi，WANG Jian，et al. Low⁃cost GPS／INS in⁃motion alignment model for open⁃pit mine transport truck monitoring and dispatch system［J］. Journal of China Coal Society，2013，38（8)：1362⁃1367.

［6］孔祥元，郭际明，刘宗泉.大地测量学基础［M］.武汉：武汉大学出版社，2005.

［8］KONG Xiang⁃yuan，GUO Ji⁃ming，LIU Zong⁃quan.Foun⁃dationofgeodesy［M］.Wuhan：WuhanUniversity Press，2005.

［7］Turner J D.A non⁃iterative and non⁃singular perturbation solution for transforming cartesian to geodetic coordinates［J］.Journal of Geodesy，2009，83（2)：139⁃145.

［8］Jones G C.New solutions for the geodetic coordinate transformation［J］.Journal of Geodesy，2002，76（8)：437⁃446.

［9］Vermeille H.Computing geodetic coordinates from geo⁃centric coordinates［J］.Journal of Geodesy，2004，78（1／2)：94⁃95.

［10］Bowring B R.The accuracy of geodetic latitude and height equations［J］.Survey Review，1985，28（218)：202⁃206.

［11］黄谟涛，翟国君，管铮，等.空间直角坐标和大地坐标的转换［J］.解放军测绘学院学报，1998，15（3)：164⁃168. HUANG Mo⁃tao，ZHAI Guo⁃jun，GUAN Zheng，et al. Ontheuseoftransformationformulaebetween rectangular and geodetic coordinates［J］.Journal of the PLA Institute of Surveying and Mapping，1998，15（3)：164⁃168.

［12］尧颖婷，沈晓蓉，邹尧，等.捷联惯性导航系统重力扰动影响分析［J］.大地测量与地球动力学，2011，31（6)：159⁃163. YAO Ying⁃ting，SHEN Xiao⁃rong，ZOU Yao，et al.A⁃nalysis of gravity disturbance influence on strapdown in⁃ertial navigation system［J］.Journal of Geodesy and Geo⁃dynamics，2011，31（6)：159⁃163.

Indirect Algorithm of SINS Normal Gravity under ECEF Frame

DING Lei⁃xiang1，2，XU Hou⁃ze1，WANG Yong1，CHAI Hua1，CAI Xiao⁃bo1，2
（1.State Key Laboratory of Geodesy and Earth's Dynamics，Institute of Geodesy and Geophysics，Wuhan 430077; 2.University of Chinese Academy of Sciences，Beijing 100049)

Compared with the navigation frame，processing GNSS／SINS integrated navigation in earth⁃centered⁃earth⁃fixed（ECEF)frame is about 30％more efficient，even if the computations of normal gravity and attitude matrix are of low efficiency.To address the issue of computation complexity of normal gravity in ECEF frame，this paper proposes a new method which computes normal gravity and attitude matrix indirectly.Compared with traditional direct method，this new in⁃direct method，which is more compact and simpler in terms of algorithm，can reach the same accuracy level.Furthermore，the computational efficiency of this new method is also higher than that of the traditional method by a factor of 3％～26％. The increased efficiency is particularly obvious，especially for high⁃speed and high⁃accuracy SINS.

SINS;normal gravity;attitude matrix;ECEF;indirect algorithm

P227.9

A

1674⁃5558（2017)01⁃01237

10.3969／j.issn.1674⁃5558.2017.01.001

2016⁃01⁃21

国家自然科学基金（编号：41274084，41406115)

丁磊香，男，博士，大地测量学与测量工程专业，研究方向为GNSS／INS组合导航理论及应用。