3)以兩個慣導系統安裝後的相對距離為固定值為約束條件,推導濾波測量方程;設定值為r,滿足以下關系:
方程(1)可以縮寫為ZK = h (xk)+vk,其中下標k代表k時間;數量度量z =[r];v是測量噪聲;將該方程作為擴展卡爾曼濾波器的測量方程;
4)濾波器輸出主慣導和從慣導的北、東、天位置坐標,並將主慣導的北、東、天位置坐標作為采煤機的定位結果傳送給數據采集與顯示系統。
在步驟(1)中,航位推算算法如下:
其中下標k-1和K分別代表時間k-1和K;n、e、u分別是慣導系統在北、東、天空方向的位置坐標;v是軸編碼器輸出的速度值;t是采樣周期;θ分別是慣導輸出的航向角和俯仰角。
本發明提供的采煤機多慣性導航定位裝置及方法,以兩個慣性導航為例,利用安裝後主慣性導航和從慣性導航之間的相對距離為固定值的約束條件,用低成本的從慣性導航校準中等成本的主慣性導航,實現高精度定位,能夠滿足煤礦井下采煤機的定位精度要求,為實現綜采工作面智能化奠定基礎。
附圖簡述
圖1為采煤機多慣性導航定位裝置。
圖2是采煤機多慣性導航定位方法的原理框圖。
圖中:1,主慣導系統;2、對於溶液體系;3.用於從屬慣性導航;4、為軸編碼器;5.它是壹個過濾器;6、防爆外殼;7.數據采集和顯示系統;8.采煤機電氣控制櫃。
詳細描述
將參照附圖進壹步解釋本發明。
實施例1:如圖1所示,以兩個慣性導航系統為例,采煤機多慣性導航定位裝置包括:主慣性導航系統1、從慣性導航系統3、軸編碼器4、解算系統2、濾波器5、數據采集與顯示系統7、防爆外殼6。
主慣導1是中等成本的慣導,從慣導3是低成本的慣導。
采煤機的前端和後端分別固定有防爆外殼6,前端防爆外殼6內裝有主慣導系統1和計算系統2,後端防爆外殼6內裝有從慣導系統3和計算系統2;軸編碼器4安裝在采煤機的行走部;過濾器5安裝在采煤機的電氣控制櫃8內;數據采集和顯示系統7安裝在網關中的工業計算機上。
如圖2所示,本發明的采煤機多慣性導航定位方法包括以下步驟:
1)以兩個慣性導航系統為例,解算系統2采集慣性導航系統的姿態角和軸角編碼器4的速度數據,利用航位推算算法解算位置坐標。解算系統將主慣性導航系統1和從慣性導航系統3的位置坐標(pMaster和pSlave)傳輸到濾波器5。
2)利用航位推算算法推導出濾波器5的狀態方程。根據航位推算算法,定位方程可以縮寫為其中下標k-1和K分別代表k-1時間和K時間;狀態量X =[N1E1U1n 2 e2u 2]t,n 1,e 1,u 1是主慣導1的北、東、天空位置坐標,N2、E2、U2分別是從慣導3的北、天空位置。v是軸編碼器4輸出的速度值;t是采樣周期;θ分別是慣導輸出的航向角和俯仰角;w是狀態噪聲。該定位方程被用作擴展卡爾曼濾波器5的狀態方程。
3)在主慣導1和從慣導3之間的相對距離固定的約束下,推導出濾波器5的測量方程。設定值為r,滿足以下關系:
方程(1)可以縮寫為ZK = h (xk)+vk,其中下標k代表k時間;z =[r];v是測量噪聲。這個方程被作為擴展卡爾曼濾波器5的測量方程。
4)濾波器5輸出主慣性導航1和從慣性導航3的位置坐標,並將主慣性導航1的位置坐標作為采煤機的定位結果傳送給數據采集及顯示系統7。
在步驟(1)中,航位推算算法如下:
其中下標k-1和K分別代表時間k-1和K;n、e、u分別是慣導系統在北、東、天空方向的位置坐標;v是軸編碼器4輸出的速度值;t是采樣周期;θ分別是慣導輸出的航向角和俯仰角。
本專利以兩個慣性導航系統為例說明采煤機的多慣性導航定位裝置和方法,同樣適用於三個或三個以上的多慣性導航定位系統。
以上僅是本發明的優選實施例,需要指出的是,本領域技術人員在不脫離本發明原理的情況下,可以進行多種改進和修飾,這些改進和修飾也應視為本發明的保護範圍。