【滤波跟踪】基于无迹卡尔曼滤波实现惯性导航+DVL的组合导航算法附matlab代码

1 内容介绍

水 下 航 行 器（Ａ Ｕ Ｖ）的 产生、发 展 直 至 实际使 用经 过了很 长的 历 程 。对 于 海洋人 们从了解 认 识 、开发 研 究、使 用和保 护等方 面的 发 展，促 使 着 水下航 行 器 的开 发 研 究 工 作也 逐 渐 活 跃起 来。在 新世纪 中水下 航行 器 还 将被 重 点开 发 应 用。水 下 航行 器 研 制 的重 要标 准 之 一 就是 它的自主 性 能的 评 价，也 正是这 一点一 直 制 约 着 水 下 航 行 器 的向前发 展。怎样 充分利 用各 种 传 感 器的 测 量 信息来改善 提高 航 行 器的定位 性 能和可 依 赖 程度正 是目前 研 究的 关 键 所 在 。因为 水 下 环 境 很 复 杂，而 水 下 航 行 器 需 要 保 证 能进行长时间 和 大范 围的 作 业 能 力，因 此 需 要组 合 系统 提高 精度较 高的 速 度、位 置 和 姿 态 角等 信 息。从目前 可以 看出捷 联惯性 导 航 系统（ＳＩ Ｎ Ｓ）和多 普 勒 计 程仪（Ｄ Ｖ Ｌ）组 合 方式的 导 航 系 统是 在国内外 水下 航 行器 的发 展中应 用较为成 熟 的捷 联 式 惯性 系统 的短时定 位 精 度高，具 有很 强的自主性 ，但 是随 着时间延 长 会 产生 位 置 误 差的 累积的 不 足。Ｓ Ｉ Ｎ Ｓ ／ ＤＶ Ｌ组 合 系 统 正 是 使 用多普 勒计 程仪 测 量 得到 的速 度 信息 的 精 度 较 高 的特 点阻 碍了ＳＩ ＮＳ随时间 增 长 位 置 误差的不 足 ，具 有了较 强的自主性 和 较高 的导 航精度，在目前 的水 下导 航 系 统 上 是 采 用 较多的方 式。水下航 行 器的定 位准 确 程 度 是 决 定它 能否 完成设定 任 务和 安全回 来 的重要影 响因素。在Ａ Ｕ Ｖ上 使 用捷 联 式惯 性 系 统有 如 下的不 足 ：其 一 是ＳＩ Ｎ Ｓ会 跟 随时 间的 延 长 产生 较大 误差 累积，这 点 对 于需 要 较 长时间在 水 下 工 作的Ａ Ｕ Ｖ来 说是不 允 许 的；其二 是满 足 精 度要求 的惯导的 成 本 较 高，而 对 于 一 些 民 用 低成 本的Ａ Ｕ Ｖ来说 价 格过 于昂贵；其 三 是 一 些 小型 的Ａ Ｕ Ｖ装配 不了满 足精 度 但 体 积 偏大 的Ｉ Ｎ Ｓ。所 以，从 成 本 和 使 用空间等多方面考 虑，Ｉ Ｎ Ｓ并不 完全适合 安 装 于 一些 小型的Ａ Ｕ Ｖ上。对 于长时间工 作的ＳＩ Ｎ Ｓ ／ Ｄ Ｖ Ｌ组合 系统 来 说，其 系 统 精 度过多 地是依 赖 于 多 普勒，在 复杂 的 水下 环境中，比方说 当Ａ Ｕ Ｖ下潜深 度较 小时 ，Ｄ Ｖ Ｌ距 离水 底 太 远 超出了它的测 量 范围，又 或 者 说 是经 过的 水 域 水底 是 淤 泥等 强吸波 物 时，Ｄ Ｖ Ｌ测速 发射的 声波 就 无法 返 回来，也 就得 不到可靠 的 速度 数 据；也 有 可 能 会发 生Ｄ Ｖ Ｌ故 障测 速 数 据 无 效，而Ｓ Ｉ Ｎ Ｓ的 误差 会 随时 间积 累，随 着Ｄ Ｖ Ｌ失 效时 间延 长，将 会 使系 统 精 度 快速 下降。因此 ，在不 增加 硬 件 传 感器 的 情 况 下，我们 迫 切需要 找到一 种 能保 证 定位 精 度，并且 满 足Ａ Ｕ Ｖ水下 导 航 系 统 对精 度 和鲁棒性 的 要求 的 导航 方 法

多 传 感 器数 据 融 合技 术在 军事 和 民 用 领 域 都得 到 了 广 泛 的 重 视 ， 与单 传 感 器相 比 ， 多 传 感 器 数 据融合技 术 具 有 如下几 个 优 势 ： 第 一 是 使 用 几 个 同 类 的 传 感 器或 者对 单 一 传 感 器 进 行 多 次测 量 可 以 得到 Ｎ 个 独 立 的 观 测 值 ， 把 这 些 观 测 值 进 行 综 合 处 理 能 提 高 对 跟 踪 目 标 的 位 置 和 速度 佔 计 值 的 精 度 ； 第 二 种 方 式 对 观 测 量 的 提 升是 通 过 多 传 感 器 之 间 的 相 对运 动 来 实 现 的 。 卡 尔 曼滤 波 估 计 的 广 泛 采 用 始 于 上 世 纪 ６ ０ 年代 ， 卡 尔 曼 滤 波从 本 质 上 说 是 一 种线性递推 最 优 最 小 方 差 估计 方 法 ， 它 把 来 自 两 个 或 多 个 导 航 传 感器 的 测 量 信 息 进行 处 理 ， 能 估 计 得 到 各 个 状 态 的 误 差 量 ， 用 这些估计 出 来 的 状 态 误 差 量 来 校 正 系 统 的 输 出 ， 从而 达 到最 优 估 计 的 目 的 。 扩 展 卡 尔曼 滤 波 （ Ｅ ｘ ｔ ｅ ｎｄ ｅ ｄ Ｋ ａ ｌ ｍ ａ ｎ Ｆ ｉｌ ｔ ｅ ｒｉ ｎ ｇ ， Ｅ Ｋ Ｆ ） 克 服 了 卡 尔曼 滤 波要求 观 测 值 是 线性 的 ， 只 适 用 于 线 性 系 统 的 不 足 ， 扩 展 卡 尔 曼 可 以 用 于 对 非 线 性 系 统 的 估计 。 １ ９ ７９ 至 １ ９ ８ ７ 年 间 ， Ｓ ｐ ｅ ｙ ｅ ｒ 和 Ｋ ｅ ｒ ｒ 等 人 先 后 提 出 并 行 分 散 卡 尔 曼滤 波 的 思 想 ， 解 决 了 集 中 式 卡尔曼 滤 波 处 理 信 息计 算 量 大 的 问 题 。 后来 又 出 现 了 同 样适 用 于 非 线性 系 统佔 计 理 论 的 无 迹 卡 尔 曼 滤波 （ Ｕ Ｋ Ｆ ） ， Ｕ Ｋ Ｆ 对 非 线 性 系 统 状 态 误 差量 的 递 推 和 更新是 通 过 设 定 了 一 些 采 样 点 ， 对这 些 采 样 点 进行 Ｕ 变 换 ［ ８ ］ 的 值 来 近 似 代 替 原 状 态 点 经过 非 线 性 变 换 后 的 值 。

2 仿真代码

3 运行结果

4 参考文献

[1]于聪. 基于多元信息融合的AUV惯性导航系统方法研究[D]. 中国海洋大学.

[2]潘学松. 基于SINS/DVL/GPS的AUV组合导航系统关键技术研究[D]. 中国海洋大学.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。