基于Matlab的轨迹发生器与惯导解算
- 格式：pdf
- 大小：1.04 MB
- 文档页数：5
4 4
由姿态转换矩阵便可计算航向角 ψ、 俯仰角 θ 和横滚角 φ。 写成矩阵形式，即：
o 0 q b q 1 = 1 ωnbx b q 2 2 ωnby b 3 ωnbz q
— ３１ —
2016年信息与电脑9下-正文.indd 31
2016/12/19 8:59:06
计算机工程应用技术
更新方法为四元数姿态更新法。 2.1.1 四元数姿态更新法
信息与电脑 China Computer&Communication
2016 年第 18 期
2.1.3 位置更新方程 捷联惯导系统的位置更新是通过速度和角速度增量实现 的，位置更新微分方程为：
１ 引言
水下航行器在海洋开发、科学研究和军事应用中都占有 重要地位，这些工作能否顺利完成、完成效果的好坏，很大 程度上依赖于水下航行器的导航与定位能力。 惯性是所有质量体的基本属性，因此，基于惯性原理工 作的惯导系统具有极好的自主性和隐蔽性，特别适合水下的 特殊环境。然而惯导系统的误差是随时间累积的，长时间的 水下导航误差必然是逐渐发散的。因此，无论是解算算法、 组合导航方式，最终目的都是抑制惯导系统的误差发散，提 高惯导系统的导航精度。然而这些研究若在真实的惯导系统 上进行，不仅耗费大量的人力、物力，研究结果也不易观察、 体现、总结。因此，众多研究者们将目光投向计算机仿真。 轨迹发生器的设计与使用应运而生，并在惯导系统的研究中 发挥非常重要的作用。轨迹发生器的设计可以模拟水下航行 器在水下运动的各类参数信息，如姿态、速度、加速度等， 极大提高了研究效率，又可在轨迹发生器的基础上进行惯导 解算的研究。 本文提出一种水下航行器轨迹生成的方法，通过这种方 法模拟航行器在水下运动的各个姿态过程，可以作为研究惯 导系统工作原理，乃至于组合导航系统的数据依据。并在此 基础上进行了惯导解算，分析了解算结果。
ObYb 轴垂直于 ObXb 轴指向上方（当航行器水平放置时）， ObZb 轴与其他两轴构成“前 - 上 - 右”载体坐标系。
图 １ 捷联惯导系统工作原理框图
三轴陀螺仪和三轴加速度计固连于载体上，分别测量载 体的角运动信息和线运动信息，导航计算机根据陀螺仪的输 出计算出姿态转换矩阵。加速度计输出的比力值减去有害加 速度后得到速度。通过姿态转换矩阵，将速度值转换至导航 坐标系下，随后进行导航解算，依次获得姿态、速度以及位 置信息。这就是捷联惯导系统的工作原理。 由以上原理分析可知，捷联惯导系统在工作过程中与外 界无任何信息交互，是一种不依赖外界信息的完全自主的导 航系统， 并可提供全方位的导航参数， 如速度、 位置、 姿态等。 2.1 捷联惯导系统更新方程 捷联惯导系统工作时的基本更新方程包括姿态更新、速 度更新和位置更新。其中，姿态更新的解算方法有四元数姿 态更新法、等效旋转矢量法、欧拉角法等。本文所用的姿态
（1）
其中，q0、q1、q2、q3 是实数，i、j、k 是互相正交的单
−1 。
（8）
由四元数构成的由载体系 b 系到导航系 n 系的姿态转换
2(q1q2 − q0 q3 ) q −q +q −q 2(q2 q3 + q0 q1 )
2 0 2 1 2 2 2 3
2(q1q3 + q0 q2 ) 2(q2 q3 − q0 q1 ) 2 2 2 2 q0 − q1 − q2 + q3
（2）
n n 其中，L、λ、h 分别为纬度、经度、高度；vE 、vN 、vUn 分
别为导航坐标系下的东向、北向、天向速度。 捷联惯导系统误差模型 对应于捷联惯导解算的三个部分，捷联惯导系统的误差 方程分为：位置误差方程（纬度误差 δL、经度误差 δλ、高度 误差 δh）、姿态误差方程（横滚角误差 φ、俯仰角误差 θ、
摘 要：捷联惯导自出现至今，一直在导航系统中扮演着重要角色。水下环境特殊、复杂，信号传输方式受限，因此， 水下多用惯导作为主要导航设备。围绕捷联惯导的研究就此展开，主要研究方向为捷联惯导系统的误差抑制方法，从而 取得更高的导航精度。研究方法有理论研究与实验研究两种。实际的跑车实验需要大量的人力、物力与财力。在研究初期， 跑车实验并不是研究工作开展的最好方式。因此，利用 Matlab 矩阵实验室研究惯导系统的工作原理与工作过程，设计了 一种基于 Matlab 的轨迹发生器，给出捷联惯导系统的比力输出与角速度输出。仿真结果表明，轨迹发生器可以模拟水下 航行器的航行状态。同时，利用 Matlab 在已有运动轨迹的基础上进行惯导解算，给出并分析解算结果。 关键词：水下航行器；捷联惯导；轨迹生成；惯导解算 中图分类号：TN96 文献标识码：A 文章编号：1003-9767（2016）18-031-05
２ 捷联惯导系统工作原理与误差模型
“捷联（Strapdown）”意为“捆绑”，即惯导系统与载 体固连，建立了数学的工作平台，图 1 为捷联惯导系统工作 原理框图。 图 1 左侧为载体坐标系，载体坐标系与载体固连，其 原点 Ob 在航行器浮心处，ObXb 轴沿航行器纵轴指向前方，
作者简介：高沛林（1988-），女，陕西兴平人，硕士，助理工程师。研究方向：智能体导航与定位、自动控制系统。
2016 年第 18 期
信息与电脑 China Computer&Communication
计算机工程应用技术
基于 Matlab 的轨迹发生器与惯导解算
高沛林 1 周 颖 2
（1. 西安科技大学 电气与控制工程学院，陕西 西安 710054； 2. 西北工业大学 航海学院，陕西 西安 710072）
n vN L= RM + h n v E sec L λ = R N +h n = h vU
“四元数”，就是由四个元构成的数，定义为 Q： Q(q0,q1,q2,q3)=q0+q1i+q2 j+q3k 位向量，又是虚单位 矩阵如式（2）。
2 2 2 q0 + q12 + q2 + q3 = C 2(q1q2 + q0 q3 ) 2(q1q3 − q0 q2 ) R b
由姿态转换矩阵便可计算航向角 ψ、 俯仰角 θ 和横滚角 φ。 写成矩阵形式，即：
o 0 q b q 1 = 1 ωnbx b q 2 2 ωnby b 3 ωnbz q
— ３１ —
2016年信息与电脑9下-正文.indd 31
2016/12/19 8:59:06
计算机工程应用技术
更新方法为四元数姿态更新法。 2.1.1 四元数姿态更新法
信息与电脑 China Computer&Communication
2016 年第 18 期
2.1.3 位置更新方程 捷联惯导系统的位置更新是通过速度和角速度增量实现 的，位置更新微分方程为：
１ 引言
水下航行器在海洋开发、科学研究和军事应用中都占有 重要地位，这些工作能否顺利完成、完成效果的好坏，很大 程度上依赖于水下航行器的导航与定位能力。 惯性是所有质量体的基本属性，因此，基于惯性原理工 作的惯导系统具有极好的自主性和隐蔽性，特别适合水下的 特殊环境。然而惯导系统的误差是随时间累积的，长时间的 水下导航误差必然是逐渐发散的。因此，无论是解算算法、 组合导航方式，最终目的都是抑制惯导系统的误差发散，提 高惯导系统的导航精度。然而这些研究若在真实的惯导系统 上进行，不仅耗费大量的人力、物力，研究结果也不易观察、 体现、总结。因此，众多研究者们将目光投向计算机仿真。 轨迹发生器的设计与使用应运而生，并在惯导系统的研究中 发挥非常重要的作用。轨迹发生器的设计可以模拟水下航行 器在水下运动的各类参数信息，如姿态、速度、加速度等， 极大提高了研究效率，又可在轨迹发生器的基础上进行惯导 解算的研究。 本文提出一种水下航行器轨迹生成的方法，通过这种方 法模拟航行器在水下运动的各个姿态过程，可以作为研究惯 导系统工作原理，乃至于组合导航系统的数据依据。并在此 基础上进行了惯导解算，分析了解算结果。
ObYb 轴垂直于 ObXb 轴指向上方（当航行器水平放置时）， ObZb 轴与其他两轴构成“前 - 上 - 右”载体坐标系。
图 １ 捷联惯导系统工作原理框图
三轴陀螺仪和三轴加速度计固连于载体上，分别测量载 体的角运动信息和线运动信息，导航计算机根据陀螺仪的输 出计算出姿态转换矩阵。加速度计输出的比力值减去有害加 速度后得到速度。通过姿态转换矩阵，将速度值转换至导航 坐标系下，随后进行导航解算，依次获得姿态、速度以及位 置信息。这就是捷联惯导系统的工作原理。 由以上原理分析可知，捷联惯导系统在工作过程中与外 界无任何信息交互，是一种不依赖外界信息的完全自主的导 航系统， 并可提供全方位的导航参数， 如速度、 位置、 姿态等。 2.1 捷联惯导系统更新方程 捷联惯导系统工作时的基本更新方程包括姿态更新、速 度更新和位置更新。其中，姿态更新的解算方法有四元数姿 态更新法、等效旋转矢量法、欧拉角法等。本文所用的姿态
（1）
其中，q0、q1、q2、q3 是实数，i、j、k 是互相正交的单
−1 。
（8）
由四元数构成的由载体系 b 系到导航系 n 系的姿态转换
2(q1q2 − q0 q3 ) q −q +q −q 2(q2 q3 + q0 q1 )
2 0 2 1 2 2 2 3
2(q1q3 + q0 q2 ) 2(q2 q3 − q0 q1 ) 2 2 2 2 q0 − q1 − q2 + q3
（2）
n n 其中，L、λ、h 分别为纬度、经度、高度；vE 、vN 、vUn 分
别为导航坐标系下的东向、北向、天向速度。 捷联惯导系统误差模型 对应于捷联惯导解算的三个部分，捷联惯导系统的误差 方程分为：位置误差方程（纬度误差 δL、经度误差 δλ、高度 误差 δh）、姿态误差方程（横滚角误差 φ、俯仰角误差 θ、
摘 要：捷联惯导自出现至今，一直在导航系统中扮演着重要角色。水下环境特殊、复杂，信号传输方式受限，因此， 水下多用惯导作为主要导航设备。围绕捷联惯导的研究就此展开，主要研究方向为捷联惯导系统的误差抑制方法，从而 取得更高的导航精度。研究方法有理论研究与实验研究两种。实际的跑车实验需要大量的人力、物力与财力。在研究初期， 跑车实验并不是研究工作开展的最好方式。因此，利用 Matlab 矩阵实验室研究惯导系统的工作原理与工作过程，设计了 一种基于 Matlab 的轨迹发生器，给出捷联惯导系统的比力输出与角速度输出。仿真结果表明，轨迹发生器可以模拟水下 航行器的航行状态。同时，利用 Matlab 在已有运动轨迹的基础上进行惯导解算，给出并分析解算结果。 关键词：水下航行器；捷联惯导；轨迹生成；惯导解算 中图分类号：TN96 文献标识码：A 文章编号：1003-9767（2016）18-031-05
２ 捷联惯导系统工作原理与误差模型
“捷联（Strapdown）”意为“捆绑”，即惯导系统与载 体固连，建立了数学的工作平台，图 1 为捷联惯导系统工作 原理框图。 图 1 左侧为载体坐标系，载体坐标系与载体固连，其 原点 Ob 在航行器浮心处，ObXb 轴沿航行器纵轴指向前方，
作者简介：高沛林（1988-），女，陕西兴平人，硕士，助理工程师。研究方向：智能体导航与定位、自动控制系统。
2016 年第 18 期
信息与电脑 China Computer&Communication
计算机工程应用技术
基于 Matlab 的轨迹发生器与惯导解算
高沛林 1 周 颖 2
（1. 西安科技大学 电气与控制工程学院，陕西 西安 710054； 2. 西北工业大学 航海学院，陕西 西安 710072）
n vN L= RM + h n v E sec L λ = R N +h n = h vU
“四元数”，就是由四个元构成的数，定义为 Q： Q(q0,q1,q2,q3)=q0+q1i+q2 j+q3k 位向量，又是虚单位 矩阵如式（2）。
2 2 2 q0 + q12 + q2 + q3 = C 2(q1q2 + q0 q3 ) 2(q1q3 − q0 q2 ) R b
