全局路徑約束的兩輪差速機器人改進導航算法

趙師兵， 張志明

（同濟大學電子與信息工程學院，上海 200092）

0 引 言

隨著人工智能的快速發(fā)展，研究機器人在不同場景下的落地應用，已成為國家和社會未來重點關注和發(fā)展的方向之一［1-3］。以人工智能和機器人為切入點，面向新工科的本科教育建設已成為熱點［4-6］。從人工智能＋機器人專業(yè)方向人才培養(yǎng)需求出發(fā)，教學實踐中涉及跨專業(yè)的多學科領域，具有理論和工程實用性兼?zhèn)涞奶攸c［4-6］。

導航技術告訴機器人該如何移動，是服務機器人研究領域的重點內容［1，7］。Leonard 把機器人導航的相關問題表述為對“我在哪”“我要去哪”及“我如何到達那里”3 個問題的回答［8］，其中“我在哪”指的是機器人定位問題，“我如何到達那里”是機器人的路徑規(guī)劃問題。基于地圖的路徑規(guī)劃是機器人導航核心技術之一，機器人在獲得與環(huán)境相關的地圖信息后進行路徑規(guī)劃，通過本體運動控制單元在規(guī)劃路徑指引下實現自主導航［7-9］。室內靜態(tài)環(huán)境下常使用忽略高度信息后的二維柵格地圖，將整個環(huán)境分為若干相同大小的柵格單元。路徑規(guī)劃即要求機器人根據某種優(yōu)化準則，在地圖上尋找一條代價最小的路線，實現從起點行駛到終點的任務，并且需避開障礙物。

兩輪差速底盤的建模與控制都較為簡單，也是室內服務機器人常采用的平臺架構［3，9］。本文以兩輪差速機器人為應用對象，分析其在經過障礙物情況下的狹窄路徑規(guī)劃和存在極端路徑情況下的運動，提出一種基于全局路徑約束的導航路徑規(guī)劃算法優(yōu)化方案，解決兩輪差速機器人在室內環(huán)境導航過程中出現的典型問題，如狹窄路徑無法通行、靠近障礙物后停止運行等。

1 室內環(huán)境導航問題描述

1.1 差速機器人運動模型

在室內環(huán)境導航中，通常把機器人建模為理想質點模型，涉及世界坐標系、里程計坐標系和機器人坐標系。機器人坐標系隨著機器人移動，全局路徑是在世界坐標系下進行解算，在導航過程中需要將機器人的速度信息轉換到世界坐標系下［9-10］。本文所用的兩輪差速機器人采用分層方式運動控制，如圖1 所示，下位機負責接收來自上位機的線速度和角速度控制指令并轉化成左／右輪的轉速，通過比例-積分-微分（Proportional-Integral-Derivative，PID）控制器調節(jié)PWM信號占空比輸出給左／右輪驅動電動機，使左／右輪穩(wěn)定為期望轉速運行。

圖1 兩輪差速機器人運動控制框圖

1.2 路徑規(guī)劃

機器人的路徑規(guī)劃可分為全局路徑規(guī)劃和局部路徑規(guī)劃［8-10］。通過全局代價地圖規(guī)劃全局路徑，再將全局路徑作為一種約束并結合局部代價地圖規(guī)劃局部路徑，是一種成熟且通用性較好的做法［11-13］。

全局路徑規(guī)劃有A*、Dijkstra 等經典算法及其改進算法［11-13］。常用的局部路徑規(guī)劃算法中，動態(tài)窗口法（Dynamic Window Approach，DWA）的前瞻性不足，且非全局最優(yōu)，多用于全向模型［14］；時間彈性帶（Time Elastic Band，TEB）算法計算復雜度大，且控制不穩(wěn)定，速度和角速度波動較大［15］；另外兩者都有參數煩多調節(jié)不易的問題。跟隨蘿卜法（Follow the Carrot，FTC）會盡可能地貼近全局路徑進行局部規(guī)劃［16］，適用于差速機器人和小房間情況，但無法后退，避障效果差。

1.2.1 加速度參數

DWA算法和TEB 算法中引入有難以確定的參數，如加速度。在差速機器人運動控制過程中，沒有對驅動電動機進行動力學建模，只涉及運動學而沒有動力學中的加速度控制，算法直接控制速度。室內環(huán)境下機器人實際運動速度不快，理論計算和實際測試表明，速度環(huán)控制能很快地穩(wěn)定，調節(jié)時間較短，而直接控制加速度比較困難，故算法中加入加速度參數意義不大。

1.2.2 原地自轉

DWA和TEB算法并未充分利用差速機器人原地旋轉的優(yōu)勢來規(guī)劃路徑。DWA 算法的評價函數與角速度相關，即使在需要自轉時，當線速度存在時總會得到更高的評分，所以DWA 算法無法得到原地自轉的效果。TEB算法可設置轉彎半徑（差速機器人的轉彎半徑為零），但在實際使用時，在剛出發(fā)情況下，算法不能規(guī)劃出轉彎半徑為零的路徑（原地自轉），在導航過程中，由于機器人線速度不為零，機器人轉彎半徑也不能設置為零。TEB 算法的另一個缺點是要求機器人到達目標點的同時方向指向目標，因此會在目標點附近進行調整，可能會導致狀態(tài)震蕩。

1.3 狹窄路徑與極端路徑的室內環(huán)境

室內移動機器人的實際應用環(huán)境中，經常會遇到存在狹窄路徑和極端路徑的情況，如圖2 所示，黑色實線為固定的墻體。機器人分別在3 個不同的起點S1、S2和S3出發(fā)，初始方向如圖中箭頭所示，終點相同均為E0。

圖2 大幅轉彎的狹窄路徑和極端路徑示意圖

機器人導航模塊規(guī)劃出3 條路徑。S1～E0路徑有一個近180°的轉彎，機器人在行駛到轉彎處時往往會因為運動慣性偏離預設路徑。由于局部路徑規(guī)劃用DWA和TEB算法都是速度控制算法，含有速度和加速度約束，其初衷是使機器人速度平滑不會突兀歸零，同時保護驅動電動機，故會控制機器人在有一定線速度的情況下進行急轉彎（非原地轉），以盡快恢復回到路徑。由于加速度和速度的約束，機器人可能會與墻壁或障礙物碰撞。急轉彎動作會導致里程計誤差增大，影響機器人的準確定位。

S2～E0路徑理論上的最優(yōu)方案應該是綠色路徑，靜止啟動機器人先自轉后再行駛，但實際規(guī)劃結果是紅色路徑，仍需要拐近180°的大彎，同樣會出現轉彎慣性問題。分析TEB算法，到達目標點的同時要求規(guī)定朝向，機器人在啟動后即進行轉彎動作；在DWA 算法中，評價函數由速度、朝向和障礙物3 部分組成，由于起點S2所在的環(huán)境開闊，且初速為零，影響軌跡主要因素由速度評價函數決定，速度越大得分越高，因此局部規(guī)劃器更傾向于拐大彎而非原地自轉。

S3～E0路徑中，由于起點S3至終點E0所途徑的周邊環(huán)境較為狹窄，障礙物評價函數起主導作用，此時機器人啟動時可先做原地自轉，再按預設路徑行駛。在狹窄路徑中，機器人與障礙物距離很近，需要機器人在路徑中心線附近行駛以避免碰撞。

在大幅轉彎的狹窄路徑和極端路徑的環(huán)境中，常常還伴隨有未在靜態(tài)地圖上出現的靜態(tài)障礙物和動態(tài)障礙物，路徑離障礙邊界過近（特別是轉彎處），需考慮機器人本身物理尺寸因素，因此防碰撞與恢復也是機器人導航中需要考慮的實際問題。

2 算法實現

差速兩輪機器人全局路徑約束的導航核心算法包括：全局路徑的平滑和機器人的導航，算法框架如圖3所示。其中，導航分為3 個核心部分：機器人自轉控制、機器人路徑跟隨和遇到障礙時的避障和恢復。機器人自轉控制是在導航前將機器人旋轉到路徑的朝向和路徑跟隨結束后將機器人旋轉到目標朝向；機器人的路徑跟隨為使用純跟蹤控制算法（Pure Pursuit，PP）進行軌跡跟蹤［17］；機器人的避障可通過全局規(guī)劃路徑規(guī)劃進行，機器人的恢復為根據障礙物的位置和非障礙物區(qū)域的位置對機器人進行控制調整到正常工作狀態(tài)。

圖3 兩輪差速機器人改進導航算法框架圖

2.1 全局路徑平滑及平滑處理

全局路徑規(guī)劃采用Dijkstra 算法，計算量較大，但可得到最優(yōu)路徑，同時避免A*算法過于貪婪無法走出U型區(qū)域的缺陷。該算法在柵格地圖上規(guī)劃得到路徑，當機器人通過狹窄路徑，或靠近膨脹區(qū)域行駛時，由于機器人激光雷達掃描的障礙物距離存在誤差，膨脹區(qū)域邊緣會擴展或收縮幾個柵格點，此時規(guī)劃的路徑就會出現大量往斜方向運動并頻繁轉向的短路徑，整體看來該部分的路徑出現鋸齒狀，平滑程度較差。借鑒文獻［17，19］中的方法，引入樣條插值函數對路徑進行平滑處理，消除尖角和往復路徑，其效果如圖4 所示。原路徑在180°U型大轉彎處和部分直線處平滑程度不佳，平滑后路徑直線部分的平滑度提高明顯，大轉彎部分也有較大改善。

圖4 路徑平滑處理效果圖

當路徑上每一個點都考察完后，如果此次路徑上的點有變動，則從當前位置開始重新再次進行優(yōu)化，直到一次優(yōu)化過程中路徑上的點不出現變動或超過最大優(yōu)化次數。

2.2 核心算法實現

改進算法的核心功能為機器人的自轉控制、路徑跟隨和恢復。機器人自轉采用位置式PID 控制，路徑采用純跟蹤算法，避障和恢復則根據遇到障礙時的障礙物區(qū)域和非障礙物區(qū)間的位置來控制。

2.2.1 機器人自轉控制

PID控制是一種常用的反饋控制器，由比例、積分和微分項組成，常見的形式有位置式和增量式兩種。位置式PID控制的優(yōu)點是靜態(tài)誤差小，溢出影響小，缺點是計算量大，累計誤差相對較大，一般結合輸出限幅和累計積分清零使用。增量式PID控制的優(yōu)點是運算量相對較小，且輸出與3 次誤差有關，當系統出現錯誤時影響相對較小，缺點是因為沒有累計誤差，因此會出現靜態(tài)誤差。

不論是導航前將機器人旋轉到路徑的朝向還是在結束后將機器人旋轉到目標朝向，目的都是使機器人的朝向與目標朝向一致，重要的是要盡量減小靜態(tài)誤差，故采用位置式PID進行自轉控制。ROS 中可通過訂閱里程計話題／Odometry／filtered 獲取機器人的位姿，目標朝向與機器人朝向做差后得到角度差err。比較，選擇絕對值較小的值作為控制器的角度誤差輸入，計算機器人旋轉角速度并對角速度輸出進行限幅。

2.2.2 導航路徑跟隨

使用全局路徑作為被跟蹤路徑的優(yōu)勢在于：由于全局路徑是實時計算的，每次計算全局路徑時都會使用機器人當前坐標點進行計算，有延時時仍可認為機器人在路徑上，沒有橫向誤差，因此只需計算機器人方向誤差控制量。本文采用純路徑跟隨算法控制機器人方向運動，相關原理如圖5 中所示。

圖5 純跟蹤算法原理

圖中：點（x，y）為路徑上的預瞄點；θ 為機器人在前進與機器人方向的夾角；d為與機器人的距離。機器人到達預瞄點的路徑可近似看成一段圓弧［20］，該段圓弧的半徑

機器人行駛的線速度

路徑預瞄點坐標（x，y）可從全局路徑中獲取，再變換到odom坐標系下，距離d和θ夾角可通過機器人odom坐標系下的位姿跟蹤坐標和先前的坐標進行計算。預瞄點的選取與前饋距離有關，d和θ 都由預瞄點決定，影響式（1）的獨立變量只有速度和前饋距離，相對于TEB 算法需要調節(jié)81 個參數，純路徑跟隨算法更加簡潔與方便。

室內兩輪差速機器人一般是在低速環(huán)境下行駛，機器人的線速度和前饋距離可保持一個定值，即使環(huán)境中存在沒有出現在地圖上的靜態(tài)障礙物和動態(tài)障礙物，激光雷達傳感器也能及時掃描到并放入代價地圖，全局路徑規(guī)劃器會重新規(guī)劃路徑。

2.2.3 避障與恢復機制

純跟蹤算法可獲得較好的跟蹤軌跡，而ROS 導航包計算的全局路徑自帶避障功能，故在有靜態(tài)障礙物和簡單動態(tài)障礙物的環(huán)境中，二者結合可獲得較好的導航效果。面對實際行駛過程中的突發(fā)情況，機器人需要一定的策略恢復到正常運動狀態(tài)。

一種情況是前方突然出現障礙物時，機器人規(guī)劃的全局路徑與機器人前進方向夾角過大，或者此時需要機器人后退導致二者方向相反；當機器人與障礙物距離過近時，可能會出現無法計算規(guī)劃出全局路徑的情況。

為應對第1 種情況，可在機器人行駛過程中計算路徑方向與前進方向的夾角，當夾角過大（如超過60°）時，機器人暫停路徑跟隨，原地自轉恢復到路徑方向，再恢復到正常行駛。對于第2 種情況，當檢測到無法規(guī)劃路徑時，可以根據局部代價地圖，計算機器人鄰域8 個方向地圖代價的下降速度，選擇代價下降最快且最接近機器人朝向直線的方向，控制機器人前進或后退往該方向行駛，直到機器人坐標的地圖代價值為零后再重新導航。

2.3 算法涉及的參數

本文所提算法的參數涉及全局路徑、機器人旋轉、機器人路徑跟隨和機器人恢復，見表1。全局路徑部分的參數主要作用是提高路徑的更新頻率，以增強機器人的避障能力；機器人旋轉和導航相關參數的作用在原理部分已經詳細說明，這里不再贅述；機器人恢復相關參數主要作用是將機器人的方向調整為和路徑方向一致，避免出現機器人角度大幅度偏移路徑的情況。

表1 本文算法需要調整的參數

3 實驗測試與結果分析

測試環(huán)境為：Ubuntu18.04 操作系統，Intel 2.60 GHz i7-9750H CPU，16GB內存，仿真實測編程環(huán)境為ROS Melodic 和Gazebo9.0。機器人操作系統（Robot Operating System，ROS）能極大的簡化機器人開發(fā)過程，已廣泛應用到機器人領域。分別在小型場地和大型場地的Gazebo仿真環(huán)境下驗證導航算法的有效性，并與目前常用的局部路徑規(guī)劃算法進行對比。

仿真測試使用的機器人是Turtlebot差速移動機器人，機器人自帶的傳感器為Kinect 深度相機，不同算法測試過程中均使用相同的全局規(guī)劃器Global Planner，里程計數據來源于Gazebo 自帶的模型里程計，全局定位數據通過ROS 功能包的AMCL 算法提供。關鍵參數的參考值已在上表1 中列舉。

3.1 小型場景導航實驗

3.1.1 場景介紹

小型場地布置為7.2 m×7.2 m的室內仿真環(huán)境，如圖6 所示，以實體墻分隔開不同空間，檢測不同算法的導航和避障性能。小型場地較為狹窄，不設置動態(tài)障礙物，錐桶作為靜態(tài)障礙物分布在場地內不同位置，保證機器人能夠在錐桶和墻體之間通行。導航目標是讓機器人從起點出發(fā)行駛到終點，多次實驗觀察機器人的導航完成度，并比較不同算法的行駛路徑、運行時間、不同速度下的行駛情況。

圖6 不同算法下導航效果對比

3.1.2 算法導航效果對比選取3 種目前最常用的局部路徑規(guī)劃算法（TEB、DWA、MPC）和本文所提算法做比較。其中TEB 和DWA 算法分別使用teb_local_planner 和dwa_local_planner功能包實現，模型預測控制（Model Predictive Control，MPC）算法將本文所提算法中的路徑跟隨控制器從PP算法替換為MPC控制器。設置機器人參考速度為0.5 m／s，每種算法在相同仿真環(huán)境中各運行10 次，記錄機器人行駛路徑，在圖6 中展示不同算法下機器人行程最遠一次的路徑，在圖7 中給出導航過程中機器人位姿的線速度變化。圖6 中黃線為機器人實際行駛路徑，黃線上的紅色箭頭指向為機器人行駛到該位置時機器人的實際朝向，部分路徑上箭頭比較密集，說明機器人在該處停留的時間較久。

圖7 導航過程中機器人線速度變化

如表2 所示為記錄機器人成功到達終點的次數、導航過程中碰撞到障礙物的次數等結果。

表2 導航過程中的數據記錄

3.1.3 算法效果分析

分析圖6 中機器人成功導航的行駛路徑，本文所用算法與TEB、MPC 算法的結果相差不大，都能使機器人導航到終點，而DWA算法效果欠佳。DWA 算法效果分圖中，箭頭較密集的路徑集中在180°轉彎路徑范圍和障礙物附近的路徑，說明DWA 算法在遇到極端路徑和存在障礙物時的路徑時導航效果不是很好。而MPC算法在正常導航的轉彎過程中多次使用原地自轉方式來修正行進路線，行駛過程中實際效果不如本文算法和TEB算法那樣穩(wěn)定。

由圖7 可見，導航過程中機器人路徑跟隨線速度變化的效果相差較大。本文算法的速度變化較為穩(wěn)定，與參考速度基本保持一致；TEB 算法在大部分采樣點上速度穩(wěn)定，但在轉彎路徑和障礙物較多的路徑上速度不穩(wěn)定；DWA和MPC算法的速度穩(wěn)定性較差，速度變化波動幅度較大。

由表2 可見，本文算法最佳，導航成功率為100%，且碰撞次數為零；MPC 算法次之，導航成功率為100%，碰撞次數為1；TEB 算法的導航成功率為90%，碰撞次數為2，略低于MPC 算法和本文算法；DWA算法導航效果最差，成功率只有10%，且碰撞次數為最多的16 次。

結合實驗結果可見，在沒有動態(tài)障礙物的狹窄場景中，本文算法能使機器人速度保持穩(wěn)定，且碰撞次數少，計算量也較小，機器人的導航效果最好。

3.2 大中型場景下導航實驗

大中型場景布置為55 m×25 m的醫(yī)院環(huán)境，該環(huán)境中有病房、病床、桌椅、柜臺、輪椅和清潔車等元素，并添加位置固定的候診人員作為靜態(tài)障礙物，環(huán)境空間較為空曠，但環(huán)境中元素較為復雜。機器人的行駛路線為：從地圖上方的起點S 開始，依次進入3 個病房，其順序如下，先進入地圖左側中央的大病房，然后通過中間過道進入右側中央大病房，接著進入地圖左下方的病房，最后回到起點。圖中的藍色實線為機器人行駛的實際路線，紅色箭頭指向為機器人行駛到該位置的實際朝向，箭頭越密集說明機器人在該段路徑停留的時間越長。

由圖8 可見，DWA算法在進入第一個病房的180°拐彎處時即與墻壁發(fā)生碰撞而未能完成導航，另外3種算法導航效果差距不大，都能完成導航，且行駛路徑較為平滑，且能避開障礙物，可順利通過狹窄的路徑、90°彎道和180°彎道等區(qū)域中的轉彎。

圖8 大中型場景下導航效果

從對比實驗可以得出結論，在大的開闊環(huán)境中，本文所用算法、TEB和MPC算法的導航情況都能獲得較好的效果，而DWA 算法在遇到180°的彎道時效果較差，會與障礙物發(fā)生碰撞。

4 結 語

針對差速兩輪機器人本身的物理運動特性，設計一種基于全局路徑約束的導航改進算法，實現機器人自轉控制、路徑跟隨和遇到障礙時的避障和恢復。在極端路徑和狹窄路徑的小環(huán)境中，相較于DWA 和TEB算法，本文所提算法應用于路徑規(guī)劃和導航時效果更優(yōu)，導航路徑更安全平滑，可有效降低因速度跳變導致的行駛震蕩幅度，機器人行駛過程更加平穩(wěn)。此外，在大型開放環(huán)境中，本文所提算法的效果也優(yōu)于DWA算法，與TEB和MPC 算法的效果相差不大。該算法可方便地部署到機器人教學實踐平臺，有助于培養(yǎng)本科生理論結合實踐的工程能力。