[发明专利]基于角速率输入的高阶增强姿态方法

权利要求书

1.一种基于角速率输入的高阶增强姿态方法，其特征在于，该方法包括如下步骤：

(1)在一个姿态更新周期H内，对陀螺仪的输出采样N次得角速率信号ω₁、ω₂、...、ω_N，并利用上一姿态更新周期内的角速率信号ω_-N、...、ω_-1、ω₀；

(2)对步骤(1)得到的2N+1个角速率信号进行拉格朗日插值，并在当前姿态更新周期内对该拉格朗日插值多项式积分，得到陀螺仪角增量的估计值

(3)利用前一姿态更新周期和当前姿态更新周期内的陀螺仪角速率信号，通过时间间隔长度不同的角速率矢量二叉积的线性组合来估计当前姿态更新周期内的二阶旋转矢量项，并利用泰勒展开法优化二叉积的系数；

(4)利用前一姿态更新周期和当前姿态更新周期内的陀螺仪角速率信号，通过时间间隔长度不同的角速率矢量三叉积的线性组合来估计当前姿态更新周期内的三阶旋转矢量项，并利用泰勒展开法优化三叉积的系数；

(5)将步骤(2)、(3)和(4)得到的估计值相加以作为一个姿态更新周期H内的旋转矢量估计值由旋转矢量估计值得到一个姿态更新周期H内的姿态变化四元数q(H)；

(6)用姿态变化四元数q(H)求得一个姿态更新周期H内的姿态四元数Q(t_m)；

步骤(2)的具体方法是：

先利用前一姿态更新周期和当前姿态更新周期内的全部角速率信息来构造拉格朗日插值多项式L(t)：

式中，t_m-2≤t≤t_m，t_m-2是前一姿态更新周期的起始时刻，t_m-1是前一姿态更新周期的结束时刻，t_m是当前姿态更新周期的结束时刻，m表示捷联惯性导航系统开始工作后的第m次姿态解算；N为一个姿态更新周期H内的陀螺采样次数，T是陀螺采样周期，当陀螺的型号固定时，T是固定值；l_k(t)为插值基函数，ω_k是t_m-1+kT时刻的角速率矢量，k和i分别表示t_m-1+kT时刻和t_m-1+iT时刻距t_m-1时刻的采样间隔数；k＝-N,...,0,...,N；i＝-N,...,0,...,N；

再求拉格朗日插值多项式L(t)在一个姿态更新周期H内的积分，即可得到θ(H)的估计值

其中，θ(H)是一个姿态更新周期内角增量的理论值，是角增量的估计值，H＝t_m-t_m-1；

步骤(3)的具体方法是：利用前一姿态更新周期和当前姿态更新周期内的全部角速率信息，使时间间隔长度不同的角速率矢量二叉积的线性组合的z轴分量逼近而二阶旋转矢量项δΦ_c(H)的z轴分量，忽略x轴和y轴分量的影响，从而得到二阶旋转矢量项的估计值

其中，δΦ_c(H)是一个姿态更新周期内二阶旋转矢量项的理论值，是二阶旋转矢量项的估计值，ω_i，i＝-N,...,0,...,N-1是t_m-1+iT时刻的角速率矢量，ω_N是t_m时刻的角速率矢量，K_N-i，N-i＝1,...,2N为二阶旋转矢量项估计值的待优化系数，在典型圆锥运动环境下对二阶旋转矢量项估计值的系数K_N-i进行优化，使得算法漂移误差最小；

步骤(4)的具体方法是：利用前一姿态更新周期和当前姿态更新周期内的全部角速率信息，使时间间隔长度不同的角速率矢量三叉积的线性组合的z轴分量逼近三阶旋转矢量项δΦ_A(H)+δΦ_B(H)的z轴分量，忽略x轴和y轴分量的影响，从而获得三阶旋转矢量项的估计值

其中，δΦ_A(H)+δΦ_B(H)是一个姿态更新周期内三阶旋转矢量项的理论值，是三阶旋转矢量项的估计值，G_g，g＝1,...,2N-1为三阶旋转矢量项估计值待优化系数；ω_i、ω_j、ω_k分别为[t_m-2,t_m]时间段内t_i、t_j、t_k时刻的角速率矢量且中的ω_i×(ω_j×ω_k)的z轴分量需要满足线性无关条件，在典型圆锥运动环境下对三阶旋转矢量项估计值的系数G_g进行优化，使得算法漂移误差最小。