[发明专利]一种航向平滑方法

说明书

本发明提供了一种航向平滑方法，所述方法包括：实时获取陀螺仪输出角速率；获取当前时刻角加速度ε_k；对n个当前时刻角加速度ε_k进行平滑处理获取平滑后的角加速度基于待平滑的航向ψ_ref、上一时刻平滑后的航向ψ_k‑1、陀螺仪输出角速率的输出周期T、平滑后的角加速度上一时刻平滑后的航向角速率ω_2，k‑1、当前时刻陀螺仪输出角速率ω_1，k获取当前时刻平滑后的航向角速率ω_2，k；基于待平滑的航向ψ_ref、上一时刻平滑后的航向ψ_k‑1、陀螺仪输出角速率的输出周期T、当前时刻平滑后的航向角速率ω_2，k获取当前时刻平滑后的航向ψ_k。本发明的技术方案对航向进行平滑，平滑后的航向较好地跟踪了真实航向变化，且测量噪声大幅降低，在受干扰时段仍然保持了良好的航向精度；还使航向保持了优良的动态性能。

技术领域

本发明涉及组合导航技术领域，尤其涉及一种航向平滑方法。

背景技术

地磁传感器以其无累积误差的优点，成为低成本、低精度微机电惯导系统航向通道常用的辅助手段。然而，地磁传感器容易受到周围动态电磁环境干扰，且输出噪声大的缺点又制约了其应用领域。

在通常系统设计中多采用滑动平均算法对磁航向进行处理，但是该算法平滑效果与平滑窗口选择密切相关，过小的平滑窗口效果有限，过大的平滑窗口会引入时间延迟，影响控制的实时性，且滑动平均结果的动态性能严重降低，因此，亟需一种保留动态性能的航向平滑方法。

发明内容

本发明提供了一种航向平滑方法，以解决现有的航向平滑方法无法保留动态性能的技术问题。

为解决上述技术问题，本发明提出如下技术方案：

一种航向平滑方法，所述方法包括：

实时获取陀螺仪输出角速率；

基于当前时刻陀螺仪输出角速率ω_1，k和上一时刻陀螺仪输出角速率ω_1，k-1获取当前时刻角加速度ε_k；

对n个当前时刻角加速度ε_k进行平滑处理获取平滑后的角加速度其中，n为平滑窗口；

基于待平滑的航向ψ_ref、上一时刻平滑后的航向ψ_k-1、陀螺仪输出角速率的输出周期T、平滑后的角加速度上一时刻平滑后的航向角速率ω_2，k-1、当前时刻陀螺仪输出角速率ω_1，k获取当前时刻平滑后的航向角速率ω_2，k，其中，在初始时刻，将陀螺仪输出角速率作为平滑后的角速率，将待平滑的航向作为平滑后的航向；

基于待平滑的航向ψ_ref、上一时刻平滑后的航向ψ_k-1、陀螺仪输出角速率的输出周期T、当前时刻平滑后的航向角速率ω_2，k获取当前时刻平滑后的航向ψ_k。

优选的，通过下式获取当前时刻角加速度ε_k：

ε_k＝ω_1，k-ω_1，k-1。

优选的，通过下式获取平滑后后的角加速度

优选的，通过下式获取当前时刻平滑后的航向角速率ω_2，k：

式中，K₁为第一平滑系数，α为滤波器系数。