基于SVD-Unscented卡爾曼濾波的掘進(jìn)機(jī)行進(jìn)調(diào)度糾偏研究

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

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

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

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

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

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

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

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

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

1 掘進(jìn)機(jī)行進(jìn)調(diào)度需求及建模基礎(chǔ)

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

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

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

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

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

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

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

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

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

對(duì)pe取微分[20-21],得

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

當(dāng)xe=0 時(shí),考察Lyapunov 函數(shù)

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

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

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

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

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

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

1.3 控制律簡(jiǎn)化及收斂性證明

為方便工業(yè)應(yīng)用,在保證pe有界且趨于0 的前提下,可將式(7)適當(dāng)簡(jiǎn)化。考慮機(jī)身轉(zhuǎn)向角速度ωc較其行進(jìn)速度vc略小,且實(shí)際運(yùn)行時(shí)vdxe在數(shù)值上遠(yuǎn)小于1;θe與sinθe同號(hào)。基于此,將式(7)簡(jiǎn)化為

式中,kx、ky、kθ取值為正,在此分別稱(chēng)為偏差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)能使機(jī)身位姿偏差pe逐漸收斂為0。

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

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

2.1 過(guò)程誤差

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

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

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

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

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

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

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

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

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

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

2.2 觀測(cè)誤差

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

圖2 掘進(jìn)機(jī)自主導(dǎo)航模擬平臺(tái)Fig.2 Experimental platform for the autonomous navigation of the road-header

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

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

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

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

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

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

式中,εk為控制實(shí)施的過(guò)程誤差;δk+1為位姿偏差的觀測(cè)誤差。

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

圖4 基于SVD-Unscented 卡爾曼濾波的掘進(jìn)機(jī)行進(jìn)調(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)所描述的非線(xiàn)性系統(tǒng)改寫(xiě)為

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

(1) 空心點(diǎn)表示實(shí)際機(jī)身移動(dòng)軌跡,與實(shí)心圓點(diǎn)所示的規(guī)劃軌跡重合度極高。

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

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

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

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

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

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

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

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

4 結(jié) 論

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

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