一种基于Hermite插值的旋转矢量获取方法

摘要

本发明提供一种基于Hermite插值的旋转矢量获取方法，引入高精度Hermite插值积分公式，对陀螺仪不同时刻的角速率进行数值积分，完成二子样角增量的提取，然后充分利用陀螺仪不同时刻的角速率，并引入前2次周期内的二子样角增量，得到基于Hermite插值的校正旋转矢量；这种二子样角增量和校正旋转矢量的获取方法，算法漂移低，解算精度高，适用于高动态环境下，高旋体姿态旋转矢量解算。

说明书

技术领域

本发明属于高动态环境下的姿态测量领域，尤其涉及一种基于Hermite插值的旋转矢量获取方法。

背景技术

姿态测量是类似于制导炮弹这样的高旋体实现精确打击目标的一项关键技术，姿态测量的精度直接影响到炮弹制导的精度。因此，姿态解算算法的分析和姿态测量系统的设计便成为研究高旋体(制导炮弹)导航制导的重要环节。目前，常用的姿态更新算法主要有欧拉角法、四元数法、方向余弦法和旋转矢量法。针对具有高过载和高转速特点的高旋体，在弹丸飞行过程中，弹丸的空间角位置与旋转次序有关，在空间的有限转动具有不可交换性，通常采用等效旋转矢量优化算法对不可交换性误差进行补偿。

在实际高旋体(制导炮弹)制导控制系统中，采用抗高过载角速率陀螺仪，其输出为角速率信息。针对实际高旋体的角增量提取，如何降低算法复杂度、提高算法精度以及保证算法实时性是迫切需要解决的问题。

角增量提取问题的核心就是一种高精度数值积分的构造。数值积分常见的有梯形公式、Simpson公式以及Lagrange插值积分公式，它们的计算虽无需提供导数值，但精度不高。

发明内容

为解决上述问题，本发明提供一种基于Hermite插值的旋转矢量获取方法，能够降低算法漂移，提高旋转矢量的解算精度。

一种基于Hermite插值的旋转矢量获取方法，包括以下步骤：

S1：基于Hermite插值公式，获取陀螺仪的二子样角增量，具体的：

其中，Δθ₁为第一时刻～第三时刻对应的二子样角增量，Δθ₂为第三时刻～第五时刻对应的二子样角增量，Δθ为第一时刻～第五时刻对应的二子样角增量，ω₁～ω₅分别为第一时刻～第五时刻的陀螺仪角速率，h为旋转矢量更新周期，其中，每五个时刻作为一个更新周期；

S2：根据所述Δθ₁、Δθ₂以及Δθ，获取陀螺仪当前更新周期的旋转矢量Φ^*：

其中，Δθ′为前1次更新周期获取的第一时刻～第五时刻对应的二子样角增量，Δθ″为再前1次更新周期获取的第一时刻～第五时刻对应的二子样角增量。

有益效果：

本发明提供一种基于Hermite插值的旋转矢量获取方法，引入高精度Hermite插值积分公式，对陀螺仪不同时刻的角速率进行数值积分，完成二子样角增量的提取，然后充分利用陀螺仪不同时刻的角速率，并引入前2次周期内的二子样角增量，得到基于Hermite插值的校正旋转矢量；这种二子样角增量和校正旋转矢量的获取方法，算法漂移低，解算精度高，适用于高动态环境下，高旋体姿态旋转矢量解算。

附图说明

图1为本发明提供的一种基于Hermite插值的旋转矢量获取方法的流程图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述。

参见图1，该图为本实施例提供的一种基于Hermite插值的旋转矢量获取方法的流程图。一种基于Hermite插值的旋转矢量获取方法，包括以下步骤：

S1：基于Hermite插值公式，获取陀螺仪的二子样角增量的表达式：

其中，Δθ₁为第一时刻～第三时刻对应的二子样角增量，Δθ₂为第三时刻～第五时刻对应的二子样角增量，Δθ为第一时刻～第五时刻对应的二子样角增量，ω₁～ω₅分别为第一时刻～第五时刻的陀螺仪角速率，h为旋转矢量更新周期，其中，每五个时刻作为一个更新周期；

S2：根据所述Δθ₁、Δθ₂以及Δθ，获取陀螺仪当前更新周期的旋转矢量Φ^*：

其中，Δθ′为前1次更新周期获取的第一时刻～第五时刻对应的二子样角增量，Δθ″为再前1次更新周期获取的第一时刻～第五时刻对应的二子样角增量。

下面对本实施例的二子样角增量和旋转矢量Φ^*具有高解算精度进行论证。

在锥动环境下，将对应的时间节点以及陀螺仪角速率代入Hermite插值积分公式，可得如下结果：

需要说明的是，锥动是刚体运动的一种几何现象，当刚体受到环境振动或本身具有角运动，即刚体在两个正交轴方向存在频率相同而相位不同的角振动速率时，将导致刚体的第三个正交轴在空间绕其平均位置做锥面或近似锥面的运动。本实施例中所述的刚体，为装载有陀螺仪的制导炮弹。

在锥动环境下，模拟陀螺仪角速率ω_k为：

式中，a为半锥角，ω为锥动角速率，t_k为不同时刻，k＝1,2,3,4,5。

采用直流误差Φ_ε作为公式(1)的精度评估准则，则有：

Φ_ε＝|2q₁-Φ_x|>

式中，q₁为姿态旋转矢量q的第二项，大小为

并将公式(1)代入旋转矢量优化二子样算法，即公式(4)

其中，Φ为矢量，Φ_x为Φ的第一项，可得直流误差Φ_ε1：

将直流误差Φ_ε1对h进行求导，则得到直流误差Φ_ε1的算法漂移为：

下面对公式(1)进行优化，使得获取的算法漂移更小。由于Hermite插值所得的角增量提取公式中，陀螺仪输出角速率ω_k前面的系数存在一定规律性，从而可设基于Hermite插值一般的二子样角增量形式为：

式中，k₁和k₂为待定优化系数。

将公式(7)联立旋转矢量二子样算法，即公式(4)与精度评估准则，即公式(3)，可得直流误差Φ_ε2：

对公式(8)中的三角函数作泰勒级数展开，并作同类项合并，则有：

由于ωh＜＜1，所以为了尽量减小直流误差Φ_ε2，应该确保ωh的低幂次项为零，为此取：

解得：

即得优化Hermite插值二子样角增量的表达式：

将公式(11)代入公式(9)得到直流误差Φ_ε2，进而可得算法漂移为：

由此可见，优化Hermite插值二子样算法的算法漂移比Hermite插值二子样算法的算法漂移小了两个量级，也就是本发明提供的公式(12)计算陀螺仪的二子样角增量，精度有两个数量级的提升，从而为后续高精度的旋转矢量解算奠定了良好的基础。

下面介绍旋转矢量的计算方法。为了充分利用陀螺仪输出信息，引入前2次旋转矢量更新周期内的二子样角增量，得到校正旋转矢量Φ^*为：

Φ^*＝Δθ+x₁Δθ₁×Δθ₂+x₂Δθ′×Δθ+x₃Δθ″×Δθ>

式中，x₁、x₂和x₃为待定优化系数。

将优化Hermite插值二子样角增量的公式(12)代入校正旋转矢量的公式(14)，并联立精度评估准则公式(3)，可得：

其中，为校正旋转矢量Φ^*的第一项，对公式(15)中的三角函数作泰勒级数展开，并作同类项合并：

由于ωh＜＜1，所以为了尽量减小直流误差Φ_ε3，应该确保ωh的低幂次项为零，为此取：

解得：

即可得到校正旋转矢量Φ^*：

根据精度评估准则，即公式(3)，可得算法漂移为：

采用同样的插值公式，校正旋转矢量的算法漂移比传统的旋转矢量法的算法漂移小，精度有两个数量级的提升，由此可见，本实施例的旋转矢量获取方法能够有效地提高旋转矢量的解算精度。

本实施例的一种基于Hermite插值的旋转矢量获取方法，引入高精度Hermite插值积分，实现对二子样角增量的提取，进一步根据算法精度评估准则，完成Hermite插值积分公式，即公式(7)系数的优化，以及校正旋转矢量二子样算法，即公式(14)系数的优化。本实施例不仅适应于高旋体高动态环境，而且在同样精度条件下，降低了对导航计算机的要求，还充分利用了陀螺仪的角速率，有效地降低了算法漂移，提高了姿态解算精度。

当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当然可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。