錢東海，孫林林，趙 偉

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

0 引言

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

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

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

1 路徑描述

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

圖1 叉車型AGV規(guī)劃路徑示意圖

本文將在滿足多約束條件下形成的路徑簇中選擇最短路徑作為規(guī)劃的最優(yōu)目標(biāo)路徑引導(dǎo)叉車型AGV回到參考路徑。目標(biāo)函數(shù)為：

(1)

1.1 規(guī)劃路徑中的約束

對于單舵輪驅(qū)動的叉車型AGV，要使規(guī)劃路徑能被跟蹤，規(guī)劃的路徑曲線需要符合AGV運(yùn)動學(xué)、結(jié)構(gòu)參數(shù)約束，路徑最大曲率約束、路徑起點(diǎn)和終點(diǎn)位姿約束。

1.1.1 叉車型AGV運(yùn)動學(xué)模型

叉車型AGV運(yùn)動時速度較低且不易發(fā)生側(cè)滑現(xiàn)象，所以路徑規(guī)劃時僅考慮AGV的運(yùn)動學(xué)模型。假設(shè)車體為剛體且車輪不變形，車輛在平面上運(yùn)動、不打滑，即車輪和地面之間為純滾動。

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

圖2 叉車型AGV運(yùn)動學(xué)模型示意圖

由圖2可知，叉車型AGV的轉(zhuǎn)彎半徑為：

(2)

當(dāng)舵輪轉(zhuǎn)角最大時有最小轉(zhuǎn)彎半徑Rmin，即：

(3)

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

(4)

1.1.2 路徑最大曲率約束

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

路徑的曲率為:

(5)

所以曲率約束為:

(6)

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

(7)

1.1.3 曲率連續(xù)約束

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

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

1.1.4 起點(diǎn)和終點(diǎn)位姿約束

為避免AGV在起點(diǎn)和終點(diǎn)出現(xiàn)就地轉(zhuǎn)向現(xiàn)象，需要對規(guī)劃路徑的起點(diǎn)和終點(diǎn)進(jìn)行位姿約束。路徑的起點(diǎn)為該時刻AGV在全局坐標(biāo)系下的位姿，即p1(x1,y1,φ1)；路徑的終點(diǎn)是AGV進(jìn)入?yún)⒖悸窂降狞c(diǎn)，記為pn(xn,yn,φn)。3次B樣條曲線在其起點(diǎn)及終點(diǎn)需滿足：

(8)

2 3次B樣條曲線

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

(9)

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

3次B樣條的基函數(shù)的矩陣形式為：

(10)

其中:

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

(11)

相應(yīng)的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)為：

(12)

(13)

式中,

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

(14)

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

圖3 B樣條曲線與控制點(diǎn)的關(guān)系

設(shè)|p0p2|=2L1，|p3p5|=2L2，L1、L2稱為起點(diǎn)和終點(diǎn)處的構(gòu)造距離，所以增加的各控制點(diǎn)坐標(biāo)為:

(15)

(16)

(17)

(18)

3 路徑規(guī)劃算法

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

3.1 路徑規(guī)劃過程

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

圖4 路徑規(guī)劃過程

3.2 驗(yàn)證搜索區(qū)間內(nèi)的收斂性

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

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

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

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

3.3 路徑規(guī)劃算法

本文采用加速步長法和二分法對行進(jìn)距離l1和構(gòu)造距離l2進(jìn)行優(yōu)化，具體步驟如下：

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

2)給定初始構(gòu)造距離l2；

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

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

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

4 仿真實(shí)驗(yàn)與分析

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

4.1 偏離直線路徑

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

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

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

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

圖7 直線段不同位姿下規(guī)劃路徑結(jié)果

4.2 偏離圓弧路徑

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

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

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

圖8 圓弧段不同位姿下規(guī)劃路徑結(jié)果

5 結(jié)束語

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