[发明专利]基于INS和GPS组合的垂线偏差动态测量装置及方法

摘要

本发明公开了一种基于INS和GPS组合的垂线偏差动态测量装置及方法。首先构建了INS/GPS姿态测量子系统和LGU/GPS姿态测量子系统，在开始测量的时刻，利用INS/GPS输出的姿态矩阵对LGU/GPS姿态测量子系统进初始化；此后，利用LGU/GPS和INS/GPS分别进行姿态解算，并计算LGU/GPS与INS/GPS输出的姿态角之差；通过建立垂线偏差测量的观测方程和状态方程，以INS/GPS与LGU/GPS输出的姿态角之差为观测量，在全球重力模型的辅助下，利用Kalman滤波的方法得到垂线偏差的最优估计值。本发明能有效降低系统的复杂度和成本，提高了测量的精度和可靠性，不依赖于测量环境，可操作性强且便于实施。

主权项

一种基于INS和GPS组合的垂线偏差动态测量方法，该方法基于以下装置实施，所述装置由惯性导航系统(1)、GPS天线(2)、GPS接收机(3)和数据处理计算机(4)组成，所述惯性导航系统(1)、GPS天线(2)以及GPS接收机(3)组成INS/GPS姿态测量子系统，所述惯性导航系统(1)包含了三个正交安装的激光陀螺，称为激光陀螺组合体，所述激光陀螺组合体、GPS天线(2)以及GPS接收机(3)组成LGU/GPS姿态测量子系统，三个激光陀螺均与GPS接收机(3)通信；所述惯性导航系统(1)、GPS天线(2)、GPS接收机(3)固联安装于测量载体(5)上，所述GPS天线(2)与所述GPS接收机(3)通信，所述惯性导航系统(1)、GPS接收机(3)通过数据线与数据处理计算机(4)连接，惯性导航系统(1)和GPS接收机(3)的测量数据通过数据线传输到数据处理计算机(4)中，在数据处理计算机(4)中完成垂线偏差的解算；其特征在于该方法包括以下步骤：1)构建INS/GPS姿态测量子系统，并进行8小时以上对准：启动INS/GPS姿态测量子系统，进行8小时以上对准，在对准过程中INS/GPS姿态测量子系统实现组合姿态测量，在整个测量过程中INS/GPS姿态测量子系统连续输出INS坐标系相对于计算导航坐标系的姿态矩阵对准过程中无垂线偏差数据输出；2)INS/GPS对准结束后，启动LGU/GPS姿态测量子系统，利用第一步中INS/GPS对准后输出的姿态矩阵对LGU/GPS进行姿态矩阵的初始化：记t_i为第i个测量采样点对应的时刻，1≤i≤N，N为整个测量过程的采样点总数，t_i时刻INS/GPS输出的姿态矩阵为LGU/GPS输出的姿态为上标n表示真实导航坐标系，启动LGU/GPS姿态测量子系统的时刻记为t₀，LGU/GPS姿态矩阵的初始化方法为令3)LGU/GPS姿态测量子系统进行姿态更新：LGU/GPS姿态测量子系统进行姿态更新，同时数据处理计算机存储全部测量采样时刻INS/GPS输出的姿态矩阵和LGU/GPS输出的姿态矩阵4)利用全部测量过程中t_i时刻INS/GPS输出的姿态矩阵和LGU/GPS输出的姿态矩阵计算坐标旋转矩阵并计算其输出的相应的姿态角之差：的计算方法为：$C_{n^{\′}\left(i\right)}^{n\left(i\right)}=C_{b\left(i\right)}^{n\left(i\right)}\·{\[C_{b\left(i\right)}^{n^{\′}\left(i\right)}\]}^T---\left(1\right)$其中[]^T表示矩阵的转置，符号·表示矩阵乘法；将任意t时刻计算得到的n′系到n系的姿态矩阵简记为任意t时刻INS/GPS与LGU/GPS输出的三个姿态角之差为：ΔΦ_E，ΔΦ_N，ΔΦ_U，其中，下标E、N、U分别表示东向、北向、天向分量，ΔΦ_E、ΔΦ_N和ΔΦ_U由下式计算得到：$\left\{\begin{array}{c}{\mathrm{\Δ\Φ}}_E=C_{n^{\′}}^n(2,3)\\ {\mathrm{\Δ\Φ}}_N=C_{n^{\′}}^n(3,1)\\ {\mathrm{\Δ\Φ}}_U=C_{n^{\′}}^n(1,2)\end{array}\right.---\left(2\right)$其中表示矩阵的第2行，第3列的元素，表示矩阵的第3行，第1列的元素，表示矩阵的第1行，第2列的元素；5)以INS/GPS与LGU/GPS输出的三个姿态角之差ΔΦ_E，ΔΦ_N，ΔΦ_U为观测量，通过建立垂线偏差测量的观测方程和状态方程，在全球重力模型的辅助下，利用Kalman滤波的方法提取垂线偏差：记t时刻测量载体所在位置真实的东向垂线偏差为η，北向垂线偏差为ξ，由全球重力模型计算得到的东向和北向垂线偏差值分别记为由全球重力模型计算得到的东向和北向垂线偏差误差分别记为δη和δξ，并存在下式的关系：$\left\{\begin{array}{c}\mathrm{\η}=\widehat{\mathrm{\η}}+\mathrm{\δ}\mathrm{\η}\\ \mathrm{\ξ}=\widehat{\mathrm{\ξ}}+\mathrm{\δ}\mathrm{\ξ}\end{array}\right.---\left(3\right)$记INS/GPS输出的相对于n′系的姿态误差角为v_E，v_N，v_U，LGU/GPS输出的相对于n系的姿态误差角为φ_E、φ_N、φ_U；具体实施方法如下：5.1)建立垂线偏差测量的状态方程：选取垂线偏差测量系统的状态变量为φ_E、φ_N、φ_U、δη、δξ、ε_U，其中ε_U为激光陀螺的等效天向零偏，分别对上述状态变量进行动态建模，φ_E、φ_N、φ_U满足如下微分方程：$\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_E=-{\mathrm{\ω}}_{ie}\cos L\·{\mathrm{\φ}}_U+{\mathrm{\ω}}_{ie}\sin L\·{\mathrm{\φ}}_N\\ {\stackrel{\·}{\mathrm{\φ}}}_N=-{\mathrm{\ω}}_{ie}\sin L\·{\mathrm{\φ}}_E\\ {\stackrel{\·}{\mathrm{\φ}}}_U={\mathrm{\ω}}_{ie}\cos L\·{\mathrm{\φ}}_E-{\mathrm{\ϵ}}_U\end{array}\right.---\left(4\right)$其中，ω_ie为地球自转角速度，L为测量点的地理纬度；ε_U建模为随机常值模型，则有：${\stackrel{\·}{\mathrm{\ϵ}}}_U=0---\left(5\right)$全球重力模型的东向和北向垂线偏差误差δη、δξ的统计模型分别由以下两式给出：其中x_E、x_N为中间状态变量，ω₀为该统计模型的固有频率，ω₀与测量载体的运动速度V之间存在固定关系ω₀＝2π×V/10000，为该统计模型的阻尼系数，w_E和w_N分别为东向和北向垂线偏差统计模型的过程噪声，w_E～(0,q_E)，w_N～(0,q_N)，即w_E和w_N分别服从均值为0的高斯分布，方差分别为q_E和q_N；选择垂线偏差测量系统的状态空间矢量为：x＝[φ_E,φ_N,φ_U,ε_U,x_E,x_N,δη,δξ]^T    (8)将式(4)‑(7)写为统一的状态方程的形式为：$\stackrel{\·}{x}=F\·x+w---\left(9\right)$其中，矩阵w＝[0,0,0,0,0,0,w_E,w_N]^T，式(9)即为垂线偏差测量系统的状态方程，过程噪声w～(0,Q)，即w服从均值为0，方差为Q的高斯分布，其中O_6×6、O_2×6、O_6×2分别表示6行6列、2行6列、6行2列的零矩阵，diag[q_E,q_N]表示以q_E和q_N为对角元素的对角矩阵；将状态方程(9)离散化得到：x_i＝M_i/i‑1·x_i‑1+w_i    (10)x_i表示x在t_i时刻的采样值,M_i/i‑1为t_i‑1到t_i时刻的状态转移矩阵，M_i/i‑1≈I_8×8+Δt·F_i，其中I_8×8为8维的单位矩阵，F_i为矩阵F在t_i时刻的采样值，Δt为采样间隔，w_i服从均值为0，方差为Q_i的高斯分布，Q_i＝Δt²·Q；5.2)建立垂线偏差测量的观测方程：以INS/GPS和LGU/GPS输出的姿态角之差ΔΦ_E，ΔΦ_N为观测量，建立如下观测方程：$\left\{\begin{array}{c}{\mathrm{\Δ\Φ}}_E=\mathrm{\ξ}-{\mathrm{\φ}}_E+v_E\\ {\mathrm{\Δ\Φ}}_N=-\mathrm{\η}-{\mathrm{\φ}}_N+v_N\end{array}\right.---\left(11\right)$将式(3)代入式(11)，整理后得到：$\left\{\begin{array}{c}{\mathrm{\Δ\Φ}}_E-\widehat{\mathrm{\ξ}}=\mathrm{\δ}\mathrm{\ξ}-{\mathrm{\φ}}_E+v_E\\ {\mathrm{\Δ\Φ}}_N+\widehat{\mathrm{\η}}=-\mathrm{\δ}\mathrm{\η}-{\mathrm{\φ}}_N+v_N\end{array}\right.---\left(12\right)$选取新的观测矢量为：$y={\[{\mathrm{\Δ\Φ}}_E-\widehat{\mathrm{\ξ}},{\mathrm{\Δ\Φ}}_N+\widehat{\mathrm{\η}}\]}^T---\left(13\right)$将式(12)重新写为：y＝H·x+v    (14)其中矩阵v＝[v_e，v_N]^T；式(14)即为垂线偏差测量系统的观测方程，INS/GPS的姿态误差角v_E、v_N分别为东向和北向观测分量的观测噪声，v_E～(0,r_E)，v_N～(0,r_N)，即v_E和v_N分别服从均值为0的高斯分布，方差分别为r_E和r_N，观测噪声v～(0,R)，即v服从均值为0，方差为R的高斯分布，观测噪声方差矩阵将观测方程离散化得到：y_i＝H_i·x_i+v_i    (15)y_i、v_i、H_i分别为y、v、H在t_i时刻的采样值，v_i服从均值为0，方差为R_i的高斯分布，R_i＝Δt²·R，H_i＝H；5.3)根据式(10)和(15)所描述的垂线偏差测量系统离散形式的状态方程和观测方程，利用Kalman滤波算法对状态矢量进行估计：Kalman滤波算法的迭代算法如下：$\left\{\begin{array}{c}{\hat{x}}_{i/i-1}=M_{i/i-1}\·{\hat{x}}_{i-1}\\ P_{i/i-1}=M_{i/i-1}\·P_{i-1}\·M_{i/i-1}^T+Q_i\\ K_i=P_{i/i-1}\·H_i^T\·{\[H_i\·P_{i/i-1}\·H_i^T+R_i\]}^{-1}\\ P_i=(I-K_i\·H_i)\·P_{i/i-1}\·{\[I-K_i\·H_i\]}^{-1}+K_i\·P_{i/i-1}\·K_i^T\\ {\hat{x}}_i={\hat{x}}_{i/i-1}+K_i\·(y_i-H_i\·{\hat{x}}_{i/i-1})\end{array}\right.---\left(16\right)$其中为x_i的估计值，为x_i的一步预测值，P_i为x_i的估计协方差矩阵，P_i/i‑1为P_i的一步预测值，K_i为滤波增益，[]^‑1符号表示矩阵的求逆运算；t_i时刻东向和北向垂线偏差误差δη、δξ的估计值分别为由t_i时刻的状态矢量估计值给出：$\left\{\begin{array}{c}\mathrm{\δ}{\widehat{\mathrm{\η}}}_i={\hat{x}}_i\left(7\right)\\ \mathrm{\δ}{\widehat{\mathrm{\ξ}}}_i={\hat{x}}_i\left(8\right)\end{array}\right.---\left(17\right)$其中，分别表示状态矢量估计值的第7和第8个元素；5.4)计算垂线偏差测量值：t_i时刻东向和北向垂线偏差测量值η_i、ξ_i分别由式(18)、(19)计算得到：${\mathrm{\η}}_i={\widehat{\mathrm{\η}}}_i+\mathrm{\δ}{\widehat{\mathrm{\η}}}_i---\left(18\right)$${\mathrm{\ξ}}_i={\widehat{\mathrm{\ξ}}}_i+\mathrm{\δ}{\widehat{\mathrm{\ξ}}}_i---\left(19\right)$其中分别为全球重力模型计算得到的t_i时刻测量点上东向和北向垂线偏差的值。