QT、ROS下實(shí)現(xiàn)導(dǎo)航與自動(dòng)避障功能小車

何威， 王建春， 曾奕雄， 謝穗文， 黃梓塨

（華南理工大學(xué) 廣州學(xué)院，廣州515000）

0 引 言

本文所研究的導(dǎo)航與自動(dòng)避障功能小車項(xiàng)目主要是使用Linux系統(tǒng)來(lái)完成的。使用Linux系統(tǒng)能保證跨平臺(tái)的硬件支持、可靠的安全性、良好的穩(wěn)定性，同時(shí)由于自動(dòng)避障的功能是通過(guò)ROS平臺(tái)實(shí)現(xiàn)的，分布式的ROS節(jié)點(diǎn)提供了各種高效的環(huán)境感知和運(yùn)動(dòng)規(guī)劃等算法，并通過(guò)它們完成了系統(tǒng)的大部分運(yùn)算，基于ROS已經(jīng)實(shí)現(xiàn)了對(duì)大部分典型傳感器和硬件設(shè)備的軟件支持，如FireWare攝像頭等[1]。同時(shí)，ROS在Linux中擁有簡(jiǎn)單的搭建與開發(fā)等功能，所以在開發(fā)的過(guò)程中能夠省去很多不必要的步驟。

路徑規(guī)劃總體上可分為全局路徑規(guī)劃和局部路徑規(guī)劃，而路徑規(guī)劃又是自動(dòng)避障功能小車項(xiàng)目的一個(gè)不可缺少的部分，它能夠通過(guò)按一定的標(biāo)準(zhǔn)來(lái)規(guī)劃路線，實(shí)現(xiàn)一條最優(yōu)或近似最優(yōu)的無(wú)碰撞路徑，進(jìn)而避開對(duì)障礙物的碰撞，以提高對(duì)小車的控制效果。

伴隨著嵌入式系統(tǒng)計(jì)算能力的不斷提高，強(qiáng)大的圖像處理分析算法完全可以在靈活小巧、機(jī)動(dòng)性強(qiáng)的嵌入式系統(tǒng)上運(yùn)行?；谇度胧较到y(tǒng)進(jìn)一步實(shí)現(xiàn)的計(jì)算機(jī)視覺(jué)完全可應(yīng)用于安防領(lǐng)域和邊海防等軍事領(lǐng)域的實(shí)時(shí)監(jiān)控[2]。

1 小車整體介紹

小車設(shè)計(jì)的目標(biāo)是不僅能夠?qū)崿F(xiàn)智能化、自動(dòng)化，而且還能實(shí)現(xiàn)運(yùn)輸物品、清掃路障等功能，與市面上物流公司的智能物流小車相比，具有更加靈活輕便和價(jià)格上的優(yōu)勢(shì)，是一款符合市場(chǎng)趨勢(shì)的功能小車。而實(shí)現(xiàn)上述功能的關(guān)鍵點(diǎn)就是得有Linux系統(tǒng)的支持和ROS機(jī)器人操作系統(tǒng)的使用，同時(shí)借助百度地圖API、樹莓派、ROS自帶的move_base框架等來(lái)實(shí)現(xiàn)上述功能。整個(gè)項(xiàng)目的流程是先利用GPS獲得當(dāng)前小車坐標(biāo)，再輸入目的地地址的坐標(biāo)，通過(guò)百度地圖API進(jìn)行路徑規(guī)劃，使用激光雷達(dá)來(lái)實(shí)現(xiàn)實(shí)時(shí)避障的功能，在這基礎(chǔ)上利用樹莓派及WIFI傳輸數(shù)據(jù)獲得當(dāng)前小車實(shí)時(shí)位置發(fā)送到監(jiān)控端，而控制模塊move_base作為小車的最關(guān)鍵的點(diǎn)，其控制著小車的轉(zhuǎn)向和速度，也間接地決定著路徑規(guī)劃、地圖導(dǎo)航、自動(dòng)避障等的準(zhǔn)確性。

程序要點(diǎn)包括：1）局部路徑規(guī)劃與百度導(dǎo)航；2）樹莓派通過(guò)WIFI發(fā)送傳感器數(shù)據(jù)；3）控制模塊move_base等幾個(gè)方面，下文講逐一講解。

2 局部路徑規(guī)劃與百度導(dǎo)航

在全局路徑的規(guī)劃中需要使用百度地圖API的導(dǎo)航功能，用GPS可確定當(dāng)前位置的經(jīng)緯度，然后選擇目的地。通過(guò)百度地圖API導(dǎo)航功能進(jìn)行全局的路徑規(guī)劃，指引小車運(yùn)動(dòng)的大致方向。局部路徑規(guī)劃，意即自動(dòng)避障。按照路徑生成方式的不同可以將局部路徑規(guī)劃大致分為4類：基于圖搜索的規(guī)劃方法，基于曲線插值的規(guī)劃方法，基于隨機(jī)采樣的規(guī)劃方法及基于群智能優(yōu)化的規(guī)劃方法。A*算法和Dijkstra算法是最常用的基于圖搜索的路徑規(guī)劃方法[3]。在這里我們采用的是ROS開源社區(qū)提供的ros-navigation導(dǎo)航集合包來(lái)進(jìn)行局部規(guī)劃，在導(dǎo)航功能指引的大致路線的情況下，如若在行駛的方向遇到障礙，通過(guò)自動(dòng)避障的算法能躲避障礙，并且遠(yuǎn)程控制端可實(shí)現(xiàn)對(duì)小車的控制和位置信息讀取。

2.1 局部路徑規(guī)劃

ROS開源社區(qū)提供的ros-navigation導(dǎo)航集合包中的move_base節(jié)點(diǎn)，是導(dǎo)航過(guò)程運(yùn)動(dòng)控制的最終執(zhí)行機(jī)構(gòu)，move_base訂閱用戶發(fā)布的導(dǎo)航目標(biāo)move_base_simple/goal，實(shí)時(shí)運(yùn)動(dòng)控制的信號(hào)cmd_vel下發(fā)給底盤用以實(shí)現(xiàn)最終運(yùn)動(dòng)控制，move_base中的各種導(dǎo)航算法模塊都是以插件的形式進(jìn)行調(diào)用的，這樣可以替換不同的算法以適應(yīng)不同的應(yīng)用，其中：local_planner用于局部路徑規(guī)劃；local_costmap是局部代價(jià)地圖，用于描述局部環(huán)境信息；recovery_behaviors是恢復(fù)策略，用于機(jī)器人碰到障礙后自動(dòng)進(jìn)行逃避恢復(fù)，其中用到的costmap代價(jià)地圖是利用激光雷達(dá)等數(shù)據(jù)生成的，大致原理是通過(guò)建立不同圖層Layer然后疊加，被填充的柵格點(diǎn)表示有障礙物。實(shí)現(xiàn)自動(dòng)避障的核心就是路徑規(guī)劃，路徑規(guī)劃就是利用環(huán)境障礙信息找到一條到達(dá)目標(biāo)并且開銷最小的路徑。本項(xiàng)目使用的路徑規(guī)劃器是teb_local_planner，即基于彈性時(shí)間帶碰撞約束的算法，算法將動(dòng)態(tài)障礙物、運(yùn)行時(shí)效、路徑平滑等約束綜合考慮。獲取激光雷達(dá)所掃描后的信息，生成2D-costmap,在完成對(duì)tf坐標(biāo)轉(zhuǎn)換、信息參數(shù)配置、里程計(jì)及消息訂閱之后，便能進(jìn)行局部路徑規(guī)劃。

路徑規(guī)劃是基于網(wǎng)格表示，通過(guò)概率模型判斷出障礙物所在的位置，起初使用的是單片成本映射，在成本映射中的大多數(shù)信息存儲(chǔ)在同一個(gè)位置的主要限制，現(xiàn)在使用分層成本圖，依然是使用二維的網(wǎng)格表示，但分層成本不是直接在網(wǎng)格里存儲(chǔ)數(shù)據(jù)，而是有序的層列表，每層都跟蹤與特定功能相關(guān)的數(shù)據(jù)，然后疊加至主成本。實(shí)現(xiàn)圖層首先要?jiǎng)?chuàng)建擴(kuò)展costmap2D:Layer類的新類，也就是實(shí)現(xiàn)初始化函數(shù)的updateBounds函數(shù)和updateCosts函數(shù)。先通過(guò)Gazebo模擬實(shí)驗(yàn)運(yùn)行，經(jīng)過(guò)多次運(yùn)行成功后，再使用項(xiàng)目小車進(jìn)行實(shí)驗(yàn)。

圖1 一組costmap圖層

2.2 百度導(dǎo)航

百度地圖API是一套由JavaScript語(yǔ)言編寫的應(yīng)用程序接口，能夠幫助程序員在網(wǎng)站中構(gòu)建功能豐富、交互性強(qiáng)的個(gè)性化地圖應(yīng)用程序[3]。在QT中使用百度地圖API，從百度地圖API的開發(fā)文檔中選擇web端接口，在QT中顯示地圖、標(biāo)點(diǎn)（當(dāng)前位置點(diǎn)，目標(biāo)位置點(diǎn)），輸入目標(biāo)名稱得到其位置以及導(dǎo)航功能。由于在項(xiàng)目中使用的是QT5.6版本，QT5.6取消了QWebKits，而是使用QWebEngine。所以在UI設(shè)計(jì)師界面上就找不到QWebView，將游覽器嵌入程序中，所以在此使用QWebEngineView，就能將HTML在程序中顯示，Qt5.6 以后與JavaScript 交互可以使用runJavaScript()函數(shù)。由于在地圖未加載完成時(shí)，無(wú)法完成交互，因此需要加入loadFinished(bool)進(jìn)行判斷。讀取GPS模塊的數(shù)據(jù)，常用的GPS模塊大多采用NMEA-0183協(xié)議，GPS固定數(shù)據(jù)輸出語(yǔ)句（$GPGGA）,是一幀GPS定位的主要數(shù)據(jù)，其標(biāo)準(zhǔn)格式為：

該項(xiàng)目主要需要使用的是：(2) 緯度（格式為dd mm. mm mm，即dd度，mm. mm mm分）；（3）經(jīng)度（格式為ddd mm. mm mm，即ddd度，mm. mm mm分）；(12)差分基準(zhǔn)站號(hào)(0000～1023)，×為總和校驗(yàn)域；hh為總和校驗(yàn)數(shù):35(CR)(LF)為回車，換行。GPS模塊讀取主要分為串口設(shè)置于數(shù)據(jù)讀取，數(shù)據(jù)的分析和需要數(shù)據(jù)的提取。在Linux中串口設(shè)備文件是/det/ttyS0orttyS1。首先打開串口，然后根據(jù)GPS模塊配置參數(shù)對(duì)波特率、校驗(yàn)等進(jìn)行設(shè)置。對(duì)GPS數(shù)據(jù)處理中通過(guò)對(duì)讀取的字符串?dāng)?shù)據(jù)的判斷來(lái)提取，首先判斷數(shù)據(jù)為什么類型，然后根據(jù)數(shù)據(jù)格式和逗號(hào)位置提取數(shù)據(jù)信息。得到的經(jīng)緯度需要和百度地圖坐標(biāo)進(jìn)行一個(gè)轉(zhuǎn)換，轉(zhuǎn)換通過(guò)百度地圖API接口進(jìn)行（如果使用離線地圖則可以直接使用經(jīng)緯度），現(xiàn)在可以使用百度地圖API提供的控件加上輸入的GPS數(shù)據(jù)信息實(shí)現(xiàn)當(dāng)前位置點(diǎn)的標(biāo)記。導(dǎo)航功能，在百度地圖API開發(fā)文檔中可以找到web端公交、駕車、步行路線規(guī)劃，其服務(wù)地址是：http://api.map.baidu.com/direction//PC&Webapp服務(wù)地址。在參數(shù)說(shuō)明中以下參數(shù)比較重要（項(xiàng)目當(dāng)前只針對(duì)一個(gè)城市）：origin（起點(diǎn)經(jīng)緯度），destination（終點(diǎn)經(jīng)緯度）及重要的coord_type坐標(biāo)類型?？梢缘玫綄?dǎo)航系統(tǒng)的方向指引。將導(dǎo)航信息打包，按照規(guī)定的數(shù)據(jù)協(xié)議發(fā)布給決策層，決策層依據(jù)全局方向、局部方向進(jìn)行判斷。

3 樹莓派通過(guò)WIFI發(fā)送傳感器數(shù)據(jù)

在嵌入式平臺(tái)之中，未改良的樹莓派里面的CPU占用率占比相對(duì)于改良后的樹莓派來(lái)說(shuō)比較高，因此未改良版容易出現(xiàn)卡死的情況，在此我們選擇改良后的樹莓派以運(yùn)行項(xiàng)目的算法，由于占用率充分下降，故能預(yù)留該算法的改進(jìn)空間?？紤]到樹莓派的性能，首先在樹莓派上使用客戶端，通過(guò)QT的network功能將包括激光雷達(dá)、里程計(jì)及底層返回的數(shù)據(jù)信息發(fā)送至服務(wù)端。服務(wù)端處理完信息之后，將控制信息發(fā)送給樹莓派，樹莓派再通過(guò)串口發(fā)布給底層。QT的網(wǎng)絡(luò)模塊包括#include、#include < QtNetwork / QNet workRequest>、#include，可以直接獲取信息。

4 控制模塊move_base

在完成導(dǎo)航的決策之后使用move_base控制小車轉(zhuǎn)向和速度，在ROS中可以直接調(diào)用相應(yīng)的package。

其中需要目標(biāo)位置、各坐標(biāo)系之間的轉(zhuǎn)換、里程計(jì)、激光雷達(dá)等的信息。Move_base會(huì)在cmd_vel的主題上發(fā)布Twist信息，包含期望的前進(jìn)速度和轉(zhuǎn)向。然后將數(shù)據(jù)發(fā)布給下位機(jī)便可以進(jìn)行控制了。

圖2 樹莓派的GPIO口

圖3 move_base的架構(gòu)

5 實(shí)驗(yàn)測(cè)試

5.1 底層硬件環(huán)境搭建

硬件包括開發(fā)PC主機(jī)一臺(tái)、樹莓派2代B型一個(gè)、激光雷達(dá)Rp2A、GPS模塊、STM32硬件控制器、電動(dòng)機(jī)和相匹配的驅(qū)動(dòng)、串口模塊等搭建的三輪底盤。

三輪控制算法方面使用了PID算法，PID算法是一種廣泛應(yīng)用的閉環(huán)控制算法，在小車的運(yùn)動(dòng)控制層中，需要控制車輪以指定速度運(yùn)動(dòng)指定時(shí)間，若由于電池電壓變化等原因影響電動(dòng)機(jī)驅(qū)動(dòng)能力，則小車的實(shí)際運(yùn)動(dòng)必然與上層指令不一致，通過(guò)引入PID算法對(duì)輪速進(jìn)行閉環(huán)控制，可以精確控制小車的輪速，保持實(shí)際輪速與目標(biāo)輪速一致。

圖4 小車測(cè)試圖片

5.2 上層軟件算法設(shè)計(jì)

通過(guò)下位機(jī)串口獲得當(dāng)前GPS實(shí)時(shí)位置信息和激光雷達(dá)收集的附近障礙物信息，通過(guò)傳感器算法融合，獲得當(dāng)前位置的雷達(dá)圖。通過(guò)已知的GPS信息再與百度地圖API結(jié)合，獲得實(shí)時(shí)可用路徑，再通過(guò)串口發(fā)送給下位機(jī)運(yùn)動(dòng)軌跡信息。

運(yùn)用QT界面，融合上述兩個(gè)算法，得到一個(gè)實(shí)時(shí)觀察的界面信息，直觀可靠地了解小車下一步的運(yùn)動(dòng)軌跡路徑，如圖5所示。

圖5 QT界面融合

6 結(jié) 語(yǔ)

功能小車通過(guò)路徑規(guī)劃、百度導(dǎo)航、傳輸數(shù)據(jù)及控制運(yùn)動(dòng)4個(gè)方面來(lái)達(dá)到項(xiàng)目的要求，設(shè)計(jì)完成的小車不僅優(yōu)于同類的功能小車，而且在降低出錯(cuò)概率的基礎(chǔ)上有著實(shí)現(xiàn)快速到達(dá)指定位置的功能，真正能夠?qū)崿F(xiàn)自動(dòng)化、自動(dòng)避障、準(zhǔn)確導(dǎo)航等功能。在以后的進(jìn)一步研究會(huì)將載重功能、實(shí)際工作效率列為考察項(xiàng)，讓小車可以實(shí)現(xiàn)真正的實(shí)用價(jià)值。