《自主導(dǎo)航技術(shù)與應(yīng)用》課件 第七章自主導(dǎo)航系統(tǒng)應(yīng)用

7.1ROS操作系統(tǒng)及使用ROS（RobotOperatingSystem）是一個開源的機(jī)器人中間件框架，而非傳統(tǒng)操作系統(tǒng)，旨在為機(jī)器人開發(fā)提供跨平臺、跨語言的支持。其核心功能包括硬件抽象、通信機(jī)制、工具鏈和庫函數(shù)，幫助開發(fā)者高效集成傳感器、執(zhí)行器、算法等模塊，實(shí)現(xiàn)機(jī)器人感知、決策與控制的協(xié)同。ROS通過節(jié)點(diǎn)（Node）作為基本執(zhí)行單元，節(jié)點(diǎn)間通過話題（Topic）、服務(wù)（Service）、動作（Action）等通信機(jī)制交互，形成分布式網(wǎng)絡(luò)拓?fù)洌≧OSGraph），支持多任務(wù)并行與模塊化開發(fā)。例如，節(jié)點(diǎn)可獨(dú)立完成圖像處理、運(yùn)動控制等功能，通過話題實(shí)時共享數(shù)據(jù)。ROS分為ROS1（基于TCPROS/ROSCPP通信）和ROS2（基于DDS協(xié)議，支持實(shí)時性與去中心化架構(gòu)）兩大版本。安裝便捷，可通過一鍵腳本（如fishros）或手動配置環(huán)境變量完成。其工具鏈豐富，如Rviz用于可視化傳感器數(shù)據(jù)與地圖，Gazebo進(jìn)行物理仿真，Catkin構(gòu)建編譯系統(tǒng)，支持Python、C++等語言開發(fā)。典型應(yīng)用場景包括SLAM建圖（如Cartographer）、機(jī)械臂運(yùn)動規(guī)劃（MoveIt!）、多機(jī)器人協(xié)同等。ROS社區(qū)活躍，提供大量功能包（Package）與教程，降低了機(jī)器人開發(fā)門檻，廣泛應(yīng)用于教育、科研及工業(yè)領(lǐng)域。7.1.1什么是ROSROS是一個靈活且強(qiáng)大的開源機(jī)器人操作系統(tǒng)，旨在為機(jī)器人軟件開發(fā)提供一個通用的平臺。ROS是構(gòu)建機(jī)器人應(yīng)用程序的一系列庫、工具和程序包的集合(如下圖)，它通過提供硬件抽象、設(shè)備驅(qū)動、庫函數(shù)、可視化工具、消息傳遞和軟件包管理等功能，可以極大簡化繁雜多樣的機(jī)器人平臺下的復(fù)雜任務(wù)創(chuàng)建與穩(wěn)定行為控制。ROS由核心通信機(jī)制、開發(fā)工具、應(yīng)用功能和生態(tài)系統(tǒng)四個部分組成7.1.1什么是ROS2）硬件無關(guān)性：ROS提供了對各種硬件設(shè)備的驅(qū)動和接口支持，使得開發(fā)者能夠方便地與不同類型的傳感器、執(zhí)行器等設(shè)備進(jìn)行交互。3）開放性和共享性：ROS是一個開源項(xiàng)目，擁有龐大的用戶社區(qū)和豐富的資源庫，開發(fā)者可以共享自己的軟件包或從社區(qū)中獲取現(xiàn)有的軟件包，加快開發(fā)進(jìn)程。4)可視化工具：ROS提供了可視化工具，如Rviz(RobotVisualizer)和RQt(RosQt)，方便開發(fā)者實(shí)時查看和調(diào)試機(jī)器人的狀態(tài)、傳感器數(shù)據(jù)和運(yùn)動軌跡。5）強(qiáng)大的生態(tài)系統(tǒng)：ROS生態(tài)系統(tǒng)中存在眾多的第三方軟件包和庫，支持各種功能，如機(jī)器人感知、導(dǎo)航、運(yùn)動控制等，能夠滿足不同應(yīng)用需求。7.1.2ROS架構(gòu)ROS一般分成操作系統(tǒng)(OperationSystem)層、中間層和應(yīng)用層。目前ROS分成ROS1和ROS2兩種，本章采用的演示環(huán)境是Ubuntu18.04+ROS(MelodicMorenia)，是基于ROS1來操作的，為避免混淆，下文ROS均表示ROS1。ROS的架構(gòu)如下圖所示。ROS并不是一個傳統(tǒng)意義上的操作系統(tǒng)，要依托于Linux系統(tǒng)在OS層，可以直接使用ROS官方支持度最好的Ubuntu操作系統(tǒng)，也可以使用macOS、Arch、Debian等操作系統(tǒng)。7.1.2ROS架構(gòu)其次，Linux并沒有針對機(jī)器人開發(fā)提供特殊的中間件，因此ROS在中間層做了大量工作，其中最重要的就是基于TCPROS/UDPROS的通信系統(tǒng)。ROS的通信系統(tǒng)基于TCP/UDP網(wǎng)絡(luò)，在此之上進(jìn)行了再次封裝，也就是TCPROS/UDPROS。通信系統(tǒng)使用發(fā)布/訂閱、客戶端/服務(wù)器等模型，實(shí)現(xiàn)多種通信機(jī)制的數(shù)據(jù)傳輸。在通信機(jī)制之上，ROS提供了大量機(jī)器人開發(fā)相關(guān)的庫，如數(shù)據(jù)類型定義、坐標(biāo)變換、運(yùn)動控制等，可以提供給應(yīng)用層使用。7.1.2ROS架構(gòu)最后，在應(yīng)用層，ROS需要運(yùn)行一個管理者——Master，負(fù)責(zé)管理整個系統(tǒng)的正常運(yùn)行。ROS社區(qū)內(nèi)共享了大量的機(jī)器人應(yīng)用功能包，這些功能包內(nèi)的模塊以節(jié)點(diǎn)為單位運(yùn)行，以ROS標(biāo)準(zhǔn)的輸入輸出作為接口，開發(fā)者不需要關(guān)注模塊的內(nèi)部實(shí)現(xiàn)機(jī)制，只需要了解接口規(guī)則即可實(shí)現(xiàn)復(fù)用，極大地提高了開發(fā)效率。7.1.3相關(guān)名詞解釋在使用ROS開發(fā)機(jī)器人時，總是不可避免的要接觸一些在ROS中特定的重要名詞概念。1）節(jié)點(diǎn)節(jié)點(diǎn)(Node)就是一些執(zhí)行運(yùn)算任務(wù)的進(jìn)程，一個系統(tǒng)一般由多個節(jié)點(diǎn)組成，也可以稱為“軟件模塊”。節(jié)點(diǎn)概念的引入使得基于ROS的系統(tǒng)在運(yùn)行時更加形象。如下圖所示，在這個圖中，進(jìn)程就是圖中的節(jié)點(diǎn)，而端對端的連接關(guān)系就是節(jié)點(diǎn)之間的連線。7.1.3相關(guān)名詞解釋2）消息節(jié)點(diǎn)之間最重要的通信機(jī)制就是基于發(fā)布/訂閱模型的消息(Message)通信。每一個消息都是一種嚴(yán)格的數(shù)據(jù)結(jié)構(gòu)，支持標(biāo)準(zhǔn)數(shù)據(jù)類型(整型、浮點(diǎn)型、布爾型等)，也支持嵌套結(jié)構(gòu)和數(shù)組(類似于C語言的結(jié)構(gòu)體struct)，還可以根據(jù)需求由開發(fā)者自主定義。例如下圖就是利用接口定義語言描述的一個點(diǎn)的消息。7.1.3相關(guān)名詞解釋3）話題消息以一種發(fā)布/訂閱(Publish/Subscribe)的方式傳遞(見下圖）。一個節(jié)點(diǎn)可以針對一個給定的話題(Topic)發(fā)布消息(稱為發(fā)布者)，也可以關(guān)注某個話題并訂閱特定類型的數(shù)據(jù)(稱為訂閱者)。發(fā)布者和訂閱者并不了解彼此的存在，系統(tǒng)中可能同時有多個節(jié)點(diǎn)發(fā)布或者訂閱同一個話題的消息。7.1.3相關(guān)名詞解釋4）服務(wù)雖然基于話題的發(fā)布/訂閱模型是一種很靈活的通信模式，但是對于雙向的同步傳輸模式并不適合。在ROS中，我們稱這種同步傳輸模式為服務(wù)(Service)，其基于客戶端/服務(wù)器(Client/Server)模型，包含兩個部分的通信數(shù)據(jù)類型：一個用于請求，另一個用于應(yīng)答，類似于Web服務(wù)器。與話題不同的是，ROS中只允許有一個節(jié)點(diǎn)提供指定命名的服務(wù)。7.1.3相關(guān)名詞解釋5）節(jié)點(diǎn)管理器為了統(tǒng)籌管理以上概念，系統(tǒng)當(dāng)中需要有一個控制器使得所有節(jié)點(diǎn)有條不紊地執(zhí)行，這就是ROS節(jié)點(diǎn)管理器(ROSMaster)。ROSMaster通過遠(yuǎn)程過程調(diào)用(RPC,RemoteProcedureCall)提供登記列表和對其他計(jì)算圖表的查找功能，幫助ROS節(jié)點(diǎn)之間相互查找、建立連接，同時還為系統(tǒng)提供參數(shù)服務(wù)器，管理全局參數(shù)。ROSMaster就是一個管理者，沒有它的話，節(jié)點(diǎn)將無法找到彼此，也無法交換消息或調(diào)用服務(wù)，整個系統(tǒng)將會癱瘓，由此可見其在ROS系統(tǒng)中的重要性。7.1.3相關(guān)名詞解釋6）文件系統(tǒng)類似于操作系統(tǒng)，ROS將所有文件按照一定的規(guī)則進(jìn)行組織，不同功能的文件被放置在不同的文件夾下，如右圖所示。功能包(Package)：功能包是ROS軟件中的基本單元，包含ROS節(jié)點(diǎn)、庫、配置文件等。功能包清單(PackageManifest)：每個功能包都包含一個名為package.xml的功能包清單，用于記錄功能包的基本信息，包含作者信息、許可信息、依賴選項(xiàng)、編譯標(biāo)志等。元功能包(MetaPackage)：在新版本的ROS中，將原有功能包集(Stack)的概念升級為“元功能包”，主要作用都是組織多個用于同一目的的功能包。例如一個ROS導(dǎo)航的元功能包中會包含建模、定位、導(dǎo)航等多個功能包。消息（Message）類型：消息是ROS節(jié)點(diǎn)之間發(fā)布/訂閱的通信信息，可以使用ROS提供的消息類型，也可以使用.msg文件在功能包的msg文件夾下自定義所需要的消息類型。服務(wù)（Service）類型：服務(wù)類型定義了ROS客戶端/服務(wù)器通信模型下的請求與應(yīng)答數(shù)據(jù)類型，可以使用ROS系統(tǒng)提供的服務(wù)類型，也可以使用.srv文件在功能包的srv文件夾中進(jìn)行定義。代碼（Code）：用來放置功能包節(jié)點(diǎn)源代碼的文件夾。7.1.4ROS通信機(jī)制ROS的分布式框架，為用戶提供多節(jié)點(diǎn)(進(jìn)程)之間的通信服務(wù)，所有軟件功能和工具都建立在這種分布式通信機(jī)制上，所以ROS的通信機(jī)制是底層也是核心的技術(shù)。在大多數(shù)應(yīng)用場景下，盡管不需要關(guān)注底層通信的實(shí)現(xiàn)機(jī)制，但是了解其相關(guān)原理一定會幫助我們在開發(fā)過程中更好地使用ROS。以下就ROS核心的三種通信機(jī)制進(jìn)行介紹。話題通信(發(fā)布訂閱模式)服務(wù)通信(請求響應(yīng)模式)參數(shù)服務(wù)器(參數(shù)共享模式)7.1.4ROS通信機(jī)制話題在ROS中使用最為頻繁，其通信模型也較為復(fù)雜。如圖所示話題通信實(shí)現(xiàn)模型是比較復(fù)雜的，該模型如下圖所示,該模型中涉及到三個角色:ROSMaster(管理者)、Talker(發(fā)布者)和Listener(訂閱者)。ROSMaster負(fù)責(zé)保管Talker和Listener注冊的信息，并匹配話題相同的Talker與Listener，幫助Talker與Listener建立連接，連接建立后，Talker可以發(fā)布消息，且發(fā)布的消息會被Listener訂閱。1.話題通信機(jī)制7.1.4ROS通信機(jī)制整個流程由以下步驟實(shí)現(xiàn):(1)Talker注冊Talker啟動，通過1234端口使用RPC向ROSMaster注冊發(fā)布者的信息，包含所發(fā)布消息的話題名；ROSMaster會將節(jié)點(diǎn)的注冊信息加入注冊列表中。ROSMaster會將節(jié)點(diǎn)的注冊信息加入到注冊表中。(2)Listener注冊Listener啟動后，也會通過RPC在ROSMaster中注冊自身信息，包含需要訂閱消息的話題名。ROSMaster會將節(jié)點(diǎn)的注冊信息加入到注冊表中。(3)ROSMaster實(shí)現(xiàn)信息匹配ROSMaster會根據(jù)注冊表中的信息匹配Talker和Listener，并通過RPC向Listener發(fā)送Talker的RPC地址信息。(4)Listener向Talker發(fā)送請求Listener根據(jù)從Master發(fā)回的Talker地址信息，通過RPC向Talker發(fā)送連接請求，傳輸訂閱的話題名稱、消息類型以及通信協(xié)議(TCP/UDP)。(5)Talker確認(rèn)請求Talker接收到Listener的請求后，也是通過RPC向Listener確認(rèn)連接信息，并發(fā)送自身的TCP地址信息。(6)Listener與Talker件里連接Listener根據(jù)步驟4返回的消息使用TCP與Talker建立網(wǎng)絡(luò)連接。(7)Talker向Listener發(fā)送消息1.話題通信機(jī)制7.1.4ROS通信機(jī)制2.服務(wù)通信機(jī)制服務(wù)是一種帶有應(yīng)答的通信機(jī)制，通信原理如圖所示，與話題的通信相比，其減少了Listener與Talker之間的RPC通信。整個流程由以下步驟實(shí)現(xiàn):服務(wù)是一種帶有應(yīng)答的通信機(jī)制，通信原理如圖所示，與話題的通信相比，其減少了Listener與Talker之間的RPC通信。具體步驟如下：(1)Talker注冊Talker啟動，通過1234端口使用RPC向ROSMaster注冊發(fā)布者的信息，包含所提供的服務(wù)名；ROSMaster會將節(jié)點(diǎn)的注冊信息加入注冊列表中。(2)Listener注冊Listener啟動，同樣通過RPC向ROSMaster注冊訂閱者的信息，包含需要查找的服務(wù)名。(3)ROSMaster進(jìn)行信息匹配Master根據(jù)Listener的訂閱信息從注冊列表中進(jìn)行查找，如果沒有找到匹配的服務(wù)提供者，則等待該服務(wù)的提供者加入；如果找到匹配的服務(wù)提供者信息，則通過RPC向Listener發(fā)送Talker的TCP地址信息。(4)Listener與Talker建立網(wǎng)絡(luò)連接Listener接收到確認(rèn)信息后，使用TCP嘗試與Talker建立網(wǎng)絡(luò)連接，并且發(fā)送服務(wù)的請求數(shù)據(jù)。(5)Talker向Listener發(fā)布服務(wù)應(yīng)答數(shù)據(jù)Talker接收到服務(wù)請求和參數(shù)后，開始執(zhí)行服務(wù)功能，執(zhí)行完成后，向Listener發(fā)送應(yīng)答數(shù)據(jù)。2.服務(wù)通信機(jī)制7.1.4ROS通信機(jī)制3.參數(shù)通信機(jī)制參數(shù)類似于ROS中的全局變量，由ROSMaster進(jìn)行管理，其通信機(jī)制較為簡單，不涉及TCP/UDP的通信7.1.4ROS通信機(jī)制整個流程由以下步驟實(shí)現(xiàn):(1)Talker設(shè)置變量Talker使用RPC向ROSMaster發(fā)送參數(shù)設(shè)置數(shù)據(jù)，包含參數(shù)名和參數(shù)值；ROSMaster會將參數(shù)名和參數(shù)值保存到參數(shù)列表中。(2)Listener查詢參數(shù)值Listener通過RPC向ROSMaster發(fā)送參數(shù)查找請求，包含所要查找的參數(shù)名。(3)ROSMaster向Listener發(fā)送參數(shù)值Master根據(jù)Listener的查找請求從參數(shù)列表中進(jìn)行查找，查找到參數(shù)后，使用RPC將參數(shù)值發(fā)送給Listener。這里需要注意的是，如果Talker向Master更新參數(shù)值，Listener在不重新查詢參數(shù)值的情況下是無法知曉參數(shù)值已經(jīng)被更新的。所以在很多應(yīng)用場景中，需要一種動態(tài)參數(shù)更新的機(jī)制。3.參數(shù)通信機(jī)制7.1.4ROS通信機(jī)制在深入研究怎么利用ROS開發(fā)機(jī)器人之前，我們簡單嘗試ROS的使用。首先啟動ROS自帶的小海龜測試程序，依次在三個終端中輸入以下三條指令：$roscore$rosrunturtlesimturtlesim_node$rosrunturtlesimturtle_teleop_key在深入研究怎么利用ROS開發(fā)機(jī)器人之前，我們簡單嘗試ROS的使用。首先啟動ROS自帶的小海龜測試程序，依次在三個終端中輸入以下三條指令：$roscore$rosrunturtlesimturtlesim_node$rosrunturtlesimturtle_teleop_key3.參數(shù)通信機(jī)制如圖所示，小海龜可以通過鍵盤上下左右移動，表明ROS基本的環(huán)境安裝成功。7.1.5ROS使用簡例7.2激光雷達(dá)導(dǎo)航系統(tǒng)實(shí)例在ROS系統(tǒng)中，激光雷達(dá)導(dǎo)航系統(tǒng)的典型實(shí)例可參考基于RPLIDARA1雷達(dá)與TARKBOT麥克納姆輪機(jī)器人的實(shí)現(xiàn)。該系統(tǒng)通過激光雷達(dá)實(shí)時掃描環(huán)境，利用Cartographer或gmapping算法構(gòu)建2D柵格地圖，并在Rviz中可視化地圖與機(jī)器人位姿。導(dǎo)航功能基于ROSNavigationStack，結(jié)合AMCL算法進(jìn)行定位，通過move_base包實(shí)現(xiàn)全局路徑規(guī)劃（如A*算法）與局部動態(tài)窗口法（DWA）避障，使機(jī)器人能自主繞開障礙物并抵達(dá)目標(biāo)點(diǎn)。實(shí)驗(yàn)中，用戶通過SSH遠(yuǎn)程控制小車，使用鍵盤或APP遙控移動，完成地圖閉合后保存為.yaml和.pgm文件，支持后續(xù)多點(diǎn)導(dǎo)航任務(wù)。此外，系統(tǒng)還可擴(kuò)展至多機(jī)協(xié)作場景，通過激光雷達(dá)數(shù)據(jù)融合實(shí)現(xiàn)多機(jī)器人協(xié)同避障與路徑優(yōu)化，適用于倉儲物流、工業(yè)巡檢等場景。7.2.1ROS中的導(dǎo)航功能簡介機(jī)器人依賴導(dǎo)航技術(shù)實(shí)現(xiàn)自主的移動。ROS系統(tǒng)具有導(dǎo)航(Navigation)功能包，包括建圖、定位、規(guī)劃三大功能。它是一個二維導(dǎo)航堆棧，接收來自里程計(jì)、傳感器流和目標(biāo)姿態(tài)的信息，并輸出發(fā)送到移動底盤的命令。ROS官方提供了一張導(dǎo)航功能包集的圖示，如下圖所示，該圖中囊括了ROS導(dǎo)航的一些關(guān)鍵技術(shù)。上圖中，白色的部分是必須且已實(shí)現(xiàn)的組件，灰色的部分是可選且已實(shí)現(xiàn)的組件，藍(lán)色的部分是必須為每一個機(jī)器人平臺創(chuàng)建的組件。其包括全局地圖、自身定位、路徑規(guī)劃、運(yùn)動控制、環(huán)境感知五個主要過程值得注意的是機(jī)器人在定位過程中，對與參考坐標(biāo)系的使用，在ROS系統(tǒng)里有兩種常用的方式：1)通過里程計(jì)定位：實(shí)時收集機(jī)器人的速度信息，計(jì)算并發(fā)布機(jī)器人坐標(biāo)系與父級參考系的相對關(guān)系。2)通過傳感器定位：傳感器收集外界環(huán)境信息，通過匹配計(jì)算并發(fā)布機(jī)器人坐標(biāo)系與父級參考系的相對關(guān)系。上述兩種定位實(shí)現(xiàn)中，機(jī)器人坐標(biāo)系一般使用機(jī)器人模型中的根坐標(biāo)系(base_link或base_footprint)，里程計(jì)定位時，父級坐標(biāo)系一般稱之為odom，如果通過傳感器定位，父級參考系一般稱之為map。當(dāng)二者結(jié)合使用時，map和odom都是機(jī)器人模型根坐標(biāo)系的父級，這是不符合坐標(biāo)變換中"單繼承"的原則的，所以，一般會將轉(zhuǎn)換關(guān)系設(shè)置為:map->doom->base_link或base_footprint。7.2.1ROS中的導(dǎo)航功能簡介7.2.2導(dǎo)航機(jī)器人模型搭建導(dǎo)航機(jī)器人模型搭建是一個融合硬件集成、軟件配置與算法優(yōu)化的系統(tǒng)性工程，核心步驟包括：硬件層面需選配差速底盤、激光雷達(dá)、攝像頭等組件并完成電路連接；軟件層面需部署ROS框架，配置傳感器驅(qū)動及URDF機(jī)器人模型；算法層面則通過SLAM（如Gmapping或Cartographer）實(shí)現(xiàn)環(huán)境建模與定位，并結(jié)合導(dǎo)航棧（如Nav2或move_base）完成路徑規(guī)劃與動態(tài)避障；最后通過TF樹驗(yàn)證、參數(shù)調(diào)優(yōu)及多傳感器融合優(yōu)化系統(tǒng)性能，確保機(jī)器人實(shí)現(xiàn)精準(zhǔn)導(dǎo)航與智能交互。7.2.3激光雷達(dá)導(dǎo)航實(shí)現(xiàn)激光雷達(dá)導(dǎo)航的實(shí)現(xiàn)基于激光雷達(dá)（LiDAR）通過發(fā)射激光脈沖并接收反射信號，利用飛行時間（ToF）原理精確計(jì)算目標(biāo)距離，結(jié)合掃描技術(shù)生成環(huán)境的三維點(diǎn)云數(shù)據(jù)。其核心流程包括：激光發(fā)射器向周圍環(huán)境發(fā)射激光束，接收器捕獲反射信號后，通過光速與時間差計(jì)算障礙物距離，并結(jié)合掃描系統(tǒng)（如旋轉(zhuǎn)或固態(tài)掃描）獲取多角度數(shù)據(jù)，形成高精度三維點(diǎn)云地圖。隨后，通過SLAM（同步定位與地圖構(gòu)建）算法，將點(diǎn)云數(shù)據(jù)與慣性導(dǎo)航系統(tǒng)（IMU）、GPS等傳感器信息融合，實(shí)時更新設(shè)備的位置、速度和姿態(tài)，實(shí)現(xiàn)動態(tài)環(huán)境中的精準(zhǔn)定位與路徑規(guī)劃。例如，在自動駕駛中，激光雷達(dá)可識別車道線、行人及障礙物，結(jié)合算法預(yù)判移動軌跡，確保安全避障；在無人機(jī)或機(jī)器人領(lǐng)域，則通過點(diǎn)云建模實(shí)現(xiàn)復(fù)雜場景的自主導(dǎo)航。這一技術(shù)依賴高效的數(shù)據(jù)處理（如濾波、點(diǎn)云配準(zhǔn)）和多傳感器協(xié)同，最終為智能系統(tǒng)提供魯棒的環(huán)境感知與決策支持。7.3視覺導(dǎo)航系統(tǒng)實(shí)例在ROS中，視覺導(dǎo)航系統(tǒng)典型實(shí)例為：基于RGB-D相機(jī)（如IntelRealSenseD435）實(shí)時采集深度與圖像數(shù)據(jù)，通過視覺SLAM算法（如rtabmap或ORB-SLAM3）構(gòu)建3D點(diǎn)云地圖并定位，結(jié)合ROSNavigationStack的全局A*路徑規(guī)劃與局部DWA動態(tài)避障，實(shí)現(xiàn)自主導(dǎo)航。系統(tǒng)通過Rviz可視化地圖與軌跡，支持多傳感器融合與實(shí)時任務(wù)調(diào)整，適用于室內(nèi)服務(wù)機(jī)器人或無人機(jī)場景。7.3.1深度相機(jī)模型實(shí)現(xiàn)在諸多SLAM算法中，一般都需要訂閱激光雷達(dá)數(shù)據(jù)用于構(gòu)建地圖，因?yàn)榧す饫走_(dá)可以感知周圍環(huán)境的深度信息，而深度相機(jī)也具備感知深度信息的功能，但是激光雷達(dá)價格比較昂貴，可以選用深度相機(jī)（Kinect）代替激光雷達(dá)，不過二者發(fā)布的消息類型是完全不同的，如果想要實(shí)現(xiàn)傳感器的置換，那么就需要將深度相機(jī)發(fā)布的三維的圖形信息轉(zhuǎn)換成二維的激光雷達(dá)信息，這一功能就是通過ROS中的一個功能包:depthimage_to_laserscan來實(shí)現(xiàn)的。在轉(zhuǎn)換之前，先來測試一下寫好的相機(jī)模型，終端執(zhí)行以下命令：#終端1$source./devel/setup.bash$roslaunchurdf_gazebotest.launch

#終端2$source./devel/setup.bash$roslaunchurdf_rvizCar.launch選擇深度相機(jī)的話題，可得到右圖所示結(jié)果7.3.2深度相機(jī)建圖導(dǎo)航實(shí)現(xiàn)在kinect中也可以以點(diǎn)云的方式顯示感知周圍環(huán)境，但是需要先進(jìn)行坐標(biāo)變換，首先在src/urdf_gazebo/urdf/gazebo/depthCamera.xacro文件中修改：<frameName>support_depth</frameName>然后在src/urdf_gazebo/launch/test.launch中添加坐標(biāo)變換關(guān)系：<nodepkg="tf2_ros"type="static_transform_publisher"name="static_transform_publisher"args="000-1.570-1.57/support/support_depth"/>最后在終端執(zhí)行以下命令：#終端1$source./devel/setup.bash$roslaunchurdf_gazebotest.launch

#終端2$source./devel/setup.bash$roslaunchurdf_rvizCar.launch添加PointCloud2話題，可以得到如右圖所示的結(jié)果，中間就是點(diǎn)云圖。在ROS系統(tǒng)中完成深度相機(jī)建圖導(dǎo)航的實(shí)現(xiàn)，需先安裝深度相機(jī)驅(qū)動及導(dǎo)航功能包（如rtabmap-ros），通過depthimage_to_laserscan將深度圖像轉(zhuǎn)換為偽激光數(shù)據(jù)，利用RTAB-Map等SLAM算法構(gòu)建三維柵格地圖，結(jié)合move_base導(dǎo)航包進(jìn)行路徑規(guī)劃，并通過閉環(huán)檢測優(yōu)化地圖精度，最后在Rviz中可視化地圖與導(dǎo)航路徑，實(shí)現(xiàn)機(jī)器人自主定位與避障。7.3.2深度相機(jī)建圖導(dǎo)航實(shí)現(xiàn)7.4機(jī)器人平臺自主導(dǎo)航實(shí)例本節(jié)導(dǎo)航實(shí)例采用Turtlebot3機(jī)器人，TurtleBot3的目標(biāo)是大幅降低平臺的尺寸和價格，而不會犧牲性能，功能和質(zhì)量。由于提供了不同可選，如底盤，計(jì)算機(jī)和傳感器，TurtleBot3可以通過各種方式進(jìn)行定制。7.4.1環(huán)境搭建機(jī)器人依賴導(dǎo)航技術(shù)實(shí)現(xiàn)自主的移動。ROS系統(tǒng)具有導(dǎo)航(Navigation)功能包，包括建圖、定位、規(guī)劃三大功能。它是一個二維導(dǎo)航堆棧，接收來自里程計(jì)、傳感器流和目標(biāo)姿態(tài)的信息，并輸出發(fā)送到移動底盤的命令。ROS官方提供了一張導(dǎo)航功能包集的圖示，如下圖所示，該圖中囊括了ROS導(dǎo)航的一些關(guān)鍵技術(shù)。7.4.2仿真測試

結(jié)合之前的內(nèi)容，首先建圖，保存地圖，然后在已知地圖環(huán)境下進(jìn)行路徑規(guī)劃，可同時啟用局部規(guī)劃策略，通過激光雷達(dá)SLAM輔助局部建圖，進(jìn)行局部避障。

首先，打開Gazebo以及Rviz：

再開一個終端，打開鍵盤控制節(jié)點(diǎn)，用于控制小車運(yùn)動建圖，如右圖(上)所示。

保存好圖像過后，關(guān)閉打開Rviz的終端（終端2），然后在終端2中執(zhí)行Turtlebot3源碼里邊的導(dǎo)航包文件。結(jié)果如右圖(下)所示。7.5無人機(jī)平臺自主導(dǎo)航實(shí)例上一節(jié)的導(dǎo)航實(shí)例仿真基于激光雷達(dá)的輪式機(jī)器人，本節(jié)將在無人機(jī)平臺上實(shí)現(xiàn)自主導(dǎo)航，由于目前激光雷達(dá)的體積一般較大，不適用于小型的四旋翼無人機(jī)用于機(jī)載建圖，因此，本節(jié)使用深度相機(jī)完成四旋翼無人機(jī)導(dǎo)航。7.5.1實(shí)現(xiàn)框架前面章節(jié)中，提到過基于相機(jī)的地圖構(gòu)建過程，本節(jié)采用魯棒性更優(yōu)的實(shí)時視覺SLAM算法框架，分為前端和后端，前端提取視覺信息（單目或者雙目視覺特征點(diǎn)）并進(jìn)行跟蹤，后端處理前端得到的特征點(diǎn)跟蹤信息，可融合IMU，GPS等外部信息，利用優(yōu)化算法實(shí)時得出當(dāng)前的狀態(tài)。本節(jié)的開源代碼EGO-Planner（Fast-Drone-250）旋翼無人機(jī)軌跡規(guī)劃算法目前有以下