设计与应用 计 算机 测量 与 控制. 2 008. 1 6( 8) Computer Measurement & Control # 1161 # 文章编号: 167124598( 2008) 08211612 03 中图分类号: T P2731 5 ; TH 8731 27 文献标识码: A 基于 MATLAB/ Simulink 的捷联惯性导航系统仿真 齐 鑫, 秦永元, 朱新颖, 张芳芳 ( 西北工业大学 自动化学院, 陕西 西安 710072) 摘要: 介绍了仿真软件 MAT LAB/ Simulin k 及先进的仿真系统平台软件 RT- Lab, 设计了龙格- 库塔积分模块, 用于解决 在使用定 步长求解器及数字时钟情况下的积分精度不高的问题; 建立了基于 MAT LAB/ Simu link 的捷联 惯性导航仿真 模型; 并将系统分 解成一系 列功能相对独立的模块, 如轨迹发生模块, 捷联惯导解算模块; 并进行了仿真验证, 得 到的姿态、速度、位 置误差与理论值 相符, 证明 了仿真模型建立正确、方法采用得当、有效。 关键词: RT- LAB; MAT LAB/ Simu link; 捷联惯导; 仿真 Simulation on Strap- down Inertial Navigation System Based on MATLAB/ Simulink Qi Xin, Qin Yongyuan, Zhu Xinying, Zhang Fangfang (Automatization College of Northwest P olytechnic university, Xi' an 710072, China) Abstract: Th e advanced simulation system platform RT- LAB and software MATL AB/ Simulin k are ex pou nded, th e Rung e- Kutta in2 tegrator models is designed , and strap- d own inertial navigation simu lation models based on M ATLAB/ Simulink is built1 Th e system was divided in to a series of independen t functional units, such as track generation unit, strap- down inertial navigation unit1 And take the vali2 date, Comparing with the real data, the results sh ow that the model is sound and the method is validity1 Key word s: RT- LAB; MATL AB/ Simulin k; strap- down inertial navigation; simulation 0 引言 MATLAB 是美国 MathWorks 公 司推 出 的功 能强 大 的大 型数学软件, 它以优秀的数值计算和卓越的数据可视化能力被 广泛应用于自动控制、数字信号处理、时间序列分析、动态系 统仿真等各个领域。作 为 M AT LAB 重要 组成 部分, Simulink 是 MATLAB 功能 的进 一 步扩 展, 由 于其 具有 简 洁的 编 程语 言、强大的计算功能、直观的 可视化 仿真结 果和灵 活的接 口, 非常适合对各种复杂系统进行仿真和开展相关研究[ 1] 。 RT- LAB 是加拿大 Opal- RT 公 司开发 的一 套基于 模型 的仿真系统平台软件包, 可以直接利用 MATLAB/ Simulink 建 立的动态系统数学模型应用于实时仿真、控制、测试及其它相 关领域。平台支持硬件在回路中的仿真; 支持与其它非实时仿 真平台的协同仿真; 能自动 划分模 型并产 生分布式 仿真代 码, 在仿真模型各子系统之 间可以 获取 实时通 信和 同步控 制[2- 3] 。 其特性为: (1) 快 速性好: 只需用 Simulink 设 计好的模型 中嵌入 RT - LAB 的 针对 实时性 的模 块, 然后 在 R T- LAB 提供的 简单 易用的界面中点几下鼠标, 就可以得到实时运行的仿真模型。 (2) 操 作性好: 具有在线参数编辑功能, 可以在运行时动 态整定模型参数。 收稿日期: 2007211206; 修回日期: 20072 12225。 基金项目: 总装备部十一五惯性技术基金( 51309060401) 。 作者简介: 齐 鑫( 19822 ) , 男, 陕西西 安人, 硕 士研究 生, 主 要从事 惯性导航及检测技术方面的研究。 秦永元( 19462) , 男, 江苏省常熟 市人, 教授, 博 士生导 师, 主 要从事 惯性导航与容错组合导航系统, 最优估计理论, GPS 信号处理的研究。 ( 3) 扩展性好: R T- LAB 支持众 多 NI 的 P CI 接 口的 硬 件板卡。 ( 4) 实时性好: 提供基 于 PC 平 台的 优秀 的硬 实时 性能, 且确保并行运算不会 改变模型的行为, 引入实时干扰, 或造成 死锁。 1 捷联惯导系统设计 依据 MAT LAB/ Simulink 作为仿真开发平台, 利用其优秀 的图形用户界面功能 , 按照自 顶向下的顺序构建仿真模型, 将 整个捷联惯导系统模 型分为三大模块: 轨迹发生器、捷联惯导 仿真器和结果比较, 如图 1 所示。仿真支 撑环境 (R T- LAB) 通过分布式节点系统 将 仿真 模 型分 布到 多 台 P C ( 目 标节 点) 上并行运算, 并将结 果在主 机上显 示。目标节 点之间 用 IEEE 1394 互联, 保证节 点间 实时数 据交 换迅 速可 靠。目标 机和 主 机通过局域网连接。 图 1 捷联惯导系统设计结构框图 轨迹发生器产生 的载体真 实运动 状态参 数 ( 诸如 角速度、 加速度、姿态、速度、位置等) 经仿真支撑环境传输给捷 联惯 导仿真器作为它的输入和激励 信号。通 过结果比较模块可以对 各模块产生的 载体 导航 参数 进行 比 较, 由主 机显 示器 显示 结 果, 对仿真结果进行分析。 

# 1162 # 计算机测量与控制 第 16 卷 2 轨迹发生器结构设计 轨迹发生器的设计目的是生成惯性器件信息源 ( 比力和角 速度), 并给出相应 轨迹点 的航行 参数 ( 姿 态、速度、 位置) 。 比力和角速度信息用作捷联算法的数据输入, 经过捷联算法解 算后其结果可以和前述航行参数作比较。为了便于对轨迹的描 述, 定义坐标系 t : 用 oxty tz t 表示, xt 轴水 平向右, yt 轴与轨 迹相切指向 轨迹前进方向, z t 轴与 yt 轴在铅直面内。不考虑载 体运行时攻角和侧滑角, 则轨迹坐标系到导航坐标系的转换矩 阵 Cn t 为: Cn t = - cosW cosHsinW - sinW cosHcosW - 0 sinH sinHsinW sinHcosW cosH (1) 载体航行是相对地球表面而言的, 其基本运行状态的改变 可以看作是由载体姿态 角和航 向角 的变化 ( 用 X(t) 表 示) 以 及载体的轨迹加速度 ( 用 a t (t) 表 示) 发生 改变引起 的, 那么 可以以 X( t) 和 a t (t) 作为输入生成轨迹, 轨迹发生器的设计方 法就是根据设定的航行运动状态解算一组轨迹微分方程组。 误差满足相应的误差 范围的情况下才可以对系统进行下一步仿 真。然而对于定步长求解器, 却无法实现误差控 制。对于 相对 平稳变化的输入使用 定步长求解器与变步长求解器仿真差别不 大, 但对于高动态输入量的积 分, 定步 长求解器会产生较大误 差, 影响仿真精度。 假设积分器输入 量为向量: X( t) = [ AXcosXt - AXsinXt ] T ( 8) 其 中 X为 10 P , A为 P / 180。仿真时间为 10s, 变步长求解器选 用 ode45, 模型 中始 终 选用 Clock。定 步 长求 解 器选 用 ode4, 时钟选用 Digital Clock ( 这是 由于某 些仿 真支撑 环境 ( 如 R T - LAB) 只支持数字 时钟) 。由于变 步长求 解器相对 定步 长求 解器精度更高, 可近似为真值 , 两者积 分结果的差值可近似为 定步长求解器的解算误差, 如图 2 所示, 可以看出定步长 求解 器对输入的积分误差 , 量级为 10 的- 5 次方, 误差较大。 姿态角微分方程: . H . C . W = X(t) 位置微分方程 . 速度微分方程: v . L . K . h . n (v ib = Cb cosC 0 1 比力: f b 角速度: Xn sf = Cb Xb nb = = vn U n ( (Xn n + Xi in + 2Xn in + Xb nb sinCcosH - sinH n = a n = Cn t a t (t) = vn N / (RM + h) = vn E secL/ ( RN + h) ie ) @) vn - gn ) 0 sinC 0 - cosCcosH # X( t) 图 2 定步长求解器积分误差 (2) (3) (4) (5) (6) (7) 其中 n 表示导航坐标系、i 表示惯性坐标系、b 表示载体坐 标系、H为 俯仰角、C为横滚角、W为偏航角, E、N、U 分别表示地 理坐标系的东、北、天向。根据 以上数 学模型 建立 仿真模 型, 其中轨迹微分方程 组的求 解由 Simulink 连 续系 统模 块库 中的 Integrator 模块实现。 3 高动态环境下积分求解器结构设计 连续系统一般由 微分 方 程或 与之 等价 的其 他方 式进 行描 述, 使用 Simulink 在对 连续 系统 进行 求解 仿真 时所 得到 的结 果均为近似解。Simulink 的 连续求 解器对微分方程的数字求 解有不同 的近 似 解 法, 如 变 步 长 求 解 器 ode45、ode23、 ode113, 以 及 定 步 长求解 器 ode5、ode4、ode3 等 等。 采用不同的连续求解器会 对连续系 统的仿真结果与仿真速度 产生不同 图 3 龙格- 库塔求解器积分误差 为此本文设计了龙格- 库 塔积分模块, 以实现在使用定步 长求解器及数字时钟 情况下的积分功能。仿真条件不变, 使用 龙格- 库塔积分模块, 误差如图 3 所示。可以看出龙格- 库塔 积分模块对输入量的 积分误差, 量级为 10 的- 13 次方, 误差 几乎为零。 针对轨迹发生器, 设计的龙格- 库 塔积分模块仿真模型框 的影 响。当 采 用 变 步 长 连 续 求 解 器, 可根 据 积 分 误 差 修 改 仿 真 步 长, 仿真步长计算受到绝 对误差与 相对误差的共同控制, 只 有在求解 图 4 龙格- 库塔法求解航行参数仿真模型框图 

第 8 期 齐 鑫, 等: 基于 MATLAB/ Simulink 的捷联惯性导航系统仿真 # 1163 # 坐标系内的投影; 加入 Transport Delay 模 块解决了姿态更新与速 度位置 更新周 期任 意匹配的问题。 为了验证算法及 模块的 精确性, 以锥 运动为输入, 轨迹发生器输入量为: Xat = [AXcosXt - AXsinXt 01@4 ] T (10) 其中 X为 10P, A为 P/ 180, 仿真时 间为 10 秒。在不考 虑各 种误差 的情 况下 分别 选 用姿态单子样算法模 块、二子样 算法模块、 三子样算法模 块。由 于 篇幅 原 因此 处紧 给 出部分仿真结果如图 6 ~ 图 8 所示。 图 7 姿态二子样算法姿态误差 图 8 姿态三子样算法姿态误差 从以 上 三 图 可 以 看 出, 在 锥 运 动 条 件 下, 仿 真 时 间 为 30s。算法姿态误差分别 为: 单 子样, 10 的- 1 次 方量 级; 二 子样, 10 的- 3 次方量级; 三子样, 10 的- 5 次方量 级, 与四 元数旋转矢量优化算 法理论完全吻合。 5 结论 本文在分析 simulink 积分求解器的基础上, 提出了一种适 用于高动态仿真环境下的积分 求解器, 进而设计了高动态捷联 惯导仿真系统。通过对模型的验证。结果表明: 捷联惯性 导航 系统模型建立正确、方法采用 得当、有 效。 参考文献: [ 1] 姚 俊, 马松辉. Simulink 建模与仿 真 [ M] . 西 安: 西安电 子科 技大学出版社, 2002. [ 2] RT - LAB 61 0 User. [ 3] 徐小英, 王 林. RT - LAB 快 速控 制原 型在随 动系 统中 的应 用 s Manual [ Z] . OPAL- RT Inc, 2004. [ J] . 系统仿真学报, 2006, 18 ( 4) : 1055- 1057. [ 4] 秦永元. 惯性导航 [ M] . 北京: 科学出版社, 2006. 图 5 捷联惯导仿真器模型框图 图如图 4 所 示。模 型的 输入 tr _ 1 是 上一 时刻 得到 的航 行参 数, wat0、wat1、 wat2 分 别 为 等 间 隔 时 间 点 上 的 运 行 信 息 X(t) 和 a t( t) 。通过四阶龙格- 库塔法计算出 当前时刻 的航行 参数 ( 姿态、速度、位置) 。 4 捷联惯导仿真器结构设计 依据捷联惯导系统核心公式[4] : 姿态四元数微分方程: dQ dt = 1 2 Xn nb ª Q (9) 比力方程: 式 (5), 位置微分 方程: 式 (6), 解算方 法为四元 数的旋转矢量法。设计捷联惯导仿真器模型如图 5 所示。模型 中输入量 X, f d 为由轨迹发生器 生成的 惯性器 件信息 源 ( 比力 和角速度); 模块单元 Add _ error 为输 入量引入各 种误差, 包 括陀螺的安装误差、刻度 系数误 差、漂移 ( 可 分为 常值漂 移、 一阶马尔柯夫过程和白 噪声) , 加速度 计的安 装误差、刻 度系 数误差、随机常值误差及白噪声; 模块单元 pulse transform 将 加入误差的信息转化为真实脉冲并计数, 再通过实测脉冲当量 换算即可得到陀螺和加速度计的模 拟输出值; 模 块单元 Get _ increment 对输入信号积 分, 两次 积分的 差值 即为角 速度 增量 和加速度增量; 模块单 元 att _ updata 为 姿态更 新模 块, 其中 包含姿态更新的单子样算法模块、二子样算法模块及三子样优 化算法模块; 模块 单元 Vn _ pos _ updata 为 速度 位置 更 新模 块, 包含速度更新的二子样算法模块及位置更新模块。由于要 求速度位置更新模 块与 姿态 更新 的三 种算 法模 块能 够任 意匹 配, 并可以在一次位置速度更新周期内完成多次姿态更新, 这 里添加了 Get _ wnin 模块 单元 及 Transport Delay 模块。 其中 Get _ wnin 模 块单元的功能是, 在速度位置 模块第一次 运行前 为姿态更新模 块提 供变 量初 值, 解 决了 初始 精度 问 题, wnin 即 Xn in , 是惯性坐标 系相对于导航坐标系 的旋转 角速度 在导航 图 6 姿态单子样算法姿态误差