基于特殊正交群最优估计的SINS捷联惯性导航系统晃动基座粗对准方法与流程

本发明公开了一种基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法，该方法属于导航方法及应用技术领域。

背景技术：

捷联惯性导航系统是一种自主式的导航系统，在工作时不依靠外界信息，也不向外界辐射任何能量，隐蔽性好、抗扰性强，能够全天时、全天候为载体提供完备的运动信息。初始对准是sins中最重要的技术之一，其目的是获得初始姿态矩阵。惯性导航系统的初始对准精度直接关系到系统的工作精度。初始对准时间是惯性导航系统的一项重要技术指标。传统的初始对准方法通常采用粗对准加精对准的方式。

粗对准过程的研究在捷联惯导系统中占有重要意义，尤其是晃动基座下的粗对准方法更是当下的研究热点。晃动基座条件下粗对准可以采用双矢量定姿和基于wahba问题求解两类完成粗对准过程。但这些方法仍然具有不可忽视的缺陷。双矢量确定方法仅利用两个重力矢量来确定初始姿态，舍弃了大量的测量信息，导致粗对准过程精度不高。此外，该方法要求确保姿态的两个重力矢量不能共线，这就使得选择的两个重力矢量之间的时间间隔必须足够长。这意味着基于双向量确定的粗对准过程需要更长的对准时间。虽然wahba方法可以充分利用所有的测量信息，但它不能保证全局最优，这将影响粗对准过程中的对准精度和时间。

为进一步提高初始对准的性能，本发明针对现有的晃动基座粗对准方法存在的问题，借鉴系最优估计的思想，设计了一种新的特殊正交群最优估计算法。得益于特殊正交群理论的完善，通过提出一种新的姿态误差函数来衡量姿态误差，然后将姿态误差函数的梯度作为更新项来补偿姿态。基于这种方法构建的估计模型避免了传统最优化方法中的奇异值问题，并且姿态估计沿着梯度方向下降，从而快速达到收敛。该方法有效地提高了对准的速度，并且在精度方面也有所提升。仿真实验证明了该算法的可行性，可以作为四元数粗对准的上位替代进行晃动基座粗对准。

技术实现要素：

由于初始对准过程中，载体容易受到外界的各种干扰因素的影响，在对准过程中很难保持静态不动。因此，晃动基座下的粗对准算法具有很高的研究意义与应用价值。本发明的目的是为了应对现有晃动基座粗对准方法存在的问题：(1)本发明通过特殊正交群代替四元数描述初始姿态矩阵，避免了传统四元数描述方法的非唯一性和非线性问题；(2)本发明利用特殊正交群微分方程建立基于特殊正交群描述的线性粗对准模型；(3)本发明设计了一种新的姿态误差函数来衡量姿态的估计误差，然后将姿态误差函数的梯度作为更新项使得姿态沿着梯度方向快速收敛。

为了达到上述目的，本发明提供如下技术方案：

基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法，其特征在于，该方法通过下述步骤实现：

步骤(1)：sins捷联惯性导航系统进行系统预热准备，启动系统，获得载体所在位置的经度λ、纬度l、当地重力加速度在导航系下的投影gⁿ基本信息，采集惯性测量单元imu中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影和加速度计输出的载体系加速度信息f^b；

步骤(2)：对采集到的陀螺仪和加速度计的数据进行预处理，基于特殊正交群微分方程，建立基于特殊正交群描述的线性晃动基座粗对准系统模型：

本方法的详细描述中坐标系定义如下：

地球坐标系e系，选取地球中心为原点，x轴位于赤道平面内，从地心指向本初子午线，z轴从地心指向地理北极，x轴、y轴和z轴构成右手坐标系，随地球自转而转动；

地心惯性坐标系i系，选取地球中心为原点，x轴位于赤道平面内，从地心指向春分点，z轴从地心指向地理北极，x轴、y轴和z轴构成右手坐标系；

导航坐标系n系，表示载体所在位置的地理坐标系，选取舰载机重心为原点，x轴指向东向e，y轴指向北向n，z轴指向天向u；本方法中导航坐标系选取为地理坐标系；

载体坐标系b系，表示捷联惯性导航系统三轴正交坐标系，选取舰载机重心为原点，x轴、y轴、z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；

初始导航坐标系n(0)系，表示sins开机运行时刻的导航坐标系，并在整个对准过程中相对于惯性空间保持静止；

初始载体坐标系b(0)系，表示sins开机运行时刻的载体坐标系，并在整个对准过程中相对于惯性空间保持静止；

导航坐标系n'系，表示由特殊正交群最优估计算法计算得到的初始导航坐标系，本坐标系和真实导航坐标系n系之间存在转动关系；

基于特殊正交群微分方程，建立基于特殊正交群描述的线性粗对准系统模型：

根据sins捷联惯性导航系统原理，sins晃动基座粗对准问题转化为姿态估计问题，姿态变换为两个坐标系之间的旋转变换，导航的姿态矩阵可以用一个3×3的正交变换矩阵表示；该正交变换矩阵符合特殊正交群的特殊正交群so(n)的性质，构成了三维旋转群so(3)：

其中，r∈so(3)表示特定的导航姿态矩阵，表示3×3的向量空间，上标t表示矩阵的转置，i表示三维单位矩阵，det(r)表示为矩阵r的行列式；

晃动基座粗对准姿态估计问题转化为对基于特殊正交群描述的姿态矩阵r的求解问题；根据基于特殊正交群描述的姿态矩阵的链式法则，将导航姿态矩阵分解为三个矩阵的乘积形式：

其中，t表示时间变量，表示当前导航系相对于当前载体系的姿态矩阵，表示当前导航系相对于初始导航系的姿态矩阵，初始姿态矩阵表示初始导航系相对于初始载体系的姿态矩阵，表示初始载体系相对于当前载体系的姿态矩阵；

根据特殊正交群微分方程，姿态矩阵和随时间变化更新过程为：

其中，表示初始载体系相对于当前载体系的姿态矩阵，表示导航系相对于惯性系的旋转角速率在导航系的投影，在晃动基座条件下其等于地球自转角速率l表示当地纬度，表示陀螺仪输出的载体系相对于惯性系的旋转角速率在载体系的投影，符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算，运算规则如下：

符号(·×)的逆运算符(·)^v表示从反对称矩阵到三维向量的操作：s^∨＝ω。

由公式(2)-(5)可以看出，和可由imu传感器数据实时计算得到，而表示初始时刻的姿态矩阵，其不随时间变化；因此，sins晃动基座粗对准过程中姿态矩阵的求解问题，转化为对基于特殊正交群描述的初始姿态矩阵的求解问题；

在晃动基座条件下，加速度计测量信息f^b表示为：

f^b＝-g^b(6)其中，g^b表示当地重力加速度在载体系下的投影

根据sins惯性导航基本原理和特殊正交群链式法则，gⁿ和g^b之间存在如下关系：

对公式(7)进行移项、整理操作，可得：

公式(8)简化表示为：

其中，表示重力加速度在初始导航系下的投影，表示重力加速度在初始载体系下的投影；

对公式(9)在[0,t]上做积分，可得：

其中，表示重力加速度对应的速度矢量，表示重力加速度对应的速度矢量；

公式(10)可以简化为：

y(t)＝rx(t)(11)

其中

由于将求解姿态矩阵的问题转化为求解初始姿态矩阵的问题，且为符合特殊正交群特性的常值矩阵，因此建立晃动基座粗对准系统的线性状态方程如下：

根据上述内容，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立具有特殊正交群结构的晃动基座粗对准系统方程，表示为：

步骤(3)：根据特殊正交群最优估计算法，直接对基于特殊正交群描述的初始姿态矩阵进行最优估计：

对于公式(10)中的测量方程，r可以将n坐标系中的向量x(t)转换为b坐标系中的向量y(t)。仅考虑向量方向的投影，式(10)可表示为:

r(t)＝ru(t)(17)其中，因此r和u都是单位向量。

向量r(t)的预测值可以下式获得：

其中，是r的预测值。

为了更好地描述姿态估计的误差，设计一种新的姿态误差函数如下：

将公式(17)和(18)带入公式(19)得：

的无穷小变化量如下：

其中，

从的定义可知,在处的切映射如下:

其中，γ(∈)＝exp(∈s)r是在r周围的一条曲线。

将公式(21)带入公式(22)可得：

对于任意的有下式成立：

其中，是反对称投影算子，是对称投影算子。

将公式(24)带入公式(23)得：

一个任意对称矩阵和一个反对称矩阵的乘积的迹等于零，因此

其中，

在s上的内积<·,·>定义如下：

<a,b>＝tr(a^tb)(27)

其中，a,b∈s.

在s上黎曼度量<·,·>r定义如下：

将公式(27)带入公式(26)得：

将公式(28)带入公式(29)得：

的切映射和的梯度有以下的关系:

其中，是的梯度。

由于公式(29)和(31)左边是相等的，所有右边也是相等的，因此：

我们选择姿态误差函数的梯度作为新的创新项补偿姿态特殊正交群,这使得姿态估计沿梯度下降的方向传播,并最终使姿态特殊正交群可以快速收敛。因此，基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准算法归纳为：

步骤(4)：求解导航系统所需的姿态矩阵从而完成晃动基座粗对准过程：

根据之前步骤中求解得到的姿态变化矩阵和信息，通过公式(2)即可求解导航姿态矩阵，完成sins捷联惯性导航系统晃动基座粗对准。

与现有技术相比，本发明具有如下优点和有益效果：

(1)本发明采用特殊正交群描述初始姿态矩阵能够有效避免传统四元数描述初始姿态矩阵产生的非唯一性和非线性问题。

(2)本发明利用特殊正交群微分方程建立基于特殊正交群描述的线性粗对准模型。

(3)本发明设计了一种新的姿态误差函数来衡量姿态的估计误差，然后将姿态误差函数的梯度作为更新项使得姿态沿着梯度方向快速收敛。相较于传统的粗对准方法，不仅能够大幅度缩短对准时间，还能够提高对准的精度。

附图说明

图1捷联惯性导航系统装置总体简图。

图2捷联惯性导航系统流程图。

图3晃动基座粗对准仿真结果图。

图4晃动基座粗对准实验结果图。

图5上位机采集真实导航数据页面示意图。

具体实施方式

本发明是基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法设计，下面结合本发明系统流程图对本发明的具体实施步骤进行详细的描述：

本发明提供的基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法，首先获取传感器实时数据；对采集到的数据进行处理，基于特殊正交群微分方程，建立基于特殊正交群描述的线性粗对准系统模型；使用特殊正交群最优估计算法，估计得到基于特殊正交群描述的初始姿态矩阵并求解姿态矩阵在粗对准期间，经过多次估计解算，最终得到精确的初始姿态矩阵和姿态矩阵完成粗对准过程。

步骤2：对采集到的陀螺仪和加速度计的数据进行处理，基于特殊正交群微分方程，建立基于特殊正交群描述的线性晃动基座粗对准系统模型，

步骤(2.1)：通过更新计算

基于特殊正交群描述的姿态矩阵链式法则，姿态矩阵可以分解为：

由于表示导航系相对于惯性系的旋转角速率，且变化通常是十分缓慢的，根据基于特殊正交群描述的姿态矩阵更新求解过程，tk-1时刻到tk时刻的姿态矩阵可以近似为：

其中，

根据公式(25)-(27)，姿态矩阵迭代过程可以近似为：

步骤(2.2)：通过陀螺仪输出的角速率信息更新计算

tk-1时刻到tk时刻的姿态矩阵可以近似为：

其中，根据双子样旋转矢量法，可得：

其中，δθ1和δθ2分别表示两个相邻半采样周期内由陀螺仪输出计算所得的角增量；

根据公式(29)-(30)，姿态矩阵迭代过程可以近似为：

步骤(2.3)：建立基于特殊正交群描述的关于初始旋转矩阵的粗对准系统模型方式为：

在晃动基座条件下，gⁿ和g^b之间存在如下关系：

对公式(32)进行移项、整理操作，可得：

公式(34)可以表示为：

其中，表示重力加速度在初始导航系下的投影，表示重力加速度在初始载体系下的投影；

对公式(35)在[0,t]上做积分，可得量测方程如下：

公式(36)可以表示成如下形式：

y(t)＝rx(t)(45)

其中，

由于姿态矩阵为常值矩阵，因此建立晃动基座粗对准系统的线性状态方程如下：

根据上述内容，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立具有特殊正交群结构的晃动基座粗对准系统方程，表示为：

3、根据权利要求1所述的基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法，其特征在于，步骤(3)中使用特殊正交群最优估计算法直接估计

特殊正交群最优估计算法为了更好地表示姿态误差，建立了新的姿态误差函数。为了使姿态误差的变化反映在姿态补偿中，采用新的姿态误差函数的梯度作为创新项，对姿态进行实时补偿，使系统快速收敛。晃动基座粗对准整体过程如下：

4、根据权利要求1所述的基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法，其特征在于，步骤(4)中求解姿态矩阵并解算姿态信息；

在步骤(2)中，将sins粗对准过程中的求解问题转化为对的求解问题，并将分解为三个矩阵乘积的形式：

根据公式(28)、公式(31)和公式(47)求解得到的和姿态矩阵的求解方式为：

并根据求解得到的姿态矩阵解算姿态信息。

本发明的有益效果如下：

(1)在以下的仿真条件下，对该方法进行仿真实验：

步骤(1)中，晃动基座条件下模拟载体收到风浪影响，其航向角ψ、俯仰角θ、横滚角γ作周期变化，姿态变化情况如下：

步骤(1)中，初始地理位置：东经118°，北纬40°；

步骤(1)中，传感器输出频率为100hz；

步骤(1)中，陀螺仪漂移：三个方向轴上的陀螺常值漂移为0.1°/h，随机漂移为0.01°/h；

步骤(1)中，加速度计零位偏置：三个方向轴上的加速度计常值偏置为1mg，随机偏置为0.1mg；

步骤(2)中，地球自转角速率7.2921158e^-5rad/s；

步骤(2)中，时间间隔t为0.02s；

步骤(3)中，特殊正交群最优估计算法初始值

方法仿真结果如下：

进行了200s仿真，以姿态角的估计误差作为衡量指标，仿真结果如图(3)所示。从图中可以看出，俯仰姿态在36s左右完成对准，收敛到0.40′；横滚姿态在32s左右完成对准，收敛到0.48′；航向姿态在40s左右完成对准，收敛到4.23′。由仿真结果可知，本方法可以快速有效的完成晃动基座下的粗对准任务。

(2)通过真实实验对本发明提出的基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法进行验证。真实试验中，不提任何供外界辅助信息，实验历时200s。上位导航计算机控制导航系统，以100hz的数据更新速率，115200bps的波特率，采集航向精度达0.1度、姿态精度达0.05度的实际三轴姿态信息，解算获得的载体姿态信息与本步骤中得到的高精度真实载体姿态信息做比较，证明本方法和系统的可行性和有效性。

系统实验结果如下：

进行了200s的真实实验，以姿态角的估计误差作为衡量对准精度的指标，结果如图(4)所示。从图中可以看出，俯仰姿态在60s左右完成对准，收敛到4.02′；横滚姿态在64s左右完成对准，收敛到3.28′；航向姿态在80s左右完成对准，收敛到24.93′。本方法可以快速有效的完成晃动基座下的粗对准任务。

本发明将晃动基座粗对准问题转化为初始旋转矩阵的最优估计问题，采用基于特殊正交群方法描述初始姿态矩阵，能够有效避免传统基于四元数方法描述初始姿态矩阵产生的非唯一性和和非线性问题；本发明利用特殊正交群微分方程建立线性粗对准系统模型；本发明设计了一种新的姿态误差函数来衡量姿态的估计误差，然后将姿态误差函数的梯度作为更新项使得姿态沿着梯度方向快速收敛。相较于传统的粗对准方法，不仅能够大幅度缩短对准时间，还能够提高对准的精度。

以上所述仅是本发明的优选实施方式，并不用于限制本发明。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。