機器人自主導航仿真

成家駿

（山西農業大學軟件學院 山西省晉中市 030800）

1 引言

移動機器人的研究始于60年代末，斯坦福研究院（SRI）的Nils Nilssen 和Charles Rosen 等人，在1966年至1972年中制造出了取名為Shakey 的自主移動的機器人[3]，時至今日，世界機器人發展迅猛猶如幾十年前計算機行業，移動機器已經融入到我們生活的各行各業，使我們的生活更便捷，使我們的生產更高效。同時伴隨著5G 技術的落地并運用覆蓋，帶寬增加推動新產業升級，加速技術融合起到了決定性的作用，各式各樣以計算機技術的發展為基礎的機器人如雨后春筍般涌現。機器人能自適應和感知周圍環境，在實際運動過程中由配備傳感器采集一系列環境信息，并用處理后的數據進行分析調整姿態繪制出地圖的機器人控制方式的廣泛應用，可以盡可能地保證了機器人在復雜和不確定環境中作業的穩定性。本文主要針對在ROS 環境下自主導航的仿真，相較鹿霖,謝樹新等人和曹正萬,平雪良,陳盛龍蔣毅等人[4-5]，采用solidworks 建立三維模型再解析URDF 導入ROS 的方法完成機械臂建模，本文主要使用Linux 下手工編碼的方式完成移動機器人建模，減少工具熟悉和學習的時間，但也對編寫代碼能力和對機器人關節關系理解要求較高。

2 ROS（Robot Operating System 機器人操作系統）

2.1 ROS綜述

ROS 是2010年Willow Garage 公司發布的專為機器人軟件開發所設計出來的一套開源操作系統。最早是基于ubuntu 系統設計，支持c++、java、python 等編程語言，極大地提高了代碼的復用，加速了開發速度，減少了開發周期[6]。同時ROS 也是一種分布式松耦合框架，不同的執行單元被分散設計，形成獨立的“黑盒”軟件包，可以根據項目的實際需要進行組合，實現相應的實驗需要。

ROS 把單個軟件包稱為節點，節點間通過發布和訂閱主題的傳遞方式，發出節點將消息發布到指定節點，訂閱節點以訂閱目標主題的方式來獲取數據信息[7]。

2.2 URDF(統一機器人描述格式 Unified Robot Description Format)

圖1：在Gazebo 環境下的xacro 模型

圖2：RViz 環境下的Astar 算法仿真

圖3：RViz+Gazebo 自主導航仿真

表1：URDF 主要標簽表

表2：Xacro 宏定義表

URDF 一種符合XML 規范，描述機器人結構格式，主要用于RViz 和Gazebo 中進行建圖和物理仿真。其主要標 簽和標簽主要屬性如表 1，＜?mxl＞標簽聲明版本信息，＜robot＞標簽來命名機器人名稱，其下通常包含＜link＞和＜joint＞標簽來描述機器人的部件和關節，＜link＞內部有部件名屬性，其下通常用＜visual＞描述部件形體和材質，＜collision＞用來配置碰撞矩陣相關參數，＜joint＞標簽name 屬性來給標簽命名，type 定義兩個部件間的連接方式，其下包含＜parent＞和＜child＞描述父子節點部件，＜origin＞描述關節量和旋轉量，＜axis＞描述關節的旋轉方向。

2.3 RViz和Gazebo綜述

2.3.1 RViz

RViz 是ROS 官方提供的一款三維坐標可視化工具，可以將激光雷達、深度攝像頭、里程計等傳感器發布的數據以及機器人仿真模型和tf 坐標進行處理并可視化顯示，并且RViz 擁有擴展機制，可以使用插件來豐富本身的功能。

2.3.2 Gazebo

Gazebo 是一款物理仿真軟件，由ROS 功能包提供對Gazebo外部接口，由于Gazebo 為物理仿真環境，需要設置碰撞矩陣、剛體質量等參數使得代碼維護擴充變得困難，為此通常用xacro 宏定義文件替代urdf 文件提高了代碼的可讀性、復用性和可擴展性。并且Gazebo 內部包含豐富的模型庫和繪圖調整工具，使得開發人員可以根據自身機器人設計情況進行事先模擬出工作環境，并可以將模擬搭建出的仿真環境進行保存成.world 文件進行重復調用。Gazebo 仿真環境在進行傳感器選型和車體設計（如圖1）方面，不但節約了開發成本降低資源浪費，也縮短了開發周期提高了研發效率，但如果在高要求高精度環境中作業的機器人也會因為xacro 建模語言忽略細節的大體表示而引起不可避免的誤差，從而影響到測量精度和作業展開。

2.3.3 Xacro 格式

當機器人在Gazebo 物理仿真環境中做仿真時，所需要的物理參數更加繁多，URDF 模型代碼冗長，重復過多，修改參數不便，沒有參數運算功能的問題，既不方便閱讀，也不便于二次開發的缺點顯露出來，Xacro 在urdf 的基礎上加入宏定義標簽如表2，＜robot＞標簽下通常先引用＜xacro:property＞標簽進行參數聲明，其內部name 和value 屬性分別寫出參數名和參數值，＜xacro:macro＞標簽通常封裝機器人部件的慣性矩陣等運算代碼和部件節點的組合封裝，將代碼模塊化，并用name 屬性描述封裝內容方便調用，最后使用base_footprin 虛擬部件來定義機器人坐標位置，并用＜gazebo＞標簽調用插件發布或訂閱節點信息。達到參數和代碼分離的目的。

3 激光SLAM建圖及實現

3.1 激光雷達傳感器

激光雷達，是激光探測及測距系統的簡稱，其由激光器、接收器、信號處理單元、旋轉機構組成的光學傳感器。其工作原理是由作為輻射源的激光器發射探測信號，之后接收器將從目標物體反射回來的目標回波進行接收，最后由信號處理單元將目標回波和探測信號進行信號比對，并在此基礎上由傳動裝置組成的旋轉機構保持旋轉檢測。該電子設備不但可以獲取目標物體的方位、距離、速度等參數，并且擁有較好的穩定性，較高的精確度，成為了被廣泛用于移動機器人在二維未知環境中感知周圍環境的主要傳感器。

3.2 柵格地圖

移動機器人進行自主導航的地圖生成法有很多比如拓撲地圖生成法[7]、特征點地圖、直接表征法以及柵格地圖生成法，拓撲地圖生成法通常由后三種地圖生成法通過相應算法提取得到，較為抽象；特征點地圖通常是由激光雷達掃描獲得數據或深度攝像頭經過特殊算法處理過后的數據圖像；而直接表征法是由傳感器接收圖像未經特征點化或柵格化處理得到的原始圖像；本文主要使用通過柵格化處理的柵格地圖上進行激光雷達特征點采集的房屋平面圖建模，采用這種方法具有較好的空間表現，較為直觀展現出房間主要形狀及其比例，以及建模效果與原房屋的對比檢驗。

4 RViz+Gazebo聯合仿真和激光雷達建圖實現

4.1 Gmapping

Gmapping 是一款基于激光雷達，使用RBPF 粒子濾波算法的ROS 功能包，其通常用于二維柵格化地圖，采用OpenSlam 開源算法的開源功能包。當移動機器人在一個未知的環境中移動，構建當前環境地圖。地圖可以用一個儲存每個單元格顏色的二維矩陣表示，每個單元格的顏色只能用黑或白兩種顏色。并且，Rao-Blackwellized 粒子濾波算法可以預先估計機器人的軌跡，而后再根據已知軌跡路徑計算地圖，解決同步定位與地圖構建的問題[8]。

4.2 move_base

move_base 是一款ROS 提供的導航功能包，使得移動機器人可以實時的根據已知建圖信息進行路徑規劃，并根據地圖的完善實時更改最優路徑。其實現原理是由全局路徑規劃Astar 算法或DijiKstra 算法進行全局方向把握，之后根據實際情況采用局部優化DWA 算法和TEB 算法進行局部優化。

Astar 算法，是采用DijiKstra 算法和貪心算法的啟發式算法，其公式表示為：

f(n) 是從初始狀態經由當前狀態n 到目標狀態的代價估計。g(n) 是在柵格空間中從初始狀態到當前狀態n 的實際代價。h(n) 是從當前狀態n 到目標 狀態的最佳路徑的估計代價。

其算法實現如下：

（1）初始化open 隊列和close 隊列，并將起始節點加入open隊列當中作為當前節點

（2）判斷是否可以到達終結點，如果不能結束算法，返回false，否則進入（3）

（3）在8 方向中，選擇當前節點的相鄰節點中f(n)最小的節點放入open 隊列中，并將open 隊列根據f(n)大小進行排序，將f(n)最小的元素作為當前節點，并彈出open 隊列，加入到close 隊列中

（4）如果n 為終結點，則從open 隊列中移除放入close 隊列當中，最后對close 隊列進行回溯遍歷，算法結束，返回true，當給g(n)和h(n)賦予權重進行算法調整，當h(n)=0 時，f(n)=g(n)，此時Astar 變成Dijkstra 算法，這只保證路徑搜索能找到最短路徑。當g(n)=0 時；f(n)=h(n)，此時Astar 變為貪心算法，到達目標點的最優運行速度而不關心最短路徑；故如果h(n)所預測的移動到目標所需要的代價剛好等于最短代價，則算法可以實現在非常快的情況下尋找出最優路徑，而如果h(n)大于估計最短代價，則機器人從n 點運動到終結點路徑會變長但運行速度更快。如圖2（上）為在RViz 中使用grid_map 的Astar 算法的三維仿真，（下）命令行終端輸出的運行時間、規劃點數(nodes_size)、目標節點坐標（Position）、偏移角（Angle）等信息。

4.3 建圖實現

通過Gazebo 構建地圖仿真物理環境，RViz 訂閱里程計數據、激光雷達數據、tf 坐標變換等數據，在add 選項中添加顯示信息，會出現激光雷達點云信息、里程計信息等顯示，最后根據建圖需要，Gazebo 中的機器人模型會根據，RViz 機器人的移動而移動，RViz中的機器人模型會根據，Gazebo 的物理信息來調整位姿狀態，再根據地圖狀態，啟動導航節點進行自主導航如圖3，為經過膨脹處理的RViz（左上）+Gazebo（右上）聯合仿真實驗，機器人模型根據/odom 息標注出機器人運行的方向，訂閱的/scan 信息提取出激光雷達掃描的點云坐標顯示在RViz 界面中，以及最后建圖結果保存地圖（右下）。

5 結語

本文基于ROS 平臺搭建了Astar 算法和自主導航仿真環境，完成了自主導航方面從機器人模型，物理仿真環境搭建，激光雷達信息、里程計信息采集訂閱，Astar 仿真地圖搭建，返回算法參考重要數據進行比對等，相較于MATLAB 仿真，在RViz 中可以進行三維Astar 仿真用C++語言實現，但仿真代碼冗余，使得算法仿真編碼效率較緩慢、非機器人領域人員使用較少等問題亟待解決。