趙以毛，歐陽嘉泰，王冠凌，代廣珍

(安徽工程大學(xué) 電氣工程學(xué)院，安徽 蕪湖 241000)

隨著機器人技術(shù)和信息技術(shù)的結(jié)合，對室內(nèi)移動機器人的研究已經(jīng)成為當(dāng)前研究的大熱門之一[1]。實現(xiàn)室內(nèi)移動機器人系統(tǒng)設(shè)計的重要關(guān)鍵技術(shù)是其本身的自主定位與導(dǎo)航的準(zhǔn)確性。機器人的定位及導(dǎo)航的精準(zhǔn)度決定其在擴展應(yīng)用層面起到關(guān)鍵性作用。但室內(nèi)環(huán)境普遍存在物件擺放復(fù)雜多樣、動靜變化因素多等問題，導(dǎo)致移動機器人在室內(nèi)小環(huán)境場景，如家庭、企業(yè)工廠等，應(yīng)用中受到約束。移動機器人最基礎(chǔ)的一項工作就是可以自主導(dǎo)航，簡而言之，當(dāng)機器人處于一個未知或者陌生環(huán)境中，可以獨立自主規(guī)劃出一條安全、平滑的最優(yōu)通行線路，沿著這條路徑到達(dá)目標(biāo)點。地圖構(gòu)建即SLAM[2]，是機器人成功完成導(dǎo)航的前提。當(dāng)前，常用的SLAM建圖方法主要有：(1)基于圖像優(yōu)化的Cartographer算法[3]，此算法是谷歌開發(fā)的室內(nèi)實時SLAM，采用谷歌開發(fā)的基于Ceres的非線性優(yōu)化思想和基于子地圖的全局地圖構(gòu)建思想，能夠有效規(guī)避地圖構(gòu)建環(huán)境中運動物體的干預(yù)。由于Cartographer算法沒有集成到ROS中，因此在ROS環(huán)境下若要使用此算法，在安裝過程中我們需要從官網(wǎng)下載源碼的方式來對其進行編譯安裝，相比于其它建圖方法其安裝過程復(fù)雜。(2)Hector_slam算法基于激光雷達(dá)運行，不需運用里程計信息，其采用高斯牛頓方法，構(gòu)建二維柵格地圖。但此算法對于雷達(dá)幀率要求很高且此算法沒有對地圖的修正能力，初值的選擇對結(jié)果的影響很大，并且在hector_slam過程中如果轉(zhuǎn)彎的速度過快，移動機器人建圖會出現(xiàn)打滑的現(xiàn)象，在之后的建圖匹配也都會出現(xiàn)較大差別從而導(dǎo)致地圖構(gòu)建失敗。(3)Gmapping-SLAM算法，此算法是一種基于粒子濾波的建圖算法，且在ROS系統(tǒng)中此算法已集成其中，是在移動機器人建圖中使用最廣泛的SLAM建圖算法[4]，在對地圖邊緣角落的修正處理上和障礙物的掃描識別標(biāo)定，以及在對小地圖場景進行建圖時其計算量相對較小且Gmapping構(gòu)建的二維柵格地圖更加穩(wěn)定，不易出現(xiàn)漂移的現(xiàn)象。Gmapping算法的建圖效果較好，但同樣也有著兩個缺陷：(1)建圖對機器人位姿要求較高，(2)頻繁的重采樣導(dǎo)致粒子耗散。對比上述幾種建圖方法的優(yōu)缺點，本文基于Gmapping算法并針對該算法的缺陷，提出了針對性的優(yōu)化方案，改善建議分布，以及加入KLD重采樣。基于改進的Gmapping算法，本文對室內(nèi)自主搭建的實驗場地來進行構(gòu)建地圖。

在建圖與定位完成后，需要機器人可以獨立自主規(guī)劃出一條從設(shè)置的起始點到目標(biāo)點并可以自主躲避在前進中遇到的障礙物的最優(yōu)安全路徑，即路徑規(guī)劃[5]。當(dāng)前，Dijkstra算法[6]、A*算法[7-8]RRT算法[9-11]和基于生物啟發(fā)式的遺傳算法[12-13]等是目前流行的全局規(guī)劃算法。其中，A*算法采用了啟發(fā)式搜索模式，減少了搜索節(jié)點的個數(shù)，搜索時間降低了，因此大大提高了搜索效率，在全局路徑規(guī)劃方面具有非常大的優(yōu)勢。

本文在ROS(Ubuntu18.04-Melodic版本)環(huán)境下，實現(xiàn)了機器人自主導(dǎo)航系統(tǒng)。首先，使用改進的Gmapping-SLAM算法完成對環(huán)境地圖構(gòu)建與定位，然后利用改進的A*算法和TEB算法完成機器人路徑規(guī)劃和運動控制，使機器人可以順利到達(dá)目標(biāo)點。

1 基于改進RBPF-Gmapping算法建圖與定位

在不熟悉的或復(fù)雜的場景中，移動機器人要想實現(xiàn)完全自主導(dǎo)航，首先必須要獲取周圍場景的信息，確定當(dāng)前姿勢，構(gòu)建環(huán)境地圖。目前，SLAM算法中應(yīng)用較為廣泛的是Gmapping算法，主要是其在小場景及邊緣角落處理方面建圖效果較好，計算量較小，精度較高。

1.1 傳統(tǒng)的RBPF-Gmapping算法

Grisetti和Steux等人提出的基于RBPF(Rao-Blackwellized partical filter)的SLAM算法[14]。Gmapping算法的地圖構(gòu)建主要運用了其理論且基于2D激光雷達(dá)使用RBPF算法完成對二維柵格地圖構(gòu)建的SLAM算法，RBPF首先對位姿估計與地圖估計進行分離，其次估計位姿，最后進行地圖估計，其根據(jù)里程計信息u1:t-1=u1,u2,…,ut-1測量值及激光雷達(dá)的觀測值z1:t，再通過觀測數(shù)據(jù)和里程計數(shù)據(jù)得到機器人位姿x1:t和地圖m的聯(lián)合后驗概率估計p(x1:t,m|z1:t,u1:t-1)[15]。

通過RB分解，SLAM模型可表示為：

p(x1:t,m|z1:t,u1:t-1)

=p(x1:t|z1:t,u1:t-1)p(m|x1:t,z1:t)

(1)

式(1)用后驗概率表述出了移動機器人的位姿與地圖信息之間關(guān)聯(lián)p(x1:t|z1:t,u1:t-1)，為運動軌跡后驗概率。RBPF-SLAM算法是基于Rao-Blackwellized分解的粒子濾波理論估計機器人的軌跡，再利用擴展卡爾曼濾波估計地圖。最后，利用粒子濾波算法對移動機器人進行位姿估計和環(huán)境地圖更新。其主要流程為：

(2)權(quán)重更新：根據(jù)當(dāng)前觀測信息Z1:t=z1,z2,…,zt，將雷達(dá)觀測得到的地圖信息與各個粒子的權(quán)值相配比，并利用式(2)更新權(quán)值。

(2)

(3)重采樣：在迭代過程中，為了降低粒子退化現(xiàn)象，一些權(quán)重低的粒子被丟棄，并復(fù)制相同的高權(quán)重粒子，避免計算損耗在無意義的低權(quán)值粒子中。

(4)更新地圖：通過機器人姿態(tài)估計和當(dāng)前激光雷達(dá)觀測信息，通過p(m|x1:t,z1:t)來對地圖進行更新。

Doucet等利用式(3)來制約建議分布q，可以得到用來計算重要性權(quán)重的遞歸公式。

q(x1:t|z1:t,u1:t-1)

=q(xt|x1:t-1,z1:t,u1:t-1)·q(x1:t-1|u1:t-2)

(3)

將式(3)代入式(2)展開得到粒子權(quán)重為：

(4)

其中，η=1/p(zt|z1:t-1,u1:t-1)表示歸一化因子。

Gmapping的建圖過程如圖1所示。

圖1 Gmapping的建圖過程

1.2 改進RBPF-Gmapping算法

1.2.1 建議分布的改進

對RBPF-SLAM算法來說，選擇正確的建議分布是至關(guān)重要。當(dāng)其所需粒子數(shù)越少[16]，其所選建議分布與真實分布越接近。在經(jīng)過迭代多次之后，粒子集中的每個粒子都會攜帶大量信息數(shù)據(jù)，從而增加了系統(tǒng)計算的難易程度。在機器人只考慮使用里程計運動模型不使用其它傳感器的情況下，式(4)可轉(zhuǎn)化為：

(5)

式(5)中運動模型的計算簡單、易于獲得，當(dāng)在使用傳感器的精度非常高時，激光雷達(dá)得到的觀測模型作為建議分布的精度比使用里程計運動模型作為建議分布的精度高，產(chǎn)生這種現(xiàn)象的原因是在后驗概率似然值低的粒子中，其里程計運動模型中的權(quán)重值設(shè)置較低。因此，本文將激光雷達(dá)掃描獲得的觀測信息zt融入到建議分布中。融合后的建議分布可以使采樣分布，在似然函數(shù)概率分布范圍內(nèi)具有較大的值。改進的建議分布優(yōu)化和粒子權(quán)重的計算公式分別定義如下。

(6)

(7)

由于最優(yōu)重要概率密度函數(shù)的形式未知，機器人在不斷向前移動過程中，機器人里程計的誤差，導(dǎo)致其后驗概率分布在采樣時將使用更多的粒子，然而當(dāng)外部傳感器為高精度激光雷達(dá)時，其似然峰值有且只有一個，那么可將傳感器觀測到的信息的似然峰值分布近似為高斯分布，如圖2所示。

圖2 運動模型與觀測模型似然分布值

通過對建議分布改進后，似然函數(shù)的峰值很好獲得，此時只需對似然函數(shù)的峰值L[i]區(qū)域進行采樣，計算公式為：

(8)

(9)

式(7)可以簡化為：

(10)

1.2.2 基于KLD重采樣

上述改進可以直接提高建議分布概率匹配精度，獲得當(dāng)前數(shù)據(jù)，使粒子收集更加準(zhǔn)確，降低粒子迭代計算的難度和在更新過程中的計算量，但在實際應(yīng)用中，采集的樣本數(shù)和迭代次數(shù)較多。為了提高智能機器人的實時反應(yīng)速度和運行效率，可以對采樣濾波算法做進一步優(yōu)化。Dieter Fox等人提出了基于KLD采樣的自適應(yīng)粒子濾波算法[17-18]，其主要思想為：根據(jù)粒子群分布的KLD動態(tài)調(diào)節(jié)粒子數(shù)，對不同分布的近似程度進行評估。那么，可以在重采樣粒子復(fù)制期間加入KLD重采樣，其可根據(jù)粒子的離散情況實時調(diào)整粒子數(shù)量的多少。

本文采用KLD重采樣方法，在其滿足1-θ的條件后，對采樣獲得的粒子集合概率與實際后驗概率進行誤差計算α。誤差α由K(p,q)距離確定的。兩概率之間的逼近誤差如下：

(11)

由式(11)可知，式中：K表示K-L距離，KLD計算值越小，說明概率分布p與概率分布q之間的誤差越小，估計的概率分布與真實概率分布越接近。當(dāng)粒子數(shù)k為固定值時，由概率理論分析，可以保證真實概率密度與估計概率密度之間K-L距離小于閾值α，通過計算可以得到兩個概率分布之間的最小逼近誤差。

(12)

式(12)為粒子數(shù)k的表達(dá)式和x2分布的分位數(shù)。

最后再利用Willson-Hilferty變換近似得到：

(13)

其中，z1-θ是上分位數(shù)為1-θ的標(biāo)準(zhǔn)正態(tài)分布。

在粒子的復(fù)制過程中加入KLD重采樣的方法來對下一階段的粒子數(shù)重采樣，可以實現(xiàn)粒子數(shù)的實時調(diào)整，計算量得以大大減小。

1.3 AMCL定位算法

定位的好壞在導(dǎo)航系統(tǒng)中起到至關(guān)重要的作用。機器人在未知室內(nèi)環(huán)境中，通過外部傳感器如激光雷達(dá)、慣性測量單元(IMU)、輪式里程計等傳感器獲取環(huán)境地圖信息，經(jīng)過算法處理，最終可以得到機器人在地圖中的位姿。

定位方面使用自適應(yīng)蒙特卡洛定位(AMCL)算法進行定位，該算法是基于地圖成功建好的前提，其利用粒子濾波器估計機器人的后驗概率分布。其算法過程如圖3所示。

圖3 AMCL算法過程

2 移動機器人路徑規(guī)劃

移動機器人自主導(dǎo)航的成功與否的關(guān)鍵就是路徑規(guī)劃。機器人對自身處于的未知環(huán)境的地圖創(chuàng)建和定位完成后，獨立自主規(guī)劃出一條安全、平滑的最優(yōu)通行線路，沿著這條路徑到達(dá)目標(biāo)點，并確保在前進道路上遇到障礙物可以順利避開并準(zhǔn)確到達(dá)目標(biāo)點。本文采用的A*和TEB的混合算法來對機器人進行路徑規(guī)劃，兩者共同完成機器人處于一個陌生環(huán)境中的路徑規(guī)劃和自主導(dǎo)航、避障等環(huán)節(jié)。

2.1 傳統(tǒng)A*算法

實現(xiàn)機器人自主導(dǎo)航功能的關(guān)鍵，重要是在已知地圖環(huán)境信息后，以及一個或多個性能指標(biāo)規(guī)劃出最短無碰撞路徑。傳統(tǒng)A*算法在得到地圖信息階段時會評估每個位置點，然后從各個位置中找到最優(yōu)點，再從此處最佳位置繼續(xù)向下不斷的搜索接下來的最佳位置，通過對比各個最優(yōu)解連成一條最優(yōu)的路徑，這種方式被稱為啟發(fā)式搜索。此種搜索方式是從起點開始，首先遍歷初始點周圍相靠近的點，然后再遍歷已經(jīng)遍歷過的點相鄰近的點，逐步的向外遍歷尋找擴散，直到找到終點，通過這種不斷地搜索可以形成一個點集，形成該點集中路徑點的安全有序的連接即是要找尋的最優(yōu)路徑。A*算法的模型為：

f(n)=g(n)+h(n)

(14)

式(14)中，f(n)是總的搜索代價，g(n)是從起點到所處最優(yōu)合理節(jié)點n的代價和，h(n)是從當(dāng)前所處最佳位置節(jié)點n到目標(biāo)節(jié)點的最優(yōu)代價。傳統(tǒng)A*算法采用的是八鄰域搜索法，如圖4所示，然而，基于八鄰域搜索的A*算法生成的搜索路徑中包含很多的導(dǎo)航路線規(guī)劃中不需要的多余冗余點，且拐點的轉(zhuǎn)向角太大，導(dǎo)致規(guī)劃產(chǎn)生的過長路徑長度和曲率不連續(xù)的路徑，這不利于機器人安全到達(dá)目標(biāo)點。

圖4 八鄰域搜索示意圖

2.2 改進A*算法

針對經(jīng)典A*算法的八鄰域搜索方向在路徑規(guī)劃過程中，需要的時間長，路徑不夠平滑。通過重新設(shè)計距離定義的啟發(fā)式函數(shù)，使改進的A*算法可以在搜索路徑的過程中考慮可通過區(qū)域[19]，優(yōu)化路徑規(guī)劃的轉(zhuǎn)彎角度，使機器人規(guī)劃出的全局路徑更加平滑。結(jié)合歐氏距離和曼哈頓距離，本文在不同的搜索方向上使用不同的但成比例的距離權(quán)值，改進的啟發(fā)函數(shù)的具體形式如式(15)和式(16)所示，路徑規(guī)劃角度的平滑改進如式(17)所示。

h(n)=nK·(|nx-gy|+|ny-gy|)

(15)

(16)

(17)

啟發(fā)函數(shù)改進后h(n)在當(dāng)前所處格點的上、下、左、右的不同方位上，采用曼哈頓距離定義，在當(dāng)前柵格的右上、右下、左上、左下四個不同方位上，采用歐氏距離定義。其中，K為距離加權(quán)系數(shù)，n為正整數(shù)，(x1,y1),(x2,y2),(x3,y3)為開始路徑規(guī)劃的三個相鄰節(jié)點坐標(biāo)，β為轉(zhuǎn)彎角度。啟發(fā)函數(shù)的改進后，計算成本節(jié)省了很多，大大減少了機器人規(guī)劃路徑的時間，并且規(guī)劃出的全局路徑更加平滑。

2.3 局部路徑規(guī)劃

移動機器人的路徑規(guī)劃不僅要求利用全局路徑規(guī)劃算法獨立自主規(guī)劃出一條安全、平滑的最優(yōu)通行線路，沿著這條路徑到達(dá)目標(biāo)點的路徑，還要求機器人能夠在復(fù)雜未知的動態(tài)環(huán)境中完成對障礙物的自適應(yīng)避障。如果僅僅只使用全局路徑規(guī)劃算法顯然不能滿足機器人在復(fù)雜環(huán)境下的自主避障功能，此時就需要局部路徑規(guī)劃算法與全局路徑規(guī)劃算法相互配合、協(xié)調(diào)以此完成機器人在復(fù)雜環(huán)境自主導(dǎo)航避障。局部路徑規(guī)劃采用TEB算法。TEB是一個多目標(biāo)優(yōu)化問題，由于依賴少數(shù)幾個連續(xù)狀態(tài)，因此通常是一個參數(shù)較少的局部問題。此外，為了顯示機器人的運動學(xué)軌跡信息，每個點與點之間都設(shè)置了運動時間，該算法利用“外力”來約束“變形”局部路徑，這些約束主要有如下幾方面。

(1)機器人動力學(xué)約束:包括有限控制量、速度、轉(zhuǎn)向角度、最小轉(zhuǎn)彎半徑等，通過約束形成一個平滑的運動軌跡。

(2)最短路徑約束:目標(biāo)函數(shù)使機器人路徑上各個位姿點在時間上均勻分布，而不是盲目地追求最短路徑。

(3)路徑跟蹤和避障:考慮了全局路徑、在地圖上找到的障礙物(靜態(tài)障礙物)以及激光雷達(dá)掃描發(fā)現(xiàn)的障礙物(動態(tài)障礙物)，障礙力迫使局部路徑遠(yuǎn)離障礙物。

使用開源G2O庫(General Graph Optimization)進行測試優(yōu)化，最終可以獲得到到達(dá)目標(biāo)點的耗時最少、距離最短且可以成功避開障礙物的局部路徑，并且使局部路徑盡可能貼合全局規(guī)劃路徑。

根據(jù)上述分析，當(dāng)配置導(dǎo)航功能時，如果TEB參數(shù)設(shè)置的不符合規(guī)劃時，會致使無法規(guī)劃出正確的行駛路線，此時機器人會出現(xiàn)抖動、旋轉(zhuǎn)、停止或者倒退的現(xiàn)象。因此，需將TEB算法中的默認(rèn)參數(shù)進行一定的優(yōu)化改進，其中一些影響路徑規(guī)劃與機器人行駛的重要優(yōu)化參數(shù)如表1所示。

表1 TEB局部路徑相關(guān)參數(shù)

3 仿真與實際場地實驗

針對建圖定位和導(dǎo)航功能，首先在Gazebo中進行了場景模型搭建，并在Rviz中加載地圖環(huán)境并運行鍵盤控制節(jié)點進行了地圖構(gòu)建和導(dǎo)航功能仿真，最終搭建線下的實驗場地中進行導(dǎo)航相關(guān)實驗。導(dǎo)航流程如圖5所示。

圖5 導(dǎo)航框架

3.1 建圖仿真實驗

仿真實驗在一臺系統(tǒng)為Ubuntu18.04-Melodic版本環(huán)境的計算機(RAM:8.00GB；處理器為：AMD R7-5800X)中操作，使用ROS中的Gazebo與Rviz工具。首先，在Gazebo中配置Word文件，并配置機器人模型URDF文件，地圖環(huán)境搭建與機器人模型如圖6所示。其次，打開新終端啟動激光雷達(dá)launch節(jié)點并調(diào)用改進后的Gmapping-SLAM功能包，并根據(jù)Gmapping-SLAM算法與激光雷達(dá)傳感器獲得的數(shù)據(jù)信息來完成對當(dāng)前地圖的構(gòu)建；運行Rviz節(jié)點，對建圖過程進行可視化觀察，可得到如圖7所示的畫面。

圖6 Gazebo仿真環(huán)境與機器人模型

圖7 Rviz可視化建圖過程

在ROS環(huán)境中進行仿真實驗，首先要在ROS中訂閱機器人激光雷達(dá)的“/scan”話題和“/cmd_vel”速度控制話題，同時啟動鍵盤節(jié)點腳本使用電腦方向按鍵控制機器人在仿真環(huán)境地圖中的前進，機器人前進過程中環(huán)境地圖被完整構(gòu)建出來，與實際環(huán)境基本一致，沒有嚴(yán)重的漂移、重影、打滑等現(xiàn)象出現(xiàn)。地圖中的黑色圖線表示機器人無法通行的地帶，白色區(qū)域為機器人的可通行地帶，地圖邊界上的點為激光雷達(dá)掃描點，地圖中剩余區(qū)域為未搜索掃描區(qū)域。傳統(tǒng)Gmapping建圖和改進Gmapping建圖如圖8(a)、圖8(b)所示，在1，2，3，4處可以看出改進的算法在相同條件下建圖效果更好。

(a)傳統(tǒng)Gmapping建圖效果

3.2 定位與路徑規(guī)劃仿真

在機器人移動導(dǎo)航過程中，路徑規(guī)劃過程需調(diào)用ROS系統(tǒng)中的兩個功能包來實現(xiàn)，第一，調(diào)用AMCL定位功能包，如圖9(a)、圖9(b)、圖9(c)、圖9(d)所示，移動機器人定位的結(jié)果，觀察Rviz中仿真環(huán)境地圖，粒子云在初始位置隨機分布，隨著機器人在不斷前進，粒子云漸漸向正確位姿靠攏。第二，路徑規(guī)劃需調(diào)用ROS中的Move_base功能包，結(jié)合改進后的A*算法及TEB算法，準(zhǔn)確評估出最優(yōu)路徑來實現(xiàn)開始位置到終點位置的路徑規(guī)劃和機器人的自主導(dǎo)航運行。

圖9 定位過程圖

在建圖與定位完成后將成功建圖的yaml文件寫入導(dǎo)航框架的launch文件中以實現(xiàn)移動機器人路徑規(guī)劃與自主導(dǎo)航實驗，實驗過程中觀察Rviz中仿真環(huán)境地圖中的激光雷達(dá)掃描點與障礙物的重合程度，同時判斷機器人在當(dāng)前地圖中的位置；可以看出地圖中的障礙物高度與激光雷達(dá)掃描點數(shù)據(jù)高度匹配。如圖10(a)、圖10(b)、圖10(c)所示。運行導(dǎo)航launch文件和Rviz節(jié)點，圖中的凸顯區(qū)域為障礙物膨脹層，是防止機器人的外殼與障礙物相撞的區(qū)域。機器人的起點在右上角，目標(biāo)點在黑色方塊點，在這個過程中，規(guī)劃的路徑逐漸縮短，機器人自主移動至目標(biāo)點。

(a)導(dǎo)航起始點

3.3 線下場地導(dǎo)航實驗

在線下場地中，機器人在快速移動中由于地面等原因易發(fā)生輪子打滑現(xiàn)象。為此，搭建了線下場地以測試在這種情況下定位和導(dǎo)航功能的準(zhǔn)確性和穩(wěn)定性，此外，實際場地的誤差因素也會干擾其定位的準(zhǔn)確性。在機器人不斷向目標(biāo)點移動過程中，定位進度逐漸準(zhǔn)確穩(wěn)定。實驗場地搭建與機器人如圖11所示，并且在線下場地驗證了經(jīng)典A*算法與改進A*算法的導(dǎo)航路徑規(guī)劃，如圖12(a)，圖12(b)所示。相關(guān)數(shù)據(jù)對比見表2所示。

從表中可知優(yōu)化后的A*算法規(guī)劃出路徑更短更平滑，到達(dá)目標(biāo)點用時更少，速度更快。

圖11 實驗場地和機器人

(a)經(jīng)典A*算法導(dǎo)航過程

表2 導(dǎo)航算法實驗數(shù)據(jù)比較

為測試機器人在導(dǎo)航過程中是否能準(zhǔn)確避開動態(tài)障礙物，在實際場地中放置了另一個機器人作為動態(tài)障礙物，如圖13(a)、圖13(b)所示。當(dāng)障礙機器人進入到機器人雷達(dá)掃描的范圍時，機器人會迅速作出反應(yīng)重新規(guī)劃一條最優(yōu)路徑，以此來規(guī)避障礙物機器人。

(a)移動機器人與移動障礙物起點

結(jié)語

本文基于ROS的室內(nèi)移動機器人自主導(dǎo)航系統(tǒng)設(shè)計研究為目的。著重對機器人SLAM建圖、AMCL定位、路徑規(guī)劃算法等進行討論研究。

(1)在建圖與定位方面，利用改進的Gmapping算法進行建圖，通過激光雷達(dá)獲取環(huán)境信息，機器人通過AMCL算法進行機器人的定位，行駛中粒子逐漸收斂且精度較高，可以有效地用于地圖構(gòu)建和導(dǎo)航中。

(2)路徑規(guī)劃方面，通過在仿真場景中搭建環(huán)境地圖和導(dǎo)航框架，最終實現(xiàn)移動機器人自主導(dǎo)航功能，并在線下搭建實驗場地，利用已有設(shè)備對自主導(dǎo)航系統(tǒng)框架進行實驗驗證，在實際場地實驗中機器人可以完成躲避靜態(tài)障礙物和動態(tài)障礙物的導(dǎo)航實驗且基于改進的A*算法規(guī)劃路徑長度，平滑性都優(yōu)于傳統(tǒng)A*算法。

(3)TEB算法進行局部路徑規(guī)劃，當(dāng)規(guī)劃時檢測到障礙物時，可以快速、敏捷地做出反應(yīng)，并規(guī)劃出時間最優(yōu)路徑，順利抵達(dá)導(dǎo)航終點。