基于B樣條曲線的智能叉車托盤拾取路徑規(guī)劃研究

呂恩利 林偉加 劉妍華 王飛仁 趙俊宏 吳 鵬

(1.華南農(nóng)業(yè)大學(xué)工程學(xué)院， 廣州 510642； 2.華南農(nóng)業(yè)大學(xué)南方農(nóng)業(yè)機(jī)械與裝備關(guān)鍵技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室， 廣州 510642；3.華南農(nóng)業(yè)大學(xué)工程基礎(chǔ)教學(xué)與訓(xùn)練中心， 廣州 510642)

0 引言

干果品類多、價(jià)值較高，倉(cāng)儲(chǔ)運(yùn)輸中多采用托盤碼放的方式，利用智能叉車進(jìn)行運(yùn)輸作業(yè)，能提高倉(cāng)儲(chǔ)效率[1]。然而貨物托盤擺放位姿存在不確定性，應(yīng)用路徑規(guī)劃技術(shù)可實(shí)現(xiàn)智能叉車托盤自主拾取，從而提高叉車作業(yè)的自主性和靈活性。

路徑規(guī)劃是智能裝備的關(guān)鍵技術(shù)，一直以來(lái)都是國(guó)內(nèi)外學(xué)者的研究熱點(diǎn)[2-7]。傳統(tǒng)的規(guī)劃方法有人工勢(shì)場(chǎng)法、柵格法等[8]，基于生物智能的算法有遺傳算法、蟻群算法等[9-10]。有學(xué)者提出結(jié)合多種方法實(shí)現(xiàn)路徑規(guī)劃，如通過(guò)神經(jīng)網(wǎng)絡(luò)-模糊理論進(jìn)行規(guī)劃[11]，其不足之處是需要依賴大量試驗(yàn)來(lái)構(gòu)建控制器模型。另外，參數(shù)化曲線的方法也多被應(yīng)用于路徑規(guī)劃領(lǐng)域[12-13]，但主要的應(yīng)用對(duì)象是自由度較高的小型移動(dòng)平臺(tái)，對(duì)于大型非完整性約束裝備的路徑規(guī)劃研究較少。此外，也有學(xué)者提出智能車輛的路徑規(guī)劃以及參數(shù)優(yōu)化方法[14-15]，并通過(guò)仿真分析規(guī)劃跟蹤的過(guò)程，但暫未實(shí)現(xiàn)實(shí)車應(yīng)用。

本文以托盤拾取路徑規(guī)劃過(guò)程中首末端點(diǎn)約束、叉車參數(shù)約束，以及路徑曲率最小為目標(biāo)，建立多約束條件下的B樣條路徑曲線，并利用Matlab對(duì)不同托盤拾取場(chǎng)景進(jìn)行參數(shù)求解，最后通過(guò)試驗(yàn)驗(yàn)證規(guī)劃算法的可行性。

1 托盤拾取

1.1 作業(yè)過(guò)程

研究的作業(yè)環(huán)境為半結(jié)構(gòu)化倉(cāng)儲(chǔ)環(huán)境。倉(cāng)庫(kù)中設(shè)置貨物緩沖區(qū)，貨物托盤在緩沖區(qū)中的具體擺放位姿具有不確定性，采用傳感感知系統(tǒng)，進(jìn)行托盤位姿識(shí)別[16-17]，通過(guò)規(guī)劃的路徑調(diào)整叉車位姿，完成托盤拾取作業(yè)任務(wù)。

整個(gè)托盤拾取的作業(yè)過(guò)程如圖1所示。叉車通過(guò)車載激光定位傳感器獲得倉(cāng)庫(kù)全局坐標(biāo)下的定位信息，在距離貨物緩沖區(qū)一定距離時(shí)，啟動(dòng)托盤位姿識(shí)別算法，通過(guò)貨叉前端的RGB-D傳感器以及激光測(cè)距傳感器，獲得托盤孔中心在全局參考坐標(biāo)系中的坐標(biāo)。其中，RGB-D傳感器獲取的圖像具有較為豐富的紋理和色彩信息，但其深度探測(cè)距離有限，主要用于近距離的托盤位姿識(shí)別；激光傳感器具有穩(wěn)定、抗干擾能力強(qiáng)的特點(diǎn)，主要用于大偏移角度、遠(yuǎn)距離的托盤位姿識(shí)別。兩者結(jié)合，可提高托盤位姿識(shí)別的準(zhǔn)確性。

托盤位姿識(shí)別成功之后，進(jìn)行路徑規(guī)劃，智能叉車按照規(guī)劃的曲線，通過(guò)純追蹤算法進(jìn)行路徑跟蹤，最終完成托盤拾取的任務(wù)。圖1虛線框?yàn)閷?shí)現(xiàn)智能叉車托盤拾取的重要環(huán)節(jié)[18]。

1.2 路徑規(guī)劃問(wèn)題

基于托盤位姿識(shí)別系統(tǒng)已經(jīng)準(zhǔn)確識(shí)別出托盤孔中心的全局坐標(biāo)，并且通過(guò)激光定位傳感器已獲得叉車在倉(cāng)庫(kù)中實(shí)時(shí)位姿為前提，研究托盤拾取路徑規(guī)劃問(wèn)題。根據(jù)規(guī)劃的方向，可分為前進(jìn)式和進(jìn)退式路徑規(guī)劃，本文研究前進(jìn)式托盤拾取路徑規(guī)劃。

托盤拾取路徑規(guī)劃模型示意圖如圖2所示。研究的智能叉車屬于前輪驅(qū)動(dòng)、后輪轉(zhuǎn)向的非完整約束車輛，以前軸中心點(diǎn)為車輛坐標(biāo)系原點(diǎn)建立右手坐標(biāo)系，并將貨叉底端中點(diǎn)作為路徑參考點(diǎn)。規(guī)劃過(guò)程如下：在倉(cāng)庫(kù)全局坐標(biāo)系下，托盤位姿識(shí)別系統(tǒng)啟動(dòng)時(shí)將貨叉的中心坐標(biāo)作為全局路徑起始點(diǎn)，將識(shí)別到的托盤孔中心位姿作為全局路徑目標(biāo)點(diǎn)，在二維平面內(nèi)規(guī)劃一條從起始點(diǎn)到目標(biāo)點(diǎn)、符合叉車運(yùn)動(dòng)學(xué)的全局可行路徑。路徑離散點(diǎn)序列為{Pstart,P1,P2,…,Pn,Pdestination}，其中Pi(xi,yi,ψi)為路徑上的一個(gè)點(diǎn)，{xi,yi}為路徑點(diǎn)的全局坐標(biāo)，ψi為叉車此時(shí)的航向角。

2 托盤拾取路徑規(guī)劃算法

托盤拾取路徑規(guī)劃的目的是：規(guī)劃符合叉車運(yùn)動(dòng)學(xué)、叉車自身參數(shù)、曲率連續(xù)約束的可行路徑，使得智能叉車對(duì)托盤進(jìn)行自主拾取。托盤拾取路徑規(guī)劃算法基于三次均勻B樣條曲線，根據(jù)托盤拾取的相關(guān)約束條件，建立多約束方程以及曲率最小化的目標(biāo)函數(shù)，通過(guò)求解曲線的相關(guān)參數(shù)，從而得到拾取路徑。

2.1 叉車運(yùn)動(dòng)學(xué)模型建立

由于倉(cāng)庫(kù)作業(yè)過(guò)程中叉車的運(yùn)動(dòng)速度比較低，在低速行駛時(shí)所受側(cè)向力較小，假設(shè)在行駛過(guò)程中不發(fā)生側(cè)滑現(xiàn)象，所以路徑規(guī)劃時(shí)只考慮叉車的運(yùn)動(dòng)學(xué)模型。

假設(shè)叉車車體為剛性，車輪形變可忽略不計(jì)，且輪子和地面的每個(gè)接觸點(diǎn)僅產(chǎn)生純滾動(dòng)，不發(fā)生滑動(dòng)，叉車的運(yùn)動(dòng)學(xué)模型如圖3所示[19]。

圖3 叉車運(yùn)動(dòng)學(xué)模型Fig.3 Kinematics model of forklift1、2.前輪驅(qū)動(dòng)輪 3、4.后輪轉(zhuǎn)向輪 5.等效驅(qū)動(dòng)輪 6.等效轉(zhuǎn)向輪

圖3中全局坐標(biāo)系為OXY，叉車坐標(biāo)系為OXvYv；ICR為叉車瞬時(shí)旋轉(zhuǎn)中心。

由圖3可知，叉車航向角為

(1)

式中 (x,y)——路徑全局坐標(biāo)值

由文獻(xiàn)[20]可知，路徑曲率κ和瞬時(shí)旋轉(zhuǎn)半徑r、前后輪的軸距L以及等效轉(zhuǎn)向輪轉(zhuǎn)角α的關(guān)系為

(2)

由式(2)可得等效轉(zhuǎn)向輪轉(zhuǎn)角為

α=arctan(Lκ)

(3)

2.2 托盤拾取路徑曲線約束

2.2.1端點(diǎn)狀態(tài)約束

路徑規(guī)劃算法輸出的路徑曲線在起止點(diǎn)滿足以下約束：①起始點(diǎn)是叉車開(kāi)始進(jìn)行托盤位姿識(shí)別時(shí)貨叉底端中心點(diǎn)的全局坐標(biāo)點(diǎn)；路徑在起始點(diǎn)處的切向角等于叉車的初始航向角；路徑起點(diǎn)曲率為0或者盡量小，以降低車輪的初始轉(zhuǎn)向角。②終點(diǎn)是托盤位姿識(shí)別系統(tǒng)所獲得的托盤孔中心全局坐標(biāo)點(diǎn)；路徑在目標(biāo)點(diǎn)的切向角等于托盤在全局坐標(biāo)中的夾角，即叉車的目標(biāo)航向角；路徑終點(diǎn)曲率為0或者盡量小，使托盤拾取終點(diǎn)處的車輪轉(zhuǎn)向角接近于0。

2.2.2叉車參數(shù)約束

路徑規(guī)劃過(guò)程中曲率過(guò)大或者曲率不連續(xù)，將導(dǎo)致叉車無(wú)法對(duì)所規(guī)劃的路徑進(jìn)行準(zhǔn)確地跟蹤。而拾取過(guò)程中最終點(diǎn)的跟蹤誤差一旦過(guò)大可能導(dǎo)致托盤拾取失敗。故托盤拾取路徑規(guī)劃時(shí)需考慮叉車的自身參數(shù)約束。

(1)最小轉(zhuǎn)彎半徑約束。曲線曲率不大于叉車前軸中心點(diǎn)最大曲率，即每個(gè)路徑點(diǎn)上曲率應(yīng)滿足叉車最小轉(zhuǎn)彎半徑約束；即，κ≤1/Rmin，Rmin為叉車的最小轉(zhuǎn)彎半徑。

(2)曲率連續(xù)約束。為避免路徑跟蹤過(guò)程中出現(xiàn)原地轉(zhuǎn)向的現(xiàn)象，對(duì)輪胎造成過(guò)度磨損，故路徑應(yīng)保證曲率連續(xù)。因?yàn)椴捎萌蜝樣條曲線，所以滿足C2連續(xù)的性質(zhì)[21-22]。

由于托盤位姿識(shí)別系統(tǒng)識(shí)別距離最大約7 m，貨叉寬度15 cm、托盤孔寬度40.5 cm，故叉車行駛距離較短，對(duì)最終點(diǎn)的精度要求較高。為保證路徑跟蹤精度，實(shí)際拾取過(guò)程中叉車速度不超過(guò)800 mm/s。在該最大速度下，符合以上約束的路徑，轉(zhuǎn)向系統(tǒng)可進(jìn)行較好地跟蹤。因此不考慮后輪轉(zhuǎn)向角速度約束。

2.3 三次B樣條曲線

根據(jù)文獻(xiàn)[23]，三次均勻B樣條曲線表達(dá)式為

(4)

式中u——參數(shù)，取值為[0,1]

Nj,3(u)——三次B樣條基函數(shù)

三次B樣條曲線寫成矩陣形式為

(5)

三次B樣條曲線的一階導(dǎo)數(shù)、二階導(dǎo)數(shù)為

(6)

(7)

(8)

(9)

式中G——三次B樣條曲線的參數(shù)矩陣

V——控制點(diǎn)坐標(biāo)值矩陣

V1,i,x、V2，i,x、V3,i,x、V4,i,x——第i段B樣條曲線4個(gè)控制點(diǎn)橫坐標(biāo)值

V1,i,y、V2,i,y,、V3,i,y、V4,i,y——第i段B樣條曲線4個(gè)控制點(diǎn)縱坐標(biāo)值

由于三次均勻B樣條曲線的端點(diǎn)一般不通過(guò)特征多邊形的首、末頂點(diǎn)，也不直接與特征多邊形的首、末邊有關(guān)聯(lián)。構(gòu)造符合規(guī)劃要求的三次均勻樣條曲線的方法如下[24]：如圖4所示，分別以托盤拾取的初始位置點(diǎn)和目標(biāo)位置點(diǎn)Pstart(xs,ys,ψs)、Pdestination(xd,yd,ψd)為中點(diǎn)，增加P0、P1、P2、P34個(gè)控制點(diǎn)。其中P0P1、P2P3的斜率是初始航向角ψs和目標(biāo)航向角ψd。這樣能保證曲線的起點(diǎn)和終點(diǎn)以及相應(yīng)的切線方向是叉車的初始位姿點(diǎn)、目標(biāo)位姿點(diǎn)及相對(duì)應(yīng)的航向角。

圖4 B樣條曲線中控制點(diǎn)與曲線的關(guān)系Fig.4 Relationship between control points and B-spline curve

2.4 多約束條件下托盤拾取路徑曲線

由于B樣條曲線形狀完全由控制點(diǎn)的位置確定，對(duì)托盤拾取路徑參數(shù)化的過(guò)程就是求出控制點(diǎn)的過(guò)程。參照文獻(xiàn)[24]，已知路徑起始點(diǎn)、托盤孔中心目標(biāo)點(diǎn)坐標(biāo)的條件下，設(shè)圖4中P0P1和P2P3的長(zhǎng)度分別為2L1、2L2，通過(guò)求解多約束條件下參數(shù)L1、L2，可得到最優(yōu)的托盤拾取路徑。

2.4.1基于三次B樣條曲線的多約束條件

如圖4所示，路徑規(guī)劃的初始已知條件為：貨叉全局起始位姿Pstart(xs,ys,ψs)、托盤孔中心的全局目標(biāo)位姿為Pdestination(xd,yd,ψd)，P0P1的距離為2L1、P2P3的距離為2L2，可得P0、P1、P2、P3控制點(diǎn)的坐標(biāo)為

(10)

(11)

(12)

(13)

結(jié)合文獻(xiàn)[23]所述的B樣條曲線的性質(zhì)以及上述路徑規(guī)劃的約束條件，可以得到三次B樣條路徑曲線的首末端點(diǎn)位置為

(14)

(15)

三次B樣條曲線首末端點(diǎn)處的一階導(dǎo)數(shù)、二階導(dǎo)數(shù)分別為

(16)

(17)

(18)

(19)

由三次B樣條曲線上任一點(diǎn)的曲率不大于最小轉(zhuǎn)彎半徑的倒數(shù)，有約束

(20)

叉車在跟蹤三次B樣條路徑曲線時(shí)，等效轉(zhuǎn)向輪轉(zhuǎn)角為

(21)

當(dāng)待優(yōu)化的參數(shù)L1、L2過(guò)小時(shí)，在首末端點(diǎn)附近，路徑曲率變化較大，不利于跟蹤。經(jīng)過(guò)多次試驗(yàn)之后，選取經(jīng)驗(yàn)值L1、L2的最小值為1 500 mm，此約束條件下所規(guī)劃的路徑較適合叉車運(yùn)動(dòng)跟蹤。

2.4.2參數(shù)最優(yōu)化求解

假設(shè)路徑曲線在任意一點(diǎn)曲率為κ(u)。采用最優(yōu)化的方法來(lái)求解滿足多約束條件的曲線參數(shù)。本文的優(yōu)化目標(biāo)為

minκmax(Pstart,P1,P2,…,Pn,Pdestination)

(22)

通過(guò)L1、L2兩個(gè)變量可以確定控制點(diǎn)的位置，從而確定了路徑曲線上的所有離散序列點(diǎn){Pstart,P1,P2,…,Pn,Pdestination}，故該優(yōu)化目標(biāo)是優(yōu)化參數(shù)L1、L2，使得路徑的最大曲率最小，從而得到較為光滑的路徑。

非等式約束條件為

(23)

等式約束條件為

(24)

3 典型場(chǎng)景選取及仿真分析

GA函數(shù)是Matlab遺傳算法工具箱中用于求解多約束條件模型的優(yōu)化函數(shù)[25]。建立托盤拾取路徑規(guī)劃數(shù)學(xué)模型后，以式(22)曲率最小為優(yōu)化目標(biāo)，以L1、L2為待優(yōu)化的變量，以式(23)、(24)為約束條件，調(diào)用GA函數(shù)，可求得典型場(chǎng)景下托盤拾取路徑的B樣條曲線參數(shù)L1、L2。

3.1 托盤拾取典型場(chǎng)景選取及曲線參數(shù)求解

如圖5所示，假定貨物緩沖區(qū)邊框形成的坐標(biāo)系與全局坐標(biāo)系只存在平移關(guān)系、不存在旋轉(zhuǎn)關(guān)系，叉車通過(guò)全局路徑行駛至貨物緩沖區(qū)附近停下。由于B樣條曲線具有仿射不變的特性，即對(duì)控制點(diǎn)坐標(biāo)進(jìn)行旋轉(zhuǎn)平移都不會(huì)改變曲線的形狀，故仿真時(shí)假設(shè)路徑的全局起始坐標(biāo)點(diǎn)為(0 mm，0 mm，90°)。隨后叉車啟動(dòng)托盤位姿識(shí)別系統(tǒng)，獲得托盤中心實(shí)際位姿Pdestination(xd,yd,ψd)，并進(jìn)行托盤拾取路徑規(guī)劃。

圖5 典型托盤拾取場(chǎng)景示意圖Fig.5 Typical pallet picking scene

圖5中托盤孔中心點(diǎn)至貨叉底端中心點(diǎn)的縱向距離定義為拾取距離L3、托盤孔中心點(diǎn)與貨叉底端中心點(diǎn)的橫向距離定義為橫向偏移距離D、探測(cè)平面上托盤孔中心所在直線與緩沖區(qū)水平軸X軸夾角定義為偏移角θ，θ的正負(fù)表示偏移方向，沿著X軸逆時(shí)針?lè)较驗(yàn)檎?，順時(shí)針?lè)较驗(yàn)樨?fù)。

智能叉車系統(tǒng)實(shí)際工作中，拾取距離L3約為6 500 mm時(shí)，托盤位姿識(shí)別系統(tǒng)便能準(zhǔn)確獲得托盤孔中心的位姿。仿真時(shí)，將拾取距離L3作為固定參數(shù)，只考慮橫向偏移距離D和偏移角θ兩個(gè)參數(shù)。

仿真時(shí)所用相關(guān)參數(shù)見(jiàn)表1。根據(jù)橫向偏移距離和偏移角不同，選取橫向偏移距離分別為500、1 000、1 500 mm，偏移角分別為-5°、5°、-10°、10°、-15°、15°等18個(gè)典型的托盤擺放位姿場(chǎng)景，利用Matlab中GA函數(shù)求解各個(gè)場(chǎng)景下路徑曲線的優(yōu)化參數(shù)L1、L2，結(jié)果見(jiàn)表2。

表1 仿真相關(guān)參數(shù)Tab.1 Simulation parameters

3.2 托盤拾取路徑規(guī)劃仿真結(jié)果分析

選取表2中場(chǎng)景1、場(chǎng)景18兩個(gè)典型的仿真場(chǎng)景，仿真結(jié)果如圖6、7所示。

表2 不同典型場(chǎng)景下所得到的曲線參數(shù)L1、L2Tab.2 Curve parameters L1, L2 under different typical scenarios

圖6 托盤拾取場(chǎng)景1的仿真結(jié)果Fig.6 Simulation results of path planning in pallet picking scenario 1

由圖6a可知，叉車從起點(diǎn)出發(fā)沿著規(guī)劃路徑到達(dá)托盤目標(biāo)位置；由圖6b可知，在路徑起始點(diǎn)和目標(biāo)點(diǎn)處，等效轉(zhuǎn)向輪轉(zhuǎn)角為零，轉(zhuǎn)向輪最大轉(zhuǎn)角為-0.076 1 rad，小于αmax，符合規(guī)劃要求；由圖6c可知，叉車航向角從1.570 8 rad到1.483 5 rad平穩(wěn)變化，符合托盤拾取的位姿要求；由圖6d可知，路徑曲率連續(xù)，最大值為-4.68×10-5mm-1,不超過(guò)叉車的最大曲率κmax,滿足規(guī)劃要求。其中，曲率的正負(fù)僅表示路徑的凹凸性不同，等效轉(zhuǎn)向輪轉(zhuǎn)角正負(fù)僅表示轉(zhuǎn)向不同。

對(duì)比圖6，由圖7可知，在路徑起始點(diǎn)和目標(biāo)點(diǎn)處，等效轉(zhuǎn)向輪轉(zhuǎn)角為零，前進(jìn)過(guò)程中轉(zhuǎn)向輪最大轉(zhuǎn)角為0.525 5 rad，比圖6b中最大轉(zhuǎn)角大；由圖7c可知，叉車的航向角變化量符合端點(diǎn)約束及托盤拾取位姿要求，但航向角變化量比圖6c大；由圖7d可知，路徑曲率連續(xù)，最大不超過(guò)3.559 6×10-4mm-1，符合叉車的最大曲率κmax的要求。

圖7 托盤拾取場(chǎng)景18的仿真結(jié)果Fig.7 Simulation results of path planning in pallet picking scenario 18

故小橫向偏移距離、小偏移角場(chǎng)景(場(chǎng)景1)下，所規(guī)劃的路徑總體平緩，有利于跟蹤階段的有效跟蹤。而相同的起始位姿下，目標(biāo)托盤的橫向偏移距離、偏移角越大(場(chǎng)景18)，路徑的曲率越大，拾取難度越高。

4 試驗(yàn)

4.1 試驗(yàn)平臺(tái)

以改裝后的杭州叉車A30作為干果叉車試驗(yàn)平臺(tái)[1]，車輛總質(zhì)量4.85 t。叉車采用前輪驅(qū)動(dòng)、后輪轉(zhuǎn)向的結(jié)構(gòu)。車的長(zhǎng)、寬、高尺寸為2.502 m×1.265 m×2.100 m，貨叉長(zhǎng)度1.6 m，貨叉寬度15 cm。平臺(tái)及硬件組成如圖8所示。

圖8 托盤拾取試驗(yàn)場(chǎng)景Fig.8 Test scenario of pallet picking1.Tim310型激光避障傳感器 2.Beckhoff型車載控制器 3.定位反光標(biāo)貼 4.NAV350型激光定位傳感器 5.轉(zhuǎn)向步進(jìn)電機(jī) 6.電子加速踏板 7.Kinect2型RGB-D傳感器 8.Tim561型激光測(cè)距傳感器 9.目標(biāo)托盤

叉車通過(guò)Sick公司的NAV350型室內(nèi)激光定位傳感器掃描環(huán)境中的反光標(biāo)貼進(jìn)行全局定位，可得到全局坐標(biāo)系下的坐標(biāo)(x,y,ψ)。NAV350型傳感器的位姿角度測(cè)量范圍為0°～360°，測(cè)量頻率8 Hz，測(cè)量精度±0.15°，定位精度為±4 mm。

智能叉車?yán)肒inect2型RGB-D傳感器、Tim561型激光測(cè)距傳感器進(jìn)行托盤位姿估計(jì)[17]。在托盤位姿識(shí)別系統(tǒng)探測(cè)距離為6 500 mm、橫向偏移距離為1 500 mm、偏移角15°情況下，托盤位置估計(jì)精度為±60 mm，角度估計(jì)精度為±6°。

4.2 試驗(yàn)設(shè)計(jì)

在某倉(cāng)庫(kù)進(jìn)行托盤拾取路徑規(guī)劃與跟蹤試驗(yàn)(圖8)。選用的托盤尺寸為1 200 mm×1 200 mm×140 mm，托盤孔寬度為405 mm。

選取仿真場(chǎng)景18作為試驗(yàn)場(chǎng)景。試驗(yàn)時(shí)路徑起始點(diǎn)位姿為(505.353 mm，6 627.26 mm，87.802°)，最終點(diǎn)托盤孔中心位姿為(2 253.54 mm，13 065 mm，102.802°)。

路徑跟蹤采用純追蹤方法[26-27]，純追蹤前視距離為700 mm。由于托盤拾取過(guò)程中前進(jìn)距離短、路徑曲率較大，拾取精度要求較高，試驗(yàn)時(shí)，叉車的最大速度不超800 mm/s。

4.3 試驗(yàn)結(jié)果分析

通過(guò)Beckhoff型車載控制器采集試驗(yàn)數(shù)據(jù)，并導(dǎo)入到Matlab中，試驗(yàn)結(jié)果如圖9～11所示。

圖9 托盤拾取路徑規(guī)劃與路徑跟蹤曲線Fig.9 Path planning and path tracking curves of pallet picking process

圖10 路徑規(guī)劃與路徑跟蹤航向角曲線Fig.10 Path planning and path tracking heading angle curves of pallet picking process

圖11 路徑規(guī)劃等效轉(zhuǎn)向輪轉(zhuǎn)角與路徑跟蹤轉(zhuǎn)角曲線Fig.11 Path planning equivalent steering wheel angle curve and path tracking angle curve of pallet picking process

由圖9可知，規(guī)劃的拾取路徑整體可較好地被跟蹤，兩條曲線的重合度較高。起始點(diǎn)附近由于轉(zhuǎn)角較大，出現(xiàn)跟蹤偏差，之后誤差收斂，最終目標(biāo)點(diǎn)的橫向偏移距離誤差為4.71 cm，滿足托盤拾取的精度要求。

由圖10可知，兩條航向角曲線的起始點(diǎn)和終點(diǎn)基本重合，首末端點(diǎn)航向滿足位姿約束條件。路徑起始點(diǎn)附近，由于航向變化較大，出現(xiàn)跟蹤偏差，之后誤差收斂。路徑終點(diǎn)航向角誤差為9.6×10-3rad，滿足托盤拾取精度要求。

由圖11可知，叉車行進(jìn)拾取過(guò)程中，由于初始位置附近轉(zhuǎn)向角變化較大，出現(xiàn)偏差，造成起始點(diǎn)附近的跟蹤偏差(圖9)，但之后誤差收斂。整個(gè)過(guò)程中轉(zhuǎn)向輪轉(zhuǎn)角最大為-0.592 rad，不超過(guò)叉車最大轉(zhuǎn)向輪轉(zhuǎn)角1.047 1 rad，滿足叉車的運(yùn)動(dòng)參數(shù)要求。

針對(duì)大橫向偏移距離和大偏移角度的拾取場(chǎng)景，拾取難度較大，但路徑跟蹤的最終點(diǎn)誤差基本能滿足托盤拾取的要求。經(jīng)過(guò)試驗(yàn)，其他拾取場(chǎng)景下，托盤拾取路徑規(guī)劃算法所規(guī)劃的路徑可達(dá)到智能叉車托盤拾取的要求。

5 結(jié)論

(1)針對(duì)干果倉(cāng)儲(chǔ)中智能叉車托盤自主拾取過(guò)程，提出了一種基于三次均勻B樣條曲線的托盤拾取路徑規(guī)劃方法。綜合考慮叉車最小轉(zhuǎn)彎半徑、首末端點(diǎn)約束、叉車自身參數(shù)等約束，建立托盤拾取多約束條件、曲率最小優(yōu)化目標(biāo)的數(shù)學(xué)模型。

(2)根據(jù)托盤初始擺放位姿不同，設(shè)計(jì)了18種典型的托盤拾取場(chǎng)景。通過(guò)Matlab優(yōu)化工具箱GA函數(shù)對(duì)優(yōu)化變量進(jìn)行求解。

(3)托盤拾取試驗(yàn)結(jié)果表明，智能叉車對(duì)托盤拾取規(guī)劃算法生成的路徑可較好地進(jìn)行跟蹤，終點(diǎn)橫向偏移誤差4.71 cm，終點(diǎn)航向角誤差為9.6×10-3rad，基本滿足托盤拾取的精度要求。