專利名稱：有效載荷的估算系統(tǒng)和方法
技術領域：
本發(fā)明涉及一種評估采礦機鏟有效載荷的方法與系統(tǒng)，特別是在評估過程中采用了卡爾曼濾波方法。
背景技術：
電動采礦機鏟(EMS)通常是用于開放式開采點裝載托運卡車的大型電動機械挖掘機。它們是許多開放式開采場所的關鍵生產裝置，不斷發(fā)展的要求就是提高它們的生產效率。圖1示意性地說明這些機鏟的一個。
自從20世紀50年代，已經認識到，測量采礦機鏟(或其它挖掘機)在一次挖掘中所采集到的材料的重量的能力能夠提高安全性，并促進提高生產效率。本發(fā)明從領域最佳狀態(tài)和參數(shù)估算來提出設想和方法，以便解決電動采礦機鏟的有效載荷稱重問題。
有效載荷的知識如何用于提高安全性和生產效率的一個例子是減少拖運卡車的超載頻率?？ㄜ嚦d的發(fā)生主要是因為礦山知道生產利益在于盡可能最大數(shù)量的裝材料到每個卡車上。許多礦山繼續(xù)支持直觀理由，即推動額定功率的極限能夠提高操作效率。而且，他們知道利益是非常足夠地用來追求這個挑釁性的目的。例如，常見的例子是對操作員的表現(xiàn)以獎金作為獎賞，如果足夠多的卡車在接近卡車容量的一個“最佳”范圍內被裝載。
卡車超載降低了安全性，減少了卡車壽命和增加了停機時間和維修費用。為了勸阻超載，采礦操作員和卡車生產商之間的維修和保單合同，目前，將通常包括一個懲罰條款，當在稱上的卡車顯示超重，要使其強制停運和卸下負荷。卸下負荷導致直接生產損失，該損失復合了與再次處理卸下材料相關的費用。
現(xiàn)代卡車配備了在線天平能夠在它們自己的托架上監(jiān)視材料重量。這些系統(tǒng)通常采用懸掛壓桿壓力的方法，當系統(tǒng)被校準后，測量重量的準確度為±3-5％。然而，這些安裝在卡車上的天平準確性只有在卡車運動時能夠被接受，特別是以二檔行馳在平坦路面。由于機械不合適所影響，如壓桿封條之間摩擦和壓桿棒彎曲同時結合其它一些造成懸掛被鎖的因素諸如在裝載區(qū)域周圍的溢出巖石和在卡車托架中材料的不均勻分布，在裝載點重量報告的準確度通常保持為±20％。
通常一個現(xiàn)代大型電動電鏟(標稱有效載荷接近80噸)裝載300噸托運卡車需要通過四關。目前，操作員采用卡車天平提供的信息(在裝載點給出很差的估算值)并結合他們自己的判斷來管理裝載過程。研制電鏟有效載荷稱重的動機是如果電鏟操作員在每次挖掘后，能夠知道在鏟斗里有多少材料，卡車天平還剩余多少容量，那么起重機的危害就能夠較好的管理。這些信息應在最后一次挖掘被倒入卡車之前做出“繼續(xù)”或“不繼續(xù)”的決定。
其它有效載荷監(jiān)視的好處也能夠想象到。例如，能夠提供操作員實時有效的載荷重量，幫助他們達到較好、更一致地操作機器的反饋信息。而且，存在潛在的以周期為基礎監(jiān)測電鏟性能的價值；每次挖掘操作采集的數(shù)量作為一個關鍵性能指示被廣泛保存著。
以前多次試圖研制采礦電鏟有效載荷系統(tǒng)，并且目前有多個商業(yè)系統(tǒng)可采用。然而制造商引用的準確度為±2％，工業(yè)上廣泛的相信這些系統(tǒng)不能可靠地實現(xiàn)這個準確度。對于一個標有80噸有效載荷的電鏟，最大誤差可能為±20噸。
有效載荷估算問題涉及的背景工作包括 (i)以前研制的方法具體地應用到電動采礦電鏟上； (ii)用于其它采礦設備的有效載荷估算方法，諸如液壓挖掘機和索斗鏟或像建筑起重機等同類設備；和 (iii)計劃用于機器人工業(yè)以識別機器人鏈接和有效載荷慣性參數(shù)的方法。
確定采礦機器有效載荷的方法可以追溯到20世紀50年代。這些有時涉及計算出有效載荷的估算值的精巧裝置的系統(tǒng)，包括采用電動和射流電路的模擬計算。但是鑒于發(fā)明者的知識水平，沒有一套系統(tǒng)研制成功，當然現(xiàn)在就沒有得到廣泛的使用。大約到20世紀80年代，這些早期發(fā)明中的多數(shù)研究由于在采礦設備的研制中可以在微處理器的使用中進行打包而變得多余。這個工作的初始假想是任何未來應用有效載荷的系統(tǒng)都是脫離微處理機技術，相應地，根據這個觀點打算實施以計算機為基礎的技術受到了限制。
電動采礦機鏟有效載荷估算 常(Chang)等人在美國專利6,225,574中對于鏟斗和鏟柄裝置使用靜力矩平衡式計算有效載荷。這個方法平衡了重力和起重繩索施加于鏟斗和鏟柄的力矩。通過仔細選擇結構的坐標，建立的關系式只是挖掘機位置和起重繩索力的函數(shù)，且獨立于大量驅動器施加的力。這個算法被認為是一些商業(yè)有效載荷系統(tǒng)的基礎。
起重繩索力通過測量電樞電流并將其與發(fā)動機力矩常數(shù)和傳輸比率相乘而得到。假設重力通過允許確定有效激蕩力的已知點(鏟斗和鏟柄裝置的質量中心)起作用。然后將這個力除以重力加速度常數(shù)得到有效載荷質量。用這個方法在周期性旋轉電鏟的期間得到幾個有效載荷估算值，把它們進行平均就確定最終有效載荷估算值。
常(Chang)等人報導了這種方法，即當電鏟在靜態(tài)情況下操作這個方法完全足夠，但是在運動時具有較差的結果。他們試圖在0和1之間賦予“模糊”置信因子，在總數(shù)上將每個有效載荷估算值加權，來克服這個局限性。計算這個置信因子是基于測量時間內所觀察到的挖掘機速度和加速度。速度或加速度越高，置信值越低；速度或加速度越低，置信值就越高。在計算最終有效載荷值之前使用置信極限來選出界外值，作為即時有效載荷估算值的加權平均值。
這個加權平均值方法看來特別嘗試于解決機器移動引起的力。采礦機鏟是一個運動的機器，其中每個驅動器上的運動是間斷性的，并且只能預測其總體特性。公開的問題是與挖掘機的加速度和速度所關聯(lián)的慣性力影響估算有效載荷質量的程度。這包括當電鏟旋轉時所產生的向心力。
常(Chang)等人的這個方法也假設有效載荷質量中心是不變的，并且固定在用于全部有效載荷大小的鏟斗的質量中心上。這個假設的有效性沒有被測試，并且沒有報告有效載荷估算值與有效載荷質量中心的誤差靈敏度。而且，并沒有在得出估算值方面進行任何明確嘗試，以便補償驅動器摩擦和其它損失來源。
Radomilovich在美國專利4,677,579中所描述的有效載荷估算方法使用動力學模型，動力學模型解釋了在起重機系統(tǒng)中保守和非保守效應。該方法的說明書中包括對與驅動電機轉動慣性和減速齒輪鏈相關的慣性力的修正，起重機繩索的拉緊以及電鏟慣性的討論。由于摩擦和發(fā)動機的無效引起的非保守損失也被提及。Radomilovich使用了類似于常(Chang)等人所述的平衡力來估算有效載荷。下面將描述算法，忽略幾個潛在重要因素 ●不包括旋轉移動。因此，忽視作用在系統(tǒng)上的離心力和回轉力。
●算法需要通過對發(fā)動機速度進行微分得到發(fā)動機加速度。采用穩(wěn)態(tài)的電動發(fā)動機公式通過測量電樞電壓和電流得到發(fā)動機速度。對速度信號的微分處理放大了噪聲。獲得低噪聲加速度信號難點是贊成采用不需要對加速度進行推斷測量的方法。
●通過對發(fā)動機速度進行積分可以得到發(fā)動機位置。由于電樞電壓和電流是噪聲信號，推斷位置的變化將隨時間無限制增長。實際上這個方法將要求電鏟運動不斷地重新標定。當然，對發(fā)動機直接進行位置感應能夠克服這個限制。
布萊爾(Blair)等其他人在美國專利4,809,794中的方法是建立在觀測的基礎上，原則上，知道了鏟斗質心的位置就可以確定鏟斗和其內物質的重量在懸臂或支撐結構的任一位置上產生的張力。反之，知道了支撐機構某一特定部位的張力和質心的位置，就可以計算出鏟斗及其內物質的重量。他們計劃通過安裝在支撐結構(在支持懸臂的A型框架上)上的應變計并結合使用經過試驗或分析得出的影響系數(shù)來將測量獲得的張力與鏟斗、手柄以及有效載荷的重量聯(lián)系起來，該影響系數(shù)為鏟斗位置的函數(shù)。該專利的大部分并沒有說明當機鏟旋轉到傾倒位置時函數(shù)關系建立的邏輯，并且關于如何確定重量和應力聯(lián)系的影響系數(shù)也只有相當小的描述。該運算法平均了在各個例子里旋轉階段某些部分獲得的重量。該方法假設了一個有效荷載的質量中心的位置，并且沒有考慮與運動聯(lián)系在一起的動態(tài)荷載。那些作者們注意到，最好將在旋轉過程開始和結束時即動態(tài)荷載通常在最高時獲得的一些重量數(shù)據從取平均值的步驟中除去。
發(fā)明內容
根據本發(fā)明的第一個方面，提供一種估算承受重力機械的有效載荷的方法，這個方法包括以下步驟 (a)建立這個承受重力機械不同級別有效載荷的一系列的卡爾曼濾波器近似值；(b)從這一系列近似值來確定挖掘機當前運行特性參數(shù)；(c)利用步驟(b)確定的參數(shù)確定重力承受機械的有效載荷估算值。
這個重力承受機械包含電動采掘機鏟。卡爾曼濾波器的輸入包含用以控制這個重力承受機械運動的驅動控制信號?？柭鼮V波器的一系列近似值模擬出不斷增大的作用于這個重力承受機械的有效載荷。步驟(b)進一步包含以下步驟形成每個模型的當前模型概率，然后求出這些模型的加權和來確定總體模型。
現(xiàn)在將根據以下附圖來對優(yōu)選實施例進行說明，其中 圖1所示的是一個電動采礦機鏟； 圖2表示的是在典型的旋轉至傾倒位置的過程中，在電動采礦機鏟上測量獲得的提升驅動信號的圖表。
圖3表示的是在典型的旋轉至傾倒位置的過程中，電動采礦機鏟的裝載驅動信號的圖表。
圖4表示的是在費力地旋轉至傾倒位置的過程中，電動采礦機鏟的旋轉和驅動信號的圖表。
圖5所示的是在機器循環(huán)的旋轉至傾倒位置的過程中，在每個三秒間隔時機鏟的機械構造。
圖6所示的是應用在優(yōu)選實施例中的卡爾曼濾波器的結構圖。
圖7所示的是應用在優(yōu)選實施例中的多模型運算法則的概覽。
圖8所示的是包括在優(yōu)選實施例中的步驟。
圖9所示的是應用在電動采礦機鏟圖形上的坐標系統(tǒng)。
圖10所示的是另外一個應用在電動采礦機鏟系統(tǒng)上的坐標系統(tǒng)。
具體實施例方式 本發(fā)明的最佳實施例產生于不能預先忽略動態(tài)載荷的觀點。因此，我們試圖包括動態(tài)載荷。采礦機鏟的剛性體動態(tài)特性 其中，θ為結構變量，例如，連接角的矢量；
為連接速度的矢量；
為連接加速度的矢量；M(θ)為參照(并取決于)相對結構變量的質量矩陣；
為依賴于速度和重力的總合成力的矢量；并且，τ為由致動器施加的總合成力矢量。
基本的問題在于如何將比如這種模型結合在問題方程式中。回顧現(xiàn)有技術，對需要機器運動的位置、速度和加速度的測量值的方程式進行了批評，其原因在于對每一自由度的所有三個變量進行同步測量，充其量是非常復雜的并且可能是不現(xiàn)實的。典型的采礦機鏟具有提供位置測量的解析裝置并且可以裝配用以提供速度測量值的測速發(fā)電機，但是，可以被安裝至馬達軸上的轉動加速器尚未得到廣泛應用而安裝至外部，例如，鏟斗的加速器則難以維護。最好期望在傳動中具有一定順次性，并且，應注意的是無論設置加速器的位置如何，相對于某些系統(tǒng)慣性而言，這些加速器均不是并排布置的。
通常的做法是通過用于獲得速度或加速度的數(shù)字微分和雙微分位置傳感器或通過對速度信號求積分和求微分來獲得位置和加速度，從而克服不能直接進行速度和加速度測量的局限。這類方案通常由應用于信號的高通濾波器(微分)或低通濾波器(積分)來實現(xiàn)。如果在基礎信號上出現(xiàn)噪音，則這種方案可能會出現(xiàn)問題。該噪音因求微分被放大并且積分信號的變化會不受限制地增大。
一種可選擇的方案為使用動態(tài)的因果(或前向)形式以根據施加的力預測將來的運動。該方案能夠立即將所述問題轉換為適于應用最佳狀態(tài)和參數(shù)估算方法的形式。以此方式對問題進行公式化的優(yōu)點在于(i)無需加速度測量，(ii)在方程中必然包括測量噪音和模型誤差；以及(iii)模型的因果結構能夠降低驅動信號上的噪音，即‖M-1‖較小。這種方案的主要優(yōu)點在于所述模型保持了常微分方程結構，因此，與要求由運動方程估算有效載荷的代數(shù)提取的情況現(xiàn)相比，這種解決過程更為復雜。
最佳狀態(tài)和參數(shù)估算量試圖對系統(tǒng)的動態(tài)和模型參數(shù)進行最佳評定(以最小方差方式)。其能夠為計算與機器運動相關的慣性載荷提供隱含但清楚、有效的框架。
最佳實施例使用了卡爾曼濾波器來確定重量估算量?？柭鼮V波器是線性遞歸估算器，其能夠使用與該狀態(tài)線性相關的觀測值來計算動態(tài)系統(tǒng)的狀態(tài)的最小變量估算值。假定被估算的狀態(tài)是由一個高斯白噪音引導的線性系統(tǒng)，并且通過該線性系統(tǒng)能獲得被高斯白噪音(Gaussian whitenoise)干擾的周期性的測量數(shù)據，卡爾曼濾波器在自動檢測中是最佳的選擇，因為它能在該假定狀態(tài)下使均方差的估算誤差最小。
過程模型 過程模型用在基于卡爾曼濾波器的方法中，以便從當前狀態(tài)的估算值和控制輸入的信息來預測系統(tǒng)的將來狀態(tài)。它將研究的動力學系統(tǒng)的先前信息編碼，并且通過從當前系統(tǒng)狀態(tài)的估算值和狀態(tài)協(xié)方差來預測未來系統(tǒng)狀態(tài)和狀態(tài)協(xié)方差的方式來及時傳播關于未來系統(tǒng)狀態(tài)信息。這樣，它以具有當前系統(tǒng)狀態(tài)信息的傳感器測量值的形式提供機械裝置暫時的新信息融合。
卡爾曼濾波器理論假定一個線性的、隨機的、不同的等式可以用來模擬這個系統(tǒng)，并且從系統(tǒng)中獲得的測量數(shù)據都和系統(tǒng)狀態(tài)線性相關。
描述系統(tǒng)模型和測量數(shù)據的等式形式如下 x(ti)＝Φ(ti，ti-1)x(ti-1)+Γ(ti-1)u(ti-1)+G(ti-1)w(ti-1)，(2) z(ti)＝H(ti)x(ti)+v(ti)，ti＝iΔt， (3) 其中x(ti)是N維的狀態(tài)矢量；u(ti)是表示系統(tǒng)輸入量的1維矢量；并且w(ti)是表示由未建模的運動導致的不確定性和輸入量的不確定性的S維過程噪音矢量。矩陣Φ(ti，ti-1)表示系統(tǒng)動態(tài)并且矩陣Γ(ti-1)和G(ti-1)表示輸入量和過程噪聲在狀態(tài)中的分布情況。變量z(ti)是m維測量數(shù)據矢量；v(ti)是測量數(shù)據干擾噪聲的m維矢量；并且H(ti)是表示測量數(shù)據與狀態(tài)關系的測量數(shù)據矩陣。在連續(xù)的測量之間的取樣周期或者時間表示為Δt。
過程噪聲w(ti)和測量數(shù)據噪聲v(ti)都被假定成是獨立的，零平均值的白高斯噪聲(white Gaussian noise)序列， E{w(ti)}＝0，(4) E{v(ti)}＝0，(5) E{w(ti)w(tj)}＝Q(ti)δij， (6) E{v(ti)v(tj)}＝R(ti)δij， (7) E{w(ti)v(ti)}＝0. (8) 過程噪聲表示在過程模型和設備實際操作之間的差異問題。過程噪聲包括三種成分(i)作用在控制輸入量上的各種干擾，包括輸入量的測量誤差和常被假定為在每個預測步驟都是不變的輸入量的未建模的變動；(ii)模型參數(shù)的不確定性，該不確定性是由給定值的誤差或者參數(shù)隨時間而在參數(shù)中產生未建模的變動的誤差而產生的；并且(iii)補償過程模型中未建模的設備動態(tài)的穩(wěn)定噪聲。值得注意的是雖然穩(wěn)定噪聲可能沒有明確定義的物理學的根據，但它卻是需要的從而確保成功的濾波操作。過程噪聲協(xié)方差Q(ti)是半正定的(positive semidefinite)。
測量數(shù)據噪聲表示從傳感器獲得的測量數(shù)據而產生的不確定性。測量數(shù)據噪聲協(xié)方差R(ti)是對所有i來說是正定的(positive definite)。
系統(tǒng)狀態(tài)的最初情況被模擬成具有已知的平均數(shù)和協(xié)方差的高斯隨機向量 卡爾曼濾波器的推導 已知有很多種推導卡爾曼濾波器的方法，包括最小方差法，最大概似法和最大歸納法(MAP)。最佳實施方式使用的是MAP方程式，因為這種方法是測算有效載荷估算值最自然的多重模型參數(shù)估算法。
方程式摘要 最佳狀態(tài)估算及其協(xié)方差從時間點ti-1預測到時間點ti使用 通過預測達到了暫時性的數(shù)據融合效果。
在時間點ti時，將測量數(shù)據z(ti)結合到狀態(tài)估算中，從而為下面方程式提供了的最新狀態(tài)估算及其協(xié)方差 圖6顯示的是上述卡爾曼濾波器方程式的方塊結構圖。
卡爾曼濾波器的擴展 卡爾曼濾波器提供了用離散線性測量對離散時間、線性系統(tǒng)的最佳狀態(tài)估算方法。然而，在某種程度上，所有的系統(tǒng)卻又都是非線性的，并且卡爾曼濾波器無法應用到很多重要的非線性系統(tǒng)上。電動采掘鏟屬于這個范疇是因為支配它們的運動方程式取決于結構和速度。雖然這些動態(tài)是非線性的，但是在某種意義上它們是平滑的，可以在各處求微分。
通常解決“平滑”動態(tài)非線性系統(tǒng)狀態(tài)估算問題的方法就是所謂的卡爾曼濾波器擴展或EKF。本質上，EKF涉及通過使非線性狀態(tài)以及關于狀態(tài)軌跡的測量方程式線性化，而將線性方法應用到非線性問題上。EKF之所以大受歡迎是因為它的概念很簡單，并且對于一個給定的非線性系統(tǒng)，按照所測量的最小均方誤差，它提供了最佳線性估算方法，因此，從這個角度上說，它是最理想的方案。
上述卡爾曼濾波器方程式應用于離散時間系統(tǒng)。當非線性動態(tài)系統(tǒng)是連續(xù)的，通常采用連續(xù)-離散混合的過程模型。非線性連續(xù)動態(tài)通常是通過應用標準常微分方程解算器例如Runge-Kutta方法而用于預測步驟。然后，校正步驟通過使用被線性化的動態(tài)的離散形式得以完成。這就形成了EKF的一個變異形式，稱之為連續(xù)-離散擴展的卡爾曼濾波器(CDEKF)。
CDEKF可以由線性化參數(shù)和卡爾曼濾波器算法推導得出。通過使用泰勒級數(shù)展開而使狀態(tài)和觀測方程式線性化可以明確推導出該濾波器。
狀態(tài)和測量方程式假定是如下的形式。
狀態(tài)和測量方程式 z(ti)＝h[x(ti)]+v(ti). (17) 非線性系統(tǒng)方程式非線性測量方程式 傳導方程式 校正方程式 其中 Φ(ti，ti-1)＝exp(F(ti-1)Δt)，(25) Δt＝ti-ti-1. (28) 很明顯，EKF與卡爾曼濾波器算法非常的類似，將它們各自的線性當量替換到傳遞和測量矩陣。
●EKF不估算狀態(tài)，但是從最初結構估算狀態(tài)的擾動量。通過將估算出的狀態(tài)擾動量加到最初狀態(tài)上，這是很容易做到的。
●EKF需要從近似狀態(tài)信息中計算出的被線性化的模型。這就加強了對初始時精確的濾波器的需求，以確保這個線性化模型是對動力學的有效描述。
●不像卡爾曼濾波器，卡爾曼增益和狀態(tài)協(xié)方差不會收斂于穩(wěn)定狀態(tài)。此外，它還必須要不斷地再估算雅可比行列式。
●如果預測狀態(tài)軌跡與實際狀態(tài)軌跡偏離太遠，那么實際協(xié)方差也會遠遠大于估算協(xié)方差。
●如果狀態(tài)或測量方程式高度非線性而且后期密度是非高斯的，那么EKF估算值會嚴重出錯。
盡管有以上缺陷，在很多應用中EKF還是可以獲得比較好的效果。
多重模型的適應性評估 卡爾曼濾波器和擴展的卡爾曼濾波器(EKF)通過用測量模型結果平衡確定性動態(tài)過程模型影響的方式建立了確定系統(tǒng)動力學狀態(tài)的方法。當目的是估算有效載荷質量(和其它可能的系統(tǒng)參數(shù))時，采用狀態(tài)估算的最主要原因是使用狀態(tài)估算來能暗中補償由機械運動產生的慣性負載。
通過多重模型的適應性評估(MMAE)，卡爾曼濾波器系統(tǒng)同時自然地擴展到系統(tǒng)狀態(tài)和參數(shù)的評估。這個方法假定待估算的參數(shù)是常數(shù)或者分段常數(shù)。
一般地，系統(tǒng)參數(shù)是可以在最大和最小值之間連續(xù)變化的。電動采掘鏟的鏟斗的有效載荷質量可以是零(鏟斗為空)和最大裝載能力(與鏟斗滿載相關)之間的任意值。多重模型方法假定一個感興趣的參數(shù)，這里指有效載荷質量為一個數(shù)r，在這些最大和最小值之間的離散值。表示如下 這個設想是用于假定的r重系統(tǒng)模型，使用卡爾曼濾波器估算每一個r值對應的模型狀態(tài)，并從每一個模型對應濾波器產生的新序列的統(tǒng)計特性里確定模型是準確的概率。隨著時間的推移，最準確的模型會有最大的概率，而不夠準確的模型會呈現(xiàn)較低概率。雖然在上面的討論中我們是以有效載荷質量舉例，但原則上我們可以將這種方法運用于任何參數(shù)的組合，例如使用有效載荷質量和有效載荷質心來定義模型。
優(yōu)選實施例使用MAP方法來獲得系統(tǒng)參數(shù)的最優(yōu)估算。然而，對于狀態(tài)估算，我們要通過已給出的測量序列Z(ti)嘗試找出最有可能的狀態(tài)估算值x(ti)，下面我們尋求已給出Z(ti)而確定最有可能的模型Mj。
通過給出測量序列Z(ti)，模型正確的概率能表現(xiàn)成如下的遞歸形式， 在上面的公式中我們可以使用貝葉斯法則(Bayes’rule)和邊緣化。如果我們使用下面的記數(shù)法， μj(ti)＝P{Mj|Z(ti)}， μj(ti-1)＝P{Mj|Z(ti-1)}， 那么公式30可以被改寫為 值得注意的是如果我們有r重模型，我們就有了這些方程式中的r。當有更多信息可用的時候，每一個模型概率會隨時間發(fā)生變化。初始條件是 P(Mj|Z0)＝μj(0)， 這里μj(0)是關于模型j是正確時的先驗概率。注意模型概率是遵守標準化條件的。
公式31是一個混合概率密度函數(shù)(PDF)，同時具有離散和連續(xù)概率的函數(shù)(PDFs)。觀察這個表達式內的個體項，我們可以看到給出到前一時間步驟的測量序列時，μj(ti-1)是模型j準確的概率。概率分布f(z(ti)|Mj，z(ti-1))可以被表示為模型Mj，如下 在上面的方程式內，通過假定M＝Mj，余量rj(ti)及其協(xié)方差Aj(ti)都被確定下來。
然后參數(shù)M的估算
可以作為所有個體參數(shù)概率的加權總和，也就是 從而可以推導出組合狀態(tài)估算的類似公式 以及狀態(tài)估算協(xié)方差的公式 這是每一個模型加權概率的總和，包含了兩個部分。第j-重模型的估算值和外積的方差的產生是由于
并不同于
本質上來說，優(yōu)選實施例的多重模型方法正如附圖7中70所示，包含運行多個獨立的r重卡爾曼濾波器，例如71，每一個卡爾曼濾波器有獨特的模型。然后，剩余額、剩余協(xié)方差以及先驗模型概率就被用來確定當前模型概率72。這個新的概率再進行加權求和73，從而確定當前參數(shù)估算值。關于MMAE算法的更多細節(jié)的詳細內容在圖8中示出。
算法說明附注 1.初始步驟確定了狀態(tài)估算和狀態(tài)協(xié)方差的初始條件。這對所有的模型來說都是相同的。還確定了每一個參數(shù)的先驗概率。如果沒有預先信息可作用于參數(shù)概率，則概率分布為平直，且所有原始概率等于1/r。
2.主循環(huán)(main for-loop)始于擺動的起點并終于擺動的終點。實際上參數(shù)估算的起始時間是由周期分解算法確定的。
3.在第二次循環(huán)(second for-loop)中，各個模型對應的卡爾曼濾波器啟用。它為每個模型獨立更新狀態(tài)估算和狀態(tài)協(xié)方差。
4.狀態(tài)預估計值的積分可以使用四階龍格庫塔法(4th orderRunge-Kutta routine)或類似的常微分方程解算器求得。
當?shù)诙窝h(huán)(for-loop)完成時，模型概率即被更新且新的參數(shù)估算也隨之確定。
機械結構與坐標系 圖9顯示的是用于描述P&H級電動采掘電鏟的結構的參數(shù)(長度和角度)以及用于描述這些機械主要移動部件相對位置的坐標系。標記為l的長度和標記為φ的角度設計為固定數(shù)值；標記為d的長度和標記為θ的角度是隨機器運動的變量。
描述電鏟主要部件的相對位置需要4個坐標系。世界坐標系用O0x0y0z0表示；O1x1y1z1-坐標系嵌入機器機架上；O2x2y2z2-是鞍座上的坐標系；O3x3y3z3-是鏟斗上的坐標系。所有固定于機身的坐標系的x-軸和z-軸都在機器機架的徑向平面內，即該平面平行于圖9所示的包括擺動坐標軸的投影面。所有坐標系的y-軸都處于這個平面的法線方向上。
4x4齊次坐標變換矩陣可以用來描述坐標系之間的關系。我們標記一個矩陣，該矩陣通過Di→j描述了從Oixiyizi到Ojxjyjzj的轉換，并記錄矩陣映射(齊次)點從j·坐標系到i-坐標系的動作。例如說，已知固定于鏟斗上的坐標系O3x3y3z3內一點p的位置，那么就可以從下面的公式得到其在O0x0y0z0坐標系內的坐標值。
p′＝D0→3p Di→j的結構是 其中Ri→j是3×3旋轉矩陣，ti→j是一個三維平移矢量。
4x4齊次變換矩陣根據下式進行交換 Dj→k＝Di→jDj→k. 當電鏟軌跡鎖定時，世界坐標系O0x0y0z0提供了一個慣性參考坐標系。這個坐標系的原點定位于軌跡上表面和機架下表面之間的接觸面。z0-軸與擺動軸共線。x0-軸指向履帶軌跡的前進方向，用右手旋轉法即可確定這個三面坐標系的y0-軸。
坐標系O1x1y1z1的原點O1與O0重合，而且z1與z0共線。當θ1＝0時，O0x0y0z0坐標系即與O1x1y1z1坐標系一致。俯視時，正角θ1對應的是機架相對于軌跡做逆時針方向旋轉的角度。齊次變換矩陣D0→1給定為 O2x2y2z2坐標系固定于鞍座，其原點O2位于鞍座的回轉中心。當θ2等于0時，O2x2y2z2的坐標方向平行于O1x1y1z1的坐標方向。描述剛體從坐標系1移動到體系2的位移矩陣給定為 其中，設計參數(shù)l1和φ1如圖9所示。
O3x3y3z3的原點O3是按照下面的方法定位的。鞍座角(θ2)被設置為90度，從而使得手柄呈水平位置。然后移動手柄以使起重鋼絲繩垂直下降(θ5＝90度，θ6＝0度)。手柄齒條的節(jié)線與起重鋼絲繩的交點即為原點O3；z3與鋼絲繩共線；x3平行于手柄齒條的節(jié)線。注意x2坐標軸與x3坐標軸正交。描述剛體位移D2→3的位移矩陣表現(xiàn)為 上述方程式相乘得出O3x3y3z3坐標系相對于O0x0y0z0坐標系的位置，也就是 吊鉤銷釘位置 鏟斗經受三個參數(shù)運動，所以在鏟斗上選取一確定點的方法非常有用，該確定點的空間位置可用于確定O3x3y3z3坐標系的位置。為此我們使用位于O3x3y3z3坐標系內的b3＝(0，0，l4，1)T的吊鉤銷釘，那么O0x0y0z0坐標系內吊鉤銷釘?shù)淖鴺耸? 約束方程 擺動角θ1，樞軸角θ2和裝載延長量d3是確定固定在機架上的坐標系相對于世界坐標系的位移和旋轉量的三個參數(shù)。我們給這些結構變量建組 θ＝(θ1，θ2，d3)T. (40) 擺動電動機的角位移θs，裝載電動機角位移θc，起重電動機角位移θh，也可以類似建組 ψ＝(θs，θc，θh)T.(41) θ的值能確定ψ的值，反之亦然。這些映射不是雙射的，可是，在這些變量的物理工作量程內，它們卻是一一對應的。需要注意的是，θ或是ψ的規(guī)格都能決定起重鋼絲繩的傾角，即如圖9所示的，標記為θ5的第七個變量。
為了建立約束方程式，通過傳動比先標記有關θ1和d3的從屬坐標θs和θc 其中，Gs是擺動驅動器的傳動比，Gc是裝載驅動器的傳動比。
將θ2和θ5關聯(lián)到θc和θh的約束方程可使用圖10所示的向量回線建立。為了簡化符號，我們研究一個映射到物理的(x1，z1)-平面的復平面，其實軸與x1軸共線，其虛軸與z1軸共線。
對圖10中的向量回線中的向量求和，其中向量zi，可以表示為一個復變數(shù) 0＝ζ(θ，ψ，θ5)＝z1+z2+z3+z4+z5-z6-z7， 還可以展開為 其中，變量li、di、θi和φi正如圖10中所定義的。然后，使用下面的經觀察得出的關系式 θ4＝θ2， (46) 公式44可以被轉寫為 為了從公式內消除ds，更方便的方法是我們先引入變量dh。如圖10所示，當起重鋼絲繩垂直懸掛時(即(ie.θ5＝90deg)，dh即為吊鉤銷釘?shù)狡鹬貦C槽輪向外緣線的距離。它通過以下方程式與起重機電動機角位移關聯(lián) 其中Gh是提升傳動比。上面的表達式和公式45就可以將ds和dh聯(lián)系起來 公式49右邊最后一項表示的是起重機鋼絲繩纏繞槽輪的角度。將公式49代入公式47得出 取公式50的實數(shù)和虛數(shù)部分將θh和θ5與廣義坐標聯(lián)系起來 (53) 注意公式51中使用了如下三角函數(shù)關系。
合并公式42和公式51得到 (57) 所有有效的結構都必須滿足公式54。尤其它形成了解決P&H型采掘電鏟的運動學、靜力學和動力學的基本解決方案。
通過牛頓-拉夫遜方法(Newton-Raphson)進行運動學追蹤 運動學追蹤問題就是給出ψ確定θ和θ5的值或者給出θ確定ψ和θ5的值。我們稱第一個問題為向前動態(tài)追蹤，第二個問題為逆向動態(tài)追蹤。為了在標記上方便區(qū)分這兩個問題，我們標記如下 θF＝(θ1，θ2，θ3，θ5) ψF＝(θs，θc，θh) 當工作于向前動態(tài)追蹤領域時，為 θ1＝(θ1，θ2，θ3) ψI＝(θc，θc，θh，θ5) 這二者的區(qū)別就是θ5分組不同。兩個問題都需要用數(shù)學方法解決非線性約束方程。我們選擇重復使用多變量牛頓方法來解決這個問題。在表述牛頓解決方案過程中發(fā)展出的雅可比行列式矩陣(Jacobian matrices)用以將電動機慣量歸諸于結構變量，同時也可以解決靜力學問題。
向前動態(tài)追蹤 將泰勒級數(shù)展開應用于公式54 目的是找到一個有效結構，也就是Γ(θF+ΔθF，ψF+ΔψF)＝0 由此得出 公式58得出迭代方程式 且 下面介紹的算法，可以用于解決P&H級電鏟的動態(tài)追蹤問題。這種算法利用當前電動機位置
并使用公式59和公式60來算出符合約束方程式的
新值。為達到可靠的收斂，這種算法需要一個好的
初始值。實際上，通過初始化如圖10所示的具有良好定義的機構可以實現(xiàn)上述目標，其中向前動態(tài)追蹤可以使用三角學明確解決。我們注意到，使用公式39，從θ角可以很容易得出吊鉤銷釘?shù)奈恢谩?br>
算法3用牛頓法向前動態(tài)追蹤 輸入當前電動機位置
輸出符合約束方程的結構變量
的值。
已知在先電動機位置和結構變量
設定初值 反復迭代直至收斂 寫出雅可比行列式矩陣 令求這個表達式的值 且 γ1＝d3cos(θ5-θ2)+(l2+l4)sin(θ5-θ2) 注意這個矩陣將電動機速度與結構變量的速度聯(lián)系了起來。
逆向動態(tài)追蹤推導出公式59和公式60的參數(shù)同樣可用以生成一個求逆向動態(tài)追蹤的迭代方程 其中
的表達式和的乘積見公式69 算法4牛頓法逆向動態(tài)追蹤 輸入當前結構變量
輸出當前電動機位置
已知先前電動機位置和結構變量
設定初值 反復迭代直至收斂
P&H級電動采掘鏟之剛體動力學 優(yōu)選實施例通過運用一系列平行的卡爾曼濾波器來估算電鏟狀態(tài)，因此它提供了估算采掘電鏟有效載荷的框架。每一個模型都通過過程模型假定了有效載荷質量(和其它任何可能的參數(shù))，并且通過使用濾波器剩余誤差的統(tǒng)計特性即可計算出每一個假定值為真實的概率。而“真實”有效載荷也可以通過濾波器模型定義的離散分布的預期值得出。
在學術上有一種被廣泛接受的論點，即過程模型應簡化，盡可能簡單，但不是特別簡單，以期使總體模型誤差最小化。給定可用的測量數(shù)據集，則優(yōu)選實施例使用的過程模型僅僅模擬與機器剛體運動相關的動力學狀態(tài)。拉格朗日方法(Lagrangian methods)被用來導出描述剛體動力學的運動方程，并且開發(fā)出一個計算剛體運動線性化的框架，此處的剛體運動是關于任意狀態(tài)軌跡，且需要執(zhí)行拓展卡爾曼濾波器(EKF)。
過程模型結構 鑒于用于有效載荷估算的過程模型是為了分析機器動力學的影響因素，因此很有必要直接且精確地測量模型狀態(tài)及其輸入。直觀上，這將導致不可測量狀態(tài)信息的最小離散。運行正確的有效載荷假設的卡爾曼濾波器會得出狀態(tài)估算值，該狀態(tài)估算值要精確于傳感器的測量值。并且直觀地，卡爾曼濾波器，這，反過來，又將得出更準確的有效載荷估算。
要測量(而不是估算)盡可能多的過程模型狀態(tài)的原理基于以下兩個過程模型(i)一個模型包含電動機的電動力學以及機架、鞍座、手柄和鏟斗的剛體動力學，以電樞電壓為輸入，以電樞電流和位置為測量值，并在卡爾曼濾波器結構內估計電動機速度；或者(ii)一個模型僅包含機架、鞍座、手柄和鏟斗的剛體動力學，以電動機電流為輸入，電動機位置和電樞電壓是測量值。一種可能的第三種可選的過程模型包含了控制器動力學，電力動力學，而剛體動力學卻因比上文定義的兩個模型復雜而被排除了，同時這個模型需要使用PI和PID控制器的內部狀態(tài)，實際上，這種狀態(tài)很難測量。
選擇第二種模型是因為它具有較少的狀態(tài)(簡單)并且依據公式70，電樞電壓和電樞電流還可以用來獲得電動機速度估算值。當然，對于模型(i)來說也是如此，可是，電動機動力學在這過程模型和測量方程中都重復存在，以致沒有明顯優(yōu)勢。
使用這個模型，需要決定哪些變量作為狀態(tài)使用。電動機位置和速度或者結構變量θ以及它們的導數(shù)
都是明顯的待選參數(shù)。這些變量通過約束方程在運動學上相互聯(lián)系。一般普通變量例如慣性、運動、各種力都可選擇?？梢灾獣?，如果我們選擇結構變量以及它們的速度作為狀態(tài)，那么方程式會較為簡單。
剛體動力學 動態(tài)模型用三個連續(xù)連接的剛體表現(xiàn)電鏟。這些部分分別為(i)機架和懸臂，(ii)鞍座，和(iii)鏟斗和手柄。每一部分都由一個電動機驅動，該電動機在傳輸過程中會產生各自的慣性。連接機身與電動機的耦合部件假定為剛體。這三個質量系統(tǒng)可以由結構坐標θ及其先期定義好的導數(shù)來表示。
拉格朗日方程結合電鏟運動幾何學的理論旋量描述法可以很高水平地描述動力學。拉格朗日方程式是建立在虛功原理基礎上的。
通過裝載件和起重機驅動之間的連接部件來推導出描述剛體動力學方程是很復雜的，這兩者都是直接作用于手柄-鏟斗裝置的。之前推導出的約束方程表現(xiàn)了結構變量和電動機位置的固有關系(即，Γ(θI，ψI(θI))＝0)。通常，在因變量無法去除的約束系統(tǒng)中，我們就使用拉格朗日乘法來建立運動方程。運用了這種方法后生成的方程式，又無法很容易地應用于卡爾曼濾波器框架中。為了克服這個難點，這里使用嵌入式約束方程來改進運動方程。這種處理就不會改變卡爾曼濾波器的方程形式。
拉格朗日方程可以寫為 拉格朗日函數(shù)(或縮寫成拉格朗日)表達式為L＝T-V，其中T表示總動能，V表示系統(tǒng)總勢能，而τi是廣義坐標θi方向上的總合成力。在這個問題中，系統(tǒng)勢能與速度無關。所以公式71又可以寫為 其中 總動能等于包括最先和最后的剛體動能和電動機動能以及傳輸慣量之和。
在上面的方程中，
是描述涉及到結構坐標θ的機架、懸臂、鞍座和手柄及鏟斗裝置慣量的3×3矩陣。我們將與這些慣量有關的動能稱為外部機身動能，因為它與機器外部機身的動能有關。
是描述涉及因變量
的電動機轉子慣量和傳輸器件慣量的4×4矩陣。我們將與這些慣量有關的動能成分稱為內部動能。
外部動能 外部動能聚集了與機架、懸臂、鞍座、手柄、鏟斗、吊鉤銷釘、起重機鋼絲繩和擺動、裝載部件以及起重機驅動器的動力學有關的能量。為了方程式的演化發(fā)展，機架和懸臂、鞍座以及手柄和鏟斗被視為三個剛體。為了減小動力學的復雜程度，并符合模型精簡的原則，我們還做了如下的假定 1.起重機鋼絲繩是不能伸展的， 2.起重機鋼絲繩是無質量的。
盡管沒有傾斜信息，O0x0y0z0坐標系中的重力向量的方向我們并未在方程式中進行假定，我們認為重力方向與z0軸一致。
為建立外部動能表達式，需要運用理論旋量公式化。規(guī)范化地描述一個無限小的剛體對應于一條空間直線的旋轉以及對應于該直線平移運動，這種觀測是公式化的基礎。也就是說，大多數(shù)剛體常規(guī)的瞬時運動是一種空間扭轉。如果這種運動瞬時對應于一種平移，則該剛體扭轉所圍繞的直線(螺旋軸)是在無窮遠平面內。那么，所有其它運動，對應的螺旋軸是在仿射空間內的直線。扭轉運動可以簡明地用普呂克(Plucker)坐標的一個6次矢量表示，用以描述平移和旋轉速度。
其中 ω＝(ωx，ωy，ωz)T， v＝(vx，vy，vz)T， 分別是剛體在同一個參照坐標系內的旋轉速度和平移速度。角速度ω是一個自由矢量，平移速度v可以認為是剛體在參考坐標系原點處一點(可能是一個假設點)的瞬時速度。
在機器內，剛體間的相對運動通常被物理連接限制，該物理連接允許為只有一個相對自由度。描述這種相對運動的普呂克坐標(Pluckercoordinates)可以寫成 其中θj是連接處的相對速度，si是該連接旋轉運動的標準普呂克坐標。慣例是通過量化表示旋轉和螺旋運動的角速度ω幅值和表示平移運動的v幅值來劃分每個要素以標準化
，這樣，連接旋量作為描述連接機構的瞬時運動的基礎。只要所有運動表示在同一參考體系中，則以通常的方式加入相對速度。例如，EMS的三個外部剛體的速度為 其中，
分別是結構參數(shù)的速度的導數(shù)，s1、s2、s3是在O0x0y0z0坐標系上的三個自由度上的連接旋量，其普呂克坐標為 s1＝(0，0，1；0，0，0)T s2＝(sinθ1，-cosθ1，0；l1sinφ1cosθ1，l1sinφ1sinθ1，-l1cosφ1)T(77) s3＝(0，0，0；cosθ1sinθ2，sinθ1sinθ2，cosθ2)T 剛體隨著慣性坐標系中普呂克矢量瞬時扭轉產生的動能為 其中m為剛體質量，J是慣性參考系中剛體的3×3旋轉慣性矩陣 并且C是在慣性坐標系中剛體質心矢量c＝(cx，cy，cz)T的反對稱形式 注意的是，C的反對稱形式跟3-維向量交叉乘積，該3-維向量放在乘號后面。
公式78可以簡化如下 其中6×6矩陣Nb是一個慣性矩陣。在剛體固定參考系b中，慣性矩陣是最容易表達的。慣量從一個剛體固定參考系(可能是移動的)到另一個坐標系，舉例來說，慣性坐標系的轉換可以使用以下方程來獲得 其中H0→b是6×6旋量轉換矩陣，其結構如下 矩陣R0→b是一個3×3旋轉矩陣，T是平移矢量t0→b的反對稱形式。
因此由公式76和公式81得到 其中為了表示方便由
替代
根據結構變量θ的導數(shù)來書寫時，可以得出 其中 且l＝max(i，j)。這個表達式是形式符合經由符號分析處理器后的擴展。必要條件(i)是固定于參考系的剛體慣量值；(ii)對于P&H級采掘電鏟，改變公式81的形式；(iii)通過公式77給出連接旋量。
內部動能 內部動能通過下式得出 其中 這里的Js是擺動電樞慣量和涉及轉子軸的擺動傳輸慣量；Jc是裝載電樞慣量和涉及轉子軸的傳輸慣量；Jh是起重機電樞慣量和涉及轉子軸的起重機傳輸慣量。我們期望以結構變量θ來表達內部動能。
約束方程相對于時間的導數(shù)為 在上面的方程中
是一個4×4矩陣，
是一個4×3矩陣。如果矩陣
滿秩，則上面的表達式可變形為 其中 我們稱Ji(θI，ψI)為雅可比約束(constraint Jacobian)。它將結構變量的速度與它們的因變量聯(lián)系起來。
因此，結構變量的內部動能表達式為 再來將表達式轉化為容易的符號求值形式。總動能是 其中 用求和符號，我們可以將公式90改寫為 由于Ji(θI，ψI)的引入，慣量項aij同時是獨立和相關坐標的函數(shù)，但是與時間無關。為簡化符號，Ji(θI，ψI)在θI和ψI上的相關性就不再表示了。
拉格朗日方程式估算 估算拉格朗日方程式左邊的導數(shù)是必要的。由于動能和勢能是相互獨立的，所以我們需要分別計算它們對于拉格朗日方程的影響。
運用公式91，公式72左邊第一項轉變?yōu)? 運用鏈式法則，aij關于時間的導數(shù)是 運用公式88，我們可以從上式中消除
可選擇地，我們也可以寫成， 這里JI，lk是即JI的第(l，k)元素。于是公式92又可表達為 現(xiàn)在考慮拉格朗日方程式左邊第二項，我們得到 將鏈式法則運用于這個方程式的偏微分項上，得到 因變量
的時間導數(shù)可以表示為 使用方程式24等同上面的表達式得到 然后使用方程式98從方程式97中消除
得到 由于廣義坐標是獨立的，所以有 使用上面的表達式，方程式99轉變?yōu)? 允許方程式96變形為 使用方程式95和方程式101，動能對于拉格朗日方程的影響為 其中 重力導致的總合成力 理論旋量方法還可以用于描述作用于一個物體上的力和力矩的合成作用。更特別地，作用于物體的任何力系都可以被簡化為作用于沿空間某條唯一直線的力和作用于這條直線的力矩。這一對力F和力矩C被認為是力旋量，以6次向量形式存在 W＝(C；F)T 正如旋量提供了一個無窮小剛體運動的規(guī)范化描述，力旋量則規(guī)范化地描述了作用于一個物體的合力。
對于電鏟模型的每一個部分都存在重力強加的力旋量。假設N是慣性坐標系中一個剛體的慣性矩陣。由于重力作用這個剛體的力旋量為 G＝Ng(104) 其中g是描述重力加速度的6維向量。在圖9中，慣性坐標系的z軸方向與重力加速度方向一致。
其中 g＝(0，0，0；0，0，g)T (105) 且g＝-9.81m/s2 假設剛體約束于關于連接旋量s的運動，并且令剛體關于螺旋軸的旋轉角θ為剛體的廣義坐標。由重力產生的合力中的一部分τg，可以從重力力旋量和連接旋量的內積得出，也就是 τg＝Ws 作用于電鏟模型的每一個剛體的重力力旋量為 或綜合為 引用Gi至廣義坐標中，我們將每一個重力力旋量和連接旋量配對，就是 如果電鏟是傾斜的，則單位矢量n＝(nx，ny，nz)就定義了重力在O0x0y0z0坐標系中的方向 g＝(0，0，0；gnx，gny，gnz)T (107) 公式的其他部分還是一樣的。在實際運用中，方向余弦n可以由傾斜計測量得到。在本例中，r是3維矢量。
執(zhí)行機構合成力 令τ為執(zhí)行機構合成力矢量，也就是 τ＝(τ1，τ2，τ3)T (109) 這些合成力是由電動機凈力矩產生的 T＝(Ts，Tc，Th，0)T. (110) 其中Ts是擺動驅動產生的凈力矩；Tc是裝載驅動產生的凈力矩；Th是起重機驅動產生的凈力矩。這些轉矩是與驅動器在傳輸過程中的減少損失量相關的電磁轉矩。這些轉矩作用于因變坐標ψ。注意方程式110中T的最后的要素，該要素與作用于θ5上的轉矩相符；當在那個方向上沒有轉矩時，該值設為0。
這就要求應用雅可比約束和虛功原理來將T與τ聯(lián)系起來。電動機凈轉矩開始工作時的速率是由T和
的內積得出的，根據定義，這些約束條件并無作用，還必須結合 代入方程式88，則得出了電動機凈轉矩與合成轉矩的關系 因此得出結論 方程式108和方程式112之和等于零，定義了涉及廣義坐標θ的P&H級采掘電鏟的靜態(tài)方程。
阻尼損失 我們可以將粘性阻尼損失合并進運動方程中以因變量的時間導數(shù)形式表示如下 其中
表示電動機阻尼和連接機構阻尼。于是 其中 剛體動力學概要 總的來說，控制機器剛體運動方程式可以表示為 或者以矩陣形式表示為 其中 且 注意，對于這個模型的輸入是凈合成力矩τ，并且模型狀態(tài)是廣義坐標θ及其導數(shù)
動力學線性化 EKF算法要求關于狀態(tài)軌跡的動力學模型在離散時間內為線性化形式。發(fā)展該線性化的方法的第一步是確定關于狀態(tài)軌跡的連續(xù)時間線性化 然后按照CDEKF算法的要求將其轉化為離散時間，也就是 x(ti)＝Φ(ti，ti-1)x(ti-1)+Γ(ti-1)u(ti-1) (119) z(ti)＝H(ti)x(ti). (120) 這一節(jié)發(fā)展該線性化。首先重新整理方程式115，于是我們將剛體動力學表達為 方程式121也有之前描述過的形式，也就是 且 uT＝(T，g).(124) 注意輸入量是T、電動機轉矩和重力加速度常數(shù)g。
系統(tǒng)矩陣線性化 將方程式121對狀態(tài)矢量取偏微分，得出以下線性化系統(tǒng)矩陣F的模塊形式 求方程式121關于結構變量θk，k＝1，2，3的微分可以得出子模塊
的表達式如下 (126) 其中使用了下面的關系式 逐元求A關于θk的微分，得到 用類似的方法，我們可以得出B、C、τ和r的偏微分分量 由于A，B和C不隨
而變化，
關于
的的偏導數(shù)為 導數(shù)
和
并無意義。
離散時間狀態(tài)轉移矩陣可以表達如下 Φ(ti，ti-1)＝exp(F(ti-1)Δt)， (133) 分布矩陣線性化 線性化分布矩陣輸入G可以類似得出 從方程式112和方程式108很容易獲得導數(shù)
和
離散時間輸入滿足 其中Γ的近似值為 Γ＝BΔt. 注意應用CDEKF法對分布矩陣并無要求。
測量方程 有用的測量值是電動機位置和電動機電樞電壓和電流。我們期望將這些測量值與狀態(tài)參數(shù)相聯(lián)系。這種聯(lián)系由之前提到的約束方程所支配。
我們假定無法直接測量電動機速度，但是，我們可以通過下面的控制直流驅動器的電力動力學方程來獲得電動機速度。電力動力學可以用矩陣形式表達 或者 這個表達式還可以根據電動機速度重新整理為 由于直流驅動器內含有很大的電感，所以乘積
在電流急劇變化時具有重要意義。電樞電流變化率無法直接測量，但是它可以通過高通濾波器電流估算得出。另一方案是可以使用卡爾曼濾波器結構內的電動機模型來估算電動機速度。
狀態(tài)參數(shù)
通過下式與
相聯(lián)系 其中Jdr按之前定義的方式獲得 且 γ1＝d3cos(θ2-θ5)-(l2+l4)sin(θ2-θ5). 測量噪音協(xié)方差 電動機位置、電樞電壓和電樞電流的測量誤差可以認為是相互獨立的，并且可以假定其平均值為零，且為高斯分布。這就在有效的測量數(shù)據上強加了一個協(xié)方差結構，該有效測量數(shù)據可以由方程式135和方程式136確定。
我們先考慮在求導電動機速度時的誤差協(xié)方差。其組成項為電樞電流、電流速率和電樞電壓的測量誤差。這些誤差由下式得到 零平均值高斯分布隨機變量和就是指一個零平均值高斯分布隨機變量協(xié)方差就等于其各組成部分協(xié)方差之和，這個理論已被廣泛承認。因此，電動機速度估算協(xié)方差就是 因為各構成矩陣都是對角線矩陣，所以
也是一個對角線矩陣。
假定電動機位置的測量誤差值是一個高斯零平均值，其協(xié)方差為 因此系統(tǒng)狀態(tài)測量y的誤差協(xié)方差為 這個矩陣不是對角線矩陣。非對角線項顯示的是裝載部件和起重機的連接。注意，因為Jdr是與結構關聯(lián)的，因此R也是結構關聯(lián)的。
實施附注 實際執(zhí)行過程中需要將運動方程及其線性化展開為可計算的形式。在一個實施例中，使用MATLAB符號數(shù)學工具箱來實現(xiàn)實際的執(zhí)行。這種方法始于轉換矩陣和約束方程，然后將它們展開形成方程式115和方程式125中的項。以上得到的方程式然后被寫成MATLAB“mfiles”文件和C語言代碼(“c-code”)片段。MATLAB代碼可用于離線開發(fā)和調試，而C代碼片段可在MATLAB內部被格式化從而直接插入實時系統(tǒng)實施方法的編碼基數(shù)中。
通過符號展開方式得到的單個元素項的表達式很長且麻煩，還包含重復的表達式，所以建議一種更有效率的計算實現(xiàn)方法相比較直接在MATLAB在下通過符號展開引出更為合理。雖然是很重的計算負擔，但是并不會沖擊現(xiàn)代計算機系統(tǒng)的實時計算能力，即便是在一個很大的過濾帶上(50個模型)以100Hz以上的頻率計算。
實時計算工具是完全由C語言編寫的，在QNX實時操作系統(tǒng)中運行。硬件平臺是基于PC-104格式，擁有1GHz奔騰處理器。系統(tǒng)配置數(shù)據采集卡來獲取上文描述的電動驅動器信號。有效載荷估算通常是在50個模型的過濾帶上以100Hz的頻率運行。在這種配置下，CPU被加載了大約一半的負荷。CPU在計算有效載荷時的工作內容包括了數(shù)據采集，數(shù)據記錄和其它一些必要的進程例如運行計算程序來檢測電鏟的運行變化，例如，電鏟循環(huán)階段的“鏟斗滿載擺動”的開始和結束。
MATLAB和C語言程序都只是在初期階段執(zhí)行MMAE算法。矩陣計算才是所有卡爾曼濾波器執(zhí)行工具的核心部分。MATLAB程序使用的是語言自帶的矩陣計算能力。而C語言程序使用MESCHACH公眾域矩陣庫。在這兩種程序中，運動方程的積分計算，如MMAE所要求的都要使用4階、固定步長的、Runge-Kutta、ODE積分器。與早期仿真中的可變步長相比較，可以發(fā)現(xiàn)早期仿真中與這種解析器有關的附加復雜性和計算成本是無法保證的。使用MATLAB或MESCHACH中的矩陣指數(shù)函數(shù)可以恰當?shù)貙⑦B續(xù)動力學轉化為離散形式，就像方程式133。
電動機作為轉矩的來源建立模型，其輸出由磁場和例如根據上文描述過的特性產生的電樞電流決定。在軟件中使用查詢表在網格點之間用線性插值來表現(xiàn)這些特性。電鏟生產廠家通過試驗生成了這些特性的數(shù)據。
電鏟生產廠家還提供諸如幾何尺寸、慣量、齒輪傳動比等需要的數(shù)據。尺寸信息和齒輪傳動比在制造圖紙中標出。各組成結構的質量和慣量可以從機器的CAD模型中獲得。
使用靜力和力矩平衡遞歸最小平方有效載荷估算 上文描述的靜力和力矩平衡可以重新整理為有效載荷質量mp的形式，如下所示 綜上所得 akmp＝bk(138) 因為方程式138的n個測量值可以給出2n方程式，排列如下 或者 amp＝b 方程式138有最小平方解 可以通過遞歸求解 且 Kk＝Pkak
是mp基于k個測量值的最小平方估算。在算法78中，方程式140是利用靜力和力矩平衡遞歸估算有效載荷的最小平方算法的基礎。
算法5靜力和力矩方程遞歸最小平方法估算有效載荷 設定初值k＝0 用于(for)擺動時間工作(do) 等待(定時中斷) 讀取(ψF，k，Tk) 使用算法3.3.1由ψF，k求解θF，K 使用方程式140計算有效載荷估算
也就是 k＝k+1 靜態(tài)有效載荷估算與動態(tài)有效載荷估算之比較 比較算法中所使用的方法如下所述 ◆方程式115給出的P&H4100電鏟總體模型參數(shù)是由P&H提供的數(shù)據建立的。該數(shù)據包括了尺寸、質量和在設計機器期間開發(fā)的P&H立體幾何模型(CAD)以及生產機器的生產圖紙中得到的慣量。電動驅動器的性質包括了電阻、電感、轉矩和速度特性，同樣也是由P&H提供的試驗特征數(shù)據。
◆參數(shù)組成動力學模型在接近每一個電動機位置處用比例微分(PD)環(huán)進行放大。。當有效載荷質量為零時，這些回路被調諧為對結構運動的每一個坐標軸上的步進輸入都做過阻尼響應。
◆對P&H4100電動采掘鏟的63次擺動運動進行了測量。采集了四個操作人員的數(shù)據來形成‘操作類型’庫。
◆被測變量列表如下，其中包括了電動機位置。這個數(shù)據是在60Hz的頻率上被采集的。在機器固定狀態(tài)下，我們從這些數(shù)據中隨機選取了電鏟的六個結構作為比較這兩種算法的對照標準。在這些情況下，這兩種算法的基礎方程式都是有效的，并且兩種算法都認為能夠得出精確結果。
◆被記錄的擺動運動和靜止控制位置被用作電動機位置環(huán)的輸入從而生成每一‘負載擺動’軌跡的電動機轉矩數(shù)據和‘機器固定’數(shù)據的時間序列。每一個‘負載擺動’的假設載荷，其取自均勻分布的0-80,000kg范圍，被加到鏟斗上，以便使生成的轉矩數(shù)據中包括了必需的移動鏟斗時的載荷。我們的目標當然就是確定這個有效載荷。
◆計算出的電動機轉矩被當成測量輸入使用。運用這些轉矩求得的電動機位置和速度被當成測量輸出。有效載荷估算值則使用上面介紹的算法從這些數(shù)據中計算得出。
下面的表格1到表格5顯示的是應用MMAE算法以及將靜態(tài)有效載荷估算誤差應用于綜合數(shù)據的結果。MMAE算法的過濾帶是基于30個卡爾曼濾波器。假想有效載荷均有規(guī)律地間隔分布在0-80,000kg的范圍內。
表格1顯示的是控制檢測的結果。如期望地，兩種算法生成的估算值都精確到了鏟斗容量的千分之一。表格1的每一行都符合將執(zhí)行位置固定于PD環(huán)的真實模型中，所以，除了某些初始運動比如電鏟模型在PD控制回路中被固定于穩(wěn)定狀態(tài)，計算出的轉矩正是那些對抗重力作用使得模型固定于穩(wěn)定狀態(tài)的力矩。MMAE算法的偏差更小，然而對于兩種方法來說，交叉于這六個控制機構的標準偏差是相似的。靜態(tài)有效載荷算法的誤差是由模型達成穩(wěn)定位置的初始運動造成的。如果在算法應用之前動力學模型就已達成，那么這個估算方法就沒有誤差。這種由應用算法產生的誤差會被放大，那是因為越復雜的估算過程，其最終估算結果產生于各個計算結果的加權和。
表格1.恒定輸入下的MMAE和靜態(tài)有效載荷估算性能比較 表格2到表格5比較了兩種算法應用于四個不同操作人員的測量的綜合數(shù)據結果。這些表格中需要注意的關鍵點是關于靜態(tài)有效載荷估算的巨大誤差。靜態(tài)有效載荷估算方法看起來系統(tǒng)性地對有效載荷值估算不足。平均誤差范圍大約在12000kg至20000kg。而這些估算誤差的標準偏差范圍大約在3600kg至8000kg。這種范圍的變化表明了靜態(tài)有效載荷估算法易受操作風格影響。通過對鏟斗軌跡的檢查顯示所紀錄的旋轉運動，在面破裂之后到材料傾倒之前，鏟斗明顯降低。在數(shù)據采集期間的臺架的條件就是如此，所以操作人員必須將移動鏟斗使其面朝上，以完成合適的裝填。之后為了向托運卡車傾卸，鏟斗被降低到了某一適當高度。這種凈向下運動就解釋了有效載荷估算不足的原因。在不同的臺架條件下，特別是在精細疏松的傾斜層開采，挖掘礦物，鏟斗通常都先填滿，然后操作人員降低鏟面將鏟斗拔出，因此需要一個凈向上運動，以便將鏟斗向上提升達到卡車傾卸高度。在這種情況下，靜態(tài)有效載荷估算就會過大。
動力學有效載荷估算法在所有的運動軌跡上都取得了顯著的更精確的有效載荷估算值。63個運動軌跡上的最大誤差為229kg，每一個操作人員的平均誤差不超過100kg，并且每個操作人員的標準偏差低于60kg。
就此我們得到的主要結論是伴隨運動產生的慣量和速度負載非常大，我們有必要對其加以考慮。結果更進一步的顯示適應評估的多重模型(MMAE)算法結合剛體動力學就可以考慮這些負載并且準確估算有效載荷。MMAE估算法填補了對專門方法需求的空白，并且有望更勝一籌。
MMAE估算法有望在這項比較中勝出，因為它集成了精密的系統(tǒng)動力學知識。我們根據這些方法回憶，產生轉矩的相同模型和被視為測量值的測量，在MMAE算法中再被用來處理這些轉矩和測量值。注意在靜止條件下，兩種算法都很適用，表現(xiàn)為在系統(tǒng)基礎方程式已知的前提下，兩種方法都能使用。在實際執(zhí)行中，由于未建模的動力學、參數(shù)誤差以及傳輸損失，MMAE估算只是機器動力學的近似認識。
表格2.恒定輸入下MMAE和靜態(tài)有效載荷估算性能比較—由操作員A擺動 表格3.恒定輸入下MMAE和靜態(tài)有效載荷估算性能比較—由操作員B擺動 表格4.恒定輸入下MMAE和靜態(tài)有效載荷估算性能比較—由操作員C擺動 表格5.恒定輸入下MMAE和靜態(tài)有效載荷估算性能比較—由操作員D擺動 前面描述了本發(fā)明的優(yōu)選實施例。在本發(fā)明的范圍內的變動對于本領域技術人員來說是顯而易見的。
權利要求
1.一種估算承重機械的有效載荷的方法，包括以下步驟
(a)建立所述承重機械不同級別的有效載荷的一系列的卡爾曼濾波器近似值；
(b)從所述系列近似值來確定挖掘機當前運行特性的近似值；
(c)利用所述步驟(b)獲得的參數(shù)確定所述承重機械的有效載荷估算值。
2.如權利要求1所述的方法，其中，所述承重機械包括一個電動采掘鏟。
3.如在前的任一權利要求所述的方法，其中，卡爾曼濾波器的輸入包括用于控制機械運動的承重機械驅動控制信號或者可直接或間接測量起重機鋼絲繩和裝載力的同類信號。
4.如在前的任一權利要求所述的方法，其中，所述卡爾曼濾波器的系列近似值模擬所述承重機械上的漸增的有效載荷。
5.如在前的任一權利要求所述的方法，其中，所述步驟(b)還包括以下步驟形成每個模型的當前模型概率，然后對模型加權求和以確定一個總體模型。
6.一種估算承重機械有效載荷的方法，該方法包括以下步驟
(a)確定一系列與機械關聯(lián)的時間變化參數(shù)；
(b)利用機械的操作模型來預計參數(shù)在未來時間的未來狀態(tài)；
(c)確定所預計的未來狀態(tài)和實際未來狀態(tài)間的測量差異；
(d)使用所述差異來改進有效載荷估算值的估算。
7.如權利要求6所述的方法，其中，通過卡爾曼濾波器技術得出所
述測量差異，以減小在所述測量差異確定過程中的噪音影響。
8.一種估算承重機械有效載荷的方法，該方法包括以下步驟
(a)使用至少一個自適應預估模型來預估一系列時間變化參數(shù)的未來狀態(tài)，該參數(shù)與機械剛體動力學方程式相關聯(lián)；
(c)確定所預計的未來狀態(tài)和實際測量狀態(tài)間的測量差異；
(d)使用所述測量差異來適應性地改進每個相應的所述自適應預估模型的模型系數(shù)；
(e)使用每個所述自適應預估模型來獲得所述有效載荷估算值。
9.如權利要求8所述的方法，其中，每個所述自適應預估模型由系統(tǒng)模型直接生成。
10.如權利要求8或9所述的方法，其中，所述模型系數(shù)被改進，以使所述測量差異的變化均方根值最小。
11.如權利要求8到10的任一權利要求所述的方法，其中，多個自適應預估模型被用來預測所述有效載荷估算值。
12.如權利要求11所述的方法，其中，每個所述自適應預估模型是獨立的并且有獨特模型。
13.如權利要求12所述的方法，其中，所述有效載荷估算值由參數(shù)的加權和產生，該參數(shù)由每個所述自適應預估模型預測產生。
14.如權利要求8或13的所述的方法，其中，所述每個自適應預估模型都是卡爾曼濾波器，并且卡爾曼濾波器技術被用于適應性地改進所述模型系數(shù)。
15.一種估算承重機械有效載荷的方法，該方法包括以下步驟使用多模型方法建立統(tǒng)計參考系統(tǒng)指導機械運行，再據此獲得有效載荷估算值。
16.用于承重機械有效載荷估算的系統(tǒng)，所述系統(tǒng)包括
一個或多個輸入傳感器，所述一個或多個輸入傳感器用以測量所述機械的輸入系數(shù)；
處理器，所述處理器用以接收來自所述輸入傳感器的所述輸入系數(shù)；所述處理器還配置成可以評估用以預測與機械剛體動力學方程式相關聯(lián)的一系列時間變化參數(shù)的未來狀態(tài)的一個自適應預估模型，確定所述未來狀態(tài)和所述測量輸入系數(shù)間的測量差異，使用所述測量差異來適應性的改進每個所述自適應預估模型的模型系數(shù)，并且使用每個所述自適應預估模型來產生所述有效載荷估算值。
17.如權利要求16所述的系統(tǒng)，其中，所述處理器設置成可以同時評估用于預測所述參數(shù)的所述未來狀態(tài)的多個自適應預估模型。
18.如權利要求17所述的系統(tǒng)，其中，每個所述自適應預估模型是獨立的且有獨特模型。
19.如權利要求17所述的系統(tǒng)，其中，所述處理器設置成通過計算每個所述自適應預估模型預測所得的參數(shù)的加權和，來使用每個所述自適應預估模型。
20.如權利要求16到19的任一項所述的系統(tǒng)，其中，每個所述自適應預估模型是卡爾曼濾波器，并且所述處理器設置成使用卡爾曼濾波器技術適應性地改進所述濾波器系數(shù)。
全文摘要
一種估算承重機械的有效載荷的方法，該方法包括以下步驟(a)建立這個承重機械不同級別有效載荷的一系列的卡爾曼濾波器近似值；(b)從這一系列近似值來確定挖掘機當前運行特性的近似值；(c)利用步驟(b)獲得的參數(shù)確定所述承重機械的有效載荷估算值。
文檔編號G01G19/14GK101467011SQ200780021485
公開日2009年6月24日 申請日期2007年4月19日 優(yōu)先權日2006年4月20日
發(fā)明者羅斯·麥卡里, 戴維·埃爾曼·沃 申請人:Cmte開發(fā)有限公司