CN103941274A - 一种导航方法及导航终端 - Google Patents

Abstract

本发明提供了一种导航方法及导航终端；方法包括：接收北斗卫星导航信号，接收内部射频和基带芯片数据，经位置、速度、时间解算处理后，得到北斗卫星导航预处理数据；获取外部观测信息及原始惯性测量数据；根据所述外部观测信息及所述原始惯性测量数据计算误差数据；所述外部观测信息包括温度、启动时间；对所述原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果；根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波，输出滤波结果。本发明将卫星导航和惯性导航有机结合，获得优于任何单一导航系统的导航精度和可靠性。

Description

一种导航方法及导航终端

技术领域

本发明涉及导航领域，尤其涉及一种导航方法及导航终端。

背景技术

全球卫星导航系统（GNSS）可为全球范围提供全天候、连续、精密的三维定位和导航服务，已广泛应用于航空、航天、航海及地面运载工具的导航和定位，其中北斗卫星导航系统是中国正在实施的自主发展、独立运行的全球卫星导航系统，是独立自主、开放兼容、技术先进、稳定可靠的卫星导航系统。北斗卫星导航系统的突出优点是其导航定位精度高，并且导航定位精度基本不受时间和地域的限制。但是当卫星信号受到遮挡或干扰时，其优势就不能发挥出来，在需要获得载体连续的位置、速度、姿态等控制信息时，也不能满足其性能要求。因此目前GNSS还只能作为一种辅助导航设备，而不能作为唯一的导航设备使用。

SINS（捷联式惯性导航系统）直接利用安装在载体上的惯性测量单元（IMU）测量出载体沿三个轴的加速度和角速率，经过变换和积分等运算得到载体的姿态、速度、位置等信息。微型捷联惯导系统（MSINS）在体积、功耗、耐冲击等方面的优点使惯性系统的应用领域更加宽广，纯惯导系统的优点是不依赖于任何外界信息就能够实现完全自主的导航，抗干扰能力强，隐蔽性、实时性好。但目前SINS的精度比较低，导致惯导系统误差随着时间积累而增大，因此，单独使用SINS来实现较高精度的载体制导任务就难以胜任。为此，需要利用其它定位手段作为参考信息源，定期对SINS进行校正和对漂移进行补偿。

发明内容

本发明要解决的技术问题是如何将卫星导航和惯性导航有机结合，获得优于任何单一导航系统的导航精度和可靠性。

为了解决上述问题，本发明提供了一种导航方法，包括：

接收北斗卫星导航信号，接收内部射频和基带芯片数据，经位置、速度、时间解算处理后，得到北斗卫星导航预处理数据；

获取外部观测信息及原始惯性测量数据；根据所述外部观测信息及所述原始惯性测量数据计算误差数据；所述外部观测信息包括温度、启动时间；

对所述原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果；

根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波，输出滤波结果。

可选地，所述原始惯性测量数据包括：陀螺仪原始测量数据、加速度计原始测量数据、磁力计原始测量数据；

根据外部观测信息及所述原始惯性测量数据计算误差数据的步骤包括：

计算各轴的位置误差C1为：

C1=(Z–B)/(1+S)

其中Z为所计算的轴的原始观测数据，B为所计算的轴的标定偏置误差，S为所计算的轴的刻度因子误差；

计算陀螺仪的稳定零偏误差：

W_稳定零偏=a₀+a₁t+a₂T+a₃T²+a₄T³+a₅△T；

其中，t为陀螺仪的启动时间，T为陀螺仪的内部温度，△T为温度变化梯度，a₀为常数项，a₁～a₅为系数；

计算姿态误差角C2为：

C2=A₁×B₁–B₂×A₂

A₁为解算前上一个采样点陀螺仪的原始观测数据，B₁为解算前上一个采样点的加速度计的原始观测数据，A₂为解算时当前采样点陀螺仪的观测数据，B₂为解算时当前采样点的加速度计的观测数据；

计算速度误差C3为：

C3=1/2×（A₂×B₂+B₁×A₁+B₁×A₂）/12.0。

可选地，对所述原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果的步骤包括：

计算解算点的地球曲率半径：

$M=a*(1-e^2)/{(1-e^2*\sin {\left(\mathrm{lat}\right)}^2)}^{\frac{3}{2}}$

其中a为地球长半轴半径，e为地球扁率，lat为捷联导航解算点的纬度；

进行速度解算：

R_d=V_n×0.5×D_t/(R_m+D_r)

V₁=V_n+0.5×D_v

进行位置解算：

W_en=V_n×k(M_v+R_n×D_v)

R=R₁–D_t×D_v

R_d为载体坐标系上四元数表示的投影分量，V_n为当前坐标系下的速度分量，V₁为迭代后的三轴速度分量，D_t是时间历元，D_v为导航系下的速度向量，D_r为地理坐标系下的位置向量，R_m为地球子午线对应的曲率半径；W_en为导航系下的转换速率，R_n为卯酉圈的曲率半径，M_v为差值速度向量，k为转换系数；R₁为上一解算时刻的位置矢量，R为解算后的位置矢量；

进行姿态解算：

$C_n^b=\left(\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)$

$\mathrm{\φ}=\underset{i=1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{\mathrm{\ω}}_i+\underset{i=1}{\overset{\mathrm{mN}-1}{\mathrm{\Σ}}}\underset{j>1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{k}_{\mathrm{ij}}{\mathrm{\ω}}_i\×{\mathrm{\ω}}_j$

其中，为转动矢量矩阵，ψ为载体的航向角，θ均为俯仰角，γ为横滚角，φ为等效转动矢量，ω角速度矢量，i为算法的i阶，j为算法的j阶，m为姿态个数，N为算法的阶数，k_ij为算法系数。

可选地，所述根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波的步骤包括：

根据所述误差数据建立卡尔曼滤波器的状态方程；

根据所述卡尔曼滤波状态方程，建立误差协方差矩阵：

根据所述卡尔曼滤波状态方程构建系统噪声矩阵；

根据所述北斗卫星导航预处理数据及所述解算结果，以及所述状态方程建立量测方程。

可选地，所述卡尔曼滤波器的状态方程为：

X为状态向量，表示东、北、天方向的姿态误差角，δV_e、δV_n、δV_u和δL、δλ、δh分别表示东、北、天方向的速度误差和纬度、经度、高度的位置误差，ε_x、ε_y、ε_z表示沿载体系xyz轴上的角度随机游走系数，表示沿载体系xyz轴上的陀螺仪零偏；

所述误差协方差矩阵为如下的15×15维的矩阵：

$A\left(t\right)=\left[\begin{array}{cccc}{F}_{11}& F_{12}& F_{13}& F_{14}\\ F_{21}& F_{22}& F_{23}& F_{24}\\ 0_{3\×3}& F_{32}& F_{33}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& F_{43}\end{array}\right]$

其中，0表示零矩阵，下标表示该零矩阵是几乘几的矩阵；

F₁₁、F₁₂、F₁₃、F₁₄、F₂₁、F₂₂、F₂₃、F₂₄、F₃₂、F₃₃、F₄₃分别为：

$F_{11}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& 0& -\frac{V_n}{R_m+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}& \frac{V_n}{R_m+h}& 0\end{array}\right]$

其中，L为纬度，h为高度，V_n为导航坐标系下的速度分量；Rn为卯酉面曲率半径，R_m为子午面曲率半径，V_e为地球坐标系下的速度矢量，ω_ie为地球自转角速率；

$F_{12}=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_n+h}& 0& 0\\ \frac{\tan L}{R_n+h}& 0& 0\end{array}\right]$

$F_{13}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}{\sec }^{2}L& 0& 0\end{array}\right]$

$F_{14}=\left[\begin{array}{cccccc}-\mathrm{\α}& & & & & \\ & -\mathrm{\α}& & & 0& \\ & & -\mathrm{\α}& & & \\ & & & -\mathrm{\β}& & \\ & 0& & & -\mathrm{\β}& \\ & & & & & -\mathrm{\β}\end{array}\right]$

其中，α为速度分量方差，β为位置分量方差；

$F_{21}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\\ -f_n& f_e& 0\end{array}\right]$

其中，f_u、f_n、f_e为分别为地理坐标系下天向、北向、东向方向上的比力；

$F_{22}=\left[\begin{array}{ccc}\frac{V_n\tan L}{R_n+h}-\frac{V_n}{R_n+h}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& -\frac{V_n}{R_m+h}& -\frac{V_n}{R_m+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})& \frac{2V_n}{R_m+h}& 0\end{array}\right]$

$F_{23}=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_n+\frac{V_e{V}_n}{R_n+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_n& 0& 0\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_e+\frac{V_e^2}{R_n+h}{\sec }^{2}L)& 0& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_e& 0& 0\end{array}\right]$

$F_{24}=\left[\begin{array}{cc}{C}_n^b& 0_{3\×3}\end{array}\right]$

$F_{32}=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{\sec L}{R_n+h}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{33}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{V_e\sec L\tan L}{R_n+h}& 0& 0\\ 0& 0& 0\end{array}\right]$

$F_{43}=\left[\begin{array}{cc}{0}_{3\×3}& C_n^b\end{array}\right]$

所述系统噪声矩阵如下：

$R={\left[\begin{array}{ccccc}{X}_1{X}_1^H& X_1{X}_2^H& X_1{X}_3^H& \·\·\·& X_1{X}_M^H\\ X_2{X}_1^H& X_2{X}_2^H& {X_{2}X}_3^H& \·\·\·& X_2{X}_M^H\\ X_3{X}_1^H& X_3{X}_2^H& X_3{X}_3^H& \·\·\·& X_3{X}_M^H\\ \·\·\·& \·\·\·& \·\·\·& \·\·\·& \·\·\·\\ X_M{X}_1^H& {X_{M}X}_2^H& X_M{X}_3^H& \·\·\·& X_M{X}_M^H\end{array}\right]}_{\mathrm{MN}\×\mathrm{MN}}$

M为代表速度和位置标准差的6维向量，H表示转置共轭;

所述量测方程如下：

Z₁=H₁X+γ_n

其中，量测噪声γ_n是零均值的白噪声，量测矩阵H₁为：

$\begin{array}{c}{H}_1=\left[\begin{array}{ccc}{V}^n{C}_n^b\left(\mathrm{1,2}\right)-V^n{C}_n^b\left(\mathrm{1,3}\right)& -V^n{C}_n^b\left(\mathrm{1,2}\right)+V^e{C}_n^b\left(\mathrm{1,3}\right)& V^n{C}_n^b\left(\mathrm{1,1}\right)-V^e{C}_n^b\left(\mathrm{1,2}\right)\\ V^n{C}_n^b\left(\mathrm{3,2}\right)-V^n{C}_n^b\left(\mathrm{3,3}\right)& -V^n{C}_n^b\left(\mathrm{3,2}\right)+V^e{C}_n^b\left(\mathrm{3,3}\right)& V^n{C}_n^b\left(\mathrm{3,1}\right)-V^e{C}_n^b\left(\mathrm{3,2}\right)\end{array}\right.\\ {\left.\begin{array}{cccc}{C}_n^b\left(\mathrm{1,1}\right)& C_n^b\left(\mathrm{1,2}\right)& C_n^b\left(\mathrm{1,3}\right)& 0_{1\×9}\\ C_n^b\left(\mathrm{3,1}\right)& C_n^b\left(\mathrm{3,2}\right)& C_n^b\left(\mathrm{3,3}\right)& 0_{1\×9}\end{array}\right]}_{2\×15}\end{array}.$

其中，Vⁿ为导航坐标系下的速度矢量，V^e为地球坐标系下的速度矢量，后括号中的数字是指在矩阵中相应行和列上的元素。

本发明还提供了一种导航终端，包括：

北斗卫星导航模块，用于接收北斗卫星导航信号，接收内部射频和基带芯片数据，经位置、速度、时间解算处理后，得到北斗卫星导航预处理数据；

惯性导航模块，用于获取原始惯性测量数据；对所述原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果；

外部观测模块，用于获取外部观测信息，根据所述外部观测信息及所述原始惯性测量数据计算误差数据；所述外部观测信息包括温度、启动时间；

组合导航模块，用于根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波，输出滤波结果。

可选地，所述原始惯性测量数据包括：陀螺仪原始测量数据、加速度计原始测量数据、磁力计原始测量数据；

所述外部观测模块根据外部观测信息及所述原始惯性测量数据计算误差数据是指：

所述外部观测模块计算各轴的位置误差C1为：

C1=(Z–B)/(1+S)

其中Z为所计算的轴的原始观测数据，B为所计算的轴的标定偏置误差，S为所计算的轴的刻度因子误差；

计算陀螺仪的稳定零偏误差：

W_稳定零偏=a₀+a₁t+a₂T+a₃T²+a₄T³+a₅△T；

其中，t为陀螺仪的启动时间，T为陀螺仪的内部温度，△T为温度变化梯度，a₀为常数项，a₁～a₅为系数；

计算姿态误差角C2为：

C2=A₁×B₁–B₂×A₂

A₁为解算前上一个采样点陀螺仪的原始观测数据，B₁为解算前上一个采样点的加速度计的原始观测数据，A₂为解算时当前采样点陀螺仪的观测数据，B₂为解算时当前采样点的加速度计的观测数据；

计算速度误差C3为：

C3=1/2×（A₂×B₂+B₁×A₁+B₁×A₂）/12.0。

可选地，所述惯性导航模块对原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果是指：

所述惯性导航模块计算解算点的地球曲率半径：

$M=a*(1-e^2)/{(1-e^2*\sin {\left(\mathrm{lat}\right)}^2)}^{\frac{3}{2}}$

其中a为地球长半轴半径，e为地球扁率，lat为捷联导航解算点的纬度；

进行速度解算：

R_d=V_n×0.5×D_t/(R_m+D_r)

V₁=V_n+0.5×D_v

进行位置解算：

W_en=V_n×k(M_v+R_n×D_v)

R=R₁–D_t×D_v

R_d为载体坐标系上四元数表示的投影分量，V_n为当前坐标系下的速度分量，V₁为迭代后的三轴速度分量，D_t是时间历元，D_v为导航系下的速度向量，D_r为地理坐标系下的位置向量，R_m为地球子午线对应的曲率半径；W_en为导航系下的转换速率，R_n为卯酉圈的曲率半径，M_v为差值速度向量，k为转换系数；R₁为上一解算时刻的位置矢量，R为解算后的位置矢量；

进行姿态解算：

$C_n^b=\left(\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)$

$\mathrm{\φ}=\underset{i=1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{\mathrm{\ω}}_i+\underset{i=1}{\overset{\mathrm{mN}-1}{\mathrm{\Σ}}}\underset{j>1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{k}_{\mathrm{ij}}{\mathrm{\ω}}_i\×{\mathrm{\ω}}_j$

其中，为转动矢量矩阵，ψ为载体的航向角，θ均为俯仰角，γ为横滚角，φ为等效转动矢量，ω角速度矢量，i为算法的i阶，j为算法的j阶，m为姿态个数，N为算法的阶数，ki_j为算法系数。

可选地，所述组合导航模块根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波是指：

所述组合导航模块根据所述误差数据建立卡尔曼滤波器的状态方程；根据所述卡尔曼滤波状态方程，建立误差协方差矩阵：根据所述卡尔曼滤波状态方程构建系统噪声矩阵；根据所述北斗卫星导航预处理数据及所述解算结果，以及所述状态方程建立量测方程。

可选地，所述卡尔曼滤波器的状态方程为：

X为状态向量，表示东、北、天方向的姿态误差角，δV_e、δV_n、δV_u和δL、δλ、δh分别表示东、北、天方向的速度误差和纬度、经度、高度的位置误差，ε_x、ε_y、ε_z表示沿载体系xyz轴上的角度随机游走系数，表示沿载体系xyz轴上的陀螺仪零偏；

所述误差协方差矩阵为如下的15×15维的矩阵：

$A\left(t\right)=\left[\begin{array}{cccc}{F}_{11}& F_{12}& F_{13}& F_{14}\\ F_{21}& F_{22}& F_{23}& F_{24}\\ 0_{3\×3}& F_{32}& F_{33}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& F_{43}\end{array}\right]$

其中，0表示零矩阵，下标表示该零矩阵是几乘几的矩阵；

F₁₁、F₁₂、F₁₃、F₁₄、F₂₁、F₂₂、F₂₃、F₂₄、F₃₂、F₃₃、F₄₃分别为：

$F_{11}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& 0& -\frac{V_n}{R_m+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}& \frac{V_n}{R_m+h}& 0\end{array}\right]$

其中，L为纬度，h为高度，V_n为导航坐标系下的速度分量；Rn为卯酉面曲率半径，R_m为子午面曲率半径，V_e为地球坐标系下的速度矢量，ω_ie为地球自转角速率；

$F_{12}=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_n+h}& 0& 0\\ \frac{\tan L}{R_n+h}& 0& 0\end{array}\right]$

$F_{13}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}{\sec }^{2}L& 0& 0\end{array}\right]$

$F_{14}=\left[\begin{array}{cccccc}-\mathrm{\α}& & & & & \\ & -\mathrm{\α}& & & 0& \\ & & -\mathrm{\α}& & & \\ & & & -\mathrm{\β}& & \\ & 0& & & -\mathrm{\β}& \\ & & & & & -\mathrm{\β}\end{array}\right]$

其中，α为速度分量方差，β为位置分量方差；

$F_{21}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\\ -f_n& f_e& 0\end{array}\right]$

其中，f_u、f_n、f_e为分别为地理坐标系下天向、北向、东向方向上的比力；

$F_{22}=\left[\begin{array}{ccc}\frac{V_n\tan L}{R_n+h}-\frac{V_n}{R_n+h}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& -\frac{V_n}{R_m+h}& -\frac{V_n}{R_m+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})& \frac{2V_n}{R_m+h}& 0\end{array}\right]$

$F_{23}=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_n+\frac{V_e{V}_n}{R_n+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_n& 0& 0\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_e+\frac{V_e^2}{R_n+h}{\sec }^{2}L)& 0& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_e& 0& 0\end{array}\right]$

$F_{24}=\left[\begin{array}{cc}{C}_n^b& 0_{3\×3}\end{array}\right]$

$F_{32}=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{\sec L}{R_n+h}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{33}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{V_e\sec L\tan L}{R_n+h}& 0& 0\\ 0& 0& 0\end{array}\right]$

$F_{43}=\left[\begin{array}{cc}{0}_{3\×3}& C_n^b\end{array}\right]$

所述系统噪声矩阵如下：

$R={\left[\begin{array}{ccccc}{X}_1{X}_1^H& X_1{X}_2^H& X_1{X}_3^H& \·\·\·& X_1{X}_M^H\\ X_2{X}_1^H& X_2{X}_2^H& {X_{2}X}_3^H& \·\·\·& X_2{X}_M^H\\ X_3{X}_1^H& X_3{X}_2^H& X_3{X}_3^H& \·\·\·& X_3{X}_M^H\\ \·\·\·& \·\·\·& \·\·\·& \·\·\·& \·\·\·\\ X_M{X}_1^H& {X_{M}X}_2^H& X_M{X}_3^H& \·\·\·& X_M{X}_M^H\end{array}\right]}_{\mathrm{MN}\×\mathrm{MN}}$

M为代表速度和位置标准差的6维向量，H表示转置共轭;

所述量测方程如下：

Z₁=H₁X+γ_n

其中，量测噪声γ_n是零均值的白噪声，量测矩阵H₁为：

$\begin{array}{c}{H}_1=\left[\begin{array}{ccc}{V}^n{C}_n^b\left(\mathrm{1,2}\right)-V^n{C}_n^b\left(\mathrm{1,3}\right)& -V^n{C}_n^b\left(\mathrm{1,2}\right)+V^e{C}_n^b\left(\mathrm{1,3}\right)& V^n{C}_n^b\left(\mathrm{1,1}\right)-V^e{C}_n^b\left(\mathrm{1,2}\right)\\ V^n{C}_n^b\left(\mathrm{3,2}\right)-V^n{C}_n^b\left(\mathrm{3,3}\right)& -V^n{C}_n^b\left(\mathrm{3,2}\right)+V^e{C}_n^b\left(\mathrm{3,3}\right)& V^n{C}_n^b\left(\mathrm{3,1}\right)-V^e{C}_n^b\left(\mathrm{3,2}\right)\end{array}\right.\\ {\left.\begin{array}{cccc}{C}_n^b\left(\mathrm{1,1}\right)& C_n^b\left(\mathrm{1,2}\right)& C_n^b\left(\mathrm{1,3}\right)& 0_{1\×9}\\ C_n^b\left(\mathrm{3,1}\right)& C_n^b\left(\mathrm{3,2}\right)& C_n^b\left(\mathrm{3,3}\right)& 0_{1\×9}\end{array}\right]}_{2\×15}\end{array}.$

其中，Vⁿ为导航坐标系下的速度矢量，V^e为地球坐标系下的速度矢量，后括号中的数字是指在矩阵中相应行和列上的元素。

本发明的一个实施例利用卫星导航系统不随时间积累的定位能力和SINS短时高精度自主导航能力，将北斗卫星导航系统和SINS有机地结合起来，利用两者性能互补的特点，不仅可充分发挥各自的特长，还可以相互弥补存在的不足，具备良好的发展前景和广泛的使用价值。一个优选实施例中，随着结合程度的加深、两者互相辅助，还能带来附加的好处。另一个优选实施例中，通过改进组合导航算法及采用辅助约束信息如零速修正等技术，可以大幅度的提高导航的效果。

附图说明

图1是实施例一的导航方法的流程示意图；

图2是实施例二的导航终端的示意框图。

具体实施方式

下面将结合附图及实施例对本发明的技术方案进行更详细的说明。

需要说明的是，如果不冲突，本发明实施例以及实施例中的各个特征可以相互结合，均在本发明的保护范围之内。另外，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

实施例一、一种导航方法，如图1所示，包括：

接收北斗卫星导航信号，接收内部射频和基带芯片数据，经PVT（位置、速度、时间）解算处理后，得到北斗卫星导航预处理数据；

获取原始惯性测量数据，对所述原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果；

获取外部观测信息，根据所述外部观测信息及所述原始惯性测量数据计算误差数据；所述外部观测信息包括温度、启动时间等；

根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波，输出滤波结果。

本实施例还能够同步输出北斗卫星导航、组合导航两种解算结果，两种导航结果通过组合滤波后能够相互辅助，在北斗信号失锁后，能继续提供全姿态的导航数据，最终提高全天候的导航结果的精度和可靠性。

本实施例的一种实施方式中，所述原始惯性测量数据可以但不限于包括：陀螺仪原始测量数据、加速度计原始测量数据、磁力计原始测量数据。

本实施方式中，根据所述外部观测信息及所述原始惯性测量数据计算误差数据的步骤具体可以包括：

分别对陀螺仪和加速度计各轴的原始测量数据进行数据误差补偿，计算各轴的位置误差C1为：

C1=(Z–B)/(1+S)  (1)

其中Z为所计算的轴的原始观测数据，B为所计算的轴的标定偏置误差，S为所计算的轴的刻度因子误差；B和S可以根据经验值或仪器厂商提供的参数得到；

计算陀螺仪的稳定零偏误差：

W_稳定零偏=a₀+a₁t+a₂T+a₃T²+a₄T³+a₅△T；

其中，t为陀螺仪的启动时间，T为陀螺仪的内部温度，△T为温度变化梯度，a₀为常数项，a₁～a₅为系数；△T、a₀、a₁～a₅可以根据经验或仿真值设定；

本实施方式中，在导航开始后，评估高频IMU原始测量数据的零偏误差、刻度因子误差，角度随机游走系数，并据此补偿高频IMU原始测量数据，可提高原始测量数据输出的精度。MEMS（Micro Electro Mechanicalsystems，微电子机械系统）陀螺仪的零偏误差同时受到多个变量因素影响，温度是其影响因素之一，本实施方式针对MEMS陀螺仪特点，设计了MEMS陀螺的温度区间零偏计算模型，可以据此进行零偏补偿。

本实施方式还可以包括：计算由于圆锥运动导致等效陀螺仪等输出漂移，本实施方式采用圆锥补偿算法来消除圆锥误差的影响，从而提高姿态计算的精度；计算姿态误差角C2为：

C2=A₁×B₁–B₂×A₂   (2)

A₁为解算前上一个采样点陀螺仪的原始观测数据，B₁为解算前上一个采样点的加速度计的原始观测数据，A₂为解算时当前采样点陀螺仪的观测数据，B₂为解算时当前采样点的加速度计的观测数据。

计算由于Sculling Motion（划船运动）导致等效陀螺仪等输出漂移，对划船效应误差进行补偿，这样能有效提高速度更新中比力的转换精度，减少了速度误差；计算速度误差C3为：

C3=1/2×（A₂×B₂+B₁×A₁+B₁×A₂）/12.0   (3)

A₁为解算前上一个采样点陀螺仪的原始观测数据，B₁为解算前上一个采样点的加速度计的原始观测数据，A₂为解算时当前采样点陀螺仪的观测数据，B₂为解算时当前采样点的加速度计的观测数据。

本实施方式中，零偏补偿、数据误差补偿、圆锥误差补偿及划船效应补偿的先后顺序可以不限，也可以按照预定顺序进行。

本实施例的一种实施方式中，所述对观测值采用捷联导航算法进行解算的步骤具体可以包括：

计算解算点的地球曲率半径：

$M=a*(1-e^2)/{(1-e^2*\sin {\left(\mathrm{lat}\right)}^2)}^{\frac{3}{2}}---\left(4\right)$

其中a为地球长半轴半径，e为地球扁率，lat为捷联导航解算点的纬度。

进行速度解算，计算旋转矢量的变化值，即经度、纬度、高度的变化量测值，并转化为导航坐标系到载体坐标系上四元数表示的投影分量值。通过对投影分量值的时间历元处理转化为旋转矢量上投影分量，最后利用上一次迭代的速度和当前观测加速度的积分值，求出当前时刻的导航坐标系统的速度分量，主要解算公式如下：

R_d=V_n×0.5×D_t/(R_m+D_r)   (5)

V₁=V_n+0.5×D_v   (6)

其中R_d为载体坐标系上四元数表示的投影分量，V_n为当前坐标系下的速度分量，V₁为迭代后的三轴速度分量，D_t是时间历元，D_v为导航系下的速度向量，D_r为地理坐标系下的位置向量，R_m为地球子午线对应的曲率半径。

进行位置解算，主要解算公式如下：

W_en=V_n×k(M_v+R_n×D_v)   (7)

R=R₁–D_t×D_v   (8)

其中W_en为导航系下的转换速率，V_n为当前坐标系下的速度分量，R_n为卯酉圈的曲率半径，M_v为差值速度向量，k为转换系数，可以根据经验值或实验值确定；R₁为上一解算时刻的位置矢量，R为解算后的位置矢量。

采用等效转动矢量法辅助姿态算法，进行姿态解算，姿态解算公式以及等效转动矢量最终结果公式如下所示：

$C_n^b=\left(\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)---\left(9\right)$

$\mathrm{\φ}=\underset{i=1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{\mathrm{\ω}}_i+\underset{i=1}{\overset{\mathrm{mN}-1}{\mathrm{\Σ}}}\underset{j>1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{k}_{\mathrm{ij}}{\mathrm{\ω}}_i\×{\mathrm{\ω}}_j---\left(10\right)$

（9）式中，为转动矢量矩阵，ψ为载体的航向角，θ均为俯仰角，γ为横滚角，（10）式中，φ为等效转动矢量，ω角速度矢量，i为算法的i阶，j为算法的j阶，m为姿态个数，N为算法的阶数，k_ij为算法系数。

等效转动矢量的迭代计算采用较高的频率，而用了等效转动矢量后的方向余弦矩阵计算则用较低迭代频率，两种方式的结合构成了捷联矩阵的计算。

本实施例的一种实施方式中，所述PVT解算处理的步骤具体可以包括：

Q=PQ+DM+KF+S_n+F+J+S_a+C+RD+H_dop+C_n0   （11）

方程中：Q表示组合算法估计出的定位质量系数，其中PQ表示定位质量，DM表示定位模式，KF表示解算模式质量因子，S_n表示参与解算的卫星数，F表示错误模式，J表示判停模式，C表示载体速度，S_a表示方位角，H_dop表示水平定位精度，C_n0表示灵敏度质量因子，以上参数都可以从标准的PVT解算结果中获取。

北斗卫星导航信号PVT解算的质量对系统误差影响很大，本实施方式针对北斗卫星导航的特点设计了PVT解算质量估计模型，通过模型可以有效的提高PVT解算的数据质量，提高组合导航的精度。

经过式（11）所示的PVT解算质量估计模型得到的定位质量系数将作为组合导航滤波时的量测误差量之一。

本实施例的一种实施方式中，所述根据北斗卫星导航预处理数据、所述误差数据和解算结果进行组合导航滤波的步骤具体可以包括：

根据所述误差数据建立卡尔曼滤波器的状态方程，具体地，选取式（1）、（4）、（2）计算的位置误差、速度误差、姿态误差角、以及陀螺仪角度随机游走系数和零偏作为状态量。

式中，X为状态向量，表示东、北、天方向的姿态误差角，δV_e、δV_n、δV_u和δL、δλ、δh分别表示东、北、天方向的速度误差和纬度、经度、高度的位置误差，ε_x、ε_y、ε_z表示沿载体系xyz轴上的角度随机游走系数，表示沿载体系xyz轴上的陀螺仪零偏。

根据上述卡尔曼滤波状态方程，建立误差协方差矩阵A(t)，A(t)为15×15维的矩阵；A(t)表示为如下形式：

$A\left(t\right)=\left[\begin{array}{cccc}{F}_{11}& F_{12}& F_{13}& F_{14}\\ F_{21}& F_{22}& F_{23}& F_{24}\\ 0_{3\×3}& F_{32}& F_{33}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& F_{43}\end{array}\right]---\left(14\right)$

其中，0表示零矩阵，下标表示该零矩阵是几乘几的矩阵；

F₁₁、F₁₂、F₁₃、F₁₄、F₂₁、F₂₂、F₂₃、F₂₄、F₃₂、F₃₃、F₄₃分别为：

$F_{11}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& 0& -\frac{V_n}{R_m+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}& \frac{V_n}{R_m+h}& 0\end{array}\right]---\left(15\right)$

其中，L为纬度，h为高度，V_n为导航坐标系下的速度分量；Rn为卯酉面曲率半径，R_m为子午面曲率半径，V_e为地球坐标系下的速度矢量，ω_ie为地球自转角速率。

$F_{12}=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_n+h}& 0& 0\\ \frac{\tan L}{R_n+h}& 0& 0\end{array}\right]---\left(16\right)$

$F_{13}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}{\sec }^{2}L& 0& 0\end{array}\right]---\left(17\right)$

$F_{14}=\left[\begin{array}{cccccc}-\mathrm{\α}& & & & & \\ & -\mathrm{\α}& & & 0& \\ & & -\mathrm{\α}& & & \\ & & & -\mathrm{\β}& & \\ & 0& & & -\mathrm{\β}& \\ & & & & & -\mathrm{\β}\end{array}\right]---\left(18\right)$

其中，α为速度分量方差，β为位置分量方差。

$F_{21}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\\ -f_n& f_e& 0\end{array}\right]---\left(19\right)$

其中，f_u、f_n、f_e为分别为地理坐标系下天向、北向、东向方向上的比力。

$F_{22}=\left[\begin{array}{ccc}\frac{V_n\tan L}{R_n+h}-\frac{V_n}{R_n+h}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& -\frac{V_n}{R_m+h}& -\frac{V_n}{R_m+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})& \frac{2V_n}{R_m+h}& 0\end{array}\right]---\left(20\right)$

$F_{23}=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_n+\frac{V_e{V}_n}{R_n+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_n& 0& 0\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_e+\frac{V_e^2}{R_n+h}{\sec }^{2}L)& 0& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_e& 0& 0\end{array}\right]---\left(21\right)$

$F_{24}=\left[\begin{array}{cc}{C}_n^b& 0_{3\×3}\end{array}\right]---\left(22\right)$

其中，为式（9）中求得的。

$F_{32}=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{\sec L}{R_n+h}& 0& 0\\ 0& 0& 1\end{array}\right]---\left(23\right)$

$F_{33}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{V_e\sec L\tan L}{R_n+h}& 0& 0\\ 0& 0& 0\end{array}\right]---\left(24\right)$

$F_{43}=\left[\begin{array}{cc}{0}_{3\×3}& C_n^b\end{array}\right]---\left(25\right)$

进一步的构建系统噪声矩阵R，对于北斗（BD）量测卡尔曼更新，R是MN×MN维的矩阵其表示形式速度和位置标准差的对角阵如下：

$R={\left[\begin{array}{ccccc}{X}_1{X}_1^H& X_1{X}_2^H& X_1{X}_3^H& \·\·\·& X_1{X}_M^H\\ X_2{X}_1^H& X_2{X}_2^H& {X_{2}X}_3^H& \·\·\·& X_2{X}_M^H\\ X_3{X}_1^H& X_3{X}_2^H& X_3{X}_3^H& \·\·\·& X_3{X}_M^H\\ \·\·\·& \·\·\·& \·\·\·& \·\·\·& \·\·\·\\ X_M{X}_1^H& {X_{M}X}_2^H& X_M{X}_3^H& \·\·\·& X_M{X}_M^H\end{array}\right]}_{\mathrm{MN}\×\mathrm{MN}}---\left(26\right)$

其中，X为式（12）中得到的X；M为代表速度和位置标准差的6维向量，N和式（10）的N含义相同；H表示转置共轭。

根据所述北斗卫星导航预处理数据及所述解算结果，以及所述状态方程建立量测方程，具体地，取北斗PVT解算位置信息与SINS捷联解算位置信息的差值以及北斗PVT解算速度信息与SINS捷联解算的速度信息的差值作为观测量值，量测方程公式如下：

Z(t)=H(t)X(t)+w(t)   （27）

X(t)为式（12）求得的状态向量，ω（t）为系统过程白噪声，H为观测矩阵。

H(t)为观测矩阵定义如式（28）。

$0H\left(t\right)=\left[\begin{array}{ccccccccccccccc}0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(28\right)$

建立非完整性约束组合导航滤波方程，加入载体约束条件，在不增加硬件的基础上提高导航精度、重新建立量测方程，选取两轴上的速度直接误差作为外部观测量Z₁，

$Z_1={\left[\begin{array}{cc}\mathrm{\δ}{V}_x^b& \mathrm{\δ}{V}_z^b\end{array}\right]}^T---\left(29\right)$

其中，为载体系上迭代计算后的x轴与z轴的速度误差。

根据速度坐标转换得到量测方程如下：

$Z_1=\left[\begin{array}{c}\mathrm{\δ}{V}_x^b\\ \mathrm{\δ}{V}_z^b\end{array}\right]=H_{1}X+{\mathrm{\γ}}_n---\left(30\right)$

其中，量测噪声γ_n是零均值的白噪声，量测矩阵H为：

$\begin{array}{c}{H}_1=\left[\begin{array}{ccc}{V}^n{C}_n^b\left(\mathrm{1,2}\right)-V^n{C}_n^b\left(\mathrm{1,3}\right)& -V^n{C}_n^b\left(\mathrm{1,2}\right)+V^e{C}_n^b\left(\mathrm{1,3}\right)& V^n{C}_n^b\left(\mathrm{1,1}\right)-V^e{C}_n^b\left(\mathrm{1,2}\right)\\ V^n{C}_n^b\left(\mathrm{3,2}\right)-V^n{C}_n^b\left(\mathrm{3,3}\right)& -V^n{C}_n^b\left(\mathrm{3,2}\right)+V^e{C}_n^b\left(\mathrm{3,3}\right)& V^n{C}_n^b\left(\mathrm{3,1}\right)-V^e{C}_n^b\left(\mathrm{3,2}\right)\end{array}\right.\\ {\left.\begin{array}{cccc}{C}_n^b\left(\mathrm{1,1}\right)& C_n^b\left(\mathrm{1,2}\right)& C_n^b\left(\mathrm{1,3}\right)& 0_{1\×9}\\ C_n^b\left(\mathrm{3,1}\right)& C_n^b\left(\mathrm{3,2}\right)& C_n^b\left(\mathrm{3,3}\right)& 0_{1\×9}\end{array}\right]}_{2\×15}\end{array}---\left(31\right)$

其中，Vⁿ为导航坐标系下的速度矢量，V^e为地球坐标系下的速度矢量，后括号中的数字是指在矩阵中相应行和列上的元素，比如（1,2）就是指中第1行第2列的元素；其它可以类推。

最后可以根据组合导航卡尔曼滤波的结果反馈给SINS系统做位置、速度、姿态信息的修正。

实施例二、一种导航终端，如图2所示，包括以下模块：

北斗卫星导航模块，用于接收北斗卫星导航信号，接收内部射频和基带芯片数据，经PVT解算处理后，得到北斗卫星导航预处理数据；

惯性导航模块，用于获取原始惯性测量数据；对所述原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果；

外部观测模块，用于获取外部观测信息，根据所述外部观测信息及所述原始惯性测量数据计算误差数据；所述外部观测信息包括温度、启动时间；

组合导航模块，用于根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波，输出滤波结果。

本实施例中，所述导航终端还可以包括：

低压差稳压源，提供稳定输入的电源；放置位置应远离惯测量性器件和磁强计以最大程度上减小热量散发对器件造成零偏影响；

数据I/O（输入/输出）接口，采用高速I²C硬件接口，用于实时输出所述滤波结果给外部模块；还可以另外输出所述北斗卫星导航预处理数据、解算结果等；

存储芯片，可以但不限于为EEPROM。

本实施例中，所述外部观测模块可以但不限于包括：

温度传感器模块，用于提供计算误差数据所需的温度。

时间等其它外部观测数据可以直接从相应仪器或模块中读取，或预先输入/保存在外部观测模块中。

本实施例的一种实施方式中，所述惯性导航模块中除了用于采用捷联惯性导航算法进行解算的处理单元之外，还可以包括测量单元；所述测量单元可以但不限于包括三轴陀螺仪、三轴加速度计、三轴磁力计。

相应地，所述原始惯性测量数据包括：陀螺仪原始测量数据、加速度计原始测量数据、磁力计原始测量数据；

所述外部观测模块根据外部观测信息及所述原始惯性测量数据计算误差数据具体可以是指：

所述外部观测模块计算各轴的位置误差C1为：

C1=(Z–B)/(1+S)

其中Z为所计算的轴的原始观测数据，B为所计算的轴的标定偏置误差，S为所计算的轴的刻度因子误差；

计算陀螺仪的稳定零偏误差：

W_稳定零偏=a₀+a₁t+a₂T+a₃T²+a₄T³+a₅△T；

其中，t为陀螺仪的启动时间，T为陀螺仪的内部温度，△T为温度变化梯度，a₀为常数项，a₁～a₅为系数；

计算姿态误差角C2为：

C2=A₁×B₁–B₂×A₂

A₁为解算前上一个采样点陀螺仪的原始观测数据，B₁为解算前上一个采样点的加速度计的原始观测数据，A₂为解算时当前采样点陀螺仪的观测数据，B₂为解算时当前采样点的加速度计的观测数据；

计算速度误差C3为：

C3=1/2×（A₂×B₂+B₁×A₁+B₁×A₂）/12.0。

本实施例的一种实施方式中，所述惯性导航模块对原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果具体可以是指：

所述惯性导航模块计算解算点的地球曲率半径：

$M=a*(1-e^2)/{(1-e^2*\sin {\left(\mathrm{lat}\right)}^2)}^{\frac{3}{2}}$

其中a为地球长半轴半径，e为地球扁率，lat为捷联导航解算点的纬度；

进行速度解算：

R_d=V_n×0.5×D_t/(R_m+D_r)

V₁=V_n+0.5×D_v

进行位置解算：

W_en=V_n×k(M_v+R_n×D_v)

R=R₁–D_t×D_v

R_d为载体坐标系上四元数表示的投影分量，V_n为当前坐标系下的速度分量，V₁为迭代后的三轴速度分量，D_t是时间历元，D_v为导航系下的速度向量，D_r为地理坐标系下的位置向量，R_m为地球子午线对应的曲率半径；W_en为导航系下的转换速率，R_n为卯酉圈的曲率半径，M_v为差值速度向量，k为转换系数；R₁为上一解算时刻的位置矢量，R为解算后的位置矢量；

进行姿态解算：

$C_n^b=\left(\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)$

$\mathrm{\φ}=\underset{i=1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{\mathrm{\ω}}_i+\underset{i=1}{\overset{\mathrm{mN}-1}{\mathrm{\Σ}}}\underset{j>1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{k}_{\mathrm{ij}}{\mathrm{\ω}}_i\×{\mathrm{\ω}}_j$

其中，为转动矢量矩阵，ψ为载体的航向角，θ均为俯仰角，γ为横滚角，φ为等效转动矢量，ω角速度矢量，i为算法的i阶，j为算法的j阶，m为姿态个数，N为算法的阶数，k_ij为算法系数。

本实施例的一种实施方式中，所述组合导航模块根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波具体可以是指：

所述组合导航模块根据所述误差数据建立卡尔曼滤波器的状态方程；根据所述卡尔曼滤波状态方程，建立误差协方差矩阵：根据所述卡尔曼滤波状态方程构建系统噪声矩阵；根据所述北斗卫星导航预处理数据及所述解算结果，以及所述状态方程建立量测方程。

该实施方式中，所述卡尔曼滤波器的状态方程可以为：

其中，X为状态向量，表示东、北、天方向的姿态误差角，δV_e、δV_n、δV_u和δL、δλ、δh分别表示东、北、天方向的速度误差和纬度、经度、高度的位置误差，ε_x、ε_y、ε_z表示沿载体系xyz轴上的角度随机游走系数，表示沿载体系xyz轴上的陀螺仪零偏。

所述误差协方差矩阵为如下的15×15维的矩阵：

$A\left(t\right)=\left[\begin{array}{cccc}{F}_{11}& F_{12}& F_{13}& F_{14}\\ F_{21}& F_{22}& F_{23}& F_{24}\\ 0_{3\×3}& F_{32}& F_{33}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& F_{43}\end{array}\right]$

其中，0表示零矩阵，下标表示该零矩阵是几乘几的矩阵；

F₁₁、F₁₂、F₁₃、F₁₄、F₂₁、F₂₂、F₂₃、F₂₄、F₃₂、F₃₃、F₄₃分别为：

$F_{11}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& 0& -\frac{V_n}{R_m+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}& \frac{V_n}{R_m+h}& 0\end{array}\right]$

其中，L为纬度，h为高度，V_n为导航坐标系下的速度分量；Rn为卯酉面曲率半径，R_m为子午面曲率半径，V_e为地球坐标系下的速度矢量，ω_ie为地球自转角速率；

$F_{12}=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_n+h}& 0& 0\\ \frac{\tan L}{R_n+h}& 0& 0\end{array}\right]$

$F_{13}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}{\sec }^{2}L& 0& 0\end{array}\right]$

$F_{14}=\left[\begin{array}{cccccc}-\mathrm{\α}& & & & & \\ & -\mathrm{\α}& & & 0& \\ & & -\mathrm{\α}& & & \\ & & & -\mathrm{\β}& & \\ & 0& & & -\mathrm{\β}& \\ & & & & & -\mathrm{\β}\end{array}\right]$

其中，α为速度分量方差，β为位置分量方差；

$F_{21}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\\ -f_n& f_e& 0\end{array}\right]$

其中，f_u、f_n、f_e为分别为地理坐标系下天向、北向、东向方向上的比力；

$F_{22}=\left[\begin{array}{ccc}\frac{V_n\tan L}{R_n+h}-\frac{V_n}{R_n+h}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& -\frac{V_n}{R_m+h}& -\frac{V_n}{R_m+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})& \frac{2V_n}{R_m+h}& 0\end{array}\right]$

$F_{23}=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_n+\frac{V_e{V}_n}{R_n+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_n& 0& 0\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_e+\frac{V_e^2}{R_n+h}{\sec }^{2}L)& 0& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_e& 0& 0\end{array}\right]$

$F_{24}=\left[\begin{array}{cc}{C}_n^b& 0_{3\×3}\end{array}\right]$

$F_{32}=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{\sec L}{R_n+h}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{33}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{V_e\sec L\tan L}{R_n+h}& 0& 0\\ 0& 0& 0\end{array}\right]$

$F_{43}=\left[\begin{array}{cc}{0}_{3\×3}& C_n^b\end{array}\right]$

所述系统噪声矩阵可以如下：

$R={\left[\begin{array}{ccccc}{X}_1{X}_1^H& X_1{X}_2^H& X_1{X}_3^H& \·\·\·& X_1{X}_M^H\\ X_2{X}_1^H& X_2{X}_2^H& {X_{2}X}_3^H& \·\·\·& X_2{X}_M^H\\ X_3{X}_1^H& X_3{X}_2^H& X_3{X}_3^H& \·\·\·& X_3{X}_M^H\\ \·\·\·& \·\·\·& \·\·\·& \·\·\·& \·\·\·\\ X_M{X}_1^H& {X_{M}X}_2^H& X_M{X}_3^H& \·\·\·& X_M{X}_M^H\end{array}\right]}_{\mathrm{MN}\×\mathrm{MN}}$

M为代表速度和位置标准差的6维向量，H表示转置共轭;

所述量测方程可以如下：

Z₁=H₁X+γ_n

其中，量测噪声γ_n是零均值的白噪声，量测矩阵H₁为：

$\begin{array}{c}{H}_1=\left[\begin{array}{ccc}{V}^n{C}_n^b\left(\mathrm{1,2}\right)-V^n{C}_n^b\left(\mathrm{1,3}\right)& -V^n{C}_n^b\left(\mathrm{1,2}\right)+V^e{C}_n^b\left(\mathrm{1,3}\right)& V^n{C}_n^b\left(\mathrm{1,1}\right)-V^e{C}_n^b\left(\mathrm{1,2}\right)\\ V^n{C}_n^b\left(\mathrm{3,2}\right)-V^n{C}_n^b\left(\mathrm{3,3}\right)& -V^n{C}_n^b\left(\mathrm{3,2}\right)+V^e{C}_n^b\left(\mathrm{3,3}\right)& V^n{C}_n^b\left(\mathrm{3,1}\right)-V^e{C}_n^b\left(\mathrm{3,2}\right)\end{array}\right.\\ {\left.\begin{array}{cccc}{C}_n^b\left(\mathrm{1,1}\right)& C_n^b\left(\mathrm{1,2}\right)& C_n^b\left(\mathrm{1,3}\right)& 0_{1\×9}\\ C_n^b\left(\mathrm{3,1}\right)& C_n^b\left(\mathrm{3,2}\right)& C_n^b\left(\mathrm{3,3}\right)& 0_{1\×9}\end{array}\right]}_{2\×15}\end{array}.$

其中，Vⁿ为导航坐标系下的速度矢量，V^e为地球坐标系下的速度矢量，后括号中的数字是指在矩阵中相应行和列上的元素。

当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明的权利要求的保护范围。

Claims (10)

1.一种导航方法，包括：

接收北斗卫星导航信号，接收内部射频和基带芯片数据，经位置、速度、时间解算处理后，得到北斗卫星导航预处理数据；

获取外部观测信息及原始惯性测量数据；根据所述外部观测信息及所述原始惯性测量数据计算误差数据；所述外部观测信息包括温度、启动时间；

对所述原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果；

根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波，输出滤波结果。

2.如权利要求1所述的导航方法，其特征在于：

所述原始惯性测量数据包括：陀螺仪原始测量数据、加速度计原始测量数据、磁力计原始测量数据；

根据外部观测信息及所述原始惯性测量数据计算误差数据的步骤包括：

计算各轴的位置误差C1为：

C1=(Z–B)/(1+S)

其中Z为所计算的轴的原始观测数据，B为所计算的轴的标定偏置误差，S为所计算的轴的刻度因子误差；

计算陀螺仪的稳定零偏误差：

W_稳定零偏a₀+a₁t+a₂T+a₃T²+a₄T³+a₅ΔT；

其中，t为陀螺仪的启动时间，T为陀螺仪的内部温度，ΔT为温度变化梯度，a₀为常数项，a₁～a₅为系数；

计算姿态误差角C2为：

C2=A₁×B₁–B₂×A₂

A₁为解算前上一个采样点陀螺仪的原始观测数据，B₁为解算前上一个采样点的加速度计的原始观测数据，A₂为解算时当前采样点陀螺仪的观测数据，B₂为解算时当前采样点的加速度计的观测数据；

计算速度误差C3为：

C3=1/2×（A₂×B₂+B1×A₁+B₁×A₂）/12.0。

3.如权利要求1或2所述的方法，其特征在于，对所述原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果的步骤包括：

计算解算点的地球曲率半径：

$M=a*(1-e^2)/{(1-e^2*\sin {\left(\mathrm{lat}\right)}^2)}^{\frac{3}{2}}$

其中a为地球长半轴半径，e为地球扁率，lat为捷联导航解算点的纬度；

进行速度解算：

R_d=V_n×0.5×D_t/(R_m+D_r)

V₁=V_n+0.5×D_v

进行位置解算：

W_en=V_n×k(M_v+R_n×D_v)

R=R₁–D_t×D_v

R_d为载体坐标系上四元数表示的投影分量，V_n为当前坐标系下的速度分量，V₁为迭代后的三轴速度分量，D_t是时间历元，D_v为导航系下的速度向量，D_r为地理坐标系下的位置向量，R_m为地球子午线对应的曲率半径；W_en为导航系下的转换速率，R_n为卯酉圈的曲率半径，M_v为差值速度向量，k为转换系数；R₁为上一解算时刻的位置矢量，R为解算后的位置矢量；

进行姿态解算：

$C_n^b=\left(\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)$

$\mathrm{\φ}=\underset{i=1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{\mathrm{\ω}}_i+\underset{i=1}{\overset{\mathrm{mN}-1}{\mathrm{\Σ}}}\underset{j>1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{k}_{\mathrm{ij}}{\mathrm{\ω}}_i\×{\mathrm{\ω}}_j$

其中，为转动矢量矩阵，ψ为载体的航向角，θ均为俯仰角，γ为横滚角，I为等效转动矢量，ω角速度矢量，i为算法的i阶，j为算法的j阶，m为姿态个数，N为算法的阶数，k_ij为算法系数。

4.如权利要求3所述的方法，其特征在于，所述根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波的步骤包括：

根据所述误差数据建立卡尔曼滤波器的状态方程；

根据所述卡尔曼滤波状态方程，建立误差协方差矩阵：

根据所述卡尔曼滤波状态方程构建系统噪声矩阵；

根据所述北斗卫星导航预处理数据及所述解算结果，以及所述状态方程建立量测方程。

5.如权利要求4所述的方法，其特征在于：

所述卡尔曼滤波器的状态方程为：

X为状态向量，表示东、北、天方向的姿态误差角，δV_e、δV_n、δV_u和δL、δλ、δh分别表示东、北、天方向的速度误差和纬度、经度、高度的位置误差，ε_x、ε_y、ε_z表示沿载体系xyz轴上的角度随机游走系数，表示沿载体系xyz轴上的陀螺仪零偏；

所述误差协方差矩阵为如下的15×15维的矩阵：

$A\left(t\right)=\left[\begin{array}{cccc}{F}_{11}& F_{12}& F_{13}& F_{14}\\ F_{21}& F_{22}& F_{23}& F_{24}\\ 0_{3\×3}& F_{32}& F_{33}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& F_{43}\end{array}\right]$

其中，0表示零矩阵，下标表示该零矩阵是几乘几的矩阵；

F₁₁、F₁₂、F₁₃、F₁₄、F₂₁、F₂₂、F₂₃、F₂₄、F₃₂、F₃₃、F₄₃分别为：

$F_{11}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& 0& -\frac{V_n}{R_m+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}& \frac{V_n}{R_m+h}& 0\end{array}\right]$

其中，L为纬度，h为高度，V_n为导航坐标系下的速度分量；Rn为卯酉面曲率半径，R_m为子午面曲率半径，V_e为地球坐标系下的速度矢量，ω_ie为地球自转角速率；

$F_{12}=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_n+h}& 0& 0\\ \frac{\tan L}{R_n+h}& 0& 0\end{array}\right]$

$F_{13}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}{\sec }^{2}L& 0& 0\end{array}\right]$

$F_{14}=\left[\begin{array}{cccccc}-\mathrm{\α}& & & & & \\ & -\mathrm{\α}& & & 0& \\ & & -\mathrm{\α}& & & \\ & & & -\mathrm{\β}& & \\ & 0& & & -\mathrm{\β}& \\ & & & & & -\mathrm{\β}\end{array}\right]$

其中，α为速度分量方差，β为位置分量方差；

$F_{21}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\\ -f_n& f_e& 0\end{array}\right]$

其中，f_u、f_n、f_e为分别为地理坐标系下天向、北向、东向方向上的比力；

$F_{22}=\left[\begin{array}{ccc}\frac{V_n\tan L}{R_n+h}-\frac{V_n}{R_n+h}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& -\frac{V_n}{R_m+h}& -\frac{V_n}{R_m+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})& \frac{2V_n}{R_m+h}& 0\end{array}\right]$

$F_{23}=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_n+\frac{V_e{V}_n}{R_n+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_n& 0& 0\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_e+\frac{V_e^2}{R_n+h}{\sec }^{2}L)& 0& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_e& 0& 0\end{array}\right]$

$F_{24}=\left[\begin{array}{cc}{C}_n^b& 0_{3\×3}\end{array}\right]$

$F_{32}=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{\sec L}{R_n+h}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{33}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{V_e\sec L\tan L}{R_n+h}& 0& 0\\ 0& 0& 0\end{array}\right]$

$F_{43}=\left[\begin{array}{cc}{0}_{3\×3}& C_n^b\end{array}\right]$

所述系统噪声矩阵如下：

$R={\left[\begin{array}{ccccc}{X}_1{X}_1^H& X_1{X}_2^H& X_1{X}_3^H& \·\·\·& X_1{X}_M^H\\ X_2{X}_1^H& X_2{X}_2^H& {X_{2}X}_3^H& \·\·\·& X_2{X}_M^H\\ X_3{X}_1^H& X_3{X}_2^H& X_3{X}_3^H& \·\·\·& X_3{X}_M^H\\ \·\·\·& \·\·\·& \·\·\·& \·\·\·& \·\·\·\\ X_M{X}_1^H& {X_{M}X}_2^H& X_M{X}_3^H& \·\·\·& X_M{X}_M^H\end{array}\right]}_{\mathrm{MN}\×\mathrm{MN}}$

M为代表速度和位置标准差的6维向量，H表示转置共轭;

所述量测方程如下：

Z₁＝H₁X+γ_n

其中，量测噪声γ_n是零均值的白噪声，量测矩阵H₁为：

$\begin{array}{c}{H}_1=\left[\begin{array}{ccc}{V}^n{C}_n^b\left(\mathrm{1,2}\right)-V^n{C}_n^b\left(\mathrm{1,3}\right)& -V^n{C}_n^b\left(\mathrm{1,2}\right)+V^e{C}_n^b\left(\mathrm{1,3}\right)& V^n{C}_n^b\left(\mathrm{1,1}\right)-V^e{C}_n^b\left(\mathrm{1,2}\right)\\ V^n{C}_n^b\left(\mathrm{3,2}\right)-V^n{C}_n^b\left(\mathrm{3,3}\right)& -V^n{C}_n^b\left(\mathrm{3,2}\right)+V^e{C}_n^b\left(\mathrm{3,3}\right)& V^n{C}_n^b\left(\mathrm{3,1}\right)-V^e{C}_n^b\left(\mathrm{3,2}\right)\end{array}\right.\\ {\left.\begin{array}{cccc}{C}_n^b\left(\mathrm{1,1}\right)& C_n^b\left(\mathrm{1,2}\right)& C_n^b\left(\mathrm{1,3}\right)& 0_{1\×9}\\ C_n^b\left(\mathrm{3,1}\right)& C_n^b\left(\mathrm{3,2}\right)& C_n^b\left(\mathrm{3,3}\right)& 0_{1\×9}\end{array}\right]}_{2\×15}\end{array}.$

其中，Vⁿ为导航坐标系下的速度矢量，V^e为地球坐标系下的速度矢量，后括号中的数字是指在矩阵中相应行和列上的元素。

6.一种导航终端，其特征在于，包括：

北斗卫星导航模块，用于接收北斗卫星导航信号，接收内部射频和基带芯片数据，经位置、速度、时间解算处理后，得到北斗卫星导航预处理数据；

惯性导航模块，用于获取原始惯性测量数据；对所述原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果；

外部观测模块，用于获取外部观测信息，根据所述外部观测信息及所述原始惯性测量数据计算误差数据；所述外部观测信息包括温度、启动时间；

组合导航模块，用于根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波，输出滤波结果。

7.如权利要求6所述的导航终端，其特征在于：

所述原始惯性测量数据包括：陀螺仪原始测量数据、加速度计原始测量数据、磁力计原始测量数据；

所述外部观测模块根据外部观测信息及所述原始惯性测量数据计算误差数据是指：

所述外部观测模块计算各轴的位置误差C1为：

C1=(Z–B)/(1+S)

其中Z为所计算的轴的原始观测数据，B为所计算的轴的标定偏置误差，S为所计算的轴的刻度因子误差；

计算陀螺仪的稳定零偏误差：

W_稳定零偏a₀+a₁t+a₂T+a₃T²+a₄T³+a₅ΔT；

其中，t为陀螺仪的启动时间，T为陀螺仪的内部温度，'T为温度变化梯度，a₀为常数项，a₁～a₅为系数；

计算姿态误差角C2为：

C2=A₁×B₁–B₂×A₂

A₁为解算前上一个采样点陀螺仪的原始观测数据，B₁为解算前上一个采样点的加速度计的原始观测数据，A₂为解算时当前采样点陀螺仪的观测数据，B₂为解算时当前采样点的加速度计的观测数据；

计算速度误差C3为：

C3=1/2×（A₂×B₂+B₁×A₁+B₁×A₂）/12.0。

8.如权利要求6或7所述的导航终端，其特征在于，所述惯性导航模块对原始惯性测量数据采用捷联惯性导航算法进行解算，得到解算结果是指：

所述惯性导航模块计算解算点的地球曲率半径：

$M=a*(1-e^2)/{(1-e^2*\sin {\left(\mathrm{lat}\right)}^2)}^{\frac{3}{2}}$

其中a为地球长半轴半径，e为地球扁率，lat为捷联导航解算点的纬度；

进行速度解算：

R_d=V_n×0.5×D_t/(R_m+D_r)

V₁=V_n+0.5×D_v

进行位置解算：

W_en=V_n×k(M_v+R_n×D_v)

R=R₁–D_t×D_v

R_d为载体坐标系上四元数表示的投影分量，V_n为当前坐标系下的速度分量，V₁为迭代后的三轴速度分量，D_t是时间历元，D_v为导航系下的速度向量，D_r为地理坐标系下的位置向量，R_m为地球子午线对应的曲率半径；W_en为导航系下的转换速率，R_n为卯酉圈的曲率半径，M_v为差值速度向量，k为转换系数；R₁为上一解算时刻的位置矢量，R为解算后的位置矢量；

进行姿态解算：

$C_n^b=\left(\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)$

$\mathrm{\φ}=\underset{i=1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{\mathrm{\ω}}_i+\underset{i=1}{\overset{\mathrm{mN}-1}{\mathrm{\Σ}}}\underset{j>1}{\overset{\mathrm{mN}}{\mathrm{\Σ}}}{k}_{\mathrm{ij}}{\mathrm{\ω}}_i\×{\mathrm{\ω}}_j$

其中，为转动矢量矩阵，ψ为载体的航向角，θ均为俯仰角，γ为横滚角，I为等效转动矢量，ω角速度矢量，i为算法的i阶，j为算法的j阶，m为姿态个数，N为算法的阶数，k_ij为算法系数。

9.如权利要求8所述的导航终端，其特征在于，所述组合导航模块根据北斗卫星导航预处理数据、所述误差数据和所述解算结果进行组合导航滤波是指：

所述组合导航模块根据所述误差数据建立卡尔曼滤波器的状态方程；根据所述卡尔曼滤波状态方程，建立误差协方差矩阵：根据所述卡尔曼滤波状态方程构建系统噪声矩阵；根据所述北斗卫星导航预处理数据及所述解算结果，以及所述状态方程建立量测方程。

10.如权利要求9所述的导航终端，其特征在于：

所述卡尔曼滤波器的状态方程为：

X为状态向量，表示东、北、天方向的姿态误差角，δV_e、δV_n、δV_u和δL、δλ、δh分别表示东、北、天方向的速度误差和纬度、经度、高度的位置误差，ε_x、ε_y、ε_z表示沿载体系xyz轴上的角度随机游走系数，表示沿载体系xyz轴上的陀螺仪零偏；

所述误差协方差矩阵为如下的15×15维的矩阵：

$A\left(t\right)=\left[\begin{array}{cccc}{F}_{11}& F_{12}& F_{13}& F_{14}\\ F_{21}& F_{22}& F_{23}& F_{24}\\ 0_{3\×3}& F_{32}& F_{33}& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& F_{43}\end{array}\right]$

其中，0表示零矩阵，下标表示该零矩阵是几乘几的矩阵；

F₁₁、F₁₂、F₁₃、F₁₄、F₂₁、F₂₂、F₂₃、F₂₄、F₃₂、F₃₃、F₄₃分别为：

$F_{11}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& 0& -\frac{V_n}{R_m+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}& \frac{V_n}{R_m+h}& 0\end{array}\right]$

其中，L为纬度，h为高度，V_n为导航坐标系下的速度分量；Rn为卯酉面曲率半径，R_m为子午面曲率半径，V_e为地球坐标系下的速度矢量，ω_ie为地球自转角速率；

$F_{12}=\left[\begin{array}{ccc}0& -\frac{1}{R_m+h}& 0\\ \frac{1}{R_n+h}& 0& 0\\ \frac{\tan L}{R_n+h}& 0& 0\end{array}\right]$

$F_{13}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h}{\sec }^{2}L& 0& 0\end{array}\right]$

$F_{14}=\left[\begin{array}{cccccc}-\mathrm{\α}& & & & & \\ & -\mathrm{\α}& & & 0& \\ & & -\mathrm{\α}& & & \\ & & & -\mathrm{\β}& & \\ & 0& & & -\mathrm{\β}& \\ & & & & & -\mathrm{\β}\end{array}\right]$

其中，α为速度分量方差，β为位置分量方差；

$F_{21}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\\ -f_n& f_e& 0\end{array}\right]$

其中，f_u、f_n、f_e为分别为地理坐标系下天向、北向、东向方向上的比力；

$F_{22}=\left[\begin{array}{ccc}\frac{V_n\tan L}{R_n+h}-\frac{V_n}{R_n+h}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h}& -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n+h})& -\frac{V_n}{R_m+h}& -\frac{V_n}{R_m+h}\\ 2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n+h})& \frac{2V_n}{R_m+h}& 0\end{array}\right]$

$F_{23}=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_n+\frac{V_e{V}_n}{R_n+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_n& 0& 0\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{V}_e+\frac{V_e^2}{R_n+h}{\sec }^{2}L)& 0& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_e& 0& 0\end{array}\right]$

$F_{24}=\left[\begin{array}{cc}{C}_n^b& 0_{3\×3}\end{array}\right]$ $F_{32}=\left[\begin{array}{ccc}0& \frac{1}{R_m+h}& 0\\ \frac{\sec L}{R_n+h}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{33}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{V_e\sec L\tan L}{R_n+h}& 0& 0\\ 0& 0& 0\end{array}\right]$

$F_{43}=\left[\begin{array}{cc}{0}_{3\×3}& C_n^b\end{array}\right]$

所述系统噪声矩阵如下：

$R={\left[\begin{array}{ccccc}{X}_1{X}_1^H& X_1{X}_2^H& X_1{X}_3^H& \·\·\·& X_1{X}_M^H\\ X_2{X}_1^H& X_2{X}_2^H& {X_{2}X}_3^H& \·\·\·& X_2{X}_M^H\\ X_3{X}_1^H& X_3{X}_2^H& X_3{X}_3^H& \·\·\·& X_3{X}_M^H\\ \·\·\·& \·\·\·& \·\·\·& \·\·\·& \·\·\·\\ X_M{X}_1^H& {X_{M}X}_2^H& X_M{X}_3^H& \·\·\·& X_M{X}_M^H\end{array}\right]}_{\mathrm{MN}\×\mathrm{MN}}$

M为代表速度和位置标准差的6维向量，H表示转置共轭;

所述量测方程如下：

Z₁＝H₁X+γ_n

其中，量测噪声γ_n是零均值的白噪声，量测矩阵H₁为：

$\begin{array}{c}{H}_1=\left[\begin{array}{ccc}{V}^n{C}_n^b\left(\mathrm{1,2}\right)-V^n{C}_n^b\left(\mathrm{1,3}\right)& -V^n{C}_n^b\left(\mathrm{1,2}\right)+V^e{C}_n^b\left(\mathrm{1,3}\right)& V^n{C}_n^b\left(\mathrm{1,1}\right)-V^e{C}_n^b\left(\mathrm{1,2}\right)\\ V^n{C}_n^b\left(\mathrm{3,2}\right)-V^n{C}_n^b\left(\mathrm{3,3}\right)& -V^n{C}_n^b\left(\mathrm{3,2}\right)+V^e{C}_n^b\left(\mathrm{3,3}\right)& V^n{C}_n^b\left(\mathrm{3,1}\right)-V^e{C}_n^b\left(\mathrm{3,2}\right)\end{array}\right.\\ {\left.\begin{array}{cccc}{C}_n^b\left(\mathrm{1,1}\right)& C_n^b\left(\mathrm{1,2}\right)& C_n^b\left(\mathrm{1,3}\right)& 0_{1\×9}\\ C_n^b\left(\mathrm{3,1}\right)& C_n^b\left(\mathrm{3,2}\right)& C_n^b\left(\mathrm{3,3}\right)& 0_{1\×9}\end{array}\right]}_{2\×15}\end{array}.$

其中，Vⁿ为导航坐标系下的速度矢量，V^e为地球坐标系下的速度矢量，后括号中的数字是指在矩阵中相应行和列上的元素。