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

权利要求书

1.一种航向平滑方法，其特征在于，所述方法包括：

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

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

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

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

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

ψ_k＝ψ_k-1+ω_2，kT+(ψ_ref-ψ_k-1)K₂T；

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

2.根据权利要求1所述的一种航向平滑方法，其特征在于，通过下式获取当前时刻角加速度ε_k：

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

3.根据权利要求2所述的一种航向平滑方法，其特征在于，通过下式获取平滑后的角加速度