引言

在這個-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節點

  <!--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>的設置，否則也會導致錯誤的控制。

  <jointname="joint_wheel_FR"type="continuous"><originxyz="0.0800000000000013 -0.0799999999999997 0.04"rpy="0 0 0" /><parentlink="base_link" /><childlink="link_wheel_FR" /><axisxyz="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 laserachieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m andstddev of 0.01m will put 99.7% of samples within 0.03m of the truereading. --><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所示。

圖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.1y: 0.0z: 0.0
angular:x: 0.0y: 0.0z: 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框架

總結

以上是生活随笔為你收集整理的【SLAM建图和导航仿真实例】（一）- 模型构建的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。