[发明专利]一种基于SINS/GPS的组合导航方法

权利要求书

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

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

将飞行器的姿态矩阵记为Cbn=C11C12C13C21C22C23C31C32C33]]>

飞行器的航向角误差为：

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；

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

姿态非线性误差方程为：

φ·x=-(1-cosφz)VyRm+h-sinφz(ωiecosL+VxRn+h)+φy(ωiesinL+VxRn+htanL)]]>

-1Rm+hδVy+Vy(Rm+h)2δh+ϵxn]]>

φ·y=-sinφzVyRm+h-(1-cosφz)(ωiecosL+VxRn+h)-φx(ωiesinL+VxRn+htanL)-ωiesinLδL]]>

+1Rn+hδVx-Vx(Rm+h)2δh+ϵyn]]>

φ·z(φycosφz+φxsinφz)VyRm+h-(φysinφz-φxcosφz)(ωiecosL+VxRm+h)+(ωiecosL+Vxsec2LRn+h)δL]]>

+tanLRn+hδVx-VxtanL(Rn+h)2δh+ϵzn]]>

速度非线性误差方程为：

δV·x=(cosφz-1)fx+sinφzfy-φyfz+VytanL-VzRn+hδVx+(2ωiesinL+VxRn+htanL)δVy]]>

-(2ωiecosL+VxRn+h)δVz+[2ωie(VzsinL+VycosL)+VxVysec2LRn+h]δL+VxVz-VxVytanL(Rn+h)2δh+▿xn]]>

δV·y=-sinφzfx+(cosφz-1)fy+φxfz-(2VxtanLRn+h+2ωiesinL)δVx-VzRm+hδVy-VyRm+hδVz]]>

-(2VxωiecosL+Vx2Rn+hsec2L)δL+(VyVz(Rm+h)2+Vx2tanL(Rn+h)2)δh+▿yn]]>

δV·z=(φycosφz+φxsinφz)fx+(φysinφz-φxcosφz)fy+2(ωiecosL+VxRn+h)δVx]]>

+2VyRm+hδVy-2VxωiesinLδL-[Vy2(Rm+h)2+Vx2(Rn+h)2]δh+▿zn]]>

位置误差方程为：

δL·=1Rm+hδVy-Vy(Rm+h)2δh]]>

δλ·=secLRn+hδVx+VxtanLsecLRn+hδL-VxsecL(Rn+h)2δh]]>

δh·=δVz]]>

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

ϵ·=0▿·=0]]>

其中，为位置速率，为地球自转速率；φ_x，φ_y，φ_z为东向，北向，方位失准角；δV_x，δV_y，δV_z为东向、北向、天向速度误差；δL，δλ，δh为纬度、经度、高度误差；f_x，f_y，f_z为东向，北向，天向的比力；为东向，北向，天向的陀螺常值漂移；为东向，北向，天向的加速度计零偏；R_m为地球子午圈上的曲率半径，R_n为地球卯酉圈上的曲率半径；陀螺常值漂移为ϵxn=C11ϵxb+C12ϵyb+C13ϵzbϵyn=C21ϵxb+C22ϵyb+C23ϵzbϵzn=C31ϵxb+C32ϵyb+C33ϵzb,]]>加速度计零偏为▿xn=C11▿xb+C12▿yb+C13▿zb▿yn=C21▿xb+C22▿yb+C23▿zb▿zn=C31▿xb+C32▿yb+C33▿zb;]]>

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

X(t)=φxφyφzδVxδVyδVzδLδλδhϵxbϵybϵzb▿xb▿yb▿zb]]>

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

X·(t)=f(X(t))+W(t)]]>

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

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

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

Z(t)=Vx-Vx_GPSVy-Vy_GPSVz-Vz_GPSL-L_GPSλ-λ_GPSh-h_GPS]]>

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

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

X(t+Δt)=X(t)+f(X(t))Δt+∂f(X(t))∂Xf(X(t))Δt22]]>

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

X(k+1)=X(k)+f(X(k))Δt+Δt22A(X(k))f(X(k))+W(k)]]>

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

其中，Δt为采样间隔；

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

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

修正过程如下：

姿态矩阵修正：

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

速度修正

Vx′Vy′Vz′=Vx-δVxVy-δVyVz-δVz]]>

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

位置修正

L′λ′h′=L-δLλ-δλh-δh]]>

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

陀螺角速率补偿

ω＝ω′-ε

其中，ω′为陀螺输出的角速率，ω为修正后的陀螺角速率，ϵ=ϵxbϵybϵzb]]>为陀螺常值漂移；

加速度补偿

acc=acc′-▿]]>

其中，acc′为加速度计输出的加速度，acc为修正后的加速度，▿=▿xb▿yb▿zb]]>为加速度计零偏；

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