吉曉冬瞿圓媛符世琛張敏駿吳淼

1.中國礦業(yè)大學(北京)機電與信息工程學院,北京 100083;2.北京建筑大學機電與車輛工程學院,北京 100044;3.清華大學機械工程系,北京 100084

縱軸懸臂式掘進機(下文簡稱“掘進機”)是煤礦井下掘進工作面的主要設(shè)備之一[1]。在巷道掘進過程中,惡劣的地質(zhì)環(huán)境、狹小地下空間的位姿感知誤差等因素,都會導致機身漸漸偏離規(guī)劃中心線,截割面出現(xiàn)超挖、欠挖等現(xiàn)象,從而影響巷道成形質(zhì)量,降低掘進效率。目前掘進機糾偏仍很大程度依靠有經(jīng)驗的操作員調(diào)度完成,掘進現(xiàn)場作業(yè)、機器人化智能發(fā)展的進程緩慢[2-3]。

機器人化掘進的實現(xiàn)取決于掘進機的位姿檢測、自主糾偏、遠程通信與控制、自主截割等關(guān)鍵環(huán)節(jié)的智能化實現(xiàn)與融合[4]。其中,基于機身位姿信息的自主糾偏,即掘進機的行進調(diào)度問題是保證巷道截割質(zhì)量的重要前提。掘進機的糾偏行進調(diào)度涉及兩個環(huán)節(jié):一是糾偏控制模型的建立;二是機身動態(tài)位姿檢測的實現(xiàn)。

針對履帶車輛的糾偏控制研究,部分文獻側(cè)重于完善履帶車運動學或動力學建模,以獲得更準確有效的機身移動模型表征;也有部分文獻更側(cè)重于優(yōu)化控制策略的使用,以獲得更智能有效的動態(tài)控制效果。如李巖等[5-6]在履帶式移動機器人動力學分析的基礎(chǔ)上提出了充分考慮履帶與地面之間滑動摩擦力對運動參數(shù)影響的滑動操縱模型[7],說明軌跡沿程打滑率是履帶車運動參數(shù)調(diào)整的重要依據(jù)。后者相關(guān)理論以模型預測控制(MPC)這一基于反饋信息的多目標滾動優(yōu)化模型為典型,如Wang、Lenain 等[8-9]先后采用不同的履帶打滑估算方法模擬實際打滑因素的不確定性,將模型預測控制用于農(nóng)業(yè)履帶式運載設(shè)備的軌跡跟蹤控制;Burke[10]運用MPC 模型結(jié)合陀螺儀的測量信息實現(xiàn)打滑率的自適應估計,由此完成某移動機器人的軌跡跟蹤控制。

由于掘進機井下工作環(huán)境復雜惡劣,且通信能力有限,井上通用可行的機身位姿測量方案往往難以直接移植到井下。同時,工作模式的不同,用于同在井下作業(yè)的采煤機其位姿測量方案也并不適用于井下掘進機。在掘進機動態(tài)位姿檢測研究方面,針對煤巷掘進機位姿檢測、定向掘進技術(shù)分別論證了一系列自動測量系統(tǒng)或方法,但其在實際工況下的應用并未得到有效驗證[11]。

上述研究從不同角度為掘進機的糾偏與軌跡跟蹤提供參考,而系統(tǒng)地針對掘進機井下糾偏行進調(diào)度跟蹤控制策略的研究有限。王福忠等[12]針對掘進機的液壓行走驅(qū)動進行了建模,并針對行走軌跡偏離巷道中線的問題提出了模糊控制策略;張敏駿等[13]研究了掘進機在不同掘進環(huán)境下的打滑率并提出基于神經(jīng)網(wǎng)絡PID 算法的自主糾偏驅(qū)動控制策略;張旭輝等[14]運用單目視覺測量技術(shù),并基于履帶車運動學分析為掘進機設(shè)計了定向掘進控制器。

綜上所述,目前在掘進機自主糾偏上有一定的研究,但缺乏對掘進機的位姿測量誤差與執(zhí)行過程誤差的反饋控制研究。

掘進機在履帶行進的執(zhí)行過程中普遍存在各類傳遞誤差,用于位姿糾偏的實時位姿測量也不可避免存在觀測誤差,故而機身行進調(diào)度仍然具有顯著的非確定性。本文以掘進機行走位姿與設(shè)定的目標位姿之間的偏差模型為基礎(chǔ),以超寬帶(UWB)測距的位姿測量實驗為依據(jù),提出一套考慮測量及執(zhí)行誤差的、基于位姿估計的掘進機行進調(diào)度跟蹤控制策略。

1 掘進機行進調(diào)度需求及建?；A(chǔ)

巷道的掘進通過執(zhí)行調(diào)度行進、截割頭掏槽、斷面截割、刷幫掃底、煤巖輔運等基礎(chǔ)工序循環(huán)進行。每完成一段掘進量會迅速執(zhí)行錨桿支護,此時,掘進機會根據(jù)巷道走向進行合理的糾偏,保證巷道走勢和成型質(zhì)量。

1.1 控制律掘進機行進調(diào)度規(guī)劃

為實現(xiàn)智能化高效掘進,需引入機身位姿自主感知環(huán)節(jié)。此環(huán)節(jié)一般通過在機身搭載定位系統(tǒng)實現(xiàn),如薛光輝等[15]提出的激光標靶系統(tǒng),符世琛等[16]提出的UWB 測試系統(tǒng),張旭輝等[14]應用的單目圖像處理技術(shù)等。在每段掘進量開始或結(jié)束間隙,機載定位系統(tǒng)會測量當前的機身質(zhì)點位置坐標,判斷是否與設(shè)計的巷道中心線有所偏移,進而規(guī)劃行進軌跡并調(diào)整工作參數(shù),為掘進機機身的行進調(diào)度糾偏。

機身行進調(diào)度包含軌跡規(guī)劃與軌跡跟蹤。在機器人跟蹤控制領(lǐng)域,動態(tài)軌跡規(guī)劃與跟蹤執(zhí)行經(jīng)常同步進行。鑒于方法適用性差異,本文著重討論已有規(guī)劃軌跡下的跟蹤策略,暫不涉及動態(tài)軌跡規(guī)劃問題。

1.2 基于位姿偏差的糾偏控制模型

在履帶式車輛的行進運動學建模研究中,韓慶玨等[17]針對某深海履帶車運用了路徑偏差模型[18],以路徑跟蹤的位姿偏差在控制迭代中有限且趨于零為目標,理論驗證了基于位姿偏差模型的履帶車控制的可行性。張敏駿[13]、張旭輝[14]等針對懸臂式掘進機的行進建模亦采用了該位姿偏差模型作為運動學建模基礎(chǔ),其原理簡述如下:

如圖1所示,以機身當前轉(zhuǎn)向瞬心到機身中心線的垂足作為分析點(設(shè)為機身坐標系xOy的原點),圖1中虛曲線表示該點的移動軌跡,為清楚示意機身的移動過程,采用了三角形代替機身簡化矩形。(X,Y)、(x,y)分別為大地坐標系與機身坐標系下的質(zhì)點坐標;參數(shù)b為左右履帶之間的寬度;v為機身行走速度;φ為機身轉(zhuǎn)向角度(由左右驅(qū)動輪轉(zhuǎn)速決定);ω為機身轉(zhuǎn)向角速度;θ為機身方向角;腳標c 表示對應參數(shù)的當前值;腳標d 表示對應參數(shù)的規(guī)劃值;腳標e 表示對應參數(shù)其當前數(shù)值與規(guī)劃值之間的偏差。

圖1 掘進機調(diào)度跟蹤時位姿變化示意Fig.1 Illustration of the variance of the position and orientation of the body of the road-header when tracking

定義質(zhì)點當前位姿pc=(Xc,Yc,θc),規(guī)劃或期望位姿pd=(Xd,Yd,θd);質(zhì)點在機身坐標系xOy中的位姿偏差pe=(xe,ye,θe),可表示為

對pe取微分[20-21],得

掘進機在一定初始偏差下的位姿糾偏過程就是通過逐步調(diào)整參數(shù)vc、ωc,最終使得位姿偏差pe在有限的步數(shù)內(nèi)有界且趨于0。顯然由式(1)、式(2)描述的偏差模型是一個兩輸入非線性系統(tǒng),現(xiàn)基于Back-stepping 設(shè)計滑模切換函數(shù):

當xe=0 時,考察Lyapunov 函數(shù)

由Lyapunov 穩(wěn)定判據(jù)可知,若滿足Ly≥0 且≤0,則ye可在有限的調(diào)整周期內(nèi)收斂為0。同時,考慮實際情況下,即sinθe與θe同號。由此可取yevd=-θe,如此只要滿足:xe→0,θe→-yevd,則ye→0。

針對由式(1)、式(2)描述的偏差模型,可設(shè)計切換函數(shù):

通過設(shè)計滑?？刂破?使si→0,(i=1,2),即實現(xiàn)xe收斂到0 且θe收斂到-vdye,從而ye→0,θe→0。對s1、s2均取指數(shù)趨近律,令=-kisi(i=1,2),其中ki為正系數(shù)。對式(4)取微分可得

將式(4)代入式(5)可得

進一步獲得控制律u=(vc,ωc)T的計算式:

由式(7)可知,控制律中的機身轉(zhuǎn)向角速度指令ωc由機身位姿偏差(xe,ye,θe)、目標速度vd、目標轉(zhuǎn)向角速度ωd結(jié)合任意正系數(shù)k2計算獲得;機身速度指令vc由機身位姿偏差(xe,ye,θe)、轉(zhuǎn)向角速度指令ωc結(jié)合任意正系數(shù)k1計算獲得。

1.3 控制律簡化及收斂性證明

為方便工業(yè)應用,在保證pe有界且趨于0 的前提下,可將式(7)適當簡化?？紤]機身轉(zhuǎn)向角速度ωc較其行進速度vc略小,且實際運行時vdxe在數(shù)值上遠小于1;θe與sinθe同號?；诖?將式(7)簡化為

式中,kx、ky、kθ取值為正,在此分別稱為偏差xe、ye、θe的伴隨系數(shù)或權(quán)值。

由Lyapunov 穩(wěn)定判據(jù)可知,若能取合適的Lyapunov 函數(shù)L(xe,ye,θe)≥0,上述控制律(8)能使(xe,ye,θe)≤0,則pe有界且

將式(8)代入式(9)可得

若kky=1,則,控制律式(8)能使機身位姿偏差pe逐漸收斂為0。

2 基于濾波估計的行進調(diào)度控制策略

通過運動學分析可知,機身軌跡跟蹤調(diào)度的實質(zhì)是對履帶驅(qū)動輪轉(zhuǎn)速進行合理的控制,但實際操作時存在機械結(jié)構(gòu)的執(zhí)行誤差、位姿檢測的測量誤差。本文將上述誤差分別稱為過程誤差和觀測誤差,采用噪聲環(huán)境下的多狀態(tài)聯(lián)合估計問題所常用的非線性濾波法,實現(xiàn)掘進機軌跡跟蹤調(diào)度在誤差影響下對參考控制指令的調(diào)整。

2.1 過程誤差

由控制律u=(vc,ωc)T指令產(chǎn)生的實際機動效果與配置值不可避免存在差異,履帶打滑現(xiàn)象是主要原因之一。履帶車行進時,履帶與地面之間常發(fā)生相對滑動,通常用打滑率來表征滑動對履帶理論速度帶來的影響,數(shù)值上以滑動速度與牽引速度的比值來表示,滑動速度與履帶運動參數(shù)有關(guān)。用iL、iR分別表示左右履帶打滑率:

式中,ωL、ωR分別為左右履帶驅(qū)動輪的角速度;r為履帶驅(qū)動輪半徑;rωL、rωR分別為沒有縱向打滑時兩邊履帶的線速度;b為左右履帶之間的寬度;為履帶車旋轉(zhuǎn)角速度;為機身沿x軸方向的速度。

式中,α為質(zhì)心處轉(zhuǎn)向半徑切線方向與機身前進方向的夾角。

α產(chǎn)生的原因,在于剛體旋轉(zhuǎn)時瞬心與剛體上各質(zhì)點的連線(轉(zhuǎn)向半徑)并不都垂直于機身的前進方向。

機身行走速度v由與其圓周運動的線速度合成,但因機身的轉(zhuǎn)向角速度往往較慢,可忽略圓周運動線速度部分,近似認為v≈;式(12)也說明,通過調(diào)整左右履帶驅(qū)動輪的轉(zhuǎn)速,可達到調(diào)整機身前進速度v和轉(zhuǎn)動角速度ω(ω=)的目的。反之,根據(jù)控制律計算的前進速度與轉(zhuǎn)動角速度,在考慮打滑因素的情況下,實際對應的左右驅(qū)動輪角速度為

本文考慮打滑率的影響,從數(shù)值統(tǒng)計的角度討論履帶驅(qū)動輪實際轉(zhuǎn)速與設(shè)定值的差異。根據(jù)Bekker 理論,履帶驅(qū)動力計算如下[13,21]:

式中,B為履帶寬度;x為接觸點距履帶前端的距離;τmax為最大剪切應力;系數(shù)Kr為殘余剪切應力與最大剪切應力τmax的比值;Kω為出現(xiàn)最大剪切應力時對應的剪切位移;i(x)為履帶打滑率,與履帶觸地長度有關(guān);C為剪切強度;CD為土壤常數(shù);φ為土壤內(nèi)摩擦角;μ為地面阻力系數(shù);P為法向應力。

土壤的壓力-沉降關(guān)系如下:

式中,KC為土壤內(nèi)聚變形量;Kφ為摩擦變形模量;z為沉降量;n為沉降指數(shù)。

韓慶玨[20]通過對深海集礦機履帶驅(qū)動力與打滑率關(guān)系的研究指出:當履帶接地長度一定時,履帶車在特定軟底質(zhì)上行駛時存在最佳打滑率,其使地面能支撐的履帶驅(qū)動力具有最大值,即對式(14)求導可得到最佳打滑率的理論值,以此作為掘進機打滑率估算的基礎(chǔ)。張敏駿等[13]計算了幾種典型土壤環(huán)境下的打滑率估算值。本文根據(jù)這些數(shù)值,假設(shè)掘進機履帶工作打滑率服從邊界值為[0.1,0.35]的均勻分布,可為狀態(tài)估計所涉及的變量分布提供數(shù)據(jù)依據(jù)。

2.2 觀測誤差

針對掘進機的位姿測量,符世琛[11,16]揭示了基于超寬帶(UWB)測距的定位算法機制,建立了UWB 定位算法誤差分布分析方法,闡明了慣性導航系統(tǒng)(INS)姿態(tài)解算機理,建立了一種基于UWB標定與INS 實時測量的井下關(guān)鍵作業(yè)裝備自主導航新方法。本文采用了美國Time Domain 公司P440 超寬帶測距模塊以及國內(nèi)Action 公司開發(fā)的微機械陀螺儀及軸編碼器,搭建完成了掘進機自主導航模擬平臺,如圖2所示。

圖2 掘進機自主導航模擬平臺Fig.2 Experimental platform for the autonomous navigation of the road-header

通過導航系統(tǒng)位姿檢測現(xiàn)場的一系列實驗,獲取完整實測數(shù)據(jù)并實現(xiàn)二維運動軌跡實時顯示。數(shù)據(jù)表明,在78 m 模擬實驗巷道場地范圍內(nèi),系統(tǒng)X軸(橫向)偏向誤差控制在10 cm 左右,Y軸(掘進方向)偏向誤差小于2 cm,機身定位點X、Y軸誤差如圖3所示。方向角精度可保持在5°以內(nèi),俯仰角及橫滾角誤差可保持在1°左右[11,16]。

圖3 機身定位點X、Y 軸誤差分布Fig.3 The position errors distribution along the X and Y axes,respectively,of the marker on the body

由測距結(jié)果進行姿態(tài)解算,可獲得掘進機的位姿參數(shù),其與調(diào)度目標軌跡上對應的位姿之差即為上文所述的狀態(tài)參數(shù)pe=(xe,ye,θe)T?；诖?本文假設(shè)參數(shù)xe、ye的測量誤差均服從均值為0、標準差為≈3.5 cm 的正態(tài)分布;而方向角偏差參數(shù)θe則服從均值為0、標準差為5°的正態(tài)分布。

2.3 基于狀態(tài)估計的調(diào)度跟蹤控制

定義機身狀態(tài)向量Q=(xe,ye,θe,ω*L,ω*R)T,pe=(xe,ye,θe)T為機身位姿跟蹤偏差,可由機身車載定位系統(tǒng)實時測取,通過式(8)、式(13)可估算滿足軌跡跟蹤的履帶左右輪轉(zhuǎn)速調(diào)度需求,即(ωL,ωR)T。

考慮打滑,(ω*L,ω*R)T=[ωL(1-iL),ωR(1-iR)]T為實際轉(zhuǎn)速,運用式(1)、式(2),可預測機身的位姿跟蹤結(jié)果。可見,由式(1)、式(2)、式(8)、式(13)所描述的機身調(diào)度軌跡跟蹤模型可由以下非線性系統(tǒng)表示

式中,εk為控制實施的過程誤差;δk+1為位姿偏差的觀測誤差。

鑒于偏差與位姿的關(guān)系,式(15)中δk+1相當于機身質(zhì)點的位姿測量誤差。針對這類噪聲環(huán)境下的多狀態(tài)聯(lián)合估計問題,非線性濾波法被廣泛運用于改善預測效果。本文提出基于 SVD-Unscented 卡爾曼濾波的狀態(tài)估計即為一種典型的非線性濾波法,可以針對性地處理履帶行進打滑導致的指令執(zhí)行誤差和位姿測量誤差。控制結(jié)構(gòu)框架如圖4所示。

圖4 基于SVD-Unscented 卡爾曼濾波的掘進機行進調(diào)度糾偏控制結(jié)構(gòu)框圖Fig.4 Structure of the path tracking for boom road-header using SVD-Unscented Kalman Filtering

2.4 SVD-Unscented 卡爾曼濾波

為方便解釋,將式(15)所描述的非線性系統(tǒng)改寫為

Unscented 卡爾曼濾波(UKF)用于狀態(tài)估計的主要思想在于直接采用非線性變換逼近系統(tǒng)參數(shù)的概率分布,而非逼近系統(tǒng)的線性化模型。其核心之一是基于Unscented 變換(UT 變換)的無導數(shù)運算估計算法的運用:對于一個N維的自變量x經(jīng)某非線性變換f(x)后獲得隨機向量y,記Px|N×N為x的協(xié)方差矩陣,為x的均值,為獲得y的統(tǒng)計量,運用Sigma 點抽樣法形成2N+1 個Sigma 點向量矩陣χ:

式中,λ為尺度參數(shù);α為很小的正常數(shù),決定周圍sigma 點的擴散;κ為二階尺度參數(shù),一般在狀態(tài)估計時設(shè)為0, 在參數(shù)估計時設(shè)為3-N;為加權(quán)平方根矩陣的第i列。

一般情況下,Px為正定對稱矩陣,對其進行特征值分解,設(shè)Px=QΛQ-1(Q為N×N方陣,其列向量為Px的特征向量;Λ是由對應特征值組成的對角矩陣),進而注意,式(17)成立的前提是Px為正定對稱矩陣。然而,在實際執(zhí)行過程中經(jīng)常會發(fā)生協(xié)方差矩陣病態(tài)的情況,即Px不滿足正定對稱矩陣的條件。

由于正定對稱矩陣的特征值分解等價于SVD分解:Px=UΛVT(其中U=V=Q,QT-Q=I,Λ同上),故

本文基于SVD 分解數(shù)值計算方面的魯棒性優(yōu)勢,采用式(18)計算協(xié)方差矩陣Px的開方,而非直接開方,稱為SVD-Unscented 卡爾曼濾波。

基于SVD-Unscented 卡爾曼濾波的狀態(tài)估計方法如下:

(1) 初始化。構(gòu)造包含狀態(tài)量、過程誤差以及觀測誤差的增廣狀態(tài)向量。

(2) 確定Sigma 點集。根據(jù)式(17)、式(18)取點向量矩陣χk。

(3) 時間更新。利用系統(tǒng)非線性函數(shù)f(x)產(chǎn)生,并計算與。

(4) 狀態(tài)更新。

以式(21)中的估計值作為狀態(tài)量xk的更新,以式(20)中的作為觀測量yk+1的更新,代入控制函數(shù)式(8),完成控制輸入u的糾正與更新,即實現(xiàn)基于SVD-Unscented 卡爾曼濾波的狀態(tài)估計及路徑糾偏控制。

3 掘進機調(diào)度跟蹤控制仿真分析

本節(jié)對偏離目標巷道的掘進機調(diào)度跟蹤控制進行仿真分析,以評估所提出糾偏方案的合理性和可行性。

本文暫不涉及軌跡規(guī)劃,故預設(shè)以如圖5所示軌跡點作為機身中心位置的規(guī)劃軌跡。軌跡參數(shù)包括期望的坐標與對應位置上的航向角(轉(zhuǎn)向角速度)變化歷程。取圖5所示S 型軌跡目的,在于考查左、右轉(zhuǎn)向調(diào)度時的控制效果。假設(shè)掘進機機身初始位姿為,其中(1,0)為機身的中心坐標,為機身航向角。機身初始位姿與調(diào)度軌跡的第一個點所配置的坐標及航向角一致。其他仿真參數(shù)見表1。

圖5 規(guī)劃調(diào)度軌跡坐標、航向角及轉(zhuǎn)向角速度Fig.5 The planned trajectory

表1 仿真參數(shù)Table 1 Data used in simulation

為初步驗證由式(1)、式(2)、式(8)、式(13)所描述的控制系統(tǒng)的有效性:

首先,進行不涉及過程誤差及測量誤差處理的行進調(diào)度仿真。圖6展示了調(diào)度過程中機身中點的坐標變化情況、位姿偏差變化情況以及由控制指令u=(vc,ωc)T換算配置的左右驅(qū)動輪轉(zhuǎn)速變化情況。從圖6可以看出,調(diào)度仿真達到了軌跡跟隨的目的:

圖6 不考慮噪聲時的調(diào)度跟蹤Fig.6 The tracking results without noises

(1) 空心點表示實際機身移動軌跡,與實心圓點所示的規(guī)劃軌跡重合度極高。

(2) 機身中心位姿偏差(Xe,Ye,θe)在有限的步數(shù)內(nèi)均趨于零值;有個別突變點出現(xiàn)在對應軌跡中航向角變化速率突變的位置,但其下一步調(diào)度即調(diào)整回歸,屬于合理現(xiàn)象。

(3) 調(diào)度過程中左右履帶驅(qū)動輪的轉(zhuǎn)速差值變化較小,個別拐點單個驅(qū)動輪的轉(zhuǎn)速數(shù)值變化亦較為平緩,說明調(diào)度過程較平穩(wěn)。

其次,考慮引入過程誤差及測量誤差,但不采用濾波措施下的調(diào)度跟蹤情況。通過隨機加入符合統(tǒng)計規(guī)律的打滑率與位姿測量誤差,觀察調(diào)度跟蹤仿真效果。圖7展示了調(diào)度過程中機身中點的坐標、位姿偏差以及由控制指令u=(vc,ωc)T換算配置的左右驅(qū)動輪轉(zhuǎn)速等變化情況。

圖7對比圖6可知,仿真調(diào)度軌跡逐漸與期望軌跡重合,但機身中心位姿偏差未能穩(wěn)定地趨近于零;履帶驅(qū)動輪的轉(zhuǎn)速變化起伏較大。

圖7 噪聲影響下的調(diào)度跟蹤Fig.7 The tracking results considering noises

最后,針對不可避免的過程誤差與觀測誤差,采用圖4所展示的SVD-Unscented 卡爾曼濾波來估計機身相對于規(guī)劃路徑的實際偏差,用以修正當前調(diào)度步驟下的參考控制指令,更好地跟蹤規(guī)劃路徑、完成調(diào)度糾偏任務。

調(diào)度仿真結(jié)果如圖8所示?？梢钥闯?機身移動軌跡與規(guī)劃軌跡非常接近,對應的左右履帶驅(qū)動輪轉(zhuǎn)速差值變化較小,除個別拐點外,單個驅(qū)動輪的轉(zhuǎn)速數(shù)值變化在小范圍內(nèi)變化,說明調(diào)度過程平穩(wěn)。對應的位姿偏差曲線顯示機身位姿偏差Ye與θe均漸趨于0,但是偏差Xe則沒能如期望的那樣同步衰減到零值附近,而是在0.25 m 上下浮動。其原因在于:回顧圖1及式(1),大地坐標系XOY中的坐標差Xd-Xc與Yd-Yc分別向機身坐標系xOy的兩軸投影,當θc接近0°時,兩者在機身坐標系x軸上的分量和多于在y軸上的分量和,即只要坐標差Xd-Xc或Yd-Yc不為零,xe必然大于ye;反之當θc接近90°時,只要坐標差Xd-Xc或Yd-Yc不為零,ye必然大于xe。誤差存在時,Xd-Xc或Yd-Yc一般不為0,故而當θc接近0°時,位姿偏差xe不會趨于0。

圖8 基于SVD-Unscented 卡爾曼濾波的調(diào)度跟蹤Fig.8 The tracking results using SVD-Unscented Kalman Filtering

4 結(jié) 論

(1) 通過分析機身行進過程中機構(gòu)執(zhí)行誤差、觀測誤差,基于理論及實驗分析總結(jié)了履帶打滑率、位姿測量誤差的統(tǒng)計規(guī)律。針對這兩類誤差,提出將SVD-Unscented 卡爾曼濾波估計引入控制策略以修正參考控制指令,能有效減小誤差對跟蹤控制帶來的影響。所提出的SVD-Unscented 卡爾曼濾波算法避免了實際計算中遭遇因協(xié)方差矩陣病態(tài)而導致運算中斷。

(2) 提出的基于位姿偏差模型的控制策略結(jié)構(gòu)簡單、調(diào)度有效,能使機身在有限的調(diào)度周期內(nèi)實現(xiàn)向目標巷道的平穩(wěn)過渡,位姿偏差收斂性良好。在考慮誤差的情況下,采用SVD-Unscented 卡爾曼濾波對實際的機身位姿偏差進行濾波估計并修正控制指令,其調(diào)度跟蹤效果極佳且重復性良好,實現(xiàn)過程簡單高效,能為井下有限空間內(nèi)的掘進機自動糾偏提供參考。