引言

在這個(gè)-SLAM建圖和導(dǎo)航仿真實(shí)例-項(xiàng)目中，主要分為三個(gè)部分，分別是

• （一）模型構(gòu)建
• （二）根據(jù)已知地圖進(jìn)行定位和導(dǎo)航
• （三）使用RTAB-MAP進(jìn)行建圖和導(dǎo)航

該項(xiàng)目的slam_bot已經(jīng)上傳我的Github。

由于之前的虛擬機(jī)性能限制，我在這個(gè)項(xiàng)目中使用了新的ubantu 16.04環(huán)境，虛擬機(jī)配置

• 內(nèi)存 8G
• CPU 四核
• 禁用硬件加速（重要：否則gazebo會(huì)打開失敗）

一、模型構(gòu)建

1、使用Solidworks建立slam_bot包

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

1. 定義底盤底面為base_link坐標(biāo)系的xoy平面，底面中點(diǎn)為坐標(biāo)原點(diǎn)，小車正方向?yàn)閤軸正方向建立右手系；
2. 各個(gè)車輪坐標(biāo)系原點(diǎn)為各個(gè)車輪內(nèi)側(cè)平面圓心，x軸與base_link坐標(biāo)系x軸對(duì)齊，y軸與車軸平齊（與base_link坐標(biāo)系y軸對(duì)齊或相向）建立右手系；
3. 車輪的旋轉(zhuǎn)軸分別為對(duì)應(yīng)的轉(zhuǎn)軸軸線。

值得注意的是，在下圖中有兩個(gè)坐標(biāo)系，分別是坐標(biāo)系1和coordinate0。在一開始，我沒有意識(shí)到使用coordinate0作為坐標(biāo)原點(diǎn)是一個(gè)多么錯(cuò)誤的選擇。在后來使用skid_steer_drive_controllergazebo插件時(shí)，模型一直在原地轉(zhuǎn)圈，直到我在rviz中查看tf轉(zhuǎn)換關(guān)系時(shí)，才發(fā)現(xiàn)是錯(cuò)誤的tf轉(zhuǎn)換，導(dǎo)致了插件的控制錯(cuò)誤。所以一定要嚴(yán)格遵循上述所說的坐標(biāo)系建立規(guī)則。

圖1-1 solidworks模型

使用solidworks的urdf_exporter插件導(dǎo)出urdf和模型文件。在圖1-2中base_link忘記選擇參考坐標(biāo)系了，若以上圖坐標(biāo)系為準(zhǔn)，則需要選擇 - 坐標(biāo)系1。

關(guān)于更多urdf_exporter安裝和使用可以參考我的博客-【從零開始的ROS四軸機(jī)械臂控制】（一）- 實(shí)際模型制作、Solidworks文件轉(zhuǎn)urdf與rviz仿真.。

圖1-2 urdf-exporter導(dǎo)出界面

若選擇無誤，就可以導(dǎo)出模型了。

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

2.更改slam_bot包

（1）urdf文件夾

導(dǎo)航到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，導(dǎo)入gazebo插件

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

在base_link節(jié)點(diǎn)之前添加base_footprint節(jié)點(diǎn)

  <!--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節(jié)點(diǎn)之后添加laser節(jié)點(diǎn)

  <!--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>

更改各個(gè)joint設(shè)置，如joint_wheel_FR。注意<axis>的設(shè)置，否則也會(huì)導(dǎo)致錯(cuò)誤的控制。

  <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設(shè)置為“continuous”，類似于轉(zhuǎn)動(dòng)關(guān)節(jié)，但對(duì)其旋轉(zhuǎn)沒有限制。它可以繞一個(gè)軸連續(xù)旋轉(zhuǎn)。關(guān)節(jié)會(huì)有自己的旋轉(zhuǎn)軸axis，一些特定的關(guān)節(jié)動(dòng)力學(xué)dynamics與關(guān)節(jié)的物理特性(如“摩擦”)相對(duì)應(yīng)，以及對(duì)該關(guān)節(jié)施加最大“努力”和“速度”的某些限制limits。這些限制對(duì)于物理機(jī)器人是有用的約束，并且可以幫助在仿真中創(chuàng)建更健壯的機(jī)器人模型。

點(diǎn)擊這里來更好地理解這些限制。

RGB-D點(diǎn)云是默認(rèn)朝上,需向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

添加如下插件

• 四輪機(jī)器人控制驅(qū)動(dòng)：skid_steer_drive_controller
• RGBD 深度攝像機(jī):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中儲(chǔ)存gazebo world。world是模型的集合，還可以定義此world特定的其他幾個(gè)物理屬性。

讓我們創(chuàng)建一個(gè)簡單的世界，其中沒有將在以后的gazebo中啟動(dòng)的對(duì)象或模型。

$ cd worlds
$ nano slam.world

將以下內(nèi)容添加到 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 環(huán)境的元素。在上面創(chuàng)建的簡單世界有以下元素

<sdf>: 基本元素，封裝了整個(gè)文件結(jié)構(gòu)和內(nèi)容。
<world>：世界元素定義了世界描述和與世界相關(guān)的幾個(gè)屬性。每個(gè)模型或?qū)傩钥梢杂懈嗟脑貋砻枋鏊＠?#xff0c;camera有一個(gè)pose元素，定義了它的位置和方向。
<include>：include元素和<uri>元素一起，提供了通往特定模型的路徑。在Gazebo中，有幾個(gè)模型是默認(rèn)包含的，可以在創(chuàng)建環(huán)境時(shí)包含它們。

（3）launch文件夾

創(chuàng)建一個(gè)新的啟動(dòng)文件，這將有助于加載URDF文件。

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

將以下內(nèi)容復(fù)制到上述文件中。

<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>

上面的代碼定義了一個(gè)參數(shù)robot_description，該參數(shù)用于設(shè)置單個(gè)命令，以使用xacro軟件包從xacro文件生成URDF。

借助robot_description生成的URDF文件，gazebo_ros包生成對(duì)應(yīng)模型。

接下來創(chuàng)建一個(gè)launch文件。ROS中的啟動(dòng)文件使我們可以同時(shí)執(zhí)行多個(gè)節(jié)點(diǎn)，這有助于避免在單獨(dú)的shell或終端中定義和啟動(dòng)多個(gè)節(jié)點(diǎn)的繁瑣工作。

$ nano slam.launch

將以下內(nèi)容添加到啟動(dòng)文件中。

<?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>元素定義某些參數(shù)。每個(gè)這樣的元素都會(huì)有一個(gè)name屬性和一個(gè)default值。
然后，在gazebo_ros 包中包含了empty_world.launch文件。empty_world文件包含了一組重要的定義，這些定義被我們創(chuàng)建的world所繼承。使用 world_name 參數(shù)和傳遞給該參數(shù)的value的.world文件的路徑，所以我們能夠在 Gazebo 中啟動(dòng)world。

3.運(yùn)行slam_bot

現(xiàn)在可以使用啟動(dòng)文件來啟動(dòng)Gazebo環(huán)境。

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

若gazebo長時(shí)間打不開，解決辦法如下

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

下載完成后將gazebo_models復(fù)制到~/.gazebo文件夾中，重命名為models。

若模型正常運(yùn)行，則如圖3-1所示。

圖3-1 模型在gazebo中運(yùn)行

（1）測試RGBD相機(jī)和激光雷達(dá)

這次，涼亭和RViz都應(yīng)該啟動(dòng)。加載后

選擇RViz窗口，然后在左側(cè)的Displays中：

• 選擇“ odom”作為fixed frame

點(diǎn)擊“Add”按鈕，然后

• 添加“ RobotModel”
• 添加“pointcloud2”并選擇在gazebo插件中定義的/camera/depth_registered/points主題
• 添加“ LaserScan”并選擇在gazebo插件中定義的hokuyo主題。

機(jī)器人模型應(yīng)在RViz中加載。

在涼亭中，單擊“插入”，然后從列表中添加機(jī)器人前面世界中的任何物品。

至此應(yīng)該能夠在Rviz中的“pointcloud2”查看器中看到該項(xiàng)目，并且也可以對(duì)該對(duì)象進(jìn)行激光掃描。

圖3-2 測試RGBD相機(jī)和激光雷達(dá)

圖3-2來自較舊版本的模型，所以看起來與之前模型會(huì)有所不同。

（2）測試四輪驅(qū)動(dòng)插件

在上述所有內(nèi)容仍在運(yùn)行時(shí)，打開一個(gè)新的終端窗口，然后輸入

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

上面的命令會(huì)將消息發(fā)布到cmd_vel，這是在驅(qū)動(dòng)器控制器插件中定義的主題。

圖3-3 測試四輪驅(qū)動(dòng)插件

可以看到模型正常移動(dòng)，gazebo因?yàn)殇浧淋浖紳⒘?#xff0c;不過正常運(yùn)行是不會(huì)這樣的。

模型應(yīng)當(dāng)沿x軸正方向移動(dòng)，如果出現(xiàn)運(yùn)動(dòng)不正常的現(xiàn)象，可以檢查

• skid_steer_drive_controller定義
• urdf中各個(gè)joint定義
• 檢查tf變換，使用rosrun tf view_frames以查看tf信息

圖3-4 slam_bot的tf樹示例

創(chuàng)建和查看tf-tree是確保所有鏈接順序正確的好方法。

也可以在RViz中繪制不同的框架并在那里進(jìn)行圖形上的檢查。

圖3-5 rviz中的tf框架

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。