[发明专利]一种基于角速率和比力输入的插值三子样划桨效应误差补偿方法

权利要求书

1.一种基于角速率和比力输入的插值三子样划桨效应误差补偿方法，其特征在于：包括以下步骤：

步骤一：分析基于输入为角速率和比力的划桨效应误差补偿方法；

步骤二：分析基于输入为角速率和比力的划桨效应误差补偿优化方法；

步骤三：设计输入为角速率和比力的插值三子样划桨效应误差补偿方法；

所述步骤一具体为：取地理坐标系为导航坐标系，惯导系统的速度误差方程为

式中，为捷联矩阵，f^b为比力在载体系的投影，V为速度矢量，g为重力加速度矢量，为地球旋转角速率在导航坐标系的投影，为位置速率在导航坐标系的投影；

设速度的更新周期为T，在[t_k-1,t_k]这一段时间内，对惯导系统的速度误差方程进行积分运算，得t_k时刻运载体在导航坐标系的速度简化为：

式中，V_k和V_k-1分别为载体在t_k和t_k-1时刻的速度，C_k-1为t_k-1时刻的捷联矩阵，Δθ为角增量；

记

ΔV_g/cork是由有害加速度引起的速度补偿量，ΔV_sfk是由比力引起的速度补偿量，下面对ΔV_sfk做详细分析；

上式中，是加速度计在[t_k-1,t_k]时间段内输出的速度增量；记

由于

所以

将Δθ(t)×f(t)代入ΔV_sfk，有

上式右端第三项记作

此项称为速度的划桨效应误差补偿项；

在[t_k-1,t_k]这一段时间内，用三次抛物线公式分别对载体的角速率和比力进行拟合，可得

ω(t_k-1+τ)＝a+2bτ+3cτ²+4dτ³,0≤τ≤Δt

f(t_k-1+τ)＝A+2Bτ+3Cτ²+4Dτ³,0≤τ≤Δt

于是，角增量和速度增量可表示为：

Δθ(τ)＝aτ+bτ²+cτ³+dτ⁴

Δν(τ)＝Aτ+Bτ²+Cτ³+Dτ⁴

角速率和比力采样时刻选为且所采角速率和比力分别记为ω₁、ω₂、ω₃和f₁、f₂、f₃，并记t_k时刻角速率和比力分别为ω₀和f₀，将其分别带入角速率ω(t_k-1+τ)和比力f(t_k-1+τ)中，可得

对上式求解，可得

所以，划桨效应误差补偿项的三子样方法为：

所述步骤二具体为：假设载体做划桨运动，角速率和比力分别为

ω(t)＝iBΩcosΩt

f(t)＝jCsinΩt

其中，i和j为沿载体坐标系相应轴的单位向量；

记ω_k和f_k+l是速度更新周期[t_k-1,t_k]内的第k，k＝0,…,N-1个角速率采样值和第k+l，l≥1,k+l≤N个比力采样值，其中，N为周期内采样个数，令Δt表示t_k-t_k-1，则有：

于是，有

其中，k为沿与i和j轴正交的单位向量；

另外一种情况：记f_k和ω_k+l是速度更新周期[t_k-1,t_k]内的第k，k＝0,…,N-1个比力采样值和第k+l，l≥1,k+l≤N个角速率采样值，则有：

于是，有

ω_k×f_k+1与f_k×ω_k+1相加，有

可以看出，当载体做ω(t)＝iBΩcosΩt、f(t)＝jCsinΩt所示形式的划桨运动时，ω_k×f_k+l+f_k×ω_k+l的值与k无关，仅与l有关；此时，有

ω₀×f₁+f₀×ω₁＝ω₁×f₂+f₁×ω₂＝ω₂×f₃+f₂×ω₃

ω₀×f₂+f₀×ω₂＝ω₁×f₃+f₁×ω₃

基于上面两个等式，将划桨效应补偿项的三子样方法表示为如下形式：

将ω_k×f_k+l+f_k×ω_k+l带入有

根据载体做划桨运动的角速率公式和比力公式，有：

所以，精确计算的划桨补偿项应为：

于是，三子样方法所引起的误差可表示为：

由于运载体振动的频率Ω不会太高，采样时间间隔Δt为ms级，所以一般情况下有ΩΔt＜1，为了尽量减小方法误差应确保ΩΔt的低次幂项尽量为0，为此取

对上式求解，可得k₁＝291/2240,k₂＝3/56,k₃＝29/6720，于是，划桨运动的三子样优化方法为：

所述步骤三具体为：假设在t_k、t_k+h/3、t_k+2h/3和t_k+h时刻采集到的角速率和比力分别为ω₀、ω₁、ω₂、ω₃和f₀、f₁、f₂、f₃，由式

ω(t_k-1+τ)＝a+2bτ+3cτ²+4dτ³,0≤τ≤Δt

f(t_k-1+τ)＝A+2Bτ+3Cτ²+4Dτ³,0≤τ≤Δt

可知，利用这四个角速率和比力可以得到ω(t_k+τ)和f(t_k+τ)的表达式；利用ω(t_k+τ)和f(t_k+τ)的表达式可以求出在t_k到t_k+h内任意时刻的角速率和比力的值；

此时可以在[t_k+2h/3,t_k+h]的时间段内分别插入两个角速率的值和比力的值和然后以ω₂、ω₃和f₂、f₃四个角速率和比力计算[t_k+2h/3,t_k+h]时间段内的划桨效应误差补偿项ΔV_sculk，最后根据t_k+2h/3时刻对应的速度更新t_k+h时刻的速度；

同理可求出[t_k+h,t_k+4h/3]时间段内的划桨效应误差补偿项ΔV_sculk，根据t_k+h时刻的速度更新t_k+4h/3时刻的速度，以此类推；

详细的过程如下：

由式

ω(t_k-1+τ)＝a+2bτ+3cτ²+4dτ³,0≤τ≤Δt

f(t_k-1+τ)＝A+2Bτ+3Cτ²+4Dτ³,0≤τ≤Δt

确定ω(t_k+τ)和f(t_k+τ)的表达式；

在τ＝2h/3时的输出角速率和输出比力分别为：

在τ＝7h/9时的插值角速率和插值比力分别为：

在τ＝8h/9时的插值角速率和插值比力分别为：

在τ＝h时的输出角速率和输出比力分别为：

通过式求得各项的叉乘为

将上面三个求得各项的叉乘等式代入下式

合并同类项后可以求出在[t_k+2h/3,t_k+1+h]内的划桨效应补偿项为：