CN113776529B - 一种基于载体系四元数姿态误差的非线性误差模型 - Google Patents
一种基于载体系四元数姿态误差的非线性误差模型 Download PDFInfo
- Publication number
- CN113776529B CN113776529B CN202111067339.XA CN202111067339A CN113776529B CN 113776529 B CN113776529 B CN 113776529B CN 202111067339 A CN202111067339 A CN 202111067339A CN 113776529 B CN113776529 B CN 113776529B
- Authority
- CN
- China
- Prior art keywords
- error
- quaternion
- carrier
- equation
- nonlinear
- 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
- 238000000034 method Methods 0.000 claims abstract description 25
- 239000000654 additive Substances 0.000 claims abstract description 12
- 230000000996 additive effect Effects 0.000 claims abstract description 12
- 239000011159 matrix material Substances 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 11
- 238000001914 filtration Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 2
- 238000002474 experimental method Methods 0.000 abstract description 8
- 238000011160 research Methods 0.000 abstract description 4
- 230000007547 defect Effects 0.000 abstract description 2
- 230000001133 acceleration Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- 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
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- 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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种基于载体系四元数非线性误差模型的姿态误差估计方法,包括:构建基于乘性四元数姿态误差和加性四元数姿态误差的非线性误差模型,然后通过车载实验得出基于载体系的四元数非线性误差模型性能优于基于导航系的四元数非线性误差模型,尤其是在初始航向角较大时基于载体系的四元数非线性误差模型性能较好。本发明改进现有文献对大失准条件下的组合导航模型研究的不足,该模型可在大失准条件下,通过较短时间内完成高精度的非线性组合导航,满足高精度的组合导航的要求。
Description
技术领域
本发明涉及导航技术领域,具体涉及一种基于载体系四元数非线性误差模型的姿态误差估计方法。
背景技术
捷联惯导系统是以一种基于牛顿运动定律以航位推算的方式实现高精度导航的系统,其核心传感器为陀螺仪和加速度计两类惯性传感器。此两类传感器构成的惯性测量单元固连于运动载体上,敏感载体线速度和角速度信息。捷联惯导系统通过惯性测量单元的传感器信息,在给定初始运动信息下,利用牛顿第二定律积分推算获取载体的姿态、速度和位置等信息。相比于其他导航系统,捷联惯导系统具有自主性强、可靠性高、隐蔽性好、输出导航信息全面以及全天候工作等优势,正被广泛应用于航空、航天、航海等领域。
然而,捷联惯性导航的缺点在于其误差随时间不断积累,长期精度差,因此无法独自完成长航时任务。在进行组合导航之前,必须进行初始对准。对准过程中,初始速度和初始位置信息获取较为容易,一般可直接通过卫星导航系统、多普勒计程仪和里程计等外界辅助设备进行装订,而很少有设备直接提供姿态信息。因此,初始对准的重难点在于对姿态信息的对准。
传统的初始对准分为粗对准和精对准两个阶段。粗对准的主要目的是在较短时间内获取较为准确的初始姿态,使失准角满足小角度的可线性化条件;精对准是在粗对准的基础上进一步获取更加精准的姿态信息。而对于在海上航行的舰船而言,在某些需紧急启动的复杂海况条件下,捷联惯导系统不具备充分的静态对准条件,无法通过粗对准达到精对准所需的小失准角要求,导致对准不成功。针对此类情况,基于大失准角的非线性误差模型,将有助于提升舰船的紧急启动能力,具有重要的研究价值。
当前研究的非线性误差模型通常都是认为姿态误差存在于导航坐标系(n系)下的,而基于载体坐标系(b系)姿态误差的模型研究相对较少。从李群的角度来看,对于以GPS信息作为观测量而言,载体系姿态误差对应的左误差模型要优于导航系姿态误差对应的右误差模型。
发明内容
针对以上问题,本发明目的在于提供一种基于载体系四元数姿态误差的非线性误差模型,旨在使舰船进行高精度的快速启动,以改进现有文献对大失准条件下的组合导航模型研究的不足。该模型可在大失准条件下,通过较短时间内完成高精度的非线性组合导航,满足高精度的组合导航的要求。
一种基于载体系四元数非线性误差模型的姿态误差估计方法,应用于大失准角条件下舰船的高精度紧急启动:
给出姿态、位置和速度误差模型如下:
(1)姿态误差方程:
A.乘性四元数误差方程
乘性四元数误差方程为:
式中的上标b代表载体坐标系,上标n代表导航坐标系;有关量的物理意义如下:表示四元数形式的姿态误差,/>表示方向余弦矩阵形式的姿态误差,/>为从b′系到n系的方向余弦矩阵;/>为陀螺输出的b系相对于惯性坐标系的载体角速度,/>为地球自转引起的角速度与载体在地球表面运动引起的角速度之和;/>为实际的陀螺输出载体角速度,/>为带误差量的惯导角速度;/>为陀螺测量误差,可大致看作陀螺常值漂移,为计算误差;I3为3×3的单位矩阵;
B.加性四元数误差方程
加性四元数误差方程为:
(2)速度误差方程
定义速度误差且/>
其中,为东向速度误差,/>为北向速度误差,/>为天向速度误差,由比力方程得到速度误差方程为:
式中,为加速度计的输出,δfb可大致看作加速度计的常值漂移/>为地球自转角速度,/>为由载体在地球表面运动而引起的角速度,且有/> 和为计算误差,分别表示为:
(3)位置误差方程
定义位置p=[L,λ,h]T,位置误差为δp=[δL,δλ,δh]T
L,λ,h分别为纬度、经度和高度,δL,δλ,δh分别为纬度误差、经度误差和高度误差;
考虑地球的曲率,则位置误差方程为:
式中,RM和RN分别为地球子午圈半径和地球卯酉圈半径;对于中低速运动的载体,对位置误差微分方程进行以下简化:
将基于载体系的加性四元数非线性误差模型记为Qb-Add model,将基于载体系的乘性四元数非线性误差模型记为Qb-Mul model,状态量的选取具体表示为:
Qb-Add model:
Qb-Mul model:
式中,εb为陀螺常值漂移,为加速度计常值漂移;
对于观测量的选取,将速度和位置作为组合导航的观测量:
式中,为由惯导解算而得的速度和位置信息,/>为由卫星导航得到的速度和位置参考信息;相对应的量测矩阵为
对于组合导航的滤波方法,采用USQUE滤波。
本发明主要针对大失准角条件下舰船的高精度紧急启动场景,从基于导航系的四元数非线性模型出发,提出基于载体系的四元数非线性误差模型,通过实验实验加性和乘性四元数非线性误差模型实质上是等价的,由此可证明提出的基于载体系的四元数非线性误差模型的正确性。然后,通过车载实验得出基于载体系的四元数非线性误差模型性能优于基于导航系的四元数非线性误差模型,尤其是在初始航向角较大时基于载体系的四元数非线性误差模型性能较好。本发明对于舰船在大失准条件下的应急启动方面具有极为重要的意义,具有很强的研究价值。
附图说明
下面结合附图对本发明的具体实施方式作进一步详细的说明。
图1为本发明涉及的基于载体系四元数姿态误差非线性误差模和基于导航系四元数姿态误差的非线性误差模型的原理框图;
图2为本发明中车载实验轨迹图;
图3为本发明实验所得的纵倾角姿态估计误差曲线图;
图4为本发明实验所得的横滚角姿态估计误差曲线图;
图5为本发明实验所得的航向角姿态估计误差曲线图。
具体实施方式
为了使本发明的目的和优点更加清楚明白,下面结合实施例对本发明作进一步描述;应当理解,此处所描述的具体实施例仅仅用于解释本发明,并不用于限定本发明。
下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是,这些实施方式仅仅用于解释本发明的技术原理,并非在限制本发明的保护范围。
参考图1至图5所示,本发明涉及一种基于载体系四元数姿态误差的非线性误差模型,为了详细说明本发明提出的基于载体系四元数非线性模型,首先给出姿态、位置和速度误差模型如下:
1.姿态误差方程
A.乘性四元数误差方程
乘性四元数误差方程为:
式中的上标b代表载体坐标系;有关量的物理意义如下:
表示四元数形式的姿态误差,/>表示方向余弦矩阵形式的姿态误差,/>为从b′到n系的方向余弦矩阵;
为陀螺输出的b系相对于惯性坐标系(i系)的载体角速度,/>为地球自转引起的角速度与载体在地球表面运动引起的角速度之和;
为实际的陀螺输出载体角速度,/>为带误差量的惯导角速度;
为陀螺测量误差,可大致看作陀螺常值漂移,/>为计算误差;
I3为3×3的单位矩阵;
B.加性四元数误差方程
加性四元数误差方程为:
2速度误差方程
定义速度误差且/>
其中,为东向速度误差,/>为北向速度误差,/>为天向速度误差。由比力方程可得到速度误差方程为:
式中,为加速度计的输出,δfb大致可看作加速度计的常值漂移/>为地球自转角速度,/>为由载体在地球表面运动而引起的角速度,且有/>
和/>为计算误差,可分别表示为:
3位置误差方程
定义位置p=[L,λ,h]T,位置误差为δp=[δL,δλ,δh]TL,λ,h分别为纬度、经度和高度,δL,δλ,δh分别为纬度误差、经度误差和高度误差;
考虑地球的曲率,则位置误差方程为:
式中,RM和RN分别为地球子午圈半径和地球卯酉圈半径。对于中低速运动的载体,可对位置误差微分方程进行以下简化:
本发明的实验验证方法具体实施包含以下步骤:
1)为了更方便表示这3种模型,将这3种模型分别记为Qb-Add model、Qb-Mulmodel和Qn model。对于这3种模型,状态量的选取具体表示为:
Qb-Add model:
Qb-Mul model:
Qn model:
式中,εb为陀螺常值漂移,为加速度计常值漂移,Qb-Add model表示基于b系的加性四元数非线性误差模型,Qb-Mul model表示基于b系的乘性四元数非线性误差模型,Qnmodel表示基于n系的非线性误差模型。
2)对于观测量的选取,将将速度和位置作为组合导航的观测量:
式中,为由惯导解算而得的速度和位置信息,/>为由卫星导航得到的速度和位置参考信息。相对应的量测矩阵为
对于组合导航的滤波方法,采用USQUE滤波。
3)进行车载组合导航实验,对此3种非线性误差模型性能进行分析和比较。汽车内放置一个较高精度的MEMS(STIM300)用于测量惯性信息,GPS天线固定于车顶用于接收卫星信号,GPS测速精度为0.1m/s,定位精度10m。由惯性级光纤惯导给出的姿态作为参考基准。车辆实验在空旷的马路上进行,时间大约2000s,轨迹如图2所示,图中黑色标记点为出发点,灰色标记点为终点。运动过程中GPS信号接收良好,利用GPS测得的速度和位置作为观测量进行组合导航,系统噪声和量测噪声方法根据传感器指标和经验值进行设定。
4)对于MEMS传感器而言,其陀螺仪和加速度计输出是最为重要的参数信息,陀螺仪和加速度计的参数信息为:
陀螺仪常值漂移:0.5°/h
陀螺仪角度随机游走:
陀螺仪量程范围:±400°/s
加速度计零偏:0.05mg
加速度计量程范围:±10g
上式中,g表示重力加速度
惯导系统初始误差:捷联惯导系统的初始误差主要初始姿态误差、初始速度误差和初始位置误差。本发明的主要应用场景是在初始姿态误差为大失准角条件下的,因此将初始的姿态误差设置为大失准角,初始速度误差和初始位置误差根据经验进行设置。参数设置如下:
初始航迹误差:
姿态误差:俯仰角、横滚角和航向角分别为[20°20°60°]。
速度误差:东北天速度误差均为1m/s
位置误差:纬度、经度、高度位置误差均为10m
根据以上参数,通过车载实验数据进行组合导航实验,得到姿态估计误差,纵倾角、横滚角、航向角估计误差分别如图3-5所示。由图可知,在大失准角的条件下,Qb-addmodel和Qb-mul model均可进行较快的组合导航,且姿态估计误差较小,满足高精度组合导航的要求。而Qn model的姿态估计误差较大,不足以满足正常组合导航的要求。且Qb-addmodel和Qb-mul model的误差曲线几乎重合,由此可得加性四元数误差模型和乘性四元数误差模型的一致性,从而证明本发明提出的基于载体系四元数姿态误差的非线性误差模型的正确性。同时,本实验结果也表明基于载体系的非线性误差模型性能优于基于导航系的非线性误差模型,进一步反映了本发明提出的基于载体系的非线性误差模型在大失准角条件应急启动应用的优势所在。
至此,已经结合附图所示的优选实施方式描述了本发明的技术方案,但是,本领域技术人员容易理解的是,本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下,本领域技术人员可以对相关技术特征做出等同的更改或替换,这些更改或替换之后的技术方案都将落入本发明的保护范围之内。
Claims (1)
1.一种基于载体系四元数非线性误差模型的姿态误差估计方法,应用于大失准角条件下舰船的高精度紧急启动,其特征在于:
给出姿态、位置和速度误差模型如下:
(1)姿态误差方程:
A.乘性四元数误差方程
乘性四元数误差方程为:
式中的上标b代表载体坐标系,上标n代表导航坐标系;有关量的物理意义如下:表示四元数形式的姿态误差,/>表示方向余弦矩阵形式的姿态误差,/>为从b′系到n系的方向余弦矩阵;/>为陀螺输出的b系相对于惯性坐标系的载体角速度,/>为地球自转引起的角速度与载体在地球表面运动引起的角速度之和;/>为实际的陀螺输出载体角速度,为带误差量的惯导角速度;/>为陀螺测量误差,可大致看作陀螺常值漂移,/>为计算误差;I 3为3×3的单位矩阵;
B.加性四元数误差方程
加性四元数误差方程为:
(2)速度误差方程
定义速度误差且/>
其中,为东向速度误差,/>为北向速度误差,/>为天向速度误差,由比力方程得到速度误差方程为:
式中,为加速度计的输出,δfb可大致看作加速度计的常值漂移/> 为地球自转角速度,/>为由载体在地球表面运动而引起的角速度,且有/> 和/>为计算误差,分别表示为:
(3)位置误差方程
定义位置p=[L,λ,h]T,位置误差为δp=[δL,δλ,δh]T
L,λ,h分别为纬度、经度和高度,δL,δλ,δh分别为纬度误差、经度误差和高度误差;
考虑地球的曲率,则位置误差方程为:
式中,RM和RN分别为地球子午圈半径和地球卯酉圈半径;对于中低速运动的载体,对位置误差微分方程进行以下简化:
将基于载体系的加性四元数非线性误差模型记为Qb-Add model,将基于载体系的乘性四元数非线性误差模型记为Qb-Mul model,状态量的选取具体表示为:
Qb-Add model:
Qb-Mul model:
式中,εb为陀螺常值漂移,为加速度计常值漂移;
对于观测量的选取,将速度和位置作为组合导航的观测量:
式中,为由惯导解算而得的速度和位置信息,/>为由卫星导航得到的速度和位置参考信息;相对应的量测矩阵为:
对于组合导航的滤波方法,采用USQUE滤波。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111067339.XA CN113776529B (zh) | 2021-09-13 | 2021-09-13 | 一种基于载体系四元数姿态误差的非线性误差模型 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111067339.XA CN113776529B (zh) | 2021-09-13 | 2021-09-13 | 一种基于载体系四元数姿态误差的非线性误差模型 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113776529A CN113776529A (zh) | 2021-12-10 |
CN113776529B true CN113776529B (zh) | 2024-04-19 |
Family
ID=78842930
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111067339.XA Active CN113776529B (zh) | 2021-09-13 | 2021-09-13 | 一种基于载体系四元数姿态误差的非线性误差模型 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113776529B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104330092A (zh) * | 2014-07-24 | 2015-02-04 | 南京理工大学 | 一种基于双模型切换的二次传递对准方法 |
CN106595701A (zh) * | 2016-09-20 | 2017-04-26 | 南京喂啊游通信科技有限公司 | 一种基于加性四元数的大方位失准角线性对准方法 |
CN107525503A (zh) * | 2017-08-23 | 2017-12-29 | 王伟 | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 |
CN112629538A (zh) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 |
CN112857398A (zh) * | 2021-01-12 | 2021-05-28 | 中国人民解放军海军工程大学 | 一种系泊状态下舰船的快速初始对准方法和装置 |
-
2021
- 2021-09-13 CN CN202111067339.XA patent/CN113776529B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104330092A (zh) * | 2014-07-24 | 2015-02-04 | 南京理工大学 | 一种基于双模型切换的二次传递对准方法 |
CN106595701A (zh) * | 2016-09-20 | 2017-04-26 | 南京喂啊游通信科技有限公司 | 一种基于加性四元数的大方位失准角线性对准方法 |
CN107525503A (zh) * | 2017-08-23 | 2017-12-29 | 王伟 | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 |
CN112629538A (zh) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 |
CN112857398A (zh) * | 2021-01-12 | 2021-05-28 | 中国人民解放军海军工程大学 | 一种系泊状态下舰船的快速初始对准方法和装置 |
Non-Patent Citations (5)
Title |
---|
Li Kailong et al..Nonlinear Error Model Based on Quaternion for the INS: Analysis and Comparison.《IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY》.2021,第70卷(第1期),263-272. * |
Yu MJ et al..Comparison of SDINS in-flight alignment using equivalent error models.《IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS》.1999,第35卷(第3期),1046-1054. * |
吕旭等.一种改进的自适应USQUE 组合导航姿态估计方法.《科学技术与工程》.2021,第21卷(第15期),6494-6500. * |
夏家和 ; 秦永元 ; 赵长山 ; .适用于低精度惯导的非线性对准方法研究.仪器仪表学报.2009,(第08期),56-60. * |
宋嘉钰等.惯性导航传递对准技术发展现状与趋势.《兵器装备工程学报》.2016,第37卷(第2期),139-143. * |
Also Published As
Publication number | Publication date |
---|---|
CN113776529A (zh) | 2021-12-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109974697B (zh) | 一种基于惯性系统的高精度测绘方法 | |
CN107588769B (zh) | 一种车载捷联惯导、里程计及高程计组合导航方法 | |
CN110501024B (zh) | 一种车载ins/激光雷达组合导航系统的量测误差补偿方法 | |
US7711483B2 (en) | Dead reckoning system | |
CN111156994B (zh) | 一种基于mems惯性组件的ins/dr&gnss松组合导航方法 | |
CN101476894B (zh) | 车载sins/gps组合导航系统性能增强方法 | |
CN101907714B (zh) | 基于多传感器数据融合的gps辅助定位方法 | |
CN104061899B (zh) | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 | |
CN100516775C (zh) | 一种捷联惯性导航系统初始姿态确定方法 | |
CN103245359B (zh) | 一种惯性导航系统中惯性传感器固定误差实时标定方法 | |
CN105698822B (zh) | 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法 | |
CN105318876A (zh) | 一种惯性里程计组合高精度姿态测量方法 | |
JP2018047888A (ja) | ビークルの角度位置を測定するシステムおよび方法 | |
CN112284415B (zh) | 里程计标度误差标定方法、系统及计算机存储介质 | |
CN104977002A (zh) | 基于sins/双od的惯性组合导航系统及其导航方法 | |
CN113340298B (zh) | 一种惯导和双天线gnss外参标定方法 | |
CN103453903A (zh) | 一种基于惯性测量组件的管道探伤系统导航定位方法 | |
CN112762961A (zh) | 一种车载惯性里程计组合导航在线标定方法 | |
CN109470276B (zh) | 基于零速修正的里程计标定方法与装置 | |
CN116164773A (zh) | 一种基于载体系四元数姿态误差的李群捷联惯导误差模型 | |
CN112432642A (zh) | 一种重力灯塔与惯性导航融合定位方法及系统 | |
CN105928519A (zh) | 基于ins惯性导航与gps导航以及磁力计的导航算法 | |
CN109945857B (zh) | 一种面向不动产实地测量的车载惯性定位方法及其装置 | |
CN113776529B (zh) | 一种基于载体系四元数姿态误差的非线性误差模型 | |
CN116878503A (zh) | 基于gps和imu姿态传递匹配的改进imu-rtk松组合导航方法及系统 |
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 |