基于機器人操作系統的液壓機器人自主導航系統設計與實現

劉廣亮，馬爭光，朱琳，肖永飛，張艷芳

(齊魯工業大學(山東省科學院) 自動化研究所 山東省機器人與制造自動化技術重點實驗室，山東 濟南 250014)

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

本文針對液壓機器人的運動特性和結構特點，對機器人的運動模型、結構模型和里程計模型進行了分析與設計，在不配備編碼器和視覺系統的情況下，采用2D平面激光里程計RF2O(range flow-based 2D odometry)模型設計實現了基于ROS平臺的液壓機器人自主導航軟硬件系統運行，通過自行設計研發的液壓機器人進行實驗驗證，結果表明機器人自主導航定位系統達到了設計要求。

1 模型設計

機器人自主導航系統中的模型設計包括運動控制模型、機器人結構模型和2D平面里程計模型。

1.1 運動控制模型

機器人自主導航的核心就是對機器人的精確定位與運動控制。本文的機器人為液壓驅動履帶式機器人，參照兩輪差速運動控制系統，構建差速運動模型的運動控制系統。

獨立兩輪差速運動學模型如圖1所示，全局坐標系{XOY}，局部坐標系{X′O′}。局部坐標Y′與兩輪的中軸線重合，從右輪指向左輪。X′指向機器人的正前方。假設機器人質心C位于兩驅動輪軸線的中點，與坐標系原點O′重合。局部坐標系相當于全局坐標系的旋轉角為θ，為機器人的方向角。

圖1 兩輪差速運動學模型Fig.1 Kinematic model of two-wheel differential drive

假定兩驅動輪軸線的長度為l，左右驅動輪半徑都為r，機器人的位姿向量為p=(x,y,θ)T，根據剛體力學知識可得兩輪差速驅動移動機器人的正運動學方程[7]為：

(1)

(2)

(3)

其中，v為機器人質心C處的線速度，w為機器人的轉向角速度，vr、vl分別為機器人的左右驅動輪線速度。

在機器人運動時，通常給定機器人中心點的線速度和角速度，然后根據式(1)計算出左右兩個驅動輪的線速度，也是機器人的驅動電機速度vr、vl與機器人線速度v和角速度w間的關系。在全局坐標系下，機器人的代表點P的位置和姿態可以通過式(2)中的速度關系式積分求得。式(3)也是機器人的航跡演算公式，用來計算每個時刻機器人軌跡姿態。

假設相鄰時間段內機器人曲線航跡近似為直線[8-9]，如圖2所示。同時假設機器人不是全向移動，只是前進和旋轉，不能縱向移動。機器人兩個相鄰運動時刻在采樣周期內運動距離短，可以將軌跡看作勻速直線運動，也就是說機器人在Δt時間內沿局部坐標系X′軸移動了vΔt，在全局坐標系統中移動距離為：

(4)

那么，t+1時刻機器人姿態為：

(5)

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

1.2 機器人結構模型

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

圖3 URDF模型結構Fig.3 Structure of the URDF model

參照輪式機器人，本文的履帶式液壓機器人定義了機器人底座、驅動液壓馬達、導向輪、激光雷達等7個關節和連接標簽，見圖4。

圖4 機器人模型圖Fig.4 Robot model

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

1.3 2D平面里程計模型

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

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

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

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

α與θ關系為：

(6)

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

(7)

舍棄掉高階項，則當掃描距離和掃描角度在[t,t+Δt]內變化時，掃描距離函數的時間梯度變化近似為：

(8)

(9)

對應的笛卡爾坐標公式：

(10)

假設掃描點相對于激光雷達的運動為一個剛體平移和旋轉，則掃描點的速度與激光雷達的速度大小相同，方向相反，即

(11)

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

(12)

在式(12)中代入每個掃描點的角度和坐標即可求解出激光雷達的速度。

2 機器人自主導航系統架構

機器人自主導航系統基本包括機器人本體、機器人運動控制系統、傳感器、ROS導航包、上位監控平臺(圖6)。基于ROS的機器人運動控制系統[15]的實現方法在文獻[16]中有詳細論述。

圖6 機器人自主導航架構Fig.6 Framework of autonomous robot navigation

2.1 機器人本體

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

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

2.2 傳感器

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

本文設計的自主導航系統使用lms1xx節點采集激光雷達數據信息并發布掃描數據話題(scan)，同時啟動激光雷達里程計節點rf2o_laser_odometry，訂閱掃描數據話題(scan)，里程計計算之后發布里程計話題(odom)。

2.3 ROS導航功能包

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

2.4 上位監控平臺

一個完整機器人導航系統除包括機器人本體運動控制和自主導航之外，另一個重要部分是上位機監控平臺系統。上位監控平臺主要功能有機器人管理、導航地圖管理、任務管理、機器人狀態監視等。機器人管理是對機器人連接地址(IP地址)、名稱以及多機器人進行管理等，導航地圖管理包括加載、保存、編輯導航地圖，任務管理是對機器人執行不同任務設定不同運動路徑進行維護管理等功能。

2.5 Tf坐標變換

本文的液壓機器人模型定義了7個link和joint，每個link對應一個局部坐標系，坐標系之間的變換關系通過ROS中的Tf實時反饋獲得。Tf坐標變換包括位置和姿態兩個方面的變換，ROS系統采用話題通信機制持續地發布不同link之間的坐標系轉換關系，圖8為本文機器人模型的Tf關系圖。

圖8 機器人Tf關系圖Fig.8 Robot Tf tree view

在機器人導航中涉及到三個坐標系，即機器人本體坐標系、里程計坐標系和地圖坐標系(圖3～4)，采用右手坐標系。

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

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

(3)地圖坐標系(map_frame)。是一個固定在環境中的世界坐標系，在此坐標系下的機器人坐標不隨時間漂移，不需要保證連續性。地圖坐標系是一個長期全局坐標系，由于坐標會跳躍改變，作為局部坐標系較差，不適用于類似避障操作的局部運動。

三種坐標系基本關系見圖9。首先機器人本體坐標通過獲取里程計信息，計算得到里程計坐標系下機器人坐標(式(5))，此坐標因為有漂移誤差，所以再經過Amcl估計校正計算從而獲得機器人在地圖坐標系下的坐標。

圖9 機器人坐標系、地圖坐標系和里程計坐標系之間關系Fig.9 Relation between base, map, and odom frames

3 實驗結果與分析

為驗證液壓機器人自主導航系統，我們在室內場地規劃4個目標點和矩形運動路徑,見圖10，對機器人系統進行了三次測試，記錄下機器人實際運動軌跡，設定允許誤差為0.3 m，導航地圖分辨率為0.05 m。

圖10 測試路徑Fig.10 Paths employed in robot testing

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

圖11中四個紅色圓點為圖10中的4個標記點，綠色箭頭為每個目標位置點機器人方向，綠色線為規劃路徑，黃色箭頭為路徑方向。藍色線為機器人實際根據配置參數的運行路線。機器人運行到每個目標點系統會記錄在地圖中的實際位置(表1)。實驗結果表明，3次4個目標點跟蹤的測量誤差均小于0.25 m，符合規劃運行誤差小于0.3 m的要求。

圖11 液壓機器人自主導航Fig.11 Autonomous navigation of the hydraulic robot

表1 定位導航測試結果

4 結論

由于液壓系統具有時滯性、強非線性、時變負載性的特點，所以液壓機器人的運動控制穩定性和精度是難點問題，本文在分析了機器人運動控制模型、結構模型和里程計模型的基礎上，采用RF2O平面激光雷達里程計方法，設計實現了履帶式液壓機器人的自主定位導航。但是由于采用純激光反饋信息，對應動態場景容易出現轉向誤差，未來可以通過增加IMU(inertial measurement unit)傳感器進行信息融合來減少定位誤差。