劉廣亮，馬爭(zhēng)光，朱琳，肖永飛，張艷芳

(齊魯工業(yè)大學(xué)(山東省科學(xué)院) 自動(dòng)化研究所 山東省機(jī)器人與制造自動(dòng)化技術(shù)重點(diǎn)實(shí)驗(yàn)室，山東 濟(jì)南 250014)

機(jī)器人操作系統(tǒng)(robot operating system，ROS)作為一個(gè)面向機(jī)器人應(yīng)用的開源平臺(tái)，提供了機(jī)器人自主導(dǎo)航專用功能包，因此很多機(jī)器人自主導(dǎo)航路徑算法和運(yùn)動(dòng)控制算法研究工作是在ROS平臺(tái)下開展的。如李貽文等[1]采用NVIDIA的Jetson TX2開發(fā)板開展的應(yīng)急監(jiān)測(cè)機(jī)器人自主導(dǎo)航研究，劉小軍等[2]針對(duì)公共衛(wèi)生領(lǐng)域的消毒機(jī)器人控制系統(tǒng)研究，秦國(guó)威等[3]、周旭龍等[4]對(duì)自主導(dǎo)航中用到的Hector-SLAM算法、基于KLD((Kullback-Leible divergence)重采樣的AMCL(adaptive Monte Carlo localization)算法、A-Star算法和DWA(dynamic window approach)動(dòng)態(tài)窗口法進(jìn)行詳細(xì)闡述,石學(xué)文等[5]、丁皓等[6]針對(duì)輪式差速運(yùn)動(dòng)模型進(jìn)行了分析研究,采用FastSLAM算法完成機(jī)器人定位與地圖構(gòu)建,全局路徑規(guī)劃采用蟻群算法,采用動(dòng)態(tài)窗口局部路徑算法對(duì)自主導(dǎo)航系統(tǒng)進(jìn)行分析驗(yàn)證。

本文針對(duì)液壓機(jī)器人的運(yùn)動(dòng)特性和結(jié)構(gòu)特點(diǎn)，對(duì)機(jī)器人的運(yùn)動(dòng)模型、結(jié)構(gòu)模型和里程計(jì)模型進(jìn)行了分析與設(shè)計(jì)，在不配備編碼器和視覺系統(tǒng)的情況下，采用2D平面激光里程計(jì)RF2O(range flow-based 2D odometry)模型設(shè)計(jì)實(shí)現(xiàn)了基于ROS平臺(tái)的液壓機(jī)器人自主導(dǎo)航軟硬件系統(tǒng)運(yùn)行，通過(guò)自行設(shè)計(jì)研發(fā)的液壓機(jī)器人進(jìn)行實(shí)驗(yàn)驗(yàn)證，結(jié)果表明機(jī)器人自主導(dǎo)航定位系統(tǒng)達(dá)到了設(shè)計(jì)要求。

1 模型設(shè)計(jì)

機(jī)器人自主導(dǎo)航系統(tǒng)中的模型設(shè)計(jì)包括運(yùn)動(dòng)控制模型、機(jī)器人結(jié)構(gòu)模型和2D平面里程計(jì)模型。

1.1 運(yùn)動(dòng)控制模型

機(jī)器人自主導(dǎo)航的核心就是對(duì)機(jī)器人的精確定位與運(yùn)動(dòng)控制。本文的機(jī)器人為液壓驅(qū)動(dòng)履帶式機(jī)器人，參照兩輪差速運(yùn)動(dòng)控制系統(tǒng)，構(gòu)建差速運(yùn)動(dòng)模型的運(yùn)動(dòng)控制系統(tǒng)。

獨(dú)立兩輪差速運(yùn)動(dòng)學(xué)模型如圖1所示，全局坐標(biāo)系{XOY}，局部坐標(biāo)系{X′O′}。局部坐標(biāo)Y′與兩輪的中軸線重合，從右輪指向左輪。X′指向機(jī)器人的正前方。假設(shè)機(jī)器人質(zhì)心C位于兩驅(qū)動(dòng)輪軸線的中點(diǎn)，與坐標(biāo)系原點(diǎn)O′重合。局部坐標(biāo)系相當(dāng)于全局坐標(biāo)系的旋轉(zhuǎn)角為θ，為機(jī)器人的方向角。

圖1 兩輪差速運(yùn)動(dòng)學(xué)模型Fig.1 Kinematic model of two-wheel differential drive

假定兩驅(qū)動(dòng)輪軸線的長(zhǎng)度為l，左右驅(qū)動(dòng)輪半徑都為r，機(jī)器人的位姿向量為p=(x,y,θ)T，根據(jù)剛體力學(xué)知識(shí)可得兩輪差速驅(qū)動(dòng)移動(dòng)機(jī)器人的正運(yùn)動(dòng)學(xué)方程[7]為：

(1)

(2)

(3)

其中，v為機(jī)器人質(zhì)心C處的線速度，w為機(jī)器人的轉(zhuǎn)向角速度，vr、vl分別為機(jī)器人的左右驅(qū)動(dòng)輪線速度。

在機(jī)器人運(yùn)動(dòng)時(shí)，通常給定機(jī)器人中心點(diǎn)的線速度和角速度，然后根據(jù)式(1)計(jì)算出左右兩個(gè)驅(qū)動(dòng)輪的線速度，也是機(jī)器人的驅(qū)動(dòng)電機(jī)速度vr、vl與機(jī)器人線速度v和角速度w間的關(guān)系。在全局坐標(biāo)系下，機(jī)器人的代表點(diǎn)P的位置和姿態(tài)可以通過(guò)式(2)中的速度關(guān)系式積分求得。式(3)也是機(jī)器人的航跡演算公式，用來(lái)計(jì)算每個(gè)時(shí)刻機(jī)器人軌跡姿態(tài)。

假設(shè)相鄰時(shí)間段內(nèi)機(jī)器人曲線航跡近似為直線[8-9]，如圖2所示。同時(shí)假設(shè)機(jī)器人不是全向移動(dòng)，只是前進(jìn)和旋轉(zhuǎn)，不能縱向移動(dòng)。機(jī)器人兩個(gè)相鄰運(yùn)動(dòng)時(shí)刻在采樣周期內(nèi)運(yùn)動(dòng)距離短，可以將軌跡看作勻速直線運(yùn)動(dòng)，也就是說(shuō)機(jī)器人在Δt時(shí)間內(nèi)沿局部坐標(biāo)系X′軸移動(dòng)了vΔt，在全局坐標(biāo)系統(tǒng)中移動(dòng)距離為：

(4)

那么，t+1時(shí)刻機(jī)器人姿態(tài)為：

(5)

圖2 相鄰點(diǎn)軌跡的直線近似Fig.2 Linear approximation of adjacent trajectory points

1.2 機(jī)器人結(jié)構(gòu)模型

在ROS架構(gòu)平臺(tái)中對(duì)機(jī)器人結(jié)構(gòu)模型的定義采用統(tǒng)一機(jī)器人描述格式，即URDF(unified robot description format)格式[10]，URDF定義的機(jī)器人模型主要由關(guān)節(jié)(joint)和連接(link)標(biāo)簽組成(圖3)，每一個(gè)link對(duì)應(yīng)著一個(gè)frame(本地坐標(biāo)系)，在joint中定義child link與parent link之間的平移(xyz三個(gè)方向)與旋轉(zhuǎn)(rpy歐拉角，即Roll(滾轉(zhuǎn)角),Pitch(俯仰角),Yaw(偏航角))關(guān)系，也是兩個(gè)本地坐標(biāo)系之間的變換關(guān)系。

圖3 URDF模型結(jié)構(gòu)Fig.3 Structure of the URDF model

參照輪式機(jī)器人，本文的履帶式液壓機(jī)器人定義了機(jī)器人底座、驅(qū)動(dòng)液壓馬達(dá)、導(dǎo)向輪、激光雷達(dá)等7個(gè)關(guān)節(jié)和連接標(biāo)簽，見圖4。

圖4 機(jī)器人模型圖Fig.4 Robot model

圖4中base_link對(duì)應(yīng)機(jī)器人本體坐標(biāo)系，原點(diǎn)與機(jī)器人中心重合，為機(jī)器人的旋轉(zhuǎn)中心；base_footprint機(jī)器人基座坐標(biāo)系，原點(diǎn)為base_link的原點(diǎn)在地面的投影，與base_link的z值不同；left_wheel_link、right_wheel_link、left_front_wheel_link、right_front_wheel_link分別為左右驅(qū)動(dòng)輪和左右導(dǎo)向輪link；laser_link對(duì)應(yīng)激光雷達(dá)坐標(biāo)系。激光雷達(dá)數(shù)據(jù)是以激光雷達(dá)坐標(biāo)系的測(cè)量數(shù)據(jù)，在進(jìn)行機(jī)器人定位時(shí)需要轉(zhuǎn)換為以機(jī)器人本體坐標(biāo)系的位置，因此需要定義激光雷達(dá)坐標(biāo)系與機(jī)器人本體坐標(biāo)系相對(duì)關(guān)系，激光雷達(dá)安裝高度0.65 m，距離中心(base_link)正前方0.85 m，如下為laser_joint在URDF中的定義格式。

1.3 2D平面里程計(jì)模型

2D平面里程計(jì)[11]反映機(jī)器人在世界坐標(biāo)系下的位姿信息，位姿信息包括平面位置(x,y)、方向角θ和速度(前進(jìn)速度v和轉(zhuǎn)向速度w)。位姿信息表示從里程計(jì)坐標(biāo)系(odom_frame)到機(jī)器人坐標(biāo)系(base_frame)的坐標(biāo)變換關(guān)系。ROS通過(guò)Tf實(shí)時(shí)發(fā)布這個(gè)坐標(biāo)系變換關(guān)系來(lái)提示機(jī)器人位置信息，這個(gè)坐標(biāo)系變換關(guān)系由式(3)、(5)計(jì)算。

本文的液壓機(jī)器人沒有配備編碼器和視覺系統(tǒng)(TransForm)[12-13]，里程計(jì)模型采用2D平面激光里程計(jì)(RF2O)[14]，該方法采集的激光雷達(dá)信息，具有快速響應(yīng)、定位精確的優(yōu)點(diǎn)，特別適合已經(jīng)采用激光雷達(dá)進(jìn)行避障或定位的機(jī)器人。該方法通過(guò)連續(xù)的掃描來(lái)估計(jì)激光雷達(dá)的平面運(yùn)動(dòng)，即機(jī)器人的運(yùn)動(dòng)。對(duì)于每一個(gè)掃描點(diǎn)，對(duì)激光雷達(dá)相對(duì)速度建立掃描距離的流約束方程，然后通過(guò)最小化幾何約束的魯棒函數(shù)獲得激光雷達(dá)的運(yùn)動(dòng)估計(jì)。

如圖5所示，設(shè)R(t,α)為激光雷達(dá)掃描距離函數(shù)，其中t為掃描時(shí)間，α∈[0,N)?R為掃描角度，N為掃描點(diǎn)數(shù)。P點(diǎn)在激光雷達(dá)局部參考系中的極坐標(biāo)為(r,θ)。

圖5 2D激光里程計(jì)(RF2O)模型Fig.5 2D lidar odometric model (RF2O)

α與θ關(guān)系為：

(6)

假設(shè)R是可微的，在第二次掃描時(shí)點(diǎn)P′掃描距離函數(shù)為R(t+Δt,α+Δα)，泰勒展開式為：

(7)

舍棄掉高階項(xiàng)，則當(dāng)掃描距離和掃描角度在[t,t+Δt]內(nèi)變化時(shí)，掃描距離函數(shù)的時(shí)間梯度變化近似為：

(8)

(9)

對(duì)應(yīng)的笛卡爾坐標(biāo)公式：

(10)

假設(shè)掃描點(diǎn)相對(duì)于激光雷達(dá)的運(yùn)動(dòng)為一個(gè)剛體平移和旋轉(zhuǎn)，則掃描點(diǎn)的速度與激光雷達(dá)的速度大小相同，方向相反，即

(11)

其中(x,y)為P點(diǎn)笛卡爾坐標(biāo)。將式(10)、(11)代入式(9)得到：

(12)

在式(12)中代入每個(gè)掃描點(diǎn)的角度和坐標(biāo)即可求解出激光雷達(dá)的速度。

2 機(jī)器人自主導(dǎo)航系統(tǒng)架構(gòu)

機(jī)器人自主導(dǎo)航系統(tǒng)基本包括機(jī)器人本體、機(jī)器人運(yùn)動(dòng)控制系統(tǒng)、傳感器、ROS導(dǎo)航包、上位監(jiān)控平臺(tái)(圖6)。基于ROS的機(jī)器人運(yùn)動(dòng)控制系統(tǒng)[15]的實(shí)現(xiàn)方法在文獻(xiàn)[16]中有詳細(xì)論述。

圖6 機(jī)器人自主導(dǎo)航架構(gòu)Fig.6 Framework of autonomous robot navigation

2.1 機(jī)器人本體

機(jī)器人本體是機(jī)器人自主導(dǎo)航系統(tǒng)的載體，搭載電源、電機(jī)、驅(qū)動(dòng)器、控制器與通信系統(tǒng)等(圖7)，液壓機(jī)器人為液壓驅(qū)動(dòng)履帶式機(jī)器人，配備激光雷達(dá)用于導(dǎo)航，沒有安裝編碼器。

圖7 履帶式液壓機(jī)器人Fig.7 A crawling hydraulic robot

2.2 傳感器

液壓機(jī)器人位置感知傳感器采用激光雷達(dá)，激光雷達(dá)采集的距離和角度數(shù)據(jù)信息，通過(guò)RF2O方法計(jì)算出機(jī)器人里程計(jì)軌跡信息，并以ROS話題方式將里程計(jì)信息發(fā)送出去。同時(shí)ROS全局路徑規(guī)劃器和局部路徑規(guī)劃器也要接收里程計(jì)信息用于代價(jià)地圖的更新。

本文設(shè)計(jì)的自主導(dǎo)航系統(tǒng)使用lms1xx節(jié)點(diǎn)采集激光雷達(dá)數(shù)據(jù)信息并發(fā)布掃描數(shù)據(jù)話題(scan)，同時(shí)啟動(dòng)激光雷達(dá)里程計(jì)節(jié)點(diǎn)rf2o_laser_odometry，訂閱掃描數(shù)據(jù)話題(scan)，里程計(jì)計(jì)算之后發(fā)布里程計(jì)話題(odom)。

2.3 ROS導(dǎo)航功能包

ROS導(dǎo)航功能包集合包含了機(jī)器人自主導(dǎo)航所需基本功能。主要包括move_base功能包、構(gòu)建地圖功能包、自適應(yīng)蒙特卡羅濾波定位(Amcl)功能包、Tf功能包、全局路徑規(guī)劃功能包、局部路徑規(guī)劃功能包以及各個(gè)功能包的詳細(xì)參數(shù)配置。ROS平臺(tái)具有良好的擴(kuò)展性，具體體現(xiàn)在豐富的參數(shù)配置功能，用戶可以在啟動(dòng)文件(launch文件)或者其他參數(shù)文件(yaml文件、urdf文件)等進(jìn)行功能和算法參數(shù)的配置，如控制器參數(shù)、機(jī)器人本體描述參數(shù)、全局路徑規(guī)劃參數(shù)、局部路徑規(guī)劃參數(shù)、全局代價(jià)地圖參數(shù)、局部代價(jià)地圖參數(shù)、move_base參數(shù)和Acml參數(shù)等，當(dāng)然ROS難點(diǎn)和復(fù)雜之處也在于如何配置這些參數(shù)才能使得機(jī)器人定位更加準(zhǔn)確，運(yùn)行更加平穩(wěn)。

2.4 上位監(jiān)控平臺(tái)

一個(gè)完整機(jī)器人導(dǎo)航系統(tǒng)除包括機(jī)器人本體運(yùn)動(dòng)控制和自主導(dǎo)航之外，另一個(gè)重要部分是上位機(jī)監(jiān)控平臺(tái)系統(tǒng)。上位監(jiān)控平臺(tái)主要功能有機(jī)器人管理、導(dǎo)航地圖管理、任務(wù)管理、機(jī)器人狀態(tài)監(jiān)視等。機(jī)器人管理是對(duì)機(jī)器人連接地址(IP地址)、名稱以及多機(jī)器人進(jìn)行管理等，導(dǎo)航地圖管理包括加載、保存、編輯導(dǎo)航地圖，任務(wù)管理是對(duì)機(jī)器人執(zhí)行不同任務(wù)設(shè)定不同運(yùn)動(dòng)路徑進(jìn)行維護(hù)管理等功能。

2.5 Tf坐標(biāo)變換

本文的液壓機(jī)器人模型定義了7個(gè)link和joint，每個(gè)link對(duì)應(yīng)一個(gè)局部坐標(biāo)系，坐標(biāo)系之間的變換關(guān)系通過(guò)ROS中的Tf實(shí)時(shí)反饋獲得。Tf坐標(biāo)變換包括位置和姿態(tài)兩個(gè)方面的變換，ROS系統(tǒng)采用話題通信機(jī)制持續(xù)地發(fā)布不同link之間的坐標(biāo)系轉(zhuǎn)換關(guān)系，圖8為本文機(jī)器人模型的Tf關(guān)系圖。

圖8 機(jī)器人Tf關(guān)系圖Fig.8 Robot Tf tree view

在機(jī)器人導(dǎo)航中涉及到三個(gè)坐標(biāo)系，即機(jī)器人本體坐標(biāo)系、里程計(jì)坐標(biāo)系和地圖坐標(biāo)系(圖3～4)，采用右手坐標(biāo)系。

(1)機(jī)器人本體坐標(biāo)系(base_frame)。對(duì)應(yīng)URDF中的base_link，以機(jī)器人本體中心的坐標(biāo)系。原點(diǎn)可以在機(jī)器人模型中定義為本體的任何位置，一般約定x軸指向機(jī)器人前方，y軸為指向小車左方，z軸為指向小車上方。

(2)里程計(jì)坐標(biāo)系(odom_frame)。是固定在環(huán)境中的坐標(biāo)系，其原點(diǎn)和方向不會(huì)隨著機(jī)器人運(yùn)動(dòng)而改變，但里程計(jì)坐標(biāo)系的位置會(huì)隨著機(jī)器人的運(yùn)動(dòng)發(fā)生漂移。因此造成odom不是長(zhǎng)周期的全局坐標(biāo)，然而在里程計(jì)坐標(biāo)系下機(jī)器人的位置是連續(xù)變化的，不會(huì)有突變和跳躍，里程計(jì)坐標(biāo)系是一個(gè)短期、局域精確坐標(biāo)系。機(jī)器人在里程計(jì)坐標(biāo)系中的坐標(biāo)是通過(guò)里程計(jì)信息(如從編碼器、視覺、陀螺儀和加速度計(jì)采集的信息)計(jì)算出來(lái)的，本文的里程計(jì)信息是從激光雷達(dá)點(diǎn)云數(shù)據(jù)計(jì)算得到的。

(3)地圖坐標(biāo)系(map_frame)。是一個(gè)固定在環(huán)境中的世界坐標(biāo)系，在此坐標(biāo)系下的機(jī)器人坐標(biāo)不隨時(shí)間漂移，不需要保證連續(xù)性。地圖坐標(biāo)系是一個(gè)長(zhǎng)期全局坐標(biāo)系，由于坐標(biāo)會(huì)跳躍改變，作為局部坐標(biāo)系較差，不適用于類似避障操作的局部運(yùn)動(dòng)。

三種坐標(biāo)系基本關(guān)系見圖9。首先機(jī)器人本體坐標(biāo)通過(guò)獲取里程計(jì)信息，計(jì)算得到里程計(jì)坐標(biāo)系下機(jī)器人坐標(biāo)(式(5))，此坐標(biāo)因?yàn)橛衅普`差，所以再經(jīng)過(guò)Amcl估計(jì)校正計(jì)算從而獲得機(jī)器人在地圖坐標(biāo)系下的坐標(biāo)。

圖9 機(jī)器人坐標(biāo)系、地圖坐標(biāo)系和里程計(jì)坐標(biāo)系之間關(guān)系Fig.9 Relation between base, map, and odom frames

3 實(shí)驗(yàn)結(jié)果與分析

為驗(yàn)證液壓機(jī)器人自主導(dǎo)航系統(tǒng)，我們?cè)谑覂?nèi)場(chǎng)地規(guī)劃4個(gè)目標(biāo)點(diǎn)和矩形運(yùn)動(dòng)路徑,見圖10，對(duì)機(jī)器人系統(tǒng)進(jìn)行了三次測(cè)試，記錄下機(jī)器人實(shí)際運(yùn)動(dòng)軌跡，設(shè)定允許誤差為0.3 m，導(dǎo)航地圖分辨率為0.05 m。

圖10 測(cè)試路徑Fig.10 Paths employed in robot testing

X坐標(biāo)最大誤差4.83，Y最大誤差4.97，換算為cm，地圖分辨率是0.05 m，即一個(gè)像素5 cm，整體小于25 cm。

圖11中四個(gè)紅色圓點(diǎn)為圖10中的4個(gè)標(biāo)記點(diǎn)，綠色箭頭為每個(gè)目標(biāo)位置點(diǎn)機(jī)器人方向，綠色線為規(guī)劃路徑，黃色箭頭為路徑方向。藍(lán)色線為機(jī)器人實(shí)際根據(jù)配置參數(shù)的運(yùn)行路線。機(jī)器人運(yùn)行到每個(gè)目標(biāo)點(diǎn)系統(tǒng)會(huì)記錄在地圖中的實(shí)際位置(表1)。實(shí)驗(yàn)結(jié)果表明，3次4個(gè)目標(biāo)點(diǎn)跟蹤的測(cè)量誤差均小于0.25 m，符合規(guī)劃運(yùn)行誤差小于0.3 m的要求。

圖11 液壓機(jī)器人自主導(dǎo)航Fig.11 Autonomous navigation of the hydraulic robot

表1 定位導(dǎo)航測(cè)試結(jié)果

4 結(jié)論

由于液壓系統(tǒng)具有時(shí)滯性、強(qiáng)非線性、時(shí)變負(fù)載性的特點(diǎn)，所以液壓機(jī)器人的運(yùn)動(dòng)控制穩(wěn)定性和精度是難點(diǎn)問(wèn)題，本文在分析了機(jī)器人運(yùn)動(dòng)控制模型、結(jié)構(gòu)模型和里程計(jì)模型的基礎(chǔ)上，采用RF2O平面激光雷達(dá)里程計(jì)方法，設(shè)計(jì)實(shí)現(xiàn)了履帶式液壓機(jī)器人的自主定位導(dǎo)航。但是由于采用純激光反饋信息，對(duì)應(yīng)動(dòng)態(tài)場(chǎng)景容易出現(xiàn)轉(zhuǎn)向誤差，未來(lái)可以通過(guò)增加IMU(inertial measurement unit)傳感器進(jìn)行信息融合來(lái)減少定位誤差。