輪式挖掘機自動循跡行駛控制系統研究

馬厚雪，王 龍

（江蘇徐工工程機械研究院有限公司，江蘇 徐州 221004）

輪式工程機械機動性高，與履帶式挖掘機相比，更加適合于快速移動。在某些環境異常惡劣（垃圾清理、地震災害搶險救災、防爆作業、放射性場合）作業人員操作困難，因此如何實現工程機械的無人化控制，實現輪式車輛自動循跡行駛已經成為現代機械研究的一個重點方向。

1 硬件系統設計

本文以輪式挖掘機為載體，采用多種傳感器融合的方案，實現多重特定的功能。這些設備包括：安裝在挖掘機的駕駛室頂部的激光雷達，安裝在挖掘機前后左右的米波雷達，安裝在后配重的GPS 定位定向天線、慣性導航接收機等。系統還配置有工控機和可編程控制器（圖1）。

圖1 硬件系統

GPS 定位定向天線與慣性導航接收機組成定位系統，用于確定挖掘機的精確位置坐標。激光和毫米波雷達用于避障。工控機用于處理傳感器信號、規劃路徑，發送控制指令到可編程控制器。可編程控制器用于接受上層工控機發出的油門、制動、轉向等控制信號，并且向底層轉向控制器以及驅動系統控制器發送控制指令以實現上層的控制指令。其中慣導、雷達、工控機等均連接到交換機上，通過以太網進行通信，可編程控制器與工控機、底層控制器等通過CAN 線通信。

2 軟件系統設計

自動循跡軟件系統包括定位數據采集模塊、曲線擬合模塊和軌跡跟蹤控制模塊組成（圖2）。這3 個模塊均部署在工控機上。

圖2 自動循跡軟件系統

1）定位數據采集模塊 主要完成挖掘機的軌跡路徑點的坐標信息以及挖掘機自身位置坐標信息的采集，并對這些信息進行坐標轉換，輸出轉換后的路徑數據以及定位數據。

2）曲線擬合模塊 主要接收定位數據采集模塊中轉換后的路徑數據，對這些離散的路徑數據進行曲線擬合，輸出挖掘機的行駛軌跡曲線，即目標軌跡。

3）軌跡跟蹤控制模塊 又包括橫向控制模塊和縱向控制模塊，其主要接收定位數據采集模塊輸出的實時定位數據，曲線擬合模塊輸出的行駛軌跡曲線，以及可編程控制器輸出的挖掘機位姿數據；通過橫縱向控制模塊的計算；輸出油門踏板深度、制動踏板深度以及轉向角等控制信息給可編程控制器。

2.1 定位數據采集模塊

由于通過GPS 天線和慣導讀到的GPS 數據是地理坐標系（WGS84），需要進行解析與坐標變換，轉換成X軸指向正東，Y軸指向正北，原點位于后軸中心的直角坐標系數據。使用高斯正算公式進行坐標變換與解析，將大地坐標經度(L)和緯度(B)坐標通過高斯投影得到高斯平面直角坐標(x,y)，然后再將平面直角坐標，轉換成當地的水平坐標，其轉換公式如式(1)所示

可將式(1)展開為l的冪級數，如式(2)所示

其中，X，Y是坐標值，其中Y軸向東為正，X軸向北為正，l是無限小的數，m，n是待定系數，是等量維度q的函數。

經過上述過程得到的坐標系轉化為當地坐標系，其中：正東向為y軸正方向，正北向為x軸正方向，與無人駕駛通用的坐標系定義不一致，使用旋轉矩陣變換，如式(3)所示

可得到無人駕駛通用坐標系，其中，X2、Y2是坐標值，其中X2軸向東為正，Y2軸向北為正。

2.2 曲線擬合模塊

由于GPS 數據點是離散的，可以通過最小二乘法進行分段連續擬合，得到挖機的平滑運行軌跡。最小二乘法通常作為一元線性回歸模型,用殘差平方和達到最小，作為擬合方法構建損失函數。從GPS 采集的全體數據點中，分段提取n組（X1，Y1），（X2，Y2），…，（Xn，Yn）。對這n組數據，采用最小化誤差的平方和尋找數據的最佳函數匹配。擬合后獲得挖掘機循跡行駛連續軌跡。作為循跡行駛的目標值發送的軌跡控制模塊。

2.3 軌跡跟蹤控制模塊

橫向控制模塊，主要基于當前車輛坐標、航向角和行駛軌跡曲線，采用預瞄PID 控制算法，計算轉向角。縱向控制模塊，主要采用PID 算法，將期望行駛速度與實際車速進行對比，將速度誤差作為PID 的輸入，計算油門踏板深度和制動踏板深度。

2.3.1 橫向控制模塊

圖3 為橫向控制模塊模型。橫向控制采用的是預瞄PID 控制算法，首先通過定位數據采集模塊獲得當前車輛坐標和航向角，然后結合曲線擬合模塊中獲取的軌跡曲線，進行最近路徑點的搜索，求出離當前位置最近的路徑點，根據得到的路徑點和下兩個路徑點，可以求出車輛橫向運動的曲率，并通過式(4)求得轉向前饋角度。

式中：a為挖掘機質心到前軸的長度；b為挖掘機質心到后軸的長度。

之后根據圖3 中的橫向控制模型，確定預瞄距離L后得到預瞄點，根據式(5)可以獲得預瞄點的橫向偏差e1a。

式中：e為車輛質心到軌跡曲線的橫向偏差，為航向角偏差。

圖3 橫向控制模塊模型

以預瞄點的橫向偏差e1a作為PID 輸入，PID的輸出為轉向反饋角度q2。則通過式（6）可以求出最終的轉向角q

式中，k1為前饋角度權重，k2為反饋角度權重。

2.3.2 縱向控制模塊

縱向控制模塊（圖4），主要通過速度閉環，控制挖掘機按照給定的速度行駛。其控制加速控制器實現加速和降速；控制制動控制器實現減速制動；控制切換器實現加速和制動的切換。根據挖掘機行駛速度傳感器或者慣性導航系統反饋的實際車速，進行行駛度閉環。閉環控制算法采用PID 算法。縱向控制模塊將期望行駛速度與實際車速進行對比，將速度誤差作為PID 的輸入，輸出為油門/制動開度的控制信息。

圖4 縱向控制模塊

3 系統測試

在某型號輪式挖掘機上進行測試。安裝響應的硬件設備。設計循跡行走軌跡分別按直線和U 型曲線行駛。輪式挖掘機由起點處，分別按加載的路徑軌跡進行自動循跡行駛，循跡過程中過彎時自動減速。對循跡行駛過程的橫向誤差進行統計，誤差與照軌跡點一一對應，正向循跡行走速度為8km/h。經測試直線循跡行駛橫向誤差<0.2m，U 型曲線循跡行駛橫向誤差<0.7m，具體效果如圖5～圖8 所示。

圖5 直線循跡行駛軌跡

圖6 直線循跡行駛橫向位置誤差

圖7 U型循跡行駛軌跡

圖8 U型循跡橫向位置誤差

4 結語

本文構建的輪式挖掘機自動循跡行駛控制系統，可以實現對給定的路徑軌跡的自動循跡行駛。在自動循跡行駛過程中，直線循跡效果較好，曲線循跡在拐彎處誤差較大。考慮車輛非線性、不確定性和時變特性地對車輛的影響，在以后的研究中，采用智能控制和其他控制相結合的方法，對橫向控制器預瞄算法進行優化，進而減小橫向控制偏差。