基于Gazebo的地下礦可視化建模和控制仿真技術(shù)

姚蔚利

（中國(guó)中煤能源集團(tuán)有限公司，北京 100120）

0 引言

現(xiàn)有的無(wú)人駕駛車輛系統(tǒng)是一個(gè)復(fù)雜的硬件設(shè)備，配備了許多傳感器和計(jì)算機(jī)，通常由復(fù)雜的分布式軟件控制。無(wú)人駕駛車輛必須在各種環(huán)境和不斷變化的條件下運(yùn)行并成功執(zhí)行特定任務(wù)。構(gòu)建不同的測(cè)試場(chǎng)測(cè)試無(wú)人駕駛車輛在多種條件下的行為既昂貴又耗時(shí)，而使用完善的模擬環(huán)境可以對(duì)正在開(kāi)發(fā)的無(wú)人駕駛車輛系統(tǒng)進(jìn)行安全且具有成本效益的測(cè)試。仿真不僅縮短了開(kāi)發(fā)周期，而且還可以廣泛應(yīng)用于不同的環(huán)境。

該文使用Gazebo 模擬無(wú)人駕駛車輛模型及地圖環(huán)境模型的搭建工作[1]，并成功地對(duì)MPC 控制算法進(jìn)行可視化測(cè)試。該文的成果如下：1）該文可以作為在Gazebo 下搭建3D環(huán)境仿真模型以及在這些環(huán)境中仿真機(jī)器人模型的教程。2）利用該模型進(jìn)行MPC 控制算法的可視化測(cè)試，模擬過(guò)程中使用的軟件無(wú)須任何修改即可成功控制真實(shí)無(wú)人駕駛車輛。

1 研究方法

1.1 基于Gazebo 的建模技術(shù)

Gazebo 三維多機(jī)器人仿真軟件平臺(tái)能夠模擬具有復(fù)雜關(guān)節(jié)的機(jī)器人，Gazebo 內(nèi)置了基于 OpenGL 圖形接口的渲染引擎（OGRE）和開(kāi)源動(dòng)力學(xué)引擎（ODE），因此能夠很好地模擬現(xiàn)實(shí)環(huán)境中的可移動(dòng)物。而且它內(nèi)建了碰撞檢測(cè)系統(tǒng)，用戶可以通過(guò)給仿真模型添加物理屬性來(lái)完成碰撞檢測(cè)任務(wù)。另外，Gazebo 提供了機(jī)器人運(yùn)動(dòng)控制的仿真接口，可以通過(guò)插件（plugin）的形式來(lái)添加需要驗(yàn)證的算法文件，其與ROS 系統(tǒng)是無(wú)縫兼容的，主要通過(guò)gazebo_ros_pkgs 功能包來(lái)完成消息通信任務(wù)，提供了消息、服務(wù)以及動(dòng)態(tài)參數(shù)配置等功能接口?；?ROS-Gazebo 框架的仿真步驟一般包括模型構(gòu)建、模型可視化、Gazebo 仿真以及運(yùn)動(dòng)控制[2]。目前，基于ROS-Gazebo 的機(jī)器人仿真已經(jīng)在國(guó)內(nèi)外得到了非常廣泛的應(yīng)用。

基于Gazebo 的便捷性，該文利用其構(gòu)建地下礦環(huán)境以及無(wú)人駕駛車輛模型。

1.2 地下礦環(huán)境的構(gòu)建

地下礦虛擬環(huán)境如圖1 所示。首先，根據(jù)圖1（a）在SolidWorks 中建立對(duì)應(yīng)的三維模型，如圖1（b）所示。其中，參照麻地梁地下礦設(shè)計(jì)地下礦環(huán)境模型，巷道寬度為6 m，車道類型為雙向通行，拐角曲率半徑為10 m～15 m，長(zhǎng)直巷道中會(huì)在側(cè)面設(shè)有避車洞（以防止會(huì)車擁擠），并將地圖設(shè)計(jì)為閉環(huán)回路（方便進(jìn)行仿真試驗(yàn)）。在建立三維模型時(shí)，為方便試驗(yàn)觀測(cè)，對(duì)二維模型中的墻壁部分進(jìn)行拉伸，拉伸長(zhǎng)度超過(guò)激光雷達(dá)的掃描范圍。其次，通過(guò)sw2urdf 插件將地下礦三維模型SLDPRT 格式的三維實(shí)體模型文件轉(zhuǎn)換為可以在Gazebo 中加載的URDF 文件，即可使用roslaunch 命令啟動(dòng)Gazebo 并在其中顯示地下礦環(huán)境，如圖1（c）所示。通用機(jī)器人描述格公式（Universal Robot Description Format，URDF）是ROS 中使用的一種機(jī)器人的描述文件，包括連桿、關(guān)節(jié)，運(yùn)動(dòng)學(xué)參數(shù)、動(dòng)力學(xué)參數(shù)、可視化模型以及碰撞檢測(cè)模型等。

圖1 地下礦虛擬環(huán)境

1.3 構(gòu)建無(wú)人駕駛車輛模型

構(gòu)建無(wú)人駕駛車輛模型（圖2）的步驟與環(huán)境構(gòu)建的步驟類似。首先，根據(jù)目前地下礦所使用的一款車輛的參數(shù)在SolidWorks 中搭建車身三維模型，并通過(guò)sw2urdf 插件將車輛模型轉(zhuǎn)換為URDF 格式文件，如圖2（a）所示。URDF 是最初也是比較簡(jiǎn)單的機(jī)器人描述文件，它的結(jié)構(gòu)簡(jiǎn)單明了，容易理解。但是這也導(dǎo)致當(dāng)機(jī)器人模型變得復(fù)雜時(shí)，URDF的結(jié)構(gòu)描述就變得冗長(zhǎng)，無(wú)法簡(jiǎn)潔、有效地描述機(jī)器人部件。xacro 在一定程度上有效地解決了這種問(wèn)題。在本質(zhì)上，xacro 與URDF 是等價(jià)的，但xacro 格式提供了一些更高級(jí)的方式來(lái)組織和編輯機(jī)器人描述，激光雷達(dá)、GPS 以及IMU等傳感器模型及驅(qū)動(dòng)均有標(biāo)準(zhǔn)的xacro 調(diào)用格式。因此，為了方便為車輛添加激光雷達(dá)、GPS 傳感器等配套硬件，需要對(duì)URDF 進(jìn)行修改，將其轉(zhuǎn)化為xacro 格式文件，其步驟如下：1）添加xacro 描述，XML 代碼如圖3 所示。2）在URDF添加后綴.xarcro，此時(shí)該文件可被識(shí)別為xacro 模型。

圖2 車輛虛擬模型

圖3 格式轉(zhuǎn)化代碼

其次，如圖4 所示（XML 代碼），在xacro 文件中修改車輛模型，依次添加車輪模型、阿克曼轉(zhuǎn)向驅(qū)動(dòng)、車輛動(dòng)力驅(qū)動(dòng)、激光雷達(dá)傳感器以及GPS 傳感器。

圖4 雷達(dá)、傳感器及車輛驅(qū)動(dòng)代碼

最后，在Gazebo 中得到的無(wú)人駕駛車輛模型，如圖2（b）所示。

2 MPC 控制建模

對(duì)車輛進(jìn)行運(yùn)動(dòng)學(xué)建模是進(jìn)行自動(dòng)駕駛車輛路徑跟蹤控制的基礎(chǔ)，不僅可以在車輛動(dòng)力學(xué)微分方程基礎(chǔ)上精確地推導(dǎo)控制器的控制律，而且還可以基于車輛運(yùn)動(dòng)學(xué)模型對(duì)車輛的行為進(jìn)行仿真，以驗(yàn)證設(shè)計(jì)的控制器性能。由于車輛本身是一個(gè)復(fù)雜的非線性系統(tǒng)，因此為了提高控制器的實(shí)時(shí)性，需要在能夠較準(zhǔn)確描述車輛動(dòng)力學(xué)特性的基礎(chǔ)上對(duì)車輛模型進(jìn)行理想化假設(shè)：1）假設(shè)車輛無(wú)垂向運(yùn)動(dòng)。2）忽略懸架的作用。3）忽略車輛前后軸載荷的轉(zhuǎn)移。4）假設(shè)輪胎的側(cè)偏特性處于線性范圍。5）假設(shè)車輛僅為前輪轉(zhuǎn)向且縱向速度保持不變。

基于上述假設(shè)，建立車輛的單軌模型，如圖5 所示[3]。其中，o-xyz為車輛坐標(biāo)系，O-XY為地面慣性坐標(biāo)系。假設(shè)車輛是具有不可變形車輪的剛體，可以根據(jù)圖5 的幾何關(guān)系獲得車輛運(yùn)動(dòng)學(xué)模型。

圖5 車輛運(yùn)動(dòng)學(xué)模型

由此可以得到汽車運(yùn)動(dòng)學(xué)模型[4-5]，其微分方程如公式（1）所示。

根據(jù)上述運(yùn)動(dòng)學(xué)模型得到狀態(tài)空間模型的輸入為u=[v,φ]T，狀態(tài)量為X=[x,y,θ]T（x、y為用笛卡爾坐標(biāo)系表示的車輛位置的橫、縱坐標(biāo)；θ為車身在全局地圖下與x軸之間的夾角，記為車身偏角）的控制系統(tǒng)如公式（2）所示。

式中：f為控制系統(tǒng)狀態(tài)方程。

利用泰勒級(jí)數(shù)在參考軌跡點(diǎn)將狀態(tài)空間模型展開(kāi)并忽略高階項(xiàng)，得到公式（3）。

線性化的無(wú)人車駕駛車輛誤差模型如公式（4）所示。

式中：T為采樣時(shí)間。

設(shè)計(jì)預(yù)測(cè)控制的目標(biāo)函數(shù)應(yīng)考慮系統(tǒng)狀態(tài)量的偏差和控制量。因此，將目標(biāo)函數(shù)設(shè)計(jì)如公式（5）所示。

在忽略系統(tǒng)方向和動(dòng)力延遲的情況下，為了保證控制的連續(xù)性，控制量和控制增量的約束條件如公式（6）所示。

式中：umin為控制量下限；umax為控制量上限。

3 仿真測(cè)試試驗(yàn)

在ROS 系統(tǒng)中通過(guò)C++實(shí)現(xiàn)MPC 控制程序，并根據(jù)環(huán)境地圖繪制行駛路線。在仿真試驗(yàn)中，通過(guò)GPS 信號(hào)獲取車輛的實(shí)時(shí)定位，使用MPC 軌跡跟蹤算法計(jì)算得到車輛的2 個(gè)控制量：加速度和前輪轉(zhuǎn)角。在rviz 下對(duì)車輛實(shí)時(shí)位置、規(guī)劃路線和預(yù)測(cè)行駛路線等進(jìn)行可視化處理（圖6），矩形表示車輛本體，車輛下面的線條表示MPC 預(yù)測(cè)的行駛路線，遠(yuǎn)離車輛的點(diǎn)云表示激光雷達(dá)實(shí)時(shí)感知到的代表周圍墻壁的點(diǎn)云。

圖6 rviz 下的可視化效果

MATLAB 與ROS 可以實(shí)現(xiàn)通信，通信時(shí)將運(yùn)行MATLAB的計(jì)算機(jī)當(dāng)作一個(gè)ROS 節(jié)點(diǎn)，通過(guò)同一個(gè)局域網(wǎng)實(shí)現(xiàn)與另一臺(tái)運(yùn)行有節(jié)點(diǎn)管理器的計(jì)算機(jī)建立通信連接的目標(biāo)，從而實(shí)現(xiàn)與ROS 網(wǎng)絡(luò)中的所有節(jié)點(diǎn)進(jìn)行通信連接的目標(biāo)。為了方便對(duì)算法效果進(jìn)行評(píng)估和優(yōu)化，可以先用MATLAB和 SIMULINK 進(jìn)行算法原型設(shè)計(jì)，然后將算法與模型分離，將算法自動(dòng)生成獨(dú)立 ROS 節(jié)點(diǎn)代碼并上傳部署到ROS 機(jī)器人上，在ROS 環(huán)境中運(yùn)行控制代碼，驅(qū)動(dòng)仿真模型，以驗(yàn)證算法的可行性并對(duì)算法效果進(jìn)行評(píng)估。

4 結(jié)語(yǔ)

該文介紹了基于Gazebo 平臺(tái)的地下礦可視化建模和控制仿真技術(shù)。在Gazebo 中搭建了仿真模型并進(jìn)行仿真驗(yàn)證，結(jié)果表明，在Gazebo 下搭建的無(wú)人駕駛環(huán)境及車輛模型對(duì)無(wú)人駕駛車輛軌跡跟蹤控制的研究具有較高的參考價(jià)值。