基于AUPF 算法的水下履帶車動(dòng)力學(xué)參數(shù)估計(jì)

陳昱衡，吳鴻云，邊有鋼?

（1.湖南大學(xué)汽車車身先進(jìn)設(shè)計(jì)制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，湖南長(zhǎng)沙 410082；2.長(zhǎng)沙礦山研究院有限責(zé)任公司，湖南長(zhǎng)沙 410012；3.國(guó)家金屬采礦工程技術(shù)研究中心，湖南長(zhǎng)沙 410012）

位于4～6 km 水深的深海底，其表層沉積物中以半埋藏狀態(tài)覆存著大量的多金屬結(jié)核礦產(chǎn)資源，因其富含錳、銅、鈷、鎳等稀貴金屬元素，極具商業(yè)開采價(jià)值.20 世紀(jì)70 年代，以美國(guó)為首的西方發(fā)達(dá)國(guó)家率先開始深海采礦技術(shù)研究，完成了不同程度的試驗(yàn)，催發(fā)了一輪深海采礦技術(shù)研究熱潮［1］.

在深海采礦作業(yè)時(shí)，采礦車沿著預(yù)設(shè)軌跡進(jìn)行自行式采集結(jié)核，需要在線獲取采礦車的動(dòng)力學(xué)參數(shù).然而，多金屬結(jié)核礦區(qū)表層含水量高，承載力弱，易流動(dòng)且伴隨有海底不規(guī)則洋流的影響，導(dǎo)致多金屬結(jié)核采礦車易發(fā)生履帶沉陷和履帶不規(guī)則打滑的現(xiàn)象［2］，故履帶沉陷時(shí)采礦車的有效驅(qū)動(dòng)輪半徑應(yīng)考慮履帶板厚度和未陷入底質(zhì)的驅(qū)動(dòng)輪部分.若忽略驅(qū)動(dòng)輪半徑變化時(shí)履帶打滑的動(dòng)態(tài)響應(yīng)，可能會(huì)對(duì)控制性能產(chǎn)生負(fù)面影響.因此，精準(zhǔn)的履帶打滑率和有效驅(qū)動(dòng)輪半徑等運(yùn)動(dòng)學(xué)參數(shù)估計(jì)顯得尤為重要.

為解決上述問題，文獻(xiàn)［3-6］基于RecurDyn 參數(shù)化建模技術(shù)，構(gòu)建了多剛體深海采礦系統(tǒng)，完成了聯(lián)動(dòng)仿真分析，并對(duì)多金屬結(jié)核采礦車的直行、差速轉(zhuǎn)向、爬坡和越障等工況時(shí)的機(jī)械特性進(jìn)行了仿真分析，并基于模型車進(jìn)行試驗(yàn)以驗(yàn)證方案的可行性，為深海采礦系統(tǒng)的整體集成設(shè)計(jì)、性能預(yù)測(cè)及作業(yè)操控提供了參考，同時(shí)為多金屬結(jié)核采礦車整車機(jī)械結(jié)構(gòu)的優(yōu)化設(shè)計(jì)提供了理論支撐［7-9］.

無跡卡爾曼濾波（Unscented Kalman Filter，UKF）算法通過無跡變換（Unscented Transformation，UT）將非線性函數(shù)模擬成真實(shí)分布的近似高斯分布，具有3 階的泰勒精度，是一種易實(shí)現(xiàn)的參數(shù)估計(jì)算法［10-12］.周兵等［13］基于UKF 對(duì)非線性輪胎模型中的路面附著系數(shù)進(jìn)行了在線估計(jì)，提高了自動(dòng)駕駛車輛的主動(dòng)轉(zhuǎn)向控制性能.Qin 等［14］基于瞬時(shí)轉(zhuǎn)向中心變化的履帶車運(yùn)動(dòng)學(xué)模型，提出了一種改進(jìn)的UKF 算法，實(shí)現(xiàn)了陸地自行式履帶車輛履帶打滑率的精準(zhǔn)在線估計(jì).

本文從多金屬結(jié)核采礦車的受力分析出發(fā)，建立底盤液壓驅(qū)動(dòng)系統(tǒng)與履帶負(fù)載特性的動(dòng)力學(xué)關(guān)系，提出了多金屬結(jié)核采礦車的有效驅(qū)動(dòng)輪半徑和履帶打滑率等動(dòng)力學(xué)參數(shù)的狀態(tài)空間模型.針對(duì)UKF 算法無法在高非線性模型中得到較高估計(jì)精度的問題［15-16］，采用基于蒙特卡洛采樣原理的自適應(yīng)無跡粒子濾波（Adaptive Unscented Particle Filter，AUPF）算法進(jìn)行多金屬結(jié)核采礦車的動(dòng)力學(xué)參數(shù)在線測(cè)算，利用自適應(yīng)無跡卡爾曼濾波（Adaptive Unscented Kalman Filter，AUKF）算法改進(jìn)概率密度函數(shù)以指導(dǎo)粒子濾波（particle filter，PF）進(jìn)行重要性采樣，克服了粒子濾波易發(fā)散的缺點(diǎn)［17］.試驗(yàn)結(jié)果表明，所提出的AUPF算法可實(shí)現(xiàn)多金屬結(jié)核采礦車的動(dòng)力學(xué)參數(shù)的精準(zhǔn)在線測(cè)算.

1 多金屬結(jié)核采礦車參數(shù)估計(jì)建模

1.1 多金屬結(jié)核采礦車受力分析

與陸地履帶車不同，多金屬結(jié)核采礦車在多金屬結(jié)核礦區(qū)稀軟的底質(zhì)上行駛，為了提高多金屬結(jié)核采礦車的抓地能力，行走履帶采用三角履齒.多金屬結(jié)核采礦車如圖1所示，三角履齒如圖2所示.

圖1 多金屬結(jié)核采礦車Fig.1 Polymetallic nodules mining vehicle

圖2 三角履齒Fig.2 Triangular track teeth

在計(jì)算采礦車履帶產(chǎn)生的牽引力F時(shí)，不僅需要考慮履帶對(duì)底質(zhì)產(chǎn)生的剪切力F1，還需要考慮履齒剪切底質(zhì)產(chǎn)生的壓力F2.F1和F2分別見式（1）和式（2）.

式中：A為履帶的接地面積，m2；W為車身正壓力，N；c為底質(zhì)內(nèi)聚壓力，kPa；φ為底質(zhì)內(nèi)摩擦角參數(shù)，(°)；l為履帶接地長(zhǎng)度，m；i為履帶打滑率；k為底質(zhì)水平剪切模變數(shù)，m.

式中：b2為履齒寬度，m；γs為底質(zhì)比重，kg∕m3；hb為履齒高度，m；Nφ=tan2(45°+0.5φ)為底質(zhì)流值；n2為履齒個(gè)數(shù).

牽引力F為：

1.2 多金屬結(jié)核采礦車動(dòng)力學(xué)參數(shù)在線測(cè)算模型

多金屬采礦車在含水量高的稀軟底質(zhì)進(jìn)行自行式作業(yè)時(shí)，底盤會(huì)發(fā)生沉陷現(xiàn)象，故實(shí)際的履帶有效驅(qū)動(dòng)輪半徑ra為：

式中：r1為履帶板厚度和履齒高度的和，m；r2為測(cè)高聲吶測(cè)得的履帶沉陷，m；r為理論驅(qū)動(dòng)輪半徑，m.

多金屬采礦車左右履帶驅(qū)動(dòng)采用兩獨(dú)立的閉式液壓回路，即采用兩電動(dòng)機(jī)分別驅(qū)動(dòng)兩變量泵，再經(jīng)過定量馬達(dá)傳遞到左右履帶，馬達(dá)直接驅(qū)動(dòng)采礦車行走.其中液壓馬達(dá)的系統(tǒng)壓力pl與馬達(dá)的驅(qū)動(dòng)力矩Tq呈線性關(guān)系，如式（5）所示.

式中：VM為液壓馬達(dá)的理論弧度流量，m3∕rad；pl為系統(tǒng)壓力，Pa；θm為液壓馬達(dá)轉(zhuǎn)角，rad；Jt為液壓馬達(dá)和負(fù)載總慣量，m·N·s2；Bm為負(fù)載和液壓馬達(dá)內(nèi)部的總黏性阻尼系數(shù)，m·N·s∕rad；G為負(fù)載的扭矩剛度，m·N∕rad；ωm=dθm∕dt為液壓馬達(dá)轉(zhuǎn)速，rad∕s；ω˙m=dωm∕dt為液壓馬達(dá)角加速度，rad∕s2.

多金屬結(jié)核采礦車為低速勻速行駛，故在建模時(shí)可假設(shè)馬達(dá)的負(fù)載力矩TL等于驅(qū)動(dòng)力矩Tq：

驅(qū)動(dòng)輪由液壓馬達(dá)直驅(qū)，故驅(qū)動(dòng)輪轉(zhuǎn)速ω等于液壓馬達(dá)轉(zhuǎn)速ωm：

聯(lián)立式（5）～式（7）負(fù)載力矩TL與驅(qū)動(dòng)輪轉(zhuǎn)速ω的方程，如式（8）所示.

同時(shí)驅(qū)動(dòng)力矩Tq滿足：

為了簡(jiǎn)化運(yùn)算，基于文獻(xiàn)［18］，對(duì)式（1）進(jìn)行簡(jiǎn)化，得

聯(lián)立式（2）式、式（4）、式（8）～式（10），可得到履帶有效驅(qū)動(dòng)輪半徑和履帶打滑率的關(guān)系.

通過離散化式（4）和式（10），定義如式（12）所示的狀態(tài)空間模型.

狀態(tài)量為：

控制量為：

觀測(cè)量為：

由于采礦車為低速行駛，故較小時(shí)間間隔內(nèi)的平均速度可視為履帶實(shí)際速度va.

式中：T為采樣時(shí)間，s；L為每個(gè)采樣時(shí)間T內(nèi)的履帶行走長(zhǎng)度，m.

2 自適應(yīng)無跡粒子濾波算法

粒子濾波算法不依賴高斯模型，基于蒙特卡羅采樣原理，可應(yīng)用于高線性系統(tǒng)的參數(shù)估計(jì).但是PF 算法存在濾波易發(fā)散的缺點(diǎn)，通常采用UKF 算法指導(dǎo)粒子重采樣以改進(jìn)PF 算法.本文采用AUKF 指導(dǎo)粒子重采樣的AUPF 進(jìn)行參數(shù)估計(jì).AUPF 的算法流程如表1所示.

表1 AUPF算法流程Tab.1 AUPF algorithm flow

AUKF算法的具體步驟如下：

1）初始化.

2）Sigma點(diǎn)權(quán)重計(jì)算.

式中：λ=n(α2-1)，α是控制Sigma 點(diǎn)分布的常數(shù)，其取值范圍為e-4≤α≤1；β為非負(fù)常數(shù)，將高階成分的信息轉(zhuǎn)換到方差中，取β=2.

3）Sigma點(diǎn)選擇和估計(jì)值更新.在UT中，隨機(jī)變量由一組最小的樣本點(diǎn)表示，稱為Sigma 點(diǎn).計(jì)算這些點(diǎn)在非線性系統(tǒng)模型傳播時(shí)的后驗(yàn)平均值和協(xié)方差.Sigma點(diǎn)由式（16）計(jì)算.

粒子集合XAUPF(k)由式（23）計(jì)算.

式中：nrand為0～1的隨機(jī)數(shù).

粒子權(quán)重計(jì)算具體步驟如下：

1）計(jì)算觀測(cè)值YAUPF(k).

2）計(jì)算粒子權(quán)重并歸一化.

通過設(shè)定權(quán)重閾值進(jìn)行重采樣并更新狀態(tài)與基本PF算法一致，本文不再贅述，見參考文獻(xiàn)［10］.

基于張?zhí)系热耍?］的研究，式（1）、式（2）中多金屬結(jié)核底質(zhì)土力學(xué)特性的參數(shù)值和采礦車動(dòng)力學(xué)參數(shù)測(cè)算模型的參數(shù)如表2所示.

表2 模型參數(shù)值Tab.2 Parameter values of the model

3 動(dòng)力學(xué)參數(shù)在線測(cè)算方法實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證本文提出的動(dòng)力學(xué)參數(shù)測(cè)算算法能否精準(zhǔn)測(cè)算多金屬結(jié)核采礦車的履帶打滑率和有效驅(qū)動(dòng)輪半徑，定義如下性能需求：有效驅(qū)動(dòng)輪最大誤差不超過5 mm，履帶打滑率的最大誤差不超過5%.以上述多金屬結(jié)核采礦車為實(shí)驗(yàn)對(duì)象，在深海采礦實(shí)驗(yàn)水池進(jìn)行行走實(shí)驗(yàn)，實(shí)驗(yàn)水池長(zhǎng)60 m，寬50 m，深5 m，實(shí)驗(yàn)室水池如圖3所示.

圖3 實(shí)驗(yàn)室水池Fig.3 Laboratory sink

水池底質(zhì)為河泥以模擬多金屬結(jié)核礦區(qū)的稀軟地形，水深保證可以淹沒整個(gè)多金屬結(jié)核采礦車.多金屬結(jié)核采礦車通過起重機(jī)下放至水池中，由慣性導(dǎo)航單元和多普勒測(cè)速組成的水下組合導(dǎo)航系統(tǒng)使采礦車完成如圖4所示的行走路線.

圖4 采礦車實(shí)驗(yàn)行走路線Fig.4 Mining vehicle’s test route

測(cè)控主系統(tǒng)為美國(guó)National Instruments 公司推出的Compact RIO 嵌入式測(cè)控平臺(tái)及軟件.車載本地測(cè)控站采集并記錄采礦車上各傳感器檢測(cè)參數(shù)，多金屬結(jié)核采礦車搭載多普勒測(cè)速儀、光纖陀螺、履帶驅(qū)動(dòng)電機(jī)編碼器、測(cè)高聲吶等傳感器實(shí)時(shí)獲取采礦車的運(yùn)動(dòng)信息.光纖陀螺給出精準(zhǔn)的采礦車的航向角φ和角速度ω′；履帶驅(qū)動(dòng)電機(jī)編碼器給出采礦車帶驅(qū)動(dòng)輪的轉(zhuǎn)速ω和加速度ω˙.

車載本地測(cè)控站記錄所測(cè)算的外側(cè)履帶打滑率io、內(nèi)側(cè)履帶打滑率ii、外側(cè)有效驅(qū)動(dòng)輪半徑ro和內(nèi)側(cè)有效驅(qū)動(dòng)輪半徑ri的實(shí)驗(yàn)數(shù)據(jù)分別如表3 和圖5～圖8 所示.同時(shí)實(shí)際的驅(qū)動(dòng)輪半徑通過式（4）計(jì)算，實(shí)際打滑率由式（26）計(jì)算.

表3 運(yùn)動(dòng)參數(shù)均方差Tab.3 Mean square error of motion parameters

圖5 外側(cè)履帶打滑率Fig.5 Lateral track skid rate

圖6 內(nèi)側(cè)履帶打滑率Fig.6 Medial track skid rate

圖7 外側(cè)有效驅(qū)動(dòng)輪半徑Fig.7 Effective radius of the lateral driving wheel

圖8 內(nèi)側(cè)有效驅(qū)動(dòng)輪半徑Fig.8 Effective radius of the medial driving wheel

式中：va為履帶實(shí)際速度，由式（13）計(jì)算.

由實(shí)驗(yàn)結(jié)果可知，有效驅(qū)動(dòng)輪半徑的誤差在3 mm 以內(nèi)，履帶打滑率的最大誤差不超過5%，有效驅(qū)動(dòng)輪半徑的誤差均方差小于0.5，履帶打滑率誤差均方差小于1.5.

誤差的產(chǎn)生可能是由于實(shí)驗(yàn)室模擬底質(zhì)的力學(xué)特性與真實(shí)底質(zhì)的力學(xué)特性有一定的差距，故狀態(tài)空間模型中的底質(zhì)力學(xué)特性與實(shí)驗(yàn)水池模擬底質(zhì)的力學(xué)特性存在不匹配的情況.但是有效驅(qū)動(dòng)輪半徑的誤差和履帶打滑誤差都小于最大誤差，且均方差均較低.通過AUPF 算法可以得到精度較高的多金屬結(jié)核采礦車動(dòng)力學(xué)參數(shù)，同時(shí)說明AUPF算法對(duì)模型的不匹配情況也具有較好的適應(yīng)性.

4 總結(jié)

針對(duì)多金屬結(jié)核采礦車在海底行駛作業(yè)時(shí)，履帶打滑率和有效驅(qū)動(dòng)輪半徑等動(dòng)力學(xué)參數(shù)難以確定的問題，本文通過分析多金屬結(jié)核采礦車的履帶牽引力和履帶的液壓負(fù)載特性，建立了多金屬結(jié)核采礦車履帶打滑率和有效驅(qū)動(dòng)輪半徑等動(dòng)力學(xué)參數(shù)在線測(cè)算的狀態(tài)空間模型.針對(duì)動(dòng)力學(xué)參數(shù)測(cè)算狀態(tài)空間模型非線性程度較高的問題，提出了利用AUPF算法進(jìn)行多金屬結(jié)核采礦車動(dòng)力學(xué)參數(shù)測(cè)算的方案.實(shí)驗(yàn)結(jié)果表明，有效驅(qū)動(dòng)輪半徑的誤差控制在3 mm 以內(nèi)，打滑率的最大誤差不超過5%，滿足動(dòng)力學(xué)參數(shù)測(cè)算的精度要求，驗(yàn)證了所提方案的可行性，具有重要的工程應(yīng)用價(jià)值.