基于Eband算法的運動控制平滑性優(yōu)化

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

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

0 引 言

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

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

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

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

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

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

(1)

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

具體流程如圖1所示。

圖1 路徑規(guī)劃

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

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

圖2 氣泡沿著路徑產生和隨著障礙物改變情況

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

2 改進的Eband算法

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

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

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

在生成的局部路徑上，截取3個氣泡，分別是band[0]，band[1]，band[2]，band[0] 代表的是機器人的位姿，后續(xù)兩個氣泡代表機器人后續(xù)的兩個位姿。那么band[0]，band[1] 和band[2] 就對應pose[0]，pose[1] 和pose[2]。 利用每個pose中的x,y坐標，就可以求出前后兩個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)展開式即得到式(4)

(4)

2.2 速度乘積因子去噪

針對狹窄路段，Eband速度乘積因子出現(xiàn)噪點，引起機器人運動卡頓問題，做了狹窄路段速度乘積因子線性去噪。

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

Max_vel_lin=max_vel_lin_*curvature_factor*
velocity_multiplier

(5)

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

Scale=4-1.2/max_vel_lin_

(6)

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

Offset=0.42/(max_vel_lin_-0.4)

(7)

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

速度乘積的計算公式為

Velocity_multiplier=Scale*bubble_radius+Offset

(8)

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

2.3 速度插值法

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

算法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 實驗分析

3.1 實驗數(shù)據(jù)集

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

3.2 實驗軟硬件配置

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

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

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

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

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

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

3.3 實驗數(shù)據(jù)對比

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

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

當d<1.0時，如圖4所示。

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

由圖4可以看出，當d<1.0時，可見曲率因子的變化，會引起線速度的劇烈變化，對原算法輸出的線速度的影響效果較為明顯。

當d<0.2時，如圖5所示。

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

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

當d<0.1時，如圖6所示。

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

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

當d<0.15時，如圖7所示。

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

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

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

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

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

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

3.3.2 速度乘積因子去噪

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

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

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

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

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

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

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

3.3.3 局部目標點插值

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

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

圖12 局部目標點插值前各參數(shù)曲線

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

圖13 局部目標點插值后各參數(shù)曲線

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

圖14 局部目標點插值可視化效果

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

4 結束語

針對Eband算法存在的氣泡突變、曲率對線速度影響較大以及機器人運動卡頓問題，主要進行了以下3方面工作：

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

(2)速度乘積因子的去噪，在一定程度上減輕了機器人運動控制的卡頓感覺，在狹窄空間內機器人運動會更加平滑；

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

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