全局路徑約束的兩輪差速機(jī)器人改進(jìn)導(dǎo)航算法

趙師兵， 張志明

（同濟(jì)大學(xué)電子與信息工程學(xué)院，上海 200092）

0 引 言

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

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

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

1 室內(nèi)環(huán)境導(dǎo)航問題描述

1.1 差速機(jī)器人運(yùn)動(dòng)模型

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

圖1 兩輪差速機(jī)器人運(yùn)動(dòng)控制框圖

1.2 路徑規(guī)劃

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

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

1.2.1 加速度參數(shù)

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

1.2.2 原地自轉(zhuǎn)

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

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

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

圖2 大幅轉(zhuǎn)彎的狹窄路徑和極端路徑示意圖

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

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

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

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

2 算法實(shí)現(xiàn)

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

圖3 兩輪差速機(jī)器人改進(jìn)導(dǎo)航算法框架圖

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

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

圖4 路徑平滑處理效果圖

當(dāng)路徑上每一個(gè)點(diǎn)都考察完后，如果此次路徑上的點(diǎn)有變動(dòng)，則從當(dāng)前位置開始重新再次進(jìn)行優(yōu)化，直到一次優(yōu)化過程中路徑上的點(diǎn)不出現(xiàn)變動(dòng)或超過最大優(yōu)化次數(shù)。

2.2 核心算法實(shí)現(xiàn)

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

2.2.1 機(jī)器人自轉(zhuǎn)控制

PID控制是一種常用的反饋控制器，由比例、積分和微分項(xiàng)組成，常見的形式有位置式和增量式兩種。位置式PID控制的優(yōu)點(diǎn)是靜態(tài)誤差小，溢出影響小，缺點(diǎn)是計(jì)算量大，累計(jì)誤差相對(duì)較大，一般結(jié)合輸出限幅和累計(jì)積分清零使用。增量式PID控制的優(yōu)點(diǎn)是運(yùn)算量相對(duì)較小，且輸出與3 次誤差有關(guān)，當(dāng)系統(tǒng)出現(xiàn)錯(cuò)誤時(shí)影響相對(duì)較小，缺點(diǎn)是因?yàn)闆]有累計(jì)誤差，因此會(huì)出現(xiàn)靜態(tài)誤差。

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

2.2.2 導(dǎo)航路徑跟隨

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

圖5 純跟蹤算法原理

圖中：點(diǎn)（x，y）為路徑上的預(yù)瞄點(diǎn)；θ 為機(jī)器人在前進(jìn)與機(jī)器人方向的夾角；d為與機(jī)器人的距離。機(jī)器人到達(dá)預(yù)瞄點(diǎn)的路徑可近似看成一段圓?。?0］，該段圓弧的半徑

機(jī)器人行駛的線速度

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

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

2.2.3 避障與恢復(fù)機(jī)制

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

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

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

2.3 算法涉及的參數(shù)

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

表1 本文算法需要調(diào)整的參數(shù)

3 實(shí)驗(yàn)測(cè)試與結(jié)果分析

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

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

3.1 小型場(chǎng)景導(dǎo)航實(shí)驗(yàn)

3.1.1 場(chǎng)景介紹

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

圖6 不同算法下導(dǎo)航效果對(duì)比

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

圖7 導(dǎo)航過程中機(jī)器人線速度變化

如表2 所示為記錄機(jī)器人成功到達(dá)終點(diǎn)的次數(shù)、導(dǎo)航過程中碰撞到障礙物的次數(shù)等結(jié)果。

表2 導(dǎo)航過程中的數(shù)據(jù)記錄

3.1.3 算法效果分析

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

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

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

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

3.2 大中型場(chǎng)景下導(dǎo)航實(shí)驗(yàn)

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

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

圖8 大中型場(chǎng)景下導(dǎo)航效果

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

4 結(jié) 語

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