基于動態識別區和B樣條曲線的智能叉車避障路徑規劃

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

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

0 引言

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

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

綜上所述，考慮到準均勻B樣條曲線的優越性，本文將對適用于倉儲環境中大型叉車的避障路徑規劃方法進行研究，并以干果倉儲智能叉車的倒車為例，實現干果倉儲智能叉車的避障路徑規劃。

1 基本概念與模型

1.1 動態識別區

為避免不必要的計算負擔，需考慮到避障路徑規劃算法的觸發機制，故設立傳感器實時識別區，即當設定的傳感器識別區中出現障礙物時，觸發路徑規劃算法；當沒有障礙物時，僅保持判斷識別區中是否出現障礙物的部分程序實時運行，而路徑規劃程序不運行，由此減輕系統運行負擔。

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

圖1 動態識別區范圍Fig.1 Ranges of dynamic identification zone

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

lP1P2=L1+L2=L

(1)

式中lP1P2——前、后軸中心點距離

驅動軸中點瞬時轉彎半徑為

(2)

叉車最外側A點瞬時轉彎半徑為

(3)

叉車瞬時轉軸中心點所在位置的全局笛卡爾坐標值為

(4)

(5)

測距傳感器瞬時全局笛卡爾坐標值為

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

(6)

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

(7)

式中lS1P2——測距傳感器到后軸中心點距離

當叉車運行于直行路段時，如圖1a所示，識別區為矩形，則

lLENTH=lLENTHs

(8)

(9)

式中lLENTHs——識別區初始長度

lWIDTHs——識別區初始寬度

lWIDTHmax——識別區最大寬度

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

當叉車運行于非直行路段時，如圖1b所示，識別區為不規則多邊形，則

(10)

(11)

式中lLENTHmax——識別區最大長度

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

其中，出于生產安全考慮，給干果倉儲智能叉車設定最大行駛速度Vmax，而V代表叉車實時車速，同時，規定倒車時車速V為負值，前進時車速V為正值。

1.2 B樣條曲線

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

(12)

(13)

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

(14)

其中

u∈[0,1]

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

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

Gi+k——第i+k個控制點的坐標值

由此可知，n次B樣條曲線是由n+1個控制點根據B樣條基函數加權生成；n+m個控制點能夠生成一條由m段B樣條曲線平滑連接構成的經過n次平滑的曲線；由多個控制點產生的多段平滑連接的B樣條曲線可通過調整局部控制點的坐標來改變曲線的局部形狀。

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

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

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

(15)

(16)

式中X′、Y′——橫、縱坐標一階導數

X″、Y″——橫、縱坐標二階導數

2 避障路徑規劃

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

2.1 算法流程

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

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

(1)通過讀取測距傳感器數據，判斷動態識別區中是否出現障礙物，若沒有則持續探測。

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

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

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

2.2 控制點選取策略

叉車行駛于已知的全局路徑通道中，遇到突然出現的障礙物，則觸發避障路徑規劃。

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

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

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

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

3 試驗與結果分析

3.1 試驗設計

3.1.1試驗平臺

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

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

3.1.2主要傳感器及輔助器具

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

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

圖4 試驗現場Fig.4 Test scene

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

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

表1 相關參數Tab.1 Related parameters

3.1.3試驗場地

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

3.2 結果分析

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

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

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

3.2.1直行路段避障路徑規劃

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

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

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

圖7 直行路段規劃結果分析Fig.7 Analysis of planning result on straight road section

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

3.2.2轉彎路段避障路徑規劃

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

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

圖8 轉彎路段規劃結果分析Fig.8 Analysis of planning result on turning road section

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

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

圖9 轉彎路段路徑規劃Fig.9 Path planning on turning road section

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

4 結束語

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