基于自抗擾控制算法的單舵輪AGV控制系統(tǒng)設計

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

引 言

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

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

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

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

圖1 控制系統(tǒng)結構框圖

1 單舵輪AGV運動學數(shù)學模型

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

圖2 單舵輪AGV結構模型

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

(1)

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

2 AGV控制系統(tǒng)設計

2.1 導航方式

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

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

圖3 激光定位系統(tǒng)工作原理框圖

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

2.2 控制問題分析

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

(1)直線路徑跟蹤

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

(2)

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

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

(3)

(2)圓弧路徑跟蹤

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

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

(4)

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

(5)

2.3 控制算法設計

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

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

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

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

(6)

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

2.4 電路設計與實現(xiàn)

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

3 調試及運行結果

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

圖5 系統(tǒng)電路原理圖

圖6 仿真環(huán)境下AGV控制性能測試

3.1 仿真驗證

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

(7)

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

3.2 車間測試

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

圖7 車間環(huán)境下AGV性能測試結果

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

結 語

參考文獻

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

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

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

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

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

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

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

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

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