基于自抗擾控制算法的單舵輪AGV控制系統設計

(濟南二機床集團有限公司，濟南 250022)

引 言

隨著科學技術的快速提升，工業4.0和智能工廠的概念在工業生產中占有了越來越重要的地位，越來越多的機器人在工業生產中代替人工完成了重復、單調以及危險的工作，節省人力成本的同時，提升了工作效率。壓力機行業中，由線首的拆垛機器人、板料傳送系統、機器人上料系統、壓力機間單元、線末的壓力機下料單元以及自動運料AGV小車組成的機器人沖壓自動化系統也得到了越來越多企業的認可[1]。AGV(Automated Guided Vehicle)，即自動引導小車，是該系統中的一個重要組成部分，運行中背載或牽引著物料沿預設的導引路線行駛。

在復雜的車間環境中，要求AGV能夠：①準確、快速到達指定位置；②遇到障礙物時能夠自動停車；③能到達并自動識別不同的站點，然后做出相應的動作。

隨著電子技術的不斷發展，出現了磁條(輔以RFID站點)、慣性導航或激光導航等自動導引裝置[2]，激光導航憑借其不需要地面輔助定位、可隨時改變行駛路徑的優點，得到了廣泛的應用[3]。

本文基于激光SLAM(Simultaneous Localization And Mapping，SLAM)導航原理和自抗擾控制算法，設計了一種AGV自動控制系統，實現了AGV自動路徑跟蹤控制。該系統只需要通過上層調度軟件更新位置坐標信息即可改變AGV行駛路徑或停靠站點。所設計的AGV控制系統結構框圖如圖1所示。

圖1 控制系統結構框圖

1 單舵輪AGV運動學數學模型

本系統選用的AGV包含一個能控制前進方向的舵輪和兩個固定的從動輪[4-5]，依靠前輪電機旋轉產生前進的驅動力，并由另一個電機控制前輪轉向，從而實現AGV方向控制，即所謂的單舵輪結構形式，其結構如圖2所示。

圖2 單舵輪AGV結構模型

單舵輪AGV的數學模型主要與舵輪和從動輪的相對位置有關，其中O1和O2分別表示舵輪的回轉中心以及從動輪的幾何中心，b是AGV回轉中心和從動輪幾何中心的距離，通常用軸距表示。可通過控制舵機旋轉角度β和行進電機轉速，實現對AGV的路徑跟蹤控制。將O2作為AGV的參考點，則AGV的運動數學模型可描述為：

(1)

式中(x,y)為AGV參考點O2在坐標系中的位置，vW為行進電機的線速度，ω為車體回轉的角速度，并規定舵輪和車體逆時針旋轉為正方向，旋轉的中心定位為O′，回轉半徑為R，θ為坐標系下車體的朝向。在全局坐標系下，AGV的運動姿態可用[x,y,θ]T來表達。

2 AGV控制系統設計

2.1 導航方式

激光SLAM導航不需要在地面或車間做任何定位標記，屬于無軌導航，因此非常便于調整或修改AGV運行路徑，得到了廣泛的應用。SLAM最早由Smith R等專家提出，能讓載有激光傳感器的機器人在未知環境中和未知的初始位置條件下分析提取環境特征，并通過特征對比感知環境并構建地圖，然后再確定本體在地圖中的位置[6]。Gmapping是目前2D激光SLAM中普遍使用的算法，其中采用Rao-Blackwellized 粒子濾波(Rao-Blackwellized particle filtering，RBPF)的方法，引入自適應重采樣機制解決了SLAM過程中粒子消耗的問題[7]。

如圖3所示是本系統基于RBPF和數據融合方法設計的SLAM定位系統工作原理框圖，其工作中分為5個步驟：采樣、權重計算、掃描匹配、重采樣與數據融合、更新地圖。系統中選用了Sick公司的NAV350激光雷達測量車間環節的結構化信息，配合9軸運動姿態傳感器以及電機編碼器構成的里程計進行導航數據融合，從而提供準確的定位數據。同時激光傳感器還能提供AGV運行中遇到的靜態障礙物和動態障礙物信息，保障AGV的安全性能。

圖3 激光定位系統工作原理框圖

在實際應用中，可以首先建立車間的導航地圖，然后通過上層調度軟件在地圖中規劃運行路線和停靠站點，最后將設定路徑的起始點、目標點、目標點的朝向角、路徑類型、AGV運行速度以及停靠站點以及停靠時間等信息傳送給AGV控制系統即可。

2.2 控制問題分析

考慮到單舵輪AGV的運動性能，將AGV的運動分解為直線行駛(直線運動)和轉彎(圓弧運動)兩個過程。行駛過程中，vW由行進電機的旋轉速度決定，對路徑跟蹤系統來說是一個常量。直接路徑跟蹤中的位置偏差計算方法介紹如下。

(1)直線路徑跟蹤

AGV起始點運動姿態為[x1,y1,θ1]T，目標點期望姿態[x2,y2,θ2]T，當前運動姿態[x,y,θ]T，則運行過程中設定路徑的直線方程Ax+By+C=0中各參數為：

(2)

其中y2≠y1，x2≠x1,從而得到AGV到直線路徑上期望位置的偏差為：

errdist=(Ax+By+C)×cos(atan2(A,-B))

(3)

(2)圓弧路徑跟蹤

當AGV的行進速度vW和舵角β不變時，AGV會沿中心點O'做半徑為R回轉運動。因此，為了讓AGV沿指定的圓弧運動，需要根據起始姿態和目標姿態計算AGV的舵角初始值。因起始位置和目標位置是圓弧上的兩個點，且圓心處于目標位置的垂線上，根據幾何知識可以得到經過圓心和目標位置的直線方程為：

-tan(θ2+pi/2)x+y+tan(θ2+pi/2)x2-y2=0

(4)

根據起始點、目標點以及圓心三者構成的三角形，可以計算出回轉半徑R和圓心坐標[xcir,ycir]，然后可計算出AGV沿圓弧行進中的位置偏差：

(5)

2.3 控制算法設計

控制算法的設計目標是讓運行過程中位置以及朝向角偏差趨向于0。PID控制算法不依賴于被控對象精確的數學模型，在目前工業控制中占有非常重要的地位，但在激光SLAM導航方式中，容易因為地圖更新不及時或外部擾動出現AGV定位信息的波動，PID算法中的微分環節會將這種擾動放大，非常容易引起震蕩。自抗擾控制算法除繼承了PID算法的優點外，增加了跟蹤微分器環節使給定輸入變得更平滑，另外，還通過狀態觀測器補償和消除系統中未建模的部分以及運行中收到的內部擾動和外部擾動，進而得到了較好的控制效果[8]。

由AGV數學模型公式可知，該系統是一個關于舵角β的二階系統，而舵機運動過程是一個滯后環節，AGV運動過程中的負載變化以及激光SLAM位置抖動等都會給系統帶來干擾，常規的線性系統控制算法難以取得優良的控制效果。因此考慮充分利用自抗擾控制器的能自動補償和消除擾動的優點實現AGV的自動路徑跟蹤控制，本項目中設計的控制系統結構如圖4所示。

圖4 路徑跟蹤自抗擾控制器結構

該控制系統由AGV行進速度和路徑跟蹤兩個子系統組成。行進速度控制器實現AGV的行進電機轉速的自抗擾控制，而路徑跟蹤控制器則通過跟蹤路徑位置偏差不斷修正轉舵角度，進而保證AGV沿設定路線行駛。離散化的自抗擾控制算法如下所示：

(6)

式中,r0稱為快速因子，與電機功率有關;h為算法采樣時間，非線性函數fhan(·)以及fal(·)的表示形式可參考相關文獻。需要調整的參數有r0、α1、α2、β01、β02、β03、b0，可按照分離性原理逐個調試。

2.4 電路設計與實現

該系統以STM32F767為核心控制器，采用模塊化的設計方法，各模塊基于車載無線路由器實現數據的交互。STM32F767是意法半導體公司生產的Cortex-M7內核的32位ARM控制器，其CPU運行頻率高達216 MHz，內部帶有雙精度浮點數字單元，能滿足系統運行控制算法的需求。具備多個串口、CAN控制器、USB接口、以太網控制器以及TFT控制器等片內外設資源，外圍設備運行速度獨立于CPU頻率。STM32F767通過LAN8720以太網接口電路和無線路由器獲取激光SLAM的定位數據以及避碰信息。但為了防止激光SLAM對復雜多變的工業現場環境處理不及時，增加了實時性更高的碰撞傳感器作為避碰急停保護，從而保護現場人員、環境以及車體的安全。為了便于現場人員管理和操作AGV，還設計了以無線遙控電路以及觸摸屏構成的人機接口單元電路。限于篇幅，僅給出控制器部分電路原理如圖5所示。

3 調試及運行結果

為了驗證本文設計的AGV控制系統的有效性，首先在仿真環境下調整了控制器參數，驗證了控制器對外部擾動及內部數學模型不確定性的控制性能，然后將算法移植到設計的硬件平臺，并在車間環境下進行了實驗驗證。

圖5 系統電路原理圖

圖6 仿真環境下AGV控制性能測試

3.1 仿真驗證

仿真測試中，選用了轉彎時典型的“直線-圓弧-直線”路徑。初始仿真條件以及目標點信息設置如下：

(7)

通過圖6可以看出，AGV在初始位置開始運動時會沖出設定路徑，這是由單舵輪結構和初始朝向角誤差較大原因引起的。而隨后AGV能迅速運動到所設定的路線上，然后沿所設定的路線運動，運行過程誤差以及超調都非常小，說明所設計的系統能夠實現AGV的自動控制。

3.2 車間測試

為了測試該系統在實際應用中的控制性能，在車間環境下，通過調度軟件給AGV設計了一條圓環形的路線，來測試AGV控制器的控制精度和重復性。所設計的路徑中直線部分長度為20 m，4個拐角分別設計了半徑為1.5 m、1 m以及2 m的圓弧路徑，循環運行5圈為一個周期，行駛速度設為0.4 m/s。然后通過遙控器將AGV小車移動到三個不同的初始位置進行測試。

圖7 車間環境下AGV性能測試結果

調度軟件端記錄的AGV軌跡如圖7所示。從圖中可以看出，AGV運行較為穩定，不同的初始位置條件下都能準確跟蹤設定路線。通過測試數據分析，運行過程中SLAM的定位數據誤差2.5 cm，而AGV路線誤差不超過3.5 cm，停靠站點的誤差不大于1 cm，達到了預期控制目標。

結 語

參考文獻

[1] 趙赦. 新型機器人沖壓自動化生產線[J]. 世界制造技術與裝備市場, 2016(1):91-93.

[2] 韓以倫, 李明波, 郭喚喚,等. 智能AGV運輸車控制系統設計[J]. 智慧工廠, 2017(9):69-71.

[3] 耿牛牛. 單舵輪AGV路徑跟蹤方法的研究[J]. 制造業自動化, 2011, 33(7):81-82.

[4] 李占賢, 馮永利. 單舵輪AGV路徑跟蹤控制器設計[J]. 控制工程, 2014(s1):70-72.

[5] 暴海寧, 宋科, 黎原,等. 激光導引AGV車載控制系統結構及其實現[J]. 起重運輸機械, 2016(5):71-74.

[6] 徐曙. 基于SLAM的移動機器人導航系統研究[D]. 上海：華中科技大學, 2014.

[7] 王依人, 鄧國慶, 劉勇,等. 基于激光雷達傳感器的RBPF-SLAM系統優化設計[J]. 傳感器與微系統, 2017(9):77-80.

[8] 王常順, 肖海榮. 基于自抗擾控制的水面無人艇路徑跟蹤控制器[J]. 山東大學學報：工學版, 2016, 46(4):54-59.

張煌(工程師)，主要研究方向為壓力機電氣自動化。