Gazebo仿真——阿克曼（Ackermann）四轮小车模型

发表于 | 分类于

Github链接：https://github.com/chanchanchan97/ROS

1.仿真描述

本项目的目的是在Gazebo仿真环境下显示一个基于阿克曼转向（Ackermann steering）的自动驾驶小车模型，在之前xacro模型的基础上添加Gazebo标签等，使小车模型具有重力、惯性和摩擦力等物理状态。

2.Gazebo简介

Gazebo是一款3D动态模拟器，能够在复杂的室内和室外环境中准确有效地模拟机器人群。与游戏引擎提供高保真度的视觉模拟类似，Gazebo提供高保真度的物理模拟，其提供一整套传感器模型，以及对用户和程序非常友好的交互方式。

2.1Gazebo的典型用途

（1）测试机器人算法；
（2）设计机器人；
（3）用现实场景进行回归测试。

2.2Gazebo的主要特点

（1）包含多个物理引擎；
（2）包含丰富的机器人模型和环境库；
（3）包含各种各样的传感器；
（4）程序设计方便和具有简单的图形界面。

3.ros_control

如上图所示，ros_control为Gazebo仿真提供了一系列控制器接口、传动装置接口、硬件接口、控制器工具箱等。通过向URDF模型文件中添加Gazebo插件，从而将Gazebo仿真模型与ros_control建立起联系。controller可以实现对URDF模型中每个joint的控制，并且提供了PID控制器，Controller Manager则提供了一种通用接口，负责管理不同的controller。

4.配置物理仿真模型

4.1为link添加惯性参数和碰撞属性

1
2
3
4
5
6
7
8
9
10
11
12
<collision>
	<origin rpy="0 0 0" xyz="0 0 0"/>
	<geometry>
		<cylinder length=".02" radius="0.025"/>
	</geometry>
</collision>

<inertial>
	<origin xyz="0 0 0.055"/>
	<mass value="1.0" />
	<inertia ixx="${1*(0.16*0.16+0.02*0.02)/12}" ixy="0.0" ixz="0.0" iyy="${1*(0.25*0.25+0.02*0.02)/12}" iyz="0.0" izz="${1*(0.16*0.16+0.25*0.25)/12}"/>
</inertial>

如果模型仅需要在Rviz中显示，则上述标签和标签可以不添加。在为模型添加了collision碰撞属性后，该link的collision属性与link的visual属性在形状大小和位置角度上都应当保证重合，可在Gazebo菜单栏中添加collision属性的显示，即如下图所示。

若如下图所示，某个link的collision属性与link的visual属性没有重合，则小车无法正常运动。

同样的在为模型添加了inertial惯性参数后，每个link都会显示一个带有绿色轴的紫色框，每个框的中心应当与其link的指定重心对齐，可在Gazebo菜单栏中添加inertial属性的显示，即如下图所示。

关于惯性矩阵的公式推导和计算可自行上网搜寻，以下为资料链接的分享：
https://wenku.baidu.com/view/8444fe93aa00b52acfc7cae5.html

4.2为link添加Gazebo标签

4.2.1颜色

1
2
3
<gazebo reference="base_link">  
	<material>Gazebo/Blue</material>  
</gazebo>

该标签的作用是定义某个link在Gazebo中显示的颜色。虽然在标签中已经对link的颜色进行了定义，但只是定义了模型在Rviz中显示的颜色，而Gazebo所能够提供的颜色标签可参考：
http://wiki.ros.org/simulator_gazebo/Tutorials/ListOfMaterials

4.2.2重力

1
2
3
<gazebo reference="base_footprint">
       <turnGravityOff>false</turnGravityOff>
   </gazebo>

base_footprint这个link是作为小车本体在地面上的映射，本身并没有任何物理意义，因此通过标签将该link的重力属性关闭。

4.3为joint添加传动装置

Gazebo中的仿真都是基于物理引擎的，因此要想让一个模型运动，从本质上讲，必须要有力施加在模型上，就实际而言，我们必须在模型上加上执行器（通常情况下就是电机了），让模型运动起来。

标签主要的针对对象是joint（因为一般两个link连接处的地方如果是非固定的，那么一定会存在一个执行装置来改变两个link的相对位置），标签的作用就是给某个joint与某类执行器相结合，有了执行器的作用，Gazebo就可以在物理层面上对模型进行驱动了。
这里以前轮转向机构为例进行说明，具体代码如下。

1
2
3
4
5
6
7
8
9
10
11
<!-- Transmission is important to link the joints and the controller -->
<transmission name="right_bridge_joint_trans">
	<type>transmission_interface/SimpleTransmission</type>
	<joint name="right_bridge_to_bridge" >
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
	</joint>
	<actuator name="right_bridge_joint_motor">		<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
	<mechanicalReduction>1</mechanicalReduction>
	<motorTorqueConstant>100</motorTorqueConstant>
	</actuator>
</transmission>

（1）标签唯一指定了一个传动的标签，name可以自己定义，也可以与joint名相同。
（2）标签指定了传动的类型，选择transmission_interface/SimpleTransmission即可。
（3）标签（可定义一个或多个）指定这个传动所依赖的关节，拥有如下标签。
◆（在joint下，可定义一个或多个）指定支持的硬件接口空间。用于结合控制器使用硬件接口来向硬件接口发送和接受指令，请注意：
●当在RobotHW中加载此transmission时，此标签的值应为hardware_interface / XXX。
●在Gazebo中加载此transmission时，此标记的值应为XXX。
（4）标签（定义一个或多个）指定了与joint传动连接的执行器，名字可以随意定义，拥有如下标签、及。
●（可选）定义了电机／关节减速比。
●（可选，只有Indigo及以前版本的在这里指定，目前版本已经移到joint标签下）定义了支持的硬件接口。
●（可选）定义了电机的转矩参数。

4.4为部分link添加阻尼系数和摩擦系数（可选）

这里以车轮为例，具体代码如下。

1
2
3
4
5
6
7
8
<gazebo reference="right_back_wheel">
	<mu1>100000000</mu1>
	<mu2>100000000</mu2>
	<kp>100000000</kp>
	<kd>1</kd>
	<minDepth>0.01</minDepth>
	<maxVel>100</maxVel>  
</gazebo>

（1）和表示沿接触表面的两个不同接触方向的摩擦系数μ，其中表示库仑摩擦系数，它的范围必须在0到无限大之间，0表示无摩擦接触，而无限大表示永不打滑的接触。请注意，无摩擦接触比具有摩擦的接触要花费更少的时间，并且无限摩擦的接触要比具有有限摩擦的接触损耗更大。这个值必须始终被设置。而如果未设置，则在两个摩擦方向上都使用。 如果设置，则将用作摩擦方向1，将用作摩擦方向2。
（2）和表示由Open Dynamics Engine (ODE)定义的刚体接触的接触刚度k_p和阻尼k_d（ODE使用erp和cfm，但是erp/cfm与刚度k_p/阻尼k_d之间存在映射关系）。
（3）表示施加接触校正脉冲之前的最小允许深度。
（4）表示最大接触修正速度截断项。
●附Gazebo官网相关链接：
http://gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros

4.5添加ros_control插件

1
2
3
4
5
6
7
8
<gazebo>
	<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
		<robotNamespace></robotNamespace>
		<robotParam>robot_description</robotParam>
		<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
		<legacyModeNS>true</legacyModeNS>
	</plugin>
</gazebo>

gazebo_ros_control是Gazebo的一个插件用来根据设定装载合适的硬件接口和控制器。这个实现非常简单，由于Gazebo的插件系统具有很强的扩展性， 使得一些高级玩家可以在ros_control和Gazebo之间创建自己的机器人硬件接口。
这里我们没有为标签添加属性reference，这样它就是对整个机器人的描述。 gazebo_ros_control的标签还可以通过如下的子标签指定一些参数：
（1）用来定义插件其对象的ROS命名空间，默认是URDF或者SDF对应的机器人名称。
（2）用来定义控制器的更新周期，单位为秒，默认使用Gazebo的周期。
（3）用来定义ROS参数服务器上的机器人描述(URDF)路径，默认是”/robot_description”。
（4）用来定义机器人仿真接口所使用的插件库名称，默认是”DefaultRobotHWSim”。
（5）用来兼容之前ROS版本的配置。

5.配置ros_control控制器

在config文件夹目录下创建smartcar_joint.yaml，并输入如下代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
joint_state_controller:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50  

rear_right_velocity_controller:
  type: "velocity_controllers/JointVelocityController"
  joint: right_back_wheel_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
rear_left_velocity_controller:
  type: "velocity_controllers/JointVelocityController"
  joint: left_back_wheel_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
right_bridge_position_controller:
  type: "effort_controllers/JointPositionController"
  joint: right_bridge_to_bridge
  pid: {p: 40.0, i: 0.0, d: 1.0}
left_bridge_position_controller:
  type: "effort_controllers/JointPositionController"
  joint: left_bridge_to_bridge
  pid: {p: 40.0, i: 0.0, d: 1.0}

用.yaml文件创建一个配置文件用来配置控制器类型和保存各个关节的PID参数，这些参数将通过launch文件加载到参数服务器上。

6.在Gazebo中添加传感器插件

6.1摄像头

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
<!-- camera -->
 	<gazebo reference="camera_link">
	<sensor type="camera" name="camera1">
		<update_rate>30.0</update_rate>
		<camera name="head">
			<horizontal_fov>1.3962634</horizontal_fov>
			<image>
				<width>800</width>
				<height>800</height>
				<format>R8G8B8</format>
			</image>
			<clip>
				<near>0.02</near>
				<far>300</far>
			</clip>
			<noise>
				<type>gaussian</type>
				<!-- Noise is sampled independently per pixel on each frame.
					That pixel's noise value is added to each of its color
					channels, which at that point lie in the range [0,1]. -->
				<mean>0.0</mean>
				<stddev>0.007</stddev>
			</noise>
		</camera>
		<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
			<alwaysOn>true</alwaysOn>
			<updateRate>0.0</updateRate>
			<cameraName>camera</cameraName>
			<imageTopicName>image_raw</imageTopicName>
			<cameraInfoTopicName>camera_info</cameraInfoTopicName>
			<frameName>camera_link</frameName>
			<hackBaseline>0.07</hackBaseline>
			<distortionK1>0.0</distortionK1>
			<distortionK2>0.0</distortionK2>
			<distortionK3>0.0</distortionK3>
			<distortionT1>0.0</distortionT1>
			<distortionT2>0.0</distortionT2>
		</plugin>
	</sensor>
</gazebo>

说明：

1
<gazebo reference="camera_link">

插件是用来描述link、joint的，其本身是一种虚无的属性描述，因此需要关联相应的实体。我们通过来定义关联的link或者joint,这里关联的是camera_link。
1
<sensor type="camera" name="camera1">

声明插件的类型，并为该插件取一个唯一的名称。
1
<update_rate>30.0</update_rate>

设置摄像头数据更新的最大频率。
1
<horizontal_fov>1.3962634</horizontal_fov>

horizontal_fov是指horizontal field of view（水平方向上的视场），此处设置视场大小。
1
2
3
4
5
<image>
	<width>800</width>
	<height>800</height>
	<format>R8G8B8</format>
</image>

设置图像的分辨率和色彩格式。
1
2
3
4
<clip>
	<near>0.02</near>
	<far>300</far>
</clip>

设置摄像头可视的最短距离和最远距离。
1
2
3
4
5
6
7
8
<noise>
	<type>gaussian</type>
	<!-- Noise is sampled independently per pixel on each frame.
	That pixel's noise value is added to each of its color
	channels, which at that point lie in the range [0,1]. -->
	<mean>0.0</mean>
	<stddev>0.007</stddev>
</noise>

此处为图像添加噪声，噪声类型为高斯噪声，设置高斯噪声的均值和标准差。噪声是在每个帧的每个像素上独立采样的，该像素的噪声值会添加到其每个颜色通道中，范围在[0,1]内。
1
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">

关联摄像头插件，该插件已经在Gazebo中实现，所以直接关联即可。
1
<cameraName>camera</cameraName>

设置摄像头消息的命名空间。
1
<imageTopicName>image_raw</imageTopicName>

设置发布的摄像头图像话题名。
1
<cameraInfoTopicName>camera_info</cameraInfoTopicName>

设置发布的摄像头信息话题名。
1
<frameName>camera_link</frameName>

设置摄像头数据所在的参考系。
1
<hackBaseline>0.07</hackBaseline>

此处在ros.wiki上的解释是Hack for right stereo camera，默认为0。
1
2
3
4
5
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>

设置摄像头图像的畸变参数，默认为0。

6.2激光雷达

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
<gazebo reference="laser_link">
	<sensor type="ray" name="head_hokuyo_sensor">
		<pose>0 0 0 0 0 0</pose>
		<visualize>false</visualize>
		<update_rate>40</update_rate>
		<ray>
			<scan>
				<horizontal>
					<samples>720</samples>
					<resolution>1</resolution>
					<min_angle>-1.570796</min_angle>
					<max_angle>1.570796</max_angle>
				</horizontal>
			</scan>
			<range>
				<min>0.10</min>
				<max>30.0</max>
				<resolution>0.01</resolution>
			</range>
			<noise>
				<type>gaussian</type>
				<!-- Noise parameters based on published spec for Hokuyo laser
					achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
					stddev of 0.01m will put 99.7% of samples within 0.03m of the true
					reading. -->
				<mean>0.0</mean>
				<stddev>0.01</stddev>
			</noise>
		</ray>
		<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
			<topicName>laser/scan</topicName>
			<frameName>laser_link</frameName>
		</plugin>
	</sensor>
</gazebo>

激光雷达的插件与摄像头相似，不再做详细说明。

6.3IMU

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
<!-- IMU sensor -->
   <gazebo reference="imu_link">
       <gravity>true</gravity>
       <sensor name="imu_sensor" type="imu">
           <always_on>true</always_on>
           <update_rate>100</update_rate>
           <visualize>true</visualize>
           <topic>__default_topic__</topic>
           <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
               <topicName>imu</topicName>
               <bodyName>imu_link</bodyName>
               <updateRateHZ>100.0</updateRateHZ>
               <gaussianNoise>0.0</gaussianNoise>
               <xyzOffset>0 0 0</xyzOffset>
               <rpyOffset>0 0 0</rpyOffset>
               <frameName>imu_link</frameName>
           </plugin>
           <pose>0 0 0 0 0 0</pose>
       </sensor>
   </gazebo>

IMU的插件与摄像头相似，不再做详细说明。

6.4检查传感器插件

打开一个新的终端，输入如下指令。

1
$ rostopic list

终端界面将会打印出目前所有的topic话题。

其中/camera、/imu和/laser分别对应摄像头、IMU和激光雷达的消息话题。

6.5传感器消息可视化

6.5.1摄像头图像的显示

输入如下指令，打开Rviz。

1
$ rosrun rviz rviz

在Rviz的左菜单栏中添加Camera的可视化插件，并在Topic中选择摄像头图像的消息话题/camera/image_raw。

注意：在虚拟机中运行摄像头仿真图像，会出现如上图所示的图像显示问题，建议使用双系统。

6.5.2激光雷达点云的显示

在Gazebo仿真环境中添加一些已有的模型，将模型放在小车周围合适的范围内。

打开一个新的终端，输入如下指令，打开Rviz。

1
$ rosrun rviz rviz

在Rviz工具的左菜单栏中添加如下所示LaserScan的可视化插件。

同时在LaserScan中的Topic选择激光雷达的消息话题/laser/scan，即如下图所示，Rviz能够显示出小车周围障碍物的点云信息。

6.5.3IMU姿态的显示

在Gazebo菜单栏的Window中选择Topic Visualization，跳出如下界面。

选择gazebo.msgs.IMU，跳出如下界面。如下图所示，IMU消息可分成三个部分，分别为以四元数表示的姿态信息、角速度和线性加速度在xyz三轴上的分量。

6.6搭建Gazebo仿真环境

在Gazebo上方的菜单栏中选择Edit-Building Editor，出现如下界面。

在左边的工具栏中选择Walls，将小车仿真环境的轮廓画出来，然后再选择Features添加门、窗和楼梯等。双击门窗会打开一个Inspector窗口，可以修改位置和尺寸。另外在工具栏中还可以选择修饰墙的颜色和纹理等细节。

完成仿真环境的绘制后，在Gazebo上方的菜单栏中选择File-Save as保存文件。