CN102830414B - 一种基于sins/gps的组合导航方法 - Google Patents

Abstract

本发明涉及一种新型的捷联惯性导航系统(SINS)和全球定位系统(GPS)的组合导航方法。此部分属于航空应用领域中无人机自驾仪上的导航计算模块，可应用于各种无人机导航系统中。本发明首先在方位误差角为大失准角的情况下推导了多阶SINS/GPS组合导航系统的非线性模型，在SINS解算之前引入PI控制器保证每一时刻进行姿态计算的角速率都是当前最优值。在SINS/GPS组合时刻，角速率的修正是在扩展卡尔曼滤波算法对陀螺进行了一定的误差补偿的基础上加入PI控制器对角速率进行再次精确补偿；而在非组合时刻，陀螺输出的角速率值仍可以由PI控制器进行修正，得到组合导航参数的最优估计。

Description

一种基于SINS/GPS的组合导航方法

技术领域

本发明涉及一种新型的捷联惯性导航系统(SINS)和全球定位系统(GPS)的组合导航方法。此部分属于航空应用领域中无人机自驾仪上的导航计算模块，可应用于各种无人机导航系统中。

背景技术

捷联惯导系统(SINS)能够连续实时地提供位置、速度、姿态等导航信息，且不受任何外界信息的干扰，是一种完全自主式的导航系统。但导航误差随时间积累的缺点使捷联惯导系统不能够单独完成长时间高精度的导航任务，需要其他辅助系统从旁协助。全球定位系统(GPS)则具有定位精度高，导航误差不随时间积累等优点，但是数据更新频率低、易受外界环境的干扰。因而SINS/GPS组合导航系统应运而生，目前已广泛应用于军事民用等的航空领域中。

组合导航系统中，实际系统总是存在不同程度的非线性，虽然有些系统可以近似看作线性系统，但是大部分系统难以用线性微分或差分方程描述。此外，实际系统中通常还存在高斯或非高斯随机噪声干扰。非线性随机动态系统广泛存在于工程实践，如飞机和舰船的惯性导航系统、组合导航系统等都属于这类系统。在这类非线性随机动态系统中，如何进行有效甚至最优状态估计尤为重要，这就需要采用非线性滤波技术。然而，每种非线性滤波方法都有其各自的局限性，因此仅依靠滤波技术在恶劣的飞行条件下不能得到准确的导航参数估计。

对于惯性导航系统，建立准确的误差方程是采用滤波技术进行对准和状态误差估计的基础，若系统模型存在误差，则会影响滤波精度，甚至使滤波发散。通常采用的线性误差方程都是在假定平台失准角(即姿态误差角)为小量的条件下导出的，当失准角较大时，系统误差呈现较严重的非线性特性，这些线性方程已不能准确描述捷联惯导系统的误差传播特性，如果继续使用可能致使滤波误差较大甚至发散，因此，需要寻找更为精确的非线性误差方程来代替。

发明内容

在方位误差角为大失准角时，组合导航系统的线性模型已经无法描述实际系统的特性，不能满足导航精度的要求。扩展卡尔曼滤波在运用过程中往往采用对非线性模型进行泰勒级数展开近似真实系统，这样做不可避免地会产生截断误差，加之计算过程中积分误差的积累，很容易造成扩展卡尔曼滤波的不稳定甚至发散。基于以上考虑，本发明首先在方位误差角为大失准角的情况下推导了SINS/GPS组合导航系统的非线性模型，然后应用PI控制器和扩展卡尔曼滤波相结合的方法进行数据融合，得到组合导航参数的最优估计。

本发明的目的是通过下述技术方案实现的。

步骤一、使用PI(比例积分)控制器补偿角速率ω。

将飞行器的姿态矩阵记为 $C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

第一种情况：

飞行器的航向角误差为：

yawerror＝yaw_SINS×yaw_GPS

其中，yaw_SINS＝[C₁₁ C₂₁ 0]^T；yaw_GPS＝[cog_x cog_y0]^T，cog为GPS给出的航向角。

重力加速度：g＝acc-ω×V_GPS

其中，VGPS为GPS给出的飞行器速度；ω为角速率，acc为加速度。

飞行器的横滚角和俯仰角误差为：

rollpitcherror＝g_SINS×g

其中，g_SINS＝[C₃₁ C₃₂ C₃₃]^T

总误差：totalerror＝ω_Yyawerror+ω_RProllpitcherror

其中，ω_Y和ω_RP分别为航向角误差，横滚俯仰角误差的权重系数。

第二种情况：

当GPS数据没有给出的时刻，yawerror＝0

则总误差totalerror＝ω_RProllpitcherror

比例(P)补偿：ω_Proportion＝K_Ptotalerror

积分(I)补偿：ω_Integral＝ω_Integral+K_ItotalerrorΔt

其中，K_P为比例系数；K_I为积分系数；Δt为采样间隔。

对角速率ω进行PI补偿，得到补偿后的角速率为

ω_correction＝ω_Proportion+ω_Integral

将补偿后的角速率ω_correction作为SINS的输入进行运算，得到姿态角(航向角ψ，俯仰角θ和横滚角γ)，速度(东向V_x，北向V_t，天向V_z)，位置(纬度L，经度λ，高度h)。

步骤二、在方位误差角为大失准角时，推导出姿态非线性误差方程、速度非线性误差方程以及位置误差方程。从中选取状态变量，建立组合导航系统的非线性误差模型。

姿态非线性误差方程为：

${\stackrel{\·}{\mathrm{\φ}}}_x=-(1-\cos {\mathrm{\φ}}_z)\frac{V_y}{R_m+h}-\sin {\mathrm{\φ}}_z({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})+{\mathrm{\φ}}_y({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)$

$-\frac{1}{R_m+h}\mathrm{\δ}{V}_y+\frac{V_y}{{(R_m+h)}^2}\mathrm{\δh}+{\mathrm{\ϵ}}_x^n$

${\stackrel{\·}{\mathrm{\φ}}}_y=-\sin {\mathrm{\φ}}_z\frac{V_y}{R_m+h}+(1-\cos {\mathrm{\φ}}_z)({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})-{\mathrm{\φ}}_x({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}$

$+\frac{1}{R_n+h}\mathrm{\δ}{V}_x-\frac{V_x}{{(R_m+h)}^2}\mathrm{\δh}+{\mathrm{\ϵ}}_y^n$

${\stackrel{\·}{\mathrm{\φ}}}_z={(\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z)\frac{V_y}{R_m+h}-({\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z)({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_m+h})+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x{\sec }^{2}L}{R_n+h})\mathrm{\δL}$

$+\frac{\tan L}{R_n+h}\mathrm{\δ}{V}_x-\frac{V_x\tan L}{{(R_n+h)}^2}\mathrm{\δh}+{\mathrm{\ϵ}}_z^n$

速度非线性误差方程为：

$\mathrm{\δ}{\stackrel{\·}{V}}_x=(\cos {\mathrm{\φ}}_z-1)f_x+{\sin \mathrm{\φ}}_z{f}_y-{\mathrm{\φ}}_y{f}_z+\frac{V_y\tan L-V_z}{R_n+h}\mathrm{\δ}{V}_x+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)\mathrm{\δ}{V}_y$

$-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})\mathrm{\δ}{V}_z+[2{\mathrm{\ω}}_{\mathrm{ie}}(V_z\sin L+V_y\cos L)+\frac{V_x{V}_y{\sec }^{2}L}{R_n+h}]\mathrm{\δL}+\frac{V_x{V}_z-V_x{V}_y\tan L}{{(R_n+h)}^2}\mathrm{\δh}+{\▿}_x^n$

$\mathrm{\δ}{\stackrel{\·}{V}}_y=-\sin {\mathrm{\φ}}_z{f}_x+(\cos {\mathrm{\φ}}_z-1)f_y+{\mathrm{\φ}}_x{f}_z-(\frac{{2V}_x\tan L}{R_n+h}+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L)\mathrm{\δ}{V}_x-\frac{V_z}{R_m+h}\mathrm{\δ}{V}_y-\frac{V_y}{R_m+h}\mathrm{\δ}{V}_z$

$-(2{V_x\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{{V_x}^2}{R_n+h}{\sec }^{2}L)\mathrm{\δL}+(\frac{V_y{V}_z}{{(R_m+h)}^2}+\frac{{V_x}^2\tan L}{{(R_n+h)}^2})\mathrm{\δh}+{\▿}_y^n$

$\mathrm{\δ}{\stackrel{\·}{V}}_z=({\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z)f_x+({\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z)f_y+2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})\mathrm{\δ}{V}_x$

$+\frac{2V_y}{R_m+h}\mathrm{\δ}{V}_y-2V_x{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-[\frac{{V_y}^2}{{(R_m+h)}^2}+\frac{{V_x}^2}{{(R_n+h)}^2}]\mathrm{\δh}+{\▿}_z^n$

位置误差方程为：

$\mathrm{\δ}\stackrel{\·}{L}=\frac{1}{R_m+h}\mathrm{\δ}{V}_y-\frac{V_y}{{(R_m+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\sec L}{R_n+h}\mathrm{\δ}{V}_x+\frac{V_x\tan L\sec L}{R_n+h}\mathrm{\δL}-\frac{V_x\sec L}{{(R_n+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{V}_z$

陀螺、加速度计误差方程为：

$\begin{array}{cc}\stackrel{\·}{\mathrm{\ϵ}}=0& \stackrel{\·}{\▿}=0\end{array}$

其中， 为位置速率，为地球自转速率；φ_x，φ_y，φ_z为东向，北向，方位失准角；V_x，V_y，V_z为东向，北向，天向的速度；δV_x，δV_y，δV_z为东向、北向、天向速度误差；L，λ，h为纬度，经度，高度；δL，δλ，δh为纬度、经度、高度误差；f_x，f_y，f_z为东向，北向，天向的比力；为东向，北向，天向的陀螺常值漂移；为东向，北向，天向的加速度计零偏；R_m为地球子午圈上的曲率半径，R_n为地球卯酉圈上的曲率半径；陀螺常值漂移为 $\begin{array}{c}{\mathrm{\ϵ}}_x^n=C_{11}{\mathrm{\ϵ}}_x^b+C_{12}{\mathrm{\ϵ}}_y^b+C_{13}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_y^n=C_{21}{\mathrm{\ϵ}}_x^b+C_{22}{\mathrm{\ϵ}}_y^b+C_{23}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_z^n=C_{31}{\mathrm{\ϵ}}_x^b+C_{32}{\mathrm{\ϵ}}_y^b+C_{33}{\mathrm{\ϵ}}_z^b\end{array},$ 加速度计零偏为 $\begin{array}{c}{\▿}_x^n=C_{11}{\▿}_x^b+C_{12}{\▿}_y^b+C_{13}{\▿}_z^b\\ {\▿}_y^n=C_{21}{\▿}_x^b+C_{22}{\▿}_y^b+C_{23}{\▿}_z^b\\ {\▿}_z^n=C_{31}{\▿}_x^b+C_{32}{\▿}_y^b+C_{33}{\▿}_z^b\end{array}.$

从以上的误差方程中，选取以下15个状态变量：

$X\left(t\right)=\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& \mathrm{\δ}{V}_x& \mathrm{\δ}{V}_y& \mathrm{\δ}{V}_z& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b& {\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]$

建立连续多阶的状态方程和观测方程，形式如下：

$\stackrel{\·}{X}\left(t\right)=f\left(X\left(t\right)\right)+W\left(t\right)$

Z(t)＝HX(t)+V(t)

以上即为SINS/GPS组合导航系统的非线性误差模型。

其中，f(·)为X(t)的非线性函数；H(t)为观测矩阵；W(t)为过程噪声序列；V(t)为观测噪声序列。Z(t)的形式如下：

$Z\left(t\right)=\left[\begin{array}{c}{V}_x-V_x\_\mathrm{GPS}\\ V_y-V_y\_\mathrm{GPS}\\ V_z-V_{z\_}\mathrm{GPS}\\ L-L\_\mathrm{GPS}\\ \mathrm{\λ}-\mathrm{\λ}\_\mathrm{GPS}\\ h-h\_\mathrm{GPS}\end{array}\right]$

V_x_GPS，V_y_GPS，V_z_GPS为GPS给出的东向，北向，天向的速度；L_GPS，λ_GPS，h_GPS为GPS给出的纬度，经度，高度信息。

步骤三、使用二阶泰勒级数展开对步骤二得到的非线性误差模型进行线性化并离散化。

$X(t+\mathrm{\Δt})=X\left(t\right)+f\left(X\left(t\right)\right)\mathrm{\Δt}+\frac{\∂f\left(X\left(t\right)\right)}{\∂X}f\left(X\left(t\right)\right)\frac{\mathrm{\Δ}{t}^2}{2}$

记X(k)＝X(t)，X(k+1)＝X(t+Δt)，则离散化后的状态方程和观测方程为：

$X(k+1)=X\left(k\right)+f\left(X\left(k\right)\right)\mathrm{\Δt}+\frac{{\mathrm{\Δt}}^2}{2}A\left(X\left(k\right)\right)f\left(X\left(k\right)\right)+W\left(k\right)$

Z(k)＝HX(k)+V(k)

其中，Δt为采样间隔。

步骤四、对步骤三离散化后的模型使用扩展卡尔曼滤波。

步骤五、使用步骤四得出的扩展卡尔曼滤波结果修正步骤一中SINS输出的姿态矩阵速度(V_x，V_y，V_z)、位置(L，λ，h)以及陀螺的输出值ω′、加速度计的输出值acc′，最后得到导航参数的最优估计即飞行器的姿态角、速度、位置的信息。

修正过程如下：

姿态矩阵修正：

其中，由姿态失准角得出；为SINS计算出的姿态矩阵；为修正后的姿态矩阵。

速度修正

$\left[\begin{array}{c}{V}_x^{\′}\\ V_y^{\′}\\ V_z^{\′}\end{array}\right]=\left[\begin{array}{c}{V}_x-\mathrm{\δ}{V}_x\\ V_y-\mathrm{\δ}{V}_y\\ V_z-\mathrm{\δ}{V}_z\end{array}\right]$

其中，V′_x，V′_y，V′_z为修正后的东向，北向和天向的速度。

位置修正

$\left[\begin{array}{c}{L}^{\′}\\ {\mathrm{\λ}}^{\′}\\ h^{\′}\end{array}\right]=\left[\begin{array}{c}L-\mathrm{\δL}\\ \mathrm{\λ}-\mathrm{\δ\λ}\\ h-\mathrm{\δh}\end{array}\right]$

其中，L′，λ′，h′为修正后的纬度，经度和高度。

陀螺角速率补偿

ω＝ω′-ε

其中，ω′为陀螺输出的角速率，ω为修正后的陀螺角速率， $\mathrm{\ϵ}=\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b\end{array}\right]$ 为陀螺常值漂移。

加速度补偿

$\mathrm{acc}=a{\mathrm{cc}}^{\′}-\▿$

其中，acc′为加速度计输出的加速度，acc为修正后的加速度， $\▿=\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]$ 为加速度计零偏。

步骤六、整个导航解算过程中，PI控制器对角速率的补偿频率与SINS的更新频率是一样的，并且要大于扩展卡尔曼滤波对角速率的修正频率。在GPS与SINS的组合时刻，按照上述步骤一至步骤五的方法，对飞行器的姿态角、速度和位置进行更新；在非组合时刻，姿态角、位置和速度按照步骤一中的情况二由SINS直接给出。

有益效果

在假定方位误差角为大失准角的情况下，建立了多阶的SINS/GPS组合导航系统的非线性模型。

本发明克服了传统组合导航方法只有在组合时刻进行陀螺和加速度计的误差补偿，而在非组合时刻不对陀螺和加速度计的误差进行修正的缺陷。本发明在捷联惯导系统(SINS)解算之前引入PI控制器保证了每一时刻进行姿态计算的角速率都是当前最优值。在SINS/GPS组合时刻，角速率的修正是在扩展卡尔曼滤波算法对陀螺进行了一定的误差补偿的基础上加入PI控制器对角速率进行再次精确补偿；而在非组合时刻，陀螺输出的角速率值仍可以由PI控制器进行修正。算法计算量没有明显增加，却大大改进了传统非线性滤波的精度。

附图说明

图1为本发明的SINS/GPS组合导航系统方法原理示意图；

图2为具体实施方式中理想导航坐标系ox_ny_nz_n与实际导航坐标系ox_n′y_n′z_n′之间的关系示意图；

图3为本发明的SINS/GPS组合导航系统方法流程图。

具体实施方式

下面结合实例及附图，对本发明作进一步详细说明。

本实例坐标及符号说明：

坐标系符号：理想导航坐标系n，实际导航坐标系n′，机体坐标系b坐标系选取：选取地理坐标系(东北天)作为导航坐标系

第一步：设计PI(比例积分)控制器补偿角速率。

将飞行器的姿态矩阵(从机体坐标系b到导航坐标系n的转换矩阵)记为

$C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

第一种情况：

飞行器的航向角误差：

yawerror＝yaw_SINS×yaw_GPS

其中，yaw_SINS＝[C₁₁ C₂₁ 0]^T；yaw_GPS＝[cog_x cog_y0]^T，cog为GPS给出的航向角。

重力加速度：g＝acc-ω×V_GPS

其中，VGPS为GPS给出的速度；acc为从加速度计读出的加速度值；g为重力加速度。

飞行器的横滚角和俯仰角误差：

rollpitcherror＝g_SINS×g

其中，g_SINS＝[C₃₁ C₃₂ C₃₃]^T

总误差：totalerror＝ω_Yyawerror+ω_RProllpitcherror

其中，ω_Y和ω_RP分别为航向角误差，横滚俯仰角误差的权重系数。

第二种情况：

当GPS数据没有给出的时刻，yawerror＝0

则总误差totalerror＝ω_RProllpitcherror

比例(P)补偿：ω_Proportion＝K_Ptotalerror

积分(I)补偿：ω_Integral＝ω_Integral+K_ItotalerrorΔt

其中，K_P为比例系数；K_I为积分系数；Δt为采样间隔。

对角速率ω进行PI补偿，得到补偿后的角速率为：

ω_correction＝ω_Proportion+ω_Integral

将补偿后的角速率ω_correction作为SINS的输入进行运算，得到姿态角(航向角ψ，俯仰角θ和横滚角γ)，速度(东向V_x，北向V_y，天向V_z)，位置(纬度L，经度λ，高度h)。

第二步：考虑水平方向上的姿态误差角基本不会出现大角度，而方位误差角在实际过程中往往会出现大角度的情况，从而假定水平误差角φ_x和φ_y为小角度，φ_z为大失准角。基于此，推导SINS/GPS组合导航的非线性模型。

①实际导航坐标系n′偏离理想导航坐标系n的失准角在东北天三个方向上分别为φ_x，φ_y，φ_z，则

$C_n^{n^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y& 0& -\sin {\mathrm{\φ}}_y\\ 0& 1& 0\\ \sin {\mathrm{\φ}}_y& 0& \cos {\mathrm{\φ}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\φ}}_x& \sin {\mathrm{\φ}}_x\\ 0& -\sin {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_z& 0\\ -\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_z& 0\\ 0& 0& 1\end{array}\right]$

$=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z-{\sin \mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_z\cos {\mathrm{\φ}}_y+\sin {\mathrm{\φ}}_x\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& -\sin {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_x\\ -\sin {\mathrm{\φ}}_z\cos {\mathrm{\φ}}_x& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_x\\ {\sin \mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-\sin {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_x\cos {\mathrm{\φ}}_y\end{array}\right]$

当方位误差为大失准角时，φ_z不能看作小角度进行近似计算。而水平方向上基本不会出现大角度偏差的情况，所以水平失准角φ_x和φ_y可看作小角度，即sinφ_x＝φ_x，cosφ_x＝1，sinφ_y＝φ_y，cosφ_y＝1，则

$C_n^{n^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_z& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z& {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& 1\end{array}\right]$

②姿态非线性误差方程为：

$\stackrel{\·}{\mathrm{\φ}}=(I-C_n^{n^{\′}}){\mathrm{\ω}}_{\mathrm{in}}^n+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{in}}^n+{\mathrm{\ϵ}}^n$

$\left[\begin{array}{c}{\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\\ {\mathrm{\φ}}_z\end{array}\right]=(\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]-\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_z& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z& {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& 1\end{array}\right])\left[\begin{array}{c}-\frac{V_y}{R_m+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L\end{array}\right]$

$+\left[\begin{array}{c}-\frac{\mathrm{\δ}{V}_y}{R_m+h}+\mathrm{\δh}\frac{V_y}{{(R_m+h)}^2}\\ -\mathrm{\δL}{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{\mathrm{\δ}{V}_x}{R_n+h}-\mathrm{\δh}\frac{V_x}{{(R_m+h)}^2}\\ \mathrm{\δL}{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\δ}{V}_x}{R_n+h}\tan L+\mathrm{\δL}\frac{V_x}{R_n+h}{\sec }^{2}L-\mathrm{\δh}\frac{V_x\tan L}{{(R_n+h)}^2}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ϵ}}_x^n\\ {\mathrm{\ϵ}}_y^n\\ {\mathrm{\ϵ}}_z^n\end{array}\right]$

可得：

${\stackrel{\·}{\mathrm{\φ}}}_x=-(1-\cos {\mathrm{\φ}}_z)\frac{V_y}{R_m+h}-\sin {\mathrm{\φ}}_z({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})+{\mathrm{\φ}}_y({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)$

$-\frac{1}{R_m+h}\mathrm{\δ}{V}_y+\frac{V_y}{{(R_m+h)}^2}\mathrm{\δh}+{\mathrm{\ϵ}}_x^n$

${\stackrel{\·}{\mathrm{\φ}}}_y=-\sin {\mathrm{\φ}}_z\frac{V_y}{R_m+h}+(1-\cos {\mathrm{\φ}}_z)({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})-{\mathrm{\φ}}_x({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}$

$+\frac{1}{R_n+h}\mathrm{\δ}{V}_x-\frac{V_x}{{(R_m+h)}^2}\mathrm{\δh}+{\mathrm{\ϵ}}_y^n$

${\stackrel{\·}{\mathrm{\φ}}}_z={(\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z)\frac{V_y}{R_m+h}-({\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z)({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_m+h})+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x{\sec }^{2}L}{R_n+h})\mathrm{\δL}$

$+\frac{\tan L}{R_n+h}\mathrm{\δ}{V}_x-\frac{V_x\tan L}{{(R_n+h)}^2}\mathrm{\δh}+{\mathrm{\ϵ}}_z^n$

③速度非线性误差方程为：

$\mathrm{\δ}{\stackrel{\·}{V}}^n=(C_n^{n^{\′}}-I)f^n+\mathrm{\δ}{V}^n\×(2{{\mathrm{\ω}}_{\mathrm{ie}}}^n+{{\mathrm{\ω}}_{\mathrm{en}}}^n)+V^n\×(2\mathrm{\δ}{{\mathrm{\ω}}_{\mathrm{ie}}}^n+\mathrm{\δ}{{\mathrm{\ω}}_{\mathrm{en}}}^n)+{\▿}^n$

$=(\left[\begin{array}{ccc}\cos {\mathrm{\φ}}_z& \sin {\mathrm{\φ}}_z& -{\mathrm{\φ}}_y\\ -\sin {\mathrm{\φ}}_z& \cos {\mathrm{\φ}}_z& {\mathrm{\φ}}_x\\ {\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z& {\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z& 1\end{array}\right]-\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right])\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]+\left[\begin{array}{ccc}0& -\mathrm{\δ}{V}_z& \mathrm{\δ}{V}_y\\ \mathrm{\δ}{V}_z& 0& -\mathrm{\δ}{V}_x\\ -\mathrm{\δ}{V}_y& \mathrm{\δ}{V}_x& 0\end{array}\right]\left[\begin{array}{c}-\frac{V_y}{R_m+h}\\ 2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h}\\ 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L\end{array}\right]$

$+\left[\begin{array}{ccc}0& -V_z& V_y\\ V_z& 0& -V_x\\ -V_y& V_x& 0\end{array}\right]\left[\begin{array}{c}-\frac{\mathrm{\δ}{V}_y}{R_m+h}+\mathrm{\δh}\frac{V_y}{{(R_m+h)}^2}\\ -2\mathrm{\δL}{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{\mathrm{\δ}{V}_x}{R_n+h}-\mathrm{\δh}\frac{V_x}{{(R_n+h)}^2}\\ 2\mathrm{\δL}{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{\mathrm{\δ}{V}_x}{R_n+h}\tan L+\mathrm{\δL}\frac{V_x}{R_n+h}{\sec }^{2}L-\mathrm{\δh}\frac{V_x\tan L}{{(R_n+h)}^2}\end{array}\right.++\left[\begin{array}{c}{\▿}_x^n\\ {\▿}_y^n\\ {\▿}_z^n\end{array}\right]$

可得：

$\mathrm{\δ}{\stackrel{\·}{V}}_x=(\cos {\mathrm{\φ}}_z-1)f_x+{\sin \mathrm{\φ}}_z{f}_y-{\mathrm{\φ}}_y{f}_z+\frac{V_y\tan L-V_z}{R_n+h}\mathrm{\δ}{V}_x+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)\mathrm{\δ}{V}_y$

$-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})\mathrm{\δ}{V}_z+[2{\mathrm{\ω}}_{\mathrm{ie}}(V_z\sin L+V_y\cos L)+\frac{V_x{V}_y{\sec }^{2}L}{R_n+h}]\mathrm{\δL}+\frac{V_x{V}_z-V_x{V}_y\tan L}{{(R_n+h)}^2}\mathrm{\δh}+{\▿}_x^n$

$\mathrm{\δ}{\stackrel{\·}{V}}_y=-\sin {\mathrm{\φ}}_z{f}_x+(\cos {\mathrm{\φ}}_z-1)f_y+{\mathrm{\φ}}_x{f}_z-(\frac{{2V}_x\tan L}{R_n+h}+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L)\mathrm{\δ}{V}_x-\frac{V_z}{R_m+h}\mathrm{\δ}{V}_y-\frac{V_y}{R_m+h}\mathrm{\δ}{V}_z$

$-(2{V_x\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{{V_x}^2}{R_n+h}{\sec }^{2}L)\mathrm{\δL}+(\frac{V_y{V}_z}{{(R_m+h)}^2}+\frac{{V_x}^2\tan L}{{(R_n+h)}^2})\mathrm{\δh}+{\▿}_y^n$

$\mathrm{\δ}{\stackrel{\·}{V}}_z=({\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z)f_x+({\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z)f_y+2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})\mathrm{\δ}{V}_x$

$+\frac{2V_y}{R_m+h}\mathrm{\δ}{V}_y-2V_x{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-[\frac{{V_y}^2}{{(R_m+h)}^2}+\frac{{V_x}^2}{{(R_n+h)}^2}]\mathrm{\δh}+{\▿}_z^n$

④位置误差方程为：

$\mathrm{\δ}\stackrel{\·}{L}=\frac{1}{R_m+h}\mathrm{\δ}{V}_y-\frac{V_y}{{(R_m+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\sec L}{R_n+h}\mathrm{\δ}{V}_x+\frac{V_x\tan L\sec L}{R_n+h}\mathrm{\δL}-\frac{V_x\sec L}{{(R_n+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{V}_z$

⑤陀螺、加速度计误差方程为：

$\begin{array}{cc}\stackrel{\·}{\mathrm{\ϵ}}=0& \stackrel{\·}{\▿}=0\end{array}$

其中， 为位置速率，为地球自转速率；φ_x，φ_y，φ_z为东向，北向，方位失准角；V_x，V_y，V_z为东向，北向，天向的速度；δV_x，δV_y，δV_z为东向、北向、天向速度误差；L，λ，h为纬度，经度，高度；δL，δλ，δh为纬度、经度、高度误差；f_x，f_y，f_z为东向，北向，天向的比力；为东向，北向，天向的陀螺常值漂移；为东向，北向，天向的加速度计零偏；R_m为地球子午圈上的曲率半径，R_n为地球卯酉圈上的曲率半径；陀螺常值漂移为 $\begin{array}{c}{\mathrm{\ϵ}}_x^n=C_{11}{\mathrm{\ϵ}}_x^b+C_{12}{\mathrm{\ϵ}}_y^b+C_{13}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_y^n=C_{21}{\mathrm{\ϵ}}_x^b+C_{22}{\mathrm{\ϵ}}_y^b+C_{23}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_z^n=C_{31}{\mathrm{\ϵ}}_x^b+C_{32}{\mathrm{\ϵ}}_y^b+C_{33}{\mathrm{\ϵ}}_z^b\end{array},$ 加速度计零偏为 $\begin{array}{c}{\▿}_x^n=C_{11}{\▿}_x^b+C_{12}{\▿}_y^b+C_{13}{\▿}_z^b\\ {\▿}_y^n=C_{21}{\▿}_x^b+C_{22}{\▿}_y^b+C_{23}{\▿}_z^b\\ {\▿}_z^n=C_{31}{\▿}_x^b+C_{32}{\▿}_y^b+C_{33}{\▿}_z^b\end{array}.$

⑥基于以上的误差方程，本实施例中选取15个状态变量如下：

$X\left(t\right)=\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& \mathrm{\δ}{V}_x& \mathrm{\δ}{V}_y& \mathrm{\δ}{V}_z& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b& {\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]$

建立连续多阶的状态方程和观测方程，即SINS/GPS组合导航系统的非线性模型：

$\stackrel{\·}{X}\left(t\right)=f\left(X\left(t\right)\right)+W\left(t\right)$

Z(t)＝HX(t)+V(t)

其中，f(·)为X(t)的非线性函数；H(t)为观测矩阵；W(t)为过程噪声序列；V(t)为观测噪声序列。Z(t)的形式如下：

$Z\left(t\right)=\left[\begin{array}{c}{V}_x-V_x\_\mathrm{GPS}\\ V_y-V_y\_\mathrm{GPS}\\ V_z-V_{z\_\mathrm{GPS}}\\ L-L\_\mathrm{GPS}\\ \mathrm{\λ}-\mathrm{\λ}\_\mathrm{GPS}\\ h-h\_\mathrm{GPS}\end{array}\right]$

V_x_GPS，V_y_GPS，V_z_GPS为GPS给出的东向，北向，天向的速度；L_GPS，λ_GPS，h_GPS为GPS给出的纬度，经度，高度信息。

第三步：使用二阶泰勒级数展开对第二步得到的非线性误差模型进行线性化并离散化。

设采样间隔为Δt

$X(t+\mathrm{\Δt})=X\left(t\right)+f\left(X\left(t\right)\right)\mathrm{\Δt}+\frac{\∂f\left(X\left(t\right)\right)}{\∂X}f\left(X\left(t\right)\right)\frac{\mathrm{\Δ}{t}^2}{2}$

记X(k)＝X(t)，X(k+1)＝X(t+Δt)，则离散化后的系统方程为：

$X(k+1)=X\left(k\right)+f\left(X\left(k\right)\right)\mathrm{\Δt}+\frac{{\mathrm{\Δt}}^2}{2}A\left(X\left(k\right)\right)f\left(X\left(k\right)\right)+W\left(k\right)$

Z(k)＝HX(k)+V(k)

其中， $A\left(X\left(k\right)\right)=\frac{\∂f\left(X\left(t\right)\right)}{\∂X}{|}_{X=X\left(k\right)}$

第四步：对第三步离散化后的模型进行扩展卡尔曼滤波：

状态一步预测： $\hat{X}(k+1,k)=\hat{X}\left(k\right)+f\left(\hat{X}\left(k\right)\right)\mathrm{\Δt}+\frac{{\mathrm{\Δt}}^2}{2}A\left(\hat{X}\left(k\right)\right)f\left(\hat{X}\left(k\right)\right)$

一步预测误差方差矩阵：P(k+1，k)＝Φ(k)P(k)Φ^T(k)+Γ(k)Q(k)Γ^T(k)

滤波增益矩阵：K(k+1)＝P(k+1，k)H^T[HP(k+1，k)H^T+R(k)]^-1

状态估计： $\hat{X}(k+1)=\hat{X}(k+1,k)+K(k+1)[Z(k+1)-H\hat{X}(k+1,k)]$

估计误差方差矩阵：P(k+1)＝[I-K(k+1)H]P(k+1，k)

其中， $\mathrm{\Φ}\left(k\right)=I_{15\×15}+A\left(\hat{X}\left(k\right)\right)\mathrm{\Δt}$

$\left\{\begin{array}{c}E\left[W\left(k\right)\right]=0,E\left[W\left(k\right)W^T\left(j\right)\right]=Q\left(k\right){\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[V\left(k\right)\right]=0,E\left[V\left(k\right)V^T\left(j\right)\right]=R\left(k\right){\mathrm{\δ}}_{\mathrm{kj}}\end{array}\right.$

即Q(k)是系统过程噪声W(k)的方差矩阵；R(k)是系统观测噪声V(k)的方差矩阵；δ_kj是Kronecker-δ函数。

第五步：使用第四步得出的扩展卡尔曼滤波结果修正第一步中SINS输出的姿态矩阵速度(V_x，V_y，V_z)、位置(L，λ，h)以及陀螺的输出值ω′、加速度计的输出值acc′，最后得到导航参数的最优估计即飞行器的姿态角、速度、位置的信息。

修正过程如下：

①姿态矩阵修正

$C_b^n=C_{n^{\′}}^n{C}_b^{n^{\′}}$

其中， 为计算出的姿态矩阵，为修正后的姿态矩阵。

θ_主＝arcsin(C₃₂)

若航向角ψ，俯仰角θ和横滚角γ的取值范围分别定义为[0，2π]，[-π，+π]。

θ＝θ_主

其中，ψ，γ，θ为经过修正后的航向角，横滚角和俯仰角。

②速度修正

$\left[\begin{array}{c}{V}_x^{\′}\\ V_y^{\′}\\ V_z^{\′}\end{array}\right]=\left[\begin{array}{c}{V}_x-\mathrm{\δ}{V}_x\\ V_y-\mathrm{\δ}{V}_y\\ V_z-\mathrm{\δ}{V}_z\end{array}\right]$

其中，V′_x，V′_v，V′_z为修正后的东向，北向和天向的速度。

③位置修正

$\left[\begin{array}{c}{L}^{\′}\\ {\mathrm{\λ}}^{\′}\\ h^{\′}\end{array}\right]=\left[\begin{array}{c}L-\mathrm{\δL}\\ \mathrm{\λ}-\mathrm{\δ\λ}\\ h-\mathrm{\δh}\end{array}\right]$

其中，L′，λ′，h′为修正后的纬度，经度和高度。

④陀螺角速率补偿

ω＝ω′-ε

其中，ω′为陀螺输出的角速率，ω为修正后的陀螺角速率， $\mathrm{\ϵ}=\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b\end{array}\right]$ 为陀螺常值漂移。

⑤加速度补偿

$\mathrm{acc}={\mathrm{acc}}^{\′}-\▿$

其中，acc′为加速度计输出的加速度，acc为修正后的加速度， $\▿=\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]$ 为加速度计零偏。

第六步：整个导航解算过程中，PI控制器对角速率的补偿频率与SINS的更新频率是一样的，并且要大于扩展卡尔曼滤波对角速率的修正频率。在GPS与SINS的组合时刻，按照上述第一步至第五步的方法，对飞行器的姿态角、速度和位置进行更新；在非组合时刻，姿态角、位置和速度按照第一步中的情况二由SINS直接给出。

基于方位误差角为大失准角的情况，建立SINS/GPS组合导航的非线性误差模型，使模型更加逼近真实系统反映实际组合系统的特性。每种非线性滤波方法都有自身的局限性，不能仅仅依靠滤波技术完成组合导航的导航参数的最优估计，因而构造一种基于PI控制器和扩展卡尔曼滤波相结合的方法进行导航参数的误差估计来获得更加精确的导航参数。既解决了扩展卡尔曼滤波在模型线性化时产生的截断误差和固定步长的积分运算造成的误差积累问题，又解决了陀螺漂移导致数学平台漂移直接影响导航参数计算不准确的问题。本发明克服了传统组合导航方法只有在组合时刻进行陀螺和加速度计的误差补偿，而在非组合时刻不对陀螺和加速度计的误差进行修正的缺陷。本发明在捷联惯导系统(SINS)解算之前引入PI控制器保证了每一时刻进行姿态计算的角速率都是当前最优值。在SINS/GPS组合时刻，角速率的修正是在扩展卡尔曼滤波算法对陀螺进行了一定的误差补偿的基础上加入PI控制器对角速率进行再次精确补偿；而在非组合时刻，陀螺输出的角速率值仍可以由PI控制器进行修正。算法计算量没有明显增加，却大大改进了传统非线性滤波的精度。

Claims (4)

1.一种基于SINS/GPS的组合导航方法，其特征在于：包括如下步骤：

步骤一、使用PI控制器补偿角速率ω；

将飞行器的姿态矩阵记为 $C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

飞行器的航向角误差为：

yawerror＝yaw_SINS×yaw_GPS

其中，yaw_SINS＝[C₁₁ C₂₁ 0]^T；yaw_GPS＝[cog_x cog_y 0]^T,cog为GPS给出的航向角；

重力加速度：g＝acc-ω×V_GPS

其中，V_GPS为GPS给出的飞行器速度；ω为角速率，acc为加速度；

飞行器的横滚角和俯仰角误差为：

rollpitcherror＝g_SINS×g

其中，g_SINS＝[C₃₁ C₃₂ C₃₃]^T

总误差：totalerror＝ω_Yyawerror+ω_RProllpitcherror

其中，ω_Y和ω_RP分别为航向角误差，横滚俯仰角误差的权重系数；

比例(P)补偿：ω_Proportion＝K_Ptotalerror

积分(I)补偿：ω_Integral＝ω_{Integral_(t-1)}+K_ItotalerrorΔt

其中，K_P为比例系数；K_I为积分系数；Δt为采样间隔；其中“ω_{Integral_(t-1)}”表示t-1时刻角速率的积分补偿量；

对角速率ω进行PI补偿，得到补偿后的角速率为

ω_correction＝ω_Proportion+ω_Integral

将补偿后的角速率ω_correction作为SINS的输入进行运算，得到三个姿态角：航向角ψ，俯仰角θ和横滚角γ；东向速度V_x，北向速度V_y，天向速度V_z；纬度L，经度λ，高度h；

步骤二、在方位误差角为大失准角时，推导出姿态非线性误差方程、速度非线性误差方程以及位置误差方程；从中选取状态变量，建立组合导航系统的非线性误差模型；

姿态非线性误差方程为：

$\begin{array}{c}{\stackrel{.}{\mathrm{\φ}}}_x=-(1-\cos {\mathrm{\φ}}_z)\frac{V_y}{R_m+h}-\sin {\mathrm{\φ}}_z({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})+{\mathrm{\φ}}_y({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)\\ -\frac{1}{R_m+h}{\mathrm{\δV}}_y+\frac{V_y}{{(R_m+h)}^2}\mathrm{\δh}-{\mathrm{\ϵ}}_x\end{array}$

$\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_y=-\sin {\mathrm{\φ}}_z\frac{V_y}{R_m+h}+(1-\cos {\mathrm{\φ}}_z)({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})-{\mathrm{\φ}}_x({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}\\ +\frac{1}{R_n+h}{\mathrm{\δV}}_x-\frac{V_x}{{(R_n+h)}^2}\mathrm{\δh}-{\mathrm{\ϵ}}_y\end{array}$

$\begin{array}{c}{\stackrel{.}{\mathrm{\φ}}}_z=({\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z)\frac{V_y}{R_m+h}-({\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z)({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_m+h})+{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{L\δL}+\frac{\tan L}{R_n+h}\mathrm{\δ}{V}_x\\ +\frac{V_x}{R_n+h}{\sec }^2\mathrm{L\δL}-\frac{V_x\tan L}{{(R_n+h)}^2}\mathrm{\δh}-{\mathrm{\ϵ}}_z\end{array}$

速度非线性误差方程为：

$\begin{array}{c}\mathrm{\δ}{\stackrel{.}{V}}_x=(\cos {\mathrm{\φ}}_z-1)f_x+\sin {\mathrm{\φ}}_z{f}_y-{\mathrm{\φ}}_y{f}_z+\frac{V_y\tan L-V_z}{R_n+h}\mathrm{\δ}{V}_x+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)\mathrm{\δ}{V}_y\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})\mathrm{\δ}{V}_z+\left[2{\mathrm{\ω}}_{\mathrm{ie}}(V_z\sin L+V_y\cos L)\right]\mathrm{\δL}+\frac{V_x{V}_z-V_x{V}_y\tan L}{{(R_n+h)}^2}\mathrm{\δh}+{\▿}_x^n\end{array}$

$\begin{array}{c}\mathrm{\δ}{\stackrel{.}{V}}_y=-\sin {\mathrm{\φ}}_z{f}_x+(\cos {\mathrm{\φ}}_z-1)f_y+{\mathrm{\φ}}_x{f}_z-(\frac{V_x\tan L}{R_2+h}+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L)\mathrm{\δ}{V}_x-\frac{V_y}{R_m+h}\mathrm{\δ}{V}_y\\ -(2V_x{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{{V_x}^2}{R_n+h}{\sec }^{2}L)\mathrm{\δL}+(\frac{V_y{V}_z}{{(R_m+h)}^2}+\frac{{V_x}^2\tan L}{{(R_n+h)}^2})\mathrm{\δh}+{\▿}_y^n\end{array}$

$\begin{array}{c}\mathrm{\δ}{\stackrel{.}{V}}_z=({\mathrm{\φ}}_y\cos {\mathrm{\φ}}_z+{\mathrm{\φ}}_x\sin {\mathrm{\φ}}_z)f_x+({\mathrm{\φ}}_y\sin {\mathrm{\φ}}_z-{\mathrm{\φ}}_x\cos {\mathrm{\φ}}_z)f_y+2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})\mathrm{\δ}{V}_x\\ +\frac{2V_y}{R_m+h}\mathrm{\δ}{V}_y-2V_x{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-[\frac{{V_y}^2}{{(R_m+h)}^2}+\frac{{V_x}^2}{{(R_n+h)}^2}]\mathrm{\δL}+{\▿}_z^n\end{array}$

位置误差方程为：

$\mathrm{\δ}\stackrel{.}{L}=\frac{1}{R_m+h}\mathrm{\δ}{V}_y-\frac{V_y}{{(R_m+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{.}{\mathrm{\λ}}=\frac{\sec L}{R_n+h}\mathrm{\δ}{V}_x+\frac{V_x\tan L\sec L}{R_n+h}\mathrm{\δL}-\frac{V_x\sec L}{{(R_n+h)}^2}\mathrm{\δh}$

$\mathrm{\δ}\stackrel{.}{h}=\mathrm{\δ}{V}_z$

陀螺、加速度计误差方程为：

$\begin{array}{cc}\stackrel{.}{\mathrm{\ϵ}}=0& \stackrel{.}{\▿}=0\end{array}$

其中，为位置速率，为地球自转速率；φ_x，φ_y，φ_z为东向，北向，方位失准角；δV_x，δV_y，δV_z为东向、北向、天向速度误差；δL，δλ，δh为纬度、经度、高度误差；f_x，f_y，f_z为东向，北向，天向的比力； 为东向，北向，天向的陀螺常值漂移；为东向，北向，天向的加速度计零偏；R_m为地球子午圈上的曲率半径，R_n为地球卯酉圈上的曲率半径；陀螺常值漂移为 $\begin{array}{c}{\mathrm{\ϵ}}_x^n=C_{11}{\mathrm{\ϵ}}_x^b+C_{12}{\mathrm{\ϵ}}_y^b+C_{13}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_y^n=C_{21}{\mathrm{\ϵ}}_x^b+C_{22}{\mathrm{\ϵ}}_y^b+C_{23}{\mathrm{\ϵ}}_z^b\\ {\mathrm{\ϵ}}_z^n=C_{31}{\mathrm{\ϵ}}_x^b+C_{32}{\mathrm{\ϵ}}_y^b+C_{33}{\mathrm{\ϵ}}_z^b\end{array},$ 加速度计零偏为 $\begin{array}{c}{\▿}_x^n=C_{11}{\▿}_x^b+C_{12}{\▿}_y^b+C_{13}{\▿}_z^b\\ {\▿}_y^n=C_{21}{\▿}_x^b+C_{22}{\▿}_y^b+C_{23}{\▿}_z^b\\ {\▿}_z^n=C_{31}{\▿}_x^b+C_{32}{\▿}_y^b+C_{33}{\▿}_z^b\end{array};$

从以上的误差方程中，选取以下15个状态变量：

$X\left(t\right)=\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& \mathrm{\δ}{V}_x& \mathrm{\δ}{V}_y& \mathrm{\δ}{V}_z& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b& {\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]$

建立连续多阶的状态方程和观测方程，形式如下：

$\stackrel{.}{X}\left(t\right)=f\left(X\left(t\right)\right)+W\left(t\right)$

Z(t)＝HX(t)+V(t)

得到SINS/GPS组合导航系统的非线性误差模型；

其中，f(·)为X(t)的非线性函数；H(t)为观测矩阵；W(t)为过程噪声序列；V(t)为观测噪声序列；Z(t)的形式如下：

$Z\left(t\right)=\left[\begin{array}{c}{V}_x-V_x\_\mathrm{GPS}\\ V_y-V_y\_\mathrm{GPS}\\ V_z-V_z\_\mathrm{GPS}\\ L-L\_\mathrm{GPS}\\ \mathrm{\λ}-\mathrm{\λ}\_\mathrm{GPS}\\ h-h\_\mathrm{GPS}\end{array}\right]$

V_x_GPS，V_y_GPS，V_z_GPS为GPS给出的东向，北向，天向的速度；L_GPS，λ_GPS，h_GPS为GPS给出的纬度，经度，高度信息；

步骤三、使用二阶泰勒级数展开对步骤二得到的非线性误差模型进行线性化并离散化；

$X(t+\mathrm{\Δt})=X\left(t\right)+f\left(X\left(t\right)\right)\mathrm{\Δt}+\frac{\∂f\left(X\left(t\right)\right)}{\mathrm{\δX}}f\left(X\left(t\right)\right)\frac{\mathrm{\Δ}{t}^2}{2}$

记X(k)＝X(t)，X(k+1)＝X(t+Δt)，则离散化后的状态方程和观测方程为：

$X(k+1)=X\left(k\right)+f\left(X\left(k\right)\right)\mathrm{\Δt}+\frac{\mathrm{\Δ}{t}^2}{2}A\left(X\left(k\right)\right)f\left(X\left(k\right)\right)+W\left(k\right)$

Z(k)＝HX(k)+V(k)

其中， $A\left(X\left(k\right)\right)\frac{\∂f\left(X\left(t\right)\right)}{\∂X}{|}_{X=X\left(k\right)},$ Δt为采样间隔；

步骤四、对步骤三离散化后的模型使用扩展卡尔曼滤波；

步骤五、使用步骤四得出的扩展卡尔曼滤波结果修正步骤一中SINS输出的姿态矩阵速度V_x，V_y，V_z、位置L，λ，h以及陀螺的输出值ω'、加速度计的输出值acc'，最后得到导航参数的最优估计即飞行器的姿态角、速度、位置的信息；

修正过程如下：

姿态矩阵修正：

其中，由姿态失准角得出；为SINS计算出的姿态矩阵；为修正后的姿态矩阵；

速度修正

$\left[\begin{array}{c}{V}_x^{\′}\\ V_y^{\′}\\ V_z^{\′}\end{array}\right]=\left[\begin{array}{c}{V}_x-\mathrm{\δ}{V}_x\\ V_y-\mathrm{\δ}{V}_y\\ V_z-\mathrm{\δ}{V}_z\end{array}\right]$

其中，V_x'，V_y'，V_z'为修正后的东向，北向和天向的速度；

位置修正

$\left[\begin{array}{c}{L}^{\′}\\ {\mathrm{\λ}}^{\′}\\ h^{\′}\end{array}\right]=\left[\begin{array}{c}L-\mathrm{\δL}\\ \mathrm{\λ}-\mathrm{\δ\λ}\\ h-\mathrm{\δh}\end{array}\right]$

其中，L'，λ'，h'为修正后的纬度，经度和高度；

陀螺角速率补偿

ω′＝ω-ε

其中，ω为陀螺输出的角速率，ω'为修正后的陀螺角速率， $\mathrm{\ϵ}=\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b\end{array}\right]$ 为陀螺常值漂移；

加速度补偿

acc′＝acc-▽

其中，acc为加速度计输出的加速度，acc'为修正后的加速度， $\▿=\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]$ 为加速度计零偏；

步骤六、在GPS与SINS的组合时刻，按照上述步骤一至步骤五的方法，对飞行器的姿态角、速度和位置进行更新；在非组合时刻，姿态角、位置和速度由SINS直接给出。

2.根据权利要求1所述的一种基于SINS/GPS的组合导航方法，其特征在于：在步骤1中，当GPS数据没有给出的非组合时刻，yawerror＝0，总误差totalerror＝ω_RProllpitcherror。

3.根据权利要求1所述的一种基于SINS/GPS的组合导航方法，其特征在于：整个导航解算过程中，PI控制器对角速率的补偿频率与SINS的更新频率相同，且大于扩展卡尔曼滤波对角速率的修正频率。

4.根据权利要求1所述的一种基于SINS/GPS的组合导航方法，其特征在于：在SINS/GPS组合时刻，角速率的修正在扩展卡尔曼滤波方法对陀螺进行误差补偿的基础上加入PI控制器对角速率再次进行补偿；而在非组合时刻，陀螺输出的角速率值由PI控制器进行修正。