(濟(jì)南二機(jī)床集團(tuán)有限公司，濟(jì)南 250022)

引 言

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

在復(fù)雜的車(chē)間環(huán)境中，要求AGV能夠：①準(zhǔn)確、快速到達(dá)指定位置；②遇到障礙物時(shí)能夠自動(dòng)停車(chē)；③能到達(dá)并自動(dòng)識(shí)別不同的站點(diǎn)，然后做出相應(yīng)的動(dòng)作。

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

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

圖1 控制系統(tǒng)結(jié)構(gòu)框圖

1 單舵輪AGV運(yùn)動(dòng)學(xué)數(shù)學(xué)模型

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

圖2 單舵輪AGV結(jié)構(gòu)模型

單舵輪AGV的數(shù)學(xué)模型主要與舵輪和從動(dòng)輪的相對(duì)位置有關(guān)，其中O1和O2分別表示舵輪的回轉(zhuǎn)中心以及從動(dòng)輪的幾何中心，b是AGV回轉(zhuǎn)中心和從動(dòng)輪幾何中心的距離，通常用軸距表示?？赏ㄟ^(guò)控制舵機(jī)旋轉(zhuǎn)角度β和行進(jìn)電機(jī)轉(zhuǎn)速，實(shí)現(xiàn)對(duì)AGV的路徑跟蹤控制。將O2作為AGV的參考點(diǎn)，則AGV的運(yùn)動(dòng)數(shù)學(xué)模型可描述為：

(1)

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

2 AGV控制系統(tǒng)設(shè)計(jì)

2.1 導(dǎo)航方式

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

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

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

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

2.2 控制問(wèn)題分析

考慮到單舵輪AGV的運(yùn)動(dòng)性能，將AGV的運(yùn)動(dòng)分解為直線行駛(直線運(yùn)動(dòng))和轉(zhuǎn)彎(圓弧運(yùn)動(dòng))兩個(gè)過(guò)程。行駛過(guò)程中，vW由行進(jìn)電機(jī)的旋轉(zhuǎn)速度決定，對(duì)路徑跟蹤系統(tǒng)來(lái)說(shuō)是一個(gè)常量。直接路徑跟蹤中的位置偏差計(jì)算方法介紹如下。

(1)直線路徑跟蹤

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

(2)

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

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

(3)

(2)圓弧路徑跟蹤

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

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

(4)

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

(5)

2.3 控制算法設(shè)計(jì)

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

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

圖4 路徑跟蹤自抗擾控制器結(jié)構(gòu)

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

(6)

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

2.4 電路設(shè)計(jì)與實(shí)現(xiàn)

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

3 調(diào)試及運(yùn)行結(jié)果

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

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

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

3.1 仿真驗(yàn)證

仿真測(cè)試中，選用了轉(zhuǎn)彎時(shí)典型的“直線-圓弧-直線”路徑。初始仿真條件以及目標(biāo)點(diǎn)信息設(shè)置如下：

(7)

通過(guò)圖6可以看出，AGV在初始位置開(kāi)始運(yùn)動(dòng)時(shí)會(huì)沖出設(shè)定路徑，這是由單舵輪結(jié)構(gòu)和初始朝向角誤差較大原因引起的。而隨后AGV能迅速運(yùn)動(dòng)到所設(shè)定的路線上，然后沿所設(shè)定的路線運(yùn)動(dòng)，運(yùn)行過(guò)程誤差以及超調(diào)都非常小，說(shuō)明所設(shè)計(jì)的系統(tǒng)能夠?qū)崿F(xiàn)AGV的自動(dòng)控制。

3.2 車(chē)間測(cè)試

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

圖7 車(chē)間環(huán)境下AGV性能測(cè)試結(jié)果

調(diào)度軟件端記錄的AGV軌跡如圖7所示。從圖中可以看出，AGV運(yùn)行較為穩(wěn)定，不同的初始位置條件下都能準(zhǔn)確跟蹤設(shè)定路線。通過(guò)測(cè)試數(shù)據(jù)分析，運(yùn)行過(guò)程中SLAM的定位數(shù)據(jù)誤差2.5 cm，而AGV路線誤差不超過(guò)3.5 cm，停靠站點(diǎn)的誤差不大于1 cm，達(dá)到了預(yù)期控制目標(biāo)。

結(jié) 語(yǔ)

參考文獻(xiàn)

[1] 趙赦. 新型機(jī)器人沖壓自動(dòng)化生產(chǎn)線[J]. 世界制造技術(shù)與裝備市場(chǎng), 2016(1):91-93.

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

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

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

[5] 暴海寧, 宋科, 黎原,等. 激光導(dǎo)引AGV車(chē)載控制系統(tǒng)結(jié)構(gòu)及其實(shí)現(xiàn)[J]. 起重運(yùn)輸機(jī)械, 2016(5):71-74.

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

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

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

張煌(工程師)，主要研究方向?yàn)閴毫C(jī)電氣自動(dòng)化。