李宏達(dá)，范繼祥

(哈爾濱師范大學(xué))

0 引言

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

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

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

1.1 坐標(biāo)系建立

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

圖1 坐標(biāo)系模型

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

1.2 機(jī)器人里程計(jì)運(yùn)動(dòng)模型

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

圖2 機(jī)器人里程計(jì)運(yùn)動(dòng)模型

在慣性坐標(biāo)系中，機(jī)器人從A點(diǎn)到B點(diǎn)做圓弧運(yùn)動(dòng)，轉(zhuǎn)動(dòng)角度為dθ.相對(duì)于以A點(diǎn)為原點(diǎn)的機(jī)器人坐標(biāo)系，B點(diǎn)的位置變化為：

(1)

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

(2)

1.3 機(jī)器人激光雷達(dá)觀測模型

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

圖3 機(jī)器人激光雷達(dá)觀測模型

當(dāng)前路標(biāo)點(diǎn)P坐標(biāo)為(px,py)，根據(jù)當(dāng)前時(shí)刻由里程計(jì)運(yùn)動(dòng)模型計(jì)算得到機(jī)器人位姿[xtytθt,]T，求得雷達(dá)觀測模型：

(3)

式中，vt為觀測時(shí)的噪聲.

2 機(jī)器人SLAM與導(dǎo)航

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

2.1 SLAM方法架構(gòu)

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

圖4 SLAM方法架構(gòu)

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

2.2 SLAM功能包與導(dǎo)航框架及算法

2.2.1 gmapping功能包

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

圖5 gmapping功能包總體框架

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

2.2.2 基于move_base的導(dǎo)航框架

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

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

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

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

圖6 move_base導(dǎo)航框架

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

(4)

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

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

(1)如果h(n)

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

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

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

3.1 機(jī)器人建模

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

圖7 URDF模型整體結(jié)構(gòu)圖

x,y,z)坐標(biāo)和(r,p,y)坐標(biāo)可以獲得各個(gè)節(jié)點(diǎn)在模型中的位置關(guān)系.

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

圖8 機(jī)器人仿真模型

3.2 機(jī)器人運(yùn)動(dòng)環(huán)境仿真

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

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

圖9 機(jī)器人運(yùn)動(dòng)環(huán)境仿真圖

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

圖10 SLAM仿真過程及結(jié)果

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

4 仿真過程及結(jié)果分析

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

對(duì)于無臨時(shí)障礙物的情況，其運(yùn)動(dòng)環(huán)境如圖9所示.圖11為此種情況下的機(jī)器人路徑規(guī)劃仿真圖，圖中A點(diǎn)為出發(fā)點(diǎn)，箭頭尾部B點(diǎn)為目標(biāo)點(diǎn)，箭頭方向?yàn)闄C(jī)器人最終姿態(tài)方向.

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

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

對(duì)于存在臨時(shí)障礙物的情況，設(shè)置新的出發(fā)點(diǎn)C和目標(biāo)點(diǎn)D，D點(diǎn)的箭頭方向?yàn)闄C(jī)器人最終姿態(tài)方向，在機(jī)器人運(yùn)動(dòng)過程中放置臨時(shí)障礙物M，其運(yùn)動(dòng)環(huán)境如圖12所示.

圖12 存在障礙物時(shí)機(jī)器人運(yùn)動(dòng)環(huán)境仿真圖

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

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

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

5 結(jié)束語

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