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

摘要

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

主权项

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_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分别为航向角误差，横滚俯仰角误差的权重系数；比例(P)补偿：ω_Proportion＝K_Ptotalerror积分(I)补偿：ω_Integral＝ω_Integral+K_ItotalerrorΔt其中，K_P为比例系数；K_I为积分系数；Δt为采样间隔；对角速率ω进行PI补偿，得到补偿后的角速率为ω_correction＝ω_Pr oportion+ω_Integral将补偿后的角速率ω_correction作为SINS的输入进行运算，得到三个姿态角：航向角ψ，俯仰角θ和横滚角γ；东向速度V_x，北向速度V_y，天向速度V_z；纬度L，经度λ，高度h；步骤二、在方位误差角为大失准角时，推导出姿态非线性误差方程、速度非线性误差方程以及位置误差方程；从中选取状态变量，建立组合导航系统的非线性误差模型；姿态非线性误差方程为：${\stackrel{·}{\phi }}_x=-(1-{\cos \phi }_z)\frac{V_y}{R_m+h}-{\sin \phi }_z({\omega }_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})+{\phi }_y({\omega }_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)$$-\frac{1}{R_m+h}{\mathrm{\delta V}}_y+\frac{V_y}{{(R_m+h)}^2}\mathrm{\delta h}+{ϵ}_x^n$${\stackrel{·}{\phi }}_y=-{\sin \phi }_z\frac{V_y}{R_m+h}-(1-{\cos \phi }_z)({\omega }_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h})-{\phi }_x({\omega }_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)-{\omega }_{\mathrm{ie}}\sin \mathrm{L\delta L}$$+\frac{1}{R_n+h}{\mathrm{\delta V}}_x-\frac{V_x}{{(R_m+h)}^2}\mathrm{\delta h}+{ϵ}_y^n$${\stackrel{·}{\phi }}_z({\phi }_y\cos {\phi }_z+{\phi }_x\sin {\phi }_z)\frac{V_y}{R_m+h}-({\phi }_y\sin {\phi }_z-{\phi }_x\cos {\phi }_z)({\omega }_{\mathrm{ie}}\cos L+\frac{V_x}{R_m+h})+({\omega }_{\mathrm{ie}}\cos L+\frac{V_x{\sec }^{2}L}{R_n+h})\mathrm{\delta L}$$+\frac{\tan L}{R_n+h}{\mathrm{\delta V}}_x-\frac{V_x\tan L}{{(R_n+h)}^2}\mathrm{\delta h}+{ϵ}_z^n$速度非线性误差方程为：${\delta \stackrel{·}{V}}_x=\left(\cos {\phi }_z-1\right)f_x+\sin {\phi }_z{f}_y-{\phi }_y{f}_z+\frac{V_y\tan L-V_z}{R_n+h}{\mathrm{\delta V}}_x+({2\omega }_{\mathrm{ie}}\sin L+\frac{V_x}{R_n+h}\tan L)\delta V_y$$-({2\omega }_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h}){\mathrm{\delta V}}_z+[{2\omega }_{\mathrm{ie}}(V_z\sin L+V_y\cos L)+\frac{V_x{V}_y{\sec }^{2}L}{R_n+h}]\mathrm{\delta L}+\frac{V_x{V}_z-V_x{V}_y\tan L}{{(R_n+h)}^2}\mathrm{\delta h}+{▿}_x^n$${\delta \stackrel{·}{V}}_y=-\sin {\phi }_z{f}_x+({\cos \phi }_z-1)f_y+{\phi }_x{f}_z-(\frac{{2V}_x\tan L}{R_n+h}+{2\omega }_{\mathrm{ie}}\sin L){\mathrm{\delta V}}_x-\frac{V_z}{R_m+h}{\mathrm{\delta V}}_y-\frac{V_y}{R_m+h}{\mathrm{\delta V}}_z$$-({2V}_x{\omega }_{\mathrm{ie}}\cos L+\frac{{V_x}^2}{R_n+h}{\sec }^{2}L)\mathrm{\delta L}+(\frac{V_y{V}_z}{{(R_m+h)}^2}+\frac{{V_x}^2\tan L}{{(R_n+h)}^2})\mathrm{\delta h}+{▿}_y^n$${\delta \stackrel{·}{V}}_z=({\phi }_y\cos {\phi }_z+{\phi }_x\sin {\phi }_z)f_x+({\phi }_y\sin {\phi }_z-{\phi }_x\cos {\phi }_z)f_y+2({\omega }_{\mathrm{ie}}\cos L+\frac{V_x}{R_n+h}){\mathrm{\delta V}}_x$$+\frac{{2V}_y}{R_m+h}{\mathrm{\delta V}}_y-{2V}_x{\omega }_{\mathrm{ie}}\sin \mathrm{L\delta L}-[\frac{{V_y}^2}{{(R_m+h)}^2}+\frac{{V_x}^2}{{(R_n+h)}^2}]\mathrm{\delta h}+{▿}_z^n$位置误差方程为：$\delta \stackrel{·}{L}=\frac{1}{R_m+h}{\mathrm{\delta V}}_y-\frac{V_y}{{(R_m+h)}^2}\mathrm{\delta h}$$\delta \stackrel{·}{\lambda }=\frac{\sec L}{R_n+h}{\mathrm{\delta V}}_x+\frac{V_x\tan L\sec L}{R_n+h}\mathrm{\delta L}-\frac{V_x\sec L}{{(R_n+h)}^2}\mathrm{\delta h}$$\delta \stackrel{·}{h}={\mathrm{\delta V}}_z$陀螺、加速度计误差方程为：$\begin{array}{cc}\stackrel{·}{ϵ}=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}{ϵ}_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},$加速度计零偏为$\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}{\phi }_x& {\phi }_y& {\phi }_z& \delta V_x& \delta V_y& \delta V_z& \mathrm{\delta L}& \mathrm{\delta \lambda }& \mathrm{\delta h}& {ϵ}_x^b& {ϵ}_y^b& {ϵ}_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}\\ \lambda -\lambda \_\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{\Delta t})=X\left(t\right)+f\left(X\left(t\right)\right)\mathrm{\Delta t}+\frac{\partial f\left(X\left(t\right)\right)}{\partial X}f\left(X\left(t\right)\right)\frac{\Delta 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{\Delta t}+\frac{{\mathrm{\Delta 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^{\prime }\\ V_y^{\prime }\\ V_z^{\prime }\end{array}\right]=\left[\begin{array}{c}{V}_x-\delta V_x\\ V_y-\delta V_y\\ V_z-\delta V_z\end{array}\right]$其中，V′_x，V′_y，V′_z为修正后的东向，北向和天向的速度；位置修正$\left[\begin{array}{c}{L}^{\prime }\\ {\lambda }^{\prime }\\ h^{\prime }\end{array}\right]=\left[\begin{array}{c}L-\mathrm{\delta L}\\ \lambda -\mathrm{\delta \lambda }\\ h-\mathrm{\delta h}\end{array}\right]$其中，L′，λ′，h′为修正后的纬度，经度和高度；陀螺角速率补偿ω＝ω′-ε其中，ω′为陀螺输出的角速率，ω为修正后的陀螺角速率，$ϵ=\left[\begin{array}{ccc}{ϵ}_x^b& {ϵ}_y^b& {ϵ}_z^b\end{array}\right]$为陀螺常值漂移；加速度补偿$\mathrm{acc}=a{\mathrm{cc}}^{\prime }-▿$其中，acc′为加速度计输出的加速度，acc为修正后的加速度，$▿=\left[\begin{array}{ccc}{▿}_x^b& {▿}_y^b& {▿}_z^b\end{array}\right]$为加速度计零偏；步骤六、在GPS与SINS的组合时刻，按照上述步骤一至步骤五的方法，对飞行器的姿态角、速度和位置进行更新；在非组合时刻，姿态角、位置和速度由SINS直接给出。