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

陳昱衡 吳鴻云 邊有鋼

摘要：針對多金屬結(jié)核采礦車在稀軟底質(zhì)行駛作業(yè)時(shí)有效驅(qū)動(dòng)輪半徑和履帶打滑率等動(dòng)力學(xué)參數(shù)難確定的問題，基于多金屬結(jié)核采礦車的牽引力分析和液壓驅(qū)動(dòng)系統(tǒng)的負(fù)載特性分析，建立用于多金屬結(jié)核采礦車動(dòng)力學(xué)參數(shù)估計(jì)的高階非線性系統(tǒng)模型.針對基于高斯模型的無跡卡爾曼濾波（Unscented Kalman Filter，UKF）算法無法在非線性系統(tǒng)取得較高估計(jì)精度的問題，提出利用基于蒙特卡洛采樣原理的自適應(yīng)無跡粒子濾波（Adaptive Unscented Particle Filter，AUPF）算法進(jìn)行動(dòng)力學(xué)參數(shù)測算方案，通過自適應(yīng)無跡卡爾曼濾波（Adaptive Unscented Kalman Filter，AUKF）改善粒子濾波（Particle Filter，PF）的概率密度函數(shù)，解決PF容易發(fā)散和UKF估計(jì)精度不高的問題.實(shí)驗(yàn)結(jié)果表明，AUPF算法得到的多金屬結(jié)核采礦車的動(dòng)力學(xué)參數(shù)誤差均小于最大允許誤差，滿足精準(zhǔn)在線測算的性能需求.

關(guān)鍵詞：深海采礦裝備；多金屬結(jié)核采礦車；參數(shù)估計(jì)；動(dòng)力學(xué)分析；自適應(yīng)無跡粒子濾波

中圖分類號：TP249文獻(xiàn)標(biāo)志碼：A

Estimation of Motion Parameters of a Underwater Track Mining Vehicle Based on Adaptive Unscented Particle Filter Algorithm

CHEN Yuheng1，WU Hongyun2，3，BIAN Yougang1

（1. State Key Laboratory of Advanced Design and Manufacturing for Vehicle Body，Hunan University，Changsha 410082，China；2. Changsha Institute of Mining Research Co.，Ltd.，Changsha 410012，China；3. National Engineering Research Center for Metal Mining，Changsha 410012，China）

Abstract：Aiming at the difficulty in determining the motion parameters such as effective drive wheels and track slip rate of a polymetallic nodule mining vehicle，based on the modeling of traction force and load characteristics of tracks，a higher-order nonlinear system for estimating the motion parameters of a polymetallic nodule mining vehicle is proposed. To address the problem that the UKF based on Gaussian model cannot achieve high estimation accuracy in high-order nonlinear systems，an adaptive traceless particle filtering algorithm （AUPF）based on Monte Carlo sampling principle is proposed，and the adaptive traceless Kalman filter （AUKF）is used to improve the Particle Filter （PF）by refining the probability density function，which solves the shortcomings that the PF is easy to diverge and theestimation accuracy of UKF is low. The experimental results show that the AUPF algorithm can achieve an accurate estimation of the motion parameters of polymetallic nodule mining vehicles，and there is an important engineering application value.

Key words：deep sea mining equipment；polymetallic nodule mining vehicle；parameter estimation；dynamics analysis；adaptive unscented particle filter

位于4～6 km水深的深海底，其表層沉積物中以半埋藏狀態(tài)覆存著大量的多金屬結(jié)核礦產(chǎn)資源，因其富含鎰、銅、鉆、鎳等稀貴金屬元素，極具商業(yè)開采價(jià)值.20世紀(jì)70年代，以美國為首的西方發(fā)達(dá)國家率先開始深海采礦技術(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ì)對控制性能產(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)仿真分析，并對多金屬結(jié)核采礦車的直行、差速轉(zhuǎn)向、爬坡和越障等工況時(shí)的機(jī)械特性進(jìn)行了仿真分析，并基于模型車進(jìn)行試驗(yàn)以驗(yàn)證方案的可行性，為深海采礦系統(tǒng)的整體集成設(shè)計(jì)、性能預(yù)測及作業(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對非線性輪胎模型中的路面附著系數(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)空間模型.針對UKF算法無法在高非線性模型中得到較高估計(jì)精度的問題[15-16]，采用基于蒙特卡洛采樣原理的自適應(yīng)無跡粒子濾波（Adaptive Unscented Particle Filter，AUPF）算法進(jìn)行多金屬結(jié)核采礦車的動(dòng)力學(xué)參數(shù)在線測算，利用自適應(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)在線測算.

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

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

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

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

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

式中：b₂為履齒寬度，m；γ_s為底質(zhì)比重，kg/m³；h_b為履齒高度，m；N_φ=tan²（45°+0.5φ）為底質(zhì)流值；n₂為履齒個(gè)數(shù).

牽引力F為：

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

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

r_a=r+r₁-r₂（4）

式中：r₁為履帶板厚度和履齒高度的和，m；r₂為測高聲吶測得的履帶沉陷，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)壓力p_l與馬達(dá)的驅(qū)動(dòng)力矩T_q呈線性關(guān)系，如式（5）所示.

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

T_q=T_L（6）

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

ω=ω_m（7）

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

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

T_q=r_aF=r_a（F₁+F₂）（9）

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

F₁=（Ac+Wtanφ）（e^-illk-0.03）（10）

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

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

狀態(tài)量為：

X（k）=[x₁（k）；x₂（k）；x₃（k）]=[i（k）；r_a（k）；θ_m（k）]

控制量為：

觀測量為：

Y（k+1）=[i（k+1）；r_a（k+1）]

式中：T為采樣時(shí)間，s；L為每個(gè)采樣時(shí)間T內(nèi)的履帶行走長度，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所示.

AUKF算法的具體步驟如下：

1）初始化.

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

式中：λ=n（α²-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ì)算.

4）狀態(tài)更新.

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

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

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

1）計(jì)算觀測值Y_AUPF（k）.

Y_AUPF（k）=H（X_AUPF（k））（24）

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

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

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

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

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

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

測控主系統(tǒng)為美國National Instruments公司推出的Compact RIO嵌入式測控平臺及軟件.車載本地測控站采集并記錄采礦車上各傳感器檢測參數(shù)，多金屬結(jié)核采礦車搭載多普勒測速儀、光纖陀螺、履帶驅(qū)動(dòng)電機(jī)編碼器、測高聲吶等傳感器實(shí)時(shí)獲取采

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

式中：v_a為履帶實(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算法對模型的不匹配情況也具有較好的適應(yīng)性.

4總結(jié)

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

參考文獻(xiàn)

[1]陳銘，冷靜.深海大型爬行機(jī)器人研究現(xiàn)狀[J].海洋工程，2020，38（5）：156-168.

CHEN M，LENG J. The development of deep-sea large-scale crawling robots [J]. The Ocean Engineering，2020，38（5）：156168. （In Chinese）

[2] DAI Y，XUE C，SU Q，et al. Numerical analysis on hydrodynamic characteristics of a deep-sea mining vehicle under three typical motions [J]. Ocean Engineering，2021，235：109446.

[3] DAI Y，ZHANG Y Y，LIX Y. Numerical and experimental investigations on pipeline internal solid-liquid mixed fluid for deep oceanmining[J].Ocean Engineering，2021，220：108411.

[4] DAI Y，ZHU X，ZHOU H B，et al. Trajectory tracking control for seafloor tracked vehicle by adaptive neural-fuzzy inference system algorithm [J]. International Journal of Computers （Communications & Control，2018，13（4）：465-476.

[5]戴瑜，張健，張?zhí)希?基于多體動(dòng)力學(xué)模型集成的深海采礦系統(tǒng)聯(lián)動(dòng)仿真[J].機(jī)械工程學(xué)報(bào)，2017，53（4）：155-160.

DAI Y，ZHANG J，ZHANG T，et al. Motion simulation of the deep ocean mining system based on its integrated multi-body dynamic model[J]. Journal of Mechanical Engineering，2017，53 （4）：155-160.（In Chinese）

[6]劉少軍，李淵文，胡小舟.基于DEM-CFD顆粒體積分?jǐn)?shù)對深海揚(yáng)礦電泵工作性能的影響[J].機(jī)械工程學(xué)報(bào)，2020，56 （10）：257-264.

LIU S J，LI Y W，HU X Z. Effect of particle volume fraction on the performance of deep-sea mining electric lifting pump based on DEM-CFD[J]. Journal of Mechanical Engineering，2020，56 （10）：257-264.（In Chinese）

[7] DAI Y，SU Q，ZHANG Y Y.A new dynamic model and trajectory tracking control strategy for deep ocean mining vehicle[J]. Ocean Engineering，2020，216：108162.

[8] DAI Y，YIN W W，MA F Y.Nonlinear multi-body dynamic modeling and coordinated motion control simulation of deep-sea mining system[J].IEEE Access，2019，7：86242-86251.

[9]張?zhí)希麒ぃ瑒⑸佘姡?深海履帶式集礦機(jī)多體動(dòng)力學(xué)建模與行走性能仿真分析[J].機(jī)械工程學(xué)報(bào)，2015，51（6）：173-180.

ZHANG T，DAI Y，LIU S J，et al. Multi-body dynamic modeling and mobility simulation analysis of deep ocean tracked miner[J]. Journal of Mechanical Engineering，2015，51（6）：173-180.（In Chinese）

[10]黃小平，王巖，繆鵬程.粒子濾波原理及應(yīng)用：MATLAB仿真[M].北京：電子工業(yè)出版社，2017：30-45.

HUANG X P，WANG Y，MIAO P C. Principle and application of particle filter：MATLAB simulation [M]. Beijing：Publishing House of Electronics Industry，2017：30-45.（In Chinese）

[11]周兵，田晨，宋義彤，等.基于路面附著系數(shù)估計(jì)的AFS控制策略研究[J].湖南大學(xué)學(xué)報(bào)（自然科學(xué)版），2017，44（4）：16-22.

ZHOU B，TIAN C，SONG Y T，et al. Control strategy of AFS based on estimation of tire-road friction coefficient[J]. Journal of Hunan University（Natural Sciences），2017，44（4）：16-22.（In Chinese）

[12]郭玉榮，龍沐恩.基于改進(jìn)IMK恢復(fù)力模型的鋼筋混凝土柱參數(shù)識別與應(yīng)用[J].湖南大學(xué)學(xué)報(bào)（自然科學(xué)版），2021，48（1）：126-134.

GUO Y R，LONG M E. Parameter identification and application of reinforced concrete column based on modified ibarra-medina- krawinkler hysteretic model[J]. Journal of Hunan University （Natural Sciences），2021，48（1）：126-134.（In Chinese）

[13]周兵，邱香，吳曉建，等.基于UKF車輛狀態(tài)及路面附著系數(shù)估計(jì)的AFS控制[J].湖南大學(xué)學(xué)報(bào)（自然科學(xué)版），2019，46 （8）：1-8.

ZHOU B，QIU X，WU X J，et al.AFS control based on estimation of vehicle state and road coefficient using UKF method[J]. Journal of Hunan University（Natural Sciences），2019，46（8）：1-8. （In Chinese）

[14] QIN Z B，CHEN L，F(xiàn)AN J J，et al. An improved real-time slip model identification method for autonomous tracked vehicles using forward trajectory prediction compensation[J]. IEEE Transactions on Instrumentation and Measurement，2021，70：1-12.

[15] LIU F X，LI X Y，YUAN S H，et al. Slip-aware motion estimation for off-road mobile robots via multi-innovation unscented Kalman filter[J].IEEE Access，2020，8：43482-43496.

[16] LIU Z，CHAN S C. Adaptive fading Bayesian unscented Kalman filter and smoother for state estimation of unmanned aircraft sys- tems[J].IEEE Access，2020，8：119470-119486.

[17]黃小平，王巖，繆鵬程.目標(biāo)定位跟蹤原理及應(yīng)用：MATLAB仿真[M].北京：電子工業(yè)出版社，2018：55-75.

HUANG X P，WANG Y，MIAO P C. Principle and application of target location and tracking：MATLAB simulation[M]. Beijing：Publishing House of Electronics Industry，2018：55-75.（In Chinese）

[18]吳鴻云.集礦機(jī)牽引性能若干影響因素研究[D].長沙：中南大學(xué)，2010.

WU H Y. Research on some faetors on driving performance of seabed traeked vehicle on seabed soft sediments [D]. Changsha：Central South University，2010.（In Chinese）