[发明专利]一种约束惯性/卫星组合导航静止时航向漂移的方法

说明书

本发明公开了一种约束惯性/卫星组合导航静止时航向漂移的方法，属于多信息融合领域。本发明根据载体运动状态进行约束，当载体运动时进行正常的组合导航解算，当载体静止时首先锁定航向，然后组合导航解算之后再采用Kalman滤波估计对航向进行约束，当载体再次运动时解锁航向锁定进行常规的组合导航解算。

技术领域

本发明属于多信息融合领域，应用在精准农业、自动驾驶等领域，涉及一种约束惯性/卫 星组合导航静止时航向漂移的方法。

背景技术

随着农业信息化、自动化的发展，国家惠农政策的普及以及农民老龄化的比例上升，智 慧农业、精准农业逐渐成为国际研究热点。精准农业的普及可以解放双手提高工作效率、提 高土地利用率、及时播种收割、节约种子等投入，达到减少劳动力、减少投入、增加产量的 目标。

在精准农业农机自动驾驶领域，常采用惯性/卫星组合导航的方式为载体提供全信息的位 置、速度和姿态信息。在正常的农机作业过程中，经常需要进行整理农具、添加种子化肥等 停车操作，如果停车时间过长比如超过30分钟，组合导航解算的航向会慢慢漂移，导致载体 航向错误，给农业生产作业带来麻烦。传统的抑制航向漂移的方法是采用双天线GNSS的航 向进行位置/速度/航向约束，或者采用ZUPT即零速修正针对陀螺的误差特性进行约束，或者 采用磁力计进行辅助。而针对低成本的单天线GNSS和6轴的IMU既没有航向组合也没有磁 力计辅助，只能进行位置/速度组合解算。而采用ZUPT虽然能起到一定效果，但是随着静止 时间推移比如静止1个小时，以及陀螺所处环境的变化比如温度升高，零速修正的方法并不 能很好的进行抑制航向依然会有小幅度的漂移。

发明内容

[技术问题]

本发明要解决的技术问题：惯性/卫星组导航在静止时航向漂移的问题。惯性/位置组合导 航技术可以提供全信息的位置、速度以及姿态信息，被广泛的应用在各行各业。但当载体静 止时，航向可观性非常弱甚至是不可观测的，随着时间推移，组合导航解算的航向信息会漂 移。

[技术方案]

本发明提供一种约束惯性/卫星组合导航静止时航向漂移的方法，在载体静止时静止 航向锁定，利用锁定的航向和组合导航解算的航向进行滤波估计，对组合导航解算的航 向进行约束，载体运动时解锁已锁定的航向，只进行常规的组合导航解算。

具体包括以下步骤：

(1)判断载体的运动状态

通过判断相邻时刻载体的位置差以及速度，来判断载体是处于静止状态还是运动状 态；所述载体包括农机；

(2)锁定静止航向

如果判断结果表明载体处于静止状态，则锁定载体静止时航向；

(3)航向约束

用锁定的航向对组合导航实时解算的航向进行Kalman滤波估计，实时约束组合导航解 算的航向，防止长时间静止漂移。

所述步骤(1)采用式(1)来判断载体的运动状态：

其中：

k-1时刻组合导航解算的载体航向；

Veh_state：载体运动状态，1代表动态，-1代表静态；

V_k-1：k-1时刻组合导航解算的载体速度；

V_k：k时刻组合导航解算的载体速度；

δx：δx＝x_k-x_k-1，x轴方向位置差；