CN113776529B - 一种基于载体系四元数姿态误差的非线性误差模型 - Google Patents

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系)的载体角速度，/>为地球自转引起的角速度与载体在地球表面运动引起的角速度之和；

为实际的陀螺输出载体角速度，/>为带误差量的惯导角速度；

为陀螺测量误差，可大致看作陀螺常值漂移，/>为计算误差；

I₃为3×3的单位矩阵；

B.加性四元数误差方程

加性四元数误差方程为：

2速度误差方程

定义速度误差且/>

其中，为东向速度误差，/>为北向速度误差，/>为天向速度误差。由比力方程可得到速度误差方程为：

式中，为加速度计的输出，δf^b大致可看作加速度计的常值漂移/>为地球自转角速度，/>为由载体在地球表面运动而引起的角速度，且有/>

和/>为计算误差，可分别表示为：

3位置误差方程

定义位置p＝[L，λ，h]^T，位置误差为δp＝[δL，δλ，δh]^TL，λ，h分别为纬度、经度和高度，δL，δλ，δh分别为纬度误差、经度误差和高度误差；

考虑地球的曲率，则位置误差方程为：

式中，R_M和R_N分别为地球子午圈半径和地球卯酉圈半径。对于中低速运动的载体，可对位置误差微分方程进行以下简化：

本发明的实验验证方法具体实施包含以下步骤：

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滤波。