專利名稱：確定電采掘機鏟的空間位姿的實時方法
技術領域：
本發(fā)明涉及對設備進行定位的領域，具體地，公幵了一種用于確定在釆掘操作中 使用的轉臂式裝載設備(如，電釆掘機鏟)的空間位姿的系統(tǒng)。
背景技術：
參考文獻Department of the Army，1993，F(xiàn)M 6-2. Tactics, Techniques，and Procedures for Field Artillery Survey, Department of the Army, Washington DC.Dizchavez, R. F.，2001，Two-antenna positioning system for surface-mine equipment，US Patent 6191733Gelb，A.，1996，Applied optimal estimation，The M. I. T. Press，CambridgeKalafut，J. J.，Alig，J. S.，2002，Method for determining a position and heading of a work machine，US Patent 6418364Pike, J. ,2006, iWorld Geodetic System 1984’， [Online]Available at http://www, globalsecurity. org/military/library/policy/army/fm/6-2/fige-l. gifSahm,W. C. et al. ,1995,Method and apparatus for determining the location of a work implement，US Patent 5404661Tu, C. H. et al.，1997，GPS compass :A novel navigation equipment，IEEE Transactions on Aerospace and Electronic Systems，33，1063—1068·Vaniceck，P.，Krakiwsky, Ε.，1986，Geodesy :The concepts，Elsevier Science Publishers B. V.，Amsterdam.Gelb, A. and Vander Velde，W. E.，Multiple-Input Describing Functions and Nonlinear System Design, McGraw-Hill Book Company, New York(1968).Graham，D. and McRuer, D.，Analysis of Nonlinear Control Systems, John Wiley & Sons Inc，New York(1961).Duddek et al. 1992 ；Method of determining mining progress in open cast mining by means of satellite geodesy, US Patent 5144317.之前已經提出了確定移動設備單元的位置和方向的問題的多種解決方案。解決方 案不變地利用多種定位傳感器，包括上述傳感器中的一些。Duddek等(1992)公開了一種使用勺輪附近的GPS傳感器和接收機來確定挖掘機 勺斗末端的位置和方向的方法。Kalafut等(2002)提出了一種用于通過使用單個定位傳感器來確定機器的位置 和方向的系統(tǒng)。隨時間從定位傳感器獲取讀數，產生運動曲線圖以估計機器的方向。該方 法具體可應用于一般在運動中并且具有良好動態(tài)特性的機器。在采掘應用中，只要這些托 運卡車在運動中，則托運卡車是這種方法的良好備選。單傳感器定位系統(tǒng)的另一示例是由Sahm等(1995)提出的單傳感器定位系統(tǒng)，該單傳感器定位系統(tǒng)使用單個傳感器，所述單個傳感器能夠采集與采掘機鏟的懸臂有關的 (X，1，ζ)位置參數。如果假定機鏟的底架在挖掘周期期間是固定的，則可以隨時間測量點 集合，以產生傳感器所在的平面。該估計可以與來自傳感器的位置的當前測量一起用于估 計機鏟勺斗的當前位置。Dizchavez (2001)還提出了根據所估計的平面來定位的方法。兩個GPS天線安裝 在機器外殼上等高度的已知位置。在機器的操作期間，可以測量外殼的旋轉，使用基于標準 差分析的計算來形成對兩個天線所在平面的估計。根據該平面以及該平面內傳感器的當前 位置和方向，在給定運動模型和適當接合位置信息的情況下可以定位機器的另一部分。希望提供一種確定采掘設備等的空間位姿的方法和設備。說明書全文中對現(xiàn)有技術的任何討論不應被看作是承認現(xiàn)有技術是公知的或構 成本領域公知常識的一部分。除非上下文明確要求，否則，在說明書全文和權利要求中，詞語“包括”等具有與排 他或窮盡含義相對的包含含義；也就是說，具有“包括但不限于”的含義。

發(fā)明內容
根據本發(fā)明的第一方面，提供了一種確定采掘機鏟的全球位姿的方法，所述方法 包括應用多級計算的步驟，所述多級計算包括(a)第一級，使用全球定位系統(tǒng)、傾斜計以及搖擺軸解算器來計算采掘機鏟車體 (c坐標系)相對于本地大地坐標系(g坐標系)的方位；(b)第二級，使用全球定位系統(tǒng)、軸慣性傳感器和搖擺軸解算器來計算相對于c坐 標系的外殼位姿(h坐標系)；(c)第三級，使用推壓和提升軸解算器來計算相對于h坐標系的勺斗位姿(b坐標 系)。優(yōu)選地，使用擴展卡爾曼濾波器來執(zhí)行所述步驟(a)和(b)?？梢允褂玫?來執(zhí)行步驟(a)直到收斂。傾斜計可以是雙軸傾斜計。軸慣性傳感器可以是六軸慣性傳感 器。機鏟的第一部分可以包括機器外殼。根據本發(fā)明的另一方面，提供了一種按照三級計算過程來確定電采掘機鏟的全球 位姿的方法，其中(a)在第一級，使用全球定位系統(tǒng)、雙軸傾斜計以及搖擺軸解算器來計 算車體(C坐標系)相對于本地大地坐標系(g坐標系)的方位，直到收斂；(b)在第二級， 使用全球定位系統(tǒng)、六軸慣性傳感器(三個速率陀螺儀和三個線性加速度)和搖擺軸解算 器來計算相對于c坐標系的外殼位姿(h坐標系)；(C)在第三級，使用推壓和提升軸解算器 來計算相對于h坐標系的勺斗位姿(b坐標系)。根據本發(fā)明的另一方面，提供了一種確定采掘機鏟的全球空間位姿的方法，所述 方法包括以下步驟(a)指定參考的第一地心地固ECEF坐標系或e坐標系；(b)指定在采 掘機鏟附近表示為g坐標系的本地大地坐標系，g坐標系被限定為e坐標系中的笛卡爾坐 標軸集合；(c)指定靠近采掘機鏟的本體或底架、表示為c坐標系的笛卡爾坐標軸集合；(d) 確定g坐標系內c坐標系的方位；(e)指定在采掘機鏟的機器外殼附近表示為h坐標系的笛 卡爾坐標軸集合；(f)確定c坐標系內h坐標系的方位；(g)指定被固定到機鏟柄和鏟斗裝 置附近、表示為b坐標系的笛卡爾坐標軸集合；以及(h)確定h坐標系內b坐標系的方位。


現(xiàn)在將參考附圖來描述本發(fā)明的優(yōu)選形式圖1示出了裝載托運卡車的電采掘機鏟；圖2示出了 e坐標系和g坐標系的定義；圖3示出了 c坐標系、h坐標系和b坐標系的定義；圖4示出了 P&H Centurion控制機鏟的搖擺軸的控制系統(tǒng)；圖5示出了飽和型非線性的特性，包括作為輸入函數的描述函數增益；圖6示出了 P&H類電采掘機鏟的坐標系；圖7示出了出于定義b坐標系的目的而處于直角配置中的P&H類電采掘機鏟；以 及圖8示出了優(yōu)選實施例的方法步驟的流程圖。
具體實施例方式如圖8所示，優(yōu)選實施例提供了一種確定電采掘機鏟的全球空間位置的改進方法 80。全球空間位姿包括 地心地固(ECEF)坐標系或e坐標系81的指示； 本地大地坐標系(g坐標系)的標識，所述本地大地坐標系被限定為在e坐標系 中例如沿著北、東和下慣例的笛卡爾坐標軸集合。該坐標系的原點在采掘機鏟附近，典型地 在機器所位于的礦產內82 ； 固定到采礦機鏟83的車體或底架的笛卡爾坐標軸集合的指示。這些軸所限定 的笛卡爾坐標系已知為c坐標系；· g坐標系內c坐標系的方位(位置和方向)的指示84 ； 固定到采掘機鏟的機器外殼的笛卡爾坐標軸的指示。這些軸所限定的笛卡爾坐 標系已知為h坐標系85 ；· c坐標系內h坐標系的方位(位置和方向)的確定86 ； 固定到機鏟柄和鏟斗(勺斗)裝置的笛卡爾坐標軸集合的指示。這些軸所限定 的笛卡爾坐標系已知為b坐標系；· h坐標系內b坐標系的方位(位置和方向)的確定。這些坐標的定義使得可以在全球坐標系中建立勺斗的方位。如圖1所示，采掘機鏟1和其他類似挖掘機的操作的基本特性是，采掘機鏟1和其 他類似挖掘機每次維持c坐標系的方位若干分鐘。即，對使用履帶2的機器的重新定位是 不頻繁地進行的，當挖掘機順序地挖掘材料并將材料裝載到托運卡車4中時在移動之間的 主要活動是機器外殼3的前后搖擺運動。優(yōu)選實施例利用采掘機鏟1的這種操作特性來解決確定機鏟位姿的問題。優(yōu)選實施例還利用若干可用附加傳感器測量的組合，包括 固定到h幀的一個或多個標識點位置在e坐標系中的實時運動全球定位系統(tǒng)； 由h坐標系相對于g坐標系的三個正交加速度和三個角速率構成的慣性測量；· h坐標系相對于g坐標系的縱搖和橫搖的傾斜計測量；
三個主運動執(zhí)行器(即，搖擺發(fā)動機、推壓發(fā)動機和提升發(fā)動機)的速度和位置 測量； 來自三個主運動執(zhí)行器(即，搖擺發(fā)動機、推壓發(fā)動機和提升發(fā)動機)的電壓和 電流測量； 機鏟操作者通常通過操縱桿設置的參考值，這些參考值是向三個主運動執(zhí)行器 (即，搖擺發(fā)動機、推壓發(fā)動機和提升發(fā)動機)的控制系統(tǒng)的輸入。優(yōu)選實施例提出了基于擴展卡爾曼濾波器的遞歸算法的公式化，所述卡爾曼濾波 器使用這些測量的組合來確定全球機鏟位姿。已知實時機鏟位姿有多種用途，這些用途包括1.已存在商業(yè)系統(tǒng)的應用，使用對挖掘期間鏟斗相對于資源分布圖的位置的知識 作為允許操作者區(qū)分礦石與廢料的手段；2.顯露重要性的應用，用于使采掘設備自動化，其中需要解決方案的重要問題控 制與諸如托運卡車之類的其他設備的交互。如果這樣的設備單元具有類似的位姿估計能 力，則可以確定設備之間的相對位姿；3.來自掃描距離傳感器(例如，激光傳感器以及可能用于自動化系統(tǒng)中的測距以 及用于開發(fā)本地數字地形圖的毫米波雷達)的數據的正確空間登記也需要機鏟位姿的知 識?，F(xiàn)有解決方案忽略了可以適于估計問題的估計理論。具體地，可以將問題簡化化 為狀態(tài)估計訓練，其中，可以將g坐標系、c坐標系、h坐標系和b坐標系的相對位置和方向 表示為動態(tài)系統(tǒng)的狀態(tài)，使用對測量的操作者命令參考與機器的結果運動之間的因果關系 (“過程模型”)的知識_及時傳播機鏟位姿的當前知識(以概率分布的形式)，以便于與來 自先前標識的傳感器的測量的組合相融合。問題簡化圖2和圖3示出了與包括多種坐標系的問題有關的幾何結構。首先轉向圖2，示出 了相對于地球21來定位地球坐標系(e坐標系)和大地坐標系(g坐標系)的幾何坐標系。 圖3示出了車體坐標系(c坐標系)、外殼坐標系(h坐標系)和勺斗坐標系(b坐標系)。以兩級來計算機鏟勺斗的位姿。第一級的目的是根據以下測量來計算c坐標系相對于h坐標系的方位· η個RTK-GPS接收機的g坐標系中的位置； 如h坐標系中固定的雙軸傾斜計測量的，Zh軸相對于Zg軸的明顯的方向；· h坐標系繞Ze軸的旋轉； 搖擺發(fā)動機的角速度； 搖擺發(fā)動機的電樞電流和電樞電壓； 來自操縱桿的操作者命令參考。這些量限定了測量矢量ζ和輸入矢量U。第二級的目的是在找到c坐標系相對于g坐標系的方位的情況下使用以下測量來 計算h坐標系相對于c坐標系的方位· η個RTK-GPS接收機的g坐標系中的位置； 在h坐標系中固定的點在三個正交方向上的角速率和線性加速度的測量，但是這在瞬時沿著正交傳感器軸的慣性系中測量的；· h坐標系繞ζ。軸的旋轉； 搖擺發(fā)動機的角速度； 搖擺發(fā)動機的電樞電流和電樞電壓； 來自操縱桿的操作者命令。這些量限定了第二測量矢量ζ和第二輸入矢量U。第三級的目的是使用以下測量以及挖掘裝置的運動模型來計算b坐標系相對于h 坐標系的方位 提升發(fā)動機的位置 推壓發(fā)動機的位置級3計算是運動的并且計算b坐標系相對于h坐標系的方位。計算進行至確定如下機鏟位姿 在機器已完成任何推進運動并進入正常挖掘活動(特征在于反復地往復搖擺) 之后，第一級計算立即運行足夠的時間以得到c坐標系相對于g坐標系的方位的收斂估計。 假定g坐標系相對于e坐標系的方位是先驗已知的； 在級1得到收斂之后，發(fā)起第二級計算和隨后的第三級計算，以規(guī)則時間步長 來進行第二和第三級計算以確定h坐標系相對于c坐標系的位置以及b坐標系相對于h坐 標系的位置； 當操作者下一次驅動機器時，在驅動運動完成之前計算停止，其中，當驅動運動 完成時，再次執(zhí)行第一級計算以找到c坐標系相對于g坐標系的方位的新的收斂估計。然 后計算進行至級2，以此類推。支持該分級計算過程的是以下構思在級1使用的測量可以提供與機器的低頻運 動有關的豐富信息，這些信息足以精確地確定c坐標系相對于g坐標系的位置。在與正常 生產相關聯(lián)的正常往復運動期間，除了大規(guī)模搖擺運動之外，具體在挖掘期間機器的外殼 也可能發(fā)生較小幅度的擺動運動。級2使用的的傳感器的測量目的在于精確地確定這些運 動。在這種情況下，級2濾波器目的在于更高的估計帶寬。級1和2的計算方法是擴展卡爾曼濾波器(EKF)、Celb (1974)。EKF需要以下形式 的系統(tǒng)模型i=f(>,u，O + w,w N(0，Q)(1)zk = h(xk, uk, k) +vk, vk N(0，R)其中，f(x, u, t)是描述系統(tǒng)動態(tài)的矢量值函數，所述系統(tǒng)用于基于操作者命令參 考的測量及時傳播狀態(tài)和狀態(tài)協(xié)方差的當前估計，因此f(x，U, t)可以與新得到的測量數 據相結合。矢量值函數h(xk，Uk, k)表示關于狀態(tài)矢量X和輸入U的測量。EKF需要關于估計狀態(tài)軌跡i對f (X，u, t)和h(xk，uk, k)的線性化，以及線性化連 續(xù)動態(tài)到離散時間形式的轉換。希望使用以下表示法
「 (Sf (χ,/) )、F = exp ~-——-At
\ ^x J其中，At是測量更新速率，以及
權利要求
一種確定采掘機鏟的全球位姿的方法，所述方法包括應用多級計算的步驟，所述多級計算包括(a)第一級，使用全球定位系統(tǒng)、傾斜計以及搖擺軸解算器來計算采掘機鏟車體(c坐標系)相對于本地大地坐標系(g坐標系)的方位；(b)第二級，使用全球定位系統(tǒng)、軸慣性傳感器和搖擺軸解算器來計算相對于c坐標系的外殼位姿(h坐標系)；(c)第三級，使用推壓和提升軸解算器來計算相對于h坐標系的勺斗位姿(b坐標系)。
2.根據權利要求1所述的方法，其中，使用擴展卡爾曼濾波器來執(zhí)行所述步驟(a)和(b)。
3.根據前述權利要求中的任一項權利要求所述的方法，其中，使用迭代例程來執(zhí)行步 驟(a)直到收斂。
4.根據前述權利要求中的任一項權利要求所述的方法，其中，傾斜計是雙軸傾斜計。
5.根據前述權利要求中的任一項權利要求所述的方法，其中，傾斜計是六軸慣性傳感ο
6.根據前述權利要求中的任一項權利要求所述的方法，其中，機鏟的第一部分包括機 器外殼。
7.—種全球定位采掘機鏟的位姿的方法，實質上按照參考附圖和/或示例中所示的本 發(fā)明任一實施例而描述的來全球定位采掘機鏟的位姿。
8.一種按照三級計算過程來確定電采掘機鏟的全球位姿的方法，其中，(a)在第一級，使用全球定位系統(tǒng)、雙軸傾斜計以及搖擺軸解算器來計算車體(c坐標 系)相對于本地大地坐標系(g坐標系)的方位，直到收斂；(b)在第二級，使用全球定位系統(tǒng)、六軸慣性傳感器(三個速率陀螺儀和三個線性加速 度)和搖擺軸解算器來計算相對于c坐標系的外殼位姿(h坐標系)；(c)在第三級，使用推壓和提升軸解算器來計算相對于h坐標系的勺斗位姿(b坐標系)。
9.一種確定采掘機鏟的全球空間位姿的方法，所述方法包括以下步驟(a)指定參考的第一地心地固ECEF坐標系或e坐標系；(b)指定在采掘機鏟附近表示為g坐標系的本地大地坐標系，g坐標系被限定為e坐標 系中的笛卡爾坐標軸集合；(c)指定靠近采掘機鏟的本體或底架、表示為c坐標系的笛卡爾坐標軸集合；(d)確定g坐標系內c坐標系的方位；(e)指定在采掘機鏟的機器外殼附近表示為h坐標系的笛卡爾坐標軸集合；(f)確定c坐標系內h坐標系的方位；(g)指定被固定到機鏟柄和鏟斗裝置附近、表示為b坐標系的笛卡爾坐標軸集合；以及(h)確定h坐標系內b坐標系的方位。
10.一種確定采掘機鏟的全球空間位姿的方法，實質上按照參考附圖和/或示例中所 示的本發(fā)明任一實施例而描述的來確定采掘機鏟的全球空間位姿。
11.一種用于確定采掘機鏟的全球空間位姿的設備，實質上按照參考附圖和/或示例 中所示的本發(fā)明任一實施例而描述的來確定采掘機鏟的全球空間位姿。
全文摘要
已知采掘挖掘機的全球定位為采掘操作的管理和自動化提供了很多好處。本發(fā)明描述了一種全球定位電采掘機鏟的位姿的方法。系統(tǒng)從安裝在機器外殼上的任意數目個RTK-GPS天線以及適合機器搖擺軸的解算器獲得測量。使用卡爾曼濾波器來產生全球方位位姿的估計。
文檔編號E02F3/43GK101970763SQ200980105400
公開日2011年2月9日 申請日期2009年1月7日 優(yōu)先權日2008年1月8日
發(fā)明者安東尼·沃爾頓·里德, 彼得·羅斯·麥卡里 申請人:易斯麥私人有限公司