37-基于ICCP算法的地磁匹配定位方法_李豫泽

《现代电子技术》2008 年第 20 期总第 283 期
计算机应用技术
2 .1 算法的基本描述
在载体在航行过程中 , 参考导航系统(一般是惯性
导航系统)会给出一系列的航迹点 , 记测量航迹点集合
为 {xn}[ n =1 , 2 , … , N] , 真实航迹点集合为{yn}, 地磁
测量值集合为{f n}。由于导航误差 , 测量航迹点坐标相 对地磁图存在误差 , 为求得载体的真实位置 , 这里将测
收稿日期 :2008-03-07 基金项目 :国防科技重点预研项目(51309060301)
12 2
过程 。 所谓地磁匹配 , 就是把预先规划好的航迹上某些点
的地磁场特征量绘制成参考图(或称基准图), 存贮在载 体计算机中 , 当载体经过这些地区时 , 由地磁匹配测量 仪器实时测量出经过这些点地磁场特征量 , 以构成实时 图 。 并在载体上的计算机中与参考图进行相关匹配 , 计 算出载体的实时坐标位置 , 供导航计算机解算导航 信 息 。 匹配精度受数字地磁图精度 、地磁匹配测量仪 、匹 配算法 、匹配特征量等的影响 。其中匹配算法是地磁导 航的核心技术 , 本文主要研究基于 ICCP(I terative Closest Co nt our P oint)算法的地磁匹配定位方法 。
LI Yuze1 , S HI Zhiy ong1 , YA N G Y untao1 , FEN G Jun2
(1.Ordnance Engineering C olleg e , Shijiazhuang , 050003 , C hina ;2 .O rdnance N .C .O A cademy o f P LA , Wuha n, 430075 , C hina)
在上述计算最优变换算法中 , 首先计算旋转矩阵 , 然后计算平移矢量 , 也就是说 , 先旋转集合 X 使其对准 集合Y 的方向 , 然后进行平移以使集合 X 的质心与集合 Y 的质心重合 。
(1)将集合 X 变换到集合 T X , 将新的集合 T X 作 为起始集合进行迭代 , 重复该过程直至收敛 , 即 T 停止 显著的变化 。
迭代算法的收敛性非常明显 , 从起始集合 X 0 开始 算 , 生成集合序列 X 1 , X 2 , … , Xk , …, X F , 每次迭 代中 距离减少 , 即 :
M(C , Xk+1)=M(C , T k X k )≤ M(C , X k ) (7)
且由于距离有下界(正值 , 正好是 0), 因此算法是 收敛的 。
是有 :
∑ R
=
cos sin
θ θ
-si n θ cos θ
,
S
N
= w n (yn n =1
-y)(xn
-x)T
(4)
S11 +S 22
0
0
S21 -S12
W=
0
0
S11 -S22 S12 -S21
S1 2 +S21 S2 2 -S11
0 0
S21 -S 12
0
0
-S11 -S22
(5)
矩阵 W 的 4 个特征值是实数 , 由下式给出 :
用迭代算法求取使距离最小的变换步骤为 :
对每一个数据点 xn 在其等值线上寻找最近点(依
据欧氏距离最小), 记这些点为 yn , 假设 y n 是 x n 的相应
等值线点 。
寻找变换 T(旋转和平移), 使得集合 Y ={yn}与
集合 X ={x n}之间距离最小 :
N
∑ M(C , T X)=M(Y , T X)= w n ‖y n -T x n ‖2 n =1
关键词 :ICCP 算法 ;自主无源导航 ;地磁匹配 ;误差校正 中图分类号 :T J765 , T N966 文献标识码 :B 文章编号 :1004-373X(2008)20-122-03
Geomagnetic Localization Based on ICCP Algorithm
λ= ±[ (S11 +S22 )2 +(S21 -S12 )2 ] 1/2 , ±[ (S11 -S22 )2 +(S12 +S21 )2 ] 1/2
(6)
记最大的特征值为 λm , 则特征向量可由式(S 11 + S22 -λm)q0 +(S 21 -S12 )q3 =0 计算出 , 由此给出旋转 角 tan(θ/2)=(S11 +S22 -λm)/(S 12 -S21 )。旋转矩阵确 定后 , 平移矢量为 t = y -Rx 。
DO I :10.16652/j .issn.1004 -373x .2008.20.009
科学计算及信息处理
李豫泽等 :基于 ICCP 算法的地磁匹配定位方法
基于 ICCP 算法的地磁匹配定位方法
李豫泽1 , 石志勇1 , 杨云涛1 , 冯 俊2
(1 .军械工程学院 河北 石家庄 050003;2 .军械士官学校 湖北 武汉 430075)
12 3
科学计算及信息处理
李豫泽等 :基于 ICCP 算法的地磁匹配定位方法
粗匹配 , 使匹配后的航迹更接近于真实航迹 , 再以得到
的航迹作为等值线匹配的输入航迹进一步匹配 , 以提高 匹配精度 。在粗匹配阶段 , 以参考位置为中心 , 按照参
考导航系统误差大小确定搜索窗口的大小 , 在搜索窗口
内 , 对每个网格点 , 取与参考航迹形状相似的地磁序列 , 作为 匹 配 航 迹 , 采 用 T ERCOM 算 法 中 的 MSD 和 MAD 准则匹配, 以最小 MSD 和 MAD 所对应的地磁 序列代替参考航迹 。
Abstract:T he geo magnetic lo calizatio n based on the ICCP alg orithm can be used to bind the position er ro rs inher ent in Ine rtial N avigatio n Sy stem (INS), w hich increases w ith time .The centr al tho ug ht o f the alg orithm is given in this pape r and have been improv ed in some par ts.T he algo rithm can realize the err or cor rection o f IN S under the noise of g eomag netic measurement data .T he seco nd is to recko n the clo sest points by use of g liding window technique so as to reduce the computatio n cost in reckoning the clo sest points in each time of conver ging to local minimum s.T he efficiency of computing is improved .
2 ICCP 算法
ICCP 算法最初来 源于图像 配准的 ICP (Iterative Closest Point)算法 , 主要是通过最近点的迭代实 现测 量图像与模型之间的对准和匹配 。
由于地磁图通常以等值线的形式给出 , 匹配单元选 为等值线上与测量点最近的点而得名 。算法采用欧氏 距离平方最小为目标函数 , 求得测量航迹与真实航迹之 间的最优变换 , 通过该变换求得校正航迹 , 以实现对测 量航迹的校正 。
摘 要 :基于 ICCP 算法的地磁匹配定位可以用于限制惯导系统随时间增长的位置误差 。 给出 ICCP 算法的设计思想并 进行推广 , 使算法能够在地磁测量数据存在误差的 情况下 , 实 现对惯 导的误 差校正 。 同 时采用 滑动窗 口技术来 快速寻 找等 值线上的最近点 , 在每一次收敛到局部最 小的过程中都减少了寻找最近点的计算量 , 计算效率 得以很大程度的提高 。
得的数据点与存储的地磁图进行匹配 。也就是要确定刚
性变换 T (旋转和平移), 该变换使图上数据点和测量数
据点间距离最小 。点集{yn}正是要确定的 。在地磁传感
器没有测量误差的情况下 , 数据点 xn 一定在地磁值为
f n 的等值线 C n 上 , 但是不知道是在等值线的哪一点
上,于是希望找到刚性变换 T 使得下式表示的距离
Keywords:ICCP alg o rithm ;auto no mous and passive navigation ;g eomag netic matching ;err or co r rection
1 引 言
实时确定载体的位置和速度 , 是提高武器装备独立 作战能力和整体作战能力的重要手段 , 也是提高制导武 器性能的基本要求 。 目前 , 导航定位的主要方式包括惯 性导航和卫星导航等 。惯 性导航 , 不借 助任何外来信 息 , 也不向外辐射任何信息 , 能够在全天候条件下 , 在全 球范围内和任何介质环境里自主地 、隐蔽地进行连续的 三维空间定位和三维空间定向 , 能够提供完整的导航参 数 , 但是它的缺点是导航误差随时间积累 , 长时间使用 必须校正 。虽可采用 GPS 、无线电和天文导航等信息 对惯性导航系统进行校正 , 但这些方法增加了载体被发 现的危险性 。随着无源导航技术的发展 , 地磁导航技术 的出现为实现这一目标提供了新的技术途径 。 由于地 磁场在全球范围内各点值都不相同 , 理论上与经纬度是 一一对应的 , 同时某些地区磁场特征也很明显 , 因此地 磁导航成目前研究的一个热点 。地磁导航技术在获取 地磁信息时对外无能量辐射 , 具有良好的隐蔽性 , 因此 可以实现载体长期高精度定位 。地磁导航就是利用地 磁图特征与当地即时测量磁特征信息进行匹配导航的