CN106525034A - 一种基于对偶四元数的惯导系统传递对准建模方法 - Google Patents

Abstract

本发明提出了一种基于对偶四元数的惯导系统传递对准建模方法，构造主惯导系统和子惯导系统间的标称对偶四元数、计算对偶四元数分别来描述子惯导系统载体系相对于主惯导系统载体系、计算载体系的旋转和平移运动，通过推理主惯导系统与子惯导系统的相对旋转和平移运动的旋量表达式，构建传递对准的标称对偶四元数微分方程和计算对偶四元数的微分方程；联合加速度计参数误差方程和陀螺仪误差微分方程获得对偶四元数误差方程；利用加速度计的线速度和陀螺仪的旋转角速度构造系统观测量方程，由kalman滤波迭代计算获得子惯导系统的初始标定参数，摆脱了旋转和平移分离计算带来的圆锥误差和划船误差的影响，有效提高了计算精度和计算效率。

Description

一种基于对偶四元数的惯导系统传递对准建模方法

技术领域

本发明涉及航空航天系统信息处理的技术领域，具体涉及一种基于对偶四元数的惯导系统传递对准建模方法。

背景技术

惯性导航系统是一套复杂的高精度机电综合系统，历经了平台式惯性导航系统和捷联式惯性导航系统，其中早期的平台式惯导系统的姿态信息是由陀螺力矩为主的万向支架构建的稳定平台直接提供，而捷联式惯导系统(捷联式惯导系统)将惯性组件(InertialMeasurement Unit，IMU)直接固连在载体上，利用计算机平台替代物理平台，具备了体积小、成本低和可靠性高等特点。但是，捷联惯性导航系统对IMU组件的性能提出了更高的要求，必须根据陀螺仪输出的角速度计算并维持一个数学平台，在此数学平台上由加速度计输出积分获得速度和位置信息。因此，捷联惯性导航方法在捷联式惯导系统中具有非常重要的作用。

在捷联式惯性导航系统中导航计算机的主要任务是实现角速度积分计算获取载体姿态信息，即姿态解算，利用获取的姿态信息将加速度变换到合适的导航坐标系，再进行加速度积分计算获取载体的速度信息，进而对速度积分获取载体位置信息，这两步也称为导航解算。为了保证导航方法误差与惯性组件引入的误差相比足够小以致可以忽略不计，上述三个积分过程必须采用高精度的数值积分方法，如选用四阶以上的Runge-Kutta法才能满足捷联式惯导系统微分方程的数值积分精度要求，以此来降低转动的不可交换性带来的负面影响，如圆锥(Coning)效应、划船(Sculling)效应和卷轴(Scrolling)效应等影响，也可以说捷联惯性导航方法的发展过程就是一个寻求高动态环境下的高精度数值积分方法的发展史。

捷联式惯性导航系统不同于平台式惯导系统的重要特点就是以计算机实现数学平台替代物理平台，数学平台根据IMU组件感测的数据信息计算和描述坐标系间的转换关系，这种转换关系中有三个转动自由度和三个平移自由度。通常的坐标转换关系采用方向余弦法(DCM)，利用DCM表示转动，向量表示平移运动，它需要九个未知元素，计算量较大；而四元数法有四个元素，计算量小、精度较高，目前大多采用四元数法实现载体姿态转动的最简洁表达方式。但是，这些传统的姿态描述方法都把载体旋转运动和平移运动割裂开来，实际上Charles定理表明任何刚体运动都可以通过绕某个轴的旋转和沿相同轴的平移运动实现。因此，这种旋转和平移组合运动很像是一种螺旋运动，可以用螺旋变换或者螺旋运动来描述一般性刚体运动。1873年Clifford提出了对偶数(Dual number)概念，并构建了对偶四元数代数(Dual Quaternion)来描述螺旋变换，其特点是对偶四元数把刚体的旋转与平移运动统一考虑，数学表达直观明了。

同时，20世纪90年代Branets把对偶四元数代数应用于惯性导航系统的领域，首次论述了捷联式惯性导航系统理论分析中对偶四元数代数建模的可行性，并勾画了利用对偶四元数描述捷联式惯性导航系统中坐标系转换的基本框架。实际上利用对偶四元数开展捷联式惯性导航系统建模研究还带来一个优势，就是可以实现捷联式惯性导航系统的无平台计算。利用对偶四元数建立捷联式惯导系统模型的过程中，基于推力坐标系T、引力坐标系I和位置坐标系U来实现，摈弃了以前物理平台或者数学平台的不利影响，这对于现代的无平台惯导系统理论研究将会开辟一条新的途径。

本发明基于对偶四元数描述方法，在推力坐标系、引力坐标系和位置坐标系基础上研究一种新型的对偶四元数描述的捷联式惯导系统传递对准模型算法。通过查阅已发表文献可知利用对偶四元数建立惯导系统导航方法和捷联式惯导系统的初始对准模型方法都已经出现了，但是对于现代导航系统在载体运动情形下的传递对准过程的建模研究还没有相关文献资料，更多的是利用四元数、修正罗德里格斯参数或者欧拉角法实现传递对准系统建模研究分析。基于前人文献资料的分析研究，本发明采用对偶四元数法构建传递对准模型算法具有较强的创新性，并且与传统的姿态描述方法建模获得的传递对准模型算法相比，传递对准的对偶四元数模型计算量小，计算效率高，并且脱离了捷联平台的影响，能够有效扩展对偶四元数在惯导系统中的应用，也开辟了捷联惯导系统模型研究的新途径。传递对准是解决舰载机捷联式惯导系统在舰船载体运动情况下海上对准问题的主要方法，它是利用舰船惯导装置导航参数与机载惯导系统相应导航参数匹配计算，利用滤波算法估计机载惯导系统失准角等导航参数，进而对机载惯导系统导航参数初始化。

发明内容

为了解决上述技术问题，本发明提出一种基于对偶四元数的惯导系统传递对准建模方法，基于对偶四元数描述，在推力坐标系、引力坐标系和位置坐标系下构建传递对准模型，脱离了捷联平台影响；利用对偶四元数来统一描述载体姿态转动和平移运动过程，摆脱了传统方法的旋转和平移分离计算带来的捷联计算中圆锥误差和划船误差的影响，有效提高了的计算精度和计算效率。

为了达到上述目的，本发明的技术方案是：一种基于对偶四元数的惯导系统传递对准建模方法，其步骤如下：

步骤一：设定舰船主惯导系统的载体坐标系为m，舰载机子惯导系统的标称载体坐标系为s，舰载机子惯导系统的计算载体坐标系为s’；利用对偶四元数定义主惯导系统载体系m与子惯导系统的标称载体坐标系s间的标称对偶四元数向量利用对偶四元数定义主惯导系统载体坐标系m相对于子惯导系统计算载体系s’间的计算对偶四元数向量其中，表示主惯导系统载体坐标系m相对于导航坐标系n在子惯导系统的标称载体坐标系s中的投影四元数，表示对偶算子，r表示主惯导系统与子惯导系统标称载体坐标系间的杆臂矢量，表示主惯导系统载体坐标系m相对于导航坐标系n在子惯导系统的计算载体坐标系s′中的投影四元数，r'表示主惯导系统相对于子惯导系统的计算载体坐标系s′间的杆臂矢量；

步骤二：在传递对准过程中，标称子惯导系统相对于主惯导系统是静止的，构造主惯导系统载体系m与子惯导系统的标称载体坐标系s间的标称对偶四元数向量误差微分方程：把标称对偶四元数向量误差微分方程作为系统模型方程的一部分参与系统状态变量的最优滤波计算；

步骤三：考虑主惯导系统载体系m与子惯导系统的标称载体坐标系s旋量间的关系，主惯导系统载体系m与子惯导系统的标称载体坐标系s相对旋转与平移的对偶四元数旋量 其中，表示主惯导系统载体坐标系m相对于导航坐标系n的对偶四元数旋量，表示子惯导系统的标称载体坐标系相对于导航坐标系n的对偶四元数旋量，ε^s表示子惯导系统中的陀螺零漂移误差量；

步骤四：构造主惯导系统载体系m与子惯导系统的计算载体坐标系为s’间的标称对偶四元数向量误差微分方程：其中，表示子惯导系统的计算载体坐标系相对于主惯导系统的旋量在导航坐标系中的投影，(.)^*表示向量的逆；

步骤五：联合加速度计误差微分方程：陀螺仪误差微分方程：主惯导系统和子惯导系统间的杆臂矢量微分方程：构建主惯导系统和子惯导系统间的传递对准系统误差模型方程：

其中，ε表示系统中陀螺仪随机漂移的三维矢量，表示系统中加速度计的三维零漂移误差量；连同三维杆臂矢量和八维的标称对偶四元数微分方程，系统模型中需要添加0_17×17的零矩阵；

步骤六：考虑线速度误差和旋转角速度误差，构造主惯导系统与子惯导系统传递对准过程中的对偶四元数旋量的观测方程：

其中，表示观测模型中的计算子惯导系统相对于主惯导系统在导航坐标系中观测到的对偶旋量，表示计算子惯导系统相对于主惯导系统的旋量在导航系中的投影的观测量；B_ω表示系统陀螺仪和加速度计的偏差的对偶量，η_ω表示系统速度旋量的观测噪声；

步骤七：利用Kalman滤波方法求解传递对准系统误差模型方程和观测旋量方程，获得系统状态变量：标称姿态失准角、计算姿态失准角、加速度计和陀螺仪参数的计算。

所述主惯导系统载体系m与子惯导系统的计算载体坐标系为s’间的标称对偶四元数向量误差微分方程的构建方法是：

根据对偶四元数的微分表达式计算对偶四元数向量的微分方程表达式：其中，表示主惯导系统与计算子惯导系统间的旋量，且

其中，表示主惯导系统相对于导航坐标系的旋转角速度在计算子惯导系统中的投影，表示主惯导系统相对于导航坐标系的线速度在计算子惯导系统中的投影的微分，表示主惯导系统相对于导航坐标系的旋转四元数在计算子惯导系统中的投影的共轭；

主惯导系统相对于导航坐标系的线速度在计算子惯导系统中的投影的微分可表示为其中表示主惯导系统的线加速度，表示计算子惯导系统的线加速度，表示杆臂矢量的加速度，根据惯导系统速度方程得：

其中，f^m表示主惯导系统的比力，表示主惯导系统的旋转四元数，表示主惯导系统的地球自转角速度，表示主惯导系统相对于地球坐标系e的旋转角速度，V^m表示主惯导系统的速度，g^m表示导航坐标系中的重力加速度；

主惯导系统和子惯导系统间的杆臂误差速度表达为其微分方程为：

其中，表示主惯导系统相对于导航坐标系的旋转角速度，r^m表示杆臂矢量，表示主惯导系统相对于地球坐标系的旋转角速度的微分；

那么可以获得主惯导系统与子惯导系统速度误差表达式：

其中，是的逆，表示主子惯导系统间的相对速度误差在导航坐标系中的投影，表示计算子惯导系统相对于导航系的旋转四元数，f^s表示子惯导系统的测量比力，表示主惯导系统相对于计算子惯导系统的旋转四元数，主子惯导系统间的旋转四元数，表示主惯导系统相对于计算子惯导系统的旋转四元数的逆，表示地球自转角速度，表示地球自转角速度在导航坐标系中的投影，表示主惯导系统相对于地球坐标系的旋转角速度，表示计算子惯导系统和主惯导系统间的相对线速度在导航系中的投影，rⁿ表示导航系中的杆臂矢量，表示子惯导系统的加速度计测量偏差；

针对主惯导系统的对偶四元数以及子惯导系统的对偶四元数的微分方程分别为：

其中，表示主惯导系统的对偶四元数的微分，表示主惯导系统的对偶四元数，表示主惯导系统的旋量；

主惯导系统与子惯导系统的旋转对偶四元数间的关系：其中，表示子惯导系统的旋量，表示主子惯导系统间的对偶四元数的共轭，表示主子惯导系统间的相对旋量在导航系中的投影，表示主子惯导系统间的对偶四元数；

利用方向余弦矩阵表达旋转角速度与间的关系为：其中，表示主子惯导系统间的方向余弦矩阵；对主惯导系统的对偶四元数分解得：对其两边同时求导整理得：

其中，表示主惯导系统相对于导航坐标系的对偶四元数在计算子惯导系统的投影，表示计算子惯导系统相对于导航系的对偶四元数，表示主惯导系统的对偶四元数的微分，表示计算子惯导系统的对偶四元数的微分，表示主惯导系统相对于导航坐标系的对偶四元数；

结合主惯导系统与子惯导系统的姿态对偶四元数微分方程获得：

其中，表示标称子惯导系统相对于导航系的旋量，表示主惯导系统相对于导航系的旋量；

考虑主惯导系统与子惯导系统间的旋量间的关系，主惯导系统与子惯导系统间的相对旋转与平移的对偶四元数旋量为：

从而得那么其逆为：

获得主惯导系统与子惯导系统的姿态平移对偶四元数误差方程为：

所述加速度计误差微分方程和陀螺仪误差微分方程的构建方法是：

系统观测旋量表达的是主子惯导系统与惯导系统间的相对旋转与平移误差对偶四元数向量，其显性表达式为：

其中，是主惯导系统与子惯导系统间的旋转误差矢量，且： 表示主子惯导系统间的相对线速度误差，且：

子惯导系统敏感到的角速度和加速度可分别表示为：

其中，表示能被主惯导系统敏感到的载体角速度在子惯导系统惯性系i中的投影，ε为陀螺零偏误差，为主惯导系统敏感到的载体加速度在子惯导系统惯性系i中的投影，为载体转动引起的子惯导系统杆臂加速度，为加速度计的零偏误差，n_g和n_a为随机噪声；

待标参数误差为常值误差，随机噪声为零均值不相关白噪声，则有：

所述主惯导系统与子惯导系统传递对准过程中的对偶四元数旋量的观测旋量方程的构建方法是：采用角速度+线速度的匹配模式获取子惯导系统解算的线速度，主惯导系统提供的经过刚性杆臂补偿后的速度，将其与子惯导系统输出速度相减获得速度差作为线速度观测量；取角速度误差观测量为可表达为：

相应的线速度误差可表达为：

其中，表示主惯导系统与计算子惯导系统的速度误差，表示子惯导系统的输出速度，表示主惯导系统的输出速度，表示主惯导系统的姿态矩阵，表示主惯导系统相对于地球系的旋转角速度，表示加速度计的偏差；

则系统观测旋量可统一表示为：

其中， 表示计算子惯导系统相对于主惯导系统的旋转角速度在导航系中的投影， 表示计算子惯导系统相对于主惯导系统的线速度在导航系中的投影；B_ω＝b_ω+εb_v， 是角速度观测量的偏差， 表示系统线速度的偏差；η_ω＝η_ω+εη_v， 表示系统角速度观测噪声， 表示系统线速度观测噪声，和均为高斯白噪声。

本发明利用对偶四元数构造主惯导系统和子惯导系统间的标称对偶四元数来描述子惯导系统载体系相对于主惯导系统载体系的旋转和平移运动，构造计算对偶四元数来描述主惯导系统载体系相对于子惯导系统计算载体系的旋转和平移运动，再通过推理主惯导系统相对于子惯导系统的相对旋转和平移运动的旋量表达式、传递对准的标称对偶四元数微分方程，联合系统加速度计参数误差方程和陀螺仪误差微分方程，获得传递对准系统的对偶四元数误差方程；利用加速度计的线速度和陀螺仪的旋转角速度构造系统旋量对偶四元数观测量方程，从而获得一种基于对偶四元数的传递对准系统误差模型，经由kalman滤波迭代计算获得子惯导系统的初始标定参数操作。本发明摆脱了旋转和平移分离计算带来的圆锥误差和划船误差的影响，有效提高了计算精度和计算效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明主惯导系统与子惯导系统见传递对准原理图。

图2为本发明标称姿态失准角计算误差数据曲线。

图3为本发明计算姿态失准角计算误差数据曲线。

图4为本发明四元数误差模型的标称姿态失准角计算误差数据曲线。

图5为本发明四元数误差模型的计算姿态失准角计算误差数据曲线。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明首先论述对偶四元数的基本概念及其运算规则，接着利用对偶四元数建立传递对准在推力坐标系下的推力速度微分方程、在引力坐标系下的引力速度微分方程以及在位置坐标系下的位置坐标微分方程；再对这三组微分方程计算求解获得捷联惯导系统的姿态角、速度和位置量，经由坐标转换获得舰载机载体在当地地理坐标系下的导航初始参数；最后对偶四元数的解算精度做出说明与论证。最后给出系统初始参数进行仿真验证算法的有效性与计算效能。

一种基于对偶四元数的惯导系统传递对准建模方法，其步骤如下：

步骤一：设定舰船主惯导系统的载体坐标系为m，舰载机子惯导系统的标称载体坐标系为s，舰载机子惯导系统的计算载体坐标系为s’；利用对偶四元数定义主惯导系统载体系m与子惯导系统的标称载体坐标系s间的标称对偶四元数向量利用对偶四元数定义主惯导系统的载体坐标系m相对于子惯导系统计算载体系s’间的计算对偶四元数向量其中，表示主惯导系统m相对于导航坐标系n在子惯导系统的标称载体坐标系s中的投影四元数，表示对偶算子，r表示主惯导系统与标称子惯导系统间的杆臂矢量，表示主惯导系统的载体坐标系m相对于导航坐标系n在计算子惯导系统s′中的投影四元数，r'表示主惯导系统相对于子惯导系统的计算载体坐标系s′间的杆臂矢量。

对偶四元数是一种元素为对偶数的特殊四元数，其定义为其中Q称为实部，Q'称为对偶部，它们都是四元数，对偶部中的是对偶单位数，满足且对偶四元数的范数定义为：

其中，表示对偶四元数的共轭，其表达为：若对偶四元数的实部非零，那么对偶四元数的逆存在，且表示为对于单位对偶四元数来说其逆等于对偶四元数的共轭，即对偶四元数作为一类特殊的四元数，它完全继承了四元数的性质，但是其有一些独特的运算规则。

对偶四元数加法定义：

对偶四元数与一个标量相乘的乘法定义：

对偶四元数间的乘法定义：

且其交换律满足

对偶四元数的点乘乘法定义：

对偶四元数叉乘乘法定义：

且其交换律满足

对偶四元数与矩阵的乘积定义：设矩阵M∈R^8×8，且那么矩阵与一个对偶四元数的乘积为

对偶四元数的圈乘定义:

在本发明中舰船主惯导系统的载体坐标系为m，舰载机子惯导系统的标称载体坐标系为s，舰载机子惯导系统的计算载体坐标系为s’。在传递对准过程中，主惯导系统是一个无误差信源的基准系统，在舰船行进过程中，舰船主惯导系统的载体坐标系m与舰载机子惯导系统的标称载体坐标系s之间的相对位置是固定不变的，那么可以定义一个标称对偶四元数表示子惯导系统相对于主惯导系统的相对旋转与平移运动：其中，是主惯导系统与子惯导系统间的旋转四元数，矢量r表示主惯导系统与子惯导系统间的位移矢量。同时主惯导系统相对于子惯导系统的计算载体坐标系定义一个计算对偶四元数那么

步骤二：构造主惯导系统载体系m与子惯导系统的标称载体坐标系s间的标称对偶四元数向量误差微分方程：

本发明利用微小扰动摄动法来推导传递对准系统的误差方程。在传递对准过程中一般认为，标称子惯导系统相对于主惯导系统是静止的，标称对偶四元数可以认为是一个常值，因此其微分可表示为：把标称对偶四元数向量误差微分方程作为系统模型方程的一部分参与系统状态变量的最优滤波计算。

步骤三：考虑主惯导系统载体系m与子惯导系统的标称载体坐标系s旋量间的关系，主惯导系统载体系m与子惯导系统的标称载体坐标系s相对旋转与平移的对偶四元数旋量 其中，表示主惯导系统相对于导航坐标系n的对偶四元数旋量,表示标称子惯导系统相对于导航坐标系的对偶四元数旋量，ε^s表示子惯导系统中的陀螺零漂移误差量。

主惯导系统与子惯导系统的旋转对偶四元数间具有这样的关系：若是利用方向余弦矩阵表达则有旋转角速度与间有可以对主惯导系统的对偶四元数分解获得并且对其两边同时求导整理获得：

结合主惯导系统和子惯导系统的姿态对偶四元数微分方程获得

可以考虑主惯导系统与子惯导系统的旋量间的关系，定义一个表达主子惯导系统间的相对旋转与平移的对偶四元数旋量其表达式为

步骤四：构造主惯导系统载体系m与子惯导系统的计算载体坐标系为s’间的标称对偶四元数向量误差微分方程：其中，表示计算子惯导系统相对于主惯导系统的旋量在导航坐标系中的投影，(.)^*表示向量的逆。

根据对偶四元数的微分表达式定义计算对偶四元数向量的微分方程表达式：其中，表示主惯导系统与子惯导系统间的旋量，也是一个对偶四元数，其一般表达式为：

其中，表示主惯导系统相对于导航坐标系的旋转角速度在计算子惯导系统中的投影，表示主惯导系统相对于导航坐标系的线速度在计算子惯导系统中的投影的微分，表示主惯导系统相对于导航坐标系的旋转四元数在计算子惯导系统中的投影的共轭。

其中，主惯导系统相对于导航坐标系的线速度在计算子惯导系统中的投影的微分可表示为其中表示主惯导系统的线加速度，表示计算子惯导系统的线加速度，表示杆臂矢量的加速度。根据惯导系统速度方程可得到：

其中，f^m表示主惯导系统的比力，表示主惯导系统的旋转四元数，表示主惯导系统的地球自转角速度，表示主惯导系统相对于地球坐标系e的旋转角速度，V^m表示主惯导系统的速度，g^m表示导航坐标系中的重力加速度。

主惯导系统和子惯导系统间的杆臂误差速度可表达为其微分方程为：

其中，表示主惯导系统相对于导航坐标系的旋转角速度，r^m表示杆臂矢量，表示主惯导系统相对于地球坐标系的旋转角速度的微分。

那么可以获得主惯导系统与子惯导系统速度误差表达式：

其中，是的逆，表示主子惯导系统间的相对速度误差在导航坐标系中的投影，表示计算子惯导系统相对于导航系的旋转四元数，f^s表示子惯导系统的测量比力，表示主惯导系统相对于计算子惯导系统的旋转四元数，主子惯导系统间的旋转四元数，表示主惯导系统相对于计算子惯导系统的旋转四元数的逆，表示地球自转角速度，表示地球自转角速度在导航坐标系中的投影，表示主惯导系统相对于地球坐标系的旋转角速度，表示计算子惯导系统和主惯导系统间的相对线速度在导航系中的投影，rⁿ表示导航系中的杆臂矢量，表示子惯导系统的加速度计测量偏差。

针对主惯导系统的对偶四元数以及子惯导系统的对偶四元数的微分方程分别为：

其中，表示主惯导系统的对偶四元数的微分，表示主惯导系统的对偶四元数，表示主惯导系统的旋量。

主惯导系统与子惯导系统的旋转对偶四元数间的关系：其中，表示子惯导系统的旋量，表示主子惯导系统间的对偶四元数的共轭，表示主子惯导系统间的相对旋量在导航系中的投影，表示主子惯导系统间的对偶四元数。

利用方向余弦矩阵表达旋转角速度与间的关系为：其中，表示主子惯导系统间的方向余弦矩阵；对主惯导系统的对偶四元数分解得：对其两边同时求导整理得：

其中，表示主惯导系统相对于导航坐标系的对偶四元数在计算子惯导系统的投影，表示计算子惯导系统相对于导航系的对偶四元数，表示主惯导系统的对偶四元数的微分、表示计算子惯导系统的对偶四元数的微分，表示主惯导系统相对于导航坐标系的对偶四元数。

结合主惯导系统与子惯导系统的姿态对偶四元数微分方程获得：

其中，表示标称子惯导系统相对于导航系的旋量，表示主惯导系统相对于导航系的旋量。

考虑主惯导系统与子惯导系统间的旋量间的关系，主惯导系统与子惯导系统间的相对旋转与平移的对偶四元数旋量为：

从而得那么其逆为：

因此，获得主惯导系统与子惯导系统的姿态平移对偶四元数误差方程为：

步骤五：联合加速度计误差微分方程：陀螺仪误差微分方程：主惯导系统和子惯导系统间的杆臂矢量微分方程：构建主惯导系统和子惯导系统间的传递对准系统误差模型方程：

其中，ε表示系统中陀螺仪随机漂移的三维矢量，表示系统中加速度计的三维零漂移误差量；连同三维杆臂矢量和八维的标称对偶四元数微分方程，系统模型中需要添加0_17×17的零矩阵阵。

系统观测旋量表达的是主子惯导系统与惯导系统间的相对旋转与平移误差对偶四元数向量，其显性表达式为：

其中，是主惯导系统与子惯导系统间的旋转误差矢量，且： 表示主子惯导系统间的相对线速度误差，且

子惯导系统敏感到的角速度和加速度可分别表示为：

其中，表示能被主惯导系统敏感到的载体角速度在子惯导系统惯性系中的投影，ε为陀螺零偏误差，为主惯导系统敏感到的载体加速度在子惯导系统惯性系中的投影，为载体转动引起的子惯导系统杆臂加速度，为加速度计的零偏误差，n_g和n_a为随机噪声；

待标参数误差为常值误差，随机噪声为零均值不相关白噪声，则有：

主惯导系统与子惯导系统间传递对准过程中杆臂矢量rⁿ是不变的，其微分方程为

步骤六：考虑线速度误差和旋转角速度误差，构造主惯导系统与子惯导系统传递对准过程中的对偶四元数旋量的观测旋量方程：

其中，表示观测模型中的计算子惯导系统相对于主惯导系统在导航坐标系中观测到的对偶旋量，表示计算子惯导系统相对于主惯导系统的旋量在导航系中的投影的观测量，其表达式为 表示计算子惯导系统相对于主惯导系统的旋转角速度在导航系中的投影， 表示计算子惯导系统相对于主惯导系统的线速度在导航系中的投影、B_ω表示系统陀螺仪和加速度计的偏差的对偶量，可表示为B_ω＝b_ω+εb_v， 是角速度观测量的偏差， 表示系统线速度的偏差、η_ω＝η_ω+εη_v表示系统速度旋量的观测噪声， 表示系统角速度观测噪声， 表示系统线速度观测噪声，我们都假设为高斯白噪声。

本发明采用角速度+线速度的匹配模式获取子惯导系统解算的线速度，主惯导系统提供的经过刚性杆臂补偿后的速度，将其与子惯导系统输出速度相减获得速度差作为线速度观测量；取角速度误差观测量为可表达为：

相应的线速度误差可表达为：

其中，表示主惯导系统与计算子惯导系统的速度误差、表示子惯导系统的输出速度、表示主惯导系统的输出速度、表示主惯导系统的姿态矩阵、表示主惯导系统相对于地球系的旋转角速度、表示加速度计的偏差。

则系统观测旋量可进一步整理为：

步骤七：利用Kalman滤波方法求解传递对准系统误差模型方程和观测旋量方程，获得系统状态变量：标称姿态失准角、计算姿态失准角、加速度计和陀螺仪参数的计算。

根据前面的子惯导系统状态参数对偶四元数传递对准模型的推理过程，可以总结获得系统过程模型方程为

和对偶数旋量的系统观测旋量方程为

下面考虑迭代递推过程中系统噪声和观测噪声方差矩阵计算问题。

针对对偶速度的系统观测旋量方程：

其中，对偶速度是一个对偶数向量，且 是主惯导系统和子惯导系统的旋转角速度误差的观测量在导航坐标系中的投影，其中，是主子惯导系统在导航坐标系中投影的线速度误差观测量。针对前面已经推导出的主惯导系统和子惯导系统的角速度误差和线速度误差的表达形式，将其观测量偏差以及噪声整理为B_ω和η_ω，且并且 是角速度误差量的偏差，其中，是线速度观测量的偏差；其中，η_ω表示角速度误差噪声，η_v表示线速度误差噪声，且假设其为高斯白噪声向量，且具有噪声方差矩阵

在迭代递推计算过程中，每一步迭代计算后都需要考虑对偶四元数的强制规范化问题。对于对偶四元数的实部部分，根据对偶四元数定义式：则其实部部分的规范化计算式：

相应的对偶四元数的对偶部分量部分Q'可以采用计算表达式

利用对偶四元数的强制规范化计算结果参与下一次迭代计算，从而保证迭代滤波计算的稳定性。

根据前面系统状态方程，可知系统状态变量为状态变量的初始值都设为0，利用舰船三轴摇摆运动模型仿真舰载机惯导系统，舰船纵摇、横摇和航向摇摆幅度分别为6°，5°和7°；摇摆频率分别为0.05，0.10，和0.05Hz；初始角分别设为0°，0°和90°；舰船东向和北向初始速度取值为10m/s，舰船所在地理位置设为北纬45.60°，东经124.75°。为了验证本发明传递对准系统对偶四元数误差模型的鲁棒性，初始姿态误差角设为15°，45°和60°，陀螺常值漂移为5°/h，随机漂移为0.01°/h；加速度计初始偏差为0.09g，随机漂移为0.004g,忽略惯性组件安装误差以及刻度因子误差，线速度测量误差0.01m/s。从而对传递对准系统的对偶四元数误差模型展开仿真计算，获得仿真结果如图2-5所示。本发明展示了主惯导系统与子惯导系统间的标称姿态失准角、计算姿态失准角在对偶四元数误差模型迭代递推计算的结果，并且和四元数误差模型进行对比验证。由图2-5可得，本发明计算的标称姿态失准角误差数据比较稳定，能够快速收敛于零轴附近，计算效能明显优于传统的四元数模型算法。

本发明综合利用对偶四元数构建的传递对准系统的对偶四元数误差模型，以及利用线速度和角速度构建的观测旋量方程，可以采用Kalman滤波方法，最终获得系统状态变量，如标称姿态失准角、计算姿态失准角以及加速度计和陀螺仪参数的估计计算，完成传递对准系统参数的初始标定操作。

本发明说明书中未作详细描述的内容属于本领域专业技术人员所公知的现有技术。以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于对偶四元数的惯导系统传递对准建模方法，其特征在于，其步骤如下：

步骤一：设定舰船主惯导系统的载体坐标系为m，舰载机子惯导系统的标称载体坐标系为s，舰载机子惯导系统的计算载体坐标系为s’；利用对偶四元数定义主惯导系统载体系m与子惯导系统的标称载体坐标系s间的标称对偶四元数向量利用对偶四元数定义主惯导系统载体坐标系m相对于子惯导系统计算载体系s’间的计算对偶四元数向量其中，表示主惯导系统载体坐标系m相对于导航坐标系n在子惯导系统的标称载体坐标系s中的投影四元数，∈表示对偶算子，r表示主惯导系统与子惯导系统标称载体坐标系间的杆臂矢量，表示主惯导系统载体坐标系m相对于导航坐标系n在子惯导系统的计算载体坐标系s′中的投影四元数，r'表示主惯导系统相对于子惯导系统的计算载体坐标系s′间的杆臂矢量；

步骤二：在传递对准过程中，标称子惯导系统相对于主惯导系统是静止的，构造主惯导系统载体系m与子惯导系统的标称载体坐标系s间的标称对偶四元数向量误差微分方程：把标称对偶四元数向量误差微分方程作为系统模型方程的一部分参与系统状态变量的最优滤波计算；

步骤三：考虑主惯导系统载体系m与子惯导系统的标称载体坐标系s旋量间的关系，主惯导系统载体系m与子惯导系统的标称载体坐标系s相对旋转与平移的对偶四元数旋量 其中，表示主惯导系统载体坐标系m相对于导航坐标系n的对偶四元数旋量，表示子惯导系统的标称载体坐标系相对于导航坐标系n的对偶四元数旋量，ε^s表示子惯导系统中的陀螺零漂移误差量；

步骤四：构造主惯导系统载体系m与子惯导系统的计算载体坐标系为s’间的标称对偶四元数向量误差微分方程：其中，表示子惯导系统的计算载体坐标系相对于主惯导系统的旋量在导航坐标系中的投影，(.)^*表示向量的逆；

步骤五：联合加速度计误差微分方程：陀螺仪误差微分方程：主惯导系统和子惯导系统间的杆臂矢量微分方程：构建主惯导系统和子惯导系统间的传递对准系统误差模型方程：

其中，ε表示系统中陀螺仪随机漂移的三维矢量，表示系统中加速度计的三维零漂移误差量；连同三维杆臂矢量和八维的标称对偶四元数微分方程，系统模型中需要添加0_17×17的零矩阵；

步骤六：考虑线速度误差和旋转角速度误差，构造主惯导系统与子惯导系统传递对准过程中的对偶四元数旋量的观测方程：

其中，表示观测模型中的计算子惯导系统相对于主惯导系统在导航坐标系中观测到的对偶旋量，表示计算子惯导系统相对于主惯导系统的旋量在导航系中的投影的观测量；B_ω表示系统陀螺仪和加速度计的偏差的对偶量，η_ω表示系统速度旋量的观测噪声；

步骤七：利用Kalman滤波方法求解传递对准系统误差模型方程和观测旋量方程，获得系统状态变量：标称姿态失准角、计算姿态失准角、加速度计和陀螺仪参数的计算。

2.根据权利要求1所述的基于对偶四元数的惯导系统传递对准建模方法，其特征在于，所述主惯导系统载体系m与子惯导系统的计算载体坐标系为s’间的标称对偶四元数向量误差微分方程的构建方法是：

根据对偶四元数的微分表达式计算对偶四元数向量的微分方程表达式：其中，表示主惯导系统与计算子惯导系统间的旋量，且

其中，表示主惯导系统相对于导航坐标系的旋转角速度在计算子惯导系统中的投影，表示主惯导系统相对于导航坐标系的线速度在计算子惯导系统中的投影的微分，表示主惯导系统相对于导航坐标系的旋转四元数在计算子惯导系统中的投影的共轭；

主惯导系统相对于导航坐标系的线速度在计算子惯导系统中的投影的微分可表示为其中表示主惯导系统的线加速度，表示计算子惯导系统的线加速度，表示杆臂矢量的加速度，根据惯导系统速度方程得：

${\stackrel{\·}{V}}^m={\left(Q_n^m\right)}^{*}{f}^m{Q}_n^m-(2{\mathrm{\ω}}_{ie}^m+{\mathrm{\ω}}_{em}^m)\×V^m+g^m,$

${\stackrel{\·}{V}}^{s\′}={\left(Q_{s^{\′}}^n\right)}^{*}(f^s+{\▿}^s)Q_{s\′}^n-(2{\mathrm{\ω}}_{ie}^s+{\mathrm{\ω}}_{es}^s)\×V^s+g^s;$

其中，f^m表示主惯导系统的比力，表示主惯导系统的旋转四元数，表示主惯导系统的地球自转角速度，表示主惯导系统相对于地球坐标系e的旋转角速度，V^m表示主惯导系统的速度，g^m表示导航坐标系中的重力加速度；

主惯导系统和子惯导系统间的杆臂误差速度表达为其微分方程为：

${\stackrel{\·}{V}}^r=\frac{1}{2}{\left(Q_n^m\right)}^{*}({\mathrm{\ω}}_n^m\×{\mathrm{\ω}}_{em}^m\×r^m)Q_n^m+\frac{1}{2}{\left(Q_n^m\right)}^{*}({\stackrel{\·}{\mathrm{\ω}}}_{em}^m\×r^m)Q_n^m+\frac{1}{2}{\left(Q_n^m\right)}^{*}({\mathrm{\ω}}_{em}^m\×r^m)Q_n^m{\mathrm{\ω}}_n^m;$

其中，表示主惯导系统相对于导航坐标系的旋转角速度，r^m表示杆臂矢量，表示主惯导系统相对于地球坐标系的旋转角速度的微分；

那么可以获得主惯导系统与子惯导系统速度误差表达式：

$\begin{array}{c}{\stackrel{\·}{v}}_{{\mathrm{ms}}^{\′}}^n={\stackrel{\·}{V}}^m-{\stackrel{\·}{V}}^{s\′}-{\stackrel{\·}{V}}^r\\ =Q_{s\′}^n(f^s-Q_m^{s\′}{Q}_s^m{f}^s{Q}_m^s{Q}_{s\′}^m)Q_n^{s\′}-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_{{\mathrm{ms}}^{\′}}^n-({\mathrm{\ω}}_{em}^n+{\mathrm{\ω}}_{en}^n)\×r^n+{\left(Q_{s^{\′}}^n\right)}^{*}{\▿}^s{Q}_{s^{\′}}^n\end{array};$

其中，是的逆，表示主子惯导系统间的相对速度误差在导航坐标系中的投影，表示计算子惯导系统相对于导航系的旋转四元数，f^s表示子惯导系统的测量比力，表示主惯导系统相对于计算子惯导系统的旋转四元数，主子惯导系统间的旋转四元数，表示主惯导系统相对于计算子惯导系统的旋转四元数的逆，表示地球自转角速度，表示地球自转角速度在导航坐标系中的投影，表示主惯导系统相对于地球坐标系的旋转角速度，表示计算子惯导系统和主惯导系统间的相对线速度在导航系中的投影，rⁿ表示导航系中的杆臂矢量，表示子惯导系统的加速度计测量偏差；

针对主惯导系统的对偶四元数以及子惯导系统的对偶四元数的微分方程分别为：

其中，表示主惯导系统的对偶四元数的微分，表示主惯导系统的对偶四元数，表示主惯导系统的旋量；

主惯导系统与子惯导系统的旋转对偶四元数间的关系：其中，表示子惯导系统的旋量，表示主子惯导系统间的对偶四元数的共轭，表示主子惯导系统间的相对旋量在导航系中的投影，表示主子惯导系统间的对偶四元数；

利用方向余弦矩阵表达旋转角速度与间的关系为：其中，表示主子惯导系统间的方向余弦矩阵；对主惯导系统的对偶四元数分解得：对其两边同时求导整理得：

其中，表示主惯导系统相对于导航坐标系的对偶四元数在计算子惯导系统的投影，表示计算子惯导系统相对于导航系的对偶四元数，表示主惯导系统的对偶四元数的微分，表示计算子惯导系统的对偶四元数的微分，表示主惯导系统相对于导航坐标系的对偶四元数；

结合主惯导系统与子惯导系统的姿态对偶四元数微分方程获得：

其中，表示标称子惯导系统相对于导航系的旋量，表示主惯导系统相对于导航系的旋量；

考虑主惯导系统与子惯导系统间的旋量间的关系，主惯导系统与子惯导系统间的相对旋转与平移的对偶四元数旋量为：

从而得那么其逆为：

获得主惯导系统与子惯导系统的姿态平移对偶四元数误差方程为：

3.根据权利要求1所述的基于对偶四元数的惯导系统传递对准建模方法，其特征在于，所述加速度计误差微分方程和陀螺仪误差微分方程的构建方法是：

系统观测旋量表达的是主子惯导系统与惯导系统间的相对旋转与平移误差对偶四元数向量，其显性表达式为：

其中，是主惯导系统与子惯导系统间的旋转误差矢量，且： 表示主子惯导系统间的相对线速度误差，且：

$v_{{\mathrm{ms}}^{\′}}^n=Q_{s^{\′}}^n(f^s-Q_m^{s^{\′}}{Q}_s^m{f}^s{Q}_m^s{Q}_{s^{\′}}^m)Q_n^{s^{\′}}-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_{{\mathrm{ms}}^{\′}}^n-({\mathrm{\ω}}_{em}^n+{\mathrm{\ω}}_{en}^n)\×r^n+{\left(Q_{s^{\′}}^n\right)}^{*}{\▿}^s{Q}_{s^{\′}}^n;$

子惯导系统敏感到的角速度和加速度可分别表示为：

${\widehat{\mathrm{\ω}}}_{is}^s={\mathrm{\ω}}_{im}^s+{\mathrm{\ϵ}}^s+n_g,$

${\hat{f}}_{is}^s=f_{im}^s+f_r^s+{\▿}^s+n_a;$

其中，表示能被主惯导系统敏感到的载体角速度在子惯导系统惯性系i中的投影，ε为陀螺零偏误差，为主惯导系统敏感到的载体加速度在子惯导系统惯性系i中的投影，为载体转动引起的子惯导系统杆臂加速度，为加速度计的零偏误差，n_g和n_a为随机噪声；

待标参数误差为常值误差，随机噪声为零均值不相关白噪声，则有：

$\stackrel{\·}{\mathrm{\ϵ}}=0,\stackrel{\·}{\▿}=0.$

4.根据权利要求1所述的基于对偶四元数的惯导系统传递对准建模方法，其特征在于，所述主惯导系统与子惯导系统传递对准过程中的对偶四元数旋量的观测旋量方程的构建方法是：采用角速度+线速度的匹配模式获取子惯导系统解算的线速度，主惯导系统提供的经过刚性杆臂补偿后的速度，将其与子惯导系统输出速度相减获得速度差作为线速度观测量；取角速度误差观测量为可表达为：

${\mathrm{\ω}}_{{\mathrm{ms}}^{\′}}^n={\widehat{\mathrm{\ω}}}_{ns}^s-C_m^s{\mathrm{\ω}}_{nm}^m+\mathrm{\ϵ};$

相应的线速度误差可表达为：

${\mathrm{\Δv}}_{{\mathrm{ms}}^{\′}}^n=v_{ns}^n-v_{nm}^n-C_m^n({\mathrm{\ω}}_{im}^m\×r^m)+\▿;$

其中，表示主惯导系统与计算子惯导系统的速度误差，表示子惯导系统的输出速度，表示主惯导系统的输出速度，表示主惯导系统的姿态矩阵，表示主惯导系统相对于地球系的旋转角速度，表示加速度计的偏差；

则系统观测旋量可统一表示为：

其中， 表示计算子惯导系统相对于主惯导系统的旋转角速度在导航系中的投影， 表示计算子惯导系统相对于主惯导系统的线速度在导航系中的投影；B_ω＝b_ω+εb_v， 是角速度观测量的偏差， 表示系统线速度的偏差；η_ω＝η_ω+εη_v， 表示系统角速度观测噪声， 表示系统线速度观测噪声，和均为高斯白噪声。