机器人SLAM与自主导航（二）

一、导航功能包

1.1导航框架

导航的关键是机器人定位和路径规划两大部分。针对这两个核心，ROS提供了两大功能包：

• move_base：实现机器人导航中的最优路径规划
• amcl：实现二维地图中的机器人定位

在该导航框架中，机器人只需要发布必要的传感器信息和导航的目标位置，ROS即可完成导航功能。

move_base功能包提供导航的主要运行、交互接口；amcl功能包保障导航路径的准确性和机器人自己所处位置的精确定位

• 首先导航功能包采集机器人的传感器信息，以达到实时避障效果
• 其次导航包获取机器人发布的里程计信息和TF变化
• 最后导航包输出Twist格式指令，要求机器人解析控制信息并完成相关动作

使用如下命令安装导航功能包：

$ sudo apt-get install ros-melodic-navigation

1.2move_base功能包

move_base功能包是ROS中完成路径规划的功能包，主要有全局路径规划和本地实时规划两部分组成

• 全局路径规划是给定的目标位置和全局地图进行总体路径的规划

• 本地实时规划是在实际情况中机器人无法严格按照全局路径规划路线行驶，而规划的每个小周期内应该行驶的路线

move_base功能包发布订阅的动作、话题、提供的服务：

1.3amcl功能包

ROS为开发者提供了一套自适应的蒙特卡罗定位方法（amcl），这是一种概率统计方法，针对已有地图使用粒子滤波跟踪一个机器人的姿态

在机器人运动过程中，里程计信息可以帮助机器人定位，而amcl也可以实现机器人定位，其区别在于：

• 里程计定位只通过里程计的数据来处理/base和/odem之间的TF变换
• amcl定位可以估算机器人在地图坐标系/map下的位姿信息，提供/base、/odem、/map之间的TF变换

1.4代价地图的配置

导航功能包使用两种代价地图存储周围环境中的障碍信息：一种用于全局路径规划（global_costmap），一种用于本地路径规划和实时避障（local_costmap）

这两种代价地图都需要一些配置文件进行配置和规划

1.4.1通用配置文件

代价地图用于存储周围环境的障碍信息，其中需要声明地图关注的机器人传感器信息，以便于地图信息的更新

针对两种代价地图通用的配置选项，创建一个名为costmap_common_params.yaml的配置文件：

obstacle_range: 2.5
raytrace_range: 3.0
#footprint: [[0.175, 0.175], [0.175, -0.175], [-0.175, -0.175], [-0.175, 0.175]]
#footprint_inflation: 0.01
robot_radius: 0.175
inflation_radius: 0.1
max_obstacle_height: 0.6
min_obstacle_height: 0.0
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

代码解析：

• 设置地图中障碍物的相关阈值

obstacle_range: 2.5
raytrace_range: 3.0

obstacle_range用于设置机器人检测障碍物的最大范围，如设置2.5表示在2.5米的范围内检测到障碍信息才会在地图中进行更新

raytrace_range用于设置机器人检测自由空间的最大范围，如设置3.0表示在3米的范围内，机器人将根据传感器信息清除范围内的自由空间

• 设置机器人在二维地图中的占用面积

#footprint: [[0.175, 0.175], [0.175, -0.175], [-0.175, -0.175], [-0.175, 0.175]]
#footprint_inflation: 0.01
robot_radius: 0.175
inflation_radius: 0.1

该参数以机器人的中心作为坐标原点，如果机器人为圆形，则需要设置机器人的外形半径robot_radius

inflation_radius参数用于设置障碍物的膨胀参数，也就是机器人应该与障碍物保持的最小安全距离，单位为米

• 设置障碍物的最大高度和最小高度

max_obstacle_height: 0.6
min_obstacle_height: 0.0

• 配置代价地图需要关注的所有传感器信息

observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

每个传感器信息都会在后面列出详细内容

1.4.2全局规划配置文件

全局规划配置文件用于存储配置全局代价地图的参数，命名为global_costmap_params.yaml

global_costmap:
   global_frame: map
   robot_base_frame: base_footprint
   update_frequency: 1.0
   publish_frequency: 1.0
   static_map: true
   rolling_window: false
   resolution: 0.01
   transform_tolerance: 1.0
   map_type: costmap

代码解析：

• global_frame：表示全局代价地图需要在那个参考系下运行，此处选择map参考系
• robot_base_frame：表示代价地图可以参考的机器人本体的坐标系
• update_frequency：决定权据地图信息更新的频率，单位为Hz
• static_map：决定代价地图是否需要map_server提供的地图信息进行初始化，如果不需要使用已有地图或者map_server最好设置为false

1.4.3本地规划配置文件

本地规划配置文件用来存储本地代价地图的配置参数，命名为：local_costmap_params.yaml

local_costmap:
   global_frame: map
   robot_base_frame: base_footprint
   update_frequency: 3.0
   publish_frequency: 1.0
   static_map: true
   rolling_window: false
   width: 6.0
   height: 6.0
   resolution: 0.01
   transform_tolerance: 1.0

代码解析：

• 其中global_frame、robot_base_frame、update_frequency、static_map的意义与全局规划配置文件中的参数相同
• publish_frequency：设置代价地图发布可视化信息的频率，单位为Hz
• rolling_window：设置在机器人移动过程中是否需要滚动窗口，以保持机器人时刻处于中心位置
• width、height、resolution：用于设置代价地图的长、高和分辨率（米/格）

1.4.4本地规划器配置

本地规划器base_local_planner的主要作用是根据规划的全局路径计算发布给机器人的速度控制指令

该规划器要根据机器人的规格配置相关参数，创建名为：base_local_planner_params.yaml

controller_frequency: 3.0
recovery_behavior_enabled: false
clearing_rotation_allowed: false

TrajectoryPlannerROS:
   max_vel_x: 0.5
   min_vel_x: 0.1
   max_vel_y: 0.0  # zero for a differential drive robot
   min_vel_y: 0.0
   max_vel_theta: 1.0
   min_vel_theta: -1.0
   min_in_place_vel_theta: 0.4
   escape_vel: -0.1
   acc_lim_x: 1.5
   acc_lim_y: 0.0  # zero for a differential drive robot
   acc_lim_theta: 1.2

   holonomic_robot: false
   yaw_goal_tolerance: 0.1 # about 6 degrees
   xy_goal_tolerance: 0.05  # 5 cm
   latch_xy_goal_tolerance: false
   pdist_scale: 0.4
   gdist_scale: 0.8
   meter_scoring: true

   heading_lookahead: 0.325
   heading_scoring: false
   heading_scoring_timestep: 0.8
   occdist_scale: 0.05
   oscillation_reset_dist: 0.05
   publish_cost_grid_pc: false
   prune_plan: true

   sim_time: 1.0
   sim_granularity: 0.05
   angular_sim_granularity: 0.1
   vx_samples: 8
   vy_samples: 0  # zero for a differential drive robot
   vtheta_samples: 20
   dwa: true
   simple_attractor: false

二、在RViz中仿真机器人导航

2.1创建launch文件

创建启动文件mrobot_navigation/launch/fake_move_base.launch启动move_base节点，并加载所有配置文件

<launch>
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find mrobot_navigation)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find mrobot_navigation)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find mrobot_navigation)/config/fake/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find mrobot_navigation)/config/fake/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find mrobot_navigation)/config/fake/base_local_planner_params.yaml" command="load" />
    </node>
</launch>

创建一个运行所有导航节点的顶层launch文件mrobot_navigation/launch/fake_nav_demo.launch

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="cloister_gmapping.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mrobot_navigation)/maps/$(arg map)"/>
    <!-- 运行move_base节点 -->
    <include file="$(find mrobot_navigation)/launch/move_base.launch"/>
    <!-- 运行虚拟定位，兼容AMCL输出 -->
    <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />
    <!-- 对于虚拟定位，需要设置一个/odom与/map之间的静态坐标变换 -->
    <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mrobot_navigation)/rviz/nav.rviz"/>
</launch>

创建一个启动机器人模型的launch文件mrobot_bringup/launch/fake_mrobot_with_laser.launch

<launch>
    <param name="/use_sim_time" value="false" />
    <!-- 加载机器人URDF/Xacro模型 -->
    <arg name="urdf_file" default="$(find xacro)/xacro --inorder '$(find mrobot_description)/urdf/mrobot_with_rplidar.urdf.xacro'" />
    <param name="robot_description" command="$(arg urdf_file)" />
    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen" clear_params="true">
        <rosparam file="$(find mrobot_bringup)/config/fake_mrobot_arbotix.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
        <param name="publish_frequency" type="double" value="20.0" />
    </node>
</launch>

2.2开始导航

启动命令：

$ roslaunch mrobot_bringup fake_mrobot_with_laser.launch
$ roslaunch mrobot_navigation fake_nav_demo.launch

菜单栏中2D Nav Goal作用是设置导航的目标点，将鼠标移动到地图上导航目标位置，点击左键（不能松开），此时可以看到目标位置有一个绿色的箭头，因为导航目标点不仅包括位置信息还有位姿信息，拖动鼠标可以设置导航目标的位姿信息

2.3自动导航

在实际应用中往往需要机器人能够自主进行定位和导航，这样才能实现智能化

下面设计一个程序，在地图中设置一个目标点的集合，然后从中随机产生目标点，让机器人自动导航到目标位置

在mrobot_navigation/scripts/random_navigation.py中实现

2.3.1代码

#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import roslib;
import rospy  
import actionlib  
from actionlib_msgs.msg import *  
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
from random import sample  
from math import pow, sqrt  

class NavTest():  
    def __init__(self):  
        rospy.init_node('random_navigation', anonymous=True)  
        rospy.on_shutdown(self.shutdown)  
 
        # 在每个目标位置暂停的时间  
        self.rest_time = rospy.get_param("~rest_time", 2)  

        # 到达目标的状态  
        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   
                       'SUCCEEDED', 'ABORTED', 'REJECTED',  
                       'PREEMPTING', 'RECALLING', 'RECALLED',  
                       'LOST']  
 
        # 设置目标点的位置  
        # 如果想要获得某一点的坐标，在rviz中点击 2D Nav Goal 按键，然后单机地图中一点  
        # 在终端中就会看到坐标信息  
        locations = dict()  

        locations['p1'] = Pose(Point(1.150, 5.461, 0.000), Quaternion(0.000, 0.000, -0.013, 1.000))  
        locations['p2'] = Pose(Point(6.388, 2.66, 0.000), Quaternion(0.000, 0.000, 0.063, 0.998))  
        locations['p3'] = Pose(Point(8.089, -1.657, 0.000), Quaternion(0.000, 0.000, 0.946, -0.324))  
        locations['p4'] = Pose(Point(9.767, 5.171, 0.000), Quaternion(0.000, 0.000, 0.139, 0.990))  
        locations['p5'] = Pose(Point(0.502, 1.270, 0.000), Quaternion(0.000, 0.000, 0.919, -0.392)) 
        locations['p6'] = Pose(Point(4.557, 1.234, 0.000), Quaternion(0.000, 0.000, 0.627, 0.779)) 

        # 发布控制机器人的消息  
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)  
 
        # 订阅move_base服务器的消息  
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  

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

        # 60s等待时间限制  
        self.move_base.wait_for_server(rospy.Duration(60))  
        rospy.loginfo("Connected to move base server")  

        # 保存机器人的在rviz中的初始位置  
        initial_pose = PoseWithCovarianceStamped()  

        # 保存成功率、运行时间、和距离的变量  
        n_locations = len(locations)  
        n_goals = 0  
        n_successes = 0  
        i = n_locations  
        distance_traveled = 0  
        start_time = rospy.Time.now()  
        running_time = 0  
        location = ""  
        last_location = ""  

        # 确保有初始位置  
        while initial_pose.header.stamp == "":  
            rospy.sleep(1)  

        rospy.loginfo("Starting navigation test")  

        # 开始主循环，随机导航  
        while not rospy.is_shutdown():    
            # 如果已经走完了所有点，再重新开始排序  
            if i == n_locations:  
                i = 0  
                sequence = sample(locations, n_locations)  
 
                # 如果最后一个点和第一个点相同，则跳过  
                if sequence[0] == last_location:  
                    i = 1  

            # 在当前的排序中获取下一个目标点  
            location = sequence[i]  

            # 跟踪行驶距离  
            # 使用更新的初始位置  
            if initial_pose.header.stamp == "":  
                distance = sqrt(pow(locations[location].position.x -   
                                    locations[last_location].position.x, 2) +  
                                pow(locations[location].position.y -   
                                    locations[last_location].position.y, 2))  
            else:  
                rospy.loginfo("Updating current pose.")  
                distance = sqrt(pow(locations[location].position.x -   
                                    initial_pose.pose.pose.position.x, 2) +  
                                pow(locations[location].position.y -   
                                    initial_pose.pose.pose.position.y, 2))  
                initial_pose.header.stamp = ""  
 
            # 存储上一次的位置，计算距离  
            last_location = location  

            # 计数器加1  
            i += 1  
            n_goals += 1  

            # 设定下一个目标点  
            self.goal = MoveBaseGoal()  
            self.goal.target_pose.pose = locations[location]  
            self.goal.target_pose.header.frame_id = 'map'  
            self.goal.target_pose.header.stamp = rospy.Time.now()  

            # 让用户知道下一个位置  
            rospy.loginfo("Going to: " + str(location))  
 
            # 向下一个位置进发  
            self.move_base.send_goal(self.goal)  

            # 五分钟时间限制  
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))   

            # 查看是否成功到达  
            if not finished_within_time:  
                self.move_base.cancel_goal()  
                rospy.loginfo("Timed out achieving goal")  
            else:  
                state = self.move_base.get_state()  
                if state == GoalStatus.SUCCEEDED:  
                    rospy.loginfo("Goal succeeded!")  
                    n_successes += 1  
                    distance_traveled += distance  
                    rospy.loginfo("State:" + str(state))  
                else:  
                  rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))  

            # 运行所用时间  
            running_time = rospy.Time.now() - start_time  
            running_time = running_time.secs / 60.0  
  
            # 输出本次导航的所有信息  
            rospy.loginfo("Success so far: " + str(n_successes) + "/" +   
                          str(n_goals) + " = " +   
                          str(100 * n_successes/n_goals) + "%")  

            rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +   
                          " min Distance: " + str(trunc(distance_traveled, 1)) + " m")  

            rospy.sleep(self.rest_time)  

    def update_initial_pose(self, initial_pose):  
        self.initial_pose = initial_pose  

    def shutdown(self):  
        rospy.loginfo("Stopping the robot...")  
        self.move_base.cancel_goal()  
        rospy.sleep(2)  
        self.cmd_vel_pub.publish(Twist())  
        rospy.sleep(1)  

def trunc(f, n):   
    slen = len('%.*f' % (n, f))  

    return float(str(f)[:slen])  

if __name__ == '__main__':  
    try:  
        NavTest()  
        rospy.spin()  

    except rospy.ROSInterruptException:  
        rospy.loginfo("Random navigation finished.")

2.3.2代码解析

• move_base提供给用户使用的主要是action接口，可以通过action实现路径规划，所以首先引入相关模块

import actionlib  
from actionlib_msgs.msg import *  
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  

• 建立导航目标点的字典集合，导航时随即从中挑选当前运动的目标点

locations = dict()  
locations['p1'] = Pose(Point(1.150, 5.461, 0.000), Quaternion(0.000, 0.000, -0.013, 1.000))  
locations['p2'] = Pose(Point(6.388, 2.66, 0.000), Quaternion(0.000, 0.000, 0.063, 0.998))  
locations['p3'] = Pose(Point(8.089, -1.657, 0.000), Quaternion(0.000, 0.000, 0.946, -0.324))  
locations['p4'] = Pose(Point(9.767, 5.171, 0.000), Quaternion(0.000, 0.000, 0.139, 0.990))  
locations['p5'] = Pose(Point(0.502, 1.270, 0.000), Quaternion(0.000, 0.000, 0.919, -0.392)) 
locations['p6'] = Pose(Point(4.557, 1.234, 0.000), Quaternion(0.000, 0.000, 0.627, 0.779)) 

• action使用客户端服务端架构，move_base用于实现服务器，用户用于实现客户端，确保通讯成功

# 订阅move_base服务器的消息  
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
rospy.loginfo("Waiting for move_base action server...")  

# 60s等待时间限制  
self.move_base.wait_for_server(rospy.Duration(60))  
rospy.loginfo("Connected to move base server")  

• 设置导航目标点作为action目标，使用send_goal()接口发送到move_base的服务器，服务器接收到该任务后会进行相关规划然后将信息反馈给客户端，客户端设定了60秒的超时设置，如果超过了时间范围则认定为规划失败

# 设定下一个目标点  
self.goal = MoveBaseGoal()  
self.goal.target_pose.pose = locations[location]  
self.goal.target_pose.header.frame_id = 'map'     
self.goal.target_pose.header.stamp = rospy.Time.now()  

# 让用户知道下一个位置  
rospy.loginfo("Going to: " + str(location))  
 
# 向下一个位置进发  
self.move_base.send_goal(self.goal)  

# 五分钟时间限制  
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))   

• 特别注意shutdown函数这个函数在中断运行时（Ctrl+C）使用，此时move_base服务器可能正在控制机器人运动，需要使用cancel_goal()来取消action任务

def shutdown(self):  
    rospy.loginfo("Stopping the robot...")  
    self.move_base.cancel_goal()  
    rospy.sleep(2)  
    self.cmd_vel_pub.publish(Twist())  
    rospy.sleep(1)