基于動態(tài)識別區(qū)和B樣條曲線的智能叉車避障路徑規(guī)劃

呂恩利 阮清松 劉妍華 王飛仁 林偉加 董 冰

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

0 引言

干果品類多、價值高，在倉儲中多采用貨架擺放等堆碼方式存放，利用智能叉車存取干果，可有效提高倉儲效率，促進(jìn)干果倉儲管理規(guī)范化、智能化。然而，智能叉車在跟蹤全局路徑時，會遇到環(huán)境發(fā)生改變的情況，若出現(xiàn)新的障礙物，此時的全局路徑將暫時性失去作用，需要借助傳感器的實時信息進(jìn)行局部避障路徑規(guī)劃，使叉車?yán)@開新出現(xiàn)的障礙物后能夠回到原全局路徑上繼續(xù)執(zhí)行任務(wù)。

為解決避障路徑規(guī)劃相關(guān)問題，部分學(xué)者將參數(shù)化曲線應(yīng)用于路徑規(guī)劃[1-11]。然而，以往學(xué)者對智能倉儲物流的研究主要集中在高自由度的小型車輛，對大噸位的低自由度叉車研究較少[12-17]。同時，當(dāng)考慮到車輛起止點位姿約束、避障約束、曲率約束等多項約束條件時，需確保所規(guī)劃的路徑可跟蹤，而以往方法所規(guī)劃的路徑多存在不足[18-20]。另外，當(dāng)考慮起止點位姿約束，以往學(xué)者采用的均勻B樣條曲線相對準(zhǔn)均勻B樣條曲線需要在起止點處增設(shè)控制點，且多是基于型值點并利用反求控制點的方式求取控制點，增加了計算量。

綜上所述，考慮到準(zhǔn)均勻B樣條曲線的優(yōu)越性，本文將對適用于倉儲環(huán)境中大型叉車的避障路徑規(guī)劃方法進(jìn)行研究，并以干果倉儲智能叉車的倒車為例，實現(xiàn)干果倉儲智能叉車的避障路徑規(guī)劃。

1 基本概念與模型

1.1 動態(tài)識別區(qū)

為避免不必要的計算負(fù)擔(dān)，需考慮到避障路徑規(guī)劃算法的觸發(fā)機制，故設(shè)立傳感器實時識別區(qū)，即當(dāng)設(shè)定的傳感器識別區(qū)中出現(xiàn)障礙物時，觸發(fā)路徑規(guī)劃算法；當(dāng)沒有障礙物時，僅保持判斷識別區(qū)中是否出現(xiàn)障礙物的部分程序?qū)崟r運行，而路徑規(guī)劃程序不運行，由此減輕系統(tǒng)運行負(fù)擔(dān)。

本研究對象是倉儲環(huán)境下的大型堆垛叉車，體形相對較大，受最小轉(zhuǎn)彎半徑約束[21]，需要對探測范圍內(nèi)的障礙物做出一定的判斷——動態(tài)識別區(qū)，以避免在非直行路段誤觸發(fā)避障路徑規(guī)劃程序，這也有利于空間資源的有效利用。其中，動態(tài)識別區(qū)在直行路段的范圍如圖1a所示，在轉(zhuǎn)彎路段的范圍如圖1b所示。

圖1 動態(tài)識別區(qū)范圍Fig.1 Ranges of dynamic identification zone

圖1中，坐標(biāo)系XOY為倉庫全局笛卡爾坐標(biāo)系，坐標(biāo)系xSy為車載導(dǎo)航傳感器的笛卡爾坐標(biāo)系；S為導(dǎo)航傳感器在叉車上的投影位置，其全局笛卡爾坐標(biāo)為(X,Y,θ)，θ為傳感器坐標(biāo)系x軸正向與全局笛卡爾坐標(biāo)系X軸正向的夾角，S1為叉車尾部的測距傳感器位置，其全局笛卡爾坐標(biāo)為(X1,Y1,θ)；A、B為整車尾部最大寬度處的兩端點，C、D為整車最大寬度處的兩端點(即前軸上最外層點)，P1、P2分別為前、后軸中心點，U為叉車瞬時旋轉(zhuǎn)中心所在位置，其全局笛卡爾坐標(biāo)為(Xu,Yu)；K為整車尾部最大寬度，L為叉車前后軸距，L1為導(dǎo)航傳感器距前軸距離，L2為導(dǎo)航傳感器距后軸距離，L3為叉車后懸距，d為導(dǎo)航傳感器距叉車中心軸的偏移距離；β為轉(zhuǎn)向軸中心點處轉(zhuǎn)向輪的等效轉(zhuǎn)向角，即Ackermann轉(zhuǎn)向角，轉(zhuǎn)向輪回正時β=0°，本文規(guī)定，所有逆時針旋轉(zhuǎn)的角為正值，順時針旋轉(zhuǎn)的角為負(fù)值；Q1、Q2、Q3代表環(huán)境中突然出現(xiàn)的障礙物，lWIDTH為識別區(qū)寬度，lLENTH為識別區(qū)長度，紅色粗虛線代表全局路徑。易知

lP1P2=L1+L2=L

(1)

式中l(wèi)P1P2——前、后軸中心點距離

驅(qū)動軸中點瞬時轉(zhuǎn)彎半徑為

(2)

叉車最外側(cè)A點瞬時轉(zhuǎn)彎半徑為

(3)

叉車瞬時轉(zhuǎn)軸中心點所在位置的全局笛卡爾坐標(biāo)值為

(4)

(5)

測距傳感器瞬時全局笛卡爾坐標(biāo)值為

X1=X-(L2+lS1P2)cosθ-dsinθ

(6)

Y1=Y-(L2+lS1P2)sinθ+dcosθ

(7)

式中l(wèi)S1P2——測距傳感器到后軸中心點距離

當(dāng)叉車運行于直行路段時，如圖1a所示，識別區(qū)為矩形，則

lLENTH=lLENTHs

(8)

(9)

式中l(wèi)LENTHs——識別區(qū)初始長度

lWIDTHs——識別區(qū)初始寬度

lWIDTHmax——識別區(qū)最大寬度

V——車速Vmax——最大車速

當(dāng)叉車運行于非直行路段時，如圖1b所示，識別區(qū)為不規(guī)則多邊形，則

(10)

(11)

式中l(wèi)LENTHmax——識別區(qū)最大長度

且識別區(qū)內(nèi)各點到瞬時轉(zhuǎn)軸U的距離小于lUA，而考慮到內(nèi)輪差，需同時大于lUC(叉車內(nèi)側(cè)C點瞬時轉(zhuǎn)彎半徑)，這樣可避免因Q2、Q3類的無關(guān)障礙物誤觸發(fā)避障路徑規(guī)劃程序。

其中，出于生產(chǎn)安全考慮，給干果倉儲智能叉車設(shè)定最大行駛速度Vmax，而V代表叉車實時車速，同時，規(guī)定倒車時車速V為負(fù)值，前進(jìn)時車速V為正值。

1.2 B樣條曲線

B樣條曲線在保留Bezier曲線優(yōu)點的同時進(jìn)行了改進(jìn)，雖然仍采用了多邊形及權(quán)函數(shù)來定義曲線，但是B樣條曲線舍棄了Bezier曲線的Bernstein函數(shù)，設(shè)計了B樣條曲線基函數(shù)，n次B樣條曲線基函數(shù)[22]為

(12)

(13)

可得第i段n次B樣條曲線的表達(dá)式為

(14)

其中

u∈[0,1]

式中n——樣條曲線的階次

m——控制點個數(shù)，整條曲線由m-n段B樣條曲線平滑連接而成

Gi+k——第i+k個控制點的坐標(biāo)值

由此可知，n次B樣條曲線是由n+1個控制點根據(jù)B樣條基函數(shù)加權(quán)生成；n+m個控制點能夠生成一條由m段B樣條曲線平滑連接構(gòu)成的經(jīng)過n次平滑的曲線；由多個控制點產(chǎn)生的多段平滑連接的B樣條曲線可通過調(diào)整局部控制點的坐標(biāo)來改變曲線的局部形狀。

然而，以往學(xué)者采用的均勻B樣條曲線缺陷是沒有保留Bezier曲線的端點幾何性質(zhì)，即樣條曲線的首末端點處與控制多邊形不相切。故而，為滿足起止點位姿約束，本文采用易實現(xiàn)多階平滑和局部形狀修改且保留Bezier曲線的端點幾何性質(zhì)的準(zhǔn)均勻B樣條曲線實現(xiàn)路徑規(guī)劃，其不同在于n次準(zhǔn)均勻B樣條曲線的節(jié)點矢量中兩端節(jié)點具有重復(fù)度n+1，所有內(nèi)節(jié)點呈均勻分布。

在B樣條曲線的形成方法上，部分學(xué)者以已知的一些點為型值點，為使生成的B樣條曲線能通過這些點，通過反求控制點的方式求出過型值點的B樣條曲線所對應(yīng)的控制點，再以求得的控制點生成參數(shù)化的B樣條曲線路徑[23]。本文鑒于B樣條曲線的強凸包性，將依托障礙物選出的特征控制點為分割點，分段生成B樣條曲線，并利用其性質(zhì)，最后合并生成過某些型值點的B樣條曲線路徑，保證整條曲線連續(xù)性的同時省去了反求控制點的步驟。

另外，本文曲率和轉(zhuǎn)向輪角速度的求取方式參照文獻(xiàn)[11]，計算公式為

(15)

(16)

式中X′、Y′——橫、縱坐標(biāo)一階導(dǎo)數(shù)

X″、Y″——橫、縱坐標(biāo)二階導(dǎo)數(shù)

2 避障路徑規(guī)劃

假設(shè)叉車滿足非完整性約束，在倉庫中以中低速行駛，且過程中不發(fā)生打滑現(xiàn)象，同時算法考慮叉車避障約束、最小轉(zhuǎn)彎半徑約束、曲率連續(xù)約束、最大轉(zhuǎn)向角及最大轉(zhuǎn)向輪角速度約束、起止點位姿約束等。

2.1 算法流程

算法流程如圖2所示，具體步驟為：

圖2 算法流程圖Fig.2 Algorithm flow chart

(1)通過讀取測距傳感器數(shù)據(jù)，判斷動態(tài)識別區(qū)中是否出現(xiàn)障礙物，若沒有則持續(xù)探測。

(2)當(dāng)發(fā)現(xiàn)障礙物時，提取障礙物所有特征角點，選出特征控制點，若沒有滿足要求的控制點，則說明前方已無法通行，叉車執(zhí)行急停并發(fā)出警報，待障礙解除繼續(xù)運行。

(3)當(dāng)存在滿足要求的特征控制點時，以其為基礎(chǔ)，結(jié)合叉車當(dāng)前位姿信息及準(zhǔn)均勻B樣條曲線性質(zhì)和約束條件，求取剩余所需控制點，并聯(lián)合生成B樣條曲線路徑。

(4)將所有路徑點傳輸給路徑跟蹤程序，并保持實時探測狀態(tài)。

2.2 控制點選取策略

叉車行駛于已知的全局路徑通道中，遇到突然出現(xiàn)的障礙物，則觸發(fā)避障路徑規(guī)劃。

(1)考慮起始位姿約束，以叉車當(dāng)前驅(qū)動軸中點為第1個控制點；以Cr為比例調(diào)節(jié)因子，在當(dāng)前位姿方向上，在距離第1個控制點Cr個車寬處選一點為第3個控制點，第2個控制點取第1個與第3個控制點的中點。

(2)考慮避障約束，針對動態(tài)識別區(qū)中的避障傳感器數(shù)據(jù)，提取障礙物邊界直線端點[24-26]，將各直線端點按順序組成特征角點點集，并記錄其在傳感器數(shù)據(jù)集中的序號，再選出序號最大和最小的兩點，兩點中以偏向車體正前方的特征角點優(yōu)先，以該特征角點為圓心，車寬的Cr1倍為半徑作圓，并過測距傳感器位置點作該圓的切線，得兩個切點；考慮到避障約束，分別以兩個切點為圓心，取微大于半個車寬的距離為半徑作圓，判斷圓內(nèi)是否有傳感器數(shù)據(jù)點，若兩個圓內(nèi)都有，轉(zhuǎn)而考慮剩余的另一個特征角點，若還是兩個圓內(nèi)都有，則說明通道前方受阻無法通行；否則，選出遠(yuǎn)離障礙物的切點為第5個控制點。

(3)考慮終止位姿約束，再選出全局路徑上距離第2步中被選出的特征角點最近的點，以之為圓心，取TL(TL≥lUA)為半徑，搜尋全局路徑上與該圓相交或在圓內(nèi)且序號大于該點在全局路徑點集中序號的點為目標(biāo)點，即第9個控制點；并在全局路徑上選出序號小于目標(biāo)點序號且與之相距車寬Cr倍距離的點為第7個控制點，第8個控制點取第7個控制點與第9個控制點的中點。

(4)兼顧最小轉(zhuǎn)彎半徑約束、最大轉(zhuǎn)向角及最大轉(zhuǎn)向輪角速度約束，過特征控制點作平行于第3個控制點與第7個控制點連線的直線，求得第3個控制點在該直線上的垂足點，以該點與特征控制點的中點為第4個控制點；而第6個控制點求法相同。

3 試驗與結(jié)果分析

3.1 試驗設(shè)計

3.1.1試驗平臺

試驗以改裝的電動A30型杭州叉車作為干果倉儲智能叉車試驗平臺，如圖3所示。

圖3 干果倉儲智能叉車Fig.3 Intelligent forklift truck in dried fruit warehouse1.導(dǎo)航傳感器 2.測距傳感器

3.1.2主要傳感器及輔助器具

主要傳感器的安裝位置如圖3所示，投影示意圖如圖1所示。

導(dǎo)航傳感器選用SICK-NAV350型導(dǎo)航激光掃描傳感器，測量范圍0.5～70 m，掃描角度為360°，測量頻率8 Hz，當(dāng)反光標(biāo)貼在距導(dǎo)航傳感器30 m內(nèi)時，測距精度為15 mm，測角精度為±0.15°。通過掃描布置在環(huán)境中的反光標(biāo)貼建立笛卡爾全局坐標(biāo)系XOY，之后利用其獲取叉車在全局坐標(biāo)系中的當(dāng)前位置信息，即(X,Y,θ)。

圖4 試驗現(xiàn)場Fig.4 Test scene

測距傳感器選用SICK-LMS111型測距激光掃描傳感器，測量范圍0.5～20 m，掃描角度為270°，因傳感器安裝于叉車尾部中間位置，兩側(cè)部分角度激光被車身阻擋，因此，試驗中只取0°～180°范圍內(nèi)的有效數(shù)據(jù)，測量頻率25 Hz，角度分辨率0.25°，主要用于測量車體到障礙物的距離及所在方位，數(shù)據(jù)點共721個。

為保證試驗結(jié)果的可靠性，對叉車主要結(jié)構(gòu)及各主要部件相對位置進(jìn)行了測量，角度尺精度為±0.30°，手持激光測距儀精度為±1 mm，12線3D激光水平儀精度為1.43×10-4，所用輔助測量工具還包括鉛垂線等。經(jīng)測量所得參數(shù)如表1所示。

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

3.1.3試驗場地

試驗在某公司的干果倉庫中進(jìn)行，倉庫中堆放大量箱裝干果；以50 cm×51 cm×70 cm紙箱為潛在障礙物，試驗現(xiàn)場如圖4a所示，倉庫布局平面示意圖如圖4b所示，其中，坐標(biāo)系XOY為導(dǎo)航傳感器所建全局坐標(biāo)系，試驗中Cr1=0.53，Cr=1.21。

3.2 結(jié)果分析

倉庫多為結(jié)構(gòu)化環(huán)境，地面平坦，全局路徑由直線和圓弧組成，即由直行路段和轉(zhuǎn)彎路段組成，因而障礙物突現(xiàn)情形也分兩種，即直行路段遇到障礙物和轉(zhuǎn)彎路段遇到障礙物，若解決以上兩種情形的規(guī)劃問題，智能叉車便可以順利繞過障礙物繼續(xù)工作；如若倉庫通道中出現(xiàn)大量或過大的障礙物，可供通過的通道過窄，則停車并報警。

當(dāng)叉車在初始位置時，距障礙物較遠(yuǎn)，不影響正常工作，不觸發(fā)避障路徑規(guī)劃，此刻導(dǎo)航傳感器的位置點坐標(biāo)為(958 mm，1 368 mm，271.99°)，測距傳感器掃描結(jié)果如圖5所示。

圖5 初始位置處測距傳感器掃描結(jié)果Fig.5 Results of ranging-sensor scan at beginning

3.2.1直行路段避障路徑規(guī)劃

叉車直行時，等效轉(zhuǎn)向角β=0°；當(dāng)叉車離障礙物越來越近，車速V=-681 mm/s時，識別區(qū)lLENTH=1 800 mm，lWIDTH=3 906 mm，當(dāng)導(dǎo)航傳感器的位置點坐標(biāo)為(1 048 mm, 8 616 mm,268.56°)時，探測到前方障礙物，觸發(fā)避障路徑規(guī)劃，產(chǎn)生的路徑控制點見表2，路徑規(guī)劃結(jié)果如圖6所示，路徑曲率變化如圖7a所示，叉車等效轉(zhuǎn)向輪角速度變化如圖7b所示。

表2 路徑控制點Tab.2 Control vertices of path mm

圖6 直行路段路徑規(guī)劃Fig.6 Path planning on straight road section

圖7 直行路段規(guī)劃結(jié)果分析Fig.7 Analysis of planning result on straight road section

由圖6可見，所得路徑滿足起止位姿約束；圖7中曲線端點橫坐標(biāo)約1 000 mm的點為變化曲線起點，曲線端點橫坐標(biāo)約950 mm的點為變化曲線終點，由圖7a可知，路徑曲率最小值約為-2.76×10-4mm-1，最大值約為2.31×10-4mm-1，均滿足最小轉(zhuǎn)彎半徑約束、曲率連續(xù)約束；由圖7b可知，等效轉(zhuǎn)向輪角速度最小值約為-0.34 rad/s，最大值約為0.37 rad/s，均滿足最大轉(zhuǎn)向輪角速度約束；其中，路徑曲率的正負(fù)僅表示路徑的凹凸性，等效轉(zhuǎn)向輪角速度正負(fù)僅表示左右轉(zhuǎn)向。

3.2.2轉(zhuǎn)彎路段避障路徑規(guī)劃

叉車行駛至轉(zhuǎn)彎路段某時刻時，等效轉(zhuǎn)向角β=-4.58°，車速V=-479 mm/s，識別區(qū)lLENTH=1 896 mm，lWIDTH=3 529 mm，Xu=21 378 mm，Yu=7 590 mm，lUA=21 174 mm，lUC=19 435 mm，導(dǎo)航傳感器的位置點坐標(biāo)為(1 048 mm，9 006 mm，268.76°)，此刻探測到前方存在障礙物，觸發(fā)避障路徑規(guī)劃，產(chǎn)生的路徑控制點見表3，路徑曲率變化如圖8a所示，叉車等效轉(zhuǎn)向輪角速度變化如圖8b所示，路徑規(guī)劃結(jié)果如圖9所示。

表3 路徑控制點Tab.3 Control vertices of path mm

圖8 轉(zhuǎn)彎路段規(guī)劃結(jié)果分析Fig.8 Analysis of planning result on turning road section

由圖9可見，所得路徑滿足起止位姿約束；由圖8a可知，曲率最小值約為3.41×10-6mm-1，最大值約為5.55×10-4mm-1，均滿足最小轉(zhuǎn)彎半徑約束、曲率連續(xù)約束；由圖8b可知，等效轉(zhuǎn)向輪角速度最小值約為-0.27 rad/s，最大值約為0.39 rad/s，均滿足最大轉(zhuǎn)向輪角速度約束；其中，曲率的正負(fù)僅表示路徑的凹凸性，等效轉(zhuǎn)向輪角速度正負(fù)僅表示左右轉(zhuǎn)向。

對獲得的路徑還需注意，由于車載測距傳感器只能檢測到物體的前面，因此在某一時刻或某一角度均無法獲得障礙物的具體尺寸及形狀，所得路徑若是經(jīng)過障礙物后側(cè)，未必滿足避障約束。

圖9 轉(zhuǎn)彎路段路徑規(guī)劃Fig.9 Path planning on turning road section

因此，本研究采取以特征控制點為分割點將路徑分為前后兩段，以前段為主，后段為輔，借助避障路徑規(guī)劃的實時性，在確保規(guī)劃所得路徑曲率連續(xù)的同時，有效解決這一潛在碰撞隱患。

4 結(jié)束語

受倉庫通道寬度限制，為降低干果倉儲智能叉車自主避障誤警率，提高倉儲空間有效利用率，選用激光導(dǎo)航傳感器和測距傳感器檢測環(huán)境，并基于動態(tài)識別區(qū)獲得的障礙物信息，以選出的特征控制點為分割點，采用四次五階準(zhǔn)均勻B樣條曲線，分段生成B樣條曲線，再依據(jù)B樣條曲線性質(zhì)，最后合并生成過某些型值點的B樣條路徑，保證整條曲線連續(xù)性的同時省去了反求控制點的麻煩，獲得了滿足叉車避障約束、最小轉(zhuǎn)彎半徑約束、曲率連續(xù)約束、最大轉(zhuǎn)向角及最大轉(zhuǎn)向輪角速度約束、起止點位姿約束的干果倉儲智能叉車避障路徑。直行路段和轉(zhuǎn)彎路段的試驗驗證了算法的可行性。