CN110851776B - 一种高动态变转速载体的姿态解算方法 - Google Patents
一种高动态变转速载体的姿态解算方法 Download PDFInfo
- Publication number
- CN110851776B CN110851776B CN201911134027.9A CN201911134027A CN110851776B CN 110851776 B CN110851776 B CN 110851776B CN 201911134027 A CN201911134027 A CN 201911134027A CN 110851776 B CN110851776 B CN 110851776B
- Authority
- CN
- China
- Prior art keywords
- attitude
- carrier
- angular
- quaternion
- current
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/13—Differential equations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Operations Research (AREA)
- Computing Systems (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种高动态变转速载体的姿态解算方法,属于惯性导航领域。该方法在解算姿态过程中加入了判断载体转速的过程,其目的在于,当载体为高转速时,惯导系统采用较高的采样频率,以保证解算结果符合精度要求;当载体为低转速时,适当降低惯导系统的采样频率,以减轻硬件的负担。在高转速时,本发明提出了一种基于角速率输出的单子样旋转矢量算法,可以按照惯导系统的采样频率更新姿态,同时利用当前以及前一姿态更新周期角增量信息和当前以及之前N个时刻的角速率信息补偿圆锥误差,误差补偿系数可通过解线性方程组求得。本发明可以有效解决高动态变转速载体的姿态求解问题。
Description
技术领域
本发明属于导航技术领域,具体涉及应用于高动态变转速载体的一种捷联惯性导航系统的姿态解算方法。
背景技术
捷联惯性导航系统中,惯性器件直接与运载体固连,同时承受运载体的角运动和线运动,通过陀螺仪测量和软件解算建立虚拟稳定平台——“数学平台”,利用数学平台将运载体坐标系下的加速度计测量转换到导航坐标系,然后进行速度和位置积分解算。捷联惯性导航系统具有抗外界干扰能力强、不需要外界环境提供额外信息等优点。
姿态解算是捷联惯性导航系统中的关键技术,解算结果的精度受陀螺仪自身器件水平以及姿态解算算法两方面的约束。刚体做有限转动时,刚体的空间角位置与旋转次序有关,即存在旋转的不可交换性误差。为了补偿旋转引起的不可交换性误差,国内外学者提出使用多子样旋转矢量算法,但是在采样频率不变的情况下使用多子样旋转矢量算法会降低姿态更新频率,而提高采样频率会使硬件负担加重并加大量化误差。
传统的角速率旋转矢量算法建立在陀螺输出为角增量的基础上,当陀螺输出为角速率时,从角速率中提取角增量再应用传统的多子样旋转矢量算法会导致补偿精度下降。有学者提出了利用角速率的多子样旋转矢量方法,但仍存在降低姿态更新频率的问题。
发明内容
为了解决多子样旋转矢量法会降低姿态更新频率的问题,提出了一种高动态变转速载体的姿态解算方法;利用当前以及上一姿态更新周期角增量信息和当前以及之前N个时刻的角速率信息的单子样旋转矢量姿态解算方法。
同时考虑到高采样率会给硬件带来较大负担,当载体转速较低时降低惯导系统的采样频率。
本发明的技术方案为:一种高动态变转速载体的姿态解算方法,具体包括如下步骤:
S1:获取当前时刻捷联惯性导航系统的数据输出;
S2:由于载体转速变化过程已知,通过当前时刻时间判断载体为高转速还是低转速:若为高转速,转至步骤S3至S5;若为低转速,转至步骤S6;
S2所述的判断载体转速的过程,其目的在于,当载体为高转速时,惯导系统采用较高的采样频率,以保证解算结果符合精度要求;当载体为低转速时,适当降低惯导系统的采样频率,以减轻硬件的负担。
S3:利用当前以及上一姿态更新周期角增量信息和当前以及之前N个时刻的角速率信息,解算姿态更新周期内的旋转矢量:
其中,h为陀螺仪采样间隔,也为姿态的更新间隔,ω0为当前时刻t陀螺仪输出的角速率,ω-i为t-ih时刻陀螺仪输出的角速率,为[t-h,t]内的角增量,为[t-2h,t-h]内的角增量,角增量由采样值的数值积分求得,Gi和K为误差补偿系数;
S6:利用毕卡法求解四元数微分方程,更新当前时刻的姿态四元数;
四阶近似的四元数毕卡法公式为:
其中,Q(tk)为上一时刻的姿态四元数,Q(tk+1)为更新后的姿态四元数, 为载体坐标系相对导航坐标系在载体坐标系中的角速率投影,为的四元数矩阵形式,Δθx、Δθy、Δθz为采样间隔[tk,tk+1]内的角增量,Δθ为角增量模值,
再通过计算得到当前时刻载体的姿态角;
S8:判断载体是否到达终点,若是,则算法结束;若否,执行S1。
以下就本发明的优化方法进行说明:
对捷联惯导系统姿态更新来说,标准圆锥运动是最恶劣的工作环境,会诱发数学平台的漂移,所以对旋转矢量算法作优化处理常以标准圆锥运动最为环境条件,假设圆锥运动用旋转矢量可以表示为:
Φ1=[0 αcosΩt αsinΩt]T (1)
其中,α为圆锥运动的半锥角,Ω为圆锥运动的锥频率,假设载体单纯做圆锥运动。
根据式(1)可以得到实时的姿态四元数为:
式(2)中Q(t)为t时刻载体坐标系相对参考参考坐标系的旋转四元数。设[t,t+h]时间段对应的坐标系的旋转四元数为q(h),则有
根据式(2)和式)(3)可以得到:
根据四元数微分方程:
由式(2)和式(5)可得载体坐标系相对参考坐标系的角速度为:
[t,t+h]时间段对应的角增量为:
由旋转矢量与q(h)的关系可得[t,t+h]时间段对应的旋转矢量为:
下面就本发明提出的单子样旋转矢量算法进行详细说明:
其中,h为姿态的更新间隔,等于陀螺仪的采样间隔,ω0为当前时刻t陀螺仪输出的角速率,ω-i为t-ih时刻陀螺仪输出的角速率,为[t-h,t]内的角增量,为[t-2h,t-h]内的角增量,Gi和K为误差补偿系数。
由式(6)可得:
由式(7)可得:
只考虑非周期项时:
考虑sin α≈α,在标准圆锥运动环境下对该算法进行优化,使得算法漂移误差最小,从而计算得到误差补偿系数Gi和K,具体表达式如下:
将式(11)和式(14)代入式(9),再将式(8)和式(9)代入式(15),得:
令λ=Ωh,则式(16)可以表示为:
下面以N=2为例计算误差补偿系数的值。
将式(18)利用下式(19)进行泰勒展开:
式(18)中的前两项的展开结果为:
式(18)中的后三项的展开结果为:
令式(20)与式(21)中λ3、λ5、λ7前系数相等,可得
解得:
此时算法的漂移误差为:
可以看出算法具体较高的精度。
本发明的有益效果:本发明的一种高动态变转速载体的姿态解算方法,与现有技术相比具有以下优点:
(1)本发明在解算姿态过程中加入了判断载体转速的过程,其目的在于,当载体为高转速时,惯导系统采用较高的采样频率,以保证解算结果符合精度要求;当载体为低转速时,适当降低惯导系统的采样频率,以减轻硬件的负担。
附图说明
图1是本发明所述的一种高动态变转速载体的姿态解算方法的流程图。
图2是本发明所述的一种高动态变转速载体的姿态解算方法的示意图。
图3是本发明实施例中载体运动轨迹。
图4是本发明实施例中载体姿态角变化情况。
图5是本发明实施例中载体姿态角解算误差。
具体实施方式
下面结合附图和具体实施例,对本发明进行详细说明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
S1:获取当前时刻捷联惯性导航系统的数据输出,得到当前时刻陀螺仪角速率输出ω0;
S2:由于载体转速变化过程已知,通过当前时刻时间判断载体为高转速还是低转速:若为高转速,转至步骤S3至S5;若为低转速,转至步骤S6;
S3:利用当前以及上一姿态更新周期角增量信息和当前以及之前2个时刻的角速率信息,解算姿态更新周期内的旋转矢量:
其中,h为陀螺仪采样间隔,也为姿态的更新间隔,ω0为当前时刻t陀螺仪输出的角速率,ω-1、ω-2分别为t-h、t-2h时刻陀螺仪输出的角速率,为[t-h,t]内的角增量,为[t-2h,t-h]内的角增量,角增量由采样值的数值积分求得;
S6:利用毕卡法求解四元数微分方程,更新当前时刻的姿态四元数,四阶近似的四元数毕卡法公式为:
其中,Q(tk)为上一时刻的姿态四元数,Q(tk+1)为更新后的姿态四元数, 为载体坐标系相对导航坐标系在载体坐标系中的角速率投影, 为的四元数矩阵形式,Δθx、Δθy、Δθz为采样间隔[tk,tk+1]内的角增量,Δθ为角增量模值,
再通过计算得到当前时刻载体的姿态角;
S8:判断载体是否到达终点,若是,则算法结束;若否,执行S1。
具体实施例:
根据上述方法步骤,本实施例利用轨迹发生器仿出一段高动态变转速载体的运动轨迹,仿真时长30s,30s内航向角从90°变化到120°,俯仰角从45°变化到-45°,横滚角起始时刻为0°,0~15s内转速从20r/s下降到5r/s,15s~20s内转速从5r/s下降到1r/s,20s~30s内转速保持1r/s。发射时刻载体经度114°E,纬度30°N,高度0米。陀螺仪的零偏稳定性设为20°/h,加速度计零偏稳定性设为100μg。
0~20s内的转速大于1r/s,判定为高转速,惯导系统采样率为1000Hz,用本发明提出的单子样旋转矢量法更新姿态四元数,20~30s内的转速为1r/s,判定为低转速,惯导系统采样率为100Hz,用毕卡法更新姿态四元数。
轨迹发生器生成的载体运动轨迹如图3所示,载体的姿态角变化如图4所示,利用本发明提出的高动态变转速载体的姿态解算方法得到的姿态角误差如图5所示。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段,还包括由以上技术特征任意组合所组成的技术方案。
Claims (4)
1.一种高动态变转速载体的姿态解算方法,其特征在于:包括如下步骤:
S1:获取当前时刻捷联惯性导航系统的数据输出;
S2:由于载体转速变化过程已知,通过当前时刻时间判断载体为高转速还是低转速:若为高转速,转至步骤S3至S5;若为低转速,转至步骤S6;
S3:利用当前以及上一姿态更新周期角增量信息和当前以及之前N个时刻的角速率信息,解算姿态更新周期内的旋转矢量:
其中,h为姿态的更新间隔,等于陀螺仪的采样间隔,ω0为当前时刻t陀螺仪输出的角速率,ω-i为t-ih时刻陀螺仪输出的角速率,为[t-h,t]内的角增量,为[t-2h,t-h]内的角增量,角增量由采样值的数值积分求得,Gi和K为误差补偿系数;
S6:利用毕卡法求解四元数微分方程,更新当前时刻的姿态四元数;
四阶近似的四元数毕卡法公式为:
其中,Q(tk)为上一时刻的姿态四元数,Q(tk+1)为更新后的姿态四元数, 为载体坐标系相对导航坐标系在载体坐标系中的角速率投影,为的四元数矩阵形式,Δθx、Δθy、Δθz为采样间隔[tk,tk+1]内的角增量,Δθ为角增量模值,
再通过计算得到当前时刻载体的姿态角;
S8:判断载体是否到达终点,若是,则算法结束;若否,执行S1。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911134027.9A CN110851776B (zh) | 2019-11-19 | 2019-11-19 | 一种高动态变转速载体的姿态解算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911134027.9A CN110851776B (zh) | 2019-11-19 | 2019-11-19 | 一种高动态变转速载体的姿态解算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110851776A CN110851776A (zh) | 2020-02-28 |
CN110851776B true CN110851776B (zh) | 2022-11-15 |
Family
ID=69602221
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911134027.9A Active CN110851776B (zh) | 2019-11-19 | 2019-11-19 | 一种高动态变转速载体的姿态解算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110851776B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111351483B (zh) * | 2020-03-31 | 2021-12-07 | 北京控制工程研究所 | 一种递归多子样大动态惯性导航方法 |
CN112284379B (zh) * | 2020-09-17 | 2023-09-22 | 江苏大学 | 一种基于非线性积分补偿的组合运动测量系统的惯性预积分方法 |
CN114763994B (zh) * | 2021-05-06 | 2024-01-30 | 苏州精源创智能科技有限公司 | 一种应用于扫地机器人的惯性姿态导航系统 |
CN113252032B (zh) * | 2021-06-04 | 2021-10-22 | 华中光电技术研究所(中国船舶重工集团公司第七一七研究所) | 一种基于流水式旋转矢量的圆锥误差补偿方法及系统 |
CN115655271B (zh) * | 2022-12-22 | 2023-03-10 | 中国船舶集团有限公司第七〇七研究所 | 一种动态条件下的大范围姿态角提取方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103712623A (zh) * | 2014-01-20 | 2014-04-09 | 东南大学 | 基于角速率输入的光纤陀螺惯导系统姿态优化方法 |
CN110345942A (zh) * | 2019-07-17 | 2019-10-18 | 哈尔滨工程大学 | 一种基于角速率输入的插值三子样圆锥误差补偿算法 |
-
2019
- 2019-11-19 CN CN201911134027.9A patent/CN110851776B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103712623A (zh) * | 2014-01-20 | 2014-04-09 | 东南大学 | 基于角速率输入的光纤陀螺惯导系统姿态优化方法 |
CN110345942A (zh) * | 2019-07-17 | 2019-10-18 | 哈尔滨工程大学 | 一种基于角速率输入的插值三子样圆锥误差补偿算法 |
Non-Patent Citations (1)
Title |
---|
一种单子样旋转矢量姿态算法;杨胜等;《宇航学报》;20100330(第03期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN110851776A (zh) | 2020-02-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110851776B (zh) | 一种高动态变转速载体的姿态解算方法 | |
CN110398257B (zh) | Gps辅助的sins系统快速动基座初始对准方法 | |
CN106767900B (zh) | 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法 | |
CN108225308B (zh) | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN107063254B (zh) | 一种陀螺地磁组合的姿态解算方法 | |
CN106767797B (zh) | 一种基于对偶四元数的惯性/gps组合导航方法 | |
CN109959374B (zh) | 一种行人惯性导航全时全程逆向平滑滤波方法 | |
CN109596144B (zh) | Gnss位置辅助sins行进间初始对准方法 | |
CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航系统及方法 | |
CN108592943B (zh) | 一种基于opreq方法的惯性系粗对准计算方法 | |
CN111024074B (zh) | 一种基于递推最小二乘参数辨识的惯导速度误差确定方法 | |
CN112902956A (zh) | 一种手持式gnss/mems-ins接收机航向初值获取方法、电子设备、存储介质 | |
CN113108781B (zh) | 一种应用于无人船行进间的改进粗对准方法 | |
CN111649747A (zh) | 一种基于imu的自适应ekf姿态测量改进方法 | |
CN110595434B (zh) | 基于mems传感器的四元数融合姿态估计方法 | |
CN111323049A (zh) | 一种粒子群算法的粗对准方法 | |
CN111912427A (zh) | 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统 | |
CN103591960B (zh) | 一种基于旋转调制的静基座惯性导航系统粗对准方法 | |
CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
CN110737194B (zh) | 一种多mems传感器组合测姿方法 | |
WO2022057350A1 (zh) | 一种基于非线性积分补偿的组合运动测量系统的惯性预积分方法 | |
CN105277212A (zh) | 一种三轴惯性稳定平台系统的二阶动态干扰力矩补偿方法 | |
CN113447018B (zh) | 一种水下惯性导航系统的姿态实时估计方法 | |
CN114877891A (zh) | 一种基于角度参数化的航向初始对准方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |