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

238
0
2021年1月12日 09时11分

引言

 

在这个-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. 车轮的旋转轴分别为对应的转轴轴线。

 

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

 

微信图片_20210107140015

图1-1 solidworks模型
 

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

 

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

 

微信图片_20210107140032

图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节点

 

  <!--base_footprint link-->
  <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节点

 

  <!--laser_link-->
  <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。注意<axis>的设置,否则也会导致错误的控制。

 

  <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

 

<?xml version="1.0"?>
<robot>

  <gazebo>
    <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
	<alwaysOn>true</alwaysOn>
        <updateRate>100.0</updateRate>
        <robotNamespace>/</robotNamespace>
        <leftFrontJoint>joint_wheel_FL</leftFrontJoint>
        <rightFrontJoint>joint_wheel_FR</rightFrontJoint>
        <leftRearJoint>joint_wheel_BL</leftRearJoint>
        <rightRearJoint>joint_wheel_BR</rightRearJoint>
        <wheelSeparation>0.16</wheelSeparation>
        <wheelDiameter>0.08</wheelDiameter>
        <robotBaseFrame>base_footprint</robotBaseFrame>
        <torque>20</torque>
        <commandTopic>cmd_vel</commandTopic>
    	<odometryTopic>odom</odometryTopic>
    	<odometryFrame>odom</odometryFrame>
    	<broadcastTF>1</broadcastTF>
    </plugin>
  </gazebo>

  <!--RGBD camera -->
  <gazebo reference="camera">
   <sensor type="depth" name="camera">
     <always_on>true</always_on>
     <visualize>false</visualize>
     <update_rate>15.0</update_rate>
     <camera name="front">
       <horizontal_fov>1.047197</horizontal_fov>
       <image>
         <!-- openni_kinect plugin works only with BGR8 -->
         <format>B8G8R8</format>
         <width>400</width>
         <height>300</height>
       </image>
       <clip>
         <near>0.01</near>
         <far>8</far>
       </clip>
     </camera>
     <plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
       <baseline>0.1</baseline>
       <alwaysOn>true</alwaysOn>
       <updateRate>15.0</updateRate>
       <cameraName>camera</cameraName>
       <imageTopicName>/camera/rgb/image_raw</imageTopicName>
       <cameraInfoTopicName>/camera/rgb/camera_info</cameraInfoTopicName>
       <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
       <depthImageCameraInfoTopicName>/camera/depth_registered/camera_info</depthImageCameraInfoTopicName>
       <pointCloudTopicName>/camera/depth_registered/points</pointCloudTopicName>
       <frameName>camera_depth_optical_frame</frameName>
       <pointCloudCutoff>0.35</pointCloudCutoff>
       <pointCloudCutoffMax>4.5</pointCloudCutoffMax>
       <CxPrime>0</CxPrime>
       <Cx>0</Cx>
       <Cy>0</Cy>
       <focalLength>0</focalLength>
       <hackBaseline>0</hackBaseline>
     </plugin>
   </sensor>
  </gazebo>

 <!-- hokuyo -->
  <gazebo reference="hokuyo">
    <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>/slam_bot/laser/scan</topicName>
        <frameName>hokuyo</frameName>
      </plugin>
    </sensor>
  </gazebo>
</robot>

 

(2)worlds文件夹

 

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

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

 

$ cd worlds
$ nano slam.world

 

将以下内容添加到 slam.world

 

<?xml version="1.0" ?>

<sdf version="1.4">

  <world name="default">

    <include>
      <uri>model://ground_plane</uri>
    </include>

    <!-- Light source -->
    <include>
      <uri>model://sun</uri>
    </include>

    <!-- World camera -->
    <gui fullscreen='0'>
      <camera name='world_camera'>
        <pose>4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190</pose>
        <view_controller>orbit</view_controller>
      </camera>
    </gui>

  </world>
</sdf>

 

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

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

 

(3)launch文件夹

 

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

 

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

 

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

 

<launch>

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

  <!-- Send fake joint values-->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="false"/>
  </node>

  <!-- Send robot states to tf -->
  <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

 

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

 

<?xml version="1.0" encoding="UTF-8"?>
<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>

  <!--spawn a robot in gazebo world-->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" 
     output="screen" args="-urdf -param robot_description -model slam_bot"/>

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

</launch>

 

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

 

首先,使用 <arg>元素定义某些参数。每个这样的元素都会有一个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所示。

 

微信图片_20210107140317

图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”查看器中看到该项目,并且也可以对该对象进行激光扫描。

 

微信图片_20210107140348

图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,这是在驱动器控制器插件中定义的主题。

 

微信图片_20210107140444

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

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

 

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

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

 

微信图片_20210107140510

 

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

 

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

 

微信图片_20210107140529

发表评论

后才能评论