基于Kalman滤波的AUV两位置对准方法
合集下载
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
得到载体坐 标系下的陀螺漂移（ 要是水平方 向） 主 ，同时实现
高精度 的初始对准 。这种方法主要是解决静基座条件 下东 向 陀螺漂移的完全不可观测 ，以及 由此导致的方位 失准角精 度
不高的 问题 。一 般情 况下 ，基于 Ｋ ｌ ｎ滤波的静基座对 准 ａ ｍａ
都是在载体完全静止 的情况下进行 的。此时 ，地 理坐标系下
系统的初始对准 。
』（ ） ＝ （０ ｌ０ Ｒ （ ０） ＋ ０） Ｒ Ａ‘ ） ’ ａＭ
从 而可得 ：
‘
（ ４ ）
㈣
即￡ 的相关时间１ 比 的相关时间１ 多 ／ （ 倍。 ／ ／ ０ ）
由此可 得到 陀螺 的误差方程 ：
基金项 Ｉ ：黑龙江省教育厅基金资助项 目（１４ ３ ０ ｉ １ １５ １９ ）
ｔ ｗｏ— ｏ ｉｉ ｎ ｉ ｔ ｇ ａ ｅ ｌ ｎ ｎ ａ ｅ ｎ Ｋａ ｍａ ｌ ｒｎ ｓｐ ｏ ｏ ｅ ｎ ｔ ｉ ａ ｅ ． ｅｉ ｅ ｔ ｌ ａ ｉ ａ ｉ ｎ ｅ ｒ ｒｍｏ ｅ ｉ ｎ ｅｐ ｉ ｃ ｐ ｅ ｏ ｐ ｓｔｏ ｅ ｒ ｔ ｄ ａ ｉ ｍｅ ｔ ｓ ｄ ｏ ｌ ｎ ｇ ｂ ｎ ｆ ｔ ｉ ｇ ｉ ｒ ｐ ｓ ｄ ｉ ｓｐ ｐ ｒ Ｔｈ ｒｉ ｖ ｇ ｔｏ ｒｏ ｄ ｌ ｓｂｕ ｌ ａ ｄ ｔ ｒｎ ｉ ｌ ｆ ｉ ｅ ｈ ｎ ａｎ ｉ ｔ ｈ ｔ ｗｏ－ ｏ ｉｉ ｎ ａ ｉ ｎ ｎ ｓ ａ ａｙ ｅ Ｔｈ ｙ ｔ ｍａ ｃ ｏ ｅ ｖ ｂ ｌ ｙ ｉ ｍｐ ｏ ｅ ｙ ｃ ｎ ｇ ｎ ｅ ｓｒ ｐ ｏ ｒ ｘ Ｔｈｅ ｄ ｇ ｅ ｆ ｏ ｅ ｖ ｂ ｌ ｙ ｆ ｒ ｐ ｓｔｏ ｌｇ ｍｅ ｔｉ ｎ ｌ ｚ ｄ． ｅ ｓ ｓｅ ｔ ｂｓ ｒ ａ ｉｉ ｓ ｉ ｒ ｖ ｄ ｂ ｈａ ｉ ｇ ｔ ｔａ ｄ ｗｎ ｍａ ｉ ｔ ｈ ． ｅ ｒ ｅ ｏ ｂｓ ｒ ａ ｉｉ ｏ ｔ
为实现 Ａ ＵＶ 的高精度定位 ，在 自主工作前需要对 其安
０
（） １
其均方值为 Ｒ 。一阶马尔可夫过程模 型方程为 ：
毫 Ｍ＝一 【 ￡ 芎 ０ ＋ Ｍ Ｍ’ ｔ
其中 是 关 间 数； ｛ ｆ＝Ｍ ） 随 程方 ， 相 时 常 Ｅ ） Ｒ（ 是 机过 （｝ ０
差 ； 是零均值 的自噪声 。 将式（） １和式（） ２合并 ， 令￡＝ ＋ ，
第 ３ 卷 第 １ 期 ７ １
Ｖｏ ．７ １３
・
计
算
机
工
程
２ １ 年 ６月 ０１
Ｊ ｎ ２ ｌ ｕ ｅ ０１
Ｎ ｏ． １ １
Ｃｏ ｍｐｕ ｅ ｇｉ ｅ ｒｎ ｔｒＥｎ ｎ ｅ ｉ ｇ
工程应用技术与实现 ・
文 编 ： ０ ＿ ４ （ ｌｌ ６ ＿ 章 号 １ ３ ８０ ）— ２ ＿ ｏ ２２ １１ ｏ
人 ，计算机仿真 ；戴学丰 ，教授、博士
ቤተ መጻሕፍቲ ባይዱ收稿 日期 ：２ １－２１ ００１— ２
Ｅｍａ ： ａｉ ９８０ ＠ｓ ａ ｏ － ｉ ｈｏ ｅ１８０ ｉ ． ｍ ｌ ｃ ｎｃ
第 ３卷 ７
第 ｌ 期 １
郝
冰 ，戴学丰 ：基于 Ｋ ｌ ｎ滤波的 Ａ ａ ｍａ ＵＶ两位置对准方法
２１ ６
其 中， 状态变量取 １ ０个 ， ｇ为当地重力加速度 ， ｏｉ Ｔ（ ＝１ ，） ， ，３ ２
则：
＝一 ‘ ＋ （） ３
装的捷联惯性 导航系统进行 初始对 准，而捷联惯导 系统初始 对 准就是确定从载 体坐标 系到计 算地理坐标系 的初始捷联矩 阵 ，并且进行姿态校 准。提高对 准精度、减少对准时 间Ｉ是 ４
初始对 准中的主要 问题 。
使 的均方值 （） ２个分量的均方值 （） Ｏ为 ０ 和 之
的误差 实际上由 ｒ 和 共 同决定 。 考虑这里 的应用对象主要是舰船导航系统 ，因此，忽略 垂直 通道的影响 ，并将 陀螺仪和加速度计误差扩充为状态变
量。此时 ，系统的状态 方程可 以写成 如下 形式 ：
（ ＝Ａ（ （ ＋ ｆ （ ｆ ） ｆ ｆ （ ｗ ｆ ） ） ） ） （３ １）
［ ｓｒｃ］Ａｃｏｄｎ ｏｔｅｓｅ ｉｃｗ ｒ ｎ ｉ ｎ ｎ ｎ ｅａｉｎ ｎ ｃｕａｙｏ ｔｎ ｍｏ ｓＵｎ ｅｗａ ｒＶ ｈｃ ｓＡ Ａｂ ｔａ ｔ ｃｒｉｇ ｔ ｐｃｆ ｏｋｅ ｖｒ ｍｅ ｔ ｄｔ ｌ ｍｅ ｔａｃｒｃ ｆＡｕｏ ｏ ｕ ｄｒ ｔ ｅｉｌ （ ＵＶ） ａｍｏ ｉｅ ｈ ｉ ｏ ａ ｈ ｇ ｅ ｅ ， ｄｆ ｄ ｉ
ＤＯＩ １．９ ９ｊｓｎ１０ —４ ８２ １．１ ９ ： ０３ ６ ／ｉ ． ０３ ２ ．０ １１ ． ０ ．ｓ ０ ０
１ 概述
自主式水下潜器（ ｕｏ ｏ ｕ ｎ ｅｗａ ｒ ｅ ｉ ｅ， Ｖ） Ａ ｔｎｍｏ ｓ ｄｒ ｔ ｈｃ ｓＡｕ Ｕ ｅＶ ｌ
噪声误差等 。惯性器件误差对 系统有很大的影响 ，因此在 ］ 这里对 其误差作全面 的分析 ， 在实际应用时，如果需要简化 ， 则可令相应 的项 为 ０ 。陀螺零漂 的误差模型方程为 ：
为载体的航向角，即载体系 的 Ｘ轴、Ｙ轴方向与地
理系东 向、 北向之间的夹角 。由式（５可知 ，东 向陀螺漂移 １） 的不可观测必然导致估计 的 、 Ｅ 不准确 ，而且 、 ￡ 偏 ｙ 离真实值 的大小 可以通过式（５定量 的描述 。由于初始对准 １）
（２ １）
时 ， 向角 也存在误差 （ 航 设为 ｒ ，因此估计得到的 ￡ 、￡ ）
立系统误差模型 ，分析 两位置组 合对准原理 ， 通过 改变惯导 系统误差模型 中的捷联矩 阵改善系统 的可观测性 ，利用谱条件数求取两位置对 准时系统参数可观测度 。仿真结果表 明，该方法能准确估计器件偏差 ，经补偿后能提高系统 的对准精度 。
关健 词 ：自主式水下潜器 ；两位置对准 ；Ｋ ｌ ａ ａ ｎ滤波；可观测性 ； ｍ 谱条件数
ｐ ｅ ｉｉｎ ｒ ｃ ｓｏ
［ ｙｗｏ ｄ ］Ａｕｏ ｏ ｕ ｎ ｅ ａｅ ｅ ｉｌｓ Ｋｅ ｒ ｓ ｔｎ ｍｏ ｓ ｄ ｒ ｔｒ ｈｃｅ（ Ｕ ｗ Ｖ ＡＵＶ）ｔ —ｏｉｏ ｉｎ ｎ； ｌ ｎｆ ｔ ｎ ；ｂ ｅｖｂｌｙ ｓｅｔａｃ ｎ ｉｏ ｕ ｅ ；ｗｏｐ ｓ ｉｎａ ｇ ｍｅ ｔＫａｍａ ｌ ｒ ｇ ｏ ｓｒａｉｔ；ｐ ｃｒｌ ｏ ｄｔ ｎｎ ｍｂｒ ｔ ｌ ｉｅ ｉ ｉ ｉ
ｘ为系统 的状态变量 ；Ｖ是系统观测噪声矢量 ，为高斯 自噪
声过程 。
３ 静基座下两位置组合对准原理
３１ 改进的两位置对准方案 ． 改进 的两位置法是指在对准过程 中，使载体在航向上处 于任意 ２个不 同位置 ，并使 Ｋ ｌ ｎ滤波器在这 ２个不同位 ａ ｍａ 置进行估计 ，利用陀螺漂移估计值之间 的关系经数据处理 ，
和， 并且 ￡和 的相关函数在相 关时间 ｆ 处 的斜率相 同。 ＝０ （） ０ 和 的值可 由以下 ２个方程 确定 ：
捷联 惯导系统的可观测性较 差，尤其 是在 静基座环境 下
系统 可观测性最弱 ， 在采用 Ｋ ｌ ａ 滤波技术 解决捷联惯导 ａ ｎ ｍ 系统精对 准问题的过程 中，较差 的观测性 影响了滤波器进行 状态估计的收敛速 度和估计精度 。而通过 Ａ Ｖ 的转动 能 Ｕ 够改变系统误差模型中的捷 联矩阵 ，从而改善惯导系统的可 观测性。为此，本文提 出一种 ＡＵ 两位置对准方法 ，利用 Ｖ Ｋ ｌａ 滤波技术实现对系统误差参数的估计 ， ａ ｎ ｍ 经补偿后完成
Ｔ ｗｏ ｐ ｓｔｏ ｉ ｎ ｅ ｔ ｅ ｈ ｄ ｏ — ｏ ｉｉ ｎ Ａｌ ｍ ｎ ｔ ｏ ｆ ｇ Ｍ ＡＵＶ
Ｂａ ｅ ｎ ａ ｍ ａ Ｆｉｔ ｒ ｎｇ ｓ ｄ ０ Ｋ ｌ ｎ ｌｅ ｉ
ＨＡＯ ｎ ， Ｂｉ ｇ ＤＡＩＸｕ － ｎ ｅｆ ｇ ｅ
（ ｏ ｌ ｅｏ Ｃ ｍｐ ｔｒ ｎ ｎｒ ｌ ｎ ｉｅ ｒ ｇ Ｑｉｉａ ｉｅ ｓ ｙ Ｑｉｉａ ６ ０ ６ Ｃ ｉａ Ｃ ｌ ｇ ｆ ｏ ｕ ｅ ｄＣｏ ｔ ｇｎ ｅ ｎ ， ｑｈ ｒ ｖ ｒｉ ， ｑｈ ｒ １ ０ ， ｈｎ ） ｅ ａ ｏＥ ｉ Ｕｎ ｔ １
为捷联矩 阵 中的元素 。 选取 ２个水平速度误差 、
观测量 ，方程如下 ：
Ｚ（） ｆ＝日 （ （） ｙ（） ｆ ｆ＋ ｆ ）
为
（４ １）
其 是 关时 常 中， 相 间 数；Ｅ （｝ （ 是 机 程 ｛ ｆ＝ ０ 随 过 方 ） ）
其，观 量＝ 】＝ ； 中为 向，Ｏ ［ ￣ ｚ 测 ｚ ｅ ； ，ｏｏ ］ ［ 日 ｏ Ｗ ｏｏｏ ｏｏ
Ｓｒｐｄ ｗ ｅｔ ｌ ｖｇｔ ｎ Ｓ ｓ ｍ（Ｉ ）ｉ ｃｍｐ ｔｄｉ ｅｃｎｅｔ ｆｔ —ｏｉｏ ｌ ｎ ｎ ｃ ｅ ｓ ｇｔｅｓｅｔ ｌ ｏ ｄｔ ｎｎ ｍｂ ｒ ｔａ—ｏ ｎ Ｉ ｒａ Ｎａｉａ ｏ ｙｔ ＳＮＳ ｓ ｏ ｕｅ ｔ ｏ ｔｘ ｐ ｓ ｎａｉ ｍｅ ｔ ｈｍｅｕｉ ｐ ｃ ａ ｎｉｏ ｕ ｅ ｎ ｉ ｉ ｅ ｎｈ ｏ ｗｏ ｉ ｔ ｇ ｓ ｎ ｈ ｒ ｃ ｉ
＝
在民用和军事方面都有广泛的应用 。军事方面如战 区侦察、 探测扫除水雷、潜艇对抗、海上预警、封锁航线或港 Ｅ、攻 ｌ 击敌舰船或潜艇、破坏石油设施及通信 网络、水下中继通信 等 ；民用方面如海洋资源勘察与开发、海洋环境时空变化的 监测、海 底地 形地 貌调查与勘测以及深海技术、水下设施检 查、海洋救 险和打捞等 ３ １］ －￣
ｈ ｏ ｙ ｉ ｌｔｏ ｅ ｕ ｔ ｈ ｗ ｔｔ ｅ ｉ ｅ ｅ ｒ ａ ｅ ｅ ｔｍａ ｅ ｙ ｔ ｏ ｅ ｗｏ ｐ ｉ ｏ ｇ ｍｅ ｔ ａ ｒ ａ ｌ ｉ ｒ ｖ ｇ ｍｅ ｔ ｔ ｅ ｒ ．Ｓ ｍ ｕ ａｉ ｎ ｒ ｓ ｌｓ ｓ ｏ ｔ ａ ｅ ｄ ｖ ｃ ｒ ｏ ｓ ｃ ｎ ｂ ｓｉ ｔ ｄ ｂ ｅ ｎ ｖ ｌ ｔ — ｏｓｔ ｎ ａ ｉ ｎ ｎ ， ｄ ｇ ｅ ｔ ｍ ｐ ｏ ｅ ｔ ｅ ａ ｉ ｎ ｎ ｈ ｈ ｈ ｉ ｌ ｎ ｙ ｈ ｌ
Ｏ Ｏ ０
的等效北向陀螺漂移 完全 可观测 ， 而等效东向陀螺 漂移
完全不可观测。
ｏ Ｏ ０
０ ０
ｇｇ， 阵￡ ［ Ｅ ］和ｖ Ｅ Ｖ］ 在载 － 矩 ＝ ｙ ＝ｖ ， Ｖ 都 体
地理坐标系下的 、
与载体坐标系下 的沿 轴、Ｙ轴
的陀螺漂移 ￡ 、 ￡ 存在 如下关系：
Ｏ ０
０ ０
：。 ｌ ｌｌ 一“ ｌ ｌ 。 ｌ ｉ
Ｌ ｓ Ｋ ｏＫＪ ． ＧＪ ｉ ｃｓ ｅ ｎ ＬＪ
或：
（） １ ５
ｉ Ｉｌ 。 ｓ ｌｌ ｊｌ ： Ｌ “ Ｉ
—
ｓＫ ｏ ｉ ｃｓ ｎ 儿 ｊ
（ １ ６ ）
其中 ，
…
２ 基于速度信息的组合对准数学模型
２ 惯性器件误差模型 ． １ 陀螺 的误差 ￡ 是根 据长时间测试结果建模来确定 的，通 常包括 随机常值 （ 即零位 漂移） ，一 阶马尔可夫过程和 白 误差
作者简介 ： 郝 冰（９９ ） 女 ， 师、 士 ， １７ － ， 讲 硕 主研方 向： 智能机器
文 标 码。 献 识 Ａ
中 分 号 Ｕ６１ 圈 类 ： ６． ６
基 于 Ｋａｍａ ｌ ｎ滤 波 的 ＡＵＶ 两位 置 对 准 方 法
郝 冰 ，藏学丰
（ 哈尔大 学计 算机与控 制工程 学院，黑龙江 齐齐哈尔 １ １０ ） 齐齐 ６ ０ ６ 摘 要 ：针对 自主式水下潜器（ Ｕ ） Ａ Ｖ 的特定工作环境及导航系统对准精度的需求 ，提出一种基于 Ｋ ｌ ａ ａ ｎ滤波的两位置组合对准方法。建 ｍ
高精度 的初始对准 。这种方法主要是解决静基座条件 下东 向 陀螺漂移的完全不可观测 ，以及 由此导致的方位 失准角精 度
不高的 问题 。一 般情 况下 ，基于 Ｋ ｌ ｎ滤波的静基座对 准 ａ ｍａ
都是在载体完全静止 的情况下进行 的。此时 ，地 理坐标系下
系统的初始对准 。
』（ ） ＝ （０ ｌ０ Ｒ （ ０） ＋ ０） Ｒ Ａ‘ ） ’ ａＭ
从 而可得 ：
‘
（ ４ ）
㈣
即￡ 的相关时间１ 比 的相关时间１ 多 ／ （ 倍。 ／ ／ ０ ）
由此可 得到 陀螺 的误差方程 ：
基金项 Ｉ ：黑龙江省教育厅基金资助项 目（１４ ３ ０ ｉ １ １５ １９ ）
ｔ ｗｏ— ｏ ｉｉ ｎ ｉ ｔ ｇ ａ ｅ ｌ ｎ ｎ ａ ｅ ｎ Ｋａ ｍａ ｌ ｒｎ ｓｐ ｏ ｏ ｅ ｎ ｔ ｉ ａ ｅ ． ｅｉ ｅ ｔ ｌ ａ ｉ ａ ｉ ｎ ｅ ｒ ｒｍｏ ｅ ｉ ｎ ｅｐ ｉ ｃ ｐ ｅ ｏ ｐ ｓｔｏ ｅ ｒ ｔ ｄ ａ ｉ ｍｅ ｔ ｓ ｄ ｏ ｌ ｎ ｇ ｂ ｎ ｆ ｔ ｉ ｇ ｉ ｒ ｐ ｓ ｄ ｉ ｓｐ ｐ ｒ Ｔｈ ｒｉ ｖ ｇ ｔｏ ｒｏ ｄ ｌ ｓｂｕ ｌ ａ ｄ ｔ ｒｎ ｉ ｌ ｆ ｉ ｅ ｈ ｎ ａｎ ｉ ｔ ｈ ｔ ｗｏ－ ｏ ｉｉ ｎ ａ ｉ ｎ ｎ ｓ ａ ａｙ ｅ Ｔｈ ｙ ｔ ｍａ ｃ ｏ ｅ ｖ ｂ ｌ ｙ ｉ ｍｐ ｏ ｅ ｙ ｃ ｎ ｇ ｎ ｅ ｓｒ ｐ ｏ ｒ ｘ Ｔｈｅ ｄ ｇ ｅ ｆ ｏ ｅ ｖ ｂ ｌ ｙ ｆ ｒ ｐ ｓｔｏ ｌｇ ｍｅ ｔｉ ｎ ｌ ｚ ｄ． ｅ ｓ ｓｅ ｔ ｂｓ ｒ ａ ｉｉ ｓ ｉ ｒ ｖ ｄ ｂ ｈａ ｉ ｇ ｔ ｔａ ｄ ｗｎ ｍａ ｉ ｔ ｈ ． ｅ ｒ ｅ ｏ ｂｓ ｒ ａ ｉｉ ｏ ｔ
为实现 Ａ ＵＶ 的高精度定位 ，在 自主工作前需要对 其安
０
（） １
其均方值为 Ｒ 。一阶马尔可夫过程模 型方程为 ：
毫 Ｍ＝一 【 ￡ 芎 ０ ＋ Ｍ Ｍ’ ｔ
其中 是 关 间 数； ｛ ｆ＝Ｍ ） 随 程方 ， 相 时 常 Ｅ ） Ｒ（ 是 机过 （｝ ０
差 ； 是零均值 的自噪声 。 将式（） １和式（） ２合并 ， 令￡＝ ＋ ，
第 ３ 卷 第 １ 期 ７ １
Ｖｏ ．７ １３
・
计
算
机
工
程
２ １ 年 ６月 ０１
Ｊ ｎ ２ ｌ ｕ ｅ ０１
Ｎ ｏ． １ １
Ｃｏ ｍｐｕ ｅ ｇｉ ｅ ｒｎ ｔｒＥｎ ｎ ｅ ｉ ｇ
工程应用技术与实现 ・
文 编 ： ０ ＿ ４ （ ｌｌ ６ ＿ 章 号 １ ３ ８０ ）— ２ ＿ ｏ ２２ １１ ｏ
人 ，计算机仿真 ；戴学丰 ，教授、博士
ቤተ መጻሕፍቲ ባይዱ收稿 日期 ：２ １－２１ ００１— ２
Ｅｍａ ： ａｉ ９８０ ＠ｓ ａ ｏ － ｉ ｈｏ ｅ１８０ ｉ ． ｍ ｌ ｃ ｎｃ
第 ３卷 ７
第 ｌ 期 １
郝
冰 ，戴学丰 ：基于 Ｋ ｌ ｎ滤波的 Ａ ａ ｍａ ＵＶ两位置对准方法
２１ ６
其 中， 状态变量取 １ ０个 ， ｇ为当地重力加速度 ， ｏｉ Ｔ（ ＝１ ，） ， ，３ ２
则：
＝一 ‘ ＋ （） ３
装的捷联惯性 导航系统进行 初始对 准，而捷联惯导 系统初始 对 准就是确定从载 体坐标 系到计 算地理坐标系 的初始捷联矩 阵 ，并且进行姿态校 准。提高对 准精度、减少对准时 间Ｉ是 ４
初始对 准中的主要 问题 。
使 的均方值 （） ２个分量的均方值 （） Ｏ为 ０ 和 之
的误差 实际上由 ｒ 和 共 同决定 。 考虑这里 的应用对象主要是舰船导航系统 ，因此，忽略 垂直 通道的影响 ，并将 陀螺仪和加速度计误差扩充为状态变
量。此时 ，系统的状态 方程可 以写成 如下 形式 ：
（ ＝Ａ（ （ ＋ ｆ （ ｆ ） ｆ ｆ （ ｗ ｆ ） ） ） ） （３ １）
［ ｓｒｃ］Ａｃｏｄｎ ｏｔｅｓｅ ｉｃｗ ｒ ｎ ｉ ｎ ｎ ｎ ｅａｉｎ ｎ ｃｕａｙｏ ｔｎ ｍｏ ｓＵｎ ｅｗａ ｒＶ ｈｃ ｓＡ Ａｂ ｔａ ｔ ｃｒｉｇ ｔ ｐｃｆ ｏｋｅ ｖｒ ｍｅ ｔ ｄｔ ｌ ｍｅ ｔａｃｒｃ ｆＡｕｏ ｏ ｕ ｄｒ ｔ ｅｉｌ （ ＵＶ） ａｍｏ ｉｅ ｈ ｉ ｏ ａ ｈ ｇ ｅ ｅ ， ｄｆ ｄ ｉ
ＤＯＩ １．９ ９ｊｓｎ１０ —４ ８２ １．１ ９ ： ０３ ６ ／ｉ ． ０３ ２ ．０ １１ ． ０ ．ｓ ０ ０
１ 概述
自主式水下潜器（ ｕｏ ｏ ｕ ｎ ｅｗａ ｒ ｅ ｉ ｅ， Ｖ） Ａ ｔｎｍｏ ｓ ｄｒ ｔ ｈｃ ｓＡｕ Ｕ ｅＶ ｌ
噪声误差等 。惯性器件误差对 系统有很大的影响 ，因此在 ］ 这里对 其误差作全面 的分析 ， 在实际应用时，如果需要简化 ， 则可令相应 的项 为 ０ 。陀螺零漂 的误差模型方程为 ：
为载体的航向角，即载体系 的 Ｘ轴、Ｙ轴方向与地
理系东 向、 北向之间的夹角 。由式（５可知 ，东 向陀螺漂移 １） 的不可观测必然导致估计 的 、 Ｅ 不准确 ，而且 、 ￡ 偏 ｙ 离真实值 的大小 可以通过式（５定量 的描述 。由于初始对准 １）
（２ １）
时 ， 向角 也存在误差 （ 航 设为 ｒ ，因此估计得到的 ￡ 、￡ ）
立系统误差模型 ，分析 两位置组 合对准原理 ， 通过 改变惯导 系统误差模型 中的捷联矩 阵改善系统 的可观测性 ，利用谱条件数求取两位置对 准时系统参数可观测度 。仿真结果表 明，该方法能准确估计器件偏差 ，经补偿后能提高系统 的对准精度 。
关健 词 ：自主式水下潜器 ；两位置对准 ；Ｋ ｌ ａ ａ ｎ滤波；可观测性 ； ｍ 谱条件数
ｐ ｅ ｉｉｎ ｒ ｃ ｓｏ
［ ｙｗｏ ｄ ］Ａｕｏ ｏ ｕ ｎ ｅ ａｅ ｅ ｉｌｓ Ｋｅ ｒ ｓ ｔｎ ｍｏ ｓ ｄ ｒ ｔｒ ｈｃｅ（ Ｕ ｗ Ｖ ＡＵＶ）ｔ —ｏｉｏ ｉｎ ｎ； ｌ ｎｆ ｔ ｎ ；ｂ ｅｖｂｌｙ ｓｅｔａｃ ｎ ｉｏ ｕ ｅ ；ｗｏｐ ｓ ｉｎａ ｇ ｍｅ ｔＫａｍａ ｌ ｒ ｇ ｏ ｓｒａｉｔ；ｐ ｃｒｌ ｏ ｄｔ ｎｎ ｍｂｒ ｔ ｌ ｉｅ ｉ ｉ ｉ
ｘ为系统 的状态变量 ；Ｖ是系统观测噪声矢量 ，为高斯 自噪
声过程 。
３ 静基座下两位置组合对准原理
３１ 改进的两位置对准方案 ． 改进 的两位置法是指在对准过程 中，使载体在航向上处 于任意 ２个不 同位置 ，并使 Ｋ ｌ ｎ滤波器在这 ２个不同位 ａ ｍａ 置进行估计 ，利用陀螺漂移估计值之间 的关系经数据处理 ，
和， 并且 ￡和 的相关函数在相 关时间 ｆ 处 的斜率相 同。 ＝０ （） ０ 和 的值可 由以下 ２个方程 确定 ：
捷联 惯导系统的可观测性较 差，尤其 是在 静基座环境 下
系统 可观测性最弱 ， 在采用 Ｋ ｌ ａ 滤波技术 解决捷联惯导 ａ ｎ ｍ 系统精对 准问题的过程 中，较差 的观测性 影响了滤波器进行 状态估计的收敛速 度和估计精度 。而通过 Ａ Ｖ 的转动 能 Ｕ 够改变系统误差模型中的捷 联矩阵 ，从而改善惯导系统的可 观测性。为此，本文提 出一种 ＡＵ 两位置对准方法 ，利用 Ｖ Ｋ ｌａ 滤波技术实现对系统误差参数的估计 ， ａ ｎ ｍ 经补偿后完成
Ｔ ｗｏ ｐ ｓｔｏ ｉ ｎ ｅ ｔ ｅ ｈ ｄ ｏ — ｏ ｉｉ ｎ Ａｌ ｍ ｎ ｔ ｏ ｆ ｇ Ｍ ＡＵＶ
Ｂａ ｅ ｎ ａ ｍ ａ Ｆｉｔ ｒ ｎｇ ｓ ｄ ０ Ｋ ｌ ｎ ｌｅ ｉ
ＨＡＯ ｎ ， Ｂｉ ｇ ＤＡＩＸｕ － ｎ ｅｆ ｇ ｅ
（ ｏ ｌ ｅｏ Ｃ ｍｐ ｔｒ ｎ ｎｒ ｌ ｎ ｉｅ ｒ ｇ Ｑｉｉａ ｉｅ ｓ ｙ Ｑｉｉａ ６ ０ ６ Ｃ ｉａ Ｃ ｌ ｇ ｆ ｏ ｕ ｅ ｄＣｏ ｔ ｇｎ ｅ ｎ ， ｑｈ ｒ ｖ ｒｉ ， ｑｈ ｒ １ ０ ， ｈｎ ） ｅ ａ ｏＥ ｉ Ｕｎ ｔ １
为捷联矩 阵 中的元素 。 选取 ２个水平速度误差 、
观测量 ，方程如下 ：
Ｚ（） ｆ＝日 （ （） ｙ（） ｆ ｆ＋ ｆ ）
为
（４ １）
其 是 关时 常 中， 相 间 数；Ｅ （｝ （ 是 机 程 ｛ ｆ＝ ０ 随 过 方 ） ）
其，观 量＝ 】＝ ； 中为 向，Ｏ ［ ￣ ｚ 测 ｚ ｅ ； ，ｏｏ ］ ［ 日 ｏ Ｗ ｏｏｏ ｏｏ
Ｓｒｐｄ ｗ ｅｔ ｌ ｖｇｔ ｎ Ｓ ｓ ｍ（Ｉ ）ｉ ｃｍｐ ｔｄｉ ｅｃｎｅｔ ｆｔ —ｏｉｏ ｌ ｎ ｎ ｃ ｅ ｓ ｇｔｅｓｅｔ ｌ ｏ ｄｔ ｎｎ ｍｂ ｒ ｔａ—ｏ ｎ Ｉ ｒａ Ｎａｉａ ｏ ｙｔ ＳＮＳ ｓ ｏ ｕｅ ｔ ｏ ｔｘ ｐ ｓ ｎａｉ ｍｅ ｔ ｈｍｅｕｉ ｐ ｃ ａ ｎｉｏ ｕ ｅ ｎ ｉ ｉ ｅ ｎｈ ｏ ｗｏ ｉ ｔ ｇ ｓ ｎ ｈ ｒ ｃ ｉ
＝
在民用和军事方面都有广泛的应用 。军事方面如战 区侦察、 探测扫除水雷、潜艇对抗、海上预警、封锁航线或港 Ｅ、攻 ｌ 击敌舰船或潜艇、破坏石油设施及通信 网络、水下中继通信 等 ；民用方面如海洋资源勘察与开发、海洋环境时空变化的 监测、海 底地 形地 貌调查与勘测以及深海技术、水下设施检 查、海洋救 险和打捞等 ３ １］ －￣
ｈ ｏ ｙ ｉ ｌｔｏ ｅ ｕ ｔ ｈ ｗ ｔｔ ｅ ｉ ｅ ｅ ｒ ａ ｅ ｅ ｔｍａ ｅ ｙ ｔ ｏ ｅ ｗｏ ｐ ｉ ｏ ｇ ｍｅ ｔ ａ ｒ ａ ｌ ｉ ｒ ｖ ｇ ｍｅ ｔ ｔ ｅ ｒ ．Ｓ ｍ ｕ ａｉ ｎ ｒ ｓ ｌｓ ｓ ｏ ｔ ａ ｅ ｄ ｖ ｃ ｒ ｏ ｓ ｃ ｎ ｂ ｓｉ ｔ ｄ ｂ ｅ ｎ ｖ ｌ ｔ — ｏｓｔ ｎ ａ ｉ ｎ ｎ ， ｄ ｇ ｅ ｔ ｍ ｐ ｏ ｅ ｔ ｅ ａ ｉ ｎ ｎ ｈ ｈ ｈ ｉ ｌ ｎ ｙ ｈ ｌ
Ｏ Ｏ ０
的等效北向陀螺漂移 完全 可观测 ， 而等效东向陀螺 漂移
完全不可观测。
ｏ Ｏ ０
０ ０
ｇｇ， 阵￡ ［ Ｅ ］和ｖ Ｅ Ｖ］ 在载 － 矩 ＝ ｙ ＝ｖ ， Ｖ 都 体
地理坐标系下的 、
与载体坐标系下 的沿 轴、Ｙ轴
的陀螺漂移 ￡ 、 ￡ 存在 如下关系：
Ｏ ０
０ ０
：。 ｌ ｌｌ 一“ ｌ ｌ 。 ｌ ｉ
Ｌ ｓ Ｋ ｏＫＪ ． ＧＪ ｉ ｃｓ ｅ ｎ ＬＪ
或：
（） １ ５
ｉ Ｉｌ 。 ｓ ｌｌ ｊｌ ： Ｌ “ Ｉ
—
ｓＫ ｏ ｉ ｃｓ ｎ 儿 ｊ
（ １ ６ ）
其中 ，
…
２ 基于速度信息的组合对准数学模型
２ 惯性器件误差模型 ． １ 陀螺 的误差 ￡ 是根 据长时间测试结果建模来确定 的，通 常包括 随机常值 （ 即零位 漂移） ，一 阶马尔可夫过程和 白 误差
作者简介 ： 郝 冰（９９ ） 女 ， 师、 士 ， １７ － ， 讲 硕 主研方 向： 智能机器
文 标 码。 献 识 Ａ
中 分 号 Ｕ６１ 圈 类 ： ６． ６
基 于 Ｋａｍａ ｌ ｎ滤 波 的 ＡＵＶ 两位 置 对 准 方 法
郝 冰 ，藏学丰
（ 哈尔大 学计 算机与控 制工程 学院，黑龙江 齐齐哈尔 １ １０ ） 齐齐 ６ ０ ６ 摘 要 ：针对 自主式水下潜器（ Ｕ ） Ａ Ｖ 的特定工作环境及导航系统对准精度的需求 ，提出一种基于 Ｋ ｌ ａ ａ ｎ滤波的两位置组合对准方法。建 ｍ