王素芳，安傳旭，蔣文婷，楊 林，王浩楓

(1.中國(guó)航天科工集團(tuán)第二研究院 七〇六所，北京 100854；2.中國(guó)科學(xué)院 軟件研究所，北京 100190；3.中國(guó)空間技術(shù)研究院 通信與導(dǎo)航衛(wèi)星總體部，北京 100094)

0 引 言

ROS自主移動(dòng)機(jī)器人[1]需要解決定位、導(dǎo)航、規(guī)劃、控制等問(wèn)題，導(dǎo)航是其核心技術(shù)，導(dǎo)航主要基于ROS的move_base框架來(lái)實(shí)現(xiàn)，導(dǎo)航[2-4]就是讓機(jī)器人可以自主按照內(nèi)部預(yù)定的信息，或依據(jù)傳感器獲取外部環(huán)境進(jìn)行相應(yīng)的引導(dǎo)，從而規(guī)劃出一條適合機(jī)器人行走的路徑。因此，導(dǎo)航成功的關(guān)鍵在于路徑規(guī)劃[5,6]，路徑規(guī)劃由全局路徑規(guī)劃和局部路徑規(guī)劃[7]組成。全局路徑規(guī)劃使用路徑規(guī)劃算法，在二維柵格地圖中規(guī)劃出一條可行路徑，而局部路徑規(guī)劃則是在全局路徑的基礎(chǔ)上做出動(dòng)態(tài)的規(guī)劃和運(yùn)動(dòng)控制，以達(dá)到實(shí)時(shí)避障的效果[8,9]。

常見(jiàn)的局部路徑規(guī)劃器有DWA(dynamic window approach)[10]動(dòng)態(tài)窗口法、TEB(timed elastic Band)[11]時(shí)間彈性帶、pure pursuit跟蹤算法[12]、MPC(model predictive control)[13]模型預(yù)測(cè)控制和Eband[14]等。TEB[15,16]是起始點(diǎn)、目標(biāo)點(diǎn)狀態(tài)由用戶/全局規(guī)劃器指定，中間插入N個(gè)橡皮筋形狀的控制點(diǎn)(機(jī)器人姿態(tài))，為了顯示軌跡的運(yùn)動(dòng)學(xué)信息，在點(diǎn)與點(diǎn)之間定義運(yùn)動(dòng)時(shí)間Time，即為TEB算法，TEB[17]可以做到對(duì)前方一段軌跡進(jìn)行優(yōu)化，對(duì)動(dòng)態(tài)障礙物的避障效果較好，但TEB算法計(jì)算復(fù)雜度較高，在控制過(guò)程中速度和角度波動(dòng)較大。Eband[18]是一種類似于人工勢(shì)場(chǎng)形成引力與斥力的方法生成彈性帶(elastic band)進(jìn)行局部規(guī)劃，以提高機(jī)器人行走的速度與動(dòng)作的連貫性。

針對(duì)規(guī)劃器缺少對(duì)機(jī)器人運(yùn)動(dòng)的考慮，以及在遇到動(dòng)態(tài)障礙物時(shí)，即使全局規(guī)劃器可以重新規(guī)劃路徑，但也會(huì)出現(xiàn)當(dāng)前位置偏離規(guī)劃出的路徑的情況[19,20]。本文采用Eband局部路徑規(guī)劃器來(lái)實(shí)現(xiàn)動(dòng)態(tài)避障，使用Eband算法來(lái)優(yōu)化前面所規(guī)劃出來(lái)的路徑，但其運(yùn)動(dòng)控制隨著“氣泡”的波動(dòng)變化，導(dǎo)致路徑有時(shí)不夠平滑，因此，本文主要針對(duì)Eband的運(yùn)動(dòng)控制平滑性和機(jī)器人運(yùn)動(dòng)卡頓等急需解決的關(guān)鍵問(wèn)題，進(jìn)行算法優(yōu)化，并通過(guò)代碼實(shí)現(xiàn)，將優(yōu)化后的算法與原Eband算法進(jìn)行對(duì)比，驗(yàn)證了本文方法的有效性。

1 Eband算法實(shí)現(xiàn)原理

Eband算法輸入為全局的路徑、局部costmap、機(jī)器人當(dāng)前位姿；輸出為控制速度(角速度與線速度)。Eband 采用氣泡表示，每個(gè)氣泡均處在free space里面，并且每個(gè)氣泡與相鄰的2個(gè)氣泡重疊。每個(gè)氣泡的半徑是機(jī)器人與障礙物的最短距離，氣泡通過(guò)計(jì)算合力(相鄰的點(diǎn)產(chǎn)生的引力與障礙物產(chǎn)生的斥力)決定其位置。通過(guò)下一個(gè)氣泡的中心位置，計(jì)算得出控制的線速度與角速度。

氣泡“bubble”就是一個(gè)代表機(jī)器人路徑上的點(diǎn)與障礙物不碰撞的圓形區(qū)域，距離障礙物近時(shí)，氣泡小，距離障礙物遠(yuǎn)時(shí)，氣泡就會(huì)隨之變大。定義氣泡函數(shù)B(b)

(1)

其中，b表示當(dāng)前氣泡的中心，q表示當(dāng)前氣泡中的任意隨機(jī)位置，ρ(b) 表示氣泡以b為圓心的圓的半徑。

具體流程如圖1所示。

圖1 路徑規(guī)劃

圖1(a)為機(jī)器人全局路徑規(guī)劃器規(guī)劃的路徑，圖1(b)為使用Eband局部路徑規(guī)劃算法之后的路徑。為了改善路徑的形狀，主要有兩個(gè)力，一個(gè)是收緊的力，一個(gè)是障礙物產(chǎn)生的斥力，直到二力平衡，讓原本松弛的路徑看上去緊繃起來(lái)。圖1(c)和圖1(d)是通過(guò)Eband局部路徑規(guī)劃器規(guī)劃后的路徑隨著機(jī)器人運(yùn)動(dòng)而根據(jù)環(huán)境中障礙物做出相應(yīng)調(diào)整后的路徑。

之后就是對(duì)產(chǎn)生的氣泡做處理，比如將氣泡前移或后移，使其在距離障礙物近的地方生成的氣泡密集,在距離障礙物遠(yuǎn)的地方生成的氣泡稀疏。氣泡沿著路徑產(chǎn)生和隨著障礙物改變情況如圖2所示。

圖2 氣泡沿著路徑產(chǎn)生和隨著障礙物改變情況

如圖2所示，空心圓表示障礙物，障礙物靠近導(dǎo)致附近的氣泡變小變密，障礙物遠(yuǎn)離后，氣泡變大變稀疏。但是在機(jī)器人附近突然出現(xiàn)障礙物時(shí)，氣泡會(huì)出現(xiàn)跳變的現(xiàn)象；或者機(jī)器人在有垃圾桶、消防栓、雜物等障礙物的狹窄走廊里移動(dòng)時(shí)，以及在狹窄的路段突然轉(zhuǎn)彎時(shí)，氣泡會(huì)變小，氣泡太小就可能存在band斷裂，此時(shí)機(jī)器人會(huì)重新規(guī)劃路徑和插值，局部路徑目標(biāo)點(diǎn)就會(huì)發(fā)生變化，就會(huì)引起機(jī)器人運(yùn)動(dòng)卡頓。

2 改進(jìn)的Eband算法

針對(duì)Eband氣泡在特定場(chǎng)景下的跳變，會(huì)直接導(dǎo)致局部路徑當(dāng)前目標(biāo)點(diǎn)的跳動(dòng)，以及運(yùn)動(dòng)控制速度的波動(dòng)，從而導(dǎo)致機(jī)器人運(yùn)動(dòng)卡頓不平滑問(wèn)題。為了解決該問(wèn)題，本文主要從曲率算法優(yōu)化、速度乘積因子去噪和速度插值3個(gè)方面進(jìn)行了優(yōu)化。

2.1 曲率算法優(yōu)化

針對(duì)突然出現(xiàn)的障礙物引起氣泡半徑的瞬間變化，導(dǎo)致運(yùn)動(dòng)曲率驟變問(wèn)題，做了曲率計(jì)算優(yōu)化，平滑大幅度的曲率變動(dòng)引起機(jī)器人線速度的驟減。

在生成的局部路徑上，截取3個(gè)氣泡，分別是band[0]，band[1]，band[2]，band[0] 代表的是機(jī)器人的位姿，后續(xù)兩個(gè)氣泡代表機(jī)器人后續(xù)的兩個(gè)位姿。那么band[0]，band[1] 和band[2] 就對(duì)應(yīng)pose[0]，pose[1] 和pose[2]。 利用每個(gè)pose中的x,y坐標(biāo)，就可以求出前后兩個(gè)pose之間的角度變化。

(2)

其中

x1=pose[1].pose.position.x-pose[0].pose.position.x
y1=pose[1].pose.position.y-pose[0].pose.position.y
x2=pose[2].pose.position.x-pose[1].pose.position.x
y2=pose[2].pose.position.y-pose[1].pose.position.y

本文用向量的夾角余弦值作為自身的曲率

(3)

將式(3)展開(kāi)式即得到式(4)

(4)

2.2 速度乘積因子去噪

針對(duì)狹窄路段，Eband速度乘積因子出現(xiàn)噪點(diǎn)，引起機(jī)器人運(yùn)動(dòng)卡頓問(wèn)題，做了狹窄路段速度乘積因子線性去噪。

速度乘積的影響主要取決于環(huán)境的狹窄度，當(dāng)環(huán)境過(guò)于狹窄時(shí)，Eband的對(duì)應(yīng)氣泡半徑就會(huì)急速減少，這時(shí)候線速度也應(yīng)該對(duì)應(yīng)減速，保障安全。算法輸出線速度公式為

Max_vel_lin=max_vel_lin_*curvature_factor*
velocity_multiplier

(5)

式中：max_vel_lin_是曲率因子，表示轉(zhuǎn)彎弧度對(duì)線速度的影響，velocity_multiplier表示空間狹窄度、氣泡半徑大小對(duì)線速度的影響

Scale=4-1.2/max_vel_lin_

(6)

式中：Scale表示環(huán)境的影響因子權(quán)重，當(dāng)最大速度設(shè)置的越大時(shí)，環(huán)境的影響因子權(quán)重也就越大，該參數(shù)的選取主要是根據(jù)機(jī)器人的應(yīng)用場(chǎng)景來(lái)決定，本文以最大線速度max_vel_lin_的取值范圍 [0.3,1] 為依據(jù)，為了約束Scale的取值范圍在 [0,2.8]， 設(shè)置了線性方程(6)，保證在速度很低的時(shí)候，Scale的影響為0，隨著速度的提高，環(huán)境因子的制約逐步擴(kuò)大，最多擴(kuò)大到3，以適應(yīng)機(jī)器人的應(yīng)用場(chǎng)景

Offset=0.42/(max_vel_lin_-0.4)

(7)

式中：Offset表示為了平衡整個(gè)速度乘積，限制速度乘積的值域，用來(lái)控制最大最小速度范圍，由于最大線速度的取值范圍在 [0.3,1]， 為了在速度最低時(shí)候，速度乘積影響達(dá)到最大，滿足值域 [0.02,1]， 選取了式(7)中的Offset的線性參數(shù)；

速度乘積的計(jì)算公式為

Velocity_multiplier=Scale*bubble_radius+Offset

(8)

從式(8)可以看到，速度乘積Velocity_multiplier由Scale和氣泡半徑bubble_radius以及Offset來(lái)決定，氣泡半徑最小的時(shí)候，Velocity_multiplier達(dá)到最小，表示周圍環(huán)境比較復(fù)雜；反之氣泡半徑最大的時(shí)候，表示周圍環(huán)境比較空曠安全，速度乘積達(dá)到最大；該因子的值域是 [0.4285,1]。

2.3 速度插值法

針對(duì)氣泡的跳變導(dǎo)致的局部路徑目標(biāo)點(diǎn)跳變問(wèn)題，本文進(jìn)行氣泡插值來(lái)規(guī)避跳變，為了機(jī)器人可以更平滑的輸出到達(dá)局部目標(biāo)點(diǎn)的速度，同時(shí)兼顧機(jī)器人避開(kāi)障礙物的反應(yīng)靈敏度，根據(jù)機(jī)器人的電機(jī)反應(yīng)能力，選取機(jī)器人前方0.2 m距離處作為插值氣泡中心，此值根據(jù)不同電機(jī)的性能做不同的選取，一般的范圍在 [0.1,0.3]。 根據(jù)此處在band上前后氣泡的半徑，算出插值氣泡半徑。插值后的氣泡作為局部路徑目標(biāo)點(diǎn)進(jìn)行迭代更新。代碼如下：

算法1：Interpolate and update bands

輸入：planned path: plan=(bubble0,bubble1…bubblen).

輸出：The target point for output: goal

(1)While (goal==empty) do

(2) band=convert(plan); //Plan is converted to band

(3) bubble_center=FindCenter (0.2, band); //Find the bubble center at 0.2 m

(4) r=FindMinDisBubble (band); //The nearest bubble radius is the radius

(5) InsertBubble (r, bubble_center, band); //Insert the bubble into the band

(6) SetLocalGoal (bubble_center); //Set the center of the bubble as the local goal

(7) PublishGoal (goal); // Publish new goal

(8)return goal;

(9)End.

3 實(shí)驗(yàn)分析

3.1 實(shí)驗(yàn)數(shù)據(jù)集

本文對(duì)比實(shí)驗(yàn)驗(yàn)證所用數(shù)據(jù)集為在同一場(chǎng)景下包含回形走廊、若干固定障礙物所錄制的bag，訂閱了/cmd_vel話題以及/vel_data話題，地圖大小為20 m*20 m。并且保證錄制環(huán)境中除了回形走廊、若干障礙物以外，其它影響因素可以忽略不計(jì)。

3.2 實(shí)驗(yàn)軟硬件配置

本文優(yōu)化后的算法與原Eband算法均在錄制的同一數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)驗(yàn)證。軟件環(huán)境為：Ubuntu18.04+ROS melodic。硬件環(huán)境使用Firefly-RK3399Pro開(kāi)發(fā)板進(jìn)行代碼部署，硬件部署環(huán)境如圖3所示。

圖3 Firefly-RK3399Pro開(kāi)發(fā)板

在進(jìn)行路徑規(guī)劃實(shí)驗(yàn)時(shí)，硬件環(huán)境采用國(guó)產(chǎn)高性能AI處理芯片RK3399Pro，集成神經(jīng)網(wǎng)絡(luò)處理器NPU，算力高達(dá)3.0 Tops，兼容多種AI框架。此芯片采用了ARM雙核Cortex-A72+四核Cortex-A53的大小核處理器架構(gòu)，主頻高達(dá)1.8 GHz，集成四核ARM高端GPU Mali-T860 MP4圖形處理器，通用運(yùn)算性能強(qiáng)悍，整體性能優(yōu)異。

ROS是一個(gè)開(kāi)源的機(jī)器人操作系統(tǒng)軟件框架。它提供了包括硬件抽象、常用功能的實(shí)現(xiàn)、進(jìn)程之間的包管理、低級(jí)設(shè)備控制和消息傳遞等希望從操作系統(tǒng)獲得的服務(wù)。它還提供了各種工具和庫(kù)來(lái)編寫和運(yùn)行代碼。

ROS的主要特點(diǎn)是：①分布式架構(gòu)，ROS通過(guò)節(jié)點(diǎn)(Nodes)進(jìn)行點(diǎn)對(duì)點(diǎn)的設(shè)計(jì)，可執(zhí)行文件可以單獨(dú)運(yùn)行；②多語(yǔ)言支持，ROS是一個(gè)語(yǔ)言中立性的軟件框架，支持C++、Python和LISP等多種不同的語(yǔ)言；③工具包豐富，諸如可視化和調(diào)試工具、仿真環(huán)境等是ROS中最為常用也最為重要的幾個(gè)工具；④免費(fèi)開(kāi)源。

基于ROS的上述特點(diǎn)，在ROS中實(shí)現(xiàn)機(jī)器人路徑規(guī)劃是非常方便的，ROS還可以解決傳感器驅(qū)動(dòng)、顯示和算法之間的溝通協(xié)調(diào)問(wèn)題，而Ubuntu是Linux系統(tǒng)的一個(gè)發(fā)行版本，具有強(qiáng)大的防御病毒功能并且免費(fèi)，因此本文所涉及到的算法均在Ubuntu18.04+ROS melodic機(jī)器人操作系統(tǒng)下編程實(shí)現(xiàn)，代碼使用C++語(yǔ)言進(jìn)行編寫。實(shí)驗(yàn)對(duì)比圖使用ROS自帶的rqt-plot繪圖工具進(jìn)行數(shù)據(jù)曲線比較。

3.3 實(shí)驗(yàn)數(shù)據(jù)對(duì)比

3.3.1 平滑曲率算法優(yōu)化

本節(jié)實(shí)驗(yàn)中優(yōu)化了曲率限制對(duì)于速度的抑制效果突兀問(wèn)題，對(duì)比不同曲率范圍時(shí)速度下降的效果，其中d為前后曲率的變化data。參數(shù)介紹如下：

當(dāng)d<1.0時(shí)，如圖4所示。

圖4 d<1.0時(shí)曲率對(duì)各參數(shù)的影響曲線

由圖4可以看出，當(dāng)d<1.0時(shí)，可見(jiàn)曲率因子的變化，會(huì)引起線速度的劇烈變化，對(duì)原算法輸出的線速度的影響效果較為明顯。

當(dāng)d<0.2時(shí)，如圖5所示。

圖5 d<0.2時(shí)曲率對(duì)各參數(shù)的影響曲線

由圖5可以看出，當(dāng)d<0.2時(shí)，有效限制了曲率因子的斷崖式變化。

當(dāng)d<0.1時(shí)，如圖6所示。

圖6 d<0.1時(shí)曲率對(duì)各參數(shù)的影響曲線

由圖6可以看出，曲率因子的波動(dòng)基本穩(wěn)定，但是對(duì)原算法輸出的線速度的影響效果也越來(lái)越小，影響最大的因子變成了速度乘積因子。

當(dāng)d<0.15時(shí)，如圖7所示。

圖7 d<0.15時(shí)曲率對(duì)各參數(shù)的影響曲線

由圖7可以看出，機(jī)器人運(yùn)動(dòng)底盤里程計(jì)線速度和算法輸出的最大線速度曲線基本擬合，此時(shí)，曲率因子的影響效果達(dá)到比較優(yōu)秀的效果。

平滑曲率前后的整體效果曲線對(duì)比，如圖8和圖9所示。

圖8 平滑曲率前整體效果曲線

圖9 平滑曲率后整體效果曲線

由圖8和圖9可以看出，針對(duì)突然出現(xiàn)的障礙物引起氣泡半徑的瞬間變化，導(dǎo)致運(yùn)動(dòng)曲率驟變的問(wèn)題，由于曲率影響著轉(zhuǎn)彎時(shí)候的最大線速度，本文算法對(duì)曲率參數(shù)的變化梯度做了優(yōu)化，使得在突然出現(xiàn)障礙物和在障礙物周圍轉(zhuǎn)彎時(shí)的線速度更加的平滑，有效地抑制了大幅度的曲率變動(dòng)引起機(jī)器人線速度驟減的情況發(fā)生。

3.3.2 速度乘積因子去噪

當(dāng)在機(jī)器人在狹窄空間內(nèi)運(yùn)動(dòng)，由于環(huán)境本身比較狹窄，Eband生成的氣泡半徑可能很少達(dá)到最大，機(jī)器人此時(shí)的線速度受到了速度乘積因子的直接影響。在此場(chǎng)景下，若突然出現(xiàn)靜態(tài)或動(dòng)態(tài)的障礙物，band對(duì)應(yīng)的氣泡半徑存在小于0.1 m的可能，而在Eband的速度設(shè)置中，氣泡半徑小于0.1的時(shí)候線速度已經(jīng)相當(dāng)微小，而真實(shí)場(chǎng)景中機(jī)器人又需要繼續(xù)緩慢平穩(wěn)向前，要實(shí)現(xiàn)這一目標(biāo)，氣泡半徑小于0.1 m時(shí)速度因子的影響對(duì)于線速度而言就成為了噪點(diǎn)，本實(shí)驗(yàn)對(duì)此問(wèn)題進(jìn)行了去噪工作：

優(yōu)化前如圖10所示。

圖10 速度乘積因子去噪前各參數(shù)曲線

觀察圖10，本實(shí)驗(yàn)設(shè)置了線速度的最小值為0.3 m/s，但是由于噪點(diǎn)存在，算法輸出的線速度出現(xiàn)了很多低于0.3的極值點(diǎn)，這些極值點(diǎn)是速度乘積瞬間值造成的，是無(wú)效不可用的點(diǎn)，對(duì)線速度的平滑性也有直接影響，需要進(jìn)行優(yōu)化去噪。

優(yōu)化后如圖11所示。

圖11 速度乘積因子去噪后各參數(shù)曲線

觀察圖11，可以看到算法輸出的線速度已經(jīng)沒(méi)有低于0.3的極值點(diǎn)，噪點(diǎn)被去除。速度乘積主要影響著機(jī)器人經(jīng)過(guò)狹窄空間時(shí)的運(yùn)動(dòng)，限制速度乘積在短時(shí)間氣泡很小的時(shí)候的影響力，減少由此造成的線速度波動(dòng)，使得機(jī)器人在狹窄空間運(yùn)動(dòng)的時(shí)候，速度更加平滑。

3.3.3 局部目標(biāo)點(diǎn)插值

經(jīng)過(guò)曲率和速度乘積的優(yōu)化后，Eband算法輸出的線速度已經(jīng)比較平穩(wěn)。Eband在局部路徑規(guī)劃和控制中，采取了分段控制的策略。首先得到全局路徑規(guī)劃的曲線后，先由設(shè)置的局部代價(jià)地圖大小，截取相應(yīng)長(zhǎng)度的全局點(diǎn)作為局部路徑目標(biāo)點(diǎn)，然后由算法生成氣泡連成的band，該band可以轉(zhuǎn)化成path，也即是局部路徑。然后交給速度控制，速度控制以機(jī)器人最近的氣泡中心作為臨時(shí)目標(biāo)點(diǎn)，進(jìn)行短距離的運(yùn)動(dòng)，這也是Eband可以快速應(yīng)對(duì)突發(fā)障礙物的原因之一。

但是此種方式也存在一定的弊端，首先機(jī)器人在不斷的運(yùn)動(dòng)中，局部路徑規(guī)劃器也在不停的規(guī)劃，面對(duì)動(dòng)態(tài)的障礙物時(shí)，算法生成的氣泡和路徑都在動(dòng)態(tài)更新，這就意味著局部臨時(shí)目標(biāo)點(diǎn)可能會(huì)發(fā)生改變。局部臨時(shí)氣泡中心的跳變直接影響到角速度和線速度的控制，也就造成了一定的震蕩和卡頓，觀察圖12，可以看出算法輸出線速度的曲折和不穩(wěn)定。

圖12 局部目標(biāo)點(diǎn)插值前各參數(shù)曲線

既然局部臨時(shí)目標(biāo)氣泡中心可能發(fā)生跳變，我們可否設(shè)置一個(gè)在band中不會(huì)發(fā)生劇烈跳變的氣泡中心作為臨時(shí)目標(biāo)點(diǎn)呢？答案是可行的。為了實(shí)時(shí)性的不受影響，我們?cè)诰嚯x機(jī)器人較近的band中進(jìn)行氣泡插值，氣泡中心的選定為距離機(jī)器人0.2 m處，氣泡半徑由該處前后兩個(gè)氣泡半徑共同影響。這樣氣泡中心的位置就變成了相對(duì)穩(wěn)定的值。觀察圖13。

圖13 局部目標(biāo)點(diǎn)插值后各參數(shù)曲線

觀察算法輸出線速度曲線，算法輸出的線速度相比圖10平滑性有了大幅度提升，相對(duì)的里程計(jì)輸出線速度也隨之趨向平穩(wěn)。局部目標(biāo)點(diǎn)插值可視化效果如圖14所示。

圖14 局部目標(biāo)點(diǎn)插值可視化效果

其中從下向上數(shù)第二個(gè)氣泡為局部目標(biāo)插值點(diǎn)，也可稱之為速度插值點(diǎn)，氣泡連成的band即為局部路徑規(guī)劃器實(shí)時(shí)規(guī)劃的局部路徑，對(duì)比左側(cè)速度控制輸出，可以看到機(jī)器人在以相對(duì)平滑的線速度平穩(wěn)運(yùn)動(dòng)，算法發(fā)布的線速度和里程計(jì)發(fā)布的速度擬合程度也更高。

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

針對(duì)Eband算法存在的氣泡突變、曲率對(duì)線速度影響較大以及機(jī)器人運(yùn)動(dòng)卡頓問(wèn)題，主要進(jìn)行了以下3方面工作：

(1)曲率因子的平滑有效緩解了曲率因子對(duì)線速度的過(guò)度抑制，更換了曲率因子的計(jì)算方式，使得其對(duì)于線速度的影響更加合理；

(2)速度乘積因子的去噪，在一定程度上減輕了機(jī)器人運(yùn)動(dòng)控制的卡頓感覺(jué)，在狹窄空間內(nèi)機(jī)器人運(yùn)動(dòng)會(huì)更加平滑；

(3)局部目標(biāo)點(diǎn)插值，有效解決了由于氣泡的跳變引起的局部目標(biāo)點(diǎn)跳變和速度控制變化問(wèn)題；

局部路徑和全局路徑規(guī)劃方法仍然存在很多不足之處，需進(jìn)一步研究改進(jìn)，除此之外，局部路徑規(guī)劃和全局路徑規(guī)劃協(xié)同工作，機(jī)器人才可以更好地規(guī)劃出從起點(diǎn)到終點(diǎn)的運(yùn)動(dòng)路徑。