[发明专利]一种基于等值线的辅助导航定位方法

摘要

本发明公开了一种基于等值线的辅助导航定位方法，具体地说，当航行器开始进行地形/重力/地磁匹配时，在较大的初始匹配误差下，利用基于价值函数的等值线匹配算法在置信区域内找到一条离实际航迹较为接近的航迹来降低惯性导航系统(INS)的初始定位误差；然后，利用ICCP算法的旋转和平移变换获得航迹向及最近点序列信息；最后，利用得到的航迹向及最近点信息在等值线上采用加密的方法得到最佳的匹配航迹。本发明可以解决当前辅助导航系统在大的初始定位误差情况下易发散的问题，提高辅助导航系统的精度及可靠性，尤其适用于存在探测盲区情况下的辅助导航定位。

主权项

1.一种基于等值线的辅助导航定位方法，其特征在于，包括以下几个步骤：步骤一：根据INS的误差置信范围划定搜索区域；搜索区域是以X为中心，以3倍的INS估计位置误差为半边长的正方形区域，其中，X表示由INS输出的当前的地形/重力/地磁位置点；所述的INS为惯性导航系统；步骤二：在搜索区域内，根据测量装置实时测得的数据提取出等值线；具体为：航行器匀速行驶进某一匹配区Ω后，测量装置实时测得的地形/重力/地磁数据经过数据处理后，获得M个地形/重力/地磁数据点序列，利用该M个数据点序列从预存的地形/重力/地磁数据库中提取出相应的等值线，并在步骤一的搜索区域内截取等值线，记为C_i,i＝1,2,…,M；同时，由INS获得M个地形/重力/地磁位置点X_i,i＝1,2,…,M和前M-1个位置点对应的航向信息，M个地形/重力/地磁位置点分别对应于M条等值线；将M个相邻的位置点做差获得M-1个相对距离，与前M-1个位置点的航向信息一起作为步骤三中价值函数等值线匹配算法的输入量；步骤三：利用价值函数等值线匹配算法，在等值线上找到离真实航迹接近的航迹，从而形成待匹配航迹；在等值线间通过搜索、跟踪和决策来连续的寻找使价值函数最小的两点作为最佳匹配位置点，在等值线上找到离真实航迹接近的航迹，形成待匹配航迹；步骤四：利用ICCP算法不断地进行旋转和平移变换，获得最终的航迹向和最近点；所述的ICCP为等值线迭代最近点算法，步骤四具体包括：根据ICCP算法利用四元数进行旋转变换的基本原理，对整条待匹配航迹进行旋转平移变换，旋转角度为α_k，其公式如下：Xi,k=R(αk)×Xi,k-1+Ti,kTi,k=Pg,k-R(αk)×Xg,kR(αk)=cosαk-sinαksinαkcosαkPg,k=1WΣi=1Mwi,kPi,k,Xg,k=1WΣi=1Mwi,kXi,kwi,k=1||Xi,k-Pi,k||]]>式中：X_i,k表示待匹配航迹经过第k次旋转变换后的第i个点所在的位置，X_i，k-1表示待匹配航迹经过第k-1次旋转变换后的第i个点所在的位置，T_i,k表示经过第k次旋转后第i个点的平移量，P_g,k、X_g,k分别表示经过第k次旋转后的最近点和匹配航迹点的重心，R(α_k)为由α_k组成的反对称旋转矩阵，P_i,k表示经过第k次旋转后第i个位置点所对应的最近点，权值w_i,k为经过第k次旋转后待匹配航迹点X_i,k到相应最近点P_i,k距离的倒数，W为权值w_i，k之和；经过最后一次旋转变换后，获得最终的位置点X_i,k所对应的最近点P_i，k和相邻两点X_i，k、X_i+1，k所形成的航迹向步骤五：采用最近点加密的方法，获得最佳匹配航迹；具体包括以下几个步骤：（1）计算相邻距离，根据INS给定的位置坐标点X_i，0，i＝1,2,…,M，分别计算X_i，0到X_i+1，0间的距离L_i，其中M表示所需匹配的总点数，即：L_i＝||X_i+1，0-X_i,0||（2）分段等值线，在搜索区域所截取的等值线C_i上找到利用ICCP算法最后一次旋转变换的位置点X_i，k所对应的最近点P_i，k，然后采用加密的方法以P_i,k为中心，以Δ为步长将等值线C_i划分为2×m段，确保真实位置点包含在m×Δ范围内，每个分段点记为l＝0,1,2,…,2m，然后以为起始点，沿X_i,k到X_i+1，k的方向，即最后一次旋转后的航迹向向等值线C_i+1分别作射线，相交点记为其中j表示存在相交点的段数，满足j≤l；（3）求取价值函数，求取匹配时刻所形成的每一条航迹的价值函数其中a表示可能形成的航迹条数，N_p表示所需采样点的总个数；判断使最小的价值函数D_a(P_i,L_i)，其公式如下式所示：Da(Pi,Li)=mina∈{1,2,...,Nh}{Ja,Np(Pi,Li)}Ja,Np(Pi,Li)=Σi=2Np||Pi-Li||2Pi=||Pi+1,kj-Pi,kj||]]>式中：N_h表示将N_p个等值线分段后，各段线段所能形成的航迹的总个数，满足N_h≤2m×(N_p-1)；由此D_a(P_i,L_i)所对应的匹配点即为最佳匹配点，D_a(P_i,L_i)所形成的航迹即为最佳的匹配航迹；最后，将最佳的匹配航迹输出至组合导航滤波器，组合导航滤波器导航输出，进行导航定位。