基于前饋補償LQR 與PID 的礦井無軌膠輪車橫縱向控制研究

江 松 武露云 付信凱 顧清華 洪 勇 章 賽 盧才武

(1.西安建筑科技大學資源工程學院,陜西 西安 710055;2.中鋼集團馬鞍山礦山研究總院股份有限公司,安徽 馬鞍山 243000;3.金屬礦山安全與健康國家重點實驗室,安徽 馬鞍山 243000;4.中鋼集團山東富全礦業有限公司,山東 濟寧 272000)

隨著智慧礦山理念的提出,礦山智能化[1]和無人化建設[2]正在不斷推進。 無軌膠輪車[3]作為井工礦重要的輔助運輸設備,主要用于井下設備和人員的運輸[4],因其爬坡能力強、載重能力大、運輸成本低等優點被廣泛應用。 井下工作環境惡劣,巷道工況復雜、岔路口和視野盲區較多、突發危險情況較多,人為駕駛極易引發安全事故。 因此,研究井下無軌膠輪車的無人駕駛技術[5],有利于減少井下人工運輸事故和經濟損失,降低人工運輸成本,提高采礦工作效率。軌跡跟蹤作為無人駕駛技術中尤為重要的一環,其跟蹤控制的精度決定了車輛行駛的安全性。 因此,解決井下無軌膠輪車的軌跡跟蹤問題[6]對于推動礦山無人化[7]建設具有重要意義。

針對軌跡跟蹤問題,國內外學者已經提出了很多處理方法,目前主流的軌跡跟蹤控制方法有模型預測控制[8]、比例積分微分控制[9]、模糊控制[10]、線性二次最優控制[11]、滑?？刂芠12]。 趙紅專等[13]以車輛運動學的MPC 理論為依據,引入車輛動態間距理念,提出了一種適用于彎道駕駛的跟蹤模型,提高了彎道工況下車與車之間的安全性、可靠性和駕駛者舒適度。方培俊等[14]提出了一種基于機理分析—數據驅動的車輛動力學串聯混合模型,進而設計了符合實際行駛環境的軌跡跟蹤控制方案。 LIN 等[15]采用線性3 自由度車輛動力學模型作為預測模型,提出了一種模型預測控制與模糊比例積分微分控制相結合的綜合控制方法。 CHEN 等[16]引入了前向—后向積分方案以生成受輪胎—道路摩擦限制的時間最優速度分布,改進后的MPC 控制器對軌跡和速度的跟蹤性能良好。CHENG 等[17]考慮隨時間變化的車輛速度,利用具有有限頂點的多面體描述車輛縱向速度,建立了線性參數變化的橫向模型,設計了基于MPC 的車輛路徑跟蹤控制器。 YU 等[18]利用智能聚類算法AGA、QPSO和ACA 對LQR 參數優化,實現了井下鉸接鏟運機的無人駕駛路徑跟蹤控制。 王宏偉等[19]針對無人車在復雜行駛工況下,非線性約束條件增多導致的算法復雜度升高、系統求解速度降低的問題,通過引入連續增加的懲罰系數,提高了模型預測控制算法的求解效率。 馬浩楠等[20]針對井下無人車在狹窄巷道軌跡跟蹤難度大的問題,提出了一種基于空間偏差的模型預測算法(SMPC),通過降低預測方程維度,在一定程度上解決了控制器求解效率低的問題。

上述無人駕駛技術在城市道路場景中已經逐漸成熟,但對于工況復雜巷道狹窄的礦山井下運輸場景,目前還未形成成熟的理論與技術體系。 車輛在運行過程中的速度[21]與軌跡是隨時間變化而變化的,現階段有關井下無軌膠輪車軌跡跟蹤控制的研究只考慮了軌跡變化對控制的影響,很少同時考慮速度變化[22]因素。 由于不同礦區井下工況的特殊性和差異性,如果不對井下場景做細分化處理,跟蹤控制方案可能面臨場景不適配等問題[23]。

為此,本研究設計了基于5 次多項式曲線的井下無軌膠輪車換道軌跡模型,來解決軌跡跟蹤過程中軌跡與速度的時變問題,并對井下不同行駛工況進行分類,驗證不同工況下橫縱向協調控制器的穩定性和魯棒性。

1 考慮輪胎側偏的車輛模型

1.1 Pacejka 經驗輪胎模型

車輛在行駛過程中,輪胎受到側向力、縱向力、垂向力等的作用,對車輛行駛過程中的穩定性和安全性有著重要影響。 由此可見,在建立井下車輛動力學模型過程中有必要充分考慮輪胎特性對其運動控制的影響。

為了使本研究試驗更加符合實際情況,以無軌膠輪車的輪胎性能參數為例,通過Pacejka 提出的魔術公式半經驗輪胎模型,建立輪胎側向力與側偏角之間的關系,從而計算出輪胎的側偏剛度。 魔術公式的一般表達式為

式中,A0為剛度因子;B0為形狀因子;C0為峰值因子;D0為曲率因子。

輪胎垂向載荷為10 000 N 時,輪胎側偏力與輪胎側偏角的擬合曲線如圖1 所示。A0、B0、C0取值可由擬合結果得到。

圖1 輪胎側偏剛度擬合曲線Fig.1 Fitting curve of tire lateral deflection stiffness

當輪胎側偏角較小時,輪胎側向力可以近似表示為輪胎側偏角的線性函數,此時曲線斜率即為輪胎的側偏剛度。

1.2 車輛動力學模型

由于本研究在Frenet 坐標系下實現了車輛橫縱向運動解耦控制,因此在建立車輛自由度模型時只需考慮橫向運動和橫擺運動2 個自由度,忽略縱向運動的影響。 由于井下運輸場景為低速小轉角場景,因此,其操縱特性可用線性2 自由度“自行車”模型來近似描述。 簡化后的動力學模型簡單穩定,計算復雜度大幅減少,且能較好地適應井下極限復雜工況。 本研究采用線性2 自由度模型作為前輪轉向無軌膠輪車動力學模型,如圖2 所示,其中XOY坐標系為大地坐標系,xoy坐標系為車身坐標系。

圖2 無軌膠輪車二自由度動力學模型Fig.2 Trackless tyred vehicle two-degree-of-freedom dynamics model

考慮輪胎側偏特性,對車輛進行受力分析,根據其受力平衡和力矩平衡可以得到:

式中,m為整車質量,kg;ay為車身坐標系下y方向加速度分量,m/ s2;為航向角導數;I為轉動慣量,kg·m2;對δf做小角度假設,則cosδf=1。

結合前后輪側偏角和側偏剛度可以得到無軌膠輪車動力學模型,本研究進一步將其轉化為矩陣形式可得:

式中,νx為車輛縱向速度,m/s;Cαf、Cαr為前后輪側偏剛度,N/rad;αf、αr為前后輪側偏角,rad。 該式即為橫向運動控制方程,通過控制δf,控制和,進而控制橫向位移y和航向角φ。 該模型能很好地反應車輛的橫擺特性,不足在于未能考慮車輛的側傾動力學特性。 但是由于井下運輸場景為低速場景,因而可以近似忽略側傾因素對車輛橫向控制的影響,簡化后的模型精度仍能得到保證。

1.3 跟蹤誤差模型

由于簡化后的模型不考慮車輛的側傾特性,只考慮橫向和橫擺運動,因此只需考慮橫向誤差和航向誤差。 本研究橫向誤差定義為車輛后軸中心與其在道路中心線上投影點之間的距離偏差,航向誤差定義為車輛航向與道路方向的偏差。 誤差模型如圖3 所示。

圖3 軌跡跟蹤誤差模型Fig.3 Trajectory tracking error model

誤差參數可以表示為

式中,νy為車輛橫向速度,m/s;為橫向誤差導數;為航向誤差導數;為參考軌跡航向角導數。 將式(5)代入式(4),化簡可得誤差矩陣

式中,A、B、C為參數系數矩陣和向量;err為誤差參數向量;為誤差參數矩陣的導數向量。

2 橫縱向控制器設計

井下巷道如圖4 所示,井下車輛的工作環境惡劣、工況復雜,場景受到巷道寬度[24]限制,單一的橫向或縱向控制難以應對井下突發情況。 橫縱向協同控制才是實現井下無軌膠輪車無人駕駛的關鍵。

圖4 井下巷道Fig.4 Underground roadway

2.1 軌跡規劃設計

本研究選擇5 次多項式法規劃無軌膠輪車的換道軌跡,基于5 次多項式曲線擬合出的車輛行駛軌跡曲率連續平滑無突變,而且保證了速度、位移和加速度連續,使得控制更加穩定。

采用5 次多項式曲線規劃的軌跡,其橫坐標x是關于時間t的5 次多項式,縱坐標y是關于x的5 次多項式擬合結果,規劃過程中需要對軌跡的曲率、速度和加速度進行限制。 基于5 次多項式規劃的軌跡可表示為

式中,a0～a5、b0～b5為多項式系數。

規劃后的橫向誤差和縱向誤差可以表示為

式中,xr為規劃軌跡橫坐標;yr為規劃軌跡縱坐標。

2.2 橫向LQR 控制器設計

線性二次型最優控制是穩定的軌跡跟蹤橫向控制方法,可利用較少的計算成本使系統達到較好的性能指標,也可以構成閉環最優控制。 因此,本研究采用離散空間LQR 控制設計橫向軌跡跟蹤器。 LQR 控制方法的本質就是求解滿足約束xk+1=A·xk+B·uk的性能函數J取得最小值時的最優控制量uk。 則有:

式中,xk為自變量矩陣;Q為半正定的狀態加權矩陣;R為正定的控制加權矩陣,兩者通常為對角矩陣;矩陣Q元素變大意味著狀態量能夠快速趨近于0;矩陣R元素變大意味著希望控制輸入能夠盡可能小,系統的狀態衰減將變慢;Q、R取值需根據具體的實際應用場景而定。

經過仿真調試驗證,最終Q、R取值為

本研究利用拉格朗日乘子法,迭代求解得到Riccati 方程的解P。P滿足

令反饋系數矩陣K=-(R+BT·P·B)-1·BT·P·A,最終得到優化的控制量

在線求解Riccatti 方程會增加系統的計算復雜度,降低系統的響應速度。 因此,本研究通過離線計算得到不同車速vx對應的反饋矩陣K,進而得到不同車速下的反饋系數,使用離線LQR 的控制系統實時性大大增強。

2.3 前饋補償控制器設計

由式(7)和式(13)結合誤差反饋可得

由式(14)可知,若只使用LQR 控制算法,不論反饋系數K取何值,和err不可能同時為0,說明系統此時存在不可消除的穩態誤差。 因此,為了消除系統穩態誤差對橫向控制結果的影響,本研究通過前饋控制引入前輪偏轉角δf,待系統穩定后,,此時

因此只需選取合適的δf,使得上式盡可能為0,即可消除系統的穩態誤差。 對上式化簡后可得到

式中,k為曲率;K3為反饋系數矩陣第3 項。

井下運輸車輛由于其自身轉向不足的特性,即車輛需要更大的轉向角度來保持行進路線,在應對復雜的行駛工況時,會增大實際軌跡與規劃軌跡之間的橫向誤差。 為了消除車輛自身特性導致的不利因素,減小橫向穩態誤差,本研究提出了前饋補償控制策略,使用PID 的積分模塊對輸出的橫向誤差ed進行積分處理,將積分后的結果補償給前輪偏轉角。 當道路曲率過大時,車輛會因轉向不足導致方向盤轉角過小,此時需要前輪偏轉角加上補償量Δδf(式(17)),得到最終的方向盤轉角。 通過對改進前后橫向誤差結果對比可知,軌跡跟蹤的精度顯著提高。

式中,ki為積分項系數。

2.4 縱向PID 控制器設計

縱向控制采用了雙PID 控制器設計,速度PID控制器主要用于車輛車速的控制,通過調節比例項與積分項來減小規劃速度與實際速度之間的誤差值ev,位置PID 控制器主要負責控制車輛實際位置與規劃位置之間的縱向位置誤差es。 通過調節比例項提高系統的響應速度,但比例項kp過大會增加穩態誤差引起超調;調節微分項kd減小超調量,提升控制的穩定性。 最終使得系統滿足精度和穩定性要求的系數分別為kp=3、ki=0、kd=0.1。

為了實現縱向控制器對縱向位置誤差和速度誤差的精確控制,還需要得到車輛準確的驅動制動參數。 因此,本研究依據某礦山井下運輸車的電機參數,建立電機仿真模型,通過試驗得到車輛在加速減速過程中的油門和剎車開度值。 電機最大功率Pmax為40 kW,最大扭矩Mmax為160 N·m,通過計算可得到電機恒扭矩與恒功率輸出的電機轉速的臨界值ωc為2 388 r/m。 因此,轉速和扭矩關系可以表示為

式中,ω為電機轉速,r/m;thr為油門踏板開度。

利用該模型,并根據車輛在行使過程中的速度和加速度可以實時輸出車輛的油門踏板開度和剎車踏板開度,從而得到4 個物理量的關系,進一步可以擬合得到三維曲面關系,如圖5 所示。

圖5 油門剎車開度三維曲面Fig.5 Three-dimensional surface of the throttle brake opening

3 仿真與分析

本研究使用5 次多項式曲線建立參考軌跡模型,以橫向控制器、前饋補償控制器和縱向速度控制器作為核心控制器,通過Carsim 與Matlab/simulink 聯合仿真,分別在井下雙車道、單車道與顛簸路面場景下進行軌跡跟蹤控制算法驗證。

3.1 車輛仿真參數設置

本研究以Carsim 中重型皮卡車型為模擬車型,并以某礦山的井下無軌膠輪車實車參數為標準,修改相關模擬參數,建立更加符合工程實際的車輛仿真模型,具體仿真參數取值見表1。

表1 井下無軌膠輪車動力學仿真參數Table 1 Basic parameters of underground transport vehicle dynamics

3.2 仿真工況模擬

本研究通過實地考察,根據某礦山井下巷道寬度設計標準和車輛運行速度標準,設計了井下雙車道避障換道工況、單車道會車工況與顛簸路面工況,測試所設計控制器的精確性和魯棒性。 通過分析在不同工況下車輛的橫向誤差和縱向誤差驗證軌跡跟蹤的效果和精度,以及分析速度誤差、轉向角變化評價車輛運行過程中的穩定性和舒適性。

工況1:井下雙車道超車或避障場景如圖6 所示。 其中黑色代表礦井巷道墻壁,巷道寬度為5. 3 m,設置車輛初速度為28.8 km/h,末速度為18 km/h,加速度為-2 m/s2;紅色表示車輛的規劃行駛路線,對應的真實場景為車輛先減速駛入變道車道再加速超過前車或避開前方障礙物,車輛行駛過程中車速小于30 km/h,符合井下運輸車輛速度標準。

圖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 段曲線共同構成車輛的軌跡。 對應的真實場景為車輛先減速駛入躲避硐室停車,完成會車后再加速駛出。

圖7 單車道會車工況Fig.7 Single-lane meeting condition

工況3:井下顛簸路面場景。 車輛在顛簸路面行駛過程中,輪胎在縱向和側向的載荷會發生改變,前輪垂向載荷增大,輪胎的側偏剛度增大,會對車輛軌跡跟蹤的控制效果產生影響。 本研究在建立車輛動力學模型時,考慮到車輛輪胎的側偏特性,通過改變輪胎的側偏剛度,模擬車輛在顛簸路面下的行駛狀態,進一步驗證該控制器在顛簸路面場景下的穩定性。顛簸路面下的車輛前輪側偏剛度為185 000 N/rad,后輪側偏剛度為205 000 N/rad。

3.3 仿真試驗與結果分析

由于井下行駛工況特殊,車輛運行速度小于30 km/h,因此仿真工況均為低速工況,3 種仿真工況下,車輛規劃軌跡與實際軌跡的誤差對比如圖8 所示。由圖8 可知:3 種工況下橫向控制誤差均小于5 cm,因此證明該控制器在井下具有較高的跟蹤精度。

圖8 3 種工況規劃軌跡與實際軌跡對比Fig.8 Comparison of planned trajectory and actual trajectory for three working conditions

雙車道工況下本研究對橫縱向控制器改進之后的仿真結果如圖9 所示。 由圖9 可知:速度PID 控制器對當前車速有很好的控制效果,但是會引起一定的超調,在7.8 s 處速度誤差最大為0.2 m/s。 圖9(c)為對位置PID 控制器調參前后控制效果對比。 分析可知:通過位置PID 對縱向控制改進之后,縱向位移誤差減小幅度明顯,最大減幅為10 cm。

圖9 雙車道工況改進前后仿真結果對比Fig.9 Comparison of simulation results before and after the improvement of the two-lane working condition

單車道會車工況下,改進前后橫向誤差與速度誤差對比分析結果如圖10 所示。 通過分析在減速和加速過程中的橫向誤差曲線,可知在減速路段誤差控制在8 cm 以內,加速路段誤差在10 cm 以內。 本研究使用改進后的橫向控制器后,減速路段橫向誤差減小到2 cm 以內,加速路段誤差減小到5 cm 以內。 由于井下單車道道路狹窄工況苛刻,因此LQR 橫向控制器誤差較大,前饋補償改進后的橫向最大誤差減少幅度在50%以上。 圖10(c)和圖10(d)為單車道減速路段和加速路段下實際速度與規劃速度的對比結果。通過對比分析可知:在車輛加速階段速度誤差較大,最大誤差出現在路段二3.2 s 處,此時速度誤差最大約為1 m/s。

圖10 單車道改進前后橫向誤差與速度誤差對比Fig.10 Comparison of lateral error and speed error before and after single-lane improvement

為驗證控制器在井下工況下的穩定性和魯棒性,本研究分析了車輛在軌跡跟蹤過程中前輪偏轉角和航向誤差參數,結果如圖11 所示。 由圖11 可知:在低速工況下車輛的前輪偏轉角變化平緩無明顯突變,航向誤差控制在±0.1 rad 以內,表明所提出的軌跡跟蹤控制方法在井下工況中也能保證車輛轉向的舒適性和穩定性。

圖11 前輪轉角與航向誤差變化Fig.11 Change of front wheel angle and course error

3.4 井下實際巷道場景試驗

為驗證本研究構建的控制器在井下實際場景下的控制效果,通過使用實驗室小車于陜西某礦山井下進行了現場試驗。 該小車底盤為阿克曼轉向結構,搭載傳感器為速騰16 線激光雷達,采樣頻率為10 Hz;HIPNUC 的9 軸IMU,采樣頻率為400 Hz,機載計算機為Intel(R) Core(TM) i7-11390H CPU,16GBRAM,512G 固態,編譯語言為C++,ROS 版本為Noetic,系統為Ubuntu 20. 04,小車使用激光SLAM 實現井下定位,定位誤差為厘米級。 車輛在3 種工況下多次運行的試驗結果見表2。 井下巷道2D 地圖如圖12 所示,井下現場試驗場景如圖13 所示。 通過對控制器多次軌跡跟蹤的誤差數據進行對比分析得出,3 種工況下橫向最大誤差為6.6 cm,縱向最大誤差為56 cm,速度最大誤差為0.5 m/s。 試驗結果表明:井下顛簸路面的確會對軌跡跟蹤誤差產生影響,但其誤差也在合理范圍內,因此,改進后的控制器在井下場景具有較好的應用效果。

表2 3 種工況下最大跟蹤誤差結果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

4 結 論

(1)提出了基于PID 積分項前饋補償的橫向控制器,優化了車輛輸出的前輪偏轉角,減小了橫向控制誤差,解決了因前輪偏轉角突變引起的方向盤抖動問題,改進后的橫向LQR 控制器轉向穩定,更能適應井下巷道工況。

(2)構建了井下實車參數電機模型,通過試驗標定得到油門、剎車與速度、加速度的變化關系,并利用PID 控制器調整車輛的位置誤差和速度誤差,同時解決了車輛運行過程中的速度和路徑的時變問題。

(3)通過Carsim 和Simulink 分別對井下雙車道、單車道和顛簸路面工況進行仿真分析,并在井下實際環境中進行了試驗驗證。 結果表明:該控制器能夠將跟蹤誤差控制在較小范圍內,具有較高的控制精度和穩定性。 但試驗中并未考慮車體質量變化對跟蹤控制的影響,因此,建立更為精確的模型實現井下全局縱橫向耦合控制是下一步研究方向。