姚蔚利

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

0 引言

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

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

1 研究方法

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

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

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

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，長直巷道中會(huì)在側(cè)面設(shè)有避車洞（以防止會(huì)車擁擠），并將地圖設(shè)計(jì)為閉環(huán)回路（方便進(jìn)行仿真試驗(yàn)）。在建立三維模型時(shí)，為方便試驗(yàn)觀測，對(duì)二維模型中的墻壁部分進(jìn)行拉伸，拉伸長度超過激光雷達(dá)的掃描范圍。其次，通過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ù)、可視化模型以及碰撞檢測模型等。

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

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

構(gòu)建無人駕駛車輛模型（圖2）的步驟與環(huán)境構(gòu)建的步驟類似。首先，根據(jù)目前地下礦所使用的一款車輛的參數(shù)在SolidWorks 中搭建車身三維模型，并通過sw2urdf 插件將車輛模型轉(zhuǎn)換為URDF 格式文件，如圖2（a）所示。URDF 是最初也是比較簡單的機(jī)器人描述文件，它的結(jié)構(gòu)簡單明了，容易理解。但是這也導(dǎo)致當(dāng)機(jī)器人模型變得復(fù)雜時(shí)，URDF的結(jié)構(gòu)描述就變得冗長，無法簡潔、有效地描述機(jī)器人部件。xacro 在一定程度上有效地解決了這種問題。在本質(zhì)上，xacro 與URDF 是等價(jià)的，但xacro 格式提供了一些更高級(jí)的方式來組織和編輯機(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 中得到的無人駕駛車輛模型，如圖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è)車輛無垂向運(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)空間模型展開并忽略高階項(xiàng)，得到公式（3）。

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

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

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

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

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

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

在ROS 系統(tǒng)中通過C++實(shí)現(xiàn)MPC 控制程序，并根據(jù)環(huán)境地圖繪制行駛路線。在仿真試驗(yàn)中，通過GPS 信號(hào)獲取車輛的實(shí)時(shí)定位，使用MPC 軌跡跟蹤算法計(jì)算得到車輛的2 個(gè)控制量：加速度和前輪轉(zhuǎn)角。在rviz 下對(duì)車輛實(shí)時(shí)位置、規(guī)劃路線和預(yù)測行駛路線等進(jìn)行可視化處理（圖6），矩形表示車輛本體，車輛下面的線條表示MPC 預(yù)測的行駛路線，遠(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)，通過同一個(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é)語

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