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

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

(1.華南農業大學工程學院， 廣州 510642； 2.華南農業大學南方農業機械與裝備關鍵技術教育部重點實驗室， 廣州 510642；3.華南農業大學工程基礎教學與訓練中心， 廣州 510642)

0 引言

干果品類多、價值較高，倉儲運輸中多采用托盤碼放的方式，利用智能叉車進行運輸作業，能提高倉儲效率[1]。然而貨物托盤擺放位姿存在不確定性，應用路徑規劃技術可實現智能叉車托盤自主拾取，從而提高叉車作業的自主性和靈活性。

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

本文以托盤拾取路徑規劃過程中首末端點約束、叉車參數約束，以及路徑曲率最小為目標，建立多約束條件下的B樣條路徑曲線，并利用Matlab對不同托盤拾取場景進行參數求解，最后通過試驗驗證規劃算法的可行性。

1 托盤拾取

1.1 作業過程

研究的作業環境為半結構化倉儲環境。倉庫中設置貨物緩沖區，貨物托盤在緩沖區中的具體擺放位姿具有不確定性，采用傳感感知系統，進行托盤位姿識別[16-17]，通過規劃的路徑調整叉車位姿，完成托盤拾取作業任務。

整個托盤拾取的作業過程如圖1所示。叉車通過車載激光定位傳感器獲得倉庫全局坐標下的定位信息，在距離貨物緩沖區一定距離時，啟動托盤位姿識別算法，通過貨叉前端的RGB-D傳感器以及激光測距傳感器，獲得托盤孔中心在全局參考坐標系中的坐標。其中，RGB-D傳感器獲取的圖像具有較為豐富的紋理和色彩信息，但其深度探測距離有限，主要用于近距離的托盤位姿識別；激光傳感器具有穩定、抗干擾能力強的特點，主要用于大偏移角度、遠距離的托盤位姿識別。兩者結合，可提高托盤位姿識別的準確性。

托盤位姿識別成功之后，進行路徑規劃，智能叉車按照規劃的曲線，通過純追蹤算法進行路徑跟蹤，最終完成托盤拾取的任務。圖1虛線框為實現智能叉車托盤拾取的重要環節[18]。

1.2 路徑規劃問題

基于托盤位姿識別系統已經準確識別出托盤孔中心的全局坐標，并且通過激光定位傳感器已獲得叉車在倉庫中實時位姿為前提，研究托盤拾取路徑規劃問題。根據規劃的方向，可分為前進式和進退式路徑規劃，本文研究前進式托盤拾取路徑規劃。

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

2 托盤拾取路徑規劃算法

托盤拾取路徑規劃的目的是：規劃符合叉車運動學、叉車自身參數、曲率連續約束的可行路徑，使得智能叉車對托盤進行自主拾取。托盤拾取路徑規劃算法基于三次均勻B樣條曲線，根據托盤拾取的相關約束條件，建立多約束方程以及曲率最小化的目標函數，通過求解曲線的相關參數，從而得到拾取路徑。

2.1 叉車運動學模型建立

由于倉庫作業過程中叉車的運動速度比較低，在低速行駛時所受側向力較小，假設在行駛過程中不發生側滑現象，所以路徑規劃時只考慮叉車的運動學模型。

假設叉車車體為剛性，車輪形變可忽略不計，且輪子和地面的每個接觸點僅產生純滾動，不發生滑動，叉車的運動學模型如圖3所示[19]。

圖3 叉車運動學模型Fig.3 Kinematics model of forklift1、2.前輪驅動輪 3、4.后輪轉向輪 5.等效驅動輪 6.等效轉向輪

圖3中全局坐標系為OXY，叉車坐標系為OXvYv；ICR為叉車瞬時旋轉中心。

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

(1)

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

由文獻[20]可知，路徑曲率κ和瞬時旋轉半徑r、前后輪的軸距L以及等效轉向輪轉角α的關系為

(2)

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

α=arctan(Lκ)

(3)

2.2 托盤拾取路徑曲線約束

2.2.1端點狀態約束

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

2.2.2叉車參數約束

路徑規劃過程中曲率過大或者曲率不連續，將導致叉車無法對所規劃的路徑進行準確地跟蹤。而拾取過程中最終點的跟蹤誤差一旦過大可能導致托盤拾取失敗。故托盤拾取路徑規劃時需考慮叉車的自身參數約束。

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

(2)曲率連續約束。為避免路徑跟蹤過程中出現原地轉向的現象，對輪胎造成過度磨損，故路徑應保證曲率連續。因為采用三次B樣條曲線，所以滿足C2連續的性質[21-22]。

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

2.3 三次B樣條曲線

根據文獻[23]，三次均勻B樣條曲線表達式為

(4)

式中u——參數，取值為[0,1]

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

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

(5)

三次B樣條曲線的一階導數、二階導數為

(6)

(7)

(8)

(9)

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

V——控制點坐標值矩陣

V1,i,x、V2，i,x、V3,i,x、V4,i,x——第i段B樣條曲線4個控制點橫坐標值

V1,i,y、V2,i,y,、V3,i,y、V4,i,y——第i段B樣條曲線4個控制點縱坐標值

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

圖4 B樣條曲線中控制點與曲線的關系Fig.4 Relationship between control points and B-spline curve

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

由于B樣條曲線形狀完全由控制點的位置確定，對托盤拾取路徑參數化的過程就是求出控制點的過程。參照文獻[24]，已知路徑起始點、托盤孔中心目標點坐標的條件下，設圖4中P0P1和P2P3的長度分別為2L1、2L2，通過求解多約束條件下參數L1、L2，可得到最優的托盤拾取路徑。

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

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

(10)

(11)

(12)

(13)

結合文獻[23]所述的B樣條曲線的性質以及上述路徑規劃的約束條件，可以得到三次B樣條路徑曲線的首末端點位置為

(14)

(15)

三次B樣條曲線首末端點處的一階導數、二階導數分別為

(16)

(17)

(18)

(19)

由三次B樣條曲線上任一點的曲率不大于最小轉彎半徑的倒數，有約束

(20)

叉車在跟蹤三次B樣條路徑曲線時，等效轉向輪轉角為

(21)

當待優化的參數L1、L2過小時，在首末端點附近，路徑曲率變化較大，不利于跟蹤。經過多次試驗之后，選取經驗值L1、L2的最小值為1 500 mm，此約束條件下所規劃的路徑較適合叉車運動跟蹤。

2.4.2參數最優化求解

假設路徑曲線在任意一點曲率為κ(u)。采用最優化的方法來求解滿足多約束條件的曲線參數。本文的優化目標為

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

(22)

通過L1、L2兩個變量可以確定控制點的位置，從而確定了路徑曲線上的所有離散序列點{Pstart,P1,P2,…,Pn,Pdestination}，故該優化目標是優化參數L1、L2，使得路徑的最大曲率最小，從而得到較為光滑的路徑。

非等式約束條件為

(23)

等式約束條件為

(24)

3 典型場景選取及仿真分析

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

3.1 托盤拾取典型場景選取及曲線參數求解

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

圖5 典型托盤拾取場景示意圖Fig.5 Typical pallet picking scene

圖5中托盤孔中心點至貨叉底端中心點的縱向距離定義為拾取距離L3、托盤孔中心點與貨叉底端中心點的橫向距離定義為橫向偏移距離D、探測平面上托盤孔中心所在直線與緩沖區水平軸X軸夾角定義為偏移角θ，θ的正負表示偏移方向，沿著X軸逆時針方向為正，順時針方向為負。

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

仿真時所用相關參數見表1。根據橫向偏移距離和偏移角不同，選取橫向偏移距離分別為500、1 000、1 500 mm，偏移角分別為-5°、5°、-10°、10°、-15°、15°等18個典型的托盤擺放位姿場景，利用Matlab中GA函數求解各個場景下路徑曲線的優化參數L1、L2，結果見表2。

表1 仿真相關參數Tab.1 Simulation parameters

3.2 托盤拾取路徑規劃仿真結果分析

選取表2中場景1、場景18兩個典型的仿真場景，仿真結果如圖6、7所示。

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

圖6 托盤拾取場景1的仿真結果Fig.6 Simulation results of path planning in pallet picking scenario 1

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

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

圖7 托盤拾取場景18的仿真結果Fig.7 Simulation results of path planning in pallet picking scenario 18

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

4 試驗

4.1 試驗平臺

以改裝后的杭州叉車A30作為干果叉車試驗平臺[1]，車輛總質量4.85 t。叉車采用前輪驅動、后輪轉向的結構。車的長、寬、高尺寸為2.502 m×1.265 m×2.100 m，貨叉長度1.6 m，貨叉寬度15 cm。平臺及硬件組成如圖8所示。

圖8 托盤拾取試驗場景Fig.8 Test scenario of pallet picking1.Tim310型激光避障傳感器 2.Beckhoff型車載控制器 3.定位反光標貼 4.NAV350型激光定位傳感器 5.轉向步進電機 6.電子加速踏板 7.Kinect2型RGB-D傳感器 8.Tim561型激光測距傳感器 9.目標托盤

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

智能叉車利用Kinect2型RGB-D傳感器、Tim561型激光測距傳感器進行托盤位姿估計[17]。在托盤位姿識別系統探測距離為6 500 mm、橫向偏移距離為1 500 mm、偏移角15°情況下，托盤位置估計精度為±60 mm，角度估計精度為±6°。

4.2 試驗設計

在某倉庫進行托盤拾取路徑規劃與跟蹤試驗(圖8)。選用的托盤尺寸為1 200 mm×1 200 mm×140 mm，托盤孔寬度為405 mm。

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

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

4.3 試驗結果分析

通過Beckhoff型車載控制器采集試驗數據，并導入到Matlab中，試驗結果如圖9～11所示。

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

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

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

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

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

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

針對大橫向偏移距離和大偏移角度的拾取場景，拾取難度較大，但路徑跟蹤的最終點誤差基本能滿足托盤拾取的要求。經過試驗，其他拾取場景下，托盤拾取路徑規劃算法所規劃的路徑可達到智能叉車托盤拾取的要求。

5 結論

(1)針對干果倉儲中智能叉車托盤自主拾取過程，提出了一種基于三次均勻B樣條曲線的托盤拾取路徑規劃方法。綜合考慮叉車最小轉彎半徑、首末端點約束、叉車自身參數等約束，建立托盤拾取多約束條件、曲率最小優化目標的數學模型。

(2)根據托盤初始擺放位姿不同，設計了18種典型的托盤拾取場景。通過Matlab優化工具箱GA函數對優化變量進行求解。

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