履帶行走車輛行走軌跡糾偏方法研究

高啟業，高 明，宋麗軍

（1.深圳市德塔工業智能電動汽車有限公司，廣東 深圳 440310；2.內蒙古鄂爾多斯市東勝區能源管理綜合行政執法大隊，內蒙古 鄂爾多斯 017000）

掘進機、挖裝機等履帶行走車輛在實現自主導航挖掘的過程中，始終存在一些技術難題，阻礙智能化煤礦中掘進機自主導航的發展。履帶行走車輛沿行駛路徑的糾偏是影響其實現自主導航的主要影響因素［1-5］，國內諸多學者對此展開研究，李睿等［6］提出了一種基于軌跡預測補償的雙重無跡卡爾曼濾波方法，利用該方法對履帶車輛滑動參數進行參數估算及補償，并取得良好的效果；張敏駿等［7］針對掘進機巷道工作過程中出現履帶滑移問題，結合掘進機履帶系統建立糾偏控制動力學模型，并通過實驗驗證其有效性；王本林等［8］提出履帶行走機構的參數優化設計方法，并確定履帶長寬比和轉向比的最優參數；卞永明等［9］提出一種基于狀態反饋控制律的履帶軌跡跟蹤控制器，該控制器可有效地實現直線跟蹤；李秋生等［10］設計一種多節履帶救援機器人，可實現糾偏、躍障等功能；付云飛［11］設計重型履帶車輛控制系統，利用實驗室虛擬儀器集成環境（laboratory virtual instrument engineering workbench，LabVIEW）開發履帶車輛控制軟件實現車輛精準控制；劉義樂等［12］為解決履帶車輛高精度定位問題，構建一種高精度履帶車輛連續運動軌跡定位方法，該方法的定位精度小于0.2 m；李睿等［13］為提高履帶車輛軌跡控制的魯棒性，提出基于拉格朗日法的履帶車輛軌跡前饋補償。

為進一步提升控制精度，本文提出一種基于虛擬前輪反饋控制算法，以提升履帶行走車輛糾偏能力及精度。

1 履帶行走裝置結構

履帶行走系統結構如圖1 所示。由圖1 可知，驅動輪為動力輸出端，將動力輸出至履帶，與地面摩擦推動機構前進或后退，負重輪主要承載行走機構及支撐體的質量，并在一定程度上對地面的履帶導向，負重輪間距離越小其接地履帶的偏移越小，誘導輪主要負責將接地部分的履帶誘導向上，將履帶導向驅動輪，由于誘導輪與驅動輪之間存在一定距離，履帶存在自重會向重力方向垂下，會改變履帶鏈節間的張緊度，拖帶輪主要支撐上半段履帶重量，可有效減小對履帶張緊度的影響，并防止履帶脫落。

圖1 履帶行走系統結構Fig.1 Structure of crawler walking system

2 履帶行走車控制模型構建

履帶車運動模型如圖2所示。XOY絕對坐標系中，xCCyC為履帶車車身相對坐標系，θ為航向角，ω為履帶車轉向角速度，v為履帶車行駛速度，vL為左履帶速度，vR為右履帶速度，履帶長度c為1 020 mm，帶寬a為100 mm，履帶車軌距b為600 mm。

圖2 履帶車運動學模型Fig.2 Kinematic model of tracked vehicle

履帶車的轉向原理類似于兩輪差速車模型，依靠左右履帶的速度差和正反轉來實現轉向功能，按照左右履帶的轉動速度和轉動方向，履帶車的轉向類型主要可分為4種，如圖3所示。

圖3 轉向類型Fig.3 Schematic diagram of steering principle

針對以上4 種運動狀態，假設時間t足夠短，當履帶車順逆時針轉向行駛時，即當|vL|≠|vR|時，則此時狀態下的運動方程為

當運動狀態為直線先前或者向后行駛時，即當速度關系為vL＝vR時，車輛無航向變化，航向角變化為0，航向為初始狀態下的航向角θ0，則此狀態下運動方程為

當運動狀態為以瞬心為中心順逆時針原地旋轉時，即當vL=-vR時，車輛瞬心在平面內無移動，車輛航向角存在變化量，則此狀態下的運動方程為

可以看出，履帶車行走裝置的控制過程是一個復雜的非完整約束、多輸入輸出、強非線性的耦合的過程。但掘進機的工作環境相對較為惡劣，過程中存在較多不確定因素，例如履帶的滑移、側滑等，均會提高軌跡的控制難度。

3 履帶行走車前輪反饋控制器

本文采用的虛擬前輪反饋控制算法的軌跡跟蹤控制器，其控制策略為：跟蹤目標軌跡時，以前輪轉角為控制量，控制前輪轉角逼近目標，具有跟蹤點采樣時間足夠短時，前輪轉角變化較小，相鄰軌跡點的曲率變化較小的特點。因此可采用圓弧擬合參考軌跡跟蹤跟蹤點，大幅降低計算負荷，其擬合參考跟蹤軌跡原理如圖4所示。

擬合后即可求解左右履帶對地面的速度值，速度求解原理如圖5所示。

圖5 左右履帶速度求解原理Fig.5 Schematic diagram of left and right track speed solution

定義弧長P1P2=l，弦長P1P2=m，θ1、θ2值分別為車輛兩個狀態下的航向角，λ為轉角，車輛從狀態P1到P2為1個采樣周期，1個采樣周期時間為t?？傻寐膸к囖D向瞬心的角速度和線速度分別為

在履帶車的運動學模型當中，瞬心點的角速度和線速度還可表示為

根據幾何關系，并將式（6）帶入式（5），可得擬合參考跟蹤軌跡后的左右履帶對地線速度的數學模型矩陣V為

式中：i為軌跡點索引序號值，i=1，2，…；m值為兩點之間的距離，根據采樣出來的離散參考軌跡點的坐標值計算。

前輪反饋控制原理圖如圖6所示，θα為橫向誤差產生的轉角，θβ為航向誤差產生的轉角，t為時間。

圖6 前輪反饋控制原理Fig.6 Front wheel feedback control schematic diagram

只考慮履帶車橫向誤差時，前輪所需轉過角度為?1；只考慮履帶車航向誤差時，前輪所轉過角度為?2，其表達式為

式中：k為控制器增益。

在實際追蹤過程中，橫向誤差與航向誤差同時存在，需將2 種誤差均考慮在內，此時前輪所需要轉過的角度?為

計算橫向誤差的微分方程為

式中：ey為橫向誤差值，單位mm。

當ey趨向于零時，則可以寫為

將式（11）積分，即可得到相應ey為

式中：kp為速度控制器參數。

4 履帶行走車軌跡糾偏仿真

履帶行走車實物圖如圖7所示，履帶系統采用前置驅動，1個驅動輪、5個負重輪、1個托帶輪和1個誘導輪，驅動裝置采用伺服電機驅動，上位機內編制控制算法，并通過無線傳輸形式給可編程控制器（programmable logic controller，PLC）發送指令，操控伺服電機，實現履帶行走車的行駛與糾偏功能。

圖7 履帶行走車實物Fig.7 Physical picture of crawler truck

在實際仿真中，設定追蹤的目標軌跡為直徑20 m 的圓形軌跡，由于圓形軌跡是封閉圖形，不利于直觀觀察起點和終點位置，因此將目標軌跡的起點與終點斷開。設定目標模型履帶車的速度為2 m/s，模型履帶車的初始位姿狀態為

設置控制器內部的參數為：控制器增益參數值k=5，采樣周期時間t=0.025 s，速度控制器參數取值kp=0.5。同樣工況及條件下，與純跟蹤算法進行仿真比較，如圖8所示。

圖8 2種算法行駛軌跡對比Fig.8 Comparison of the driving trajectories of the two algorithms

預設的行駛軌跡圓形速度2 m/s的低速工況下，本文虛擬前輪反饋控制算法下跟蹤軌跡式能在較短的路徑內運動到目標軌跡，且能平穩地跟蹤軌跡。純跟蹤法到達目標軌跡點后，出現較大的超調量，振蕩問題較突出，出現抖振現象，且擺動較大。

本文算法的仿真橫向誤差、縱向誤差及直線誤差結果與純跟蹤法的誤差對比如圖9所示。

圖9 橫向誤差、縱向誤差及直線誤差曲線Fig.9 Transverse error， longitudinal error and straight-line error curve

圖9 中，xe、ye分別為仿真模型履帶車轉向瞬心在X、Y方向上的位置誤差，直線誤差值e=。由圖9 可知，在低速工況下，本文算法下模型履帶車的橫向誤差xe與縱向誤差ye最大值不超過0.15 m，直線誤差值e最大值不超過0.12 m，純跟蹤法下的模型履帶車橫向誤差xe最大值為0.31 m，縱向誤差ye最大值為0.53 m，直線誤差e最大值為0.64 m。在低速工況下，本文算法下的軌跡跟蹤精度較純跟蹤法相比具有更高的精度值。

5 結語

本文針對模型履帶車的軌跡跟蹤糾偏控制問題，提出一種基于虛擬前輪反饋控制算法的履帶車軌跡跟蹤方案。針對該算法，設計軌跡跟蹤控制器，提供虛擬參考跟蹤軌跡，并提出圓弧曲線擬合方法求解控制量，將多輸入輸出系統轉化成單輸入輸出系統，降低系統的計算負荷，并在一定程度上提高軌跡跟蹤精度。此外，分別進行低速工況下模擬巷道環境下的軌跡跟蹤仿真實驗，并與純跟蹤法對比，本文算法下的軌跡跟蹤精度較純跟蹤法相比，具有更高的精度值。