當前位置：首頁 > 人文社科 > 生活经验 >内容正文

生活经验

【SLAM建图和导航仿真实例】（三）- 使用RTAB-MAP进行SLAM建图和导航

發布時間：2023/11/27 生活经验 71 豆豆

生活随笔收集整理的這篇文章主要介紹了【SLAM建图和导航仿真实例】（三）- 使用RTAB-MAP进行SLAM建图和导航小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

引言

在這個-SLAM建圖和導航仿真實例-項目中，主要分為三個部分，分別是

（一）模型構建
（二）根據已知地圖進行定位和導航
（三）使用RTAB-MAP進行建圖和導航

該項目的slam_bot已經上傳我的Github。

這是第三部分，完成效果如下

圖1 建圖和導航

三、使用RTAB-Map進行建圖和導航

1. rtabmap_ros介紹

之前在GraphSLAM博客中我提到了RTAB-Map。

RTAB-Map是具有實時約束的RGB-D SLAM方法，它是一種基于增量基于外觀的閉環檢測器的RGB-D，基于立體聲和激光雷達圖的SLAM方法。閉環檢測器使用詞袋方法來確定新圖像來自先前位置或新位置的可能性。當接受循環閉合假設時，新約束將添加到地圖的圖形中，然后圖形優化器將地圖中的錯誤最小化。使用內存管理方法來限制用于閉環檢測和圖形優化的位置數量，以便始終遵守對大型環境的實時約束。

代碼庫：

https://github.com/introlab/rtabmap_ros.git
http://introlab.github.io/rtabmap

節點：

所有sensor_msgs/Image話題使用image_transport.

rtabmap

這是核心節點，是RTAB-Map核心庫的封裝，這是在檢測到循環閉合時增量構建和優化地圖的圖形。
節點的在線輸出是具有地圖上最新添加數據的本地圖。
默認RTAB-Map 數據庫的位置是 “~/.ros/rtabmap.db”，工作空間也設置為 “~/.ros”
通過訂閱cloud_map話題獲取3D點云圖, grid_map or proj_map話題獲取2D網格圖

rtabmapviz

RTAB-Map的可視化接口，是 RTAB-Map GUI圖形庫的封裝，類似rviz但有針對RTAB-Map的可選項

2.rtabmap_ros安裝

安裝kinetic版本：

$ sudo apt-get install ros-kinetic-rtabmap-ros

必要的依賴安裝 (Qt, PCL, VTK, OpenCV, …)：

$ sudo apt-get install ros-kinetic-rtabmap ros-kinetic-rtabmap-ros
$ sudo apt-get remove ros-kinetic-rtabmap ros-kinetic-rtabmap-ros

下載安裝RTAB-Map

源碼安裝rtabmap

$ cd ~
$ git clone https://github.com/introlab/rtabmap.git rtabmap
$ cd rtabmap/build
$ cmake -DCMAKE_INSTALL_PREFIX=~/catkin_ws/devel ..
$ make -j4
$ make install

源碼安裝rtabmap_ros

$ cd ~/catkin_ws
$ git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
$ catkin_make -j1

內存太小的話，使用-j1 ，內存大可以去掉。

3.創建啟動文件

根據ROS wiki的建議，我們可以用類似以下的方式搭建mapping的啟動文件。

圖3-1 Kinect + Odometry + 2D laser

<?xml version="1.0" encoding="UTF-8"?><launch><!-- Arguments for launch file with defaults provided --><arg name="database_path"     default="rtabmap.db"/><arg name="rgb_topic"   default="/camera/rgb/image_raw"/><arg name="depth_topic" default="/camera/depth/image_raw"/><arg name="camera_info_topic" default="/camera/rgb/camera_info"/>  <!-- Mapping Node --><group ns="rtabmap"><node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start"><!-- Basic RTAB-Map Parameters --><param name="database_path"       type="string" value="$(arg database_path)"/><param name="frame_id"            type="string" value="base_footprint"/><param name="odom_frame_id"       type="string" value="/odom"/><param name="subscribe_depth"     type="bool"   value="true"/><param name="subscribe_scan"      type="bool"   value="true"/><!-- RTAB-Map Inputs --><remap from="scan"            to="/slam_bot/laser/scan"/><remap from="rgb/image"       to="$(arg rgb_topic)"/><remap from="depth/image"     to="$(arg depth_topic)"/><remap from="rgb/camera_info" to="$(arg camera_info_topic)"/><!-- RTAB-Map Output --><remap from="grid_map" to="/map"/><!-- Rate (Hz) at which new nodes are added to map --><param name="Rtabmap/DetectionRate" type="string" value="1"/> <!-- 2D SLAM --><param name="Reg/Force3DoF" type="string" value="true"/>  <!-- Loop Closure Constraint --><!-- 0=Visual, 1=ICP (1 requires scan)--><param name="Reg/Strategy" type="string" value="0"/>  <!-- Loop Closure Detection --><!-- 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE--><param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- Maximum visual words per image (bag-of-words) --><param name="Kp/MaxFeatures" type="string" value="400"/>  <!-- Used to extract more or less SURF features --><param name="SURF/HessianThreshold" type="string" value="100"/><!-- Minimum visual inliers to accept loop closure --><param name="Vis/MinInliers" type="string" value="15"/> <!-- Set to false to avoid saving data when robot is not moving --><param name="Mem/NotLinkedNodesKept" type="string" value="false"/></node> </group>
</launch>

讓我們分解一下這個啟動文件：

      <param name="database_path"       type="string" value="$(arg database_path)"/>

參數---delete_db_on_start將使rtabmap在啟動時刪除數據庫（默認位于~/.ros/rtabmap.db）。如果想讓機器人繼續從之前的映射會話中進行映射，應該刪除 --delete_db_on_start。

      <param name="database_path"       type="string" value="$(arg database_path)"/><param name="frame_id"            type="string" value="base_footprint"/><param name="odom_frame_id"       type="string" value="/odom"/>

設置database_path,fixed frame和odom。

      <param name="subscribe_depth"     type="bool"   value="true"/><param name="subscribe_scan"      type="bool"   value="true"/>

默認情況下， subscribe_depth為true。但是，在這個設置中，我們將使用RGB-D圖像輸入，所以將subscribe_depth設置為false，將subscribe_rgbd設置為true。因為我們有一個2D lidar，所以將 subscribe_scan 設置為 true。如果我們有一個3D lidar發布sensor_msgs/PointCloud2消息，則將sensemble_scan_cloud設置為true，并重映射相應的scan_cloud主題而不是scan。

當 subscribe_rgbd=true時，應設置 rgbd_image 輸入主題。
當 subscribe_scan=true時，需要設置掃描輸入主題。

      <remap from="scan"            to="/slam_bot/laser/scan"/><remap from="rgb/image"       to="$(arg rgb_topic)"/><remap from="depth/image"     to="$(arg depth_topic)"/><remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>

設置所需的輸入主題。若話題沒有/，則意味著在其命名空間中訂閱了相應話題，例如“rgbd_image” 訂閱/rtabmap/rgbd_image。

      <!-- 2D SLAM --><param name="Reg/Force3DoF" type="string" value="true"/>  <!-- Loop Closure Constraint --><!-- 0=Visual, 1=ICP (1 requires scan)--><param name="Reg/Strategy" type="string" value="0"/>

Reg/Force3DoF：強制3DoF配準，不會估計roll、pitch和z。
Reg/Strategy：使用ICP來細化使用激光掃描發現的ICP的全局閉環。

以下是mapping.launch中沒有提到的參數的簡要概述。

<!-- RTAB-Map's parameters -->
<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
<param name="RGBD/ProximityBySpace"     type="string" value="true"/>
<param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
<param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="Grid/FromDepth"            type="string" value="false"/> <!-- ICP parameters -->
<param name="Icp/VoxelSize"                 type="string" value="0.05"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>

RGBD/NeighborLinkRefining：使用ICP對輸入lidar主題進行正確的里程測量。
RGBD/ProximityBySpace：根據機器人在地圖中的位置，尋找局部環形閉合。這在機器人朝反方向回來時非常有用。由于攝像頭朝后，無法找到全局環形閉合點。所以利用位置和之前添加的激光掃描到地圖上，我們用ICP找到變換。
RGBD/AngularUpdate：機器人應該移動更新地圖（如果不是0，則為0）。
RGBD/LinearUpdate：機器人應該移動來更新地圖（如果不是0的話）。
RGBD/OptimizeFromGraphEnd：設置為false（默認為false），在循環關閉時，圖形將從地圖中的第一個姿勢開始優化。TF /map -> /odom會在這種情況下改變。當設置為false時，圖形將從添加到地圖中的最新節點開始優化，而不是第一個節點。通過從最后一個節點開始優化，最后一個姿勢保持它的值，而所有之前的姿勢都會根據它來修正（所以/odom和/map總是匹配在一起）。
Grid/FromDepth: 如果為true，則從深度相機生成的云層中創建占用網格。如果為false，則從激光掃描中生成占領網格。
Icp/VoxelSize：掃描在做ICP之前被過濾到5厘米的體元。
Icp/MaxCorrespondenceDistance：ICP注冊時點之間的最大距離。

更多可以查看SetupOnYourRobot。

4.配置控制程序teleop

使用以下python程序來控制模型在gazebo中的移動。

$ cd ~/catkin_ws/slam_bot/launch
$ nano teleop
$ sudo chmod 777 teleop

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty
msg = """
Control Your  SLAM-Bot!
---------------------------
Moving around:u    i    oj    k    lm    ,    .q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothlyCTRL-C to quit
"""moveBindings = {'i':(1,0),'o':(1,-1),'j':(0,1),'l':(0,-1),'u':(1,1),',':(-1,0),'.':(-1,1),'m':(-1,-1),}speedBindings={'q':(1.1,1.1),'z':(.9,.9),'w':(1.1,1),'x':(.9,1),'e':(1,1.1),'c':(1,.9),}def getKey():tty.setraw(sys.stdin.fileno())rlist, _, _ = select.select([sys.stdin], [], [], 0.1)if rlist:key = sys.stdin.read(1)else:key = ''termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)return keyspeed = .2
turn = 1def vels(speed,turn):return "currently:\tspeed %s\tturn %s " % (speed,turn)if __name__=="__main__":settings = termios.tcgetattr(sys.stdin)rospy.init_node('teleop')pub = rospy.Publisher('~/cmd_vel', Twist, queue_size=5)x = 0th = 0status = 0count = 0acc = 0.1target_speed = 0target_turn = 0control_speed = 0control_turn = 0try:print(msg)print(vels(speed,turn))while(1):key = getKey()if key in moveBindings.keys():x = moveBindings[key][0]th = moveBindings[key][1]count = 0elif key in speedBindings.keys():speed = speed * speedBindings[key][0]turn = turn * speedBindings[key][1]count = 0print(vels(speed,turn))if (status == 14):print(msg)status = (status + 1) % 15elif key == ' ' or key == 'k' :x = 0th = 0control_speed = 0control_turn = 0else:count = count + 1if count > 4:x = 0th = 0if (key == '\x03'):breaktarget_speed = speed * xtarget_turn = turn * thif target_speed > control_speed:control_speed = min( target_speed, control_speed + 0.02 )elif target_speed < control_speed:control_speed = max( target_speed, control_speed - 0.02 )else:control_speed = target_speedif target_turn > control_turn:control_turn = min( target_turn, control_turn + 0.1 )elif target_turn < control_turn:control_turn = max( target_turn, control_turn - 0.1 )else:control_turn = target_turntwist = Twist()twist.linear.x = control_speed; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = control_turnpub.publish(twist)except Exception as e:print(e)finally:twist = Twist()twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0pub.publish(twist)termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

5.運行和測試

從Github中獲取環境將其置于worlds文件夾中。

更改slam.launch

    <arg name="world_name" value="$(find slam_bot)/worlds/kitchen_dining.world"/>

運行程序

$ cd ~/catkin_ws
$ source devel/setup.bash
$ roslaunch slam_bot slam.luaunch

啟動Teleop節點

$ rosrun slam_bot teleop

啟動建圖節點

roslaunch slam_project mapping.launch

使用teleop控制模型四處移動，使用ctrl+c停止建圖節點。

使用rtabmap-databaseViewer打開建圖數據庫

rtabmap-databaseViewer ~/.ros/rtabmap.db

打開后，我們將需要添加一些窗口以更好地查看相關信息，因此：

同意使用數據庫參數
View -> Constraint View
View -> Graph View

圖3-2 rtabmap-databaseViewer

可以在左下方看到循環關閉的數量。這些代碼代表以下各項：
鄰居(Neighbor)、鄰居合并(Neighbor Merged)、全局環路閉合、按空間劃分的局部環路閉合、按時間劃分的局部環路閉合、用戶環路閉合、優先鏈接。

圖3-3 3d地圖

可以使用的另一個工具是rtabmapviz，它是用于實時可視化特征映射，循環閉合等功能的附加節點。由于計算量大，不建議在仿真中進行映射時使用此工具。rtabmapviz非常適合在實時映射期間部署在真實的機器人上，以確保獲得完成循環閉合所必需的功能。

要啟動，將以下代碼添加到mapping.launch文件中：

<!-- visualization with rtabmapviz --><node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen"><param name="subscribe_depth"             type="bool" value="true"/><param name="subscribe_scan"              type="bool" value="true"/><param name="frame_id"                    type="string" value="base_footprint"/><remap from="rgb/image"       to="$(arg rgb_topic)"/><remap from="depth/image"     to="$(arg depth_topic)"/><remap from="rgb/camera_info" to="$(arg camera_info_topic)"/><remap from="scan"            to="/scan"/></node>

6. 導航

在導航中我們重新使用jackal_race.world。

首先對整個地圖經行建圖。

圖3-4 jackal_race.world的3d地圖

localization.launch定位節點

復制mapping.launch文件并更名localization.launch。

還需要對localization.launch文件進行以下更改：

args="–delete_db_on_start"從節點啟動器中刪除，因為您還將需要數據庫進行本地化。
刪除Mem / NotLinkedNodesKept參數
最后，添加字符串類型的Mem / IncrementalMemory參數并將其設置為false，以完成使機器人進入本地化模式所需的更改。

move.launch導航節點

如之前一樣，使用move_base包

<?xml version="1.0"?>
<launch><!-- Move base --><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"><rosparam file="$(find slam_bot)/config/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find slam_bot)/config/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find slam_bot)/config/local_costmap_params.yaml" command="load" /><rosparam file="$(find slam_bot)/config/global_costmap_params.yaml" command="load" /><rosparam file="$(find slam_bot)/config/base_local_planner_params.yaml" command="load" /><remap from="cmd_vel" to="/cmd_vel"/><remap from="odom" to="/odom"/><remap from="scan" to="/slam_bot/laser/scan"/><param name="base_global_planner" type="string" value="navfn/NavfnROS" /><param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/></node>
</launch>

rivz節點

我們將rviz節點從slam.launch中移除，等各個節點建立完成后我們再打開rviz，這樣需要為rviz專門編寫一個launch文件。

<launch><!--launch rviz--><node name="rviz" pkg="rviz" type="rviz" args="-d $(find slam_bot)/launch/config/robot_slam.rviz"/>
</launch>

其中args="-d $(find slam_bot)/launch/config/robot_slam.rviz"引入了rviz的設置文件，這樣就不需要一一添加各個節點了。

批量運行

由于需要使用的運行命令太多，提供如下的程序

#! /bin/bashgnome-terminal -x bash -c "killall gzserver" &
sleep 1 &&echo ' '
read -p 'Would you like to clear the previous map database? (y/n): ' ansinputif [ “$ansinput” = “y” ]
thenprintf '\n Map deleted \n'rm -f ~/.ros/rtabmap.dbelif [ “$ansinput” = “n” ]
thenprintf '\n Map kept \n'elseecho 'Warning: Not an acceptable option. Choose (y/n).'
fiecho ' 'read -p 'Enter target world destination or d for default: ' input_choiceif [ “$input_choice” = “d” ]
then
gnome-terminal -x bash -c " roslaunch slam_bot slam.launch "&else
gnome-terminal -x bash -c "roslaunch slam_bot slam.launch world_file:=$input_choice"  &
fisleep 3 &&gnome-terminal -x bash -c " rosrun slam_bot teleop" &sleep 3 &&echo ' '
read -p 'mapping or localization (m/l): ' inputif [ “$input” = “m” ]
then
gnome-terminal -x bash -c "  roslaunch slam_bot mapping.launch simulation:=true" elif [ “$input” = “l” ]
then
gnome-terminal -x bash -c "  roslaunch slam_bot localization.launch" 
gnome-terminal -x bash -c "  roslaunch slam_bot move.launch" elseecho 'Warning: Not an acceptable option. Choose (m/l).'
fi
sleep 3 &&gnome-terminal -x bash -c "  roslaunch slam_bot rviz.launch"echo ' '
echo 'Script Completed'
echo ' '

在【SLAM建圖和導航仿真實例】（二）中提供了一個測試程序navigation_goal，我們在這里還是借助這個程序。

打開一個新的terminal

$ cd ~/catkin_ws
$ rosrun slam_bot navigation_goal

圖3-5 建圖和導航

總結

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

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

上一篇： Python 之 Numpy 框架入门
下一篇： Walrus 0.4发布：单一配置、多态