【SLAM建图和导航仿真实例】（一）- 模型构建

引言

在这个-SLAM建图和导航仿真实例-项目中，主要分为三个部分，分别是

• （一）模型构建
• （二）根据已知地图进行定位和导航
• （三）使用RTAB-MAP进行建图和导航

该项目的slam_bot已经上传我的Github。

由于之前的虚拟机性能限制，我在这个项目中使用了新的ubantu 16.04环境，虚拟机配置

• 内存 8G
• CPU 四核
• 禁用硬件加速（重要：否则gazebo会打开失败）

一、模型构建

1、使用Solidworks建立slam_bot包

使用Solidworks建立模型如下图，其中有很多部分值得注意。

1. 定义底盘底面为base_link坐标系的xoy平面，底面中点为坐标原点，小车正方向为x轴正方向建立右手系；
2. 各个车轮坐标系原点为各个车轮内侧平面圆心，x轴与base_link坐标系x轴对齐，y轴与车轴平齐（与base_link坐标系y轴对齐或相向）建立右手系；
3. 车轮的旋转轴分别为对应的转轴轴线。

值得注意的是，在下图中有两个坐标系，分别是坐标系1和coordinate0。在一开始，我没有意识到使用coordinate0作为坐标原点是一个多么错误的选择。在后来使用skid_steer_drive_controllergazebo插件时，模型一直在原地转圈，直到我在rviz中查看tf转换关系时，才发现是错误的tf转换，导致了插件的控制错误。所以一定要严格遵循上述所说的坐标系建立规则。

图1-1 solidworks模型

使用solidworks的urdf_exporter插件导出urdf和模型文件。在图1-2中base_link忘记选择参考坐标系了，若以上图坐标系为准，则需要选择 - 坐标系1。

关于更多urdf_exporter安装和使用可以参考我的博客-【从零开始的ROS四轴机械臂控制】（一）- 实际模型制作、Solidworks文件转urdf与rviz仿真.。

图1-2 urdf-exporter导出界面

若选择无误，就可以导出模型了。

我在Github中提供了最初始的slam_bot package文件。

2.更改slam_bot包

（1）urdf文件夹

导航到urdf文件夹

$ ~/catkin_ws/src/slam_bot/urdf

将slam_bot.urdf 更名成slam_bot.xacro，并新建slam_bot.gazebo.xacro文件。

更改slam_bot.xacro

添加xacro namespace。

<robot  name="slam_bot" xmlns:xacro="http://www.ros.org/wiki/xacro">

添加slam_bot.gazebo.xacro，导入gazebo插件

  <xacro:include filename="$(find slam_bot)/urdf/slam_bot.gazebo.xacro" />

在base_link节点之前添加base_footprint节点

  
  <link name="base_footprint">  link>

  <joint name="base_link_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <parent link="base_footprint"/>
    <child link="base_link" />
  joint>

在camera节点之后添加laser节点

  
  <link name="hokuyo">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://slam_bot/meshes/hokuyo.dae"/>
      geometry>
    collision>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="package://slam_bot/meshes/hokuyo.dae"/>
      geometry>
      <material name="red">
        <color rgba="1.0 0 0 1.0"/>
      material>
    visual>
    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 1.57 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    inertial>
  link>

  <joint name="hokuyo_joint" type="fixed">
    <axis xyz="0 0 0" />
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="hokuyo"/>
  joint>

更改各个joint设置，如joint_wheel_FR。注意的设置，否则也会导致错误的控制。

  <joint
    name="joint_wheel_FR"
    type="continuous">
    <origin
      xyz="0.0800000000000013 -0.0799999999999997 0.04"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="link_wheel_FR" />
    <axis
      xyz="0 1 0" />
  joint>

joint type设置为“continuous”，类似于转动关节，但对其旋转没有限制。它可以绕一个轴连续旋转。关节会有自己的旋转轴axis，一些特定的关节动力学dynamics与关节的物理特性(如“摩擦”)相对应，以及对该关节施加最大“努力”和“速度”的某些限制limits。这些限制对于物理机器人是有用的约束，并且可以帮助在仿真中创建更健壮的机器人模型。

点击这里来更好地理解这些限制。

RGB-D点云是默认朝上,需向slam_bot.xacro中添加如下的link和joint

  <link name="camera_depth_optical_frame" />

  <joint name="camera_depth_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0" />
    <parent link="camera"/>
    <child link="camera_depth_optical_frame" />
  joint>

更改slam_bot.gazebo.xacro

添加如下插件

• 四轮机器人控制驱动：skid_steer_drive_controller
• RGBD 深度摄像机:libgazebo_ros_openni_kinect
• 激光传感器: head_hokuyo_sensor

<robot>

  <gazebo>
    <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
	<alwaysOn>truealwaysOn>
        <updateRate>100.0updateRate>
        <robotNamespace>/robotNamespace>
        <leftFrontJoint>joint_wheel_FLleftFrontJoint>
        <rightFrontJoint>joint_wheel_FRrightFrontJoint>
        <leftRearJoint>joint_wheel_BLleftRearJoint>
        <rightRearJoint>joint_wheel_BRrightRearJoint>
        <wheelSeparation>0.16wheelSeparation>
        <wheelDiameter>0.08wheelDiameter>
        <robotBaseFrame>base_footprintrobotBaseFrame>
        <torque>20torque>
        <commandTopic>cmd_velcommandTopic>
    	<odometryTopic>odomodometryTopic>
    	<odometryFrame>odomodometryFrame>
    	<broadcastTF>1broadcastTF>
    plugin>
  gazebo>

  
  <gazebo reference="camera">
   <sensor type="depth" name="camera">
     <always_on>truealways_on>
     <visualize>falsevisualize>
     <update_rate>15.0update_rate>
     <camera name="front">
       <horizontal_fov>1.047197horizontal_fov>
       <image>
         
         <format>B8G8R8format>
         <width>400width>
         <height>300height>
       image>
       <clip>
         <near>0.01near>
         <far>8far>
       clip>
     camera>
     <plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
       <baseline>0.1baseline>
       <alwaysOn>truealwaysOn>
       <updateRate>15.0updateRate>
       <cameraName>cameracameraName>
       <imageTopicName>/camera/rgb/image_rawimageTopicName>
       <cameraInfoTopicName>/camera/rgb/camera_infocameraInfoTopicName>
       <depthImageTopicName>/camera/depth/image_rawdepthImageTopicName>
       <depthImageCameraInfoTopicName>/camera/depth_registered/camera_infodepthImageCameraInfoTopicName>
       <pointCloudTopicName>/camera/depth_registered/pointspointCloudTopicName>
       <frameName>camera_depth_optical_frameframeName>
       <pointCloudCutoff>0.35pointCloudCutoff>
       <pointCloudCutoffMax>4.5pointCloudCutoffMax>
       <CxPrime>0CxPrime>
       <Cx>0Cx>
       <Cy>0Cy>
       <focalLength>0focalLength>
       <hackBaseline>0hackBaseline>
     plugin>
   sensor>
  gazebo>

 
  <gazebo reference="hokuyo">
    <sensor type="ray" name="head_hokuyo_sensor">
      <pose>0 0 0 0 0 0pose>
      <visualize>falsevisualize>
      <update_rate>40update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720samples>
            <resolution>1resolution>
            <min_angle>-1.570796min_angle>
            <max_angle>1.570796max_angle>
          horizontal>
        scan>
        <range>
          <min>0.10min>
          <max>30.0max>
          <resolution>0.01resolution>
        range>
        <noise>
          <type>gaussiantype>
          
          <mean>0.0mean>
          <stddev>0.01stddev>
        noise>
      ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
        <topicName>/slam_bot/laser/scantopicName>
        <frameName>hokuyoframeName>
      plugin>
    sensor>
  gazebo>
robot>

（2）worlds文件夹

在worlds中储存gazebo world。world是模型的集合，还可以定义此world特定的其他几个物理属性。

让我们创建一个简单的世界，其中没有将在以后的gazebo中启动的对象或模型。

$ cd worlds
$ nano slam.world

将以下内容添加到 slam.world

<sdf version="1.4">

  <world name="default">

    <include>
      <uri>model://ground_planeuri>
    include>

    
    <include>
      <uri>model://sunuri>
    include>

    
    <gui fullscreen='0'>
      <camera name='world_camera'>
        <pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190pose>
        <view_controller>orbitview_controller>
      camera>
    gui>

  world>
sdf>

.world 文件使用 XML 文件格式来描述所有被定义为 Gazeboeboard 环境的元素。在上面创建的简单世界有以下元素

: 基本元素，封装了整个文件结构和内容。
：世界元素定义了世界描述和与世界相关的几个属性。每个模型或属性可以有更多的元素来描述它。例如，camera有一个pose元素，定义了它的位置和方向。
：include元素和元素一起，提供了通往特定模型的路径。在Gazebo中，有几个模型是默认包含的，可以在创建环境时包含它们。

（3）launch文件夹

创建一个新的启动文件，这将有助于加载URDF文件。

$ cd ~/catkin_ws/src/slam/launch/
$ nano robot_description.launch

将以下内容复制到上述文件中。

<launch>

  
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find slam_bot)/urdf/slam_bot.xacro'" />

  
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="false"/>
  node>

  
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen"/>

launch>

上面的代码定义了一个参数robot_description，该参数用于设置单个命令，以使用xacro软件包从xacro文件生成URDF。

借助robot_description生成的URDF文件，gazebo_ros包生成对应模型。

接下来创建一个launch文件。ROS中的启动文件使我们可以同时执行多个节点，这有助于避免在单独的shell或终端中定义和启动多个节点的繁琐工作。

$ nano slam.launch

将以下内容添加到启动文件中。

<launch>
  <include file="$(find slam_bot)/launch/robot_description.xml"/>

  <arg name="world" default="empty"/> 
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find slam_bot)/worlds/kitchen_dining.world"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="gui" value="$(arg gui)"/>
    <arg name="headless" value="$(arg headless)"/>
    <arg name="debug" value="$(arg debug)"/>
  include>

  
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" 
     output="screen" args="-urdf -param robot_description -model slam_bot"/>

  
  <node name="rviz" pkg="rviz" type="rviz" respawn="false"/>

launch>

和.world文件一样，.launch文件也是基于XML的。

首先，使用 元素定义某些参数。每个这样的元素都会有一个name属性和一个default值。
然后，在gazebo_ros 包中包含了empty_world.launch文件。empty_world文件包含了一组重要的定义，这些定义被我们创建的world所继承。使用 world_name 参数和传递给该参数的value的.world文件的路径，所以我们能够在 Gazebo 中启动world。

3.运行slam_bot

现在可以使用启动文件来启动Gazebo环境。

$ cd ~/catkin_ws/
$ catkin_make
$ source devel/setup.bash
$ roslaunch slam slam.launch

若gazebo长时间打不开，解决办法如下

cd ~/
hg clone https://bitbucket.org/osrf/gazebo_models

下载完成后将gazebo_models复制到~/.gazebo文件夹中，重命名为models。

若模型正常运行，则如图3-1所示。

图3-1 模型在gazebo中运行

（1）测试RGBD相机和激光雷达

这次，凉亭和RViz都应该启动。加载后

选择RViz窗口，然后在左侧的Displays中：

• 选择“ odom”作为fixed frame

点击“Add”按钮，然后

• 添加“ RobotModel”
• 添加“pointcloud2”并选择在gazebo插件中定义的/camera/depth_registered/points主题
• 添加“ LaserScan”并选择在gazebo插件中定义的hokuyo主题。

机器人模型应在RViz中加载。

在凉亭中，单击“插入”，然后从列表中添加机器人前面世界中的任何物品。

至此应该能够在Rviz中的“pointcloud2”查看器中看到该项目，并且也可以对该对象进行激光扫描。

图3-2 测试RGBD相机和激光雷达

图3-2来自较旧版本的模型，所以看起来与之前模型会有所不同。

（2）测试四轮驱动插件

在上述所有内容仍在运行时，打开一个新的终端窗口，然后输入

$ rostopic pub /cmd_vel geometry_msgs/Twist  "linear:
  x: 0.1
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" 

上面的命令会将消息发布到cmd_vel，这是在驱动器控制器插件中定义的主题。

图3-3 测试四轮驱动插件

可以看到模型正常移动，gazebo因为录屏软件奔溃了，不过正常运行是不会这样的。

模型应当沿x轴正方向移动，如果出现运动不正常的现象，可以检查

• skid_steer_drive_controller定义
• urdf中各个joint定义
• 检查tf变换，使用rosrun tf view_frames以查看tf信息

图3-4 slam_bot的tf树示例

创建和查看tf-tree是确保所有链接顺序正确的好方法。

也可以在RViz中绘制不同的框架并在那里进行图形上的检查。

图3-5 rviz中的tf框架