移動機(jī)器人平滑JPS路徑規(guī)劃與軌跡優(yōu)化方法

黃健萌 吳宇雄 林謝昭

(福州大學(xué)機(jī)械工程及自動化學(xué)院， 福州 350116)

0 引言

隨著移動機(jī)器人被廣泛應(yīng)用于農(nóng)業(yè)等諸多領(lǐng)域，移動機(jī)器人導(dǎo)航技術(shù)逐漸成為研究熱點(diǎn)。路徑規(guī)劃是移動機(jī)器人完成導(dǎo)航任務(wù)的核心技術(shù)之一，其目的是讓機(jī)器人在充滿障礙物的復(fù)雜環(huán)境中找到從起點(diǎn)到終點(diǎn)的最佳無碰撞路徑[1]，通常是以規(guī)劃出的路徑最短作為優(yōu)化目標(biāo)。目前常用的路徑規(guī)劃方法有柵格法、人工勢場法、可視圖法等[2]，以及蟻群算法、粒子群算法等智能算法[3-4]。柵格法簡單有效，是目前使用最廣泛的一種方法。

柵格法中，Dijkstra算法[5]搜索時(shí)只考慮已花費(fèi)成本，雖然能保證找到最短路徑，但會擴(kuò)展大量無用節(jié)點(diǎn)。BFS算法采用啟發(fā)函數(shù)快速接近目標(biāo)點(diǎn)，不考慮已花費(fèi)成本，其效率高于Dijkstra，但不能保證找到最短路徑。A*算法[6]兼顧了兩者的優(yōu)點(diǎn)，在考慮已花費(fèi)成本的同時(shí)采用啟發(fā)式搜索，并能保證找到最短路徑。但A*算法規(guī)劃的路徑通常是一組曲折路徑，僅是離散空間上的最優(yōu)解，與連續(xù)空間上實(shí)際最優(yōu)解有較大差距。“視野線”法[7-9]常被用來對A*算法規(guī)劃的路徑進(jìn)行平滑后處理，但只在后處理時(shí)使用平滑方法往往無法使路徑跨過障礙物，優(yōu)化效果有限，平滑后的路徑有時(shí)仍與最優(yōu)解存在較大差距。為此，Theta*算法[10]提出了任意角度規(guī)劃路徑的新思路，該方法在A*算法的基礎(chǔ)上，采取實(shí)時(shí)收縮父節(jié)點(diǎn)，即邊搜索邊平滑的策略，與采用后處理平滑的路徑規(guī)劃方法相比，能更好地接近最優(yōu)解。在此基礎(chǔ)上許多學(xué)者進(jìn)一步提出了Lazy-Theta*、Strict-Theta*、Batch-Theta*等Theta*類算法[11-13]，在效率和平滑處理等方面進(jìn)行改進(jìn)。由于Theta*類算法是在A*算法基礎(chǔ)上的改進(jìn)，因此隨著柵格環(huán)境的增大，存在擴(kuò)展節(jié)點(diǎn)急劇增加、搜索時(shí)間大幅延長的情況。文獻(xiàn)[14-16]提出的跳點(diǎn)搜索(Jump point search,JPS)算法采用了完全不同的節(jié)點(diǎn)擴(kuò)展方式，與A*算法相比，JPS算法有效減少了需要維護(hù)的節(jié)點(diǎn)數(shù)量，尤其是在大柵格環(huán)境中，有效提高了尋路效率，但JPS所規(guī)劃路徑仍然只是離散空間的最優(yōu)路徑。

采用柵格法規(guī)劃的路徑通常無法直接用于機(jī)器人運(yùn)動，需進(jìn)行軌跡優(yōu)化處理。圓切線[8,17]方法雖然能生成可行軌跡，但并未考慮機(jī)器人運(yùn)動的速度、慣性等物理參數(shù)。通過微分平坦[18]，多項(xiàng)式對各類機(jī)器人[19-20]在軌跡優(yōu)化方面有著廣泛應(yīng)用，相比單段多項(xiàng)式，多段多項(xiàng)式可以更好地適應(yīng)復(fù)雜環(huán)境。文獻(xiàn)[21]利用多段高階多項(xiàng)式的特性將加速度相關(guān)導(dǎo)數(shù)作為優(yōu)化目標(biāo)。文獻(xiàn)[22-23]將多段多項(xiàng)式約束在安全范圍內(nèi)，以避免軌跡發(fā)生碰撞。目前，主要依靠迭代獲取多段多項(xiàng)式軌跡優(yōu)化的最優(yōu)解。

本文在JPS算法的基礎(chǔ)上，提出一種平滑JPS(SJPS)算法。首先針對現(xiàn)有平滑方法的缺點(diǎn)提出一種路徑平滑方法，對JPS搜索規(guī)則進(jìn)行修改，遍歷對稱路徑，并利用本文平滑方法對每條所得路徑進(jìn)行平滑處理，再權(quán)衡長度與角度兩指標(biāo)，最后對所得初始路徑用多段多項(xiàng)式進(jìn)行軌跡優(yōu)化，改進(jìn)時(shí)間分配問題，加快迭代效率，并結(jié)合SJPS的特點(diǎn)對端點(diǎn)進(jìn)行合理約束。

1 路徑平滑

“視野線”方法在部分情況下存在去除必要節(jié)點(diǎn)，留下冗余節(jié)點(diǎn)的問題。如圖1所示，路徑上每個柵格中心點(diǎn)均為路徑點(diǎn)，若按“視野線”方法平滑，則路徑將無改變，因?yàn)槠瘘c(diǎn)與轉(zhuǎn)折點(diǎn)連線檢查會將原本必要的節(jié)點(diǎn)去除。為得到穩(wěn)定且良好的平滑效果，本文提出以2個目標(biāo)對路徑點(diǎn)序列進(jìn)行優(yōu)化，即路徑每個折角處均在障礙物柵格的頂點(diǎn)上，且與路徑轉(zhuǎn)角進(jìn)行接觸的障礙物位于轉(zhuǎn)角小于180°一側(cè)。

1.1 路徑單側(cè)有障礙物的情形

本文路徑平滑遵循以下步驟：

(1)將起點(diǎn)作為平滑路徑點(diǎn)的第1個點(diǎn)，記為qc(xc,yc)。

(2)將qc依次與后續(xù)JPS路徑點(diǎn)連接，直到找到連線Lm經(jīng)過障礙物，對應(yīng)連接路徑點(diǎn)qm(xm,ym)，并記錄前一條沒經(jīng)過障礙物的連線Lm-1，對應(yīng)路徑點(diǎn)qm-1(xm-1,ym-1)，并將點(diǎn)qm-1和qm之間的路徑離散成單個柵格移動的路徑點(diǎn)。若連接到了終點(diǎn)也未經(jīng)過障礙物，則終點(diǎn)為最后一個平滑路徑點(diǎn)，結(jié)束搜索。

(3)再將點(diǎn)qc依次與步驟(2)中離散的路徑點(diǎn)(包括qm)連接，直到找到一條經(jīng)過障礙物的連線作為新的Lm，對應(yīng)連接點(diǎn)作為新的qm，前一條連線記為新的Lm-1，對應(yīng)路徑點(diǎn)為新的qm-1。

(4)從Lm經(jīng)過的障礙物柵格中，從可能頂點(diǎn)中找到一柵格頂點(diǎn)qj(xj,yj)，使點(diǎn)qc和qj的連線與Lm的夾角達(dá)到最大，則qj為新的平滑路徑點(diǎn)。將qj作為新的起始點(diǎn)qc從步驟(2)開始新一輪搜索。

以圖2的情況為例，首先將起點(diǎn)作為第1個平滑路徑點(diǎn)，并作為搜索起點(diǎn)qc，從第2個點(diǎn)(4,5)開始，將后續(xù)路徑點(diǎn)依次與qc連接，每次與qc連接的點(diǎn)記為qm。直到連接qc(2,3)和點(diǎn)(8,11)時(shí)，其連線經(jīng)過障礙物，則將兩點(diǎn)間的路徑離散成單個柵格移動的路徑點(diǎn)，再將點(diǎn)qc依次與離散的路徑點(diǎn)連接。連接到點(diǎn)(5,8)時(shí)，兩點(diǎn)連線經(jīng)過障礙物柵格(5，7)，記為Lm。然后選取障礙物柵格左上角頂點(diǎn)(4.5,7.5)作為下一個平滑路徑點(diǎn)，并以其為新的起始點(diǎn)繼續(xù)搜索。點(diǎn)(4.5,7.5)作為起始點(diǎn)搜索時(shí)，連接到終點(diǎn)也未經(jīng)過障礙物，因此將終點(diǎn)作為最后一個平滑路徑點(diǎn)。最終產(chǎn)生的平滑路徑即圖2中的虛線路徑。

1.2 平滑路徑點(diǎn)選取

對于步驟(4)中障礙物柵格頂點(diǎn)的選取，使用一定的規(guī)則。如圖3所示，當(dāng)經(jīng)過障礙物的連線Lm非水平或垂直時(shí)，挑選連線上經(jīng)過的障礙物，連線水平或垂直時(shí)，則只挑選第1個遇到的障礙物柵格。對于挑選的柵格(xobs,yobs)，每個柵格有4個頂點(diǎn)，但對于每次平滑路徑點(diǎn)搜索只有一個方向的頂點(diǎn)有可能被選取，延用步驟(2)中的qm-1、qc和步驟(3)中的qm標(biāo)記，可能被選取的頂點(diǎn)應(yīng)落在由qm、qc和qm-13點(diǎn)形成的三角形區(qū)域內(nèi)。若步驟(2)中沒有按要求離散，則三角形區(qū)域?qū)⒋嬖诒贿z漏的障礙物柵格。如圖4所示，頂點(diǎn)的選取有16種可能情況。先將點(diǎn)qm到點(diǎn)qc和qm-1方向的兩向量Vm,c和Vm,m-1叉乘判斷兩向量的順逆時(shí)針關(guān)系，再根據(jù)qm和qc連線斜率和Vm,c的朝向選擇頂點(diǎn)(xj,yj)

(1)

(2)

(3)

(4)

(5)

式(1)分別對應(yīng)圖4中的16種情況，對應(yīng)情形的向量位于哪一側(cè)則選取該側(cè)頂點(diǎn)。

選出各障礙物柵格頂點(diǎn)(xj,yj)后，將其與qc連線，并求其連線與Lm的夾角θ為

(6)

其中

V1=[xm-xc,ym-yc]

(7)

V2=[xj-xc,yj-yc]

(8)

從所有夾角中選出最大夾角所對應(yīng)頂點(diǎn)qj作為下一個平滑路徑點(diǎn)和新的搜索起點(diǎn)qc。

1.3 路徑雙側(cè)均有障礙物的情形

當(dāng)路徑雙側(cè)均有障礙物時(shí)，可能出現(xiàn)如圖5a所示情況。圖5a中虛線路徑為按上述規(guī)則優(yōu)化后的平滑路徑，圖5b中虛線為理想平滑路徑?？芍詈笠粋€轉(zhuǎn)折處，轉(zhuǎn)折角小于180°處所在區(qū)域并無障礙物，不滿足第2個目標(biāo)。這是因?yàn)閷ふ业?個新路徑點(diǎn)時(shí)，在從終點(diǎn)開始逐個連線進(jìn)行搜索的過程中，受到了右側(cè)障礙物的干擾。

為此，在上述平滑路徑搜索步驟中增加1個局部平滑步驟。每3個連續(xù)路徑點(diǎn)產(chǎn)生一折角，從第3個新路徑點(diǎn)開始，每產(chǎn)生一個路徑點(diǎn)O3(x3,y3)，則設(shè)其前2個點(diǎn)依次為O2(x2,y2)、O1(x1,y1)，W1(W2)為O2指向O1(O3)的單位向量。如圖6所示，3點(diǎn)連線在障礙物一側(cè)的夾角為

θ′=α+β+γ=90°+β+γ

(9)

若該轉(zhuǎn)角θ′<180°，則有

β+γ<90°?β<90°且γ<90°

(10)

因此在圖6所示的局部坐標(biāo)系下，W1(W2)總指向第三(一)象限，則2個單位向量的和向量W12(x′12,y′12)滿足

(11)

由式(11)可知，W12總指向局部坐標(biāo)系第四象限，即障礙物應(yīng)在象限，于是可得符合要求的轉(zhuǎn)角所對應(yīng)的障礙物坐標(biāo)(xp,yp)為

(xp,yp)=round((x2,y2)+W′12)

(12)

式中W′12——W12的單位向量

round()——四舍五入函數(shù)

通過上述方式可判斷O2處所在轉(zhuǎn)角小于180°一側(cè)對應(yīng)柵格是否為障礙物，其他方向判斷與此同理。

(13)

式中 ceil()——往正無窮大方向取整的函數(shù)

步長s過長將導(dǎo)致選取平滑路徑點(diǎn)時(shí)遺漏障礙物。

2 平滑JPS算法

2.1 搜索規(guī)則

平滑JPS(SJPS)算法與原JPS算法相比有以下區(qū)別：

(1)終止條件。在開放列表第1次彈出終點(diǎn)時(shí)，SJPS不停止搜索，將第1次得到的路徑(未平滑)的長度作為約束值fz=l1，繼續(xù)擴(kuò)展所有使啟發(fā)函數(shù)滿足f(n)≤fz的跳點(diǎn)，以遍歷已搜索路徑，直到不再有滿足要求的跳點(diǎn)。圖8是一個簡單的例子，圖8a為JPS路徑。在SJPS中，終點(diǎn)被彈出前，只有跳點(diǎn)1、2、3、4和起點(diǎn)被彈出，終點(diǎn)首次被彈出后將計(jì)算當(dāng)前所得路徑，即圖8a路徑的長度，作為約束值fz，繼續(xù)擴(kuò)展開放列表中滿足f(n)≤fz的跳點(diǎn)，因此跳點(diǎn)5將繼續(xù)被擴(kuò)展。

(2)更新與記錄父節(jié)點(diǎn)方式。1個跳點(diǎn)可能被2個以上跳點(diǎn)所擴(kuò)展，在JPS中，只記錄使啟發(fā)函數(shù)最小的跳點(diǎn)，舊父節(jié)點(diǎn)將被完全舍棄，而在SJPS中，被舍棄的父節(jié)點(diǎn)將被單獨(dú)保存，僅在回溯路徑時(shí)被使用，并且即使被擴(kuò)展跳點(diǎn)已處于關(guān)閉列表仍然會進(jìn)行更新。

(3)路徑回溯。在JPS中，終點(diǎn)被彈出后，從終點(diǎn)開始循環(huán)查找父節(jié)點(diǎn)，直到找到起點(diǎn)。在SJPS中，如區(qū)別(2)所述，回溯路徑時(shí)1個節(jié)點(diǎn)可能有多個父節(jié)點(diǎn)，此時(shí)將對每個父節(jié)點(diǎn)均進(jìn)行路徑回溯。如圖8b，終點(diǎn)有2個父節(jié)點(diǎn)為跳點(diǎn)3和5，回溯路徑如圖8b所示兩條對稱路徑。每條回溯的路徑，均對其進(jìn)行平滑處理，如圖8c所示。最后從所獲平滑路徑中，選取最理想的路徑為最終路徑，如圖8d所示。

2.2 路徑選擇

大多數(shù)路徑規(guī)劃方法都以長度作為指標(biāo)進(jìn)行尋找，但在一些情況下，更長一點(diǎn)的路徑可能會有更少的轉(zhuǎn)折和總轉(zhuǎn)角。因此，設(shè)立閾值Lz，對于每一條回溯的路徑，在平滑后記錄其長度和總轉(zhuǎn)折角。先將第1條平滑路徑作為當(dāng)前路徑長度lnow和總轉(zhuǎn)折角φnow，然后遍歷其余平滑路徑長度lother和總轉(zhuǎn)折角φother進(jìn)行對比，若滿足約束條件

(14)

式中w1——角度傾向權(quán)重

w2——長度傾向權(quán)重

其中之一，則將lother和φother作為新的lnow和φnow，對應(yīng)路徑作為新的當(dāng)前路徑，并繼續(xù)遍歷對比。最后所得當(dāng)前路徑則為最終路徑。

3 軌跡優(yōu)化

3.1 基于多段多項(xiàng)式的軌跡優(yōu)化

直線路徑不利于機(jī)器人的運(yùn)動，在SJPS規(guī)劃路徑基礎(chǔ)上利用多項(xiàng)式進(jìn)行軌跡優(yōu)化處理。將多項(xiàng)式各階導(dǎo)數(shù)作為軌跡的位置、速度(v)、加速度(a)和加加速度(j)等參數(shù)，單段多項(xiàng)式難以適應(yīng)較為復(fù)雜的環(huán)境，因此使用多段高階多項(xiàng)式可以更好地在各種場景進(jìn)行軌跡優(yōu)化。為了能限制加速度導(dǎo)數(shù)，使用五次多項(xiàng)式。使用多段多項(xiàng)式平滑路徑，首先將軌跡按路徑點(diǎn)分段，設(shè)共分成m段。每小段路徑擬合為一多項(xiàng)式曲線，則對第n段路徑有

(15)

對于整條路徑，則有

(16)

式中T0、T1、…、Tm——路徑端點(diǎn)代表的時(shí)刻

由式(16)可知，通過求導(dǎo)可得任意時(shí)刻t的位置(P=f(t))、速度(v=f′(t))等參數(shù)并施加約束。結(jié)合本文平滑路徑點(diǎn)的特點(diǎn)，先約束多項(xiàng)式曲線經(jīng)過起、止點(diǎn)，再限制其他路徑點(diǎn)只朝向與其接觸的障礙物相反的方向(如1.3節(jié)所述與W12相反的方向)

(17)

式中Pi——第i個路徑點(diǎn)

S——約束放寬的長度

m段多項(xiàng)式對應(yīng)m+1個路徑點(diǎn)，約束的放寬對解的質(zhì)量有重要影響。每個路徑點(diǎn)只限制了一段多項(xiàng)式曲線，因此需施加連續(xù)性約束，使不同段的多項(xiàng)式曲線在路徑點(diǎn)處位移、速度和加速度連續(xù)

(18)

可根據(jù)需要選取最小化目標(biāo)函數(shù)的導(dǎo)數(shù)階數(shù)，此處以最小化j為目標(biāo)以增加運(yùn)動平穩(wěn)性，因此軌跡優(yōu)化問題將轉(zhuǎn)換為如下最優(yōu)化問題

(19)

該優(yōu)化問題屬于QP問題。優(yōu)化后的一些物理量可能超過預(yù)設(shè)值，可對優(yōu)化后的時(shí)間比例乘一系數(shù)

(20)

式中M——速度等物理量預(yù)期值

2月7日，水利部與中國氣象局在京簽訂《關(guān)于加快水利和氣象發(fā)展的合作備忘錄》。雙方將加強(qiáng)合作與信息共享，推進(jìn)水利跨越式發(fā)展，加快氣象現(xiàn)代化建設(shè)，促進(jìn)水利、氣象事業(yè)全面協(xié)調(diào)可持續(xù)發(fā)展，共同提高國家綜合防災(zāi)減災(zāi)能力和水平。水利部部長、黨組書記陳雷，中國氣象局局長、黨組書記鄭國光出席簽字儀式并代表雙方簽訂備忘錄。

以此控制相關(guān)物理量的最高值，這不會影響優(yōu)化的效果[24]。

優(yōu)化時(shí)的軌跡存在重新碰撞障礙物的可能，可通過約束多項(xiàng)式端點(diǎn)的方式避開障礙物。如圖9所示，當(dāng)某段軌跡被檢測到碰撞后，進(jìn)行連線檢查，當(dāng)有連線未經(jīng)障礙物時(shí)，將該連線端點(diǎn)，及上一經(jīng)過障礙物的連線的其中一個端點(diǎn)，3個點(diǎn)作為路徑點(diǎn)按前述方法做平滑，得到新的平滑路徑點(diǎn)作為軌跡的約束，使軌跡經(jīng)過該點(diǎn)。

3.2 時(shí)間分配

上述QP問題，實(shí)際上可以看作是尋找最優(yōu)時(shí)間分配問題。通常先給定一個時(shí)間分配初值，再用迭代的方法尋找最優(yōu)解。對于時(shí)間初值的給定，目前有采用均速運(yùn)動推算時(shí)間，即按路徑長度比例分配時(shí)間的方法[21]，雖然能收斂到最優(yōu)解，但缺乏合理性，初值與最優(yōu)解偏差較大，將增大迭代難度，增加運(yùn)算時(shí)間。合理的時(shí)間初值可以大大減少迭代次數(shù)。

針對起止為靜止?fàn)顟B(tài)的時(shí)間分配，設(shè)移動機(jī)器人從起點(diǎn)開始，平穩(wěn)加速到最大允許速度時(shí)均速運(yùn)動，接近終點(diǎn)時(shí)再平穩(wěn)減速。若以最小化j為目標(biāo)，則加速期間加速度應(yīng)有一個平緩的變化率，因此借助余弦函數(shù)設(shè)加速度函數(shù)為

(21)

(22)

式中wj——期望的最大j值

vmax——最大允許速度

且從靜止加速到最大允許速度的時(shí)間為2π/wv。為使加速度不大于給定值amax，令

(23)

到達(dá)最大允許速度前的位移函數(shù)為

(24)

優(yōu)化后的軌跡往往長于初始軌跡，因此將初始路徑乘一常系數(shù)ws，即s′i=wssi，并令sinit=spro1(2π/wv)，對于首段位移的時(shí)間分配，有

(25)

末段位移時(shí)間的計(jì)算與首段位移同理。對于首、末段之間位移s′i的時(shí)間分配則有

(26)

式中SFsum、SBsum——s′i之前、之后的位移

TFsum、TBsum——s′i之前、之后的累計(jì)時(shí)間

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

Matlab R2019b提供了ROS與導(dǎo)航相關(guān)工具箱，為驗(yàn)證本文方法，在 I5-6300HQ型便攜式計(jì)算機(jī)上使用Matlab R2019b軟件與ROS平臺對本文算法進(jìn)行仿真與實(shí)驗(yàn)。

4.1 與Lazy-Theta*對比

Theta*類算法為得到更短的路徑，通常將路徑點(diǎn)設(shè)置于柵格頂點(diǎn)而非中心點(diǎn)。為方便對比，將SJPS的起始點(diǎn)在平滑前設(shè)置為當(dāng)前柵格的柵格頂點(diǎn)，使其與Lazy-Theta*的起始點(diǎn)一致。此處SJPS在路徑選擇中，將Lz設(shè)為1倍柵格長度，w1、w2均設(shè)為30，啟發(fā)函數(shù)采用歐幾里得距離。Lazy-Theta*為八鄰域搜索，啟發(fā)函數(shù)同樣采用歐幾里得距離。在長寬為40×40場景隨機(jī)分布40個邊長為1.5倍柵格長度的方形障礙物，以不同精度劃分為柵格環(huán)境分別進(jìn)行20次實(shí)驗(yàn)并取平均值，結(jié)果如圖10所示，圖10橫坐標(biāo)為橫/縱柵格數(shù)。

由圖10a可知，與Lazy-Theta*相比，SJPS的路徑長度稍短，總體較為接近。SJPS平滑性[25](總轉(zhuǎn)折角)明顯低于Lazy-Theta*。由圖10b可知，Lazy-Theta*和SJPS在低柵格精度時(shí)，搜索效率接近，隨著柵格精度提高，Lazy-Theta*的運(yùn)行時(shí)間因?yàn)樗阉鞴?jié)點(diǎn)大幅增加而急劇增長，而SJPS得益于跳點(diǎn)搜索，運(yùn)行時(shí)間隨精度的提高上升較為平緩。其中柵格數(shù)為400×400時(shí)，Lazy-Theta*的平均運(yùn)行時(shí)間為21.897 s，SJPS的平均運(yùn)行時(shí)間為0.247 s。

4.2 與JPS對比

在長寬為40×40場景隨機(jī)分布160個邊長為1的方形障礙物并劃分為100×100柵格環(huán)境，在該環(huán)境進(jìn)行的SJPS與JPS對比示例如圖11所示。

圖11b SJPS在路徑選擇中，將Lz設(shè)為1倍柵格長度，w1、w2均設(shè)為30，啟發(fā)函數(shù)兩者均為歐幾里得距離。如圖11a所示,僅在后處理對路徑進(jìn)行平滑，無法使路徑跨越障礙物，除非將平滑處理變成另一意義上的路徑規(guī)劃，因而很難接近實(shí)際最優(yōu)解。圖11b中，SJPS通過挑選有價(jià)值的路徑，以本文方法逐一進(jìn)行平滑，再以一定方法進(jìn)行路徑選取，以此可以得到更好的路徑。將障礙物數(shù)量設(shè)為160(10%障礙物)、320(20%障礙物)和480個(30%障礙物)分別進(jìn)行20次實(shí)驗(yàn)并取平均值，其數(shù)據(jù)如表1所示。由表1可知，與平滑后處理的JPS相比，SJPS對長度和總轉(zhuǎn)角有改善作用，運(yùn)行時(shí)間增加7.06%～10.60%。對使用后處理的JPS，后處理的時(shí)間為0.8～1.2 ms。

表1 100×100柵格環(huán)境下比較結(jié)果Tab.1 Results comparison in 100×100 grid environment

4.3 軌跡優(yōu)化

對圖11b所得SJPS路徑進(jìn)行軌跡優(yōu)化，設(shè)起點(diǎn)和終點(diǎn)為靜止?fàn)顟B(tài)，以柵格長度為單位，允許速度、加速度最大值分別為vmax=0.25 m/s、amax=0.25 m/s2，wj為1，ws為1.1，結(jié)果如圖11c所示。其中實(shí)線為最終優(yōu)化結(jié)果，虛線為本文方法初始解(未考慮碰撞)，與最終解貼合程度較高，星號線為相同總時(shí)間下按路徑比分配時(shí)間的初始解(未考慮碰撞)，與最終解偏差較大。兩種初始解均能收斂于同一最優(yōu)解。

圖11c的3種解對應(yīng)規(guī)劃軌跡的速度、加速度、加加速度如圖12所示。本文方法初始解與最終解的各項(xiàng)參數(shù)吻合程度較好，采用路徑比分配時(shí)間初始解的方法，在首、末段軌跡帶來較大沖擊，從而影響了其他軌跡。但通過迭代，兩種初始解均獲得了同一最優(yōu)時(shí)間分配，最終結(jié)果符合最小化加加速度的目標(biāo)。本文所得初始解在第7次迭代接近最終解，耗時(shí)約1.2 s。按路徑比方法所得初始解，在第37次迭代收斂于同一最終解，耗時(shí)約6.5 s。

4.4 實(shí)驗(yàn)驗(yàn)證

使用兩輪差速移動機(jī)器人，利用Matlab與ROS對移動機(jī)器人進(jìn)行聯(lián)合控制并將相關(guān)數(shù)據(jù)發(fā)送到Matlab，在如圖13a所示的環(huán)境進(jìn)行實(shí)驗(yàn)，基于Gmapping方法建立如圖13b所示柵格地圖，設(shè)膨化安全距離0.12 m，vmax=0.15 m/s、amax=0.15 m/s2，wj為0.5，ws為1.05，起點(diǎn)為(0,-0.98 m)，終點(diǎn)為(3.04 m,-1.54 m)。實(shí)際跟隨誤差約為0.015 m,規(guī)劃最高車速0.150 m/s,最大車速跟隨誤差約0.015 m/s。

5 結(jié)論

(1)提出一種平滑JPS路徑規(guī)劃方法，將改進(jìn)后的JPS算法與本文的平滑方法相結(jié)合，對路徑增設(shè)了角度指標(biāo)，以較小的長度犧牲,換取平滑性更為良好的路徑。結(jié)果表明，在不同障礙物密度環(huán)境下SJPS相對平滑后處理的JPS長度減少了0.48%～1.80%，總轉(zhuǎn)折角減少了16.93%～52.75%，搜索時(shí)間增加7.06%～10.60%。與Lazy-Theta*的對比結(jié)果表明，隨著柵格環(huán)境精度的提高，SJPS的搜索時(shí)間增加較為平緩，不會出現(xiàn)搜索時(shí)間急劇增加的情況，且SJPS路徑亦具有良好的平滑性。

(2)使用多段高階多項(xiàng)式對初始路徑進(jìn)行優(yōu)化，結(jié)合余弦函數(shù)提出了一種時(shí)間分配方法，可以更好地加快迭代速度，并結(jié)合SJPS的特點(diǎn)對多項(xiàng)式端點(diǎn)進(jìn)行約束。通過仿真及實(shí)驗(yàn)驗(yàn)證了本文方法的可行性。