關(guān)鍵節(jié)點(diǎn)和平滑處理的PRM路徑優(yōu)化方法

魏念巍，姜媛媛，劉延彬，辛元芳，洪 炎

安徽理工大學(xué) 電氣與信息工程學(xué)院，安徽 淮南 232001

1 引言

路徑規(guī)劃是機(jī)器人研究領(lǐng)域的一項(xiàng)基礎(chǔ)并且關(guān)鍵的技術(shù)。路徑規(guī)劃技術(shù)迅速發(fā)展，研究人員提出諸如單元分解法、快速搜索隨機(jī)樹（Rapidly Exploring Random Tree，RRT）法[1]、人工勢(shì)場(chǎng)法[2]、概率路圖（Probabilistic Roadmap，PRM）[3]法等各種路徑規(guī)劃算法[4]。一些傳統(tǒng)的規(guī)劃算法需要對(duì)圖中的障礙物進(jìn)行建模確定其位置，計(jì)算難度及規(guī)劃路徑變得復(fù)雜?；诓蓸拥腜RM算法可以在含有障礙物的地圖中規(guī)避障礙物，并在自由空間內(nèi)規(guī)劃路徑，降低了計(jì)算難度?；诓蓸拥囊?guī)劃方法不僅應(yīng)用在各種機(jī)器人的路徑規(guī)劃方面[5]，在建筑物疏散路徑規(guī)劃、分支管路自動(dòng)布局[6]、計(jì)算機(jī)動(dòng)畫、生物學(xué)等其他領(lǐng)域也被運(yùn)用[7]。

為了提高PRM 算法中路線圖的連通性，Huang 和Gupta提出應(yīng)用Delaunay三角剖分特性的臨近點(diǎn)選擇策略[8]。Mali提出使用多重樣本節(jié)點(diǎn)以及節(jié)點(diǎn)和地圖路線的有效性等特征提高PRM 的表現(xiàn)力[9]。Janson 等人提出使用某種確定性采樣序列替代隨機(jī)性采樣序列實(shí)現(xiàn)PRM路徑的漸進(jìn)最優(yōu)[10]。

采用基本PRM算法進(jìn)行路徑規(guī)劃所形成的路徑含有的拐點(diǎn)過多，在需大幅轉(zhuǎn)向的位置路徑不夠平滑。機(jī)器人在路徑的拐點(diǎn)處需要調(diào)整姿態(tài)完成轉(zhuǎn)向，過多地調(diào)整姿態(tài)會(huì)使得整體的行駛時(shí)間加長，整體行駛性能下降[11]。初始路徑不夠平滑使得部分路徑不能遵循機(jī)器人的行駛規(guī)律，使得路徑不能確定可行。在此問題的基礎(chǔ)上提出使用D-P算法提取PRM初始路徑節(jié)點(diǎn)中的關(guān)鍵節(jié)點(diǎn)，以降低機(jī)器人調(diào)整姿態(tài)的次數(shù)，從而降低行駛時(shí)間。基本的D-P 算法對(duì)復(fù)雜曲線簡化時(shí)易產(chǎn)生自相交等錯(cuò)誤，閾值的選取對(duì)最終簡化結(jié)果有較大影響，閾值過大會(huì)導(dǎo)致局部區(qū)域失真，閾值過小不能有效化簡曲線。劉波等人提出基于單調(diào)鏈與二分法的D-P 改進(jìn)方法解決壓縮復(fù)雜曲線時(shí)的自相交問題[12]。Zhai 等人將D-P 算法與多路樹模型相結(jié)合用于簡化線性特征質(zhì)量評(píng)估，無需設(shè)置 D-P 算法的閾值[13]。Zhao 和 Shi 考慮D-P算法閾值可變性，根據(jù)船舶尺寸和水域情況選擇閾值實(shí)現(xiàn)對(duì)船舶軌跡的壓縮[14]。路徑的平滑處理使用Clothoid曲線進(jìn)行擬合。Clothoid曲線是一種曲率連續(xù)的平面參數(shù)曲線，可使擬合曲線連續(xù)、平滑[15]，最初在道路以及橋梁的設(shè)計(jì)中應(yīng)用較多，后來被應(yīng)用于機(jī)床加工[16]、無人機(jī)航行軌跡[17]等方面。

2 基本PRM算法

PRM 算法是一種基于圖搜索的方法，基本的PRM算法包括學(xué)習(xí)階段和查詢階段。學(xué)習(xí)階段主要是構(gòu)建路徑網(wǎng)絡(luò)圖，首先在給定地圖的自由空間(Cfree)中隨機(jī)撒點(diǎn)，然后通過局部規(guī)劃器連接節(jié)點(diǎn)，形成路徑網(wǎng)絡(luò)圖。查詢階段是將給定的初始節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)與路徑圖相連接，并通過A*等搜索算法找到起始節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的路徑。

學(xué)習(xí)階段構(gòu)造路徑網(wǎng)絡(luò)圖R(N,E)的過程如下：

算法1 PRM

1.N←NULL

2.E←NULL

3.loop

4.c←a randomly chosen free configuration

5.N←N∪{c}

6.Nc←a set of candidate neighbors ofcchosen fromN

7.for alln∈Nc, in order of increasingD(c,n) do

8.if (c,n) is not same connected component ofEand Δ(c,n)≠NIL then

9.E←E∪(c,n)

10.updateR(N,E)

R(N,E)為無向圖，其中N為隨機(jī)點(diǎn)的點(diǎn)集，E為所有可能兩點(diǎn)連接的路徑集。首先對(duì)R(N,E)初始化，N、E初始狀態(tài)為空。然后在自由空間內(nèi)隨機(jī)撒點(diǎn)，每個(gè)隨機(jī)點(diǎn)要確保機(jī)器人不與障礙物相碰撞。在點(diǎn)集N中選取ni作為ci的鄰域點(diǎn)構(gòu)成鄰域點(diǎn)集Nc，鄰域點(diǎn)的距離要在一定范圍內(nèi)，即Nc={n∈N|D(c,n)＜d}，d為設(shè)定的最大允許距離。使用局部規(guī)劃器連接采樣點(diǎn)c以及其鄰域點(diǎn)n，生成一條無碰撞路徑。如果連接線段與威脅體碰撞，則不能相連，即保持連接線段全部處于自由空間中。若經(jīng)過碰撞檢測(cè)，鄰域點(diǎn)集Nc中存在節(jié)點(diǎn)n能與隨機(jī)采樣點(diǎn)c相連，將相應(yīng)的連線作為圖的邊加入到對(duì)應(yīng)的路線子圖中。若新采樣點(diǎn)與其鄰近點(diǎn)集不存在直線段相連，則把該采樣點(diǎn)作為一個(gè)新的子圖存儲(chǔ)。當(dāng)采樣點(diǎn)可以同時(shí)與多個(gè)子圖連接時(shí)，被連接的子圖就通過新采樣點(diǎn)合并成一個(gè)子圖。隨著采樣點(diǎn)的增多，不同的子圖連接在一起，當(dāng)所有子圖連接成一張路線圖并滿足采樣點(diǎn)密度等其他終止條件時(shí)，路徑網(wǎng)絡(luò)圖構(gòu)建完成。圖1 為PRM 算法在學(xué)習(xí)階段構(gòu)建的路徑網(wǎng)絡(luò)圖。

圖1 路徑網(wǎng)絡(luò)圖

在查詢階段將初始點(diǎn)s和目標(biāo)點(diǎn)g與路徑網(wǎng)絡(luò)中的兩個(gè)節(jié)點(diǎn)ci、cj分別連接，然后在無向路徑網(wǎng)絡(luò)中尋找ci與cj之間的路徑，由此可構(gòu)建s節(jié)點(diǎn)與g節(jié)點(diǎn)之間的路徑。在學(xué)習(xí)階段的關(guān)鍵在于如何尋找s到ci以及g到cj的路徑，可以采用學(xué)習(xí)階段的局部規(guī)劃器的方法來尋找無碰撞路徑。學(xué)習(xí)階段尋找到的一條機(jī)器人路徑如圖2所示。

3 路徑優(yōu)化

PRM 算法在構(gòu)建路徑網(wǎng)絡(luò)圖時(shí)采用隨機(jī)采樣策略，采樣點(diǎn)隨機(jī)分布在自由空間Cfree中，在查詢階段搜索路徑時(shí)依賴于采樣點(diǎn)的分布。若在構(gòu)建網(wǎng)絡(luò)圖時(shí)選擇的采樣點(diǎn)過少，則隨機(jī)采樣策略不能保證采樣點(diǎn)覆蓋整個(gè)自由空間，最終導(dǎo)致規(guī)劃路徑失?。蝗暨x取過多的采樣點(diǎn)，則會(huì)增加算法的計(jì)算量，延長路徑規(guī)劃時(shí)間，路徑節(jié)點(diǎn)個(gè)數(shù)會(huì)大量增加，最終形成的機(jī)器人路徑包含的拐點(diǎn)過多。雖然研究人員提出了在構(gòu)建路徑網(wǎng)絡(luò)圖時(shí)的改進(jìn)方法，如在自由空間內(nèi)采樣時(shí)用均勻采樣代替隨機(jī)采樣以保證采樣點(diǎn)均勻分布在整個(gè)自由空間，或采用高斯采樣或橋測(cè)試等方法增強(qiáng)在障礙物周圍的連通性，提高計(jì)算效率。但為了保證路徑規(guī)劃成功，并不能選取過少的采樣點(diǎn)，故規(guī)劃路徑仍存在拐點(diǎn)過多的問題。由于機(jī)器人需在拐點(diǎn)處調(diào)整姿態(tài)轉(zhuǎn)向，這使得機(jī)器人整體行駛時(shí)間加長。由于規(guī)劃所得的路徑為在路徑網(wǎng)絡(luò)圖中兩節(jié)點(diǎn)間的最短路徑，路徑中存在的斜線多，一些拐角過陡。路徑的曲率突然改變，會(huì)影響到機(jī)器人的整體行駛性能和效率。機(jī)器人不能急轉(zhuǎn)彎，需要一定的角度才能完成轉(zhuǎn)向，初始路徑包含一些機(jī)器人無法遵循的急轉(zhuǎn)彎。針對(duì)這一問題，提出一種路徑優(yōu)化方法，使用D-P算法以及Clothoid 曲線提取路徑關(guān)鍵節(jié)點(diǎn)并進(jìn)行平滑處理。優(yōu)化過程如圖3所示。

圖2 PRM路徑

圖3 路徑優(yōu)化過程

在一給定障礙地圖中，首先使用PRM 算法進(jìn)行路徑規(guī)劃，構(gòu)建自由空間內(nèi)的路徑網(wǎng)絡(luò)圖，然后通過查詢階段得到一條從起始點(diǎn)到目標(biāo)點(diǎn)的初始機(jī)器人路徑。對(duì)于初始路徑中的路徑節(jié)點(diǎn)使用D-P 算法提取其中的關(guān)鍵節(jié)點(diǎn)，連接路徑關(guān)鍵節(jié)點(diǎn)形成的是含有較少拐點(diǎn)的優(yōu)化路徑。使用Clothoid 曲線進(jìn)行擬合可以得到最終的比較平滑的機(jī)器人路徑。

3.1 關(guān)鍵節(jié)點(diǎn)的提取

在PRM算法形成的初始路徑中，路徑節(jié)點(diǎn)過多，機(jī)器人在拐點(diǎn)處需要調(diào)整姿態(tài)轉(zhuǎn)彎，拐點(diǎn)過多使得路徑過長，機(jī)器人行駛時(shí)間加長。提取路徑節(jié)點(diǎn)中的關(guān)鍵節(jié)點(diǎn)進(jìn)行路徑優(yōu)化可以有效減少路徑的總長以及機(jī)器人行駛時(shí)間。

算法2 D-P算法

1.Function DP=D-P_original({N1,N2,…,Nn},φ)

2.DP=NULL

2.Fit a linel withN1andNn

4.D←{d1,d2,…,dn} //求各節(jié)點(diǎn){N1,N2,…,Nn}到基準(zhǔn)線l的距離{d1,d2,…,dn}

5.for alldi∈Ddo

6.dmax←max{d1,d2,…,dn} and pointNmaxcorresponding todmax

7.ifdmax≤φthen

8.DP←{DP,N1,Nn}

9.else

10.DP←{DP,D-P_max(N1,Nmax)} and DP←{DP,D-P_max(Nmax,Nn)}

11.end if

12.return DP

使用D-P算法提取路徑關(guān)鍵節(jié)點(diǎn)的基本步驟如下：首先確定一閾值φ，連接初始路徑節(jié)點(diǎn)的初始點(diǎn)和目標(biāo)點(diǎn)形成一條基準(zhǔn)線。然后計(jì)算初始點(diǎn)和目標(biāo)點(diǎn)之間所有節(jié)點(diǎn)到基準(zhǔn)線的距離，得到距離基準(zhǔn)線最遠(yuǎn)的節(jié)點(diǎn)。比較此距離與預(yù)先給定的閾值φ的大小。如果小于φ，則該基準(zhǔn)線段作為新的路徑，該段路徑處理完畢。如果距離大于給定的閾值φ，把此節(jié)點(diǎn)納入關(guān)鍵節(jié)點(diǎn)集，該關(guān)鍵節(jié)點(diǎn)分別與初始點(diǎn)和目標(biāo)點(diǎn)相連接形成兩條新的基準(zhǔn)線，并對(duì)這兩段基準(zhǔn)線重復(fù)上面步驟提取新的關(guān)鍵節(jié)點(diǎn)。最后可以得到關(guān)鍵節(jié)點(diǎn)集，依次連接關(guān)鍵節(jié)點(diǎn)，即可得到新的優(yōu)化路徑。閾值的選取對(duì)路徑優(yōu)化的影響很大，閾值過大雖然會(huì)有效減少拐點(diǎn)個(gè)數(shù)，但無法確保優(yōu)化路徑可行性；閾值過小不能有效減少路徑中拐點(diǎn)的個(gè)數(shù)。故需根據(jù)地圖障礙物的分布情況以及初始路徑節(jié)點(diǎn)個(gè)數(shù)來確定D-P算法的閾值，在含有較多障礙物的復(fù)雜地圖中，選取較小的閾值有助于保證優(yōu)化路徑成功可行；在含較少障礙物或障礙物分布簡單的地圖中，可選擇較大閾值提取路徑節(jié)點(diǎn)中關(guān)鍵節(jié)點(diǎn)。

以圖4 的路徑為例，初始路徑節(jié)點(diǎn)為A1～A8。設(shè)定一合適閾值，連接A1、A8，在A1和A8之間的路徑節(jié)點(diǎn)中距離線段A1A8最遠(yuǎn)的節(jié)點(diǎn)為A3,A3與A1A8的距離為d，大于預(yù)先設(shè)定的閾值φ，把A3視為一個(gè)關(guān)鍵節(jié)點(diǎn)。分別連接A1、A3和A3、A8形成兩條新的基準(zhǔn)線段A1A3和A3A8。由圖4（b）可以看出，在A1與A3之間與線段A1A3距離最遠(yuǎn)的路徑節(jié)點(diǎn)為A2，在A3和A8之間距離線段A3A8最遠(yuǎn)的路徑節(jié)點(diǎn)為A5，分別與閾值相比較，找出新的路徑關(guān)鍵節(jié)點(diǎn)。依次重復(fù)下去可以得到路徑關(guān)鍵節(jié)點(diǎn)集，連接路徑關(guān)鍵節(jié)點(diǎn)即可得到圖4（c）所示的新路徑B1—B2—B3—B4。

圖4 D-P算法提取路徑關(guān)鍵節(jié)點(diǎn)

3.2 路徑平滑處理

Clothoid曲線是一種參數(shù)平面螺旋曲線。該曲線的最大特點(diǎn)是：以弧長為參變量及曲率連續(xù)變化，并囊括直線與圓兩種特例。Clothoid曲線的曲率變化與曲線的弧長成正比，Clothoid曲線表達(dá)式如式（1）：

式中，(x0,y0)為初始點(diǎn)，θ0表示初始切線角，k0為初始曲率，c表示曲率銳度的參數(shù)，s表示曲線的弧長。Clothoid曲線的曲率k以及切線角可以表示為式（2）：

Clothoid曲線大多應(yīng)用在根據(jù)已知的一系列離散坐標(biāo)點(diǎn)，在特定的條件下生成曲率連續(xù)變化的曲線。若設(shè)D-P算法提取的關(guān)鍵節(jié)點(diǎn)是坐標(biāo)為P(xi,yi)(i=1,2,…,k)的一組離散點(diǎn)，那么使用Clothoid 曲線進(jìn)行擬合，就是在曲率連續(xù)的條件下求解上述k個(gè)離散點(diǎn)的各Clothoid曲線段。對(duì)一組離散點(diǎn)用Clothoid 曲線進(jìn)行擬合形成的曲線如圖5所示。

對(duì)于其中第l段Clothoid 曲線來說，其兩端端點(diǎn)坐標(biāo)為(xl,yl)、(xl+1,yl+1)，由式（1）可知兩點(diǎn)應(yīng)滿足下列條件：

圖5 一組離散點(diǎn)和Clothoid曲線

其中，sl為第l個(gè)Clothoid曲線段的弧長，θ0l以及k0l分別表示(xl,yl)點(diǎn)處的切線角和曲率。為了保證在各個(gè)點(diǎn)處的曲率連續(xù)性和曲線平滑性，(xl+1,yl+1)作為第l段Clothoid曲線的終點(diǎn)以及第l+1 段Clothoid曲線的起點(diǎn)，此處各參數(shù)應(yīng)當(dāng)滿足：

式中，θ0l+1、k0l+1分別表示第l+1 段Clothoid曲線的初始切線角和初始曲率，分別與第l段Clothoid 曲線在(xl+1,yl+1)處的切線角θl以及曲率kl相等。即在兩段Clothoid曲線的連接處的切線角和曲率的大小不變。

4 仿真結(jié)果

使用Matlab進(jìn)行仿真，在如圖6所示的含障礙物的250 m×250 m大小的地圖中，設(shè)定機(jī)器人的起始點(diǎn)坐標(biāo)為（10，240），機(jī)器人的目標(biāo)點(diǎn)坐標(biāo)為（240，10）。選擇隨機(jī)點(diǎn)的個(gè)數(shù)為500，使用基本PRM方法進(jìn)行路徑規(guī)劃得到的初始機(jī)器人路徑如圖6 所示。圖中的節(jié)點(diǎn)為機(jī)器人路徑的路徑節(jié)點(diǎn)，連線為初始路徑。

圖6 PRM路徑規(guī)劃的初始路徑

由圖6可以看出，經(jīng)基本PRM方法進(jìn)行路徑規(guī)劃得到的初始路徑共有14 個(gè)路徑節(jié)點(diǎn)，在此基礎(chǔ)上進(jìn)行路徑優(yōu)化，選擇D-P算法的閾值為4，提取路徑節(jié)點(diǎn)中的關(guān)鍵節(jié)點(diǎn)，并使用Clothoid 曲線對(duì)路徑關(guān)鍵節(jié)點(diǎn)進(jìn)行擬合，得到最終較為平滑的新路徑如圖7所示。圖中點(diǎn)劃線路徑為初始路徑，星點(diǎn)為經(jīng)D-P算法提取的路徑節(jié)點(diǎn)中的關(guān)鍵節(jié)點(diǎn)，虛線為關(guān)鍵節(jié)點(diǎn)組成的新路徑，實(shí)線路徑為經(jīng)Clothoid曲線平滑處理過后的平滑優(yōu)化路徑。

圖7 經(jīng)優(yōu)化后的平滑路徑

由圖7 可知，經(jīng)D-P 算法提取路徑節(jié)點(diǎn)中的關(guān)鍵節(jié)點(diǎn)，路徑節(jié)點(diǎn)由原先的14 個(gè)縮減為5 個(gè)關(guān)鍵節(jié)點(diǎn)，有效減少了機(jī)器人路徑中拐點(diǎn)的個(gè)數(shù)，經(jīng)Clothoid曲線平滑處理后的最終路徑與初始路徑相比更加平滑。

在圖8所示的較為復(fù)雜的250 m×250 m大小的障礙地圖中，選擇機(jī)器人的起始點(diǎn)坐標(biāo)為（10，10），目標(biāo)點(diǎn)坐標(biāo)選為（10，120），由PRM 方法進(jìn)行路徑規(guī)劃后得到的初始路徑如圖8所示。

圖8 較復(fù)雜地圖PRM路徑規(guī)劃的初始路徑

由圖8 可以看出，機(jī)器人路徑共有27 個(gè)路徑節(jié)點(diǎn)，經(jīng)過圓形障礙物時(shí)需大幅轉(zhuǎn)向行駛。初始路徑在此處的拐角過陡，有突出棱角，機(jī)器人需在拐點(diǎn)處調(diào)整姿態(tài)來完成轉(zhuǎn)向，路徑不夠平滑，使得機(jī)器人的行駛時(shí)間加長。選擇D-P 算法的閾值為4，提取路徑節(jié)點(diǎn)中的關(guān)鍵節(jié)點(diǎn)，再經(jīng)平滑處理后的最終路徑如圖9所示。

圖9 較復(fù)雜地圖經(jīng)優(yōu)化后的平滑路徑

由圖9可知，優(yōu)化后的平滑路徑的路徑節(jié)點(diǎn)由原先的27 個(gè)減少為12 個(gè)，有效減少了機(jī)器人行駛過程中的拐點(diǎn)個(gè)數(shù)。在圓形障礙物處的機(jī)器人路徑變得更加平滑，無較突出的棱角。經(jīng)過優(yōu)化后的最終路徑整體比較平滑，能有效減少機(jī)器人的行駛時(shí)間，并提高機(jī)器人的整體行駛性能。

在圖10 的障礙地圖中，設(shè)機(jī)器人初始點(diǎn)坐標(biāo)為（10，10），目標(biāo)點(diǎn)坐標(biāo)為（10，240），機(jī)器人行駛路徑較復(fù)雜。使用基本的PRM方法進(jìn)行路徑規(guī)劃得到的初始路徑如圖10所示。

圖10 復(fù)雜路徑經(jīng)PRM路徑規(guī)劃的初始路徑

初始路徑中的路徑節(jié)點(diǎn)為23 個(gè)，由于路徑較為復(fù)雜，初始路徑含有較多的尖銳拐角，有較多的急轉(zhuǎn)彎，這使得機(jī)器人在行駛過程中需要經(jīng)常大幅度調(diào)整姿態(tài)形成轉(zhuǎn)向，機(jī)器人的整體行駛時(shí)間加長。選擇D-P算法的閾值為3，提取路徑關(guān)鍵節(jié)點(diǎn)，Clothoid曲線進(jìn)行平滑處理得到的平滑路徑如圖11所示。

經(jīng)優(yōu)化后的最終路徑的路徑節(jié)點(diǎn)的個(gè)數(shù)由23個(gè)減為14 個(gè)。最終路徑與初始路徑相比，在較為尖銳的拐角處更加平滑，能夠有效減少機(jī)器人在拐角處調(diào)整姿態(tài)所需的時(shí)間。經(jīng)優(yōu)化處理后的最終路徑與初始路徑相比，拐點(diǎn)個(gè)數(shù)明顯減少，平滑性明顯提高，可以有效降低機(jī)器人的整體行駛時(shí)間。

表1 PRM初始路徑和優(yōu)化平滑路徑實(shí)驗(yàn)數(shù)據(jù)比較

圖11 復(fù)雜路徑經(jīng)優(yōu)化后的平滑路徑

由表1可以看出，相較于基本PRM規(guī)劃路徑形成的初始路徑，經(jīng)優(yōu)化后的最終平滑路徑的路徑節(jié)點(diǎn)個(gè)數(shù)以及路徑轉(zhuǎn)折次數(shù)明顯減少，相應(yīng)地機(jī)器人的拐點(diǎn)個(gè)數(shù)也會(huì)降低。與簡單障礙地圖相比，較復(fù)雜的障礙地圖以及復(fù)雜路徑障礙地圖中初始PRM路徑節(jié)點(diǎn)以及路徑轉(zhuǎn)折次數(shù)較多，經(jīng)優(yōu)化處理后的平滑路徑可以有效降低機(jī)器人拐點(diǎn)的個(gè)數(shù)。

5 結(jié)束語

移動(dòng)機(jī)器人路徑規(guī)劃使用基本PRM算法生成的初始路徑中拐點(diǎn)個(gè)數(shù)過多，采用D-P算法提取初始路徑節(jié)點(diǎn)中的關(guān)鍵節(jié)點(diǎn)，從而減少機(jī)器人行駛路徑拐點(diǎn)的個(gè)數(shù)。針對(duì)路徑部分拐角過陡和路徑不夠平滑的問題，使用Clothoid曲線平滑路徑。仿真結(jié)果表明，在復(fù)雜路徑或復(fù)雜障礙地圖情況下該優(yōu)化方法都可有效減少路徑中拐點(diǎn)的個(gè)數(shù)并使路徑更加平滑。