导航框架

move_base

安装：sudo apt-get install ros-kinetic-navigation

---

全局路径规划（global planner）

• 全局最优路径规划
• Dijkstra或A^* 算法

本地实时规划（local planner）

• 规划机器人每个周期内的线速度、角速度，使之尽量符合全局最优路径
• 实时避障
• Trajectory Rollout 和 Dynamic Window Approaches算法
• 搜索躲避和行进的多条路径，综合各评价标准选取最优路径

---

move_base功能包中的话题和服务

---

配置move_base节点：
src5/mbot_navigation/launch/move_base.launch

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

其中所引用的文件在src5/mbot_navigation/config/mbot

总用量 16
-rw-rw-r-- 1 ddu ddu 1027 4月  26  2018 base_local_planner_params.yaml
-rw-rw-r-- 1 ddu ddu  364 4月  26  2018 costmap_common_params.yaml
-rw-rw-r-- 1 ddu ddu  238 4月  26  2018 global_costmap_params.yaml
-rw-rw-r-- 1 ddu ddu  246 4月  26  2018 local_costmap_params.yaml

里面的参数含义参考：http://wiki.ros.org/move_base

base_local_planner_params.yaml
该配置文件里面配置了局部路径规划相关的配置

controller_frequency: 3.0
recovery_behavior_enabled: false
clearing_rotation_allowed: falseTrajectoryPlannerROS:max_vel_x: 0.5min_vel_x: 0.1max_vel_y: 0.0  # zero for a differential drive robotmin_vel_y: 0.0max_vel_theta: 1.0min_vel_theta: -1.0min_in_place_vel_theta: 0.5escape_vel: -0.1acc_lim_x: 1.5acc_lim_y: 0.0 # zero for a differential drive robotacc_lim_theta: 1.2holonomic_robot: falseyaw_goal_tolerance: 0.1 # about 6 degreesxy_goal_tolerance: 0.1  # 10 cmlatch_xy_goal_tolerance: falsepdist_scale: 0.9gdist_scale: 0.6meter_scoring: trueheading_lookahead: 0.325heading_scoring: falseheading_scoring_timestep: 0.8occdist_scale: 0.1oscillation_reset_dist: 0.05publish_cost_grid_pc: falseprune_plan: truesim_time: 1.0sim_granularity: 0.025angular_sim_granularity: 0.025vx_samples: 8vy_samples: 0 # zero for a differential drive robotvtheta_samples: 20dwa: truesimple_attractor: false

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.15
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}

global_costmap_params.yaml
该配置文件里面配置了全局代价地图相关参数

global_costmap:global_frame: maprobot_base_frame: base_footprintupdate_frequency: 1.0publish_frequency: 1.0static_map: truerolling_window: falseresolution: 0.01transform_tolerance: 1.0map_type: costmap

local_costmap_params.yaml
该配置文件里面配置了局部代价地图共用的参数

local_costmap:global_frame: odomrobot_base_frame: base_footprintupdate_frequency: 3.0publish_frequency: 1.0static_map: truerolling_window: falsewidth: 6.0height: 6.0resolution: 0.01transform_tolerance: 1.0

---

amcl

• 蒙特卡罗定位方法
• 二维环境定位
• 针对已有地图使用粒子滤波器跟踪一个机器人的姿态

---

amcl功能包中的话题和服务

---

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

---

配置amcl节点
src5/mbot_navigation/launch/amcl.launch

<launch><arg name="use_map_topic" default="false"/><arg name="scan_topic" default="scan"/><node pkg="amcl" type="amcl" name="amcl" clear_params="true"><param name="use_map_topic" value="$(arg use_map_topic)"/><!-- Publish scans from best pose at a max of 10 Hz --><param name="odom_model_type" value="diff"/><param name="odom_alpha5" value="0.1"/><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="60"/><param name="laser_max_range" value="12.0"/><param name="min_particles" value="500"/><param name="max_particles" value="2000"/><param name="kld_err" value="0.05"/><param name="kld_z" value="0.99"/><param name="odom_alpha1" value="0.2"/><param name="odom_alpha2" value="0.2"/><!-- translation std dev, m --><param name="odom_alpha3" value="0.2"/><param name="odom_alpha4" value="0.2"/><param name="laser_z_hit" value="0.5"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_model_type" value="likelihood_field"/><!-- <param name="laser_model_type" value="beam"/> --><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.25"/><param name="update_min_a" value="0.2"/><param name="odom_frame_id" value="odom"/><param name="resample_interval" value="1"/><!-- Increase tolerance because the computer can get quite busy --><param name="transform_tolerance" value="1.0"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/><remap from="scan" to="$(arg scan_topic)"/></node>
</launch>

机器人自主导航

导航示例（《ROS by Example》）

roslaunch rbx1_bringup fake_turtlebot.launch
roslaunch rbx1_nav fake_move_base_map_with_obstacles.launch
rosrun rviz rviz -d `rospack find rbx1_nav`/nav_obstacles.rviz
rosrun rbx1_nav move_base_square.py

效果如下：

运行roslaunch rbx1_bringup fake_turtlebot.launch时，如果出错的话，运行一下命令：
sudo apt-get install ros-kinetic-arbotix-*

正常运行的话，机器人会走过指定的四个点，在运动的过程中，自主进行路径规划和避障

导航仿真

roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
roslaunch mbot_navigation nav_cloister_demo.launch

效果如下：

在机器人导航的过程中，在gazebo中给出临时障碍物，机器人可以顺利避开

导航slam仿真

roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
roslaunch mbot_navigation exploring_slam_demo.launch

效果如下：

自主探索slam仿真

roslaunch mbot_gazebo mbot_laser_nav_gazebo.launch
roslaunch mbot_navigation exploring_slam_demo.launch
rosrun mbot_navigation exploring_slam.py

效果展示：

代码分析：
src5/mbot_navigation/scripts/exploring_slam.py

#!/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('exploring_slam', anonymous=True)  rospy.on_shutdown(self.shutdown)  # 在每个目标位置暂停的时间 (单位：s)self.rest_time = rospy.get_param("~rest_time", 2)  # 是否仿真？  self.fake_test = rospy.get_param("~fake_test", True)  # 到达目标的状态  goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   'SUCCEEDED', 'ABORTED', 'REJECTED',  'PREEMPTING', 'RECALLING', 'RECALLED',  'LOST']  # 设置目标点的位置  # 在rviz中点击 2D Nav Goal 按键，然后单击地图中一点  # 在终端中就会看到该点的坐标信息  locations = dict()  locations['1'] = Pose(Point(4.589, -0.376, 0.000),  Quaternion(0.000, 0.000, -0.447, 0.894))  locations['2'] = Pose(Point(4.231, -6.050, 0.000),  Quaternion(0.000, 0.000, -0.847, 0.532))  locations['3'] = Pose(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))  locations['4'] = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))  locations['5'] = Pose(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940))  locations['6'] = Pose(Point(2.924, 0.018, 0.000),   Quaternion(0.000, 0.000, 0.000, 1.000))  # 发布控制机器人的消息  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("Exploring SLAM finished.")

---

小结

机器人必备条件

• 硬件要求：差分轮式、速度控制指令、深度信息、外观圆形或方形
• 里程计信息：获取仿真机器人/真实机器人的实时位置、速度
• 仿真环境：构建仿真环境，为后续SLAM、导航仿真作准备

ROS SLAM功能包应用方法

• gmapping
  • 输入：激光雷达、里程计信息
  • 输出：二维栅格地图
• hector_slam
  • 输入： 输入激光雷达信息
  • 输出：二维栅格地图
• cartographer
  • 输入：激光雷达信息
  • 输出：二维或三维地图
• orb_slam
  • 输入：单目摄像头信息
  • 输出：三维点云地图

ROS的导航框架

• move_base：全局规划和局部规划
• amcl：二维概率定位

ROS机器人自主导航

• rviz+Arbotix的功能仿真
• gazebo环境下自主导航的仿真
• 导航过程中同步slam建图