吳明陽(yáng), 李小波, 代嘉惠, 雷世威, 潘長(zhǎng)松, 薛春榮
(中煤科工集團(tuán)重慶研究院有限公司, 重慶 400037)
煤礦機(jī)器人是煤礦智能化的重要載體,研發(fā)和應(yīng)用煤礦機(jī)器人是實(shí)現(xiàn)煤礦無(wú)人化的重要途徑[1]。國(guó)家煤礦安全監(jiān)察局發(fā)布的《煤礦機(jī)器人重點(diǎn)研發(fā)目錄》建議將機(jī)器人納入煤礦重點(diǎn)科技專項(xiàng)范圍[2],以提升煤礦智能化水平,逐步實(shí)現(xiàn)煤礦少人及無(wú)人化。
路徑跟蹤控制是實(shí)現(xiàn)煤礦機(jī)器人自主導(dǎo)航行走的關(guān)鍵技術(shù)之一。當(dāng)完成規(guī)劃任務(wù)后,需要利用跟蹤算法計(jì)算機(jī)器人執(zhí)行器目標(biāo)輸入,以保證機(jī)器人跟蹤預(yù)定的路徑[3]?,F(xiàn)有的機(jī)器人路徑跟蹤算法包括純追蹤算法[4]、滑??刂扑惴╗5-6]、模型預(yù)測(cè)控制算法[7]等。其中純追蹤算法簡(jiǎn)單、直觀,控制參數(shù)少,在低速場(chǎng)合跟蹤效果良好,因此,成為當(dāng)前移動(dòng)機(jī)器人導(dǎo)航和控制中廣泛應(yīng)用的路徑跟蹤算法之一[8-9]。前視距離是影響純追蹤算法跟蹤精度的重要因素,為實(shí)現(xiàn)前視距離的自適應(yīng)調(diào)整,李逃昌等[10]提出基于模糊理論調(diào)整純追蹤算法的前視距離;李革等[11]通過(guò)定義以速度和彎度為自變量的函數(shù),實(shí)現(xiàn)前視距離調(diào)整,平均跟蹤誤差為0.077 m。
已有的路徑跟蹤算法均需采集機(jī)器人兩側(cè)驅(qū)動(dòng)輪轉(zhuǎn)速作為反饋量進(jìn)行轉(zhuǎn)速閉環(huán)控制,對(duì)于開關(guān)閥控制不可閉環(huán)調(diào)速的液壓履帶移動(dòng)平臺(tái)并不完全適用。由于成本、加工難易度及用戶喜好等因素,開關(guān)閥控液壓履帶移動(dòng)平臺(tái)在礦業(yè)、農(nóng)業(yè)、軍工等領(lǐng)域應(yīng)用十分廣泛。目前,在掘進(jìn)機(jī)[12]、鉆孔機(jī)[13]、噴漿機(jī)[14]等大中型煤礦機(jī)械中均大量使用該類型底盤。因此,研究開關(guān)閥控履帶移動(dòng)平臺(tái)的路徑跟蹤控制對(duì)于提高煤礦機(jī)械智能化水平具有重要意義。
針對(duì)開關(guān)閥控履帶移動(dòng)平臺(tái)路徑跟蹤問(wèn)題,本文借鑒純追蹤算法的思想,提出一種基于航向角偏差帶邊界層的bang-bang路徑跟蹤控制算法,并通過(guò)李雅普諾夫穩(wěn)定性定理分析了邊界層內(nèi)外系統(tǒng)狀態(tài)的穩(wěn)定性;通過(guò)仿真驗(yàn)證算法在不同前視距離下對(duì)矩形目標(biāo)路徑的跟蹤效果,并與純追蹤算法進(jìn)行對(duì)比,分析兩者的路徑跟蹤性能差異;最后,在試驗(yàn)樣機(jī)上對(duì)bang-bang控制算法進(jìn)行了地面試驗(yàn)。
定義導(dǎo)航坐標(biāo)系為xoy,由于平臺(tái)速度較低,忽略縱向滑移和側(cè)向滑移,假設(shè)重心與幾何中心重合,則履帶移動(dòng)平臺(tái)點(diǎn)追蹤簡(jiǎn)化運(yùn)動(dòng)學(xué)模型如圖1所示。圖1中,(x,y)為平臺(tái)實(shí)時(shí)位置;(xr,yr)為當(dāng)前時(shí)刻追蹤的目標(biāo)路徑點(diǎn);θ為航向角;θerr為航向角偏差;ρ為當(dāng)前位置和目標(biāo)路徑上目標(biāo)點(diǎn)的距離;r為純追蹤算法轉(zhuǎn)彎半徑;l為純追蹤算法前視距離;a,b分別為機(jī)器人局部坐標(biāo)系下前視點(diǎn)到機(jī)器人幾何中心的側(cè)向距離和縱向距離;d=r-a;Δx,Δy分別為全局坐標(biāo)系下目標(biāo)路徑點(diǎn)到平臺(tái)當(dāng)前位置的距離在x,y方向的分量。
圖1 履帶移動(dòng)平臺(tái)點(diǎn)追蹤簡(jiǎn)化運(yùn)動(dòng)學(xué)模型Fig.1 Simplified kinematics model of point tracking for crawler mobile platform
履帶移動(dòng)平臺(tái)的動(dòng)力學(xué)響應(yīng)可近似為一階環(huán)節(jié),定義ρ、θerr、速度v及航向角速度ω為狀態(tài)變量,根據(jù)幾何及運(yùn)動(dòng)學(xué)關(guān)系得到點(diǎn)追蹤模型[15]:
(1)
式中:Tv和Tω為一階系統(tǒng)時(shí)間常數(shù);uv和uω分別為速度及航向角速度的虛擬控制輸入。
定義左右兩側(cè)履帶線速度為vl和vr,則其與虛擬控制輸入的關(guān)系為
(2)
式中h為履帶間距。
純追蹤算法是一種幾何控制算法,可模擬人駕駛車輛行為對(duì)平臺(tái)進(jìn)行控制。純追蹤算法僅包含前視距離l一個(gè)可調(diào)參數(shù),在平臺(tái)運(yùn)動(dòng)過(guò)程中,平臺(tái)將依據(jù)自身位置和目標(biāo)路徑搜尋目標(biāo)路徑前進(jìn)方向上不小于前視距離且與前視距離最接近的離散路徑點(diǎn)作為目標(biāo)點(diǎn),然后基于幾何關(guān)系獲得目標(biāo)曲率。設(shè)目標(biāo)曲率為γ,根據(jù)圖1可得
(3)
因?yàn)閟inθerr=a/l,所以,式(3)可寫為
(4)
從式(4)可看出,純追蹤算法本質(zhì)上是依據(jù)航向角偏差進(jìn)行控制,算法以前視距離l作為調(diào)整參數(shù),選擇較大的l可使跟蹤平穩(wěn),但收斂時(shí)間變長(zhǎng);選擇較小的l可提高跟蹤精度,但l過(guò)小可能造成振蕩甚至不穩(wěn)定。
定義速度控制輸入為uv=v2,結(jié)合式(4)可得到航向角速度輸入量:
uω=v2γ
(5)
在獲得虛擬控制量uv和uω后,利用式(2)可得到履帶的目標(biāo)速度。根據(jù)式(2),純追蹤算法需要通過(guò)控制兩側(cè)履帶速度得到虛擬控制量uv和uω,但是開關(guān)閥控履帶移動(dòng)平臺(tái)兩側(cè)履帶不可閉環(huán)調(diào)速,因此,純追蹤算法無(wú)法直接在開關(guān)閥控履帶移動(dòng)平臺(tái)上應(yīng)用。
bang-bang控制又稱繼電控制,適用于開關(guān)控制輸入系統(tǒng),便于編程實(shí)現(xiàn)及調(diào)試,參數(shù)校準(zhǔn)及調(diào)整簡(jiǎn)單,在航天、液壓等領(lǐng)域均有應(yīng)用。bang-bang控制屬于變結(jié)構(gòu)控制,可在2種狀態(tài)之間切換,開關(guān)閥控履帶移動(dòng)平臺(tái)的控制信號(hào)包含開、關(guān)2種狀態(tài),因此,本文基于航向角偏差,利用bang-bang控制實(shí)現(xiàn)平臺(tái)的路徑跟蹤。
(6)
(7)
根據(jù)式(5),uω的符號(hào)與v2和γ有關(guān),由于vr=uv>0,故sign(uω)=sign(γ)。根據(jù)式(4),由于前視距離l>0,故sign(γ)=sign(θerr)。綜上可得,sign(uω)=sign(θerr),故式(6)、式(7)中uω可以用θerr替換:
(8)
(9)
由于存在擾動(dòng)或傳感器噪聲,式(8)、式(9)會(huì)造成電磁閥頻繁切換。為減小擾動(dòng)對(duì)系統(tǒng)的影響及電磁閥切換頻率,借鑒滑??刂浦袦p小抖振的方法,引入邊界層,則控制律修改為
(10)
(11)
式中ε為邊界層厚度,rad,0<ε?π/2,ε越大,電磁閥切換頻率越低,但跟蹤精度也會(huì)下降。
在實(shí)際應(yīng)用中,應(yīng)在保證滿足跟蹤精度要求的前提下,選擇較大的ε,以保護(hù)執(zhí)行器。
對(duì)于開關(guān)閥控制履帶移動(dòng)平臺(tái),其兩側(cè)履帶的線速度大小近似相等,即vl≈vr。因此,根據(jù)式(2)可得到在式(10)、式(11)作用下的虛擬控制輸入:
(12)
(13)
式中:v3=(vr+vl)/2;ω0=2(vr+vl)/h。
從式(12)、式(13)可知,當(dāng)航向角偏差超過(guò)邊界層時(shí),移動(dòng)平臺(tái)通過(guò)原地轉(zhuǎn)向調(diào)整姿態(tài),從而減小航向角偏差;當(dāng)航向角偏差在邊界層之內(nèi)時(shí),平臺(tái)朝目標(biāo)點(diǎn)直線行駛。因此,該算法適用于直線或折線目標(biāo)路徑。
穩(wěn)定是控制系統(tǒng)的重要性能,也是系統(tǒng)能夠正常運(yùn)行的首要條件??刂葡到y(tǒng)在實(shí)際運(yùn)行過(guò)程中,會(huì)受到內(nèi)外因素的擾動(dòng)。如果系統(tǒng)不穩(wěn)定,任何微小的擾動(dòng)作用都將導(dǎo)致系統(tǒng)偏離原來(lái)的平衡狀態(tài),并隨著時(shí)間發(fā)散。因而,分析閉環(huán)系統(tǒng)的穩(wěn)定性,是控制理論的基本任務(wù)之一。李雅普諾夫穩(wěn)定性理論[16]是控制領(lǐng)域判斷系統(tǒng)穩(wěn)定性的普遍理論,不僅適用于單變量、線性、定常系統(tǒng),對(duì)于多變量、非線性、時(shí)變系統(tǒng)同樣適用。由于本文中的閉環(huán)控制系統(tǒng)為非線性系統(tǒng),證明其李雅普諾夫穩(wěn)定性,可判斷當(dāng)系統(tǒng)狀態(tài)在平衡點(diǎn)鄰域內(nèi)不會(huì)發(fā)散,即航向角偏差、到目標(biāo)點(diǎn)的距離等關(guān)鍵狀態(tài)會(huì)收斂至原點(diǎn)附近的鄰域內(nèi),從而保證路徑跟蹤正常運(yùn)行。
根據(jù)式(12)、式(13),在路徑跟蹤過(guò)程中存在原地姿態(tài)調(diào)整和直線行駛2種動(dòng)態(tài)過(guò)程,2種動(dòng)態(tài)過(guò)程根據(jù)θerr的值進(jìn)行切換。下面分別討論2種動(dòng)態(tài)過(guò)程中閉環(huán)系統(tǒng)的穩(wěn)定性。
移動(dòng)平臺(tái)在原地調(diào)整姿態(tài)下,|θerr|≥ε,將式(12)、式(13)代入式(1),得
(14)
定義該過(guò)程初始時(shí)刻速度v(0)=v0,v在原點(diǎn)漸進(jìn)穩(wěn)定,其解為
v=v0exp(-t/Tv)
(15)
式中t為時(shí)間。
將式(15)代入式(14),消去v,得
(16)
(17)
基于李雅普諾夫穩(wěn)定性理論分析簡(jiǎn)化后狀態(tài)空間模型的穩(wěn)定性,得出如下結(jié)論:當(dāng)|θerr|≥ε時(shí),在式(12)、式(13)的作用下,Tω>0且ω0>0時(shí),系統(tǒng)在原點(diǎn)是李雅普諾夫穩(wěn)定的。下面對(duì)該結(jié)論進(jìn)行證明。
選取備選李雅普諾夫函數(shù):
(18)
當(dāng)θerr≠0,ω≠0時(shí),因?yàn)棣?>0,Tω>0,得到2Tωω0θerrsign(θerr)>0。
(19)
當(dāng)θerr=0,ω=0時(shí),V=0。
(20)
移動(dòng)平臺(tái)在沿直線行駛狀態(tài)下,|θerr|<ε,將式(12)、式(13)代入式(1),得
(21)
分析ρ和v的穩(wěn)定性,得出如下結(jié)論:當(dāng)|θerr|<ε時(shí),在式(12)、式(13)的作用下,v在平衡點(diǎn)v3處是漸進(jìn)穩(wěn)定的,ρ在原點(diǎn)處是李雅普諾夫穩(wěn)定的。下面對(duì)該結(jié)論進(jìn)行證明。
先證明v在平衡點(diǎn)v3處漸進(jìn)穩(wěn)定。在該過(guò)程的初始時(shí)刻,有v(0)=0,根據(jù)式(21)可得到v(t)=v3-v3exp(-t/Tv)。因此,v是漸進(jìn)穩(wěn)定的,其平衡點(diǎn)為v3。當(dāng)t>0時(shí),v>0且最終收斂至v3。
下面證明ρ在原點(diǎn)處李雅普諾夫穩(wěn)定。當(dāng)t>0時(shí),選擇備選李雅普諾夫函數(shù):
(22)
對(duì)式(22),當(dāng)且僅當(dāng)ρ=0時(shí),V=0,否則,V>0。對(duì)V求導(dǎo),并代入v(t)可得
(23)
上述證明過(guò)程說(shuō)明,當(dāng)|θerr|<ε且ε?π/2時(shí),狀態(tài)θerr和ω并不影響ρ和v的穩(wěn)定性。但是,狀態(tài)θerr在原點(diǎn)是不穩(wěn)定的。為便于分析,僅考慮ρ和v到達(dá)穩(wěn)態(tài)的情況,此時(shí),ρ=l,v=v3。在θerr=0及ω=0附近線性化,sinθerr≈θerr,從而可得
(24)
線性化系統(tǒng)的狀態(tài)矩陣存在特征值v3/l,因此,當(dāng)θerr≠0時(shí),θerr將發(fā)散,直到θerr超過(guò)邊界層,控制律使其重新收斂到ε內(nèi)。θerr的發(fā)散速度取決于v3/l,選擇較大的前視距離可減小θerr的發(fā)散速度,避免平臺(tái)頻繁調(diào)整姿態(tài)。
為驗(yàn)證上述控制算法的路徑跟蹤性能,以中煤科工集團(tuán)重慶研究院的履帶移動(dòng)平臺(tái)為原型,在Recurdyn中搭建動(dòng)力學(xué)仿真模型,同時(shí)在Matlab/Simulink平臺(tái)建立路徑跟蹤控制算法,通過(guò)Recurdyn和Matlab/Simulink聯(lián)合仿真驗(yàn)證算法的有效性,分析前視距離對(duì)控制算法的影響,并與純追蹤算法進(jìn)行對(duì)比。
采用矩形路徑作為平臺(tái)的目標(biāo)路徑,設(shè)置ε=0.087 rad,初始平臺(tái)位置在(0,0)附近,初始航向角為0,平臺(tái)直線行駛速度為0.15 m/s,采樣頻率為10 Hz,前視距離l分別為0.4 m和0.8 m,仿真結(jié)果如圖2所示。路徑跟蹤誤差有多種定義方式,本文基于文獻(xiàn)[17]中的方法,定義路徑跟蹤誤差e為平臺(tái)幾何中心到目標(biāo)路徑的最短距離。
從圖2(a)可看出,當(dāng)前視距離為0.4 m和0.8 m時(shí),算法對(duì)直線目標(biāo)路徑均具有較高的跟蹤精度,但在轉(zhuǎn)角處都出現(xiàn)了“抄近道”的現(xiàn)象。這是由于算法總是根據(jù)前視距離的大小預(yù)瞄目標(biāo)路徑點(diǎn),根據(jù)當(dāng)前位置和目標(biāo)位置提前做出轉(zhuǎn)向動(dòng)作。從圖2(b)可看出,因?yàn)槌跏键c(diǎn)與目標(biāo)路徑相距較遠(yuǎn),所以最大跟蹤誤差出現(xiàn)在初始點(diǎn)處,當(dāng)平臺(tái)開始運(yùn)動(dòng)后,誤差逐漸減小。在穩(wěn)態(tài)跟蹤階段,路徑跟蹤誤差峰值出現(xiàn)在轉(zhuǎn)角處,當(dāng)前視距離為0.4 m時(shí),轉(zhuǎn)角處最大誤差為0.180 9 m,前視距離0.8 m時(shí),轉(zhuǎn)角處最大誤差為0.371 5 m。
(a) 運(yùn)動(dòng)路徑
(b) 跟蹤誤差
采用平均誤差和穩(wěn)態(tài)誤差作為統(tǒng)計(jì)指標(biāo)衡量路徑跟蹤質(zhì)量[18],平均誤差為整個(gè)跟蹤過(guò)程路徑跟蹤誤差的平均值,穩(wěn)態(tài)誤差為路徑跟蹤誤差首次到達(dá)0之后路徑跟蹤誤差平均值,統(tǒng)計(jì)結(jié)果見(jiàn)表1。從表1可看出,在保證穩(wěn)定的前提下,選擇較小的前視距離可提高路徑跟蹤精度。
表1 不同前視距離下路徑跟蹤仿真統(tǒng)計(jì)結(jié)果Table 1 Statistical results of path tracking simulation with different look-ahead distance m
設(shè)置純追蹤算法前視距離為0.4 m,且履帶驅(qū)動(dòng)輪可調(diào)速,其余條件與上文相同,對(duì)純追蹤算法和bang-bang控制算法進(jìn)行對(duì)比仿真分析,結(jié)果如圖3所示。
從圖3可看出,直線跟蹤階段,純追蹤算法的跟蹤誤差略小于bang-bang控制算法;在整個(gè)跟蹤過(guò)程中,純追蹤算法的平均跟蹤誤差為0.063 8 m,穩(wěn)態(tài)跟蹤誤差為0.030 4 m,也略優(yōu)于bang-bang控制算法;但在轉(zhuǎn)角處,純追蹤算法出現(xiàn)了明顯的超調(diào),跟蹤誤差峰值為0.348 9 m,高于bang-bang控制算法。
對(duì)比2種算法的跟蹤效率,純追蹤算法跟蹤至最終目標(biāo)點(diǎn)耗時(shí)約為208.8 s,bang-bang控制算法的耗時(shí)為284.8 s,純追蹤算法效率高于bang-bang控制算法,其主要原因在于帶調(diào)速的純追蹤算法可以在速度為0.1 m/s的前提下持續(xù)跟蹤目標(biāo)點(diǎn)。
(a) 平臺(tái)運(yùn)動(dòng)路徑
(b) 跟蹤誤差
本文試驗(yàn)樣機(jī)由中煤科工集團(tuán)重慶研究院履帶式移動(dòng)平臺(tái)進(jìn)行改裝得到,如圖4所示。樣機(jī)質(zhì)量約為8 t,采用液壓驅(qū)動(dòng)的履帶式摩擦底盤,履帶間距為0.93 m,最大線速度約為0.15 m/s。移動(dòng)平臺(tái)電控箱上方安裝有1個(gè)激光雷達(dá)和1個(gè)六軸慣性測(cè)量單元(Inertial Measurement Unit,IMU),用于同步定位與建圖(Simultaneous Localization and Mapping,SLAM)。激光雷達(dá)型號(hào)為Velodyne VLP-16,測(cè)量距離為100 m,檢測(cè)精度為±0.03 m。IMU型號(hào)為L(zhǎng)PMS-BE1,其歐拉角測(cè)量分辨率小于0.1°,靜態(tài)、動(dòng)態(tài)測(cè)量精度小于2°。工控機(jī)用于SLAM和路徑跟蹤控制,PLC用于底盤電磁閥控制,工控機(jī)與PLC之間通過(guò)以太網(wǎng)Modbus協(xié)議通信。
自主導(dǎo)航行走系統(tǒng)架構(gòu)如圖5所示。
圖5 自主導(dǎo)航行走系統(tǒng)架構(gòu)Fig.5 Architecture of autonomous navigation system
采用激光雷達(dá)及IMU建立測(cè)試場(chǎng)地的二維地圖,然后基于地圖坐標(biāo)系規(guī)劃目標(biāo)路徑。路徑函數(shù)為y=-0.051 2x-0.143 5,起始目標(biāo)點(diǎn)坐標(biāo)為(2.4,-0.266 5)。履帶移動(dòng)平臺(tái)以約0.05 m/s的速度跟蹤目標(biāo)路徑,SLAM以10 Hz的頻率實(shí)時(shí)輸出激光雷達(dá)位置,通過(guò)坐標(biāo)變換得到履帶移動(dòng)平臺(tái)幾何中心的位置。設(shè)控制算法前視距離l=0.5 m,ε=0.087 rad。進(jìn)行2次試驗(yàn),試驗(yàn)1中履帶移動(dòng)平臺(tái)初始位置為(0.729 1,-0.262 7),試驗(yàn)2中履帶移動(dòng)平臺(tái)初始位置為(1.520 7,-0.232 8),試驗(yàn)結(jié)果如圖6所示。
(a) 平臺(tái)運(yùn)動(dòng)路徑
(b) 跟蹤誤差
從圖6(a)可看出,履帶移動(dòng)平臺(tái)啟動(dòng)后迅速向目標(biāo)路徑運(yùn)動(dòng),在行駛過(guò)程中,當(dāng)其偏離目標(biāo)路徑時(shí),會(huì)調(diào)整前進(jìn)方向,回到目標(biāo)路徑上。從圖6(b)可看出,初始誤差為履帶移動(dòng)平臺(tái)與起始目標(biāo)點(diǎn)的距離,當(dāng)履帶移動(dòng)平臺(tái)開始運(yùn)動(dòng)后,誤差逐漸收斂至0附近。
2次試驗(yàn)的統(tǒng)計(jì)結(jié)果見(jiàn)表2。因?yàn)槠骄`差與初始位置有關(guān),所以試驗(yàn)1的平均誤差大于試驗(yàn)2;2次試驗(yàn)穩(wěn)態(tài)誤差均為0.02 m左右,說(shuō)明系統(tǒng)路徑跟蹤穩(wěn)態(tài)誤差小且一致性較好。
表2 路徑跟蹤試驗(yàn)統(tǒng)計(jì)結(jié)果Table 2 Statistical results of path tracking experiments
(1) bang-bang控制算法的平均跟蹤誤差及穩(wěn)態(tài)跟蹤誤差略高于帶閉環(huán)調(diào)速的純追蹤算法,但在轉(zhuǎn)角處的跟蹤誤差小于純追蹤算法。
(2) 仿真分析和樣機(jī)試驗(yàn)結(jié)果表明,bang-bang控制算法可用于無(wú)閉環(huán)調(diào)速履帶移動(dòng)平臺(tái)的直線或折線類型路徑跟蹤,且穩(wěn)態(tài)誤差均小于9 cm。
(3) 在保證穩(wěn)定的前提下,bang-bang控制算法可選擇較小的前視距離,以獲得更高的跟蹤精度。
(4) 純追蹤算法僅包含前視距離1個(gè)調(diào)整參數(shù),而bang-bang控制算法引入了邊界層厚度,該參數(shù)對(duì)路徑跟蹤精度及跟蹤效率有重要影響。邊界層厚度越大,則跟蹤精度越低,跟蹤效率越高。下一步將對(duì)前視距離和邊界層厚度的自適應(yīng)調(diào)整進(jìn)行研究。