錢(qián)東海，孫林林，趙 偉

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

0 引言

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

路徑規(guī)劃技術(shù)[1-5]是移動(dòng)智能裝備的關(guān)鍵技術(shù)，受到國(guó)內(nèi)外學(xué)者的大量關(guān)注。常見(jià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)等問(wèn)題，但離實(shí)際最優(yōu)仍有一定差距。快速搜索隨機(jī)樹(shù)算法[10]是一種基于采樣的搜索算法，搜索速度快，不需要對(duì)地圖預(yù)處理，但搜索的盲目性較大，且路徑不連續(xù)，不利于AGV跟蹤。Rastelli J P[11]等使用貝塞爾曲線對(duì)城市轉(zhuǎn)彎道路進(jìn)行路徑規(guī)劃，通過(guò)動(dòng)態(tài)調(diào)整控制點(diǎn)的位置實(shí)現(xiàn)路徑平滑，提高乘坐的舒適性，但沒(méi)有實(shí)車實(shí)驗(yàn)。另外，參數(shù)化曲線也被用來(lái)進(jìn)行路徑平滑[12-14]。

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

1 路徑描述

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

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

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

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

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

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

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

(2)

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

(3)

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

(4)

1.1.2 路徑最大曲率約束

規(guī)劃路徑若曲率過(guò)大，會(huì)導(dǎo)致其轉(zhuǎn)彎半徑小于叉車的最小轉(zhuǎn)彎半徑，使得叉車型AGV無(wú)法準(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)成，且可以通過(guò)改變控制點(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)象，需要對(duì)規(guī)劃路徑的起點(diǎn)和終點(diǎn)進(jìn)行位姿約束。路徑的起點(diǎn)為該時(shí)刻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個(gè)控制點(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樣條曲線本身不通過(guò)首末控制點(diǎn)，為了使B樣條曲線通過(guò)規(guī)劃路徑的起點(diǎn)和終點(diǎn)并滿足起點(diǎn)和終點(diǎn)的航向角約束，以起點(diǎn)和終點(diǎn)為中點(diǎn)，在其兩端各增加一個(gè)控制點(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è)該時(shí)刻參考路徑上距離AGV的最近點(diǎn)為pN(xN,yN,θN)，pN點(diǎn)為最近點(diǎn)，但并不滿足曲率要求，所以終點(diǎn)需以pN點(diǎn)為起點(diǎn)，沿參考路徑方向?qū)ふ?。令l1為自pN點(diǎn)出發(fā)，沿參考路徑行進(jìn)的距離。在本算法中，再令構(gòu)造距離L1=L2=l2，這樣只需要優(yōu)化求解l1和l2，即可確定3次B樣條曲線，從而尋找出最短路徑。

3.1 路徑規(guī)劃過(guò)程

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

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

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

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

圖5 直線路徑中各因素對(duì)路徑長(zhǎng)度的影響

圖6 圓弧路徑中各因素對(duì)路徑長(zhǎng)度的影響

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

3.3 路徑規(guī)劃算法

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

4)以上一步驟求得的行進(jìn)距離l1為基礎(chǔ)，優(yōu)化構(gòu)造距離l2：在初始構(gòu)造距離l2的基礎(chǔ)上加上試探步長(zhǎng)t0，按步驟3)中同樣的方法，求取最短路徑；若本次所得到的最優(yōu)路徑長(zhǎng)度小于上一次，則取上一次步長(zhǎng)的β倍作為遞增步長(zhǎng)，即ti=βti-1，所以l2,j=l2,j-1+β·ti-1；若該次所得到的最優(yōu)路徑長(zhǎng)度大于上一次，以該次最優(yōu)路徑對(duì)應(yīng)的構(gòu)造距離l2和上一次最優(yōu)路徑對(duì)應(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ù)過(guò)小時(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在不同起始位姿時(shí)，規(guī)劃路徑的最大路徑曲率均在最大曲率要求范圍內(nèi)、曲率連續(xù)，且起點(diǎn)和終點(diǎn)位置的曲率趨近于0，使規(guī)劃路徑具有良好的跟蹤性能，曲率的正負(fù)表示路徑的凹凸性；在圖7(c)反映了行進(jìn)距離l1迭代次數(shù)隨構(gòu)造距離l2迭代步數(shù)(迭代的推進(jìn))的變化，可以看出通過(guò)十多次構(gòu)造距離l2的迭代，每次l2迭代中嵌套的十多次的行進(jìn)距離l1迭代，可快速求得最優(yōu)路徑。

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

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

4.2 偏離圓弧路徑

以不同的初始位姿規(guī)劃重回圓弧的最短路徑，圓弧路徑的半徑為1.44 m，假設(shè)圓弧路徑起點(diǎn)為(0，0)，圓心為(0，1.44)。AGV在圓弧路徑外部時(shí)，假設(shè)全局坐標(biāo)為(0，-1)和(1，-1)，航向角分別為-15°、0、45°，當(dāng)AGV再圓弧路徑內(nèi)部時(shí)，全局坐標(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)中，可以看出通過(guò)十多次構(gòu)造距離l2迭代，每次l2迭代中嵌套的十多次的行進(jìn)距離l1迭代，可快速求得最優(yōu)路徑。

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

5 結(jié)束語(yǔ)

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