江 松 武露云 付信凱 顧清華 洪 勇 章 賽 盧才武
(1.西安建筑科技大學(xué)資源工程學(xué)院,陜西 西安 710055;2.中鋼集團馬鞍山礦山研究總院股份有限公司,安徽 馬鞍山 243000;3.金屬礦山安全與健康國家重點實驗室,安徽 馬鞍山 243000;4.中鋼集團山東富全礦業(yè)有限公司,山東 濟寧 272000)
隨著智慧礦山理念的提出,礦山智能化[1]和無人化建設(shè)[2]正在不斷推進。 無軌膠輪車[3]作為井工礦重要的輔助運輸設(shè)備,主要用于井下設(shè)備和人員的運輸[4],因其爬坡能力強、載重能力大、運輸成本低等優(yōu)點被廣泛應(yīng)用。 井下工作環(huán)境惡劣,巷道工況復(fù)雜、岔路口和視野盲區(qū)較多、突發(fā)危險情況較多,人為駕駛極易引發(fā)安全事故。 因此,研究井下無軌膠輪車的無人駕駛技術(shù)[5],有利于減少井下人工運輸事故和經(jīng)濟損失,降低人工運輸成本,提高采礦工作效率。軌跡跟蹤作為無人駕駛技術(shù)中尤為重要的一環(huán),其跟蹤控制的精度決定了車輛行駛的安全性。 因此,解決井下無軌膠輪車的軌跡跟蹤問題[6]對于推動礦山無人化[7]建設(shè)具有重要意義。
針對軌跡跟蹤問題,國內(nèi)外學(xué)者已經(jīng)提出了很多處理方法,目前主流的軌跡跟蹤控制方法有模型預(yù)測控制[8]、比例積分微分控制[9]、模糊控制[10]、線性二次最優(yōu)控制[11]、滑??刂芠12]。 趙紅專等[13]以車輛運動學(xué)的MPC 理論為依據(jù),引入車輛動態(tài)間距理念,提出了一種適用于彎道駕駛的跟蹤模型,提高了彎道工況下車與車之間的安全性、可靠性和駕駛者舒適度。方培俊等[14]提出了一種基于機理分析—數(shù)據(jù)驅(qū)動的車輛動力學(xué)串聯(lián)混合模型,進而設(shè)計了符合實際行駛環(huán)境的軌跡跟蹤控制方案。 LIN 等[15]采用線性3 自由度車輛動力學(xué)模型作為預(yù)測模型,提出了一種模型預(yù)測控制與模糊比例積分微分控制相結(jié)合的綜合控制方法。 CHEN 等[16]引入了前向—后向積分方案以生成受輪胎—道路摩擦限制的時間最優(yōu)速度分布,改進后的MPC 控制器對軌跡和速度的跟蹤性能良好。CHENG 等[17]考慮隨時間變化的車輛速度,利用具有有限頂點的多面體描述車輛縱向速度,建立了線性參數(shù)變化的橫向模型,設(shè)計了基于MPC 的車輛路徑跟蹤控制器。 YU 等[18]利用智能聚類算法AGA、QPSO和ACA 對LQR 參數(shù)優(yōu)化,實現(xiàn)了井下鉸接鏟運機的無人駕駛路徑跟蹤控制。 王宏偉等[19]針對無人車在復(fù)雜行駛工況下,非線性約束條件增多導(dǎo)致的算法復(fù)雜度升高、系統(tǒng)求解速度降低的問題,通過引入連續(xù)增加的懲罰系數(shù),提高了模型預(yù)測控制算法的求解效率。 馬浩楠等[20]針對井下無人車在狹窄巷道軌跡跟蹤難度大的問題,提出了一種基于空間偏差的模型預(yù)測算法(SMPC),通過降低預(yù)測方程維度,在一定程度上解決了控制器求解效率低的問題。
上述無人駕駛技術(shù)在城市道路場景中已經(jīng)逐漸成熟,但對于工況復(fù)雜巷道狹窄的礦山井下運輸場景,目前還未形成成熟的理論與技術(shù)體系。 車輛在運行過程中的速度[21]與軌跡是隨時間變化而變化的,現(xiàn)階段有關(guān)井下無軌膠輪車軌跡跟蹤控制的研究只考慮了軌跡變化對控制的影響,很少同時考慮速度變化[22]因素。 由于不同礦區(qū)井下工況的特殊性和差異性,如果不對井下場景做細分化處理,跟蹤控制方案可能面臨場景不適配等問題[23]。
為此,本研究設(shè)計了基于5 次多項式曲線的井下無軌膠輪車換道軌跡模型,來解決軌跡跟蹤過程中軌跡與速度的時變問題,并對井下不同行駛工況進行分類,驗證不同工況下橫縱向協(xié)調(diào)控制器的穩(wěn)定性和魯棒性。
車輛在行駛過程中,輪胎受到側(cè)向力、縱向力、垂向力等的作用,對車輛行駛過程中的穩(wěn)定性和安全性有著重要影響。 由此可見,在建立井下車輛動力學(xué)模型過程中有必要充分考慮輪胎特性對其運動控制的影響。
為了使本研究試驗更加符合實際情況,以無軌膠輪車的輪胎性能參數(shù)為例,通過Pacejka 提出的魔術(shù)公式半經(jīng)驗輪胎模型,建立輪胎側(cè)向力與側(cè)偏角之間的關(guān)系,從而計算出輪胎的側(cè)偏剛度。 魔術(shù)公式的一般表達式為
式中,A0為剛度因子;B0為形狀因子;C0為峰值因子;D0為曲率因子。
輪胎垂向載荷為10 000 N 時,輪胎側(cè)偏力與輪胎側(cè)偏角的擬合曲線如圖1 所示。A0、B0、C0取值可由擬合結(jié)果得到。
圖1 輪胎側(cè)偏剛度擬合曲線Fig.1 Fitting curve of tire lateral deflection stiffness
當(dāng)輪胎側(cè)偏角較小時,輪胎側(cè)向力可以近似表示為輪胎側(cè)偏角的線性函數(shù),此時曲線斜率即為輪胎的側(cè)偏剛度。
由于本研究在Frenet 坐標(biāo)系下實現(xiàn)了車輛橫縱向運動解耦控制,因此在建立車輛自由度模型時只需考慮橫向運動和橫擺運動2 個自由度,忽略縱向運動的影響。 由于井下運輸場景為低速小轉(zhuǎn)角場景,因此,其操縱特性可用線性2 自由度“自行車”模型來近似描述。 簡化后的動力學(xué)模型簡單穩(wěn)定,計算復(fù)雜度大幅減少,且能較好地適應(yīng)井下極限復(fù)雜工況。 本研究采用線性2 自由度模型作為前輪轉(zhuǎn)向無軌膠輪車動力學(xué)模型,如圖2 所示,其中XOY坐標(biāo)系為大地坐標(biāo)系,xoy坐標(biāo)系為車身坐標(biāo)系。
圖2 無軌膠輪車二自由度動力學(xué)模型Fig.2 Trackless tyred vehicle two-degree-of-freedom dynamics model
考慮輪胎側(cè)偏特性,對車輛進行受力分析,根據(jù)其受力平衡和力矩平衡可以得到:
式中,m為整車質(zhì)量,kg;ay為車身坐標(biāo)系下y方向加速度分量,m/ s2;為航向角導(dǎo)數(shù);I為轉(zhuǎn)動慣量,kg·m2;對δf做小角度假設(shè),則cosδf=1。
結(jié)合前后輪側(cè)偏角和側(cè)偏剛度可以得到無軌膠輪車動力學(xué)模型,本研究進一步將其轉(zhuǎn)化為矩陣形式可得:
式中,νx為車輛縱向速度,m/s;Cαf、Cαr為前后輪側(cè)偏剛度,N/rad;αf、αr為前后輪側(cè)偏角,rad。 該式即為橫向運動控制方程,通過控制δf,控制和,進而控制橫向位移y和航向角φ。 該模型能很好地反應(yīng)車輛的橫擺特性,不足在于未能考慮車輛的側(cè)傾動力學(xué)特性。 但是由于井下運輸場景為低速場景,因而可以近似忽略側(cè)傾因素對車輛橫向控制的影響,簡化后的模型精度仍能得到保證。
由于簡化后的模型不考慮車輛的側(cè)傾特性,只考慮橫向和橫擺運動,因此只需考慮橫向誤差和航向誤差。 本研究橫向誤差定義為車輛后軸中心與其在道路中心線上投影點之間的距離偏差,航向誤差定義為車輛航向與道路方向的偏差。 誤差模型如圖3 所示。
圖3 軌跡跟蹤誤差模型Fig.3 Trajectory tracking error model
誤差參數(shù)可以表示為
式中,νy為車輛橫向速度,m/s;為橫向誤差導(dǎo)數(shù);為航向誤差導(dǎo)數(shù);為參考軌跡航向角導(dǎo)數(shù)。 將式(5)代入式(4),化簡可得誤差矩陣
式中,A、B、C為參數(shù)系數(shù)矩陣和向量;err為誤差參數(shù)向量;為誤差參數(shù)矩陣的導(dǎo)數(shù)向量。
井下巷道如圖4 所示,井下車輛的工作環(huán)境惡劣、工況復(fù)雜,場景受到巷道寬度[24]限制,單一的橫向或縱向控制難以應(yīng)對井下突發(fā)情況。 橫縱向協(xié)同控制才是實現(xiàn)井下無軌膠輪車無人駕駛的關(guān)鍵。
圖4 井下巷道Fig.4 Underground roadway
本研究選擇5 次多項式法規(guī)劃無軌膠輪車的換道軌跡,基于5 次多項式曲線擬合出的車輛行駛軌跡曲率連續(xù)平滑無突變,而且保證了速度、位移和加速度連續(xù),使得控制更加穩(wěn)定。
采用5 次多項式曲線規(guī)劃的軌跡,其橫坐標(biāo)x是關(guān)于時間t的5 次多項式,縱坐標(biāo)y是關(guān)于x的5 次多項式擬合結(jié)果,規(guī)劃過程中需要對軌跡的曲率、速度和加速度進行限制。 基于5 次多項式規(guī)劃的軌跡可表示為
式中,a0~a5、b0~b5為多項式系數(shù)。
規(guī)劃后的橫向誤差和縱向誤差可以表示為
式中,xr為規(guī)劃軌跡橫坐標(biāo);yr為規(guī)劃軌跡縱坐標(biāo)。
線性二次型最優(yōu)控制是穩(wěn)定的軌跡跟蹤橫向控制方法,可利用較少的計算成本使系統(tǒng)達到較好的性能指標(biāo),也可以構(gòu)成閉環(huán)最優(yōu)控制。 因此,本研究采用離散空間LQR 控制設(shè)計橫向軌跡跟蹤器。 LQR 控制方法的本質(zhì)就是求解滿足約束xk+1=A·xk+B·uk的性能函數(shù)J取得最小值時的最優(yōu)控制量uk。 則有:
式中,xk為自變量矩陣;Q為半正定的狀態(tài)加權(quán)矩陣;R為正定的控制加權(quán)矩陣,兩者通常為對角矩陣;矩陣Q元素變大意味著狀態(tài)量能夠快速趨近于0;矩陣R元素變大意味著希望控制輸入能夠盡可能小,系統(tǒng)的狀態(tài)衰減將變慢;Q、R取值需根據(jù)具體的實際應(yīng)用場景而定。
經(jīng)過仿真調(diào)試驗證,最終Q、R取值為
本研究利用拉格朗日乘子法,迭代求解得到Riccati 方程的解P。P滿足
令反饋系數(shù)矩陣K=-(R+BT·P·B)-1·BT·P·A,最終得到優(yōu)化的控制量
在線求解Riccatti 方程會增加系統(tǒng)的計算復(fù)雜度,降低系統(tǒng)的響應(yīng)速度。 因此,本研究通過離線計算得到不同車速vx對應(yīng)的反饋矩陣K,進而得到不同車速下的反饋系數(shù),使用離線LQR 的控制系統(tǒng)實時性大大增強。
由式(7)和式(13)結(jié)合誤差反饋可得
由式(14)可知,若只使用LQR 控制算法,不論反饋系數(shù)K取何值,和err不可能同時為0,說明系統(tǒng)此時存在不可消除的穩(wěn)態(tài)誤差。 因此,為了消除系統(tǒng)穩(wěn)態(tài)誤差對橫向控制結(jié)果的影響,本研究通過前饋控制引入前輪偏轉(zhuǎn)角δf,待系統(tǒng)穩(wěn)定后,,此時
因此只需選取合適的δf,使得上式盡可能為0,即可消除系統(tǒng)的穩(wěn)態(tài)誤差。 對上式化簡后可得到
式中,k為曲率;K3為反饋系數(shù)矩陣第3 項。
井下運輸車輛由于其自身轉(zhuǎn)向不足的特性,即車輛需要更大的轉(zhuǎn)向角度來保持行進路線,在應(yīng)對復(fù)雜的行駛工況時,會增大實際軌跡與規(guī)劃軌跡之間的橫向誤差。 為了消除車輛自身特性導(dǎo)致的不利因素,減小橫向穩(wěn)態(tài)誤差,本研究提出了前饋補償控制策略,使用PID 的積分模塊對輸出的橫向誤差ed進行積分處理,將積分后的結(jié)果補償給前輪偏轉(zhuǎn)角。 當(dāng)?shù)缆非蔬^大時,車輛會因轉(zhuǎn)向不足導(dǎo)致方向盤轉(zhuǎn)角過小,此時需要前輪偏轉(zhuǎn)角加上補償量Δδf(式(17)),得到最終的方向盤轉(zhuǎn)角。 通過對改進前后橫向誤差結(jié)果對比可知,軌跡跟蹤的精度顯著提高。
式中,ki為積分項系數(shù)。
縱向控制采用了雙PID 控制器設(shè)計,速度PID控制器主要用于車輛車速的控制,通過調(diào)節(jié)比例項與積分項來減小規(guī)劃速度與實際速度之間的誤差值ev,位置PID 控制器主要負責(zé)控制車輛實際位置與規(guī)劃位置之間的縱向位置誤差es。 通過調(diào)節(jié)比例項提高系統(tǒng)的響應(yīng)速度,但比例項kp過大會增加穩(wěn)態(tài)誤差引起超調(diào);調(diào)節(jié)微分項kd減小超調(diào)量,提升控制的穩(wěn)定性。 最終使得系統(tǒng)滿足精度和穩(wěn)定性要求的系數(shù)分別為kp=3、ki=0、kd=0.1。
為了實現(xiàn)縱向控制器對縱向位置誤差和速度誤差的精確控制,還需要得到車輛準確的驅(qū)動制動參數(shù)。 因此,本研究依據(jù)某礦山井下運輸車的電機參數(shù),建立電機仿真模型,通過試驗得到車輛在加速減速過程中的油門和剎車開度值。 電機最大功率Pmax為40 kW,最大扭矩Mmax為160 N·m,通過計算可得到電機恒扭矩與恒功率輸出的電機轉(zhuǎn)速的臨界值ωc為2 388 r/m。 因此,轉(zhuǎn)速和扭矩關(guān)系可以表示為
式中,ω為電機轉(zhuǎn)速,r/m;thr為油門踏板開度。
利用該模型,并根據(jù)車輛在行使過程中的速度和加速度可以實時輸出車輛的油門踏板開度和剎車踏板開度,從而得到4 個物理量的關(guān)系,進一步可以擬合得到三維曲面關(guān)系,如圖5 所示。
圖5 油門剎車開度三維曲面Fig.5 Three-dimensional surface of the throttle brake opening
本研究使用5 次多項式曲線建立參考軌跡模型,以橫向控制器、前饋補償控制器和縱向速度控制器作為核心控制器,通過Carsim 與Matlab/simulink 聯(lián)合仿真,分別在井下雙車道、單車道與顛簸路面場景下進行軌跡跟蹤控制算法驗證。
本研究以Carsim 中重型皮卡車型為模擬車型,并以某礦山的井下無軌膠輪車實車參數(shù)為標(biāo)準,修改相關(guān)模擬參數(shù),建立更加符合工程實際的車輛仿真模型,具體仿真參數(shù)取值見表1。
表1 井下無軌膠輪車動力學(xué)仿真參數(shù)Table 1 Basic parameters of underground transport vehicle dynamics
本研究通過實地考察,根據(jù)某礦山井下巷道寬度設(shè)計標(biāo)準和車輛運行速度標(biāo)準,設(shè)計了井下雙車道避障換道工況、單車道會車工況與顛簸路面工況,測試所設(shè)計控制器的精確性和魯棒性。 通過分析在不同工況下車輛的橫向誤差和縱向誤差驗證軌跡跟蹤的效果和精度,以及分析速度誤差、轉(zhuǎn)向角變化評價車輛運行過程中的穩(wěn)定性和舒適性。
工況1:井下雙車道超車或避障場景如圖6 所示。 其中黑色代表礦井巷道墻壁,巷道寬度為5. 3 m,設(shè)置車輛初速度為28.8 km/h,末速度為18 km/h,加速度為-2 m/s2;紅色表示車輛的規(guī)劃行駛路線,對應(yīng)的真實場景為車輛先減速駛?cè)胱兊儡嚨涝偌铀俪^前車或避開前方障礙物,車輛行駛過程中車速小于30 km/h,符合井下運輸車輛速度標(biāo)準。
圖6 雙車道避障工況Fig.6 Two-lane obstacle avoidance condition
工況2:井下單車道會車場景道路工況如圖7 所示。 由于單車道巷道狹窄,因此,運輸過程中車輛需要借助躲避硐室進行會車。 為驗證控制器在該場景下的可靠性,以5 次多項式模擬車輛的行駛路線,第一段曲線車輛的初速度為18 km/h,末速度為0,加速度為-1 m/s2,第二段曲線車輛初速度為0,末速度為18 km/h,加速度為2 m/s2,2 段曲線共同構(gòu)成車輛的軌跡。 對應(yīng)的真實場景為車輛先減速駛?cè)攵惚茼鲜彝\?完成會車后再加速駛出。
圖7 單車道會車工況Fig.7 Single-lane meeting condition
工況3:井下顛簸路面場景。 車輛在顛簸路面行駛過程中,輪胎在縱向和側(cè)向的載荷會發(fā)生改變,前輪垂向載荷增大,輪胎的側(cè)偏剛度增大,會對車輛軌跡跟蹤的控制效果產(chǎn)生影響。 本研究在建立車輛動力學(xué)模型時,考慮到車輛輪胎的側(cè)偏特性,通過改變輪胎的側(cè)偏剛度,模擬車輛在顛簸路面下的行駛狀態(tài),進一步驗證該控制器在顛簸路面場景下的穩(wěn)定性。顛簸路面下的車輛前輪側(cè)偏剛度為185 000 N/rad,后輪側(cè)偏剛度為205 000 N/rad。
由于井下行駛工況特殊,車輛運行速度小于30 km/h,因此仿真工況均為低速工況,3 種仿真工況下,車輛規(guī)劃軌跡與實際軌跡的誤差對比如圖8 所示。由圖8 可知:3 種工況下橫向控制誤差均小于5 cm,因此證明該控制器在井下具有較高的跟蹤精度。
圖8 3 種工況規(guī)劃軌跡與實際軌跡對比Fig.8 Comparison of planned trajectory and actual trajectory for three working conditions
雙車道工況下本研究對橫縱向控制器改進之后的仿真結(jié)果如圖9 所示。 由圖9 可知:速度PID 控制器對當(dāng)前車速有很好的控制效果,但是會引起一定的超調(diào),在7.8 s 處速度誤差最大為0.2 m/s。 圖9(c)為對位置PID 控制器調(diào)參前后控制效果對比。 分析可知:通過位置PID 對縱向控制改進之后,縱向位移誤差減小幅度明顯,最大減幅為10 cm。
圖9 雙車道工況改進前后仿真結(jié)果對比Fig.9 Comparison of simulation results before and after the improvement of the two-lane working condition
單車道會車工況下,改進前后橫向誤差與速度誤差對比分析結(jié)果如圖10 所示。 通過分析在減速和加速過程中的橫向誤差曲線,可知在減速路段誤差控制在8 cm 以內(nèi),加速路段誤差在10 cm 以內(nèi)。 本研究使用改進后的橫向控制器后,減速路段橫向誤差減小到2 cm 以內(nèi),加速路段誤差減小到5 cm 以內(nèi)。 由于井下單車道道路狹窄工況苛刻,因此LQR 橫向控制器誤差較大,前饋補償改進后的橫向最大誤差減少幅度在50%以上。 圖10(c)和圖10(d)為單車道減速路段和加速路段下實際速度與規(guī)劃速度的對比結(jié)果。通過對比分析可知:在車輛加速階段速度誤差較大,最大誤差出現(xiàn)在路段二3.2 s 處,此時速度誤差最大約為1 m/s。
圖10 單車道改進前后橫向誤差與速度誤差對比Fig.10 Comparison of lateral error and speed error before and after single-lane improvement
為驗證控制器在井下工況下的穩(wěn)定性和魯棒性,本研究分析了車輛在軌跡跟蹤過程中前輪偏轉(zhuǎn)角和航向誤差參數(shù),結(jié)果如圖11 所示。 由圖11 可知:在低速工況下車輛的前輪偏轉(zhuǎn)角變化平緩無明顯突變,航向誤差控制在±0.1 rad 以內(nèi),表明所提出的軌跡跟蹤控制方法在井下工況中也能保證車輛轉(zhuǎn)向的舒適性和穩(wěn)定性。
圖11 前輪轉(zhuǎn)角與航向誤差變化Fig.11 Change of front wheel angle and course error
為驗證本研究構(gòu)建的控制器在井下實際場景下的控制效果,通過使用實驗室小車于陜西某礦山井下進行了現(xiàn)場試驗。 該小車底盤為阿克曼轉(zhuǎn)向結(jié)構(gòu),搭載傳感器為速騰16 線激光雷達,采樣頻率為10 Hz;HIPNUC 的9 軸IMU,采樣頻率為400 Hz,機載計算機為Intel(R) Core(TM) i7-11390H CPU,16GBRAM,512G 固態(tài),編譯語言為C++,ROS 版本為Noetic,系統(tǒng)為Ubuntu 20. 04,小車使用激光SLAM 實現(xiàn)井下定位,定位誤差為厘米級。 車輛在3 種工況下多次運行的試驗結(jié)果見表2。 井下巷道2D 地圖如圖12 所示,井下現(xiàn)場試驗場景如圖13 所示。 通過對控制器多次軌跡跟蹤的誤差數(shù)據(jù)進行對比分析得出,3 種工況下橫向最大誤差為6.6 cm,縱向最大誤差為56 cm,速度最大誤差為0.5 m/s。 試驗結(jié)果表明:井下顛簸路面的確會對軌跡跟蹤誤差產(chǎn)生影響,但其誤差也在合理范圍內(nèi),因此,改進后的控制器在井下場景具有較好的應(yīng)用效果。
表2 3 種工況下最大跟蹤誤差結(jié)果Table 2 Maximum tracking error results under 3 working conditions
圖12 井下巷道2D 地圖Fig.12 2D map of underground roadway
圖13 井下顛簸路段與躲避硐室Fig.13 Underground bumpy road and escape chamber
(1)提出了基于PID 積分項前饋補償?shù)臋M向控制器,優(yōu)化了車輛輸出的前輪偏轉(zhuǎn)角,減小了橫向控制誤差,解決了因前輪偏轉(zhuǎn)角突變引起的方向盤抖動問題,改進后的橫向LQR 控制器轉(zhuǎn)向穩(wěn)定,更能適應(yīng)井下巷道工況。
(2)構(gòu)建了井下實車參數(shù)電機模型,通過試驗標(biāo)定得到油門、剎車與速度、加速度的變化關(guān)系,并利用PID 控制器調(diào)整車輛的位置誤差和速度誤差,同時解決了車輛運行過程中的速度和路徑的時變問題。
(3)通過Carsim 和Simulink 分別對井下雙車道、單車道和顛簸路面工況進行仿真分析,并在井下實際環(huán)境中進行了試驗驗證。 結(jié)果表明:該控制器能夠?qū)⒏櫿`差控制在較小范圍內(nèi),具有較高的控制精度和穩(wěn)定性。 但試驗中并未考慮車體質(zhì)量變化對跟蹤控制的影響,因此,建立更為精確的模型實現(xiàn)井下全局縱橫向耦合控制是下一步研究方向。