煤礦巷道七自由度噴漿機器人軌跡規劃與跟蹤控制

程歡， 鄧立營

（沈陽工業大學 化工裝備學院，遼寧 沈陽 110000）

0 引言

煤礦巷道的常見支護方式為噴錨支護，噴漿工作是噴錨支護的重要施工環節。在巷道施工中噴漿工作通常采用人工或人與機械相結合的方式。巷道施工環境惡劣，工作強度大，施工效率低，回彈率高，質量可控性差，因此應用自動化噴漿機器人是實現煤礦巷道高效、安全噴漿施工的重要途徑[1]。

國內外對機器人工作臂軌跡規劃與運動控制開展了大量研究。在軌跡規劃方面，通常采用插值法、時間最優等軌跡規劃算法實現工作臂的直線運動或圓弧運動。在運動控制方面，一般根據控制需求設計工作臂的控制系統，將算法與系統相結合，得到滿足需求的工作臂最優運動控制。鄧稼敏等[2]對六自由度錨護機器人進行分段式軌跡規劃，求得在最短時間運動到達終點的工作臂軌跡。劉送永等[3]采用環境網絡法對噴漿機器人工作臂軌跡進行規劃，得到較優的運動軌跡和運動參數。肖振楠等[4]采用4-3-4 多項式關節插值算法，對六自由度機械手進行軌跡規劃。Liu Gangfeng 等[5]提出了八自由度噴漿機器人工作臂軌跡規劃方法，并在實際巷道中進行實驗。Chen Gang 等[6]應用時間最優軌跡跟蹤控制算法對冗余工作臂末端執行器進行研究，獲得時間最優的軌跡。Liu Yibo 等[7]針對滿足Pieper 準則的六自由度工作臂提出了一種工作子空間劃分方法，有效避免了由于大量的逆解選擇問題導致的工作臂關節突變和軌跡錯位，使規劃的軌跡平滑。劉俊輝等[8]為解決液壓工作臂關節存在的響應遲滯問題，提出了一種基于三次B 樣條函數的軌跡糾偏算法。謝斌等[9]對八自由度噴漿機器人工作臂進行軌跡規劃及控制系統設計，實現工作臂自主工作。許萬等[10]針對移動機器人軌跡跟蹤問題，設計了帶有干擾觀測器的混合控制器，并用Lyapunov 函數直接法證明了系統的收斂性和穩定性。Yang Libo 等[11]采用模型預測控制（Model Predictlve Control，MPC）算法對輪式移動機器人進行跟蹤控制，實現了移動機器人多輸入多輸出系統的精確控制。H. Cheon 等[12]提出了未知復雜動態環境中移動機器人的高效實時局部運動規劃算法，實現在狀態-時間-空間中逐步進行軌跡規劃。

煤礦巷道環境復雜，噴漿機器人工作過程中存在動作不連續、位置誤差大、穩定性差等問題。因此，本文提出了煤礦巷道七自由度噴漿機器人軌跡規劃與跟蹤控制方法。根據煤礦巷道噴漿施工要求，對巷道進行截面劃分；設計噴漿機器人在巷道截面間的移動軌跡和各截面上工作臂運動軌跡，提升噴漿機器人動作的連續性；采用多項式插值法生成噴漿機器人移動軌跡和工作臂運動軌跡，通過MPC 算法對噴漿機器人移動軌跡進行跟蹤控制，提高噴漿機器人移動的準確性和平穩性。

1 噴漿機器人軌跡規劃與跟蹤控制

1.1 噴漿機器人結構

噴漿機器人由移動底盤、工作臂和噴槍組成，如圖1 所示，其中移動底盤采用輪式結構，包括平臺和基座，工作臂包括大臂、小臂和腕部。

圖1 噴漿機器人組成Fig. 1 Composition of shotcrete robot

噴漿機器人工作臂由7 個關節組成，關節間采用串聯的結構形式，如圖2 所示。工作臂采用液壓驅動，可以實現平臺旋轉、基座旋轉、大臂伸縮、小臂伸縮、腕部旋轉、腕部伸縮、末端噴槍自轉。

圖2 工作臂關節設置Fig. 2 Working arm joint setup

工作臂自由度為

式中：m為關節數；Pl為低副數量；Ph為高副數量。

本文中Pl=7,Ph=0，根據式（1），該噴漿機器人的工作臂具有7 個自由度。

1.2 噴漿機器人軌跡規劃

參考GB 50086—2015《巖土錨桿與噴射混凝土支護工程技術規范》，噴射作業應分段分片進行[13]。根據噴漿機器人靜止時工作臂的運動范圍和噴槍沿巷道走向的噴漿長度，將煤礦巷道沿走向劃分為n個截面。噴漿機器人在巷道中的軌跡如圖3 所示。噴漿工作開始時，噴漿機器人由起點沿巷道走向移動到巷道待噴截面n-1 后，工作臂沿軌跡A→D運動，噴槍實施噴漿工作；工作臂移動到D后保持靜止狀態，由移動底盤控制機器人移動到截面n，該過程中機器人移動軌跡為D→E，同時噴槍處于噴漿狀態；機器人運動到截面n后，工作臂沿軌跡E→H運動，噴槍實施噴漿工作。

圖3 噴漿機器人在巷道中的軌跡Fig. 3 Trajectory of shotcrete robot in roadway

將噴漿機器人完成巷道內任一截面噴漿工作的過程定義為1 個工作周期（如機器人沿D→E→H移動完成截面n噴漿為1 個工作周期）。在1 個工作周期內噴漿機器人工作流程如圖4 所示。先對噴漿機器人移動軌跡進行規劃，機器人沿軌跡移動時，對機器人移動軌跡進行跟蹤控制；機器人到達噴漿位置后，對工作臂軌跡進行規劃，由工作臂驅動噴槍完成噴漿工作。機器人的移動軌跡規劃和工作臂軌跡規劃在每一個工作周期內交替進行，以保證噴漿機器人噴漿動作的連續性。

圖4 噴漿機器人工作流程Fig. 4 Work flow of shotcrete robot

1.3 噴槍機器人運動學模型

建立噴漿機器人運動學模型，如圖5 所示。{w}為巷道坐標系，其中x軸為機器人前進方向，y軸垂直于x軸指向巷道壁面；{q}為機器人移動坐標系，其中xq，yq軸與x，y軸方向相同。當機器人處于轉向狀態時，令左側輪的坐標系為{l}，其中xl軸與xq軸方向相同，yl軸垂直于xl軸指向左側巷道壁面；令右側輪的坐標系為{r}，其中xr軸與xq軸方向相同，yr軸垂直于xr軸指向右側巷道壁面。 θ為機器人方向角；φ為xq軸與y軸間的夾角；2L為底盤寬度。

圖5 噴漿機器人運動學模型Fig. 5 Kinematics model of shotcrete robot

機器人左右側輪在機器人移動坐標系{q}下的位置分別為

在機器人移動坐標系{q}中，機器人的運動方程可表示為

式中：v(t)為機器人在移動坐標系{q}下的速度，t為時間；ω(t)為角速度；vl(t)，vr(t)分別為機器人左右側輪在移動坐標系{q}下的速度。

移動底盤控制機器人移動時，在車輪軸線的投影方向上，車輪速度分量為0，即

機器人在式（2）和式（3）約束下的無側向移動非完整數學模型為[8]

式中：z(t)為機器人移動狀態，z(t)=[x(t)y(t) θ(t)]T；u(t)為機器人控制量，u(t)=[v(t) ω(t)]T。

為描述機器人控制效果，定義機器人移動過程中的位置誤差為

式中：Xc(t)為機器人理想位置；X(t)為實際輸出位置。

1.4 基于MPC 算法的軌跡跟蹤控制

噴漿機器人移動軌跡跟蹤控制步驟：先采用三次多項式插值進行軌跡規劃，再通過MPC 算法對三次多項式插值生成的參考軌跡進行跟蹤控制。通過設計合適的控制量u（t），使得機器人在預測時域內[14-16]實現參考軌跡的跟蹤。給定參考軌跡為

式中 β0— β3為多項式系數。

機器人的控制輸出Yc(t)和約束輸出Yb(t)分別為

式中：gc（·）為控制輸入；gb（·）為約束輸入。

機器人控制量和輸出的約束為

式中：umin(t)，umax(t)分別為控制量的最小值和最大值；Ybmin(t)，Ybmax(t)分別為約束輸出的最小值和最大值。

為得出機器人軌跡預測跟蹤的最佳控制輸入，即預測輸出與期望輸出越接近越好，定義預測輸出與期望輸出之間的累計誤差為目標函數J，引入懲罰后的目標函數為

式中：Np，Nc分別為預測時域和控制時域，且滿足Nc≤Np；i=0，1，···，Nc-1 ；γ(·)為期望控制輸出；Q，R為加權矩陣，可以是時變的；uˉ(·)為預測的控制量；uk(·)為輸入的參考控制量；uˉi為當前輸出控制量。

尋找最佳輸入可描述為如下的優化問題：

求解式（16）可歸結為求解非線性規劃問題，通過序列二次規劃（Sequential Quandratic Programming，SQP）求解器進行求解[17]。

2 噴漿機器人工作臂軌跡規劃

2.1 噴漿機器人工作臂運動學模型

噴漿機器人采用七自由度工作臂對煤礦巷道進行噴漿作業。以工作臂相鄰關節軸線的公垂線和關節軸線交點為原點，關節軸方向為z'軸，公垂線的指向為x'軸，y'軸由右手法則確定，建立工作臂各關節坐標系。利用標準的D-H 參數法[18-19]，由各關節坐標系間的變換獲得工作臂的連桿參數，見表1。 ?j為連桿長度， αj為連桿扭轉角，dj為連桿偏距， ?j為關節角。對于轉動關節j， ?j為關節變量，其他3 個連桿參數固定不變；對于移動關節，dj為關節變量（可用來表示關節位移），其他3 個連桿參數固定不變。

表1 工作臂D-H 參數Table 1 D-H parameters of working arm

其中：

通過工作臂關節的變換矩陣，由已知關節的角度、位移能夠得到工作臂相對于煤礦巷道的位姿[20-21]。

2.2 3-5-3 分段多項式插值軌跡規劃

對于任意巷道截面工作臂運動軌跡，給定軌跡上的4 個軌跡點并將運動軌跡分為3 段，如圖6 所示（以截面n-1 為例，A，B，C，D為4 個軌跡點）。對軌跡A→B和C→D采用三次多項式插值，對軌跡B→C采用五次多項式插值，通過3-5-3 分段多項式插值軌跡規劃使工作臂在噴漿過程中具有連續的加速度。

圖6 噴槍參考軌跡Fig. 6 Airbrush reference trajectory

A→B段關節j的運動軌跡Sj1與噴漿時間t1的關系：

B→C段關節j的運動軌跡Sj2與噴漿時間t2的關系：

C→D段關節j的運動軌跡Sj3與噴漿時間t3的關系：

式中fj10，fj11，···，fj14，fj20，fj21，···，fj25，fj30，fj31，···，fj34為多項式系數。

3 仿真驗證

根據噴漿機器人移動及工作臂運動的要求，利用Matlab/Simulink 搭建噴漿機器人軌跡規劃與跟蹤控制仿真系統。當機器人處于初始狀態時，輸入機器人初始位置，系統預測下一段時域的位置輸出，然后控制機器人到達輸出位置。機器人完成移動后，輸入工作臂液壓系統的驅動信號，驅動工作臂沿巷道截面軌跡完成噴漿工作。為模擬煤礦巷道復雜環境，在系統中引入2 個隨機干擾，選用Simulink 中的階躍信號隨機作用于機器人移動過程。

機器人位置跟蹤曲線如圖7 所示?？煽闯鰴C器人在巷道內的實際跟蹤軌跡與參考軌跡重合，機器人移動過程中的最大位置誤差為0.07 m，誤差較小，滿足巷道內機器人移動精度要求。機器人方向角跟蹤曲線如8 所示?？煽闯鰴C器人移動過程中的實際方向角和參考方向角曲線大致重合，二者差值較小，最大差值僅為0.99 rad，表明機器人移動時具有較好的穩定性。

圖7 機器人位置跟蹤曲線Fig. 7 Robot position tracking curve

圖8 機器人方向角跟蹤曲線Fig. 8 Robot direction angle tracking curve

機器人移動速度控制曲線如圖9 所示?？煽闯鰴C器人移動速度總體穩定在10 m/s，速度發生波動后能在較短時間內回到穩定運動狀態，表明MPC 算法滿足巷道內機器人平穩移動控制要求。工作臂運動軌跡如圖10 所示?？煽闯鲕壽E點間由圓滑的曲線連接，表明噴漿軌跡穩定、平滑，能夠滿足巷道噴漿施工要求。

圖9 機器人移動速度控制曲線Fig. 9 Robot moving speed control curve

圖10 工作臂運動軌跡Fig. 10 Working arm motion trajectory

將軌跡點（噴槍位置）數據代入式（18）對矩陣求逆解，得到各關節的關節位移和角度變化曲線，如圖11所示?？煽闯龉ぷ鞅鄣淖畲箨P節位移為-4.83 m，最大關節角為2.83 rad，未超過關節運動范圍；各關節位移和角度曲線平滑，表明在噴漿過程中各關節運動連續、無突變。

圖11 工作臂關節變量變化曲線Fig. 11 The change curves of working arm joint variable

關節3 的速度和加速度曲線分別如圖12 和圖13所示?？煽闯鲫P節3 的速度和加速度曲線均為連續、平滑的曲線，表明工作臂關節在噴漿過程中處于穩定運動狀態。

圖12 關節3 速度曲線Fig. 12 Velocity curve of joint 3

圖13 關節3 加速度曲線Fig. 13 Acceleration curve of joint 3

4 結論

1） 對煤礦巷道進行截面劃分，設計七自由度噴漿機器人在各截面間的移動軌跡和各截面上的工作臂運動軌跡，機器人移動軌跡規劃和工作臂運動軌跡規劃在每一個工作周期內交替進行，以保證噴漿機器人噴漿動作的連續性。

2） 根據機器人運動學模型，采用三次多項式插值法生成機器人移動軌跡，并利用MPC 算法對機器人移動軌跡進行跟蹤控制，以實現機器人在巷道內準確、穩定移動。

3） 利用標準的D-H 參數法建立了機器人工作臂運動學模型，采用3-5-3 分段多項式插值法對機器人在各巷道截面的運動軌跡進行規劃，得到了工作臂各關節的運動軌跡。

4） 仿真結果表明，MPC 算法可對機器人位置、方向角進行精確跟蹤，且機器人移動速度穩定，實現了機器人在巷道內移動的準確性和穩定性；工作臂在噴漿過程中動作連續、噴漿軌跡穩定，滿足巷道噴漿施工需求。