[發明專利]一種約束慣性/衛星組合導航靜止時航向漂移的方法

在審

• 申請號: 202011502457.4
• 申請日: 2020-12-18
• 公開（公告）號: CN112729281A
• 公開（公告）日: 2021-04-30
• 發明（設計）人: 吳飛;朱龍泉;陳向東;程方
• 申請（專利權）人: 無錫卡爾曼導航技術有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01S19/47
• 代理公司: 哈爾濱市陽光惠遠知識產權代理有限公司 23211
• 代理人: 林娟

本發明公開了一種約束慣性/衛星組合導航靜止時航向漂移的方法，屬于多信息融合領域。本發明根據載體運動狀態進行約束，當載體運動時進行正常的組合導航解算，當載體靜止時首先鎖定航向，然后組合導航解算之后再采用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軸方向位置差；