基于天文姿态基准保持的垂线偏差动态测量装置和方法

摘要

本发明公开了一种基于天文姿态基准保持的垂线偏差动态测量装置及方法，构建了INS/GPS姿态测量子系统和LGU/GPS姿态测量子系统，其中LGU/GPS姿态测量子系统进行姿态更新采用的初始值由星敏感器输出的姿态信息提供。将LGU/GPS姿态测量子系统输出的姿态和INS/GPS姿态测量子系统输出的姿态求差，进而计算垂线偏差。最后修正垂线偏差测量值中的跳变误差，并利用全球重力模型数据修正垂线偏差测量值中的低频误差。本发明系统简单；本发明的方法鲁棒性强，测量结果稳定；降低了GPS精度的需求，相比传统的矢量航空重力仪依赖于高精度差分GPS，本发明只需一般的GPS单点定位即可满足精度要求，因此拓展了测量方法的应用范围。

说明书

技术领域

本发明涉及测绘领域,特别是一种基于天文姿态基准保持的垂线偏差动态测量方法。

背景技术

由于地球形状的不规则、地形的起伏、密度分布不均等因素，实际的重力场与正常重 力模型之间存在差异，称之为重力扰动，其垂直分量称为重力异常，水平分量表现为垂线 偏差。垂线偏差是大地测量学和空间科学中非常重要的数据。通过垂线偏差可以计算高程 异常，确定大地水准面，还可应用于天文大地测量观测数据的归算、弹道计算、空间技术 及高精度惯性导航领域。此外，通过研究垂线偏差的时变特性与地震、火山活动之间的关 系，可以为地震和火山监测提供新的手段。

Journal of Surveying Engineering（《测绘工程杂志》）2010年第一期中“Modern  Determination of Vertical Deflections Using Digital Zenith Cameras”（利用数字天顶相机的垂 线偏差现代测量方法）论文公开了一种利用天顶数字相机测量垂线偏差的方法，其原理是 利用高精度的天顶数字相机获取载体相对于参考椭球面的水平姿态角，同时利用高精度电 子水平仪测量载体相对于真实大地水准面的水平姿态角，将两者求差可得到垂线偏差。该 方法的测量精度极高，可以达到0.1″，但由于只能进行静态测量，测量效率很低，因而对 人力、物力及时间的消耗巨大。

美国专利US005112126公开了一种用于矿井内垂线偏差测量的方法。其实施方法为： 利用天文经纬仪测量得到本地参考椭圆面的法线方向，即正常重力的方向，采用三个高精 度激光陀螺进行姿态解算，用于该法线方向的姿态保持，并将激光陀螺组合体放入矿井中， 同时将一个摆仪与激光陀螺组合体固联安装，利用摆仪测量当地真实垂线的方向，该发明 设计了一种光学测量装置用于测量真实垂线的方向与正常重力方向之间偏差角，即可得到 垂线偏差。该专利的缺点在于不能实现动态测量，且测量精度容易受陀螺测量误差的影响， 所设计的用于测量垂线偏差的光学测量装置结构较为复杂，通用性不强。

美国专利US3803916公开了一种重力扰动矢量测量的方案。其实施方法为：利用 LORAN（long range electronic navigation system远距电子导航系统）进行定位，测量载体 在天文坐标系中的位置；同时利用惯性导航系统进行定位解算，将解算结果和LORAN定 位结果做差，该差值反映的是陀螺零偏与垂线偏差对惯性导航系统的影响。在扣除陀螺零 偏引起的位置误差后，通过捷联惯性导航的反向滤波技术，利用位置误差反算出垂线偏差。 这种专利主要缺陷有两点：一是重力扰动模型建模（包括模型的形式和模型参数辨识）较 为困难，当模型存在误差时，所估计得到的重力扰动矢量误差较大；二是导航的水平姿态 误差角与垂线偏差存在严重的耦合，垂线偏差将引起导航的水平姿态角误差，同时水平姿 态角的误差也会引起严重的水平重力扰动估计误差，这两种状态量在只有位置辅助的条件 下是不可分离的。

美国专利US5924056和Journal of Geodesy（《大地测量杂志》）2001年的论文“A new  approach for airborne vector gravimetry using GPS-INS”（利用GPS/INS实现航空矢量重力 测量的新方法）中公开了一类矢量重力测量方法。它的基本原理是：利用INS（Inertial  Navigation System惯性导航系统）和GPS（Global Position System全球定位系统）进行组 合导航实现比力测量，利用高精度动态差分GPS测量载体的运动加速度，将比力输出和运 动加速度做差即可得到矢量重力扰动，这是目前国外研究中普遍采用的矢量重力测量方 案。该方案的主要缺陷在于比力的水平分量测量精度不高，垂线偏差的存在将导致水平姿 态角测量误差，进而将天向重力加速度耦合到比力的水平分量上，国内外采用多次迭代求 解的方法抑制这一影响，但效果欠佳，没有从本质上解决耦合问题，此外这种方案对GPS 精度要求极高，需要建立差分GPS基站，在远洋条件下无法使用。根据目前公开的文献， 该类矢量重力测量技术尚未有成熟的产品，其精度和分辨率难以令人满意。

重力梯度仪也是垂线偏差动态测量的有力工具，但目前只有少数国家发展出成熟的产 品，如Bell Geospace公司的Air-FTG重力梯度仪、美国专利US5357802公开的一种旋转 加速度计重力梯度仪等。由于涉及军事领域，重力梯度仪严格限制出口，并对关键技术严 格保密。国内的研究进展较为缓慢，离实用还有较大的差距。

鉴于以上原因，全球只有少数国家和地区具有较为丰富的高精度垂线偏差数据，且这 些垂线偏差数据大多数都未公开。近年来，卫星重力测量技术的发展使全球重力模型的精 度和分辨率不断提高，促使全球垂线偏差数据精度有了较大提高，并在大地测量、地球物 理、惯性导航等领域中得到广泛应用。然而，对于某些特殊领域的应用，如地震和火山的 监测、矿藏勘探及超高精度导航等，仍难以满足要求。

针对高精度垂线偏差数据获取困难的问题，迫切需要发展一种能够在动态条件下实现 垂线偏差测量的新方法，以更高效地获取高精度、高分辨率的垂线偏差数据，降低测量所 消耗的成本和时间，推动其他相关学科领域的发展。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种基于天文姿态基准保持 的垂线偏差动态测量装置及方法，在满足测量精度和分辨率需求的基础上，降低垂线偏差 测量所消耗的成本和时间，提高测量效率，同时使测量精度稳定，无测量实施的地域限制。

为解决上述技术问题，本发明所采用的技术方案是：一种基于天文姿态基准保持的垂 线偏差动态测量装置，包括由惯性导航系统（INS）、GPS天线、GPS接收机组成的INS/GPS 姿态测量子系统、星敏感器，所述惯性导航系统包括三个正交安装的激光陀螺，所述三个 正交安装的激光陀螺与GPS接收机组成LGU/GPS姿态测量子系统，三个激光陀螺均与GPS 接收机通信；所述惯性导航系统、GPS天线、GPS接收机、星敏感器均安装于测量载体上； 所述惯性导航系统、GPS接收机、星敏感器均与数据处理计算机连接；所述GPS天线与所 述GPS接收机通信。

INS、GPS天线以及星敏感器固联安装于测量载体上，测量载体可以是测量船、测量 车等运载工具。INS、GPS接收机和星敏感器的测量数据通过数据线传输到数据处理计算 机中，在数据处理计算机中完成垂线偏差的解算。

所采用的星敏感器安装于测量载体时，应使其光轴尽可能朝向天顶。由于星敏感器的 光轴指向测量精度优于横滚角测量精度，当光轴指向天顶时可以提高水平姿态角的测量精 度，此外还可以抑制大气折射误差的影响。INS采用龙兴武等在2010年《中国惯性技术学 报》第2期“激光陀螺单轴旋转惯性导航系统”论文中公开的单轴旋转式结构，通过单轴旋 转结构对惯性器件进行周期性调制，可以有效抑制惯性器件的零偏误差，提高INS的姿态 解算精度。

本发明还提供了一种利用上述装置动态测量垂线偏差的方法，该方法为：

1）启动INS/GPS姿态测量子系统，进行8小时以上对准。对准过程中INS/GPS姿态 测量子系统即开始进行姿态测量，但无垂线偏差数据输出。

INS/GPS组合姿态测量的方法可参照全伟等著、国防工业出版社2011年出版的《惯性 /天文/卫星组合导航技术》中120～123页介绍的方法建立INS/GPS组合导航的状态方程和 观测方程，再根据第50～52页介绍的Kalman滤波算法，对INS的姿态误差、速度误差、 位置误差、陀螺零偏、加速度计零偏进行估计，并利用估计得到的东向、北向和天向姿态 误差对INS的姿态矩阵输出进行修正，得到修正后的姿态信息为INS坐标系 （b系）相对于计算导航坐标系n'的姿态矩阵修正方法参考秦永元等著、西北工业 大学出版社2012年出版的《卡尔曼滤波与组合导航原理》中337页中的公式（8.5.37）～ （8.5.39）。Kalman滤波估计得到的X、Y、Z三个陀螺的零偏分别记为ε_x、ε_y、ε_z。在 整个测量过程中INS/GPS组合姿态测量子系统连续输出姿态信息和三个陀螺零偏的估 计值ε_x、ε_y、ε_z。

2）完成INS/GPS姿态测量子系统的对准后，启动LGU/GPS测量子系统，开始进行垂 线偏差测量，

3）令i=1；

4）利用星敏感器观测恒星，计算t_i,1时b系相对于n系的姿态矩阵其中b系是 指惯性导航坐标系O_b-x_by_bz_b；n系是指真实导航坐标系，即东-北-天坐标系O-xyz；

5）利用上述姿态矩阵对LGU/GPS姿态测量子系统进行姿态初始化，令LGU/GPS 姿态测量子系统的姿态矩阵为

6）令j=2；

7）利用GPS输出的位置信息计算t_i,j-1到t_i,j时刻n系的变化矩阵将t_i,j时刻 测量载体所在的经度和纬度分别记为λ_i,j、L_i,j，则t_i,j-1和t_i,j时刻n系相对于地球坐标系e 系的姿态矩阵分别为和的计算公式如下：

$C_e^{n(i,j)}=\left(\begin{array}{ccc}-{\sin \lambda }_{i,j}& \cos {\lambda }_{i,j}& 0\\ -\sin L_{i,j}{\cos \lambda }_{i,j}& {-\sin L}_{i,j}{\sin \lambda }_{i,j}& {\cos L}_{i,j}\\ {\cos L}_{i,j}{\cos \lambda }_{i,j}& {\cos L}_{i,j}{\sin \lambda }_{i,j}& {\sin L}_{i,j}\end{array}\right);$

其中所述t_i,j时刻是指第i次利用星敏感器观测恒星后，第j次LGU/GPS姿态测量子系 统姿态更新的时刻；t_i,j=t_i,j-1+δt，δt为激光陀螺的采样周期；

则$C_{n(i,j-1)}^{n(i,j)}=C_e^{n(i,j)}·{\left[C_e^{n(i,j-1)}\right]}^T,$其中，[]^T表示矩阵的转置；

8）将t_i,j-1到t_i,j时刻采样周期δt内三个激光陀螺的输出角增量分别记为并利用t_i,j时刻INS/GPS姿态测量子系统中Kalman滤波估计得到的三个激光陀螺的零偏 ε_x、ε_y、ε_z修正三个激光陀螺的输出角增量，得到修正后的三个激光陀螺角增量分别为： ${\theta }_x={\hat{\theta }}_x-{ϵ}_x,$${\theta }_y={\hat{\theta }}_y-{ϵ}_y,$${\theta }_z={\hat{\theta }}_z-{ϵ}_z;$

9）利用下式构造四元数q：

$q=\left(\begin{array}{c}\cos \frac{\theta }{2}\\ \frac{{\theta }_x}{\theta }\sin \frac{\theta }{2}\\ \frac{{\theta }_y}{\theta }\sin \frac{\theta }{2}\\ \frac{{\theta }_z}{\theta }\sin \frac{\theta }{2}\end{array}\right);$

其中$\theta =\sqrt{{\theta }_x^2+{\theta }_y^2+{\theta }_z^2};$

10）计算t_i,j-1到t_i,j时刻测量INS坐标系的变化矩阵

$C_{b(i,j-1)}^{b(i,j)}=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right);$

其中，q₀、q₁、q₂、q₃分别为q的第1～4个元素；

11）计算t_i,j时刻b系相对于n系的姿态矩阵

$C_{b(i,j)}^{n(i,j)}=C_{n(i,j-1)}^{n(i,j)}·C_{b(i,j-1)}^{n(i,j-1)}·{\left[C_{b(i,j-1)}^{b(i,j)}\right]}^T;$

12）计算t_i,j时刻n'系到n系之间的姿态旋转矩阵其 中n'系是指O'-x'y'z'，即计算导航坐标系，为t_i,j时刻INS/GPS姿态测量子系统输 出的姿态矩阵；；

13）利用下式计算t_i,j时刻测量位置东向垂线偏差η_i,j和北向垂线偏差ξ_i,j： $\left(\begin{array}{c}{\xi }_{i,j}=C_{n^{\prime }(i,j)}^{n(i,j)}\left(\mathrm{3,1}\right)\\ {\eta }_{i,j}=C_{n^{\prime }(i,j)}^{n(i,j)}\left(\mathrm{2,3}\right)\end{array}\right);$其中，表示矩阵的第3行，第1列的元素；表示矩阵的第2行，第3列的元素；$C_{n^{\prime }(i,j)}^{n(i,j)}\approx \left(\begin{array}{ccc}1& 0& -{\xi }_{i,j}\\ 0& 1& {\eta }_{i,j}\\ {\xi }_{i,j}& -{\eta }_{i,j}& 1\end{array}\right);$

14）判断是否接收到“结束测量”的命令，如果是，跳转到步骤16）；否则，执行步 骤15）；

15）当j·δt>T_i时，令i=i+1，并跳转到步骤4）；否则，令j=j+1，并跳转到步骤7）； 其中，T_i为星敏感器观测恒星时刻t_i,1到下一次星敏感器观测恒星时刻t_i+1,1所经历的时间， T_i的取值范围为6～8小时；

16）修正上述步骤13）中的东向垂线偏差η_i,j和北向垂线偏差ξ_i,j的低频误差和跳变误 差；

17）结束测量。

与现有技术相比，本发明所具有的有益效果为：本发明构建了LGU/GPS姿态测量子系 统进行姿态测量，将LGU/GPS的姿态输出与INS/GPS姿态测量子系统的姿态输出求差，该 装置结构简单，可以直接测量n系与n′系坐标系偏差，不需要测量实施区域内垂线偏差 的先验信息，因而本发明方法鲁棒性强，测量结果稳定；降低了GPS精度的需求，相比传 统的矢量航空重力仪依赖于高精度差分GPS，本发明只需一般的GPS单点定位即可满足精 度要求，因此拓展了测量方法的应用范围；惯性导航系统（INS）采用的是单轴旋转式的 惯性导航系统，通过单轴旋转结构对惯性器件进行周期性调制，可以有效抑制惯性器件的 零偏误差，提高了INS的导航解算精度；本发明可利用某测量点的垂线偏差数据替代星敏 感器对LGU/GPS姿态测量子系统进行初始化，从而减小了对星敏感器等天文姿态基准的依 赖。

附图说明

图1是本发明垂线偏差动态测量装置结构示意图；

图2是导航坐标系与垂线偏差的关系；

图3是IMU的结构示意图；

图4是本发明的垂线偏差动态测量方法流程图；

图5是利用LGU/GPS姿态测量子系统解算姿态矩阵结合INS/GPS姿态测量子 系统的姿态输出计算东向和北向垂线偏差η、ξ的方法流程图。

具体实施方式

本发明采用的垂线偏差动态测量装置如图1所示，由INS1、GPS天线2、GPS接收 机3、星敏感器4、测量载体5和数据处理计算机6构成。其中INS1、GPS天线2及星 敏感器4固联安装于测量载体5上，测量载体可以是测量船、测量车等运载工具。INS1、 GPS接收机3和星敏感器4的测量数据通过数据线传输到数据处理计算机6中，在数据 处理计算机6中完成垂线偏差的解算。

所采用的星敏感器安装于测量载体时，应使其光轴尽可能朝向天顶。由于星敏感器的 光轴指向测量精度优于横滚角测量精度，当光轴指向天顶时可以提高水平姿态角的测量精 度，此外还可以抑制大气折射误差的影响。INS采用单轴旋转式结构，通过单轴旋转结构 对惯性器件进行周期性调制，可以有效抑制惯性器件的零偏误差，提高INS的导航解算精 度。

图4为本发明测量方法总体流程图，主要包含以下七个步骤：

步骤1：构建INS/GPS姿态测量子系统。

步骤2：构建LGU/GPS姿态测量子系统。

步骤3：启动INS/GPS姿态测量子系统，进行8小时以上对准。

步骤4：启动星敏感器和LGU/GPS姿态测量子系统，开始测量。

步骤5：利用LGU/GPS姿态测量子系统解算姿态矩阵结合INS/GPS姿态测量子 系统的姿态输出计算东向和北向垂线偏差η、ξ。

步骤6：修正垂线偏差中的低频误差和跳变误差。

步骤7：结束测量。

如图1所示，定义INS的坐标系为b系O_b-x_by_bz_b，星敏感器坐标系为O_ss-x_ssy_ssz_ss。 如图2所示，定义东-北-天坐标系O-xyz为真实导航坐标系，即n系，其中O-xy平面与 地球的参考椭球面平行。由于垂线偏差的存在，惯性导航解算得到的计算导航坐标系与真 实的导航坐标系存在偏差，定义计算导航坐标系为O'-x'y'z'，即n'系，计算导航坐标系 的定义可参考高钟毓著、清华大学出版社2012年出版的《惯性导航系统技术》第225-226 页的描述。

本发明具体实施步骤如下：

步骤1：构建INS/GPS姿态测量子系统。

由图1装置中的INS1、GPS天线2和GPS接收机3构建INS/GPS姿态测量子系统， INS/GPS组合姿态测量的方法可参照全伟等著、国防工业出版社2011年出版的《惯性/天 文/卫星组合导航技术》中120～123页介绍的方法建立INS/GPS组合导航的状态方程和观 测方程，再根据第50～52页介绍的Kalman滤波算法，对INS的姿态误差、速度误差、位 置误差、陀螺零偏、加速度计零偏进行估计，并利用估计得到的东向、北向和天向姿态误 差对INS的姿态矩阵输出进行修正，得到修正后的姿态信息为INS坐标系（b 系）相对于计算导航坐标系n'的姿态矩阵修正方法参考秦永元等著、西北工业大学 出版社2012年出版的《卡尔曼滤波与组合导航原理》中337页中的公式（8.5.37）～（8.5.39）。 Kalman滤波估计得到的X、Y、Z三个陀螺的零偏分别记为ε_x、ε_y、ε_z。在整个测量过程 中INS/GPS组合姿态测量子系统连续输出姿态信息和三个陀螺零偏的估计值ε_x、ε_y、 ε_z。

步骤2：构建LGU/GPS姿态测量子系统。图1中INS1的核心测量部件为IMU（Inertial  Measure Unit惯性测量单元），如图3所示，IMU中包含3个正交安装的激光陀螺111、 112、113和3个正交安装的加速度计12。利用IMU中的3个激光陀螺构建新的测量组件， 称为LGU，再利用LGU和GPS可构建另一个组合姿态测量子系统，称为LGU/GPS姿态 测量子系统。LGU/GPS实现姿态测量算法将在后续步骤中详细说明。

步骤3：启动INS/GPS姿态测量子系统，进行8小时以上对准。对准过程中无垂线偏 差数据输出。

步骤4：启动星敏感器和LGU/GPS姿态测量子系统，开始测量。

步骤5：利用LGU/GPS姿态测量子系统解算姿态矩阵结合INS/GPS姿态测量子 系统输出的姿态矩阵计算东向和北向垂线偏差η、ξ，如图2所示。具体实施方法如下：

记第i次利用星敏感器观测恒星的时刻记为t_i,1。

步骤5.1：令i=1。

步骤5.2：利用星敏感器观测恒星，获取b系相对于真实导航坐标系n系的姿态矩阵 并对LGU/GPS姿态计算进行初始化，具体实施方法为：

步骤5.2.1：利用星敏感器观测恒星，获取星敏感器坐标系相对于n系的姿态矩阵利用星敏感器与INS之间的安装矩阵通过式（1）计算b系相对于n系的姿态矩阵

$C_b^n=C_{\mathrm{ss}}^n·C_b^{\mathrm{ss}}---\left(1\right)$

其中“·”表示矩阵相乘，采用王岩在2009年《战术导弹控制技术》期刊第3期 “一种星敏感器与捷联惯导高精度安装误差标定方法”论文中公开的方法事先标定得到。

t_i,1时刻由式（1）计算得到的b系相对于n系的姿态矩阵记为

还可以采用以下替代方案计算矩阵：

当已知t_i,1时刻测量载体所在位置上的垂线偏差η_i,1,ξ_i,1时，可利用η_i,1,ξ_i,1代替星敏感器 数据计算b系相对于n系的姿态矩阵计算方法如下：

利用η_i,1,ξ_i,1由式（2）计算n系和n′系之间的旋转矩阵

$C_{n^{\prime }(i,1)}^{n(i,1)}\approx \left(\begin{array}{ccc}1& 0& -{\xi }_{i,1}\\ 0& 1& {\eta }_{i,1}\\ {\xi }_{i,1}& -{\eta }_{i,1}& 1\end{array}\right)---\left(2\right)$

利用INS/GPS姿态测量子系统在t_i,1时刻输出的姿态矩阵由式（3）计算

$C_{b(i,1)}^{n(i,1)}=C_{n^{\prime }(i,1)}^{n(i,1)}·C_{b(i,1)}^{n^{\prime }(i,1)}---\left(3\right)$

使用该替代方案可以脱离对星敏感器等天文姿态基准的依赖。

步骤5.2.2：用矩阵对LGU/GPS姿态测量子系统进行姿态初始化，令LGU/GPS 的姿态矩阵为

步骤5.3：以为初值，利用激光陀螺和GPS的输出进行LGU/GPS姿态更新，计 算b系相对于n系的姿态矩阵更新算法如下：

记第i次利用星敏感器观测恒星后，第j次LGU/GPS姿态更新的时刻记为t_i,j， t_i,j=t_i,j-1+δt，δt为激光陀螺的采样周期。记t_i,j时刻b系相对于n系的姿态矩阵为

步骤5.3.1：令j=2。

步骤5.3.2：利用GPS输出的位置信息计算t_i,j-1到t_i,j时刻导航坐标系的变化矩阵。

t_i,j时刻载体所在的经度和纬度分别记为λ_i,j,L_i,j。则t_i,j-1和t_i,j时刻导航坐标系相对于地 球坐标系e系的姿态矩阵分别记为和可由式(4)计算：

$C_e^{n(i,j)}=\left(\begin{array}{ccc}-{\sin \lambda }_{i,j}& \cos {\lambda }_{i,j}& 0\\ -\sin L_{i,j}{\cos \lambda }_{i,j}& {-\sin L}_{i,j}{\sin \lambda }_{i,j}& {\cos L}_{i,j}\\ {\cos L}_{i,j}{\cos \lambda }_{i,j}& {\cos L}_{i,j}{\sin \lambda }_{i,j}& {\sin L}_{i,j}\end{array}\right)---\left(4\right)$

将式（4）中j替换为j-1可计算得到由式(5)计算

$C_{n(i,j-1)}^{n(i,j)}=C_e^{n(i,j)}·{\left[C_e^{n(i,j-1)}\right]}^T---\left(5\right)$

步骤5.3.3：利用激光陀螺输出的角增量计算t_i,j-1到t_i,j时刻INS坐标系的变化矩阵 $C_{b(i,j-1)}^{b(i,j)}.$

t_i,j-1到t_i,j时刻采样周期δt内，三个激光陀螺111、112、113输出的角增量分别记为采用步骤1由Kalman滤波估计得到的三个陀螺的零偏ε_x、ε_y、ε_z对陀螺角增量 进行修正，修正后的陀螺角增量为：${\theta }_x={\hat{\theta }}_x-{ϵ}_x,$${\theta }_y={\hat{\theta }}_y-{ϵ}_y,$${\theta }_z={\hat{\theta }}_z-{ϵ}_z;$

由式（6）构造旋转四元数：

$q=\left(\begin{array}{c}\cos \frac{\theta }{2}\\ \frac{{\theta }_x}{\theta }\sin \frac{\theta }{2}\\ \frac{{\theta }_y}{\theta }\sin \frac{\theta }{2}\\ \frac{{\theta }_z}{\theta }\sin \frac{\theta }{2}\end{array}\right)---\left(6\right)$

其中q₀、q₁、q₂、q₃分别为q的第1～4个元素。可由式(7) 计算：

$C_{b(i,j-1)}^{b(i,j)}=\left(\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right)---\left(7\right)$

步骤5.3.4：t_i,j时刻b系相对于n系的姿态矩阵可由式（8）计算：

$C_{b(i,j)}^{n(i,j)}=C_{n(i,j-1)}^{n(i,j)}·C_{b(i,j-1)}^{n(i,j-1)}·{\left[C_{b(i,j-1)}^{b(i,j)}\right]}^T---\left(8\right)$

其中[]^T表示矩阵的转置。

步骤5.4：计算t_i,j时刻n'系到n系之间的姿态旋转矩阵

t_i,j时刻INS/GPS姿态测量子系统输出的姿态矩阵记为由式（9）可计算矩阵 $C_{n^{\prime }(i,j)}^{n(i,j)}.$

$C_{n^{\prime }(i,j)}^{n(i,j)}=C_{b(i,j)}^{n(i,j)}·{\left[C_{b(i,j)}^{n^{\prime }(i,j)}\right]}^T---\left(9\right)$

步骤5.5：由矩阵计算t_i,j时刻测量位置上的垂线偏差。

记东向垂线偏差为η_i,j，北向垂线偏差为ξ_i,j。η_i,j、ξ_i,j与矩阵近似满足式（10） 的关系：

$C_{n^{\prime }(i,j)}^{n(i,j)}\approx \left(\begin{array}{ccc}1& 0& -{\xi }_{i,j}\\ 0& 1& {\eta }_{i,j}\\ {\xi }_{i,j}& -{\eta }_{i,j}& 1\end{array}\right)---\left(10\right)$

可以通过式（11）求得垂线偏差：

$\left(\begin{array}{c}{\xi }_{i,j}=C_{n^{\prime }(i,j)}^{n(i,j)}\left(\mathrm{3,1}\right)\\ {\eta }_{i,j}=C_{n^{\prime }(i,j)}^{n(i,j)}\left(\mathrm{2,3}\right)\end{array}\right)---\left(11\right)$

其中表示矩阵的第3行，第1列的元素，表示矩阵的 第2行，第3列的元素。

步骤5.6：判断是否收到“结束测量”的命令，如果接受到“结束测量”的命令则跳转 到步骤6。否则，继续执行步骤5.7。

步骤5.7：本次星敏感器观测恒星时刻t_i,1到下一次星敏感器观测恒星时刻t_i+1,1所经历 的时间记为T_i。当j·δt>T_i时，令i=i+1，并跳转到步骤5.2。否则，令j=j+1，并跳转到 步骤5.3.2。

T_i可设置的范围为：6小时<T_i<8小时，可以根据测量的条件灵活设定。一般选择星敏 感器观测到的恒星成像清晰，无云雾遮挡，测量载体运动状态平稳的时刻观测恒星。

步骤6：修正垂线偏差的低频误差和跳变误差，其具体修正方法如下：

步骤6.1：修正垂线偏差数据的跳变误差。

由步骤5得到的垂线偏差测量序列为η_i,j、ξ_i,j（i=1…M，j=1…N_i），M为全部测量过 程利用星敏感器对LGU/GPS姿态初始化的次数，N_i为第i次观测恒星后LGU/GPS连续进 行步骤5.3姿态更新的次数。

由于星敏感器具有一定的姿态测量误差，因此每次在步骤5.2对LGU/GPS进行姿态初 始化时，t_i,1时刻的垂线偏差η_i,1、ξ_i,1相对于其前一时刻的垂线偏差存 在跳变误差。记第i次对LGU/GPS进行姿态初始化时产生的东向垂线偏差和北向垂线偏差 的跳变误差分别为和的计算方法如下：

步骤6.1.1：令${▿}_{\eta ,1}=0,{▿}_{\xi ,1}=0$

步骤6.1.2：令i=i+1

步骤6.1.3：$\left(\begin{array}{c}{▿}_{\eta ,i}={▿}_{\eta ,i-1}+({\eta }_{i,1}-{\eta }_{i-1,N_{i-1}})\\ {▿}_{\xi ,i}={▿}_{\xi ,i-1}+({\xi }_{i,1}-{\xi }_{i-1,N_{i-1}})\end{array}\right)---\left(12\right)$步 骤6.1.4：$\left(\begin{array}{c}{\eta }_{i,j}={\eta }_{i,j}-{▿}_{\eta ,i}\\ {\xi }_{i,j}={\xi }_{i,j}-{▿}_{\xi ,i}\end{array}\right)---\left(13\right)$

其中对全部1≤j≤N_i执行式（13）。

步骤6.1.5：若i<M，，跳转到步骤6.1.2，直到i=M。

步骤6.2：修正垂线偏差数据的低频误差。

为了便于描述，将全部测量时刻t_i,j按时间顺序排列，并记为单下标的形式t_k（1≤k≤ N），k为第k个垂线偏差测量数据点的序号，N为垂线偏差测量数据点的总个数。将t_i,j时 刻对应的垂线偏差数据η_i,j、ξ_i,j记为η_k、ξ_k，相应的EGM2008垂线偏差数据记 为${\hat{\eta }}_k.{\hat{\xi }}_k..$

步骤6.2.1：根据t_k时刻载体所在的位置读取全球重力数据库EGM2008的垂线偏差数 据，记为主要包含垂线偏差数据的低频信息。

步骤6.2.2：将测量得到的垂线偏差数据η_k、ξ_k中的低频部分利用式（14）移除，得到 剩余量δη_k、δξ_k包含垂线偏差的高频信息。

$\left(\begin{array}{c}{\mathrm{\delta \eta }}_k={\eta }_k-{\hat{\eta }}_k\\ {\mathrm{\delta \xi }}_k={\xi }_k-{\hat{\xi }}_k\end{array}\right)---\left(14\right)$

对全部1≤k≤N执行式（14）的操作。

步骤6.2.3：利用多项式模型拟合δη_k、δξ_k中存在的缓变误差，并将该缓变误差扣除， 得到垂线偏差的高频信息具体的修正方法如下：

以k（1≤k≤N）为自变量，对数据序列δη_k、δξ_k分别进行多项式拟合，采用的拟合 方法为最小二乘法，拟合的模型为：

$\left(\begin{array}{c}{y}_{\eta }\left(k\right)=a_{\eta ,0}+a_{\eta ,1}·k+a_{\eta ,2}·k^2\\ y_{\xi }\left(k\right)=a_{\xi ,0}+a_{\xi ,1}·k+a_{\xi ,2}·k^2\end{array}\right)---\left(15\right)$

其中a_η,0、a_η,1、a_η,2为由自变量k和应变量δη_k拟合得到的模型系数，y_η(k)为由拟合 模型计算得到的第k个垂线偏差测量数据点的δη_k缓变误差。a_ξ,0、a_ξ,1、a_ξ,2为由自变量k 和应变量δξ_k拟合得到的模型系数，y_ξ(k)为由拟合模型计算得到的第k个数据点的δξ_k缓 变误差。

对全部1≤k≤N执行式（15）的操作。

利用式（16）扣除δη_k和δξ_k中的缓变误差：

$\left(\begin{array}{c}\delta {\hat{\eta }}_k={\mathrm{\delta \eta }}_k-y_{\eta }\left(k\right)\\ \delta {\hat{\xi }}_k={\mathrm{\delta \xi }}_k-y_{\xi }\left(k\right)\end{array}\right)---\left(16\right)$

对全部1≤k≤N执行式（16）的操作。

步骤6.2.4：将EGM2008的垂线偏差数据序列与式（17）求得的垂线偏差高频 量δη_k、δξ_k相加即可得到垂线偏差。

$\left(\begin{array}{c}{\eta }_k={\hat{\eta }}_k+\delta {\hat{\eta }}_k\\ {\xi }_k={\hat{\xi }}_k+\delta {\hat{\xi }}_k\end{array}\right)---\left(17\right)$

对全部1≤k≤N执行式（17）的操作。

步骤7：结束测量。