基于ROS平臺(tái)的導(dǎo)航機(jī)器人路徑規(guī)劃研究

摘? 要：隨著智能技術(shù)的發(fā)展，自主移動(dòng)機(jī)器人逐步進(jìn)入生產(chǎn)生活當(dāng)中，其中路徑規(guī)劃是實(shí)現(xiàn)機(jī)器人導(dǎo)航功能的重要環(huán)節(jié)?；跈C(jī)器人操作系統(tǒng)平臺(tái)，對(duì)導(dǎo)航機(jī)器人的整體架構(gòu)和路徑規(guī)劃進(jìn)行研究，在Dijkstra算法基礎(chǔ)上，加入啟發(fā)式搜索思想，提出A*算法規(guī)劃全局路徑，并進(jìn)行兩種算法的對(duì)比分析，同時(shí)提出一種基于動(dòng)態(tài)窗口的局部路徑規(guī)劃算法。經(jīng)實(shí)驗(yàn)驗(yàn)證，該方法可實(shí)現(xiàn)機(jī)器人導(dǎo)航過(guò)程中的路徑規(guī)劃和實(shí)時(shí)動(dòng)態(tài)避障功能，且具有一定的路徑尋優(yōu)能力和較高的執(zhí)行效率。

關(guān)鍵詞：ROS;移動(dòng)機(jī)器人;自主導(dǎo)航;路徑規(guī)劃;動(dòng)態(tài)避障

Abstract：With the development of intelligent technology，autonomous mobile robot has gradually entered the production and life，among which path planning technology is an important link to realize the navigation function of the robot. Based on the ROS platform，the overall architecture and path planning technology of navigation robots are studied. On the basis of Dijkstra algorithm，the idea of heuristic search was added to propose A* algorithm for global path planning，and a comparative analysis was conducted between the two algorithms. At the same time，a local path planning algorithm based on dynamic window is proposed. The experimental results show that this method can satisfy the functions of path planning and real-time dynamic obstacle avoidance in the process of robot navigation，and has certain path optimization ability and high execution efficiency.

Keywords：ROS;mobile robot;autonomous navigation;path planning;obstacle avoidance

0? 引? 言

隨著當(dāng)今機(jī)器人技術(shù)的發(fā)展，自主移動(dòng)機(jī)器人憑借其智能化和實(shí)用性，被廣泛地應(yīng)用于服務(wù)行業(yè)、工業(yè)制造、醫(yī)療看護(hù)等多種場(chǎng)景。該類機(jī)器人擁有強(qiáng)大的移動(dòng)能力，通過(guò)目標(biāo)點(diǎn)的設(shè)定，可實(shí)現(xiàn)自主導(dǎo)航功能，以替代人類完成部分復(fù)雜、危險(xiǎn)的工作[1]。其中路徑規(guī)劃作為實(shí)現(xiàn)機(jī)器人自主導(dǎo)航功能的關(guān)鍵性技術(shù)之一，成為當(dāng)下研究的熱點(diǎn)。國(guó)外對(duì)于路徑規(guī)劃研究起步較早，較為經(jīng)典的有Dijkstra提出的用于解決最短路徑問(wèn)題的Dijkstra算法、Khatib提出的人工勢(shì)場(chǎng)法、Lavalle提出的快速隨機(jī)樹算法。國(guó)內(nèi)的Zhang提出了一種基于混沌人工勢(shì)場(chǎng)法的路徑規(guī)劃新方法[2]，即在傳統(tǒng)的人工勢(shì)場(chǎng)方法中，結(jié)合了混沌優(yōu)化算法。但是現(xiàn)階段部分的路徑規(guī)劃算法仍處于理論研究，可實(shí)際應(yīng)用于導(dǎo)航機(jī)器人的路徑規(guī)劃算法，仍然存在尋優(yōu)能力差、效率低、算法魯棒性差等問(wèn)題。

本文基于作者所就讀的吉林大學(xué)儀器科學(xué)與電氣工程學(xué)院的大學(xué)生創(chuàng)新訓(xùn)練項(xiàng)目，為完成該項(xiàng)目中導(dǎo)航機(jī)器人的系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)，滿足移動(dòng)機(jī)器人在導(dǎo)航過(guò)程中，對(duì)于全局路徑的搜尋以及實(shí)時(shí)動(dòng)態(tài)避障的實(shí)現(xiàn)要求[3]，介紹了一種基于ROS（Robot Operating System）平臺(tái)的機(jī)器人導(dǎo)航路徑規(guī)劃方法，通過(guò)建立仿真的二維柵格地圖對(duì)比不同算法的優(yōu)勢(shì)與不足，利用搭建的ROS機(jī)器人實(shí)物平臺(tái)，完成導(dǎo)航功能中路徑規(guī)劃的驗(yàn)證。機(jī)器人可以通過(guò)本文提出的路徑規(guī)劃方法，完成導(dǎo)航功能，具有較高的路徑搜索效率和避障成功率。

1? 系統(tǒng)總體架構(gòu)

系統(tǒng)選用上下位機(jī)的架構(gòu)進(jìn)行設(shè)計(jì)，其中上位機(jī)選用Intel酷睿i3處理器，連接激光雷達(dá)和姿態(tài)傳感器，分別完成機(jī)器人距離信息和位姿信息的采集，下位機(jī)為STM32F103C8T6單片機(jī)，通過(guò)單片機(jī)輸出的PWM（Pulse Width Modulation，脈沖寬度調(diào)制）控制舵機(jī)和電機(jī)，完成機(jī)器人轉(zhuǎn)角和速度的底層驅(qū)動(dòng)控制。上位機(jī)搭載ROS，ROS是一個(gè)專門用于機(jī)器人開發(fā)的框架，提供適當(dāng)?shù)牟僮飨到y(tǒng)服務(wù)，不僅包含一系列的工具、庫(kù)和約定等，而且其分布式管理的特點(diǎn)極大程度地提高了開發(fā)效率。系統(tǒng)利用上位機(jī)主控電腦強(qiáng)大的計(jì)算處理能力，完成算法的運(yùn)行與計(jì)算。機(jī)器人搭載的傳感器不斷采集數(shù)據(jù)，上位機(jī)進(jìn)行數(shù)據(jù)處理，將采集到的環(huán)境信息最終計(jì)算轉(zhuǎn)換為機(jī)器人移動(dòng)的角速度與線速度，通過(guò)串口傳輸與下位機(jī)進(jìn)行通訊，最終下位機(jī)接收到速度信息后，完成機(jī)器人的移動(dòng)控制。系統(tǒng)總體架構(gòu)圖如圖1所示。

2? 全局路徑規(guī)劃方法

全局路徑規(guī)劃是一種基于地圖的靜態(tài)路徑規(guī)劃方法，利用已建立的環(huán)境地圖信息和位置信息，機(jī)器人在開始移動(dòng)前完成路徑的搜尋[4]?；谠撀窂皆诃h(huán)境無(wú)動(dòng)態(tài)障礙增加的條件下，應(yīng)滿足所搜尋路徑無(wú)限接近理想的最短路徑，且具有較高的搜索效率。

A*算法作為一種全局路徑規(guī)劃算法，其繼承了Dijkstra算法遍歷點(diǎn)集的思想，并在此基礎(chǔ)上加入了啟發(fā)式搜索的思想。由于Dijkstra算法需要遍歷全部點(diǎn)集，在實(shí)際應(yīng)用過(guò)程中雖然可以得到較優(yōu)的路徑，但是搜索效率有所欠缺，于是A*算法引入評(píng)估函數(shù)，不僅評(píng)估搜索點(diǎn)到目標(biāo)點(diǎn)的評(píng)分估值，而且增加起始點(diǎn)到搜索點(diǎn)的評(píng)分估值，通過(guò)為兩者評(píng)分估值賦予不同權(quán)重，將最終評(píng)分作為啟發(fā)式搜索的代價(jià)，可極大地提高算法運(yùn)行效率。評(píng)價(jià)函數(shù)如式（1）所示：

H（s）函數(shù)的選取決定著A*算法的成敗，在評(píng)估當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的代價(jià)值即H（s）時(shí)，應(yīng)綜合考慮搜索效率、是否有最優(yōu)解等因素?？蛇x用式（2）中的歐幾里得距離作為評(píng)估，使用此方法可得到的路徑較短，但是會(huì)增加計(jì)算量，使得搜索效率有所下降，在應(yīng)用于對(duì)搜索效率要求較高的場(chǎng)景時(shí)，可選用式（3）中的曼哈頓距離作為評(píng)估。定義（xi，yi）和（xj，yj）分別表示當(dāng)前和目標(biāo)點(diǎn)坐標(biāo)，評(píng)價(jià)公式如式（2）、式（3）所示：

使用A*算法進(jìn)行路徑規(guī)劃時(shí)，以當(dāng)前搜索點(diǎn)為父節(jié)點(diǎn)，通過(guò)評(píng)價(jià)函數(shù)確定下一個(gè)與父節(jié)點(diǎn)連通的子節(jié)點(diǎn)并記錄下來(lái)，通過(guò)不斷更新子節(jié)點(diǎn)，直至子節(jié)點(diǎn)更新為目標(biāo)點(diǎn)時(shí)算法完成遍歷，通過(guò)記錄的子節(jié)點(diǎn)回溯父節(jié)點(diǎn)，構(gòu)成完整的點(diǎn)集，即為獲得的規(guī)劃路徑。

3? 局部路徑規(guī)劃方法

局部實(shí)時(shí)路徑規(guī)劃采用DWA算法，即動(dòng)態(tài)窗口算法，在機(jī)器人完成全局路徑規(guī)劃后[5]，每次做出移動(dòng)動(dòng)作前調(diào)用此算法，以判斷在該固定窗口范圍內(nèi)全局規(guī)劃的路徑是否安全，計(jì)算出最優(yōu)的實(shí)時(shí)路徑完成機(jī)器人的控制[6，7]。該算法原理為通過(guò)對(duì)機(jī)器人移動(dòng)的速度不斷采樣[8]，同時(shí)用當(dāng)前采樣速度來(lái)模擬機(jī)器人在固定時(shí)間內(nèi)的移動(dòng)軌跡，并進(jìn)行綜合評(píng)價(jià)，選用最優(yōu)路徑完成對(duì)機(jī)器人的實(shí)時(shí)控制。DWA算法具體流程如下。

3.1? 運(yùn)動(dòng)建模

系統(tǒng)的機(jī)械結(jié)構(gòu)通過(guò)舵機(jī)進(jìn)行轉(zhuǎn)向，通過(guò)電機(jī)提供前進(jìn)動(dòng)力[9]。根據(jù)機(jī)器人運(yùn)動(dòng)原理，可以得到機(jī)器人的運(yùn)動(dòng)速度（vt，ωt），其中，vt為機(jī)器人的線速度，ωt為機(jī)器人的角速度，設(shè)定機(jī)器人航向角為θ，則在t+1時(shí)刻機(jī)器人的運(yùn)動(dòng)位姿方程如下：

3.2? 速度采樣

通過(guò)對(duì)機(jī)器人速度的不斷采樣，完成機(jī)器人在該運(yùn)動(dòng)速度下的運(yùn)動(dòng)軌跡模擬，在進(jìn)行速度采樣的過(guò)程中，可根據(jù)機(jī)器人傳感器的性能、電機(jī)的性能、驅(qū)動(dòng)的性能等因素，對(duì)運(yùn)動(dòng)速度進(jìn)行限制，以保證機(jī)器人具有最佳的運(yùn)動(dòng)性能。

3.3? 綜合評(píng)價(jià)

選用式（7）的指定評(píng)價(jià)函數(shù)對(duì)移動(dòng)機(jī)器人的軌跡進(jìn)行綜合評(píng)價(jià)，通過(guò)對(duì)不同路徑的結(jié)果分析，得到最為合適的局部路徑，可以有效地躲避障礙并且極大程度地貼近原始全局路徑。

其中，vn表示機(jī)器人實(shí)時(shí)速度，Gn表示移動(dòng)到目標(biāo)點(diǎn)的代價(jià)，ΔGn表示當(dāng)前路徑的距離和全局規(guī)劃路徑距離的代價(jià)值差值，Hn表示移動(dòng)到達(dá)目標(biāo)點(diǎn)的代價(jià)。通過(guò)調(diào)整式（7）中四個(gè)常數(shù)α、β、γ、δ，進(jìn)而調(diào)整不同代價(jià)所占權(quán)值，從而求解局部路徑中的最優(yōu)規(guī)劃，最終將最優(yōu)規(guī)劃信息轉(zhuǎn)換為移動(dòng)機(jī)器人的速度指令進(jìn)行發(fā)布。

4? 實(shí)驗(yàn)

4.1? 仿真與分析

使用MATLAB建立帶障礙物的柵格地圖，假設(shè)0代表無(wú)障礙可通過(guò)處即白色部分，1代表有障礙物即黑色部分，設(shè)置起始點(diǎn)與目標(biāo)點(diǎn)，分別使用Dijkstra算法和A*算法進(jìn)行路徑規(guī)劃，所得規(guī)劃路徑如圖2、圖3所示。

在保證起始點(diǎn)坐標(biāo)與目標(biāo)點(diǎn)坐標(biāo)不發(fā)生改變的情況下，進(jìn)行10次重復(fù)實(shí)驗(yàn)，記錄實(shí)驗(yàn)數(shù)據(jù)如表1所示。

通過(guò)仿真所得規(guī)劃路徑可以看出，兩種路徑規(guī)劃算法均可避開障礙物，順利到達(dá)終點(diǎn)，得到較優(yōu)的路徑。根據(jù)實(shí)驗(yàn)數(shù)據(jù)可知，A*算法所得路徑與Dijkstra算法所得路徑長(zhǎng)度幾乎一致，但是搜索時(shí)間明顯縮短，搜索效率較Dijkstra算法更高。

4.2? 實(shí)物環(huán)境實(shí)驗(yàn)

利用搭建的基于ROS的導(dǎo)航機(jī)器人平臺(tái)，在實(shí)物環(huán)境下進(jìn)行路徑規(guī)劃技術(shù)的實(shí)驗(yàn)。分別選擇矩形環(huán)境、環(huán)形環(huán)境、不同角度的U形環(huán)境進(jìn)行實(shí)驗(yàn)，利用機(jī)器人構(gòu)建環(huán)境地圖后，設(shè)置目標(biāo)點(diǎn)導(dǎo)航，進(jìn)行路徑規(guī)劃實(shí)驗(yàn)。全局路徑規(guī)劃算法選擇A*算法，局部路徑規(guī)劃使用DWA算法。如圖4所示是機(jī)器人導(dǎo)航過(guò)程中起始點(diǎn)與目標(biāo)點(diǎn)間完成的路徑規(guī)劃。

保證每次機(jī)器人的出發(fā)點(diǎn)與目標(biāo)點(diǎn)相同的條件下，在不同環(huán)境中分別在靜態(tài)條件和設(shè)置動(dòng)態(tài)障礙物條件下進(jìn)行實(shí)驗(yàn)，重復(fù)20次實(shí)驗(yàn)，分別將實(shí)驗(yàn)結(jié)果記錄在表2中。

通過(guò)實(shí)驗(yàn)結(jié)果統(tǒng)計(jì)可知，使用本文提出的方法，可滿足移動(dòng)機(jī)器人導(dǎo)航過(guò)程中對(duì)于路徑規(guī)劃的要求。不僅可以規(guī)劃出較優(yōu)的全局路徑，使得機(jī)器人獲得較短的行進(jìn)路線，而且對(duì)于設(shè)置的動(dòng)態(tài)障礙物，有著較高的避障成功率。

5? 結(jié)? 論

對(duì)于全局路徑規(guī)劃，經(jīng)典的Dijkstra算法與本文介紹的A*算法相比，雖然可規(guī)劃出更優(yōu)的路線，但是耗時(shí)較長(zhǎng)，A*算法擁有更高的效率，更適用于對(duì)實(shí)時(shí)性要求較高的導(dǎo)航機(jī)器人。經(jīng)過(guò)實(shí)驗(yàn)驗(yàn)證，本文提出的基于ROS的導(dǎo)航機(jī)器人路徑規(guī)劃方法，在實(shí)際應(yīng)用中有著良好的效果，不僅可在較短時(shí)間內(nèi)完成全局路徑規(guī)劃，且可以得到較短的路線，而且對(duì)于動(dòng)態(tài)的障礙躲避，局部路徑規(guī)劃也有著較高的成功率。

參考文獻(xiàn)：

[1] 彭曉燕，謝浩，黃晶.無(wú)人駕駛汽車局部路徑規(guī)劃算法研究 [J].汽車工程，2020，42（1）：1-10.

[2] ZHANG C. Path Planning for Robot Based on Chaotic Artificial Potential Field Method [J]. IOP Conference Series：Materials Science and Engineering，2018，317（1）：12-56.

[3] ZHANG Y，CHEN H Y，WASLANDER S L，et al. Hybrid Trajectory Planning for Autonomous Driving in Highly Constrained Environments [J].IEEE Access，2018，6：32800-32819.

[4] VOISAN E I，PAULIS B，PRECUP R E，et al. ROS-based robot navigation and human interaction in indoor environment [C]//IEEE Jubilee International Symposium on Applied Computational Intelligence and Informatics. IEEE，2015：31-36.

[5] 鮑慶勇，李舜酩，沈峘，等.自主移動(dòng)機(jī)器人局部路徑規(guī)劃綜述 [J].傳感器與微系統(tǒng)，2009，28（9）：1-4+11.

[6] 張琳，章新杰，郭孔輝，等.未知環(huán)境下智能汽車軌跡規(guī)劃滾動(dòng)窗口優(yōu)化 [J].吉林大學(xué)學(xué)報(bào)（工學(xué)版），2018，48（3）：652-660.

[7] AFANASYEV I，SAGITOV A，MAGID E. ROS-Based SLAM for a Gazebo-Simulated Mobile Robot in Image-Based 3D Model of Indoor Environment [C]//International Conference on Advanced Concepts for Intelligent Vision Systems，ACIVS 2015. Switzerland：Springer，Cham，2015：273-283.

[8] 胡思旺，李春杰.基于ROS的自主導(dǎo)航系統(tǒng)仿真設(shè)計(jì) [J].自動(dòng)化與儀表，2019，34（07）：80-83+108.

[9] 孫小霞，石長(zhǎng)華.基于PIC16F873單片機(jī)的汽車自適應(yīng)巡航控制系統(tǒng)的研究 [J].現(xiàn)代信息科技，2019，3（17）：42-44.

作者簡(jiǎn)介：李崢（1997—），男，漢族，吉林汪清人，本科，研究方向：測(cè)控技術(shù)與儀器。