[发明专利]一种适用于极区的惯导系统综合校正方法

摘要

本发明公开了能够实现极区航行运载器在无阻尼条件下进行综合校正的一种适用于极区的惯导系统综合校正方法。本方法通过以横地理坐标系作为导航坐标系，在极区范围内通过横坐标系转换，将导航参数转换到横地理坐标系下。以横地理坐标系下的惯导输出误差量作为状态量，速度作为外部观测量，建立卡尔曼滤波模型，估测出水平误差角并引入到传统两点校正算法中，在无阻尼状态条件下完成极区惯导系统综合校正过程。本发明在无阻尼条件下实现极区航行运载器综合校正，解除了传统综合校正方案对运载器航行方式的限制，提高了极区惯导系统工作的灵活性。

主权项

一种适用于极区的惯导系统综合校正方法，其特征在于，包括以下几个步骤：步骤一：将船用惯导系统输出的导航参数量转换到横地理坐标系下，导航参数包括：经纬度值、航向、速度和姿态信息；地理坐标系与横地理坐标系的转换矩阵为：$C_n^{\stackrel{\‾}{n}}=C_i^{\stackrel{\‾}{n}}\·C_n^i$$C_i^{\stackrel{\‾}{n}}=\left[\begin{array}{ccc}\cos \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ω}t& \cos \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ω}t& -\sin \stackrel{\‾}{\mathrm{\λ}}\\ -\sin \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ω}t-\cos \stackrel{\‾}{L}\sin \mathrm{\Ω}t& \cos \stackrel{\‾}{L}\cos \mathrm{\Ω}t-\sin \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ω}t& -\sin \stackrel{\‾}{L}\cos \stackrel{\‾}{\mathrm{\λ}}\\ \cos \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\cos \mathrm{\Ω}t-\sin \stackrel{\‾}{L}\sin \mathrm{\Ω}t& \cos \stackrel{\‾}{L}\sin \stackrel{\‾}{\mathrm{\λ}}\sin \mathrm{\Ω}t+\sin \stackrel{\‾}{L}\cos \mathrm{\Ω}t& \cos \stackrel{\‾}{L}\cos \stackrel{\‾}{\mathrm{\λ}}\end{array}\right]$$C_n^i=\left[\begin{array}{ccc}-\sin (\mathrm{\λ}+\mathrm{\Ω}t)& -\sin L\cos (\mathrm{\λ}+\mathrm{\Ω}t)& \cos L\cos (\mathrm{\λ}+\mathrm{\Ω}t)\\ \cos (\mathrm{\λ}+\mathrm{\Ω}t)& -\sin L\sin (\mathrm{\λ}+\mathrm{\Ω}t)& \cos L\sin (\mathrm{\λ}+\mathrm{\Ω}t)\\ 0& \cos L& \sin L\end{array}\right]$表示惯性坐标系到横地理坐标系的转换矩阵，表示地理坐标系到惯性坐标系的转换矩阵，Ω为地球自转角速度，t为时间，λ为地理坐标系下的经度值，横地理坐标系下的经纬度值为：$\stackrel{\‾}{L}=arcsin\left(\cos Lsin\mathrm{\λ}\right)$$\stackrel{\‾}{\mathrm{\λ}}=arctan\left(\frac{\cos Lcos\mathrm{\λ}}{\sin L}\right)$L为地理坐标系下的纬度值，为横地理坐标系下的纬度值，为横地理坐标系下的经度值，横地理坐标系下航向与速度为：$\stackrel{\‾}{K}=K+arctan\frac{sin\mathrm{\λ}}{\sin L\cos \mathrm{\λ}}-\mathrm{\π}$$\stackrel{\‾}{V}=\left[\begin{array}{c}{\stackrel{\‾}{V}}_x\\ {\stackrel{\‾}{V}}_y\end{array}\right]=\left[\begin{array}{c}V\sin \stackrel{\‾}{K}\\ V\cos \stackrel{\‾}{K}\end{array}\right]$K表示地理坐标系下的航向，为横地理坐标系下的航向，π为圆周率，V为地理坐标系下的速度，为横地理坐标系下的速度，为横地理坐标系下的东向速度，为横地理坐标系下的北向速度；步骤二：利用得到的横地理坐标系下的导航参数进行横地理坐标系下的导航机械编排，确定系统的状态变量和量测量；步骤三：根据系统的状态变量和量测量建立卡尔曼滤波器，由卡尔曼滤波方法得到横地理坐标系下的导航系统的x轴姿态误差角和y轴姿态误差角步骤四：将得到的x轴姿态误差角和y轴姿态误差角引入到导航系统的两点校正中，完成极区两点校正；所述的步骤二中，导航机械编排为：$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{L}}=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_y}{R}\\ \mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{\mathrm{\λ}}}=\frac{{\stackrel{\‾}{V}}_x}{R}\mathrm{\δ}\stackrel{\‾}{L}\tan \stackrel{\‾}{L}\sec \stackrel{\‾}{L}+\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}\sec \stackrel{\‾}{L}\\ \mathrm{\δ}{\stackrel{\·}{\stackrel{\‾}{V}}}_x={\stackrel{\‾}{\mathrm{\φ}}}_z{\stackrel{\‾}{f}}_y+\frac{{\stackrel{\‾}{V}}_y}{R}\tan \stackrel{\‾}{L}\·\mathrm{\δ}{\stackrel{\‾}{V}}_x+(2\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L})\·\mathrm{\δ}{\stackrel{\‾}{V}}_y+(2\mathrm{\Ω}\cos \stackrel{\‾}{L}\·{\stackrel{\‾}{V}}_y+\frac{{\stackrel{\‾}{V}}_x{\stackrel{\‾}{V}}_y}{R}{\sec }^2\stackrel{\‾}{L})\·\mathrm{\δ}\stackrel{\‾}{L}+\mathrm{\Δ}{\stackrel{\‾}{A}}_x\\ \mathrm{\δ}{\stackrel{\·}{\stackrel{\‾}{V}}}_y=-{\stackrel{\‾}{\mathrm{\φ}}}_z{\stackrel{\‾}{f}}_x-(2\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{2{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L})\·\mathrm{\δ}{\stackrel{\‾}{V}}_x-(2\mathrm{\Ω}\cos \stackrel{\‾}{L}{\stackrel{\‾}{V}}_x+\frac{{\left({\stackrel{\‾}{V}}_x\right)}^2}{R}{\sec }^2\stackrel{\‾}{L})\·\mathrm{\δ}\stackrel{\‾}{L}+\mathrm{\Δ}{\stackrel{\‾}{A}}_y\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_x=-\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_y}{R}+(\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x\tan \stackrel{\‾}{L}}{R}){\stackrel{\‾}{\mathrm{\φ}}}_y-(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}){\stackrel{\‾}{\mathrm{\φ}}}_z+{\stackrel{\‾}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_y=-\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}-\mathrm{\Ω}\sin \stackrel{\‾}{L}\mathrm{\δ}\stackrel{\‾}{L}-(\mathrm{\Ω}\sin \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x\tan \stackrel{\‾}{L}}{R}){\stackrel{\‾}{\mathrm{\φ}}}_x-\frac{{\stackrel{\‾}{V}}_y}{R}{\stackrel{\‾}{\mathrm{\φ}}}_z+{\stackrel{\‾}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\stackrel{\‾}{\mathrm{\φ}}}}_z=\frac{\mathrm{\δ}{\stackrel{\‾}{V}}_x}{R}\tan \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_y}{R}{\stackrel{\‾}{\mathrm{\φ}}}_y+(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}{\sec }^2\stackrel{\‾}{L})\mathrm{\δ}\stackrel{\‾}{L}+(\mathrm{\Ω}\cos \stackrel{\‾}{L}+\frac{{\stackrel{\‾}{V}}_x}{R}){\stackrel{\‾}{\mathrm{\φ}}}_x+{\stackrel{\‾}{\mathrm{\ϵ}}}_z\end{array}\right.$分别为横纬度误差和横经度误差，为当地横地理纬度，分别为载体的横东向速度误差和横北向速度误差，分别为横地理坐标系下东向和北向加速度计误差，分别为横地理坐标系下加速度计x轴、y轴和z轴测量的比力信息，分别为横地理坐标系下载体x轴、y轴和z轴姿态误差角，分别为横地理坐标系下x轴、y轴和z轴陀螺漂移，系统的状态变量为：$X={\left[\begin{array}{cccccccccccc}\mathrm{\δ}\stackrel{\‾}{L}& \mathrm{\δ}\stackrel{\‾}{\mathrm{\λ}}& \mathrm{\δ}{\stackrel{\‾}{V}}_x& \mathrm{\δ}{\stackrel{\‾}{V}}_y& {\stackrel{\‾}{\mathrm{\φ}}}_x& {\stackrel{\‾}{\mathrm{\φ}}}_y& {\stackrel{\‾}{\mathrm{\φ}}}_z& {\stackrel{\‾}{\mathrm{\ϵ}}}_x& {\stackrel{\‾}{\mathrm{\ϵ}}}_y& {\stackrel{\‾}{\mathrm{\ϵ}}}_z& \mathrm{\Δ}{\stackrel{\‾}{A}}_x& \mathrm{\Δ}{\stackrel{\‾}{A}}_y\end{array}\right]}^T$系统的量测量为：$Z=\left[\begin{array}{c}{\stackrel{\‾}{V}}_{sx}-{\stackrel{\‾}{V}}_{dx}\\ {\stackrel{\‾}{V}}_{sy}-{\stackrel{\‾}{V}}_{dy}\end{array}\right]=\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\‾}{V}}_x\\ \mathrm{\δ}{\stackrel{\‾}{V}}_y\end{array}\right]={\stackrel{\‾}{V}}_s-{\stackrel{\‾}{V}}_d$为横地理坐标系下惯导输出速度，为横地理坐标系下电磁计程仪输出速度，为惯导输出速度在横东向上分量，为惯导输出速度在横北向上分量，为电磁计程仪输出速度在横东向上分量，为电磁计程仪输出速度在横北向上分量，为横东向速度误差，为横北向速度误差；所述的步骤四中，将得到的x轴姿态误差角和y轴姿态误差角引入到导航系统的两点校正中，得到：$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\‾}{L}+{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_x\\ \mathrm{\δ}\stackrel{\‾}{\mathrm{\λ}}-\frac{{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_y}{\cos \stackrel{\‾}{L}}\\ \mathrm{\δ}\stackrel{\‾}{K}-{\widehat{\stackrel{\‾}{\mathrm{\φ}}}}_y\tan \stackrel{\‾}{L}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& -\sec \stackrel{\‾}{L}& 0\\ 0& -\tan \stackrel{\‾}{L}& 1\end{array}\right]{\left[\begin{array}{c}{\mathrm{\ψ}}_x\\ {\mathrm{\ψ}}_y\\ {\mathrm{\ψ}}_z\end{array}\right]}^{\stackrel{\‾}{n}}$为横地理坐标系下的航向误差，ψ_x、ψ_y、ψ_z为惯导系统平台漂移角。