[发明专利]一种基于水下地形高程数据库的水下航行器辅助导航定位方法

权利要求书

1.一种基于水下地形高程数据库的水下航行器辅助导航定位方法，其特征为：

步骤1：获取主惯导系统指示航迹序列和实测高程序列

当水下航行器驶入匹配区时，在主惯性导航系统INS的导航下航行一段距离，与此同时每隔一个时间段Times，Times的典型值为10分钟，通过航行器的高程测量装置测量高程，由此得到惯性导航系统给出的一个惯导系统指示航迹序列Pi=LatiPiLongtiPi,]]>i=1，2…N，N代表序列点的个数，取值为20，代表惯导系统指示航迹序列P_i的纬度，代表惯导系统指示航迹序列P_i的经度，和一个由高程测量装置给出的对应于惯导系统指示航迹序列P_i的实测高程序列D_i，i=1，2…N，考虑高程测量装置的误差可得：

Di=D~i+γ,i=1,2...N]]>

其中表示理想高程序列，γ表示高程测量装置的测量噪声序列，服从均值为0标准差为1的正态分布。

步骤2：匹配定位预处理

在加载地形高程数据库中，选择以惯导系统指示航迹序列P_i为中心、以惯性导航系统6倍误差为边长的区域，将此区域的高程数据存入一个预处理二维矩阵，预处理二维矩阵行标为row，1≤row≤row_max，row_max为所述区域行标的最大值，预处理二维矩阵列标为col，1≤col≤col_max，col_max为所述区域列标的最大值，矩阵中的值存为储高程数据；所述区域的起始纬度为lati₀，起始经度为longti₀，矩阵行列交点row，col处的经度、纬度分别为：lati₀+(row-1)×grid，longti₀+(col-1)×grid，grid为地形数据库中相邻高程数据的位置间距，考虑地形高程数据库建库时测量和量化所产生的误差可得：

DB(row,col)=DB~(row,col)+η]]>

其中row为预处理二维矩阵的行标，col为预处理二维矩阵的列标，DB(row，col)表示预处理二维矩阵中行标为row、列标为col的高程数据，表示对应于DB(row，col)的理想高程数据，η表示地形高程数据库建库时的测量和量化噪声，服从均值为0标准差为1的正态分布；

将实测高程序列D_i和在DB(row，col)中所提取的高程序列作相关运算，所述的相关运算采用均方差MSD算法，使用去均值后的数据进行相关运算：

Ral(row,col)=Σi=1N[DB(row,col)-DBA(row,col)-Di+DA]2]]>

其中，Ral(row，col)表示MSD相关运算的结果，DB(row，col)表示惯导系统指示航迹P_i平移至预处理二维矩阵row，col对应位置lati₀+(row-1)×grid，longti₀+(col-1)×grid处高程数据序列，DBA(raw，col)为DB(row，col)中所提取的高程序列的均值，D_i表示实测高程序列，DA表示实测序列的均值；

运算结束后，取得使Ral(row，col)最小的row，col，则将惯导系统指示航迹P_i平移至预处理二维矩阵row，col对应位置lati₀+(row-1)×grid，longti₀+(col-1)×grid，得到预处理位置序列为Pi′=LatiPi′LongtiPi′,]]>表示得到预处理位置序列P′_i的纬度，表示得到预处理位置序列P′_i的经度；

步骤3：一级匹配定位

将地形高程数据库中以预处理位置序列P′_i为中心，以惯性导航系统3倍误差为边长的一级匹配区域内的高程数据存入一个一级匹配二维矩阵，并利用双线性插值算法在该一级匹配区域提取高程为实测高程D_i的一级等高线C_i；

步骤3.1：

惯导系统给出的航向误差为ψ_INS，纬度和经度误差分别为t_INSlati和t_INSlongti，分别在-3ψ_INS～3ψ_INS，--3t_INSlati～3t_INSlati，--3t_INSlongti～3t_INSlongti范围内分别随机取一个航向误差、一个纬度和一个经度且值分别为：ψ_INSrand，t_INSlatirand，t_{INSlongtirand}；对预处理位置序列P′_i作一级随机旋转和一级随机平移变换，得到一级随机旋转和平移位置序列P′_iand：

Pirand′=cosψINSrand-sinψINSrandsinψINSrandcosψINSrandPi′+tINSlatirandtINSlongtirand]]>

步骤3.2：

通过一级随机旋转和平移位置序列P′_irand向一级等高线C_i作垂线，得到一个一级匹配垂足序列Y_i，求取一个刚性变换Trans且所述刚性变换Trans包括旋转矩阵Rotation=cosφICCP-sinφICCPsinφICCPcosφICCP]]>和平移矢量translation=tICCPLatitICCPLongti,]]>φ_ICCP为一级匹配旋转角，t_ICCPLati代表纬度偏移量，t_ICCPLongti代表经度偏移量，使得一级随机旋转和平移位置序列P′_irand逐渐逼近一级匹配垂足序列Y_i，也就是使以下目标函数最小：

Objectitetation(Yi,Trans·Pirand′)=Σi=1Nweighti||Yi-Trans·Pirand′||2iteration≥10iteration=0]]>

weighti=1-Dist(Pirand′,Yi)Distmax]]>

其中Object为目标函数，iteration代表第iteration次迭代，iteration=0表示迭代未开始，weighti代表序列点的权值，Trans为刚性变换，Dist(P′_irand，Y_i)为P′_irand和Y_i之间的距离，Dist_max为P′_irand和Y_i序列点之间距离的最大值；

求取刚性变换Trans采用四元数法；

设四元数为quaternion：

quaternion=(qua1,qua2,qua3,qua4)=(cosφICCP2,0,0,sinφICCP2)]]>

qua₁为四元数quaternion的第一个元素，qua₂为四元数quaternion的第二个元素，qua₃为四元数quaternion的第三个元素，qua₄为四元数quaternion的第四个元素，φ_ICCP为一级匹配旋转角；

构造矩阵Mat_ICCP：

MatICCP=Mat11Mat12Mat21Mat22=Σi=1Nweighti(Yi-Y-i)(Pirand′-P-irand′)T]]>

Mat₁₁为矩阵Mat_ICCP第一行第一列的元素，Mat₁₂为矩阵Mat_ICCP第一行第二列的元素，Mat₂₁为矩阵Mat_ICCP第二行第一列的元素，Mat₂₃为矩阵Mat_ICCP第二行第二列的元素，和分别为一级匹配垂足序列的质心、随机旋转和平移位置序列的质心，weight_i代表序列点的权值，T代表转置；

为求解旋转矩阵Rotation而构造一个对称的哈密尔顿矩阵：

Hamilton=Mat11+Mat2200Mat21-Mat120Mat11-Mat22mat12+Mat2100Mat12+Mat21Mat22-Mat110Mat21-Mat1200-Mat11-Mat22]]>

求解并得到哈密尔顿矩阵Hamilton的四个特征值

记最大的特征值为最大的特征值为可使下式成立：

由此可得一级匹配旋转角为：

所以旋转矩阵Rotation为：

Rotation=cosφICCP-sinφICCPsinφICCPcosφICCP]]>

平移量translation为：

translation=Y-i-Rotation·P-irand′]]>

步骤3.3：

利用步骡3.2得到的包括旋转矩阵Rotation和平移矢量translation的刚性变换Trans，对一级随机旋转和平移后的位置序列P′_irand作P′_irand=Trans·P′_irand处理，此时，若迭代次数iteration大于最大迭代次数iteration_max，表明收敛速度过慢，舍弃此次迭代的结果，返回步骤3.1；若iteration小于等于iteration_max并且|Object_iteration-Object_iretration-1|＞τ，则返回步骤3.2，若iteration小于等于iteration_max并且|Object_iteation-Object_ireration-1|≤τ，则迭代终止并记录Object_itetation和Trans·P′_irand的值，iteration_max值为100，τ值为10^-6；

步骤3.4：

重复3.1-3.3步骤，得到50个Object_itetation(Y_i，Trans·P′_irand)值后，找出其中最小的Object_itetation(Y_i，Trans·P′_irand)对应的Trans·P′_irand，得到一级匹配位置序列Pi′′=Trans·Pirand′=LatiP′′LongtiP′′,]]>代表一级匹配位置序列的纬度，代表一级匹配序列的经度；

步骤4：二级匹配定位

选择以加载地形高程数据库中以一级匹配位置序列P″_i为中心、3×grid为边长的区域，将此区域的高程数据存入一个二级匹配二维矩阵，并利用双线性插值算法在该区域提取高程为实测高程D_i的二级等高线C′_i，在二级等高线C′_i上寻找一级匹配位置序列P″_i的对应垂足得到一个二级垂足序列Yi′=LatiYi′LongtiYi′,]]>代表二级垂足序列的纬度，代表二级垂足序列的经度；对二级垂足序列Y′_i到一级匹配位置序列P″_i作仿射变换G且所述仿射变换包括在纬度方向上的平移因子t_affinex，在经度方向上的平移因子t_affiney，缩放因子α_affine，旋转因子θ_affine，e_i为二级垂足序列Y′_i与二级匹配定位的输入序列P″_i的经仿射变换后的坐标的差：

ei=taffinextaffiney+αaffinecosθaffine-αaffinesinθaffineαaffinesinθaffineαaffinecosθaffinePi''-Yi'=10LatiPi''-LongtiPi''01LongtiPi''LatiPi''taffinextaffineyαaffinecosθaffineαaffinesinθaffine-LatiYi'LongtiYi']]>

令仿射变换求解向量为t_affine，仿射变换求解矩阵为

taffine=taffinextaffineyαaffinecosθaffineαaffinesinθaffine]]>

CPi′′=10LatiPi′′-LongtiPi′′01LongtiPi′′LatiPi′′]]>

则ei=CPi′′taffine-Yi′;]]>

欧式平方距离ESD_affine为：

ESDaffine=Σi=1NeiTei=taffineTCPi′′TCPi′′taffine-2Yi′TCPi′′taffine+Yi′TYi′]]>

T代表转置；

将使ESD_affine最小的t_affine记为对ESD_affine求导可得：

dESDaffinedtaffine=2CPi′′TCPi′′taffine-2CPi′′TYi′]]>

令dESDaffinedtaffine=0]]>可得

t^affine=(CPi′′TCPi′′)-1CPi′′TYi′]]>

当ESD_affine对t_affine的二阶导数时，就是使ESD_affine最小的解：

t^affine=(CPi′′TCPi′′)-1CPi′′TYi′=1detlp0-μxpμyp0lp-μyp-μxp-μxp-μypN0μyp-μxp0Nμxqμyqlp+qlp-q]]>

其中：代表一级匹配位置序列的纬度的之和，代表二级垂足序列的纬度之和，代表一级匹配位置序列的经度的之和，代表二级垂足序列的经度之和，代表一级匹配位置序列和二级垂足序列的点积之和，代表一级匹配位置序列和二级垂足序列的叉积之和，代表一级匹配位置序列的模的平方之和，det=N×lp-μxp2-μyp2]]>代表矩阵的模；

由此就可以得到：

t^affine=(t^affinex,t^affiney,α^affinecosθ^affine,α^affinesinθ^affine)T]]>

其中为求解所得的平移因子，为求解所得的缩放因子，为求解所得的旋转因子；

对一级匹配位置序列作仿射变换得二级匹配位置序列P_i″′：

Pi'''=LatiPi'''LongtiPi'''=t^affinext^affiney+α^affinecosθ^affine-α^affinesinθ^affineα^affinesinθ^affineα^affinecosθ^affinePi'']]>

代表二级匹配位置序列的纬度，代表二级匹配位置序列的经度；

步骤5：将二级匹配位置序列P_i″′中的最新位置信息、P₂₀″′作为观测信息，利用卡尔曼滤波器中的间接滤波法估计并修正主惯性导航系统位置误差，卡尔曼滤波器接收惯性导航系统的当前位置信息和最新位置信息P₂₀″′的差值，经过滤波计算，估计出惯性导航系统位置误差量，用估计出的位置误差去修正惯性导航系统输出的位置信息，作为导航位置信息输出。

2.根据权利要求1所述的一种基于水下地形高程数据库的水下航行器辅助导航定位方法，惯性导航系统位置误差量的估计采用如下方法：

步骤5.1：建立惯性导航系统状态方程，选取东北天坐标系O_ENU作为导航坐标系，其中OE为东向轴，ON为北向轴，OU表示天向轴，载体坐标系Oxyz，其中Ox轴沿航行器横轴指向右舷，Oy轴沿航行器纵轴指向前，Oz轴垂直于Ox与Oy轴所确定的平面构成右手坐标系，选取状态向量X_INS为：

其中，δV_E、δV_N、δV_U分别是东向、北向和天向速度误差；φ_E、φ_N、φ_U分别是东向、北向和天向失准角；δLati、δLongti、δh分别是纬度、经度和天向误差；分别是Ox、Oy、Oz向的加速度计偏置；ε_bx、ε_by、ε_bz分别是Ox、Oy、Oz向的陀螺漂移；

建立惯性导航系统线性状态方程为：

X·INS=FINSXINS+WINS]]>

其中：

FINS=F11F12F130-fUfNF170F19c11c21c31000F21-VURadiusn+h-VNRadiusn+hfU0-fEF270F29c21c22c32000F312VURadiusn+h0-fNfE0-2ωieVEsinLongti0F39c13c23c330000-1Radiusn+h00F45F4600VN(Radiusn+h)2000-c11-c21-c311Radiusn+h00F540-VNRadiusn+h-ωiesinLongti0VE(Radiusn+h)2000-c12-c22-c23tanLongtiRadiusn+h00F64VNRadiusn+h0F670VEtanLongti(Radiusn+h)2000-c13-c23-c3301Radiusn+h000000-VN(Radiusn+h)20000001(Radiusn+h)cosLongti00000VEtanLongti(Re+h)cosLongti0-VE(Radiusn+h)2cosLongti00000000100000000000006×15]]>

WINS=ωVEωVNωVUωφEωφNωφUωδLongtiωδLatiωδh000000T]]>

F11=VNtanLongtiRadiusn+h-VURadiuse+h,F12=2ωiesinlongti+VEtanlongtiRadiuse+h]]>

F13=-(2ωiecosLongti+VERadiuse+h),F17=2ωiesinLongti×VU+2ωiecosLongti×VN+VEVN(secLongti)2Radiuse+h]]>

F19=VEVU(Radiuse+h)2-VEVVtanLongti(Radiuse+h)2,F21=-2ωiesinLongti-VEtanLongtiRadiuse+h]]>

F27=-(2ωiecosLongtiVE+VE2(secLongti)2Radiuse+h),F29=VNVU(Radiusn+h)2+VE2tanLongti(Radiuse+h)2]]>

F31=2ωiecosLongti+2VERadiuse+h,F39=-(VN2(Rn+h)2+VE2tanLongti(Radiuse+h)2)]]>

F45=ωiesinLongti+VEtanLongtiRadiuse+h,F46=-(ωiecosLongti+VE(secLongti)2Radiuse+h)]]>

F54=-(ωiesinLongti+VEtanLongtiRadiuse+h),F64=ωiecosLongti+VE(secLongti)2Radiuse+h]]>

F67=ωiecosLongti+VE(secLongti)2Radiuse+h]]>

其中F₁₁为矩阵F_INS的第1行第1列元素，F₁₂为矩阵F_INS的第1行第2列元素，F₁₃为矩阵F_INS的第1行第3列元素，F₁₇为矩阵F_INS的第1行第7列元素，F₁₉为矩阵F_INS的第1行第9列元素，F₂₁为矩阵F_INS的第2行第1列元素，F₂₇为矩阵F_INS的第2行第7列元素，F₂₉为矩阵F_INS的第2行第9列元素，F₃₁为矩阵F_INS的第3行第1列元素，F₃₉为矩阵F_INS的第3行第9列元素，F₄₅为矩阵F_INS的第4行第5列元素，F₄₆为矩阵F_INS的第4行第6列元素，F₅₄为矩阵F_INS的第5行第4列元素，F₆₄为矩阵F_INS的第6行第4列元素，F₆₇为矩阵F_INS的第6行第7列元素，c_ij为姿态转移矩阵的元素，F_INS为系统状态转移矩阵；W_INS为系统随机干扰向量，ω_ie为地球自转角速度，Radius_n、Radius_e分别为沿子午圈和卯四圈的主曲率半径，V_E、V_N及V_U为载体真实东向、北向和天向速度，Longti、Lati及h表示载体所处的真实经度、纬度及高程；卡尔曼滤波用离散化模型来描述系统，采用离散化后的差分方程描述连续系统，离散化后的系统状态方程为：

Xk=Φk,k-1X^k-1+Wk-1]]>

其中X_k为k时刻的系统状态向量，Φ_k,k-1为k-1到k时刻的一步转移矩阵，为系统状态向量的估计，W_k-1为k-1时刻的系统噪声，且

Φk,k-1=I+ΔPerΣj=1σFj-1]]>

I为单位矩阵，ΔPer为将滤波周期Per分成σ个时间间隔ΔPer，F_j-1为F_INS在时刻k-1+(j-1)ΔPer时的值；

步骤5.2：建立观测方程，将二级匹配位置序列P_i′″中的最新位置信息P₂₀′″与惯性导航系统输出的经度Longti_INS、纬度Lati_INS位置信息的差值，高程测量装置实测高程D_i的最新实测高程D₂₀与主惯导系统输出高程h_INS的差值构成位置量测向量：

Zk=LongtiINS-LongtiP20'''LatiINS-LatiP20'''hINS-D20=HkXk-υk]]>

其中：Hk=100003×601003×60001-1]]>为观测矩阵，υ_k为观测噪声；

步骤5.3：卡尔曼滤波器误差估计：

步骤5.3.1时间更新：

X^k,k-1=Φk,k-1X^k-1Pk,k-1=Φk,k-1Pk-1Φk,k-1T+Γk-1Qk-1Γk-1T]]>

步骤5.3.1量测更新：

Kk-1=Pk,k-1HkT(HkPk,k-1HkT+Rk)TX^k=X^k,k-1+Kk(Zk-HkX^k,k-1)Pk=(I-KkHk)Pk,k-1]]>

T代表转置，为对系统状态向量的估计，P_k为状态估计误差的协方差阵，K_k为滤波增益，表示对量测值的一步预测，为新息序列，Qk-1、R_k分别为W_k-1、υ_k的方差阵；给定初始条件X₀和P₀，由Z_k即可得到每一步的状态最优估计

步骤5.4利用当前卡尔曼滤波器得出的误差量最优估计向量中的位置误差量，修正惯性导航系统位置信息，得到修正后的位置信息

L~ongtiINS=LongtiINS+δL^ongti,L~atinINS=LatinINS+δL^ati]]>其中分别是卡尔曼滤波器输出的纬度、经度误差。