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

摘要

本发明公开了一种基于水下地形高程数据库的水下航行器辅助导航定位方法，其主要目的在于解决水下航行器长时间航行时由于惯性导航定位系统的误差会随着时间的积累逐渐增大的固有缺陷以至于不能满足航行器长时间精确导航定位需要的问题。本发明的主要步骤包括：主惯导系统指示航迹序列和实测高程序列的获取、匹配定位预处理、一级匹配定位、二级匹配定位以及修正过程。本发明可以解决水下航行器长时间航行所带来的初始位置误差过大致使匹配失败问题以及匹配过程中由于惯性器件的误差所引起的匹配误差问题，从而修正主惯导系统，提高导航定位精度。

主权项

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₂₀″′的差值，经过滤波计算，估计出惯性导航系统位置误差量，用估计出的位置误差去修正惯性导航系统输出的位置信息，作为导航位置信息输出。