数据融合的精确极大似然配准算法在六自由度DPS中的应用

（ 3）
在 这 种 意 义 上 ， 方 法 更 普 遍 ， 它 根 据 传 感 器 的 噪 837 声 方 差 对 量 测 加 权 。 但 实 际 上 人 们 发 现 ， 方 法 837 比 37 方 法 并 没 有 显 著 的 改 进 ， 尤 其 在 工 程 的 实 际 应 用 中 。 因 此 在 文 中 用 37 配 准 方 法 与 9:3 方 法 进 行 比 较 。 ,;. 精 确 极 大 似 然 9:3 配 准 算 法 一 般 的 配 准 公 式 没 有 考 虑 量 测 噪 声 的 影 响 ， 并 把 系 统 平 面 内 源 自 各 传 感 器 量 测 的 偏 差 统 统 归 咎 于
作 者 简 介 ： 严 浙 平 （ +,-./） ， 男 ， 讲 师 ， 现 从 事 非 线 性 系 统 辨 识 、 多 传 感 器 数 据 融 合 和 智 能 控 制 技 术 研 究 。 万方数据
救 生 艇 在 海 洋 中 具 有 六 个 自 由 度 的 运 动 ， 即 刚 体 在 空 间 中 的 三 个 位 置 自 由 度 （ 纵 荡 、 横 荡 和 垂 荡 ） 和 三 个 姿 态 自 由 度 （ 转 艏 、 纵 摇 和 横 摇 ） 。 潜 器 在 海 洋 中 运 动 或 作 业 时 会 受 到 海 浪 、 海 流 等 海 洋 环 境 力 的 扰 动 ， 这 些 扰 动 对 于 潜 器 的 操 纵 性 和 控 制 是 不 利 的 。 由 于 特 殊 作 业 时 要 求 潜 器 停 留 在 海 洋 中 的 某 一
& ｛ 分 别 表 示 传 感 器 ! (& "， !"｝ & 次 量 测 。 用｛ ( & 和 !， !! ｝
图 !
配 准 误 差 的 几 何 关 系
器 ! 和 " 间 的 位 置 连 线 把 量 测 方 程 分 成 两 组 ， 对 每 组 的 量 测 求 平 均 ， 它 得 到 每 组 量 测 的 两 个 方 程 。 配 准 误 差 向 量 可 以 通 过 求 解 一 个 0 2 0 矩 阵 来 求 # 就 得 。 另 一 方 面 ， 方 法 通 过 求 基 于 & 个 量 测 集 的 最 37 小 二 乘 值 来 解 得# ， 即 3 # + ") 其 中
・ ,/ ・ 从 而 使 多 传 感 器 数 据 处 理 产 生 目 标 的 误 差 （ 虚 象 ） 。 配 准 可 分 为 两 步 处 理 ： 传 感 器 初 始 化 和 相 对 校 准 。 传 感 器 初 始 化 使 每 个 传 感 器 相 对 于 系 统 坐 标 系 分 别 对 准 。 一 旦 完 成 传 感 器 初 始 化 ， 就 可 以 用 共 同 目 标 来 开 始 有 关 传 感 器 的 校 准 过 程 。 在 有 关 的 传 感 器 的 校 准 过 程 中 ， 必 须 收 集 足 够 多 的 数 据 点 来 计 算 系 统 偏 移 ， 并 借 此 来 调 整 后 续 输 入 的 传 感 器 数 据 ， 以 便 进 一 步 处 理 。 目 前 的 配 准 算 法 有 !"#$% "&’ 研 制 的 实 时 质 量 控 制 程 序 %#(’、 最 小 二 乘 算 法 )* 和 广 义 最 小 二 乘 算 法 +)*。 %#(’ 程 序 把 来 自 每 个 传 感 器 的 量 测 进 行 平 均 以 计 算 配 准 误 差 ， 而 )* 方 法 把 配 准 过 程 表 述 为 一 个 普 通 的 或 均 权 的 最 小 二 乘 问 题 。 法 是 +)* 方 法 的 推 广 ， 它 根 据 量 测 误 差 方 差 对 量 测 加 权 。 )* 方 然 而 ， 由 于 上 述 方 法 都 没 有 考 虑 传 感 器 量 测 噪 声 的 影 响 ， 并 认 为 公 共 坐 标 系 中 的 偏 差 完 全 由 传 感 器 配 准 误 差 引 起 ， 所 以 只 有 在 量 测 噪 声 比 较 小 时 ， 它 们 才 有 理 想 的 性 能 。 当 量 测 噪 声 比 较 大 时 ， 其 性 能 变 差 。 ,-, 配 准 问 题 的 一 般 描 述 它 们 测 量 同 一 目 标 的 考 虑 两 个 传 感 器 ! 和 " ， 距 离 和 方 位 。 配 准 几 何 原 理 如 图 . 所 示 。 为 讨 论 问 ， 传 感 器 " 位 于 题 的 方 便 ， 假 定 传 感 器 ! 位 于 原 点 。 %& 表 示 第 ’ 位 置 （ # ， $ ）
图 ! 水 池 试 验 潜 器 模 型
级 处 理 为 威 胁 估 计 。 而 对 于 !"# 而 言 ， 需 要 重 点 考 虑 的 是 一 级 处 理 ， 特 别 是 配 准 问 题 。 配 准 是 指 为 确 保 多 传 感 器 数 据 进 行 必 要 的 无 差 错 转 换 的 过 程 。 当 传 感 器 测 量 没 有 正 确 的 目 标 或 出 错 时 ， 就 需 要 进 行 数 据 融 合 的 配 准 处 理 。 配 准 误 差 主 要 来 源 包 括 公 共 参 考 系 的 方 位 偏 移 误 差 和 各 传 感 器 的 距 离 偏 移 误 差 。 配 准 误 差 属 于 系 统 误 差 ， 它 们 给 融 合 引 入 偏 差 ，
.=== 年
第 > 期
数 据 融 合 的 精 确 极 大 似 然 配 准 算 法 在 六 自 由 度 !"# 中 的 应 用
哈 尔 滨 工 程 大 学 严 浙 平 边 信 黔 戴 学 丰 施 小 成
摘 要 文 章 阐 述 了 潜 器 六 自 由 度 动 力 定 位 的 基 本 原 理 和 多 传 感 器 数 据 融 合 的 基 本 概 念 ， 着 重 研 究 了 配 准 的 极 大 似 然 算 法 $%&。 该 算 法 通 过 两 步 递 归 最 优 化 方 法 来 实 现 ， 并 采 用 改 进 的 高 斯 — 牛 顿 法 来 确 保 算 法 的 快 速 收 敛 性 。 最 后 结 合 ’#()— * 潜 器 模 型 水 池 实 验 的 实 时 数 据 给 出 了 仿 真 算 例 ， 证 实 了 $%& 算 法 在 实 际 环 境 中 的 有 效 性 。 关 键 词 数 据 融 合 动 力 定 位 系 统 配 准
和 " 对 目 标 第 & 次 测 量
的 距 离 和 方 位 。 相 应 的 传 感 器 偏 差 表 示 为 ｛ (! ， "
& & 和 ｛ 。 假 定｛ ) ’ ， ｝ 是 在 公 共 直 角 坐 * ’ " !! ｝ "(" ， " !" ｝ 标 系 中 的 真 实 位 置 。 则 有 & ［ ( & ［ & ) ’ -./ + ! , "(! ］ ! ," ! !! ］ & & ［ ( & ［ * ’ 01+! ］ & ［ ( & ［ & ) ’ -./ + 2 # " , "(" ］ " ," ! !" ］ & & ［ ( & ［ 01* ’ + 2 $ " , "(" ］ !" , " !" ］ & & 使 用 一 阶 近 似 对 ) ’ ， 列 出 相 应 的 方 程 ， 可 得 * ’ & 3# + ")&
图 %
有 融 合 功 能 的 "#$ 原 理
%
.<+
数 据 融 合 的 配 准 算 法
数 据 融 合 及 配 准 概 念 数 据 融 合 定 义 为 一 个 对 来 自 多 源 的 数 据 和 信 息
进 行 互 联 、 相 关 、 组 合 处 理 以 求 得 精 确 位 置 和 识 别 估 计 的 过 程 。 数 据 融 合 通 常 被 分 为 三 级 ： 一 级 处 理 包 括 配 准 、 互 联 、 滤 波 和 识 别 ； 二 级 处 理 为 态 势 估 计 ； 三
为 了 进 行 潜 器 六 自 由 度 动 力 定 位 （ !012345 "2/ 技 术 的 研 究 ， 我 们 参 照 某 型 艇 的 647481419 #067:3 !"#） 尺 寸 制 造 了 缩 尺 度 的 水 池 试 验 潜 器 模 型 ’#()— * （ 参 见 图 +） ， 它 长 约 ; < +3， 艇 体 宽 = < -3， 最 大 高 度 + < 带 有 五 台 推 进 电 机 、 纵 横 倾 调 节 装 置 ， 艇 上 还 装 +3， 有 . 台 深 度 传 感 器 、 短 基 线 定 位 声 纳 、 测 高 声 纳 、 罗 经 和 纵 横 倾 传 感 器 等 传 感 器 件 。
% % % % ， ， … ， ］ （ 3 +［ 3（ /） 3（ ,） 3 （ & ） 4） % % % % ， ， … ， )（ & ） ］ （ /） ,） 5） ") +［ ")（ ")（ " 因 此 式 （ 3） 的 37 解 为 % ,/ % （ 6） #37 +（ 3 3 ） 3 ") 可 见 这 种 做 法 对 传 感 器 量 测 都 加 了 相 等 的 权 。
!
潜 器 模 型 简 介 及 "#$ 原 理
位 置 并 保 持 一 定 的 姿 态 ， 就 必 须 在 船 上 安 装 动 力 定 位 控 制 系 统 以 提 供 反 抗 海 洋 扰 动 的 反 力 和 反 力 矩 ， 其 作 用 原 理 图 见 图 .。 它 与 传 统 的 !"# 相 比 ， 主 要 是 增 加 了 融 合 中 心 这 一 环 节 ， 控 制 中 心 使 用 的 检 测 数 据 是 经 过 融 合 处 理 的 。