基于卡尔曼滤波的地形辅助导航

１ 引 言
海 底 地形 辅 助 导 航 系 统 Ｅ ７是 近 几 十 年 出 现 的一 种 新 ｌ７ －
Ｃ ｎｏ ｒＰ ｉｔ Ｌ ， 是 Ｉ Ｐ 算 法 （ ｅａｉ ｌ ｅｔ ｏ ｔｕ ｏｎ ）” 它 Ｃ ｈ ｒｔ ｅ Ｃｏ ｓ ｖ ｓ
Ｐ ｉｔ的一个特例 。 ｏｎ） 多波束测深 系统 ＿ 是在 回声测深 仪 的基 础上发展 起 ２ ， 来的一项 海底 地形 精密勘 测技术 ， 近几 十年来 海洋测 绘 是 界进 行海 底地 形条带式探测 的热 点 。本文利用 的海 洋测绘
总 第 ２４期 １ ２ １ 年 第 ４期 ０２
舰 船 电 子 工 程
Ｓ ｉ ｌ ｃ ｒ ｎｃ Ｅｎ ｉ ｅ ｒ ｇ ｈ ｐ Ｅ ｅ ｔ ｏ ｉ ｇｎ ｅ ｉ ｎ
Ｖｏ ＿ ２ Ｎｏ ４ ｌ３ ．
５ ４
基 于 卡 尔 曼 滤 波 的 地 形 辅 助 导 航
周亦军 郑 彤
武汉 ４０６） ３ ０ ４ （ 军 驻 四 三八 厂 军 事 代 表 室 海
１ ２ … ， 构 成 ， （一 １２ … ， 表 示 的 是 当 地 实 测 水 深 ， ， Ｎ） Ｇ ， ， Ｎ）
和接收阵组成 。发射 阵平行船 纵 向排列 ， 两侧堆 成 向下 呈 方发射 ２（ 船纵 向） ４ （ 。沿 ×４。 沿船 横 向 ） 的扇 形脉 冲声 波 。
等值 线 。由于惯导误差 的存 在 ，Ｐ ：一１ …， 和 ｛ ：一 ｛ ， Ｎ｝ Ｘ ｉ １ … ， 之 间必定存 在误 差 。Ｘ 必定位于 Ｃ ， Ｎ｝ 的等值 线上 ，
中， 采用 ＩＣ Ｃ Ｐ算法修 正惯 性导航 系统 累计 误差 ， 提高水 下 载体的导航精度 ， 为水 下载体 实现水 下 、 长期 、 确导航 提 精 供技术支撑 。
航形１ 一地 系图豆 ｆ１ Ｉｌ ． 统 ｌ’ Ｉｌ 数嚣 海配 字 底ｌ １ ｌ ｌ 首
图 ｌ 水 下地 形辅 助 导 航 基 本 原 理
后， 使用专业软件 Ｃ Ｉ Ｐ ｎ ＩＳ进行 处理 （ ＡＲ ＳＨＩＳａ ｄＳＰ 如图 ４
所 示 ）就 可 以得 到 真 实 的地 形 数 据 了 ， 为 保 密 起 见 ， 文 ， 但 本 中涉 及 的地 形 图都 将 经 纬 度 隐去 。
配
迹
图 ２ ＩＣ Ｃ Ｐ算 法 原 理 示 意 图
迭代实现测量图像与模型之间 的对准和 匹配。算 法采用 欧 氏距离平方最小为 目标 函数 ， 求得 测量 航迹 与真实航 迹之
间 的最 优 变 换 ， 过 该 变 换校 正航 迹 ， 现 对 测 量 航 迹 的校 通 实
正 ， 图 ２所示 。 如
Ｍａｃ ｉｇ 在空 中载体 中的应用 ， 在各种地形和不 同覆盖 ｔｈｎ ） 其 层上空均显 出了 自主、 可靠的隐蔽特性 。但受 Ｉ ＮＳ精度 、 数 字地形 图的精度 、 雷达高 度表测 量精 度和 匹配算 法的影 响
（ ｉｔ ｒ ｐｒｓ ｎ ａｉｅＯｆｉｅｉ ｈ ３ ｔ ｃｏ ｙ，Ｗ ｕｈ ｎ ４ ０ ６ Ｍ ｌ ａ ｙＲｅ ｅ ｅ ｔｔｖ ｆ ｎ ｔ ｅ４ ８ ｈ Ｆａ ｔｒ ｉ ｃ ａ ３ ０ ４）
Ａｂｓ ｒ ｃ Ｔｅ ｒ ｉ ｔ ｈ ｎ ｓ ｉｔ ｎ ｖ ｇ ｔ ｎ ｉ ａ ｎ ｗ ｔ ｏ ｎ ｎ ｖ ｇ ｔｏ ｅ ｈ ｏ ｏ ｙ ｏ ｈ ｎ ｅ ｗａ ｅ ｅ ｉｌ ｓ Ｉ ｈ ｓｐ ｐ ｒ ｈｅ ｔａ ｔ ｒ ａ ｎ ｍａ ｃ ｉ ｇ ａ ｓｓ ａ ｔｎａ ｉ ａ ｉ ｓ ｅ ｍｅ ｈ ｄ ｉ ａ ｉ ａ ｉ ｎ ｔ ｃ ｎ ｌ ｇ ｆｔ ｅ ｕ ｄ ｒ ｔ ｒｖ ｈ ｃ ｅ ． ｎ ｔ ｉ ａ ｅ ，ｔ ｏ
ｔ ｅ ｍ ｅ ｓ ｒ ｄ ｔ ｒ ａ ｎ ｄ ｔ ｎ ｈ ｌ ｏ ｉｈ ，ｔ ｅ ａ ｃ ｍｕｌｔｖ ｒ ｏ ｓ ｏ ｈ ｎ ｒｉ ｌ ｖｇａ ｉ ｎ Ｓｙ ｔ ｍ ａ ｏ ｒ ｃ ｅ ｎｄ ｔ ｐ ｉ ｌ ｔ ｈ ａ ｕ ｅ ｅ ｒ ｉ ａ ａａ ｄ ｔ ｅ ａ ｇ ｒｔ ｍ ｈ ｃ ｕ ａ ｉ ｅ ｅ ｒ ｒ ｆ ｅ Ｉ ｅ ｔａ ｔ Ｎａ ｉ ｔｏ ｓ ｅ ｃ ｎ ｂｅｃ ｒ ｅ ｔ ｄ ａ ｈｅｏ ｔｍａ ｍａ ｅ
值 与 已存 在 的 地 图 进 行 匹 配 ， 水 下 载 体 进 行 定 位 ， 本 原 对 基 理 ［ 。如 图 １ 示 。在 海 底 地 形 辅 助 导 航 系 统 中 ， 确 的 ］ 所 精 数 字 地 图 和 合 适 的 匹 配 算 法 是 核 心 技 术 ， 也 是 当 前 相 关 这 领域的重点和热点研究问题 。
那 么按 照 一 定 的 准 则 ， Ｐ 靠 拢 到 Ｃ 上 ， 而 找 到 最 优 估 使 从 计 点 和 对 应 的 “ 配 航 迹 ” 实 现 对 真 实 航 迹 的 最 优 估 匹 ，
计。
接收阵沿船横 向排列 ， 但在束 控方 向上接 收方 式与 发射方
式 正 好 相 反 ， ２ 。沿 船 纵 向） 。沿 船 横 向 ） １ 接 以 ０（ ×２ （ 的 ６个 收波 束 角 接 收 来 自海 底 照 射 面 积 为 发 射 ２ （ 船 纵 向 ） 。沿 × ４ 。沿 船 横 向 ） ４（ 的扇 区 回 波 。 接 收 的 回 波 经 过 原 始 的编 辑
ｅ ｒ ｓ ｃ ｎ ｂ ｓ ｉ ｔ ｄ ａ ｃ ｒ ｔ ｌ ｔ ｈｅ ｍａ ｃ ｎ ｏ ｉｉ ｎ ｄ ｆ ｅ ｅ ｅ ． ｒ ｏｒ ａ ｅ ｅ ｔｍａ ｅ ｃ ｕ ａ ｅｙ ｗｉｈ ｔ ｔ ｈｉ ｇ ｐ ｓｔｏ ｉｆ ｒ ｎｃ ｓ
Ｋｅ ｏｒ ｕ ｅｗａ ｅ ｅ ｉｌｓ ｙＷ ｄｓ ｎｄ ｒ ｔｒｖ ｈｃｅ ，ｍ ｕｔ ｂｅｍ ｏ ｎ ｉｇ ｓ ｓｅ ，Ｉ ｌｉ ａ ｓ ｕ ｄｎ ｙ ｔｍ — ＣＣＰ ａｇ ｒｔ ｍ ，ｔｒａｎ ｍａｃ ｉｇ，ｉｅｔａ ａ ｉａ ｉｎｓ ｓｅ ｌｏ ｉｈ ｅｒ ｉ ｔ ｈｎ ｎ ｒｉｌｎ ｖｇ ｔｏ ｙ ｔｍ ＣＩ镐 Ｎｕ ｂ ｒ Ｕ６ ６ １ ａ ｍ ｅ ６ ．１
Ｕ６ ６ １ ６． １ 中 图分 类 号
Ｓ ａ Ｂｏ ｔ ｍ ｒ ａｉ ｄｅ ａ ｉ ａ ｉｎ ｅ ｔｏ Ｔｅ ｒ ｎ Ａｉ ｄ Ｎ ｖ ｇ ｔｏ Ｂａｓ ｄ ｏ ｈｅ Ｋ ａ ｍ ａｎ Ｆｉｔｒ ｅ ｎ ｔ ｌ ｌｅ
Ｚ Ｕ ｊｎ ＺＨ ＨＯ Ｙｉ ｕ ＥＮＧ Ｔ ｎ ｏｇ
多 波 束 测 量 试 验 测 得 的 真 实 地 形 数 据 来 建 立 的真 实 地 形 数 据库 ， 在 此 基 础 上 ， 过 仿 真 研 究 水 下 载 体 在 航 行 过 程 并 通
型的导航 系统 ， 是水下运动载体导航技术 的一个发展方 向。
利用海底地形进行导航的就是将水下载体实际测量 的水深
ｒ ａ ｅ ｒｉ ａ ａａｅｍｅ ｓ ｒ ｄｂ ｌｉｂ ａ ｓ ｕ ｄｎ ｙ ｔｍ ，ａ ｄ Ｉ Ｐ ｌｏ ｉｈ ｉ ｅｅ ｔｄ ａ ｅ ｉቤተ መጻሕፍቲ ባይዱａ ｉ ｔｈｎ ｌｏ ｉｈ ｅ ｌｔｒａｎｄ ｔ ｒ ａ ｕ ｅ ｙｍｕ ｔ ｅ ｍ ｏ ｎ ｉｇ ｓ ｓｅ — ｎ ＣＣ ａｇ ｒｔ ｍ ｓｓ ｌｃｅ ｓａｒｇｓｒ ｔ ｏｎｍａ ｃ ｉｇａｇ ｒｔ ｍ． Ｂ ｓ ｄｏ ａｅ ｎ
２ 匹配 算 法
ＩＣ Ｃ Ｐ算法最初来源于图像配准的 Ｉ Ｐ算法 ， Ｃ 不需 事前
确定对应估计 ， 只是 不 断 重 复 （ 始 ） 动 变 换 一 确 定 最 近 初 运 点 一 求 运 动 变 换 的过 程 ， 步 改 进 运 动 的估 计 。算 法 的 主 逐
要思路是 ： 将测 得的沿着 航迹 的水 深值 连接起来 构成曲线 ，
摘
要
利用海底地形辅 助导航是水下载体导航技术致力研究 的新方 向。该３ ＊ Ｎ多波束测深 系统测量真实地形数据 ， ｃ， Ｉ 在此实测地形
图的基础上 ， 采用 ＩＣ Ｃ Ｐ算法为对准匹配算法 ， 得到水 下载体 的最佳匹配位置 ， 提高水下载体 的导航精度。并利用仿真 以平 台式惯 导系统为
例 ， 卡 尔 曼 滤 波 对 惯 导 系 统误 差 进行 最 优 估 计 ， 得 了 较 好 的 效 果 。 用 取 关键词 水 下 载 体 ；多波 束 测 深 系统 ；ＩＣ Ｃ Ｐ算 法 ；地 形 匹配 ；导 航
与 已存 在 的水 深 等 值 线 图进 行 匹配 。主 要 是 通 过 最 近 点 的
文献 ｒ ～ １ ］ 究 了 ＳＴＡＮ（ ａｄａＩｔｒ ａ Ｔ ｒａ － ８ ４研 Ｉ Ｓ ｎ ｉ ｎｅｔ ｌ ｅｒｉ ｉ ｎ
Ａｉｅ ｖｇｔ ｎ 算 法 和 ＴＥ Ｃ ｄ ｄＮａ ｉ ｉ ） ａｏ Ｒ ＯＭ 算 法 （ ｅｒｉ ｏ ｔｕ Ｔ ｒａ Ｃ ｎｏ ｒ ｎ
较 大 。近 年 来 使 用 比较 多 的是 Ｉ Ｃ Ｃ Ｐ算 法 （ｔｒｔ ｅＣｏｅｔ Ｉ ａｉ ｌ ｓ ｅ ｖ ｓ
图 ２中 由 Ｐ （— ｌ２ … ， 点 构 成 的 是 “ 示 航 迹 ” Ｎ ， ， Ｎ） 指 ，
收 稿 日期 ：０ １年 １ ２１ Ｏ月 ２ 日， 回 日期 ：０ １年 ｌ ８ 修 ２１ ２月 １日
图 ３ 多波 束 测 深 系统 示 意 图
（ ） 的距离最小 。由于集合 ｛ ） 定在 等值 线上 ， 一 只是 无 法确定在等值线上 的确 切位置 ， 因而在 最优 化 目标 函数
的基 础 上 采 用 迭 代 方 法 寻 找 刚 性 变 换 Ｔ， 式 （ ） 示 的距 使 １表 离最小 。
Ｎ
５ 仿 真 分 析
考虑水下载体以某一 航迹航 行 ， 水下 载体 实际航迹 为 以某一 角度 自西向东航 行 。图 ４所示 载体 的真实 航迹 ， 指
示 航 迹 以及 匹 配 航迹 。 如 图所 示 匹 配航 迹 跟 真 实 航 迹 形 状
对 于 ＩＣ Ｃ Ｐ算法 的匹 配过程 可解 释为 ： 已知 点 Ｐ 出 从 。
发 ， 真 实 航 迹 航 行 ， 不 断 测 量 水 深 。在 实 时 测 深 传 感 器 沿 并 没 有 测 量 误 差 的 情 况 下 ， 些 测 量 点 一 定 落 在 对 应 的 等 值 这 线 上 。 由于 有 测 量 误 差 ， 量 航 迹 给 出 的 位 置 一 定 偏 离 对 测 应 等 值 线 。分 别 设 指 示 航 迹 位 置 点 集 合 为 ｛ ， 应 的 实 Ｐ ）对 际 航 迹 点 集 合 为 ｛ ） 为 了 求 得 真 实 位 置 ， 测 量 航 迹 与 ， 将 真 实 航 迹 匹 配 ， 要 确 定 刚 性 变 换 Ｔ（ 括 一 个 旋 转 分 量 和 需 包 二 个 平 移 分 量 ） 通 过 刚 性 变 换 Ｔ 使 集 合 ｛ 与 集 合 ， Ｐ ）
ｈｎ ｏ ｉｉｎ ｃ ｎ ｂ ｏｔ ｎ ｉｇ ｐ ｓｔｏ ａ ｅｇ ｔｅ ．Ｔｈ ｌｔｏ ｍ ｒｉｌｅ ｒ ｒ ｒ ｓｉ ａｅｗｉ ｈ ａｍａ ｉ ｅ ｎＩ ．Ｆｒ ｍ ｈ ｉ ｌｔｏ ｅ ｕ ｔ ，ｔ ｅｉｅ ｔ ｌ ｅ ｐａｆ ｒ ｉ ｔａ ｒｏ ｓａ ｅｅｔｍ ｔ ｔ ｔ ｅｋ ｌ ｎ ｆｌ ｒｉ ＮＳ ｎｅ ｈ ｔ ｏ ｔ ｅｓｍｕ ａｉｎ ｒｓ ｌｓ ｈ ｎ ｒｉ ａ
作者简介 ： 周亦军 ， ， 男 工程师 ， 研究方 向： 舰船 电子武备系统和设备监造 。郑彤 ， ， 女 博士研究生 ， 研究方 向： 舰船 电子武备系统和设 备
监造 。
２１ 年第 ４ ０２ 期
舰 船 电 子 工 程
５ ５
为航迹 点 序 号。 弯 曲 实 线 称 为 “ 际航 迹 ” 由 Ｘ ｉ 实 ， （一