本期通過對turtlebot的Kinect深度傳感器進(jìn)行地圖構(gòu)建，并通過路徑規(guī)劃完成自主導(dǎo)航。

ROS定位導(dǎo)航的框架圖如圖1所示：

圖1 ROS導(dǎo)航定位框架

其中move_base提供了ROS導(dǎo)航的配置、運(yùn) 行、交互接口，它主要包括兩個(gè)部分：

（1） 全局路徑規(guī)劃（global planner）：根據(jù)給定的目標(biāo)位置進(jìn)行總體路 徑的規(guī)劃；

（2） 本地實(shí)時(shí)規(guī)劃（local planner）：根據(jù)附近的障礙物進(jìn)行躲避路線 規(guī)劃。

1.數(shù)據(jù)結(jié)構(gòu)

ROS中定義了MovebaseActionGoal數(shù)據(jù)結(jié)構(gòu)來存儲(chǔ)導(dǎo)航的目標(biāo)位置數(shù)據(jù)，其中最重要的就是位置坐標(biāo)（position）和方向（orientation）

$ rosmsg show MovebaseActionGoal

顯示結(jié)果如下：

[move_base_msgs/MovebaseActionGoal]:

     std_msgs/Header header

uint32 seq

    time stamp

    string frame_id

      actionlib_msgs/GoalID goal_id

    time stamp

    string id

     move_base_msgs/MovebaseGoal goal

     geometry_msgs/PoseStamped target_pose

       std_msgs/Header header

         uint32 seq

         time stamp

         string frame_id

       geometry_msgs/Pose pose

         geometry_msgs/Point position

           float64 x

           float64 y

           float64 z

         geometry_msgs/Quaternion orientation

           float64 x

           float64 y

           float64 z

           float64 w

2.配置文件

move_base使用前需要配置一些參數(shù)：運(yùn)行成本、機(jī)器人半徑、到達(dá)目 標(biāo)位置的距離，機(jī)器人移動(dòng)的速度，這些參數(shù)都在rbx1_nav包的以下幾個(gè)配 置文件中：

l  base_local_planner_params.yaml

l  costmap_common_params.yaml

l  global_costmap_params.yaml

l  local_costmap_params.yaml

3.全局路徑規(guī)劃

在ROS的導(dǎo)航中，首先會(huì)通過全局路徑規(guī)劃，計(jì)算出機(jī)器人到目標(biāo)位置的全局路線。這一功能是navfn這個(gè)包實(shí)現(xiàn)的。navfn通過Dijkstra最優(yōu)路徑的算法，計(jì)算costmap上的最小花費(fèi)路徑，作為機(jī)器人的全局路線。將來在算法上應(yīng)該還會(huì)加入A*算法。

4.本地實(shí)時(shí)規(guī)劃（local  planner）

本地實(shí)時(shí)規(guī)劃是利用base_local_planner包實(shí)現(xiàn)的。該包使用Trajectory Rollout 和Dynamic Window approaches算法計(jì)算機(jī)器人每個(gè)周期內(nèi)應(yīng)該行駛的速度和角度（dx，dy，dtheta velocities）。

base_local_planner這個(gè)包通過地圖數(shù)據(jù)，通過算法搜索到達(dá)目標(biāo)的多條 路經(jīng)，利用一些評價(jià)標(biāo)準(zhǔn)（是否會(huì)撞擊障礙物，所需要的時(shí)間等等）選取最優(yōu) 的路徑，并且計(jì)算所需要的實(shí)時(shí)速度和角度。

其中，Trajectory Rollout 和Dynamic Window approaches算法主要思路如下：

（1）采樣機(jī)器人當(dāng)前的狀態(tài)（dx,dy,dtheta）；

（2）針對每個(gè)采樣的速度，計(jì)算機(jī)器人以該速度行駛一段時(shí)間后的狀態(tài)，得出一條行駛的路線；

（3）利用一些評價(jià)標(biāo)準(zhǔn)為多條路線打分；

（4）根據(jù)打分，選擇最優(yōu)路徑；

（5）重復(fù)上面過程。

5.ArbotiX仿真

安裝ArbotiX模擬器：

$ sudo aptitude install   ros-indigo-arbotix –y

下載rbx1例子：

$ cd catkin_ws/src

$ git clone   https://github.com/pirobot/rbx1.git

為了簡化，我們暫時(shí)使用空白地圖（blank_map.pgm）在空地上進(jìn)行無障礙仿真。首先運(yùn)行ArbotiX節(jié)點(diǎn)，并且加載機(jī)器人的URDF文件：

$ roslaunch rbx1_bringup   fake_turtlebot.launch

然后運(yùn)行move_base和加載空白地圖的launch文件（fake_move_bas

e_blank_map.launch）：

$ roslaunch rbx1_nav   fake_move_base_blank_map.launch

該文件的具體內(nèi)容如下：

其中調(diào)用了fake_move_base.launch文件，是運(yùn)行move_base節(jié)點(diǎn)并進(jìn)行參數(shù)配置。

然后調(diào)用rviz就可以看到機(jī)器人了（如圖2所示）：

$ rosrun rviz rviz –d   ~/ catkin_ws /src/rbx1/rbx1_nav/nav_obstacles.rviz

圖2 使用Rviz進(jìn)行TurtleBot仿真

我們先以1m的速度進(jìn)行一下測試, 讓機(jī)器人前進(jìn)一米：

$   rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {frame_id:"base_link"},pose:{position:{x:1,y:0,z:0},orientation:{x:0,y:0,z:0,w:1}}}'

讓機(jī)器人后退一米，回到原來的位置：

$   rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {frame_id:"map"},pose:{position:{x:0,y:0,z:0},orientation:{x:0,y:0,z:0,w:1}}}'

在rviz中的軌跡如圖3：

圖3 TurtleBot運(yùn)動(dòng)軌跡

在機(jī)器人移動(dòng)過程中，有一條藍(lán)色的線（被黃線擋住了）就是機(jī)器人的全局規(guī)劃的路徑；紅色的箭頭是實(shí)施規(guī)劃的路線，會(huì)不斷更新，有的時(shí)候會(huì)呈現(xiàn)很大的弧線，那是因?yàn)闄C(jī)器人在轉(zhuǎn)向的過程中盡量希望保持平穩(wěn)的角度。如果覺得路徑規(guī)劃的精度不夠，可以修改配置文件中的pdist_scale參數(shù)進(jìn)行修正。然后我們可以認(rèn)為的確定目標(biāo)位置，點(diǎn)擊rviz上方的2D Nav Goal按鍵，然后左鍵選擇目標(biāo)位置，機(jī)器人就開始自動(dòng)導(dǎo)航了。

圖4 TurtleBot自主導(dǎo)航

6.ArbotiX仿真——帶有障礙物的路徑規(guī)劃

首先我們讓機(jī)器人走一個(gè)正方形的路線。先通過上面的命令，讓機(jī)器人回到原始位置（0，0，0），然后按reset按鍵，把所有的箭頭清除，接著運(yùn)行走正方形路徑的代碼：

$   rosrun rbx1_nav move_base_square.py

在rviz中可以看到圖5所示的結(jié)果：

圖5 TurtleBot繞正方形路徑運(yùn)動(dòng)

圖5中四個(gè)頂角的粉色圓盤就是我們設(shè)定的位置，正方形比較規(guī)則，可見定位還是比較準(zhǔn)確的。TurtleBot繞正方形路徑運(yùn)動(dòng)的代碼如下：

#!/usr/bin/env   python

   import roslib;   roslib.load_manifest('rbx1_nav')

   import rospy

   import actionlib

   from actionlib_msgs.msg import *

   from geometry_msgs.msg import Pose, Point,   Quaternion, Twist

   from move_base_msgs.msg import   MovebaseAction, MovebaseGoal

   from tf.transformations import   quaternion_from_euler

   from visualization_msgs.msg import Marker

   from math import radians, pi

   class MovebaseSquare():

       def    init (self):

           rospy.init_node('nav_test',   anonymous=False)

           rospy.on_shutdown(self.shutdown)

           # 設(shè)定正方形的尺寸，默認(rèn)是一米

           square_size =   rospy.get_param("~square_size", 1.0) # meters

           #創(chuàng)建一個(gè)列表，保存目標(biāo)的角度數(shù)據(jù)

           quaternions = list()

           #定義四個(gè)頂角處機(jī)器人的方向角度

           #將上面的Euler angles轉(zhuǎn)換成Quaternion的格式

           for angle in euler_angles:

               q_angle =   quaternion_from_euler(0, 0, angle, axes='sxyz')

               q = Quaternion(*q_angle)

               quaternions.append(q)

           #創(chuàng)建一個(gè)列表存儲(chǔ)導(dǎo)航點(diǎn)的位置

           waypoints = list()

           #創(chuàng)建四個(gè)導(dǎo)航點(diǎn)的位置（角度和坐標(biāo)位置）

          waypoints.append(Pose(Point(square_size,   0.0, 0.0), quaternion s[0]))

          waypoints.append(Pose(Point(square_size,   square_size, 0.0), quaternions[1])) s[2]))

waypoints.append(Pose(Point(0.0,   square_size, 0.0), quaternion

                 waypoints.append(Pose(Point(0.0, 0.0,   0.0), quaternions[3]))

              #初始化可視化標(biāo)記

              self.init_markers()

            #給每個(gè)定點(diǎn)的導(dǎo)航點(diǎn)一個(gè)可視化標(biāo)記

              p = Point()

              p = waypoint.position

              self.markers.points.append(p)

              #發(fā)布TWist消息控制機(jī)器人

              self.cmd_vel_pub =   rospy.Publisher('cmd_vel', Twist)

            #訂閱move_base服務(wù)器的消息

              self.move_base =   actionlib.SimpleActionClient("move_base", Move

baseAction)

           rospy.loginfo("Waiting for   move_base action server...")

           # 等待move_base服務(wù)器建立

             self.move_base.wait_for_server(rospy.Duration(60))

           rospy.loginfo("Connected to   move base server")

           rospy.loginfo("Starting   navigation test")

           #初始化一個(gè)計(jì)數(shù)器，記錄到達(dá)的頂點(diǎn)號(hào)

           i = 0

           # 主循環(huán),環(huán)繞通過四個(gè)定點(diǎn)

           while i < 4 and not   rospy.is_shutdown():

               # 發(fā)布標(biāo)記指示四個(gè)目標(biāo)的位置，每個(gè)周期發(fā)布一起                 self.marker_pub.publish(self.markers)

               #初始化goal為MovebaseGoal類型

               goal = MovebaseGoal()

在實(shí)際中，往往需要讓機(jī)器人自動(dòng)躲避障礙物。move_base包的一個(gè)強(qiáng)大的功能就是可以在全局規(guī)劃的過程中自動(dòng)躲避障礙物，而不影響全局路徑。障礙物可以是靜態(tài)的（比如墻、桌子等），也可以是動(dòng)態(tài)的（比如人走過）?，F(xiàn)在我們嘗試在之前的正方形路徑中加入障礙物。

把之前運(yùn)行fake_move_base_blank_map.launch的終端停止（Ctrl­C）掉，然后運(yùn)行：

$   roslaunch rbx1_nav fake_move_base_obstacle.launch

運(yùn)行結(jié)果如圖6所示，在rviz中出現(xiàn)了障礙物：

圖6 Rviz中出現(xiàn)的障礙物

然后再運(yùn)行之前繞正方形運(yùn)動(dòng)的節(jié)點(diǎn)：

$   rosrun rbx1_nav move_base_square.py

這回可以看到，在全局路徑規(guī)劃的時(shí)候，機(jī)器人已經(jīng)將障礙物繞過去了，如圖7所示：

圖7 TurtleBot避障

圖7中，中間的線是障礙物，周圍紅色的橢圓形是根據(jù)配置文件中的inflation_radius參數(shù)計(jì)算出來的安全緩沖區(qū)。全局規(guī)劃的路徑基本已經(jīng)是最短路徑了。在仿真的過程中也可以動(dòng)態(tài)重配置那四個(gè)配置文件，修改仿真參數(shù)。

7.實(shí)物測試

首先啟動(dòng)turtlebot：

$   roscore

$   roslaunch turtlebot_bringup minimal.launch

運(yùn)行地圖繪制demo：

$   roslaunch turtlebot_navigation maping_demo.launch

打開rviz查看地圖：

$   roslaunch turtlebot_rviz_launchers view_navigation.launch

通過鍵盤控制機(jī)器人移動(dòng)，建立地圖：

$   roslaunch turtlebot_teleop keyboard.launch

    建圖過程如圖8所示：

圖8 TurtleBot建圖

此時(shí)，可以選中2D Pose Estimate，然后用鼠標(biāo)選中一個(gè)位置單擊鼠標(biāo)左鍵，機(jī)器人就會(huì)移動(dòng)至你所指定的位置。

要保存建立的地圖，運(yùn)行：

$   rosrun map_server map_saver –f /tmp.my_map

要加載保存過的地圖，運(yùn)行：

$roslaunch   turtlebot_navigation amcl_demo.launch

map_file:=/tmp/my_map.yaml

接下來的操作和上面一樣，機(jī)器人會(huì)根據(jù)你指定的位置進(jìn)行自主導(dǎo)航。