一种基于角增量和比力输出的划桨速度计算方法

摘要

本发明公开了一种基于角增量和比力输出的划桨速度计算方法，包括：(1)在速度算法单次更新周期(t_m-1，t_m)内，采用N个间隔，分别对角增量陀螺仪和比力加速度计的输出采样，得到角增量输出为Δθ_i，i＝1，2，...，N；比力输出为f_j，j＝1，2，...N+1；(2)计算单次算法更新周期(t_m-1，t_m)内机体的角增量Δθ_m和速度增量△V_m；(3)计算旋转效应补偿值ΔV_rotm；(4)计算划桨效应补偿值ΔV_sculm；(5)计算(t_m-1，t_m)周期内比力积分的补偿值ΔV_sfm；(6)根据速度更新方程计算t_m时刻的机体速度V_m。本发明可根据角增量陀螺仪和比力角速度计的输出直接高精度的计算出机体的速度，具有方法简单、精度高的优势。

主权项

一种基于角增量和比力输出的划桨速度计算方法，其特征在于，包括以下步骤：(1)在速度算法单次更新周期(t_m‑1，t_m)内，采用N个间隔，角增量陀螺仪输出N次，比力加速度计输出N+1次，所述的N为≥2的自然数，分别对角增量陀螺仪和比力加速度计输出采样，得到角增量输出为Δθ_i，i＝1，2，...，N；比力输出为f_j，j＝1，2，...N+1；(2)计算单次算法更新周期(t_m‑1，t_m)内机体的角增量Δθ_m和速度增量ΔV_m：Δθ_m＝Δθ₁+Δθ₂+...+Δθ_N，所述的Δθ_m是(t_m‑1，t_m)周期内角增量的输出Δθ₁、Δθ₁...Δθ_N之和，ΔV_m根据比力加速度计的输出采样f_j经过数字积分公式计算得到；(3)计算旋转效应补偿值Δv_rotm：${\mathrm{\Delta V}}_{\mathrm{rotm}}=\frac{1}{2}{\mathrm{\Delta \theta }}_m\times {\mathrm{\Delta V}}_m;$(4)计算划桨效应补偿值ΔV_sculm：所述的l＝2，...N，k_l‑1为划桨优化算法系数，H为速度算法更新周期，H＝t_m‑t_m‑1；(5)计算(t_m‑1，t_m)周期内比力积分的补偿值ΔV_sfm：ΔV_sfm＝ΔV_m+ΔV_rotm+ΔV_sculm；(6)根据速度更新方程计算t_m时刻的机体速度V_m：$\begin{array}{c}{V}_m=V_{m-1}+C_{m-1}{\mathrm{\Delta V}}_{\mathrm{sfm}}+{\mathrm{\Delta V}}_{g/\mathrm{corn}}\\ =V_{m-1}+C_{m-1}[{\mathrm{\Delta V}}_m+{\Delta \hat{V}}_{\mathrm{rotm}}+\Delta {\hat{V}}_{\mathrm{sculm}}]+{\mathrm{\Delta V}}_{g/\mathrm{corn}}\\ =V_{m-1}+C_{m-1}[{\mathrm{\Delta V}}_m+\frac{1}{2}{\mathrm{\Delta \theta }}_m\times {\mathrm{\Delta V}}_m+\underset{l=2}{\overset{N}{\Sigma }}{k}_{l-1}({\mathrm{\Delta \theta }}_1\times f_l-{\mathrm{\Delta \theta }}_l\times f_1)H]+[g-({2\omega }_{\mathrm{ie}}+{\omega }_{\mathrm{en}})\times V_{m-1}]H\end{array}$所述的，V_m‑1为t_m‑1时刻机体速度，C_m‑1为t_m‑1时刻方向余弦阵，g为重力加速度，ω_ie为地理系对惯性系的角速率，ω_en为导航系对地理系的角速率。