基于Gazebo的地下矿可视化建模和控制仿真技术

姚蔚利

（中国中煤能源集团有限公司，北京 100120）

0 引言

现有的无人驾驶车辆系统是一个复杂的硬件设备，配备了许多传感器和计算机，通常由复杂的分布式软件控制。无人驾驶车辆必须在各种环境和不断变化的条件下运行并成功执行特定任务。构建不同的测试场测试无人驾驶车辆在多种条件下的行为既昂贵又耗时，而使用完善的模拟环境可以对正在开发的无人驾驶车辆系统进行安全且具有成本效益的测试。仿真不仅缩短了开发周期，而且还可以广泛应用于不同的环境。

该文使用Gazebo 模拟无人驾驶车辆模型及地图环境模型的搭建工作[1]，并成功地对MPC 控制算法进行可视化测试。该文的成果如下：1）该文可以作为在Gazebo 下搭建3D环境仿真模型以及在这些环境中仿真机器人模型的教程。2）利用该模型进行MPC 控制算法的可视化测试，模拟过程中使用的软件无须任何修改即可成功控制真实无人驾驶车辆。

1 研究方法

1.1 基于Gazebo 的建模技术

Gazebo 三维多机器人仿真软件平台能够模拟具有复杂关节的机器人，Gazebo 内置了基于 OpenGL 图形接口的渲染引擎（OGRE）和开源动力学引擎（ODE），因此能够很好地模拟现实环境中的可移动物。而且它内建了碰撞检测系统，用户可以通过给仿真模型添加物理属性来完成碰撞检测任务。另外，Gazebo 提供了机器人运动控制的仿真接口，可以通过插件（plugin）的形式来添加需要验证的算法文件，其与ROS 系统是无缝兼容的，主要通过gazebo_ros_pkgs 功能包来完成消息通信任务，提供了消息、服务以及动态参数配置等功能接口。基于 ROS-Gazebo 框架的仿真步骤一般包括模型构建、模型可视化、Gazebo 仿真以及运动控制[2]。目前，基于ROS-Gazebo 的机器人仿真已经在国内外得到了非常广泛的应用。

基于Gazebo 的便捷性，该文利用其构建地下矿环境以及无人驾驶车辆模型。

1.2 地下矿环境的构建

地下矿虚拟环境如图1 所示。首先，根据图1（a）在SolidWorks 中建立对应的三维模型，如图1（b）所示。其中，参照麻地梁地下矿设计地下矿环境模型，巷道宽度为6 m，车道类型为双向通行，拐角曲率半径为10 m～15 m，长直巷道中会在侧面设有避车洞（以防止会车拥挤），并将地图设计为闭环回路（方便进行仿真试验）。在建立三维模型时，为方便试验观测，对二维模型中的墙壁部分进行拉伸，拉伸长度超过激光雷达的扫描范围。其次，通过sw2urdf 插件将地下矿三维模型SLDPRT 格式的三维实体模型文件转换为可以在Gazebo 中加载的URDF 文件，即可使用roslaunch 命令启动Gazebo 并在其中显示地下矿环境，如图1（c）所示。通用机器人描述格公式（Universal Robot Description Format，URDF）是ROS 中使用的一种机器人的描述文件，包括连杆、关节，运动学参数、动力学参数、可视化模型以及碰撞检测模型等。

图1 地下矿虚拟环境

1.3 构建无人驾驶车辆模型

构建无人驾驶车辆模型（图2）的步骤与环境构建的步骤类似。首先，根据目前地下矿所使用的一款车辆的参数在SolidWorks 中搭建车身三维模型，并通过sw2urdf 插件将车辆模型转换为URDF 格式文件，如图2（a）所示。URDF 是最初也是比较简单的机器人描述文件，它的结构简单明了，容易理解。但是这也导致当机器人模型变得复杂时，URDF的结构描述就变得冗长，无法简洁、有效地描述机器人部件。xacro 在一定程度上有效地解决了这种问题。在本质上，xacro 与URDF 是等价的，但xacro 格式提供了一些更高级的方式来组织和编辑机器人描述，激光雷达、GPS 以及IMU等传感器模型及驱动均有标准的xacro 调用格式。因此，为了方便为车辆添加激光雷达、GPS 传感器等配套硬件，需要对URDF 进行修改，将其转化为xacro 格式文件，其步骤如下：1）添加xacro 描述，XML 代码如图3 所示。2）在URDF添加后缀.xarcro，此时该文件可被识别为xacro 模型。

图2 车辆虚拟模型

图3 格式转化代码

其次，如图4 所示（XML 代码），在xacro 文件中修改车辆模型，依次添加车轮模型、阿克曼转向驱动、车辆动力驱动、激光雷达传感器以及GPS 传感器。

图4 雷达、传感器及车辆驱动代码

最后，在Gazebo 中得到的无人驾驶车辆模型，如图2（b）所示。

2 MPC 控制建模

对车辆进行运动学建模是进行自动驾驶车辆路径跟踪控制的基础，不仅可以在车辆动力学微分方程基础上精确地推导控制器的控制律，而且还可以基于车辆运动学模型对车辆的行为进行仿真，以验证设计的控制器性能。由于车辆本身是一个复杂的非线性系统，因此为了提高控制器的实时性，需要在能够较准确描述车辆动力学特性的基础上对车辆模型进行理想化假设：1）假设车辆无垂向运动。2）忽略悬架的作用。3）忽略车辆前后轴载荷的转移。4）假设轮胎的侧偏特性处于线性范围。5）假设车辆仅为前轮转向且纵向速度保持不变。

基于上述假设，建立车辆的单轨模型，如图5 所示[3]。其中，o-xyz为车辆坐标系，O-XY为地面惯性坐标系。假设车辆是具有不可变形车轮的刚体，可以根据图5 的几何关系获得车辆运动学模型。

图5 车辆运动学模型

由此可以得到汽车运动学模型[4-5]，其微分方程如公式（1）所示。

根据上述运动学模型得到状态空间模型的输入为u=[v,φ]T，状态量为X=[x,y,θ]T（x、y为用笛卡尔坐标系表示的车辆位置的横、纵坐标；θ为车身在全局地图下与x轴之间的夹角，记为车身偏角）的控制系统如公式（2）所示。

式中：f为控制系统状态方程。

利用泰勒级数在参考轨迹点将状态空间模型展开并忽略高阶项，得到公式（3）。

线性化的无人车驾驶车辆误差模型如公式（4）所示。

式中：T为采样时间。

设计预测控制的目标函数应考虑系统状态量的偏差和控制量。因此，将目标函数设计如公式（5）所示。

在忽略系统方向和动力延迟的情况下，为了保证控制的连续性，控制量和控制增量的约束条件如公式（6）所示。

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

3 仿真测试试验

在ROS 系统中通过C++实现MPC 控制程序，并根据环境地图绘制行驶路线。在仿真试验中，通过GPS 信号获取车辆的实时定位，使用MPC 轨迹跟踪算法计算得到车辆的2 个控制量：加速度和前轮转角。在rviz 下对车辆实时位置、规划路线和预测行驶路线等进行可视化处理（图6），矩形表示车辆本体，车辆下面的线条表示MPC 预测的行驶路线，远离车辆的点云表示激光雷达实时感知到的代表周围墙壁的点云。

图6 rviz 下的可视化效果

MATLAB 与ROS 可以实现通信，通信时将运行MATLAB的计算机当作一个ROS 节点，通过同一个局域网实现与另一台运行有节点管理器的计算机建立通信连接的目标，从而实现与ROS 网络中的所有节点进行通信连接的目标。为了方便对算法效果进行评估和优化，可以先用MATLAB和 SIMULINK 进行算法原型设计，然后将算法与模型分离，将算法自动生成独立 ROS 节点代码并上传部署到ROS 机器人上，在ROS 环境中运行控制代码，驱动仿真模型，以验证算法的可行性并对算法效果进行评估。

4 结语

该文介绍了基于Gazebo 平台的地下矿可视化建模和控制仿真技术。在Gazebo 中搭建了仿真模型并进行仿真验证，结果表明，在Gazebo 下搭建的无人驾驶环境及车辆模型对无人驾驶车辆轨迹跟踪控制的研究具有较高的参考价值。