蔡楊華,譚金,王冠

(1.廣東電網(wǎng)有限責(zé)任公司電力科學(xué)研究院，廣東 廣州 510080;2.廣東電科院能源技術(shù)有限責(zé)任公司，廣東 廣州 510080)

現(xiàn)有計量檢定中心庫前區(qū)多采用人工裝卸貨方式，存在工作效率低、錯誤率高、管理混亂等問題。當庫前區(qū)使用自動裝卸裝置時，需實現(xiàn)表計周轉(zhuǎn)箱從立體倉庫往返配送車輛的運輸、定位、擺放等過程的自動化[1-2]。受到庫前區(qū)貨物堆放雜多、環(huán)境干擾大、配送車輛規(guī)格不一等因素限制，現(xiàn)有的自動裝卸技術(shù)只能實現(xiàn)特定規(guī)格車廂的裝卸，難以滿足無人化庫前區(qū)的需求。目前，國外自動裝卸貨系統(tǒng)主要是通過對自有物流的車廂、月臺以及各銜接環(huán)節(jié)實施標準化設(shè)計制造來實現(xiàn)貨物的自動裝卸。我國采用第三方物流，車廂大多未進行標準化設(shè)計或改造，無法使用現(xiàn)有國外成熟的自動裝卸方案，同時國內(nèi)對適于現(xiàn)有國情的自動裝卸系統(tǒng)也鮮有研究；因此，開展庫前區(qū)自動裝卸裝置的研究對響應(yīng)國家智能化制造策略和提高計量檢定中心智能化及無人化運行水平意義重大[3]。廣東電網(wǎng)電能計量檢定中心旨在建設(shè)成國內(nèi)技術(shù)與管理水平領(lǐng)先的計量檢測單位，實現(xiàn)計量檢測的標準化與自動化，滿足市場改革形勢下廣東“十三五”及“十四五”期間電能計量設(shè)備大規(guī)模推廣的檢定需求[4]。計量庫前作業(yè)區(qū)作為計量檢定自動化的起始區(qū)域，是提高廣東電網(wǎng)計量檢定中心自動化出入庫作業(yè)效率極其重要的一個環(huán)節(jié)。計量庫前區(qū)的主體為自動導(dǎo)引運輸車(automated guided vehicle，AGV)，實現(xiàn)電能計量周轉(zhuǎn)箱垛的自動存入、自動取出及存儲等功能。AGV的核心之一便是穩(wěn)定準確的控制系統(tǒng)；為此，本文對庫前區(qū)自動裝卸裝置的控制系統(tǒng)進行了建模和仿真，利用運動學(xué)模型和物理模型的建立、軌跡規(guī)劃控制算法的仿真，驗證現(xiàn)實運行過程工況的參數(shù)及方法，為智能匹配作業(yè)環(huán)境和實現(xiàn)對計量周裝箱自動裝卸的控制策略提供基礎(chǔ)。

1 控制系統(tǒng)與仿真流程

AGV的控制系統(tǒng)需滿足智能AGV的運動及裝卸物料控制要求[5-7]，具體內(nèi)容如下：自動裝卸貨AGV在控制系統(tǒng)作用下將1垛周轉(zhuǎn)箱自動裝車及卸車，其涉及到智能判別貨物及車廂位置、精確的路徑規(guī)劃和準確的停車位置等技術(shù)[8-10]。依據(jù)表1相應(yīng)的需求進行控制算法及系統(tǒng)設(shè)計，并對AGV控制系統(tǒng)進行仿真模擬，根據(jù)仿真過程的表現(xiàn)及參數(shù)來驗證控制系統(tǒng)設(shè)計的合理性，最終優(yōu)化改進控制算法及控制方案。

表1 控制功能主要需求

控制系統(tǒng)在設(shè)計過程中涉及到了車體的運動學(xué)及動力學(xué)問題[11]，對其進行仿真分析可保證所設(shè)計控制系統(tǒng)的可靠性和穩(wěn)定性；此外，軌跡規(guī)劃及跟蹤控制的算法也需經(jīng)過仿真驗證可行之后才能作為實際車體調(diào)試的最終輸入。

本文主要使用MATLAB對設(shè)計的控制算法進行仿真模擬，用以驗證控制系統(tǒng)的可行性及準確度。使用機器人運動仿真V-rep軟件來模擬AGV的整體運行情況，并應(yīng)用MATLAB控制程序?qū)-rep物理模型進行運動控制，有效模擬自動裝卸貨AGV的現(xiàn)實工況。

2 系統(tǒng)模型建立

2.1 運動學(xué)模型建立

為方便分析作如下假設(shè)：①車體質(zhì)心位于左右對稱中心處； ②兩后輪關(guān)于中心線對稱分布；③車體運行在平穩(wěn)地面且在Z方向不考慮移動，X和Y方向上不考慮翻轉(zhuǎn)；④車體為剛體[12]。激光雷達安裝在兩支撐輪的連線中心，如圖1所示。以激光雷達位置點C為中心建立瞬時慣性坐標系XCY(車體坐標系)，規(guī)定逆時針轉(zhuǎn)角和角速度為正。車體在轉(zhuǎn)向過程中能夠平穩(wěn)轉(zhuǎn)動的條件是：每個車輪同時圍繞一個速度瞬心做圓周運動(如圖1所示)，車體以左右車輪延長線上的某一點O以角速度ω轉(zhuǎn)動，此時底盤運動過程中無側(cè)滑。圖1中，VA為舵輪在A點的線速度，ωA為舵輪在A點的角速度，R為舵輪半徑，φ為舵輪轉(zhuǎn)角，ω為車體角速度，L為前輪軸心與后輪軸心線的間距，VX為跟蹤點沿X軸方向的速度，VY為跟蹤點沿Y軸方向的速度，X為跟蹤點的橫坐標，Y為跟蹤點的縱坐標。

圖1 車體運動分析

假設(shè)舵輪在前進方向不打滑，只作純滾動，即：

VA=ωAR.

(1)

ω=ωARsinφ/L.

(2)

車架激光雷達位置處的運動方程為：

VX=ωL-VAsinφ=0.

(3)

VY=VAcosφ=ωARcosφ.

(4)

圖2為以地圖原點位置為中心建立瞬時慣性坐標系XOY，規(guī)定逆時針轉(zhuǎn)角和角速度為正，則在地圖坐標系下有以下關(guān)系：

(5)

(6)

(7)

圖2 車體運動分析

AGV在運動過程中，初始位姿狀態(tài)可用[XYθ]T表示，目標位姿狀態(tài)可用[X1Y1θ1]T表示，其中位姿誤差為[XeYeθe]T。

算法驗證過程采用小車的運動學(xué)模型來計算模擬實際運行過程中小車的位置坐標，考慮到實際情況的各種干擾，在運算結(jié)果中加入高斯分布的誤差值進行模擬[13-15]。

對式(5)—(7)進行離散化便于計算機進行處理，Ts表示采樣時間間隔，離散結(jié)果如下：

Xn+1=Xn+TsωARcosφncosθn,n=1,2,….

(8)

Yn+1=Yn+TsωARcosφnsinθn.

(9)

θn+1=θn+TsωARsinφn/L.

(10)

擾動信號采用高斯噪聲，由于采樣時間間隔很小，所以噪聲數(shù)量級為毫米級。

2.2 物理模型建立

本文使用具有真實物理引擎的仿真軟件V-rep對AGV的物理模型進行運動學(xué)和動力學(xué)仿真，檢查控制算法的正確性，依據(jù)算法規(guī)劃有效的路徑[16]，可有效避免小車在實際運行過程中不按照預(yù)想軌跡運行甚至撞車等異常情況。仿真前首先需要在V-rep中建立AGV小車的物理抽象模型，主要功能是模擬小車的各個運動關(guān)節(jié)，如圖3(a)所示。主要功能模擬如下：①車體——以立方體代替，設(shè)置為基體，并設(shè)置其質(zhì)量、轉(zhuǎn)動慣量等參數(shù)。②從動輪——從動輪數(shù)量為2個，以圓柱體代替，設(shè)置其質(zhì)量和轉(zhuǎn)動慣量等物理參數(shù)。③從動關(guān)節(jié)——從動關(guān)節(jié)數(shù)量為2個，連接基體與2個從動輪，設(shè)置為從動關(guān)節(jié)。④舵輪轉(zhuǎn)架——以圓盤代替，設(shè)置其質(zhì)量和轉(zhuǎn)動慣量等物理參數(shù)。⑤轉(zhuǎn)向關(guān)節(jié)——使能，并設(shè)置最大速度，使用其位置控制功能。⑥舵輪——以圓柱體代替，設(shè)置其質(zhì)量和轉(zhuǎn)動慣量等物理參數(shù)。⑦行走關(guān)節(jié)——使能，設(shè)置最大加速度，使用其速度控制功能。⑧貨叉——以立方體代替，并設(shè)置其質(zhì)量和轉(zhuǎn)動慣量等物理參數(shù)。⑨位移關(guān)節(jié)——使能，并設(shè)置最大速度，使用其位置控制功能。

對AGV小車進行外形建模，因為仿真的需求，外形模型主要以提取特征點、滿足關(guān)鍵的運動關(guān)節(jié)處為主，整體的模型效果如圖3(b)所示。

將外觀模型導(dǎo)入V-rep仿真軟件中，并將相關(guān)部件與抽象的物理模型進行融合，得到在仿真軟件中可控的、具有物理參數(shù)的真實AGV模型，如圖3(c)所示。建立的AGV小車物理模型有3個控制電機作為輸入控制窗口，分別控制轉(zhuǎn)向關(guān)節(jié)、行走關(guān)節(jié)、貨叉位移關(guān)節(jié)。

2.3 運行場景搭建

為了真實模擬現(xiàn)實自動裝卸貨的運行過程，本文在仿真軟件V-rep中搭建了AGV裝卸貨的運行環(huán)境，如圖4所示。運行環(huán)境中實際存在的物理實體為登車橋、車廂和貨物，其中車廂和登車橋在環(huán)境中是靜止的，貨物則設(shè)置成90 kg質(zhì)量以便模擬被AGV小車貨叉裝卸和搬運。

在搭建的環(huán)境中還設(shè)置了AGV運行過程中的各個站點，在現(xiàn)實運行環(huán)境中這些站點是不會標記在地面上的，只是存儲在地圖中與AGV系統(tǒng)進行比對。在仿真環(huán)境中為了得到更好的顯示效果，將這些站點以圓盤的形式顯示在搭建環(huán)境中，以便在仿真過程中比對站點的跟蹤效果；此外，為了顯示貨叉取貨的場景，仿真中在AGV車尾添加了相機傳感器，可將車后的場景實時顯示在監(jiān)視畫面上。至此已完成了仿真場景的搭建，其中設(shè)置的站點是可按照實際運行環(huán)境進行添加及變更的。

圖3 AGV模型

圖4 模擬運行物理環(huán)境

3 軌跡規(guī)劃算法仿真

3.1 直線行駛路徑仿真

直線行駛要求實時規(guī)劃舵輪與終點的直線路徑，每一個采樣周期計算能使舵輪朝向終點直線前進的舵輪轉(zhuǎn)角和剩余行駛距離[17]，并且輸出控制，具體步驟如下：①獲取上一周期位姿信息及舵輪轉(zhuǎn)角，并計算本周期小車的位姿；②利用本周期位置坐標和終點坐標計算剩余行駛距離；③計算舵輪位置坐標；④利用舵輪位置和終點位置構(gòu)成直線和車體的夾角計算本周期舵輪轉(zhuǎn)角，具體計算式如下：

(11)

(12)

φt=0,S

(13)

(14)

(15)

式中：S為跟蹤點與終點的距離；(xt，yt)為t時跟蹤點位置坐標；(xe，ye)為終點位置坐標；φt為舵輪t時轉(zhuǎn)角；α為跟蹤點和終點連線與跟蹤直線的夾角；θt為t時姿態(tài)車體中心線與橫坐標的夾角；h為跟蹤點與跟蹤直線的距離；(x0，y0)為起始點位置坐標。

直線跟蹤算法仿真得出的結(jié)果如圖5所示。從圖5中可以看出：該算法可很好地跟蹤確定直線路徑，小車實際行走路徑與理論路徑非常接近，并且跟蹤過程中舵輪轉(zhuǎn)角控制量波動能保持在0.05°以內(nèi)，對于直線行走控制系統(tǒng)來說較為穩(wěn)定。通過計算可以得到跟蹤過程中距離偏離量的變化和姿態(tài)角度變化的結(jié)果，如圖6所示。從圖6中可知:直線跟蹤路徑偏移量最大為 3 mm,滿足控制設(shè)計要求(10 mm);姿態(tài)角度偏差最大為0.02°，滿足行車要求。

3.2 圓弧行駛路徑仿真

實時規(guī)劃雷達跟蹤點與終點的劣弧路徑，每一個采樣周期計算雷達點到終點行走固定圓弧的舵輪轉(zhuǎn)角和剩余行駛距離，并輸出控制[18]。具體步驟如下：①獲取上一周期位姿信息及舵輪轉(zhuǎn)角，并計算本周期小車的位姿；②利用本周期位置坐標和終點坐標計算剩余行駛距離；③計算當前跟蹤點與終點構(gòu)成圓弧的割線角、圓弧半徑以及圓心角；④利用舵輪與兩后輪共心轉(zhuǎn)動的特點并結(jié)合圓心位置計算本周期舵輪轉(zhuǎn)角，具體計算式如下：

圖5 直線軌跡仿真

圖6 跟蹤偏差

S=γR.

(16)

(17)

γ=β-θt.

(18)

(19)

(20)

式中：γ為跟蹤點與終點規(guī)劃圓弧的圓心半角；β為跟蹤點與終點連線與橫坐標軸的夾角。

圓弧跟蹤算法仿真結(jié)果如圖7所示。由圖7可看出：該算法可很好地跟蹤確定圓弧路徑，小車實際行走路徑與理論路徑非常接近，并且跟蹤過程中舵輪轉(zhuǎn)角控制量波動較小，系統(tǒng)較為穩(wěn)定，只有在起始階段舵輪角有階躍輸入。通過計算可以得到跟蹤過程中距離偏離量的變化和姿態(tài)角度變化，結(jié)果如圖8所示。由圖8可知：路徑偏移量最大為 6 mm，滿足控制系統(tǒng)路徑偏移量最大為10 mm的設(shè)計要求；姿態(tài)角度偏差最大為0.05°，AGV運行穩(wěn)定性良好。

圖7 圓弧軌跡跟蹤

圖8 圓弧跟蹤偏差

3.3 原地旋轉(zhuǎn)仿真

原地旋轉(zhuǎn)比較簡單，只要將舵輪角度調(diào)制90°或-90°進行運行即可，運行時檢測姿態(tài)角的偏差量，達到誤差范圍內(nèi)時停止。需要注意的是，原地旋轉(zhuǎn)運動時，車速是很低的。AGV原地旋轉(zhuǎn)從30°調(diào)整到0°仿真結(jié)果如圖9所示。

圖9 原地旋轉(zhuǎn)運行

3.4 裝卸貨時方向調(diào)整路徑仿真

AGV在裝卸貨時需要調(diào)整自身方向使其可以準確地取放貨物，由于取貨時AGV在車廂內(nèi)部，其行走路線規(guī)劃不合理就會和車廂發(fā)生碰撞[19-20]，所以本文所用的控制方法是在起始位置首先調(diào)整好角度使AGV在逼近目標位置時自身狀態(tài)是偏向車廂內(nèi)側(cè)的，以防與車廂碰撞。具體步驟如下：①根據(jù)相機反饋貨物位置進行初始姿態(tài)角度的計算；②依據(jù)計算的初始位姿角度對AGV進行原地旋轉(zhuǎn)調(diào)整到所需角度；③AGV進行圓弧軌跡規(guī)劃跟蹤，逼近目標的位置；④進入取貨位置后，AGV進行一段緩慢的直線運行后插貨。圖10為裝卸貨時方向調(diào)整的仿真結(jié)果。

圖10 裝卸貨路徑調(diào)整

從圖10可以看出：在自動裝卸貨AGV對正貨物調(diào)整路徑時，控制算法可以實現(xiàn)10 mm精度以內(nèi)的跟蹤控制，系統(tǒng)的控制量轉(zhuǎn)角也相對穩(wěn)定。

3.5 控制系統(tǒng)運行仿真

對AGV小車物理模型和運行環(huán)境進行控制運行仿真。仿真過程中，AGV小車的實時坐標會顯示在窗口界面中，小車的運行軌跡也在仿真環(huán)境中進行了顯示，此外對相機的視頻數(shù)據(jù)也進行了實時顯示，如圖11、12所示。仿真的主要內(nèi)容是：利用MATLAB的軌跡跟蹤控制算法的代碼不停地計算舵輪轉(zhuǎn)角和轉(zhuǎn)速2個控制量，然后傳輸至V-rep軟件中的關(guān)節(jié)電機模型中，以此控制AGV小車進行運動。在仿真運行過程中既要控制小車運動，也要對貨叉的運動進行控制仿真，在AGV小車運行到裝卸貨位置時，貨叉電機會受到控制信號進行位置變動，以此來完成貨物的取放，如圖13所示。

圖11 AGV裝貨運動軌跡

圖12 AGV卸貨運動軌跡

圖13 自動裝卸貨全過程

通過控制系統(tǒng)的整體仿真和模擬現(xiàn)實的工作情況，可以驗證得出本自動裝卸貨AGV的控制算法及控制流程可以滿足庫前區(qū)自動裝卸貨的工作任務(wù)，所設(shè)計的控制算法滿足精度要求。

4 控制系統(tǒng)實驗驗證

AGV實驗測試環(huán)境如圖14(a)所示，在測試范圍內(nèi)布設(shè)適量用于定位與導(dǎo)航的激光反光柱，搭建模擬取放計量周裝箱的車廂環(huán)境及登車橋。圖14(b)為AGV實物圖，采用單貨叉形式，裝配了距離傳感器和深度視覺相機。

圖14 AGV實驗測試

在AGV的運行路徑上設(shè)置固定的站點編號，使AGV在站點間進行直線運行和圓弧曲線運行。在AGV上安放激光燈指示器，在站點放置坐標紙以記錄激光燈指示器到達指定位置所投射的坐標。依據(jù)AGV每次到達目的地后測得2個激光指示燈位置坐標的變化數(shù)據(jù)，求解雷達中心的位置坐標的變化量和姿態(tài)角度的變化量，從而得到AGV在控制算法下的重復(fù)停止精度變化，如圖15、16所示。

從圖15、16可以看出：AGV行駛到各個站點位置時其橫、縱坐標偏差保持在±6 mm以內(nèi)，姿態(tài)偏差角度保持在±0.8°以內(nèi)。由此可知，AGV在實際實驗環(huán)境運行時具有較高的運行控制精度，具備實驗現(xiàn)場的應(yīng)用。

圖15 站點的橫縱坐標偏差值

圖16 站點姿態(tài)偏差角度值

5 結(jié)束語

電能計量檢定自動化無人化是電網(wǎng)智能化發(fā)展的重要環(huán)節(jié)，作為計量檢定中心庫前作業(yè)區(qū)自動化起始區(qū)域主體的AGV，其控制算法和控制精度都影響著出入庫的作業(yè)效率。為此，本文對AGV的控制系統(tǒng)進行建模和仿真，重點介紹在控制設(shè)計過程中的算法仿真分析及整體控制流程的運動仿真分析。根據(jù)控制系統(tǒng)的要求建立控制模型和仿真模型，搭建運行環(huán)境以模擬自動裝卸貨的全部過程，同時利用現(xiàn)場測試的實驗數(shù)據(jù)驗證該控制系統(tǒng)的準確性。仿真及實驗結(jié)果表明本文所設(shè)計的自動裝卸貨AGV的控制算法及控制流程可滿足計量中心庫前區(qū)自動裝卸的工作任務(wù)要求，仿真設(shè)計控制算法滿足精度要求，可為實際AGV裝置的開發(fā)調(diào)試節(jié)省大量的時間并可有效避免物理碰撞事故發(fā)生。