专利名称：一种基于三角形约束模型的无源导航的等值线匹配方法
技术领域：
本发明涉及了地球物理场(地形/地磁/重力)辅助惯性导航，主要针对辅助导航系统中匹配算法的初始匹配方法。
背景技术：
地球物理场(地形/地磁/重力)辅助导航方法是一种十分有效的对惯性导航系统误差进行重调校正的方法，属于真正意义上的无源导航，因此受到了越来越多的关注。对于无源导航系统，通常在进入地球物理场导航图适配区域时，运载体在经过了相当长时间航行后由单纯惯性导航输出的导航位置和航向角都已积累了较大的误差，因此，利用地球物理场信息来校正惯导系统的大位置误差是地球物理场辅助惯性导航应首要解决的问题。目前地球物理场辅助导航方法主要有分批处理的TERCOM算法(Terrain Contour Matching)和递归处理的 SITAN 算法(Sandia Inertial Terrain Aided Nayi gat ion)两种。TERCOM 算法主要以 ICCP 算法(Iterated Closest Contour Point)为代表，ICCP 算法首先在地球物理场观测值所在的等值线上寻找距离惯导指示位置最近的点集，在此基础上进行迭代，求取每次迭代的旋转和平移变换，使上次迭代点集经变换后与该点集在等值线上的最近点点集的距离平方价值函数最小，将经过若干次迭代后的结果作为匹配点集。 ICCP算法是在将惯导指示位置在对应等值线上的最近点作为真实位置的基础上建立起来的，因此算法的应用受到惯导指示位置与载体真实位置之间的误差必须足够小的假设条件的约束。SITAN算法利用Kalman滤波技术对实时采集的测量数据进行递归处理，从而得到惯导系统误差的最优估计。但是，载体位置误差与地球物理场异常值变化量成线性关系的假设只有当真实位置在惯导指示位置不远处时才成立，在惯导位置误差很大的情况下会引起误匹配。由以上的分析可知，无论是TERCOM算法还是SITAN算法，在惯导定位误差较大的情况下均不可用。
发明内容
本发明的技术解决问题克服现有技术的不足，提供一种基于三角形约束模型的无源导航的等值线匹配方法，它利用惯导系统连续两次采样间的距离信息和相对转向角构建高精度三角形几何约束模型，再基于三角形约束模型进行等值线匹配，故而保证三角形的精度只与惯导短时精度有关，而不受惯导系统长期累积误差的影响，具有可靠的置信度。本发明采用的技术方案步骤如下一种基于三角形约束模型的无源导航的等值线匹配方法，实现步骤如下第一步，判断运载体已进入地球物理场适配区域Ω，选取运载体所承载的地球物理场传感器连续的三个采样点，三个采样点所对应的时间分别为、、t2和t3时刻；第二步，构建三角形几何约束模型，并结合地球物理场参考图所提供的场值与实测地球物理场值之间的尺度变化构建几何约束模型的三角形顶点位置误差置信区间δ ； 所述构建三角形几何约束模型的过程为首先获取惯导系统在时间段、t2、t2t3内输出的运载体相对行驶距离li、I2及在t2时刻的相对转向角θ ；然后，利用Ipl2和θ三个参数构建三角形几何约束模型；第三步，获取运载体承载的地球物理场传感器在、、t2和t3时刻三个采样点地球物理场值分别对应的全部等值线Cp C2和C3 ；第四步，构建运载体在采样点、时刻惯导系统提供的航向角扇形误差置信区间V， 然后以等值线C1的点集P1内任意一点为起点，基于航向角扇形误差置信区间V和三角形几何约束模型，寻找运载体在等值线C2上与之对应的初始匹配点，最终获取运载体在等值线
C2上的初始匹配点集尸2'；第五步，以初始匹配点集P2'内任意一点为起点，基于三角形几何约束模型，寻找运
载体在等值线C3上与之对应的初始匹配点，获取运载体在等值线C3上的初始匹配点集P3'第六步，获取同时满足tp t2和t3时刻匹配点形成初始匹配集；第七步，采用加权概率估计模型算法，计算初始匹配集中各组数据的代价值，取代价值最小的那组数据为运载体最终初始匹配位置。所述第四步构建运载体在采样点、时刻惯导系统提供的航向角扇形误差置信区间V为首先，根据惯导系统的角度误差特性，估计采样点、时刻惯导系统输出的运载体航向角的误差置信区间φ ；然后，以ti时刻运载体的航向为扇形顶角的角平分线，扇形的顶角大小为2 Φ，构建、时刻运载体的航向角扇形误差置信区间V。所述第四步寻找运载体在等值线C2上与之对应的初始匹配点，最终获取运载体在等值线C2上的初始匹配点集尸2'的过程如下首先，取等值线C1的点集P1 (A、Pn--Pu、…、化,，i为自然数，l<i<Li山为点集P1所包含的点数)内任意一点Pli为扇形误差置信区间V的顶点；其次搜索等值线C2 的点集P2 ( Pn、…、Pik、…、P2L2，k为自然数，1 < k < L2, L2为点集P2所包含的点数) 内位于扇形误差置信区间V内的所有点，生成点集仏(如、仏2、···、qtJ、…、为自然数， 1 < i < L1, j为自然数，1 < j < N2, N2为点集Qi所包含的点数)；再次，利用点Pli与点集 A内所有点生成匹配对集P1A (凡如、1U,2、…、“…、斗^v2 )，并计算匹配对集P1A内
每个匹配对所对应的两点间的距离/ “；最后，搜索匹配对集？工兑内满足不等式 (I1为第二步提到的三角形约束模型的一条边的边长，δ为第二步提到的三角形顶点位置误差置信区间S)的所有匹配对，生成匹配对集(^、^、…、^、…、^；，η为自然数，1< <》2，冗为匹配对所包含的点数)。遍历点集？工内所有点，重复上面的过程，最终获取运载体在等值线C2上的初始匹配点集ρ2'(Α、Ρ2'2、…、Ρ2；、...、P‘)(k为自然
m,l<k<L'2丄丨为点集巧所包含的点数)，以及所有匹配对集初(Plq” p2q2.....pmqm.....
P q ) (m为自然数，1 < i < M，M为匹配对集@所包含的匹配对数，pm、Qffl分别表示等值线 C1、等值线C2上的点)。所述第五步以初始匹配点集内任意一点为起点，基于三角形几何约束模型，寻找运载体在等值线C3上与之对应的初始匹配点，获取运载体在等值线C3上的初始匹配点集P/的过程如下从匹配对集@中选取任意一个匹配对 ，以等值线(2上的点(^为起点，搜索等值线(3的点集P3(&、Py1、…、Pv、…、&，1为自然数，1<士<1^山表示点集？3所包含的点数)内所有满足^ -^卜^且幺!^!^^ =…。。-Θ条件的点，从而获取运载体在等值线C3上的初始匹配点集/>/。所述第六步获取同时满足、、t2和t3时刻匹配点形成初始匹配点集的过程如下 在匹配对集@中任选一组匹配对;^，以匹配对位于等值线C2上的点qm为起点，搜索等值线C3的点集P3内满足I^p3, ~h\ < 5且Z PfflQfflP3i = 180° - θ条件的点，从而形成满足三角形几何约束模型的一组匹配对^^，遍历匹配对集^^内所有匹配对，从而获取同时满足 、、、和 t3 时刻匹配点的匹配对集/QPi ( J^,p2q2p'2、…、pm.qm,p'm,、...、pM,qM.p: ) (m'为自然数，l<m' <M'，M'为匹配对集_所包含的匹配对数，pm,、cv、/^.分别表示等值线C1、等值线C2、等值线C3上的点)。所述第七步，采用加权概率估计模型算法，计算初始匹配集中各组数据的代价值， 取代价值最小的那组数据为运载体最终初始匹配位置的具体过程如下构建的代价函数为
权利要求
1.一种基于三角形约束模型的无源导航的等值线匹配方法，其特征在于实现步骤如下第一步，判断运载体已进入地球物理场适配区域Ω，选取运载体所承载的地球物理场传感器连续的三个采样点，三个采样点所对应的时间分别为、、t2和t3时刻；第二步，构建三角形几何约束模型，并结合地球物理场参考图所提供的场值与实测地球物理场值之间的尺度变化构建几何约束模型的三角形顶点位置误差置信区间S ；所述构建三角形几何约束模型的过程为首先获取惯导系统在时间段、t2、t2t3内输出的运载体相对行驶距离I:、I2及在t2时刻的相对转向角θ ；然后，利用Ipl2和θ三个参数构建三角形几何约束模型；第三步，获取运载体承载的地球物理场传感器在、、t2和t3时刻三个采样点地球物理场值分别对应的全部等值线Cp C2和C3 ；第四步，构建运载体在采样点、时刻惯导系统提供的航向角扇形误差置信区间V，然后以等值线(^的点集P1内任意一点为起点，基于航向角扇形误差置信区间V和三角形几何约束模型，寻找运载体在等值线C2上与之对应的初始匹配点，最终获取运载体在等值线C2上的初始匹配点集P2';第五步，以初始匹配点集P2'内任意一点为起点，基于三角形几何约束模型，寻找运载体在等值线C3上与之对应的初始匹配点，获取运载体在等值线C3上的初始匹配点集P;,第六步，获取同时满足ti、t2和t3时刻匹配点形成初始匹配集； 第七步，采用加权概率估计模型算法，计算初始匹配集中各组数据的代价值，取代价值最小的那组数据为运载体最终初始匹配位置。
2.根据权利要求1所述的基于三角形约束模型的无源导航的等值线匹配方法，其特征在于所述第四步构建运载体在采样点、时刻惯导系统提供的航向角扇形误差置信区间V 为首先，根据惯导系统的角度误差特性，估计采样点、时刻惯导系统输出的运载体航向角的误差置信区间Φ ；然后，以、时刻运载体的航向为扇形顶角的角平分线，扇形的顶角大小为2 Φ，构建、时刻运载体的航向角扇形误差置信区间V。
3.根据权利要求1所述的基于三角形约束模型的无源导航的等值线匹配方法，其特征在于所述第四步寻找运载体在等值线C2上与之对应的初始匹配点，最终获取运载体在等值线C2上的初始匹配点集P2'的过程如下a.取等值线C1的点集P1，设定点集P1包含的点有Λ、Pn-Pu、…、A,，i为自然数， 1 < i < L1, L1为点集P1所包含的点数，内任意一点Pli为扇形误差置信区间V的顶点；b.搜索等值线C2的点集P2内位于扇形误差置信区间V内的所有点，生成点集Qi，设定点集Qi包含的点有知<1丨2、…、^lj、…、h，i为自然数，1 < i < L1, j为自然数，1 < j < N2, N2为点集Qi所包含的点数；c.利用点Pli与点集A内所有点生成匹配对集PliQi,设定匹配对集P1A包含的匹配对有彻、ρ—、…、·、p^lN2，并计算匹配对集PliGii内每个匹配对所对应的两点间的距离;d.搜索匹配对集PliQi内满足不等式、％-A (I1为第二步提到的三角形约束模型的一条边的边长，s为第二步提到的三角形顶点位置误差置信区间)的所有匹配对， 生成匹配对集，设定匹配对集,包含的匹配对有^^、...、^...、，η 为自然数，，》2为匹配对集所包含的点数，遍历点集P1内所有点，重复上面的过程，最终获取运载体在等值线C2上的初始匹配点集尸2'，设定点集P2'包含的点有 AP Ki、…、Kk、…、di，k为自然数，1<众<《4为点集T32'所包含的点数，以及所有匹配对集歷，设定匹配对集取包含的匹配对有=P1QpPW2.....PmQm.....P q , m为自然数，1 < i< M，M为匹配对集@所包含的匹配对数，pm、Qffl分别表示等值线C1、等值线C2上的点。
4.根据权利要求1所述的基于三角形约束模型的无源导航的等值线匹配方法，其特征在于所述第五步以初始匹配点集P2'内任意一点为起点，基于三角形几何约束模型，寻找运载体在等值线C3上与之对应的初始匹配点，获取运载体在等值线C3上的初始匹配点集 P3’的过程如下从匹配对集⑩中选取任意一个匹配对^；，以等值线C2上的点qm为起点， 搜索等值线C3的点集P3内所有满足-l2\< 5且Z PfflQfflP3i = 180° -θ条件的点，从而获取运载体在等值线C3上的初始匹配点集戶/，i为自然数，1 < i < L3, L3表示点集P3所包含的点数。
5.根据权利要求1所述的基于三角形约束模型的无源导航的等值线匹配方法，其特征在于所述第六步获取同时满足、、t2和t3时刻匹配点形成初始匹配点集的过程如下 在匹配对集@中任选一组匹配对i，以匹配对位于等值线C2上的点1为起点，搜索等值线C3的点集P3内满足|/9』,-/2|<5且ZPmqmP3i = 180° -θ条件的点，从而形成满足三角形几何约束模型的一组匹配对凡札A ,遍历匹配对集@内所有匹配对，从而获取同时满足、、t2和t3时刻匹配点的匹配对集P1 QP3，设定匹配对集^^包含的匹配对有 p^p2q2p'r..·、Pm,qm.P'm‘、..·、PM.qM,P'M.，m‘为自然数，l<m' <M' ,M'为匹配对集_ 所包含的匹配对数，Pm,、qm,、；^分别表示等值线C1、等值线C2、等值线C3上的点。
6.根据权利要求1所述的基于三角形约束模型的无源导航的等值线匹配方法，其特征在于所述第七步，采用加权概率估计模型算法，计算初始匹配集中各组数据的代价值，取代价值最小的那组数据为运载体最终初始匹配位置的具体过程如下a.构建的代价函数为cos t = f (1, α ) (1)式中1表示匹配点与惯导系统所提供的对应位置点之间的距离参数，α表示匹配点的方向角与惯导系统所提供的对应点的方向角之间的夹角；b.从匹配对集中任选一组匹配对^^，由公式(1)分别计算匹配对K^S 所对应的三个匹配点Pm,、qm,和Λ的匹配代价值Costpm,、COStqm,和COS炉:^，可得匹配对 pm,qm,p'm,的匹配代价值costj^ = Costpm, + Costqm, + cos^，选取cosi^；^*最小值所对应的一组匹配对为运载体最终的初始匹配值。
全文摘要
一种基于三角形约束模型的无源导航的等值线匹配方法，本发明针对无源导航(地形/地磁/重力辅助惯形导航)系统，当运载体刚进入地球物理场适配区域时，惯导系统提供的运载体位置信息存在大误差条件下，为了快速获取高精度的初始匹配值而提出的一种等值线匹配方法，其特征在于利用已知的高精度三角形约束模型进行等值线匹配。本发明是一种自主式匹配方法，具有精度高、鲁棒性好的特点，满足无源导航系统地球物理场匹配技术对运载体初始位置精度的要求以及运载体对无源导航系统实时性要求，可应用于地形/地磁/重力辅助惯性导航等无源导航系统中的初始匹配。
文档编号G01C21/20GK102168979SQ201010592980
公开日2011年8月31日 申请日期2010年12月8日 优先权日2010年12月8日
发明者房建成, 朱庄生 申请人:北京航空航天大学