[发明专利]基于有效节点筛选的AUV地形匹配定位方法

摘要

本发明的目的在于提供基于有效节点筛选的AUV地形匹配定位方法，根据推算导航的输出结果确定搜索范围，再确定搜索范围后再利用测量地形数据与先验地形进行初步的匹配定位并获得匹配后的地形测量误差估计，然后分别计算节点的有效性和二次搜索范围，根据有效节点矩阵和二次搜索范围进行二次精确匹配定位，最后输出匹配定位结果。本发明所采用的初匹配与精细搜索匹配相结合的匹配定位方法有效的增加了搜索配准的效率同时提高定位精度。

主权项

基于有效节点筛选的AUV地形匹配定位方法，其特征是：(1)初匹配搜索范围确定：根据推算导航的输入信息,搜索区间由下式确定：xt-(λxlx+3Px)<(XS1)x<xt+(λxlx+3Px)yt-(λyly+3Py)<(XS1)y<yt+(λyly+3Py)]]>其中：xt和yt表示推算导航的位置，由AUV的导航系统给出；λx和λy表示导航偏差占载体的运行里程的百分比，由参导航系统的统计结果事先给出；lx和ly表示载体在x方向和y方向的运行里程；Px和Py表示推算导航在x方向和y方向的误差，由推算导航计算得到；以(xt,yt)为中心，2lxλx和2lyλy为x方向和y方向的长度构成的矩形区域就是搜索区间(2)初匹配定位、潮差估计：根据步骤(1)中计算得到的搜索范围XS开始搜索匹配，将搜索步长调整到1～2个网格边长，由于先验地形和实时测量地形的潮位差不确定，需要对其进行实时的估计，假设潮差为htide，搜索点ij的先验地形与测量地形的高度偏差序列：假设地形高度偏差序列满足白噪声分布而潮差是常量，得到搜索点ij的潮差和地形测量误差估计：tij=mean(Δhij)σij=std(Δhij)]]>得到了潮差的估计值之后，计算测量地形和先验地形的真实相似程度：其中：h(xij,yij)表示测量序列点(xij,yij)在先验地形图中的插值结果，zij∈Z，Z表示测量地形，zij表示测量地形内的点；查找似然函数Lij值最大的位置得到地形匹配定位的位置同时获得地形匹配定位位置的先验地形和测量地形的高度偏差并计算其均值和标准差，将其标点为定位点位置的潮差估计和测量地形的误差估计(3)估计定位点的测量误差、剔除误差节点：通过步骤(2)得到定位点后，计算测量序列和先验地形序列在位置的高度差并计算其均值式中：表示测量序列在先验地形图位置处的地形插值高度序列；计算统计量：得到误差的统计结果后对误差节点进行筛选，误差节点选择准则：式中：表示偏差序列中ij号节点；Fij表示测量节点的有效性标记矩阵；(4)计算二次匹配搜索区域：根据步骤(3)中计算得到的潮差和测量误差估计计算分布的置信度为α的分位点再计算似然函数下界Lminshrehold：以Lminshrehold为等高平面截取步骤(2)中的似然函数Lij获得的截平面为定位点的置信区间中的x和y的最大值与最小值构成的矩形区域就是二次搜索匹配的搜索区间：min((xs2)x)≤(Xs2)x≤max((xs2)x)min((xs2)y)≤(Xs2)y≤max((xs2)y);]]>(5)二次精确搜索匹配：通过步骤(4)得到搜索区间采用1～0.5网格边长的搜索步长，并根据步骤(3)得到的节点标记矩阵F，步骤(2)得到潮差估计tp测量误差估计σp，计算每一个搜索点的测量地形与先验地形高度差Δhij，再利用极大似然估计方法：查找取得最大值时的搜索点坐标得到二次精确搜索匹配的结果：得到定位位置