蔡楊華,譚金,王冠
(1.廣東電網(wǎng)有限責(zé)任公司電力科學(xué)研究院,廣東 廣州 510080;2.廣東電科院能源技術(shù)有限責(zé)任公司,廣東 廣州 510080)
現(xiàn)有計(jì)量檢定中心庫前區(qū)多采用人工裝卸貨方式,存在工作效率低、錯(cuò)誤率高、管理混亂等問題。當(dāng)庫前區(qū)使用自動(dòng)裝卸裝置時(shí),需實(shí)現(xiàn)表計(jì)周轉(zhuǎn)箱從立體倉庫往返配送車輛的運(yùn)輸、定位、擺放等過程的自動(dòng)化[1-2]。受到庫前區(qū)貨物堆放雜多、環(huán)境干擾大、配送車輛規(guī)格不一等因素限制,現(xiàn)有的自動(dòng)裝卸技術(shù)只能實(shí)現(xiàn)特定規(guī)格車廂的裝卸,難以滿足無人化庫前區(qū)的需求。目前,國外自動(dòng)裝卸貨系統(tǒng)主要是通過對自有物流的車廂、月臺(tái)以及各銜接環(huán)節(jié)實(shí)施標(biāo)準(zhǔn)化設(shè)計(jì)制造來實(shí)現(xiàn)貨物的自動(dòng)裝卸。我國采用第三方物流,車廂大多未進(jìn)行標(biāo)準(zhǔn)化設(shè)計(jì)或改造,無法使用現(xiàn)有國外成熟的自動(dòng)裝卸方案,同時(shí)國內(nèi)對適于現(xiàn)有國情的自動(dòng)裝卸系統(tǒng)也鮮有研究;因此,開展庫前區(qū)自動(dòng)裝卸裝置的研究對響應(yīng)國家智能化制造策略和提高計(jì)量檢定中心智能化及無人化運(yùn)行水平意義重大[3]。廣東電網(wǎng)電能計(jì)量檢定中心旨在建設(shè)成國內(nèi)技術(shù)與管理水平領(lǐng)先的計(jì)量檢測單位,實(shí)現(xiàn)計(jì)量檢測的標(biāo)準(zhǔn)化與自動(dòng)化,滿足市場改革形勢下廣東“十三五”及“十四五”期間電能計(jì)量設(shè)備大規(guī)模推廣的檢定需求[4]。計(jì)量庫前作業(yè)區(qū)作為計(jì)量檢定自動(dòng)化的起始區(qū)域,是提高廣東電網(wǎng)計(jì)量檢定中心自動(dòng)化出入庫作業(yè)效率極其重要的一個(gè)環(huán)節(jié)。計(jì)量庫前區(qū)的主體為自動(dòng)導(dǎo)引運(yùn)輸車(automated guided vehicle,AGV),實(shí)現(xiàn)電能計(jì)量周轉(zhuǎn)箱垛的自動(dòng)存入、自動(dòng)取出及存儲(chǔ)等功能。AGV的核心之一便是穩(wěn)定準(zhǔn)確的控制系統(tǒng);為此,本文對庫前區(qū)自動(dòng)裝卸裝置的控制系統(tǒng)進(jìn)行了建模和仿真,利用運(yùn)動(dòng)學(xué)模型和物理模型的建立、軌跡規(guī)劃控制算法的仿真,驗(yàn)證現(xiàn)實(shí)運(yùn)行過程工況的參數(shù)及方法,為智能匹配作業(yè)環(huán)境和實(shí)現(xiàn)對計(jì)量周裝箱自動(dòng)裝卸的控制策略提供基礎(chǔ)。
AGV的控制系統(tǒng)需滿足智能AGV的運(yùn)動(dòng)及裝卸物料控制要求[5-7],具體內(nèi)容如下:自動(dòng)裝卸貨AGV在控制系統(tǒng)作用下將1垛周轉(zhuǎn)箱自動(dòng)裝車及卸車,其涉及到智能判別貨物及車廂位置、精確的路徑規(guī)劃和準(zhǔn)確的停車位置等技術(shù)[8-10]。依據(jù)表1相應(yīng)的需求進(jìn)行控制算法及系統(tǒng)設(shè)計(jì),并對AGV控制系統(tǒng)進(jìn)行仿真模擬,根據(jù)仿真過程的表現(xiàn)及參數(shù)來驗(yàn)證控制系統(tǒng)設(shè)計(jì)的合理性,最終優(yōu)化改進(jìn)控制算法及控制方案。
表1 控制功能主要需求
控制系統(tǒng)在設(shè)計(jì)過程中涉及到了車體的運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)問題[11],對其進(jìn)行仿真分析可保證所設(shè)計(jì)控制系統(tǒng)的可靠性和穩(wěn)定性;此外,軌跡規(guī)劃及跟蹤控制的算法也需經(jīng)過仿真驗(yàn)證可行之后才能作為實(shí)際車體調(diào)試的最終輸入。
本文主要使用MATLAB對設(shè)計(jì)的控制算法進(jìn)行仿真模擬,用以驗(yàn)證控制系統(tǒng)的可行性及準(zhǔn)確度。使用機(jī)器人運(yùn)動(dòng)仿真V-rep軟件來模擬AGV的整體運(yùn)行情況,并應(yīng)用MATLAB控制程序?qū)-rep物理模型進(jìn)行運(yùn)動(dòng)控制,有效模擬自動(dòng)裝卸貨AGV的現(xiàn)實(shí)工況。
為方便分析作如下假設(shè):①車體質(zhì)心位于左右對稱中心處; ②兩后輪關(guān)于中心線對稱分布;③車體運(yùn)行在平穩(wěn)地面且在Z方向不考慮移動(dòng),X和Y方向上不考慮翻轉(zhuǎn);④車體為剛體[12]。激光雷達(dá)安裝在兩支撐輪的連線中心,如圖1所示。以激光雷達(dá)位置點(diǎn)C為中心建立瞬時(shí)慣性坐標(biāo)系XCY(車體坐標(biāo)系),規(guī)定逆時(shí)針轉(zhuǎn)角和角速度為正。車體在轉(zhuǎn)向過程中能夠平穩(wěn)轉(zhuǎn)動(dòng)的條件是:每個(gè)車輪同時(shí)圍繞一個(gè)速度瞬心做圓周運(yùn)動(dòng)(如圖1所示),車體以左右車輪延長線上的某一點(diǎn)O以角速度ω轉(zhuǎn)動(dòng),此時(shí)底盤運(yùn)動(dòng)過程中無側(cè)滑。圖1中,VA為舵輪在A點(diǎn)的線速度,ωA為舵輪在A點(diǎn)的角速度,R為舵輪半徑,φ為舵輪轉(zhuǎn)角,ω為車體角速度,L為前輪軸心與后輪軸心線的間距,VX為跟蹤點(diǎn)沿X軸方向的速度,VY為跟蹤點(diǎn)沿Y軸方向的速度,X為跟蹤點(diǎn)的橫坐標(biāo),Y為跟蹤點(diǎn)的縱坐標(biāo)。
圖1 車體運(yùn)動(dòng)分析
假設(shè)舵輪在前進(jìn)方向不打滑,只作純滾動(dòng),即:
VA=ωAR.
(1)
ω=ωARsinφ/L.
(2)
車架激光雷達(dá)位置處的運(yùn)動(dòng)方程為:
VX=ωL-VAsinφ=0.
(3)
VY=VAcosφ=ωARcosφ.
(4)
圖2為以地圖原點(diǎn)位置為中心建立瞬時(shí)慣性坐標(biāo)系XOY,規(guī)定逆時(shí)針轉(zhuǎn)角和角速度為正,則在地圖坐標(biāo)系下有以下關(guān)系:
(5)
(6)
(7)
圖2 車體運(yùn)動(dòng)分析
AGV在運(yùn)動(dòng)過程中,初始位姿狀態(tài)可用[XYθ]T表示,目標(biāo)位姿狀態(tài)可用[X1Y1θ1]T表示,其中位姿誤差為[XeYeθe]T。
算法驗(yàn)證過程采用小車的運(yùn)動(dòng)學(xué)模型來計(jì)算模擬實(shí)際運(yùn)行過程中小車的位置坐標(biāo),考慮到實(shí)際情況的各種干擾,在運(yùn)算結(jié)果中加入高斯分布的誤差值進(jìn)行模擬[13-15]。
對式(5)—(7)進(jìn)行離散化便于計(jì)算機(jī)進(jìn)行處理,Ts表示采樣時(shí)間間隔,離散結(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)
擾動(dòng)信號(hào)采用高斯噪聲,由于采樣時(shí)間間隔很小,所以噪聲數(shù)量級(jí)為毫米級(jí)。
本文使用具有真實(shí)物理引擎的仿真軟件V-rep對AGV的物理模型進(jìn)行運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)仿真,檢查控制算法的正確性,依據(jù)算法規(guī)劃有效的路徑[16],可有效避免小車在實(shí)際運(yùn)行過程中不按照預(yù)想軌跡運(yùn)行甚至撞車等異常情況。仿真前首先需要在V-rep中建立AGV小車的物理抽象模型,主要功能是模擬小車的各個(gè)運(yùn)動(dòng)關(guān)節(jié),如圖3(a)所示。主要功能模擬如下:①車體——以立方體代替,設(shè)置為基體,并設(shè)置其質(zhì)量、轉(zhuǎn)動(dòng)慣量等參數(shù)。②從動(dòng)輪——從動(dòng)輪數(shù)量為2個(gè),以圓柱體代替,設(shè)置其質(zhì)量和轉(zhuǎn)動(dòng)慣量等物理參數(shù)。③從動(dòng)關(guān)節(jié)——從動(dòng)關(guān)節(jié)數(shù)量為2個(gè),連接基體與2個(gè)從動(dòng)輪,設(shè)置為從動(dòng)關(guān)節(jié)。④舵輪轉(zhuǎn)架——以圓盤代替,設(shè)置其質(zhì)量和轉(zhuǎn)動(dòng)慣量等物理參數(shù)。⑤轉(zhuǎn)向關(guān)節(jié)——使能,并設(shè)置最大速度,使用其位置控制功能。⑥舵輪——以圓柱體代替,設(shè)置其質(zhì)量和轉(zhuǎn)動(dòng)慣量等物理參數(shù)。⑦行走關(guān)節(jié)——使能,設(shè)置最大加速度,使用其速度控制功能。⑧貨叉——以立方體代替,并設(shè)置其質(zhì)量和轉(zhuǎn)動(dòng)慣量等物理參數(shù)。⑨位移關(guān)節(jié)——使能,并設(shè)置最大速度,使用其位置控制功能。
對AGV小車進(jìn)行外形建模,因?yàn)榉抡娴男枨?,外形模型主要以提取特征點(diǎn)、滿足關(guān)鍵的運(yùn)動(dòng)關(guān)節(jié)處為主,整體的模型效果如圖3(b)所示。
將外觀模型導(dǎo)入V-rep仿真軟件中,并將相關(guān)部件與抽象的物理模型進(jìn)行融合,得到在仿真軟件中可控的、具有物理參數(shù)的真實(shí)AGV模型,如圖3(c)所示。建立的AGV小車物理模型有3個(gè)控制電機(jī)作為輸入控制窗口,分別控制轉(zhuǎn)向關(guān)節(jié)、行走關(guān)節(jié)、貨叉位移關(guān)節(jié)。
為了真實(shí)模擬現(xiàn)實(shí)自動(dòng)裝卸貨的運(yùn)行過程,本文在仿真軟件V-rep中搭建了AGV裝卸貨的運(yùn)行環(huán)境,如圖4所示。運(yùn)行環(huán)境中實(shí)際存在的物理實(shí)體為登車橋、車廂和貨物,其中車廂和登車橋在環(huán)境中是靜止的,貨物則設(shè)置成90 kg質(zhì)量以便模擬被AGV小車貨叉裝卸和搬運(yùn)。
在搭建的環(huán)境中還設(shè)置了AGV運(yùn)行過程中的各個(gè)站點(diǎn),在現(xiàn)實(shí)運(yùn)行環(huán)境中這些站點(diǎn)是不會(huì)標(biāo)記在地面上的,只是存儲(chǔ)在地圖中與AGV系統(tǒng)進(jìn)行比對。在仿真環(huán)境中為了得到更好的顯示效果,將這些站點(diǎn)以圓盤的形式顯示在搭建環(huán)境中,以便在仿真過程中比對站點(diǎn)的跟蹤效果;此外,為了顯示貨叉取貨的場景,仿真中在AGV車尾添加了相機(jī)傳感器,可將車后的場景實(shí)時(shí)顯示在監(jiān)視畫面上。至此已完成了仿真場景的搭建,其中設(shè)置的站點(diǎn)是可按照實(shí)際運(yùn)行環(huán)境進(jìn)行添加及變更的。
圖3 AGV模型
圖4 模擬運(yùn)行物理環(huán)境
直線行駛要求實(shí)時(shí)規(guī)劃舵輪與終點(diǎn)的直線路徑,每一個(gè)采樣周期計(jì)算能使舵輪朝向終點(diǎn)直線前進(jìn)的舵輪轉(zhuǎn)角和剩余行駛距離[17],并且輸出控制,具體步驟如下:①獲取上一周期位姿信息及舵輪轉(zhuǎn)角,并計(jì)算本周期小車的位姿;②利用本周期位置坐標(biāo)和終點(diǎn)坐標(biāo)計(jì)算剩余行駛距離;③計(jì)算舵輪位置坐標(biāo);④利用舵輪位置和終點(diǎn)位置構(gòu)成直線和車體的夾角計(jì)算本周期舵輪轉(zhuǎn)角,具體計(jì)算式如下:
(11)
(12)
φt=0,S (13) (14) (15) 式中:S為跟蹤點(diǎn)與終點(diǎn)的距離;(xt,yt)為t時(shí)跟蹤點(diǎn)位置坐標(biāo);(xe,ye)為終點(diǎn)位置坐標(biāo);φt為舵輪t時(shí)轉(zhuǎn)角;α為跟蹤點(diǎn)和終點(diǎn)連線與跟蹤直線的夾角;θt為t時(shí)姿態(tài)車體中心線與橫坐標(biāo)的夾角;h為跟蹤點(diǎn)與跟蹤直線的距離;(x0,y0)為起始點(diǎn)位置坐標(biāo)。 直線跟蹤算法仿真得出的結(jié)果如圖5所示。從圖5中可以看出:該算法可很好地跟蹤確定直線路徑,小車實(shí)際行走路徑與理論路徑非常接近,并且跟蹤過程中舵輪轉(zhuǎn)角控制量波動(dòng)能保持在0.05°以內(nèi),對于直線行走控制系統(tǒng)來說較為穩(wěn)定。通過計(jì)算可以得到跟蹤過程中距離偏離量的變化和姿態(tài)角度變化的結(jié)果,如圖6所示。從圖6中可知:直線跟蹤路徑偏移量最大為 3 mm,滿足控制設(shè)計(jì)要求(10 mm);姿態(tài)角度偏差最大為0.02°,滿足行車要求。 實(shí)時(shí)規(guī)劃雷達(dá)跟蹤點(diǎn)與終點(diǎn)的劣弧路徑,每一個(gè)采樣周期計(jì)算雷達(dá)點(diǎn)到終點(diǎn)行走固定圓弧的舵輪轉(zhuǎn)角和剩余行駛距離,并輸出控制[18]。具體步驟如下:①獲取上一周期位姿信息及舵輪轉(zhuǎn)角,并計(jì)算本周期小車的位姿;②利用本周期位置坐標(biāo)和終點(diǎn)坐標(biāo)計(jì)算剩余行駛距離;③計(jì)算當(dāng)前跟蹤點(diǎn)與終點(diǎn)構(gòu)成圓弧的割線角、圓弧半徑以及圓心角;④利用舵輪與兩后輪共心轉(zhuǎn)動(dòng)的特點(diǎn)并結(jié)合圓心位置計(jì)算本周期舵輪轉(zhuǎn)角,具體計(jì)算式如下: 圖5 直線軌跡仿真 圖6 跟蹤偏差 S=γR. (16) (17) γ=β-θt. (18) (19) (20) 式中:γ為跟蹤點(diǎn)與終點(diǎn)規(guī)劃圓弧的圓心半角;β為跟蹤點(diǎn)與終點(diǎn)連線與橫坐標(biāo)軸的夾角。 圓弧跟蹤算法仿真結(jié)果如圖7所示。由圖7可看出:該算法可很好地跟蹤確定圓弧路徑,小車實(shí)際行走路徑與理論路徑非常接近,并且跟蹤過程中舵輪轉(zhuǎn)角控制量波動(dòng)較小,系統(tǒng)較為穩(wěn)定,只有在起始階段舵輪角有階躍輸入。通過計(jì)算可以得到跟蹤過程中距離偏離量的變化和姿態(tài)角度變化,結(jié)果如圖8所示。由圖8可知:路徑偏移量最大為 6 mm,滿足控制系統(tǒng)路徑偏移量最大為10 mm的設(shè)計(jì)要求;姿態(tài)角度偏差最大為0.05°,AGV運(yùn)行穩(wěn)定性良好。 圖7 圓弧軌跡跟蹤 圖8 圓弧跟蹤偏差 原地旋轉(zhuǎn)比較簡單,只要將舵輪角度調(diào)制90°或-90°進(jìn)行運(yùn)行即可,運(yùn)行時(shí)檢測姿態(tài)角的偏差量,達(dá)到誤差范圍內(nèi)時(shí)停止。需要注意的是,原地旋轉(zhuǎn)運(yùn)動(dòng)時(shí),車速是很低的。AGV原地旋轉(zhuǎn)從30°調(diào)整到0°仿真結(jié)果如圖9所示。 圖9 原地旋轉(zhuǎn)運(yùn)行 AGV在裝卸貨時(shí)需要調(diào)整自身方向使其可以準(zhǔn)確地取放貨物,由于取貨時(shí)AGV在車廂內(nèi)部,其行走路線規(guī)劃不合理就會(huì)和車廂發(fā)生碰撞[19-20],所以本文所用的控制方法是在起始位置首先調(diào)整好角度使AGV在逼近目標(biāo)位置時(shí)自身狀態(tài)是偏向車廂內(nèi)側(cè)的,以防與車廂碰撞。具體步驟如下:①根據(jù)相機(jī)反饋貨物位置進(jìn)行初始姿態(tài)角度的計(jì)算;②依據(jù)計(jì)算的初始位姿角度對AGV進(jìn)行原地旋轉(zhuǎn)調(diào)整到所需角度;③AGV進(jìn)行圓弧軌跡規(guī)劃跟蹤,逼近目標(biāo)的位置;④進(jìn)入取貨位置后,AGV進(jìn)行一段緩慢的直線運(yùn)行后插貨。圖10為裝卸貨時(shí)方向調(diào)整的仿真結(jié)果。 圖10 裝卸貨路徑調(diào)整 從圖10可以看出:在自動(dòng)裝卸貨AGV對正貨物調(diào)整路徑時(shí),控制算法可以實(shí)現(xiàn)10 mm精度以內(nèi)的跟蹤控制,系統(tǒng)的控制量轉(zhuǎn)角也相對穩(wěn)定。 對AGV小車物理模型和運(yùn)行環(huán)境進(jìn)行控制運(yùn)行仿真。仿真過程中,AGV小車的實(shí)時(shí)坐標(biāo)會(huì)顯示在窗口界面中,小車的運(yùn)行軌跡也在仿真環(huán)境中進(jìn)行了顯示,此外對相機(jī)的視頻數(shù)據(jù)也進(jìn)行了實(shí)時(shí)顯示,如圖11、12所示。仿真的主要內(nèi)容是:利用MATLAB的軌跡跟蹤控制算法的代碼不停地計(jì)算舵輪轉(zhuǎn)角和轉(zhuǎn)速2個(gè)控制量,然后傳輸至V-rep軟件中的關(guān)節(jié)電機(jī)模型中,以此控制AGV小車進(jìn)行運(yùn)動(dòng)。在仿真運(yùn)行過程中既要控制小車運(yùn)動(dòng),也要對貨叉的運(yùn)動(dòng)進(jìn)行控制仿真,在AGV小車運(yùn)行到裝卸貨位置時(shí),貨叉電機(jī)會(huì)受到控制信號(hào)進(jìn)行位置變動(dòng),以此來完成貨物的取放,如圖13所示。 圖11 AGV裝貨運(yùn)動(dòng)軌跡 圖12 AGV卸貨運(yùn)動(dòng)軌跡 圖13 自動(dòng)裝卸貨全過程 通過控制系統(tǒng)的整體仿真和模擬現(xiàn)實(shí)的工作情況,可以驗(yàn)證得出本自動(dòng)裝卸貨AGV的控制算法及控制流程可以滿足庫前區(qū)自動(dòng)裝卸貨的工作任務(wù),所設(shè)計(jì)的控制算法滿足精度要求。 AGV實(shí)驗(yàn)測試環(huán)境如圖14(a)所示,在測試范圍內(nèi)布設(shè)適量用于定位與導(dǎo)航的激光反光柱,搭建模擬取放計(jì)量周裝箱的車廂環(huán)境及登車橋。圖14(b)為AGV實(shí)物圖,采用單貨叉形式,裝配了距離傳感器和深度視覺相機(jī)。 圖14 AGV實(shí)驗(yàn)測試 在AGV的運(yùn)行路徑上設(shè)置固定的站點(diǎn)編號(hào),使AGV在站點(diǎn)間進(jìn)行直線運(yùn)行和圓弧曲線運(yùn)行。在AGV上安放激光燈指示器,在站點(diǎn)放置坐標(biāo)紙以記錄激光燈指示器到達(dá)指定位置所投射的坐標(biāo)。依據(jù)AGV每次到達(dá)目的地后測得2個(gè)激光指示燈位置坐標(biāo)的變化數(shù)據(jù),求解雷達(dá)中心的位置坐標(biāo)的變化量和姿態(tài)角度的變化量,從而得到AGV在控制算法下的重復(fù)停止精度變化,如圖15、16所示。 從圖15、16可以看出:AGV行駛到各個(gè)站點(diǎn)位置時(shí)其橫、縱坐標(biāo)偏差保持在±6 mm以內(nèi),姿態(tài)偏差角度保持在±0.8°以內(nèi)。由此可知,AGV在實(shí)際實(shí)驗(yàn)環(huán)境運(yùn)行時(shí)具有較高的運(yùn)行控制精度,具備實(shí)驗(yàn)現(xiàn)場的應(yīng)用。 圖15 站點(diǎn)的橫縱坐標(biāo)偏差值 圖16 站點(diǎn)姿態(tài)偏差角度值 電能計(jì)量檢定自動(dòng)化無人化是電網(wǎng)智能化發(fā)展的重要環(huán)節(jié),作為計(jì)量檢定中心庫前作業(yè)區(qū)自動(dòng)化起始區(qū)域主體的AGV,其控制算法和控制精度都影響著出入庫的作業(yè)效率。為此,本文對AGV的控制系統(tǒng)進(jìn)行建模和仿真,重點(diǎn)介紹在控制設(shè)計(jì)過程中的算法仿真分析及整體控制流程的運(yùn)動(dòng)仿真分析。根據(jù)控制系統(tǒng)的要求建立控制模型和仿真模型,搭建運(yùn)行環(huán)境以模擬自動(dòng)裝卸貨的全部過程,同時(shí)利用現(xiàn)場測試的實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證該控制系統(tǒng)的準(zhǔn)確性。仿真及實(shí)驗(yàn)結(jié)果表明本文所設(shè)計(jì)的自動(dòng)裝卸貨AGV的控制算法及控制流程可滿足計(jì)量中心庫前區(qū)自動(dòng)裝卸的工作任務(wù)要求,仿真設(shè)計(jì)控制算法滿足精度要求,可為實(shí)際AGV裝置的開發(fā)調(diào)試節(jié)省大量的時(shí)間并可有效避免物理碰撞事故發(fā)生。3.2 圓弧行駛路徑仿真
3.3 原地旋轉(zhuǎn)仿真
3.4 裝卸貨時(shí)方向調(diào)整路徑仿真
3.5 控制系統(tǒng)運(yùn)行仿真
4 控制系統(tǒng)實(shí)驗(yàn)驗(yàn)證
5 結(jié)束語