初始姿态最优估计方法与流程

本发明涉及惯性导航技术领域，尤其涉及一种初始姿态最优估计方法。

背景技术：

惯导系统的初始对准是影响系统使用性能的关键技术之一，对准的精度与速度直接关系到惯性系统的精度与启动特性。根据惯导系统原理，若要完全消除误差需要进行多位置对准计算，但在绝大多数情况下，这个要求无法满足，一般只能在一个位置进行对准。因此，提高单位置初始对准精度具有重要应用价值。

当前常用的方法是基于重力矢量的对准方法，通过计算重力矢量在惯性空间内旋转的角度求取惯导系统的初始姿态，该方法具有估计精度高，鲁棒性强的特点。使用卡尔曼滤波、递推最小二乘等递推算法，可以实现姿态的实时最优估计。但传统的重力矢量方法在计算时对系统姿态进行了线性化假设，即表示姿态的四元数中的常数项q＝0，因此当转动角度为180°时会无法收敛。

技术实现要素：

本发明的目的在于克服现有技术不足，提供了一种初始姿态最优估计方法，能够解决现有技术中的问题。

本发明的技术解决方案：一种初始姿态最优估计方法，其中，该方法包括：

计算重力矢量在体坐标系下的测量值和重力矢量在参考坐标系下的测量值

基于测量值和测量值计算损失函数构造矩阵k；

计算损失函数构造矩阵k的特征值λi和特征向量qi；

根据特征值λi和特征向量qi计算扩展矩阵h及其伴随矩阵h^*；

基于伴随矩阵h^*得到初始姿态最优解。

优选地，通过下式基于测量值和测量值计算损失函数构造矩阵k：

s＝b+b^t，

其中，αi为权重系数，且共n个系数，对应n组重力矢量测量值。

优选地，通过下式计算损失函数构造矩阵k的特征值λi和特征向量qi：

其中，i＝1,2,3,4，λi和qi分别表示4个特征值和4个特征向量。

优选地，通过下式根据特征值λi和特征向量qi计算扩展矩阵h：

优选地，通过下式计算伴随矩阵h^*：

其中，λj,k,l为不同于λi的其他三个特征值。

优选地，基于伴随矩阵h^*得到初始姿态最优解包括：

令伴随矩阵h^*中λ＝λmax＝λ1，则伴随矩阵h^*中除了第一项外全部为0，得到下式，并通过下式得到初始姿态最优解：

其中，qopt为初始姿态最优解，λmax为损失函数构造矩阵k的所有特征值中最大的一个值。

通过上述技术方案，可以基于重力矢量在体坐标系下的测量值和重力矢量在参考坐标系下的测量值计算损失函数构造矩阵k，然后可以通过损失函数构造矩阵k的特征值λi和特征向量qi计算扩展矩阵h及其伴随矩阵h^*，进而可以根据伴随矩阵h^*得到初始姿态最优解。由此，能够有效提高单位置条件下的初始姿态估计精度。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种初始姿态最优估计方法的流程图。

具体实施方式

下面将结合附图对本发明的具体实施例进行详细说明。在下面的描述中，出于解释而非限制性的目的，阐述了具体细节，以帮助全面地理解本发明。然而，对本领域技术人员来说显而易见的是，也可以在脱离了这些具体细节的其它实施例中实践本发明。

在此需要说明的是，为了避免因不必要的细节而模糊了本发明，在附图中仅仅示出了与根据本发明的方案密切相关的设备结构和/或处理步骤，而省略了与本发明关系不大的其他细节。

图1为本发明实施例提供的一种初始姿态最优估计方法的流程图。

如图1所示，本发明实施例提供了一种初始姿态最优估计方法，其中，该方法包括：

s100，计算重力矢量在体坐标系下的测量值和重力矢量在参考坐标系下的测量值

s102，基于测量值和测量值计算损失函数构造矩阵k；

s104，计算损失函数构造矩阵k的特征值λi和特征向量qi；

s106，根据特征值λi和特征向量qi计算扩展矩阵h及其伴随矩阵h^*；

s108，基于伴随矩阵h^*得到初始姿态最优解。

通过上述技术方案，可以基于重力矢量在体坐标系下的测量值和重力矢量在参考坐标系下的测量值计算损失函数构造矩阵k，然后可以通过损失函数构造矩阵k的特征值λi和特征向量qi计算扩展矩阵h及其伴随矩阵h^*，进而可以根据伴随矩阵h^*得到初始姿态最优解。由此，能够有效提高单位置条件下的初始姿态估计精度。

根据本发明一种实施例，通过下式基于测量值和测量值计算损失函数构造矩阵k：

s＝b+b^t，(4)

其中，αi为权重系数，且共n个系数，对应n组重力矢量测量值。

每组重力矢量测量值包括重力矢量在体坐标系下的测量值和重力矢量在参考坐标系下的测量值。

上述的式(1)-(4)为中间变量，用于简化计算过程。也就是，根据(1)-(4)计算损失函数构造矩阵k。

根据本发明一种实施例，通过下式计算损失函数构造矩阵k的特征值λi和特征向量qi：

其中，i＝1,2,3,4，λi和qi分别表示4个特征值和4个特征向量。

在本发明中，可以根据实对称阵的特性，得到上述式(6)。

根据本发明一种实施例，通过下式根据特征值λi和特征向量qi计算扩展矩阵h：

根据本发明一种实施例，通过下式计算伴随矩阵h^*：

其中，λj,k,l为不同于λi的其他三个特征值。

根据本发明一种实施例，基于伴随矩阵h^*得到初始姿态最优解包括：

令伴随矩阵h^*中λ＝λmax＝λ1，则伴随矩阵h^*中除了第一项外全部为0，得到下式，并通过下式得到初始姿态最优解：

其中，qopt为初始姿态最优解，λmax为损失函数构造矩阵k的所有特征值中最大的一个值。

由此可知，h^*中的每一组列向量都是待求的(乘以了一个系数后的)最优四元数。从实际情况来看，求取应该使用范数最大的一列，这是因为如果qopt中某一项趋近于0，那么会使得h^*中乘以该项的一列由于计算机字长限制，使得解算出的四元数产生很大的量化误差。由于k是实对称阵，因此只要选择h^*对角线上的元素是最大值一列就可以得到初始姿态最优解。

下面对通过本发明上述的方法得到的初始姿态最优解进行验证。

姿态余弦矩阵a与姿态四元数字的转换关系为：

其中，θ为旋转角，为旋转轴的单位向量。

为实现姿态最优估计，定义如下损失函数：

则有：

g(a)＝1-l(a)＝tr[ab^t](13)

为实现损失函数最小，只需满足转换后的损失函数g(a)最大即可。

将式(10)带入式(13)可得：

四元数的元素具有唯一约束：

为求取式(14)在式(15)约束条件下对最大值，重新构建方程：

令可得：

由上式分析可知，λ是k的一个特征根，则是对应的特征向量，因此式(9)的结果是一种最优估计。

本领域技术人员应当理解，上述验证过程仅仅是一种验证本发明所述的方法的方式，并不作为本发明技术方案的一部分，也可以采用现有技术中其他的验证方式对本发明所述的方法进行验证。

如上针对一种实施例描述和/或示出的特征可以以相同或类似的方式在一个或更多个其它实施例中使用，和/或与其它实施例中的特征相结合或替代其它实施例中的特征使用。

应该强调，术语“包括/包含”在本文使用时指特征、整件、步骤或组件的存在，但并不排除一个或更多个其它特征、整件、步骤、组件或其组合的存在或附加。

本发明以上的方法可以由硬件实现，也可以由硬件结合软件实现。本发明涉及这样的计算机可读程序，当该程序被逻辑部件所执行时，能够使该逻辑部件实现上文所述的装置或构成部件，或使该逻辑部件实现上文所述的各种方法或步骤。本发明还涉及用于存储以上程序的存储介质，如硬盘、磁盘、光盘、dvd、flash存储器等。

这些实施例的许多特征和优点根据该详细描述是清楚的，因此所附权利要求旨在覆盖这些实施例的落入其真实精神和范围内的所有这些特征和优点。此外，由于本领域的技术人员容易想到很多修改和改变，因此不是要将本发明的实施例限于所例示和描述的精确结构和操作，而是可以涵盖落入其范围内的所有合适修改和等同物。

本发明未详细说明部分为本领域技术人员公知技术。