一种航向平滑方法与流程

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

背景技术：

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

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

技术实现要素：

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

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

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

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

基于当前时刻陀螺仪输出角速率ω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：

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

优选的，通过下式获取当前时刻平滑后的航向ψk：

ψk＝ψk-1+ω2，kt+(ψref-ψk-1)k2t，

式中，k2为第二平滑系数。

应用本发明的技术方案，在航向输出前对航向进行平滑，平滑后的航向较好地跟踪了真实航向变化，且测量噪声大幅降低，在受干扰时段仍然保持了良好的航向精度；通过引入航向角速率信息，使航向保持了优良的动态性能。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了根据本发明的实施例提供的航向平滑方法的流程示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

图1示出了根据本发明的实施例提供的航向平滑方法的流程示意图。

如图1所示，本发明实施例提供了一种航向平滑方法，所述方法包括：

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

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

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

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

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

本发明实施例中提供的航向平滑方法，通过在航向输出前对航向进行平滑，平滑后的航向较好地跟踪了真实航向变化，且测量噪声大幅降低，在受干扰时段仍然保持了良好的航向精度；通过引入航向角速率信息，使航向保持了优良的动态性能。

本发明一种实施例所述的s2中，通过下式获取当前时刻角加速度εk：

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

本发明一种实施例所述的s3中，通过下式获取平滑后后的角加速度

本发明一种实施例所述的s4中，通过下式获取当前时刻平滑后的航向角速率ω2，k：

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

本发明实施例中提供的航向平滑方法，通过引入航向角速率，使航向保持了优良的动态性能。

本发明一种实施例所述的s5中，通过下式获取当前时刻平滑后的航向ψk：

ψk＝ψk-1+ω2，kt+(ψref-ψk-1)k2t，

式中，k2为第二平滑系数。

本发明实施例中提供的航向平滑方法，通过在航向输出前对航向进行平滑，平滑后的航向较好地跟踪了真实航向变化，且测量噪声大幅降低，在受干扰时段仍然保持了良好的航向精度。

本发明一种实施例所述的航向可以为地磁航向。

采用本发明所述的航向平滑方法对地磁航向进行平滑，得到地磁航向平滑效果仿真结果。基于仿真结果可知，平滑后的地磁航向曲线较好的跟踪了真实航向变化，且测量噪声大幅降低，由原来的20°降低至小于5°。由此可见，在地磁航向受干扰而导致测量噪声变大时仍然保持了良好的航向精度。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。