基于四元数法的捷联式惯性导航系统的姿态解算_张荣辉

co s γ 表示旋转轴 n 与参考坐标系轴间的方向余
始的姿态角 。 ψ θ γ ψ θ γ 0 0 0 0 0 0 cos cos + sin sin sin 2 2 2 2 2 2 λ (0) ψ θ γ ψ θ γ 0 0 0 0 0 0 cos cos sin - sin sin cos p1 (0) 2 2 2 2 2 2 = , ψ θ γ ψ θ γ p2 (0) 0 0 0 0 0 0 cos sin cos + sin cos sin 2 2 2 2 2 2 p3 (0) ψ θ γ ψ θ γ 0 0 0 0 0 0 sin cos cos - cos sin sin 2 2 2 2 2 2 ( 4) ( 2 ) 四元数标量部分与矢量部分 λ 、 p1 、 p2 、 p3
2. 1 欧拉角微分方程式
一个动坐标系相对参考坐标系的方位可以完 全由动坐标系依次绕 3 个不同的轴转动的 3 个角 度来确定 。如把载体坐标系作为动坐标系 , 把导 航坐标系作为参考坐标系 , 则姿态角即为一组欧 拉角 ,按一定的转动顺序得到导航坐标系到载体 坐标系的关系 。 b γ EbX cosθ sin γ sinθ cos γ sinθ ω 1 b θ = γ - sin γ 0 cosθ cos cosθ ω . EbY cosθ b ψ γ 0 sin γ cos ω EbZ
( 1 ) 初始四元数的确定 , 如式 ( 4 ) 其输入为初
四元数的数学概念是 1843 年由哈密顿首先 提出的 ,它是代数学中的内容之一 。随着捷联式 惯性导航技术的发展 , 为了更简便地描述刚体的 角运动 ,采用了四元数这个数学工具 ,用它来弥补 通常描述刚体角运动的 3 个欧拉角参数在设计控 制系统时的不足 。四元数可以描述一个坐标系或 一个矢量相对某一个坐标系的旋转 , 四元数的标 量部分表示了转角的一半余弦值 , 而其矢量部分 则表示瞬时转轴的方向 、 瞬时转动轴与参考坐标 系轴间的方向余弦值 。因此 , 一个四元数既表示 了转轴的方向 ,又表示了转角的大小 ,往往称其为 转动四元数 。 工程上一般运用范数为 1 的特征四元数 , 特 征四元数的标量部分表示转角的一般余弦值 , 其 矢量部分表示瞬时转轴 n 的方向 。比如式 ( 3 ) 表 示矢量 R 相对参考坐标系旋转一个转角θ, 旋转 轴 n 的方向由四元数的虚部确定 ,co s α、 co s β 、
( 1)
根据欧拉角微分方程 , 由角速度可以求解 3 个姿态角 。欧拉角微分方程式只有 3 个 , 但每个 方程 x = f ( co s x , sin x ) ω 都含有三角函数的运 算 , 计算速度慢 , 且方程会出现 “奇点” , 方程式退 化 , 故不能全姿态工作 。 2. 2 方向余弦矩阵微分方程式 当一个坐标系相对另一个坐标系做一次或多 次旋转后可得到另外一个新的坐标系 , 前者往往 被称为参考坐标系或固定坐标系 , 后者被称为动 坐标系 , 他们之间的相互关系可用方向余弦表来 表示 。方向余弦矩阵微分方程式可写为载体坐标 系相对导航坐标系旋转角速度的斜对称矩阵表达 式 , 方向余弦表是对这两种坐标系相对转动的一 种数学描述 。 E E b ( 2) Cb = Cb Ω Eb , b 式中 ,ΩEb 为载体坐标系相对导航坐标系旋转角速 度的斜对称矩阵表达式 。用方向余弦法计算姿态 矩阵 , 没有方程退化问题 , 可以全姿态工作 , 但需
1965
要求解 9 个微分方程 Cij ( x ) = Cij ( x ) ω, 计算量较 大 , 实时性较差 , 无法满足工程实践要求 。
2. 3 四元数微分方程式
3. 1 四元数法正确性和有效性的验证
本文根据四元数法与方向余弦法两种解算方 法进行计算 , 通过对比两种方法的计算结果 , 验证 四元数法的正确性和有效性 。 四元数法姿态矩阵计算的步骤如下 :
2 姿态矩阵的计算方法
由于载体的姿态方位角速率较大 ,所以针对 姿态矩阵的实时计算提出了更高的要求 。通常假 定捷联系统 “数学平台” 模拟地理坐标系 , 即导航 坐标系 ; 而确定载体的姿态矩阵即为研究载体坐 标系 ( b) 和导航坐标系 ( E) 的空间转动关系 , 一般
第 10 期
张荣辉 ,等 : 基于四元数法的捷联式惯性导航系统的姿态解算
收稿日期 :2008208207 ; 修订日期 :2008209203. 基金项目 : 吉林省杰出青年基金资助项目 ( No . 20060115) ; 中国科学院 “三期创新” 平台资助项目
1964
光学 精密工程
第 16 卷
1 引 言
捷联惯导是一种自主式的导航方法 。该方法 将陀螺仪和加速度计直接安装在载体上 , 省掉机 电式导航平台 ,利用计算机软件建立一个 “数学平 [1] 台” 来代替机电平台实体 。由于其结构简单且 抗干扰能力强 , 目前已成为航空航天 、 航海 、 机器 人、 智能交通等领域的研究热点之一 。 姿态解算是捷联式惯性导航系统的关键技 术 ,通过姿态矩阵可以得到载体的姿态和导航参 数计算需要的数据 , 是捷联式惯导算法中的重要 工作 。载体的姿态和航向体现了载体坐标系与导 航坐标系之间的方位关系 , 确定两个坐标系之间 的方位关系需要借助矩阵法和力学中的刚体定点 运动的位移定理 。通过矩阵法推导方向余弦表 , 而刚体定点运动的位移定理表明 , 定点运动刚体 的任何有限位移都可以绕过定点的某一轴经过一 次转动来实现 。目前描述动坐标相对参考坐标系 方位关系的方法有多种 ,可简单地将其分为 3 类 , 即三参数法 、 四参数法和九参数法[ 122 ] 。三参数法 也叫欧拉角法 ,四参数法通常指四元数法 ,九参数 法称作方向余弦法 。欧拉角法由于不能用于全姿 态飞行运载体上而难以广泛用于工程实践 , 且实 时计算困难 。方向余弦法避免了欧拉法的 “奇点” 现象 ,但方程的计算量大 ,工作效率低 。随着飞行 运载体导航控制系统的迅速发展和数字计算机在 运动控制中的应用 , 控制系统要求导航计算环节 能更加合理地描述载体的刚体空间运动 , 四元数 法的研究得到了广泛重视 。本文全面分析了 3 种 解算方法的特点 , 通过对比四参法与九参法的计 算结果以验证四元数法的正确性和有效性 , 基于 数值仿真和转台实验相结合的分析方法得到进一 步减少姿态解算误差的有效途径 , 为捷联式惯性 导航技术的工程实践提供参考 。
( 1 . Chan gchun I nsti t ute of O ptics , Fi ne M echanics an d Ph ysics , Chi nese A ca dem y of S ciences , Chan gchun 130033 , Chi na;
2 . Gra d uate U ni versi t y of Chi nese A ca dem y of S ciences , B ei j i n g 100039 , Chi na) Abstract : The at tit ude solutio n for carrier is o ne of t he key technologies for p recisio n navigatio n of St rap down Inertial Navigatio n System ( SIN S) . In t his paper ,t he st ro ngpoint s and defect s of Euler al2 go rit hm , directio n co sine algorit hm and quaternio n algorit hm were analyzed. Then , t he carrier at ti2 t ude was co mp uted separately acco rding to quaternio n algorit hm and direct co sine algo rit hm , and t he co mparative deviatio n was o btained by co nt rasting t hese t wo solutio n deviatio n wit h t heory result s. These co mparso ns p rove t he validit y of quaternio n algorit hm and co nclude t hat increasing sampling f requency and adopting high2level co mp ute algorit hm can lower co mp ute deviatio n. Digital simulatio n and experiment result s show t hat t he carrier at tit ude solutio n relative deviatio n is 10 - 10 % wit h numer2 ical t heory and 10 - 3 % wit h test experiment result s , t he co mp ute time is 36 μs. The p ropo sed at tit ude solutio n met hod can satisf y t he real2time demand , and can offer a usef ul reference for st rap down iner2 tial navigatio n technolo gy applied in engineering. Key words : St rap down Inertial Navigatio n System ( SIN S) ; at tit ude solutio n ; quaternio n algorit hm ; navigatio n and co nt rol technology of aircraf t