基于ROS的輪式機器人建模與路徑規(guī)劃仿真

李宏達，范繼祥

(哈爾濱師范大學)

0 引言

隨著近幾年機器人行業(yè)的快速發(fā)展[1]，自主移動機器人成為了機器人行業(yè)發(fā)展和投資的一個熱點，也是機器人技術發(fā)展的必然趨勢[2-4].自主移動機器人[5]能夠通過傳感器感知周圍陌生環(huán)境和估計自身所處位置的狀態(tài)，在一個有障礙物的陌生環(huán)境中實現(xiàn)一個由出發(fā)點駛向目標點的自主移動，從而實現(xiàn)人們的作業(yè)需求[6].相比較其他類型機器人，自主移動機器人具有小巧靈活、操作簡單等優(yōu)點，可以廣泛的應用于軍事、醫(yī)療和其他日常生產(chǎn)生活中[3-4].新冠疫情期間，基于自主移動機器人改進的消毒機器人和轉運機器人在防疫過程中發(fā)揮了巨大的作用.

目前主流的SLAM(Simultaneous localization and mapping)算法[7-9]已經(jīng)與具有高掃描頻率的測距傳感器相結合，通過傳感器數(shù)據(jù)與掃描地圖互相驗證確定機器人的位姿.在室內環(huán)境下基于RBPF(Rao-Blackwellized Particle Filter)的粒子濾波算法已經(jīng)優(yōu)化成ROS(Robot Operating System)中的一個功能包gmapping[10]，該功能包可將傳感器采集到的信息整合分析，從而生成二維柵格地圖.該文使用ROS[11]開源系統(tǒng)作為機器人開發(fā)平臺.利用底層設備控制、進程之間消息傳遞、包管理等功能對雙輪差速機器人及其周圍空間環(huán)境進行建模和仿真，對機器人的路徑規(guī)劃、自主導航與避障系統(tǒng)進行研究.

1 機器人系統(tǒng)模型

1.1 坐標系建立

為了描述機器人在空間中的位置和姿態(tài)，建立了如圖1所示坐標系模型.XOY坐標系為慣性坐標系，XRORYR為機器人坐標系，XR方向為機器人前進方向，因傳感器安裝在機器人本體上，所以二者共用一個坐標系.

圖1 坐標系模型

機器人的位姿用三維向量(X,Y,θ)T表示，該向量包括了機器人在平面環(huán)境中的空間位置(X,Y)T和姿態(tài)角θ.姿態(tài)角θ為機器人坐標系的YR軸與慣性坐標系Y軸 (或XR與X軸)之間的夾角，用來描述機器人的運動方向，具體定義為：以Y軸(或X軸)的方向為0°，逆時針方向為正，姿態(tài)角θ的范圍為-180°～ +180°.

1.2 機器人里程計運動模型

機器人在定位、地圖構建和移動過程中都需要采集里程計信息[12]，里程計信息為機器人在每2個時間間隔內位置和姿態(tài)的變化量.該文所設計的雙輪差速機器人里程計運動模型如圖2所示.

圖2 機器人里程計運動模型

在慣性坐標系中，機器人從A點到B點做圓弧運動，轉動角度為dθ.相對于以A點為原點的機器人坐標系，B點的位置變化為：

(1)

式中，R為機器人做圓弧運動所對應圓的半徑，與左右輪子的角速度有關.根據(jù)當前時刻兩個電機的瞬時角速度可求解得到(dx,dy,dθ)T，進而可推出機器人的運動模型：

(2)

1.3 機器人激光雷達觀測模型

在SLAM過程中，需要激光雷達傳輸回來的2個數(shù)據(jù)：路標點P到雷達的距離r和路標點P與當前雷達基準方向夾角φ，如圖3所示.

圖3 機器人激光雷達觀測模型

當前路標點P坐標為(px,py)，根據(jù)當前時刻由里程計運動模型計算得到機器人位姿[xtytθt,]T，求得雷達觀測模型：

(3)

式中，vt為觀測時的噪聲.

2 機器人SLAM與導航

未知環(huán)境中的機器人自主導航已經(jīng)成為了機器人發(fā)展過程中一項極為關鍵的技術[13].SLAM問題描述為機器人在一個陌生環(huán)境的未知位置開始移動，在移動的過程中根據(jù)傳感器傳回的信息進行自身位置估計，同時構造增量式地圖.

2.1 SLAM方法架構

SLAM使用卡爾曼濾波器對采集到的環(huán)境數(shù)據(jù)和測得的機器人運動模型進行狀態(tài)估計，進而得到地圖數(shù)據(jù)和機器人姿態(tài)，其方法架構如圖4所示.

圖4 SLAM方法架構

首先，通過激光雷達和攝像頭等傳感器對周圍未知環(huán)境進行數(shù)據(jù)采集并提取環(huán)境特征；然后，利用里程計、IMU慣性單元等對機器人本體信息進行感知得到機器人運動模型；進一步，使用擴展卡爾曼濾波器對得到的傳感器觀測值和機器人運動模型進行處理，得到地圖數(shù)據(jù)和機器人姿態(tài)估計；最后，將得到的估計數(shù)據(jù)與實際的地圖信息和機器人姿態(tài)進行校驗，驗證結論數(shù)值的準確性.

2.2 SLAM功能包與導航框架及算法

2.2.1 gmapping功能包

gmapping功能包為目前在ROS中最成熟也是應用最多的一種SLAM功能包.gmapping功能包總體框架如圖5所示.

圖5 gmapping功能包總體框架

將激光雷達測得的深度信息、IMU信息、里程計信息輸入到功能包中，使用Rao- Blackwellized粒子濾波算法來創(chuàng)建二維柵格地圖，輸出nav_msgs/OccupancyGrid地圖話題.

2.2.2 基于move_base的導航框架

move_base功能包主要完成全局路徑規(guī)劃和本地實時規(guī)劃[14]2個工作.全局路徑規(guī)劃主要通過Dijkstra或A*算法來進行全局最優(yōu)路徑規(guī)劃.本地實時規(guī)劃主要完成的工作為規(guī)劃機器人每個周期內的線速度、角速度，幫助機器人行駛路徑盡量符合全局最優(yōu)路徑；通過傳感器信息判斷障礙物并進行躲避.move_base導航框架如圖6所示.

由圖6可以看出，將導航定位信息、激光雷達信息、里程計數(shù)據(jù)信息進行坐標變換得到機器人的位置姿態(tài)輸入到框架中.全局權值地圖節(jié)點讀取地圖信息節(jié)點輸出的nav_msgs地圖話題進行全局規(guī)劃，局部權值地圖根據(jù)激光雷達/Kinect節(jié)點傳入的sensor_msgs話題實時更新地圖信息并進行局部規(guī)劃，躲避新出現(xiàn)在地圖中的障礙物.機器人底盤控制節(jié)點接收cmd_vel(速度控制指令)信息實現(xiàn)機器人的運動控制.

2.2.3 A*路徑規(guī)劃算法

A*路徑規(guī)劃算法[15]是一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法，也是許多其他問題的常用啟發(fā)式算法.公式表示為：

圖6 move_base導航框架

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

(4)

式中，f(n)是從初始狀態(tài)經(jīng)由狀態(tài)n到目標狀態(tài)的代價估計，g(n)是在狀態(tài)空間中從初始狀態(tài)到狀態(tài)n的實際代價，h(n)是從狀態(tài)n到目標狀態(tài)的最佳路徑的估計代價.

以d(n)表示狀態(tài)n到目標狀態(tài)的距離，那么h(n)的選取有如下3種情況：

(1)如果h(n)

(2)如果h(n)=d(n)，即距離估計h(n)等于最短距離，那么搜索將嚴格沿著最短路徑進行， 此時的搜索效率是最高的.

(3)如果h(n)>d(n)，搜索的點數(shù)少，搜索范圍小，效率高，但不能保證得到最優(yōu)解.

3 機器人及空間環(huán)境建模仿真

3.1 機器人建模

該文使用Rviz+Arbotix來對機器人進行整體建模.在Rviz中，URDF文件使用可擴展標記語言XML[16]對機器人的剛體外觀、坐標位置、關節(jié)類型等方面進行描述，并在界面中呈現(xiàn)出來；同時還可以通過圖形化的方式實時顯示傳感器數(shù)據(jù)、機器人運動狀態(tài)、周圍環(huán)境的變化等信息.ArbotiX所配套的ROS功能包提供了一個差速控制器，通過接收速度控制指令更新機器人的joint狀態(tài)，從而實現(xiàn)機器人在rviz中的運動.該文所建立的URDF模型整體結構如圖7所示.

圖7 URDF模型整體結構圖

x,y,z)坐標和(r,p,y)坐標可以獲得各個節(jié)點在模型中的位置關系.

機器人模型在Rviz中可視化顯示如圖8所示.

圖8 機器人仿真模型

3.2 機器人運動環(huán)境仿真

機器人運動環(huán)境的建立使用Gazebo三維動態(tài)物理仿真器，其具備強大的物理引擎、高質量的圖形渲染、方便的編程和圖形接口，能夠準確高效地仿真在復雜的室內外環(huán)境下機器人群體.機器人的傳感器信息也可以通過插件的形式加入到仿真環(huán)境中并進行可視化的顯示，為程序和用戶提供交互接口，其典型應用場景包括：測試機器人算法、現(xiàn)實情境下的回溯測試等.

在Gazebo仿真器中設計如圖9所示機器人運動環(huán)境，機器人所處環(huán)境為一個長方形的封閉空間，內部分割成三個小房間，機器人可以通過各個房間之間的小門在該環(huán)境中自由移動.

圖9 機器人運動環(huán)境仿真圖

控制機器人的移動對機器人運動環(huán)境進行掃描并將傳感器采集到的深度信息和里程計信息輸入到gmapping功能包中，建立柵格地圖過程如圖10所示.

圖10 SLAM仿真過程及結果

由圖10可以看出，機器人首先掃描房間①的地圖(如圖10a所示)，然后掃描房間②的地圖(如圖10b所示)，其次掃描房間③的地圖(如圖10c所示)，最后生成gmapping SLAM地圖(如圖10d所示).結果表明，激光雷達能夠成功掃描各個房間的邊界，并成功構建地圖.

4 仿真過程及結果分析

該文機器人路徑規(guī)劃分為2種情況進行仿真：第1種為無臨時障礙物的路徑規(guī)劃仿真，第2種為有臨時障礙物的路徑規(guī)劃仿真.

對于無臨時障礙物的情況，其運動環(huán)境如圖9所示.圖11為此種情況下的機器人路徑規(guī)劃仿真圖，圖中A點為出發(fā)點，箭頭尾部B點為目標點，箭頭方向為機器人最終姿態(tài)方向.

圖11 無臨時障礙物的機器人路徑規(guī)劃仿真圖

由圖11可以看出，機器人從房間③的A點出發(fā)(如圖11a所示)，沿著規(guī)劃好的路徑移動，經(jīng)由房間③和房間②之間的門到達房間②的目標點B(如圖11b所示)，到達目標點B后機器人保持設定的姿態(tài)(如圖11c所示).

對于存在臨時障礙物的情況，設置新的出發(fā)點C和目標點D，D點的箭頭方向為機器人最終姿態(tài)方向，在機器人運動過程中放置臨時障礙物M，其運動環(huán)境如圖12所示.

圖12 存在障礙物時機器人運動環(huán)境仿真圖

圖13為有臨時障礙物的機器人路徑規(guī)劃仿真圖，由圖可知，機器人從房間②的C點出發(fā)(如圖13a所示)，沿著規(guī)劃好的路徑移動(如圖13b所示).到達E點時探測到前進路線上放置的臨時障礙物M(如圖13c所示)，機器人對運動路徑進行了重新規(guī)劃，并沿著新的路徑運動.當運動到障礙物M的左下角時，探測到障礙物的下側面，再次重新進行路徑規(guī)劃并沿著新路徑運動(如圖13d所示)，當機器人運動到障礙物的右下角時，探測到前進方向上無其他障礙物(如圖13e所示)，重新規(guī)劃最短路徑到達目標點D，并保持設定姿態(tài)(如圖13f所示).

圖13 有臨時障礙物的機器人路徑規(guī)劃仿真圖

仿真結果表明：機器人可以在無臨時障礙物出現(xiàn)的情況下進行自主規(guī)劃路徑與導航；在行進過程中遇到臨時障礙物，能夠及時發(fā)現(xiàn)障礙物并重新規(guī)劃路徑進行規(guī)避，實現(xiàn)機器人的路徑規(guī)劃、自主導航與避障的功能.

5 結束語

該文搭建了基于ROS的導航仿真平臺，在Rviz中建立了雙輪差速機器人模型，并在Gazebo中搭建了機器人運動環(huán)境.機器人可以在未知的空間環(huán)境中通過傳感器進行SLAM地圖構建進而生成二維柵格地圖，并且在Rviz中實時觀看地圖建立過程.應用A*導航算法，針對所構建的地圖該機器人能夠成功實現(xiàn)自主規(guī)劃最優(yōu)路徑及導航功能；若在行進過程中遇到臨時出現(xiàn)的障礙物，能夠通過激光雷達傳感器及時檢測到障礙物，并更新前進路徑，避開障礙物，進而成功到達目標點，實現(xiàn)機器人的路徑規(guī)劃、自主導航與避障功能.對于后期搭建機器人硬件平臺并在真實環(huán)境中進行驗證具有一定的指導意義.