馬厚雪，王 龍

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

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

1 硬件系統(tǒng)設(shè)計

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

圖1 硬件系統(tǒng)

GPS 定位定向天線與慣性導(dǎo)航接收機組成定位系統(tǒng)，用于確定挖掘機的精確位置坐標(biāo)。激光和毫米波雷達(dá)用于避障。工控機用于處理傳感器信號、規(guī)劃路徑，發(fā)送控制指令到可編程控制器?？删幊炭刂破饔糜诮邮苌蠈庸た貦C發(fā)出的油門、制動、轉(zhuǎn)向等控制信號，并且向底層轉(zhuǎn)向控制器以及驅(qū)動系統(tǒng)控制器發(fā)送控制指令以實現(xiàn)上層的控制指令。其中慣導(dǎo)、雷達(dá)、工控機等均連接到交換機上，通過以太網(wǎng)進(jìn)行通信，可編程控制器與工控機、底層控制器等通過CAN 線通信。

2 軟件系統(tǒng)設(shè)計

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

圖2 自動循跡軟件系統(tǒng)

1）定位數(shù)據(jù)采集模塊 主要完成挖掘機的軌跡路徑點的坐標(biāo)信息以及挖掘機自身位置坐標(biāo)信息的采集，并對這些信息進(jìn)行坐標(biāo)轉(zhuǎn)換，輸出轉(zhuǎn)換后的路徑數(shù)據(jù)以及定位數(shù)據(jù)。

2）曲線擬合模塊 主要接收定位數(shù)據(jù)采集模塊中轉(zhuǎn)換后的路徑數(shù)據(jù)，對這些離散的路徑數(shù)據(jù)進(jìn)行曲線擬合，輸出挖掘機的行駛軌跡曲線，即目標(biāo)軌跡。

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

2.1 定位數(shù)據(jù)采集模塊

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

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

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

經(jīng)過上述過程得到的坐標(biāo)系轉(zhuǎn)化為當(dāng)?shù)刈鴺?biāo)系，其中：正東向為y軸正方向，正北向為x軸正方向，與無人駕駛通用的坐標(biāo)系定義不一致，使用旋轉(zhuǎn)矩陣變換，如式(3)所示

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

2.2 曲線擬合模塊

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

2.3 軌跡跟蹤控制模塊

橫向控制模塊，主要基于當(dāng)前車輛坐標(biāo)、航向角和行駛軌跡曲線，采用預(yù)瞄PID 控制算法，計算轉(zhuǎn)向角?？v向控制模塊，主要采用PID 算法，將期望行駛速度與實際車速進(jìn)行對比，將速度誤差作為PID 的輸入，計算油門踏板深度和制動踏板深度。

2.3.1 橫向控制模塊

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

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

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

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

圖3 橫向控制模塊模型

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

式中，k1為前饋角度權(quán)重，k2為反饋角度權(quán)重。

2.3.2 縱向控制模塊

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

圖4 縱向控制模塊

3 系統(tǒng)測試

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

圖5 直線循跡行駛軌跡

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

圖7 U型循跡行駛軌跡

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

4 結(jié)語

本文構(gòu)建的輪式挖掘機自動循跡行駛控制系統(tǒng)，可以實現(xiàn)對給定的路徑軌跡的自動循跡行駛。在自動循跡行駛過程中，直線循跡效果較好，曲線循跡在拐彎處誤差較大?？紤]車輛非線性、不確定性和時變特性地對車輛的影響，在以后的研究中，采用智能控制和其他控制相結(jié)合的方法，對橫向控制器預(yù)瞄算法進(jìn)行優(yōu)化，進(jìn)而減小橫向控制偏差。