基于三次B樣條曲線的叉車型AGV路徑規劃研究

錢東海，孫林林，趙 偉

(上海大學 機電工程與自動化學院 上海 200444)

0 引言

為提高工廠的運行效率，叉車型AGV得到了越來越廣泛的使用。在工廠內，叉車型AGV通常沿規劃好的路徑行駛，但當叉車型AGV在運行時因避障或人為使用后會偏離規定參考路徑，因此，研究路徑規劃技術實現叉車型AGV自主回到參考路徑，提高叉車型AGV的作業效率具有重要意義。

路徑規劃技術[1-5]是移動智能裝備的關鍵技術，受到國內外學者的大量關注。常見的路徑規劃算法，如A*算法[6]、Dijkstra算法[7]需要首先繪制柵格地圖，柵格地圖的精度決定了規劃路徑的質量，且搜索速度慢，效率低。遺傳算法[8]具有良好的全局優化能力，但其收斂速度較慢，效率低，規劃結果因為初始種群的隨機性只能找出較優路徑。L.Zhang[9]等提出了一種改進的粒子群算法，用以提高算法效率，避免陷入局部最優等問題，但離實際最優仍有一定差距。快速搜索隨機樹算法[10]是一種基于采樣的搜索算法，搜索速度快，不需要對地圖預處理，但搜索的盲目性較大，且路徑不連續，不利于AGV跟蹤。Rastelli J P[11]等使用貝塞爾曲線對城市轉彎道路進行路徑規劃，通過動態調整控制點的位置實現路徑平滑，提高乘坐的舒適性，但沒有實車實驗。另外，參數化曲線也被用來進行路徑平滑[12-14]。

本文針對工廠環境中常用的叉車型AGV在偏離參考路徑較大距離時，需重新回到參考路徑的問題，研究相應的路徑規劃算法。論文以AGV的實時位姿為起點，以最短路徑為優化目標，建立符合最小轉彎半徑要求的3次B樣條路徑曲線，并在參考路徑上尋找出最優的路徑終點。論文將路徑規劃問題轉化為參數優化問題，并提出一種參數尋優算法，解決當AGV大幅度偏離參考路徑時能以最短距離重新回到參考路徑。論文安排如下：第一節給出了規劃路徑需要滿足的約束；第二節給出了路徑規劃中3次B樣條曲線的控制點的構造方法；第三節給出了路徑規劃算法的具體步驟；第四節通過Matlab仿真，驗證本文算法的有效性。

1 路徑描述

如圖1所示，本文以大幅度偏離參考路徑后的叉車型AGV的實時位姿作為全局路徑規劃的起點，將AGV從起點位置出發回到參考路徑的過程中形成的一系列離散點序列p={P1,P2,P3,…Pn-2,Pn-1,Pn}稱為規劃路徑,其中Pi(xi,yi,φi)為規劃路徑上一個點的全局位姿，(xi,yi)為全局坐標，φi為AGV的航向角。

圖1 叉車型AGV規劃路徑示意圖

本文將在滿足多約束條件下形成的路徑簇中選擇最短路徑作為規劃的最優目標路徑引導叉車型AGV回到參考路徑。目標函數為：

(1)

1.1 規劃路徑中的約束

對于單舵輪驅動的叉車型AGV，要使規劃路徑能被跟蹤，規劃的路徑曲線需要符合AGV運動學、結構參數約束，路徑最大曲率約束、路徑起點和終點位姿約束。

1.1.1 叉車型AGV運動學模型

叉車型AGV運動時速度較低且不易發生側滑現象，所以路徑規劃時僅考慮AGV的運動學模型。假設車體為剛體且車輪不變形，車輛在平面上運動、不打滑，即車輪和地面之間為純滾動。

叉車型AGV的運動學模型如圖2所示，在世界坐標系XOY下，(xf,yf)代表小車前輪坐標，(xr,yr)代表后輪中心坐標，o為AGV小車的瞬時轉動中心，L表示AGV的前后輪軸距，v表示舵輪線速度，δ和φ分別代表舵輪轉角和小車航向角，R為車體的轉彎半徑。

圖2 叉車型AGV運動學模型示意圖

由圖2可知，叉車型AGV的轉彎半徑為：

(2)

當舵輪轉角最大時有最小轉彎半徑Rmin，即：

(3)

叉車型AGV工作時，需始終確保舵輪轉角小于最大舵輪轉角：

(4)

1.1.2 路徑最大曲率約束

規劃路徑若曲率過大，會導致其轉彎半徑小于叉車的最小轉彎半徑，使得叉車型AGV無法準確跟蹤，從而造成再次偏離規劃路徑。所以規劃路徑曲率κ要滿足最大曲率約束。

路徑的曲率為:

(5)

所以曲率約束為:

(6)

結合式(3)、(6)可得:

(7)

1.1.3 曲率連續約束

為了避免規劃路徑的曲率不連續而造成跟蹤失敗，故路徑要滿足曲率連續，即規劃的路徑要滿足C2連續。

B樣條曲線是受控制點影響，并由多段連續曲線構成，且可以通過改變控制點的位置而局部修改整條曲線。所以本文選擇滿足曲率約束條件的3次B樣條曲線進行路徑規劃。

1.1.4 起點和終點位姿約束

為避免AGV在起點和終點出現就地轉向現象，需要對規劃路徑的起點和終點進行位姿約束。路徑的起點為該時刻AGV在全局坐標系下的位姿，即p1(x1,y1,φ1)；路徑的終點是AGV進入參考路徑的點，記為pn(xn,yn,φn)。3次B樣條曲線在其起點及終點需滿足：

(8)

2 3次B樣條曲線

3次B樣條曲線[15]的表達式為：

(9)

式中，s為參數，s∈[0,1]，Bj,3(s)為3次B樣條基函數；Ci+j為第i段中的第j個控制點。

3次B樣條的基函數的矩陣形式為：

(10)

其中:

將式(9)代入式(8)可得3次B樣條曲線上的點為：

(11)

相應的一階導數和二階導數為：

(12)

(13)

式中,

由上式可得3次B樣條曲線的曲率為：

(14)

由于B樣條曲線本身不通過首末控制點，為了使B樣條曲線通過規劃路徑的起點和終點并滿足起點和終點的航向角約束，以起點和終點為中點，在其兩端各增加一個控制點。

圖3 B樣條曲線與控制點的關系

設|p0p2|=2L1，|p3p5|=2L2，L1、L2稱為起點和終點處的構造距離，所以增加的各控制點坐標為:

(15)

(16)

(17)

(18)

3 路徑規劃算法

要規劃出滿足約束條件的最短路徑，不僅跟B樣條曲線的控制點的位置有關，還和選擇的終點位置有關。假設該時刻參考路徑上距離AGV的最近點為pN(xN,yN,θN)，pN點為最近點，但并不滿足曲率要求，所以終點需以pN點為起點，沿參考路徑方向尋找。令l1為自pN點出發，沿參考路徑行進的距離。在本算法中，再令構造距離L1=L2=l2，這樣只需要優化求解l1和l2，即可確定3次B樣條曲線，從而尋找出最短路徑。

3.1 路徑規劃過程

路徑規劃過程如圖4所示。傳感器檢測到此時刻叉車型AGV的位姿p1(x1,y1,φ1)，并通過計算得知已偏離參考路徑；然后找到參考路徑上距離AGV最近的坐標點pN(xN,yN,φN)；在最大曲率約束、起點和終點位姿約束的條件下，對于給定的初始構造距離l2(通常取一個很小的值)，找到此時最短路徑對應的沿參考路徑的行進距離l1；然后以此l1為基礎，優化構造距離l2，同時優化行進距離l1，循環往復，最終尋找到符合多約束條件的最優目標路徑，從而完成路徑規劃目標。上述優化求解本質上由兩個一維搜索構成，其中行進距離l1的一維搜索內嵌在構造距離l2一維搜索的內部，即l2每移動一個步長，都將通過一維搜索求得最優的行進距離l1。

圖4 路徑規劃過程

3.2 驗證搜索區間內的收斂性

利用枚舉法分別求取直線和圓弧參考路徑在不同的行進距離l1和構造距離l2時3次B樣條曲線的路徑長度。圖5和圖6中下方的線條代表不考慮規劃路徑曲率約束時，不同構造距離l2形成的B樣條曲線中，路徑長度為最短時對應的終點位置；上方的線條代表同時考慮規劃路徑曲率約束時各B樣條曲線中，路徑取長度最短時對應的終點位置。

圖5 直線路徑中各因素對路徑長度的影響

圖6 圓弧路徑中各因素對路徑長度的影響

從圖中可以看出以l1、l2為設計變量，構造設計空間，求得的3次B樣條曲線路徑長度函數的曲面為凸的，在考慮規劃路徑曲率約束時，形成的約束邊界也是凸的，故可用優化設計的方法求得滿足約束條件的全局最優解。

3.3 路徑規劃算法

本文采用加速步長法和二分法對行進距離l1和構造距離l2進行優化，具體步驟如下：

1)找到參考路徑上距離AGV的幾何最近點pN，并求其位置信息；

2)給定初始構造距離l2；

3)在給定構造距離l2下，尋找最優路徑對應的行進距離l1：用一個試探步長t0沿參考路徑的行進方向移動一步，若不滿足約束條件，則取上一次步長的β倍來遞增步長，即tj=βtj-1，所以l1,j=l1,j-1+β·tj-1。通過加速步長的方法快速求得搜索區間，以最后一次不滿足約束的位置和第一次滿足約束的位置為搜索區間進行二分法搜索，直至l1的變化已小于給定計算精度ε1，從而求得該構造距離下滿足約束條件的最短路徑和對應的行進距離l1；

4)以上一步驟求得的行進距離l1為基礎，優化構造距離l2：在初始構造距離l2的基礎上加上試探步長t0，按步驟3)中同樣的方法，求取最短路徑；若本次所得到的最優路徑長度小于上一次，則取上一次步長的β倍作為遞增步長，即ti=βti-1，所以l2,j=l2,j-1+β·ti-1；若該次所得到的最優路徑長度大于上一次，以該次最優路徑對應的構造距離l2和上一次最優路徑對應的構造距離l2為區間進行二分法搜索，直至l2的變化已小于給定計算精度ε2。

至此，得到最優參數l1和l2，從而得到滿足多約束條件的最短3次B樣條曲線路徑。

4 仿真實驗與分析

為了驗證提出的路徑規劃算法，本文利用Matlab建立了仿真驗證程序。AGV的前后軸距為1.44，舵輪最大轉角限定為，所以最大曲率為2.592 m-1，l1和l2精度ε1=ε2=0.02。由圖5和圖6可知，當優化參數過小時，找不到符合約束條件的路徑，經多次仿真實驗，選取直線段構造距離l1最小值為0.5 m，圓弧段構造距離l1最小值為0.3 m，在此約束條件下能夠較快地找到符合約束條件的最優目標路徑。

4.1 偏離直線路徑

以不同的初始位姿規劃·到直線的最短路徑，AGV在y軸初始偏差分別為1 m、2 m、3 m的情況下，航向角分別為-45°、0、45°，仿真結果如表1所示。

表1 直線段不同位姿下得到的參數l1、l2

選取表中第1、5、7三種情況進行分析，結果如圖7所示。在圖7(a)中，叉車型AGV在3種大偏差下都能滿足多約束條件，規劃出一條最短路徑；在圖7(b)中，AGV在不同起始位姿時，規劃路徑的最大路徑曲率均在最大曲率要求范圍內、曲率連續，且起點和終點位置的曲率趨近于0，使規劃路徑具有良好的跟蹤性能，曲率的正負表示路徑的凹凸性；在圖7(c)反映了行進距離l1迭代次數隨構造距離l2迭代步數(迭代的推進)的變化，可以看出通過十多次構造距離l2的迭代，每次l2迭代中嵌套的十多次的行進距離l1迭代，可快速求得最優路徑。

對比表1和圖7(a)、圖7(b)可以看出，當AGV位于直線參考路徑上方時，此時若AGV初始航向與參考路徑方向之間夾角為負值，如圖中的-45°，AGV能以更短的路徑距離回到參考路徑；相反，AGV初始航向與參考路徑方向之間夾角為正值，如圖中的45°，AGV規劃路徑長度較長，但曲率變化較為平緩，這有利于AGV的路徑跟蹤。

圖7 直線段不同位姿下規劃路徑結果

4.2 偏離圓弧路徑

以不同的初始位姿規劃重回圓弧的最短路徑，圓弧路徑的半徑為1.44 m，假設圓弧路徑起點為(0，0)，圓心為(0，1.44)。AGV在圓弧路徑外部時，假設全局坐標為(0，-1)和(1，-1)，航向角分別為-15°、0、45°，當AGV再圓弧路徑內部時，全局坐標為(0，0.5)，航向角分別為-45°、0、15°,仿真結果如表2所示。

表2 直線段不同位姿下得到的參數l1、l2

選取表中第1、6、8三種情況進行分析，結果如圖8所示。在圖8 (a)、圖8 (b)中可以看出，叉車型AGV在圓弧路徑內外都能在多約束條件下規劃出一條曲率連續、具有良好的跟蹤性能規劃路徑；在圖8 (c)中，可以看出通過十多次構造距離l2迭代，每次l2迭代中嵌套的十多次的行進距離l1迭代，可快速求得最優路徑。

圖8 圓弧段不同位姿下規劃路徑結果

5 結束語

本文針對工廠環境下叉車型AGV因避障或人為使用后大幅度偏離參考路徑后需重新自動回到參考路徑時的路徑規劃問題進行了研究，設計了基于3次B樣條曲線的路徑規劃算法，從而實現了AGV在多約束條件下，規劃出回到參考路徑的最短路徑。最后通過Matlab對直線和圓弧參考路段進行了仿真實驗，實驗表明本文提出的軌跡規劃算法能夠快速、高質量地規劃出具有良好跟蹤效果的最短路徑。