[發明專利]一種基于位置不變約束及零速校正的采煤機定姿定向方法

在審

• 申請號: 202111380954.6
• 申請日: 2021-11-20
• 公開（公告）號: CN114061574A
• 公開（公告）日: 2022-02-18
• 發明（設計）人: 王飛;李瑞;葉夢鈞;馬義寧;湯宏志;邢程
• 申請（專利權）人: 北京唯實深藍科技有限公司
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 暫無信息
• 代理人: 暫無信息

本發明公開了一種基于位置不變約束及零速校正的采煤機定姿定向方法。本發明針對采煤機中的捷聯慣導系統長時間工作后，由于慣性器件誤差累積導致姿態航向誤差發散的問題，發明了一種在慣導系統靜止狀態時以速度為觀測量以及在系統動態工作時以位置不變為觀測量兩種組合模態下的航向姿態誤差抑制方法，對慣導系統的平臺偏角以及慣性器件誤差進行估計，利用估計出的平臺偏角對慣導系統的姿態航向進行修正，從而提高慣導系統在長時間工作時的姿態、航向精度。本發明為采煤機中捷聯慣導系長時間工作后的姿態航向保持能力問題提供了解決方案，具有很強的實用價值。

技術領域

本發明涉及一種基于位置不變約束及零速校正的采煤機定姿定向方法，主要適用于提高慣導系統長時間工作后姿態航向的保持能力，屬于采煤機用慣性導航技術領域。

背景技術

煤炭是我國重要的能源來源之一，由于開采煤礦環境的惡劣，自動化、無人化、智能化成為我國當前開采煤礦的重要發展趨勢。因此，提高采煤機的定位定向精度具有重要的實用意義。

目前，針對采煤機的研究大多為利用無線傳感器網絡、超聲波反射等方法提高其定位精度，但由于煤礦內信號弱，且隨著工作長度的增加，信號會衰減等原因會導致定位出現偏差，且無法獲取采煤機的姿態航向信息。慣性導航技術得到初始信息后，可無需借助其他導航方式即可進行自主導航，并且既可以提供系統實時位置信息，也可以提供姿態航向信息，因此成為采煤機定位定向的有效解決方案。

然而，在實際應用中，采煤機的工作時間較長，慣導系統在長時間工作后，由于慣性器件誤差累積會導致慣導系統測定姿態航向不準確。為提高慣導系統的長航時姿態航向保持能力，本發明提出了一種結合位置和速度兩種量測信息的平臺偏角估計方法，進而提高慣導系統長時間導航后的姿態航向精度。

發明內容

本發明解決的技術問題是：針對采煤機長時間工作后，慣導系統難以保持高精度姿態航向的問題，提供一種基于位置不變約束及零速校正的采煤機定姿定向方法，在采煤機運動和靜止兩種模態下，分別選取位置不變約束和零速度作為量測，設計卡爾曼濾波器進行濾波，對系統的平臺偏角進行估計并進行補償，可有效提高慣導系統長航時的姿態航向精度。

本發明的技術解決方案：一種基于位置不變約束及零速校正的采煤機定姿定向方法，其特點在于：利用采煤機時走時停的特點，設計了在運動和靜止兩種模態下分別利用位置與速度進行觀測的卡爾曼濾波模型，通過該方法，可實現對慣導系統平臺偏角的估計，補償后可減小慣導系統的姿態航向誤差。該種基于位置不變約束以及零速校正的采煤機定姿定向方法包括如下步驟：

步驟一：對捷聯慣導系統進行導航解算，得到慣導系統的實時位置、速度以及姿態航向信息；

步驟二：在執行步驟一的過程中，當慣導系統處于動態工作狀態時，由于采煤機總體行進距離較短，為200米左右，可認為其位置基本沒有變化，因此，以初始位置信息作為基準，利用慣導系統實時位置信息與初始位置信息之差為觀測的卡爾曼濾波模型對系統運動過程中的平臺偏角進行估計，得到運動過程中慣導系統平臺偏角的估計結果；當慣導系統處于靜止狀態時，以零速為基準，利用慣導系統靜止時刻的速度與零速之差為觀測量，進行卡爾曼濾波對系統靜止狀態下的平臺偏角進行估計，得到靜止狀態下慣導系統平臺偏角的估計結果；

步驟三：將兩種模態下得到的平臺偏角的估計結果補償進慣導系統中，得到修正后的慣導系統姿態航向信息。

所述步驟二中，結合慣導系統位置和速度信息為觀測量的卡爾曼濾波模型對系統運動過程中的平臺偏角進行估計過程如下：

卡爾曼濾波模型以誤差傳播方程和慣性器件誤差特性為基礎建立狀態方程，狀態量包括位置誤差、速度誤差，東向、北向、天向平臺偏角以及機體系陀螺漂移，共10維；

卡爾曼濾波模型為：