当前位置:   article > 正文

ROS高效进阶第六章 -- 机器人SLAM建图与自主导航之move_base_slam自主导航

slam自主导航

1 背景资料

本文是机器人SLAM建图与自主导航的第三篇文章,我们将基于之前ROS高效进阶第六章 – 机器人SLAM建图与自主导航之gmapping算法的 mbot_navigation 样例进行扩充,使用 ros 的 move_base 导航与路径规划包,在有地图和没地图两种情况下,控制轮式机器人探索整个屋子。本文的测试环境是ubuntu20.04 + ros noetic。
本文参考资料如下:
(1)《ROS机器人开发实践》胡春旭 第9章
(2)ROS高效进阶第六章 – 机器人SLAM建图与自主导航之gmapping算法
(3)ROS高效进阶第六章 – Ros Action通信机制
(4)Turtlebot的建图导航之旅(宝藏文章)
(5)ROS move_base
(6)ROS Amcl
(7)ROS Navigation
(8)amcl的原理,可参考《概率机器人》

2 正文

2.1 move_base导航规划框架

(1)下面的图是move_base导航规划框架,中间白色框内是move_base,左右两边是它的依赖,蓝色图框是必须依赖,灰色图框是可选依赖。下面是它的输出,也就是经典的/cmd_vel topic,发送给机器人控制器。
在这里插入图片描述
安装 ros navigation包:

sudo apt-get install ros-noetic-navigation
  • 1

(2)move_base依赖 lidar 识别周边环境,依赖里程计确定自己的方位。但 move_base 并不强依赖map_server的全局地图,没有地图也可以边建图边导航。如果move_base 使用了全局地图,就需要使用amcl(Adaptive Monte Carlo Localization,自适应蒙特卡罗定位)进行自身地位,确定自身在全局地图中的位置。
(3)一个机器人从起点到终点需要解决两个问题,一是路由/路线导航,二是轨迹规划。前者对应move_base的global planner,使用经典的Dijkstra或A*算法;后者对应move_base的local_planner,使用Trajectory Rollout 和 Dynamic Window Approaches。
除此之外,recovery_vehaviors是move_base的脱困功能,在机器人卡死后,进行自主脱困。
(4)在自动驾驶领域,路线导航由导航模块依赖车载地图完成。轨迹规划依赖planning模块,这是自动驾驶领域的关键,其使用路径-速度解耦规划算法,Lattice规划算法等。
(5)如果想系统的学习move_base导航规划框架,这里推荐一个宝藏博客:Turtlebot的建图导航之旅(宝藏文章)。如果想更深入学习,请去ros网站学习,链接在第一章。

2.2 mbot_navigation 样例

(1)创建相关launch文件,和控制程序文件(cpp,py各一份)

cd ~/catkin_ws/src/mbot_navigation
mkdir scritps
touch launch/amcl.launch launch/move_base.launch 
touch launch/navigation_with_map.launch launch/navigation_with_gmapping.launch
touch src/exploring_house.cpp scripts/exploring_house.py
  • 1
  • 2
  • 3
  • 4
  • 5

(2)amcl.launch(所有参数一律来自官方样例)

<launch>
    <arg name="scan_topic" default="scan"/>
    <arg name="initial_pose_x" default="0.0"/>
    <arg name="initial_pose_y" default="0.0"/>
    <arg name="initial_pose_a" default="0.0"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="min_particles"             value="500"/>
        <param name="max_particles"             value="3000"/>
        <param name="kld_err"                   value="0.02"/>
        <param name="update_min_d"              value="0.20"/>
        <param name="update_min_a"              value="0.20"/>
        <param name="resample_interval"         value="1"/>
        <param name="transform_tolerance"       value="0.5"/>
        <param name="recovery_alpha_slow"       value="0.00"/>
        <param name="recovery_alpha_fast"       value="0.00"/>
        <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>
        <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>
        <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>
        <param name="gui_publish_rate"          value="50.0"/>

        <remap from="scan"                      to="$(arg scan_topic)"/>
        <param name="laser_max_range"           value="3.5"/>
        <param name="laser_max_beams"           value="180"/>
        <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_likelihood_max_dist" value="2.0"/>
        <param name="laser_model_type"          value="likelihood_field"/>

        <param name="odom_model_type"           value="diff"/>
        <param name="odom_alpha1"               value="0.1"/>
        <param name="odom_alpha2"               value="0.1"/>
        <param name="odom_alpha3"               value="0.1"/>
        <param name="odom_alpha4"               value="0.1"/>
        <param name="odom_frame_id"             value="odom"/>
        <param name="base_frame_id"             value="base_footprint"/>
    </node>
</launch>
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42

(2)move_base.launch如下,里面的配置参数文件来自官方样例,见:mbot_navigation

<launch>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />

    <rosparam file="$(find mbot_navigation)/config/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mbot_navigation)/config/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find mbot_navigation)/config/move_base/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/move_base/global_costmap_params.yaml" command="load" />

    <rosparam file="$(find mbot_navigation)/config/move_base/move_base_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/move_base/dwa_local_planner_params.yaml" command="load" />
  </node>
 
</launch>
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14

(3)navigation_with_map.launch实现了在有全局地图下的导航和规划

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="gmapping_save.yaml" />

    <!-- 运行地图服务器,并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mbot_navigation)/maps/$(arg map)"/>

    <!-- 运行move_base节点 -->
    <include file="$(find mbot_navigation)/launch/move_base.launch"/>

    <!-- 启动AMCL节点 -->
    <include file="$(find mbot_navigation)/launch/amcl.launch" />

    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/navigation.rviz"/>

</launch>
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17

(4)navigation_with_gmapping.launch实现了无全局地图下的导航和规划

<launch>
    <include file="$(find mbot_navigation)/launch/gmapping.launch"/>

    <!-- 运行move_base节点 -->
    <include file="$(find mbot_navigation)/launch/move_base.launch" />

    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/navigation.rviz"/>
</launch>
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

(5)exploring_house.cpp作为move_base action服务的客户端,用来向move_base发送目标点。ros action的原理和使用方法见:ROS高效进阶第六章 – Ros Action通信机制

#include <ros/ros.h>
#include <list>
#include <geometry_msgs/Pose.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

geometry_msgs::Pose createPose(double px, double py, double pz, double ox, double oy, double oz, double ow) {
    geometry_msgs::Pose pose;
    pose.position.x = px;
    pose.position.y = py;
    pose.position.z = pz;
    pose.orientation.x = ox;
    pose.orientation.y = oy;
    pose.orientation.z = oz;
    pose.orientation.w = ow;
    return pose;  
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "move_test");
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client("move_base", true);

  ROS_INFO("Waiting for move_base action server...");  
  move_base_client.waitForServer();
  ROS_INFO("connected to move base server");

  std::vector<geometry_msgs::Pose> target_list;
  target_list.push_back(createPose(6.543, 4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
  target_list.push_back(createPose(5.543, -4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
  target_list.push_back(createPose(-5.543, 4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
  target_list.push_back(createPose(-5.543, -4.779, 0.000, 0.000, 0.000, 0.645, 0.764));

  for (uint8_t i = 0; i < target_list.size(); i ++) {
    ros::Time start_time = ros::Time::now();
    ROS_INFO("going to %u goal, position: (%f, %f)", i, target_list[i].position.x, target_list[i].position.y);

    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose = target_list[i];

    move_base_client.sendGoal(goal);
    move_base_client.waitForResult();

    if (move_base_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
      ros::Duration running_time =  ros::Time::now() - start_time;
      ROS_INFO("go to %u goal succeeded, running time %f sec", i, running_time.toSec());
    } else {
      ROS_INFO("goal failed");
    }
  }

  return 0;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54

(6)exploring_house.py的功能同exploring_house.cpp,只是用python实现一遍

#!/usr/bin/env python3
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  

def main():
  rospy.init_node("move_test", anonymous=True)
  move_base_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
  rospy.loginfo("Waiting for move_base action server...")  
  while move_base_client.wait_for_server(rospy.Duration(5.0)) == 0:
    rospy.loginfo("connected to move base server")
  
  target_list = []
  target_list.append(Pose(Point(6.543, 4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
  target_list.append(Pose(Point(5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
  target_list.append(Pose(Point(-5.543, 4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
  target_list.append(Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
  
  for i, target in enumerate(target_list):
    start_time = rospy.Time.now()  
    
    goal = MoveBaseGoal()
    goal.target_pose.pose = target
    goal.target_pose.header.frame_id = 'map'
    goal.target_pose.header.stamp = rospy.Time.now()
    
    rospy.loginfo("going to {0} goal, {1}".format(i, str(target)))
    move_base_client.send_goal(goal)
    finished_within_time = move_base_client.wait_for_result(rospy.Duration(300))
    
    if not finished_within_time:
      move_base_client.cancel_goal()
      rospy.loginfo("time out, failed to goal")
    else:
      running_time = (rospy.Time.now() - start_time).to_sec()
      if move_base_client.get_state() == GoalStatus.SUCCEEDED:
        rospy.loginfo("go to {0} goal succeeded, run time: {1} sec".format(i, running_time))
      else:
        rospy.loginfo("goal failed")
    
if __name__ == "__main__":
  main()
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44

(7)CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(mbot_navigation)
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  move_base_msgs
  roscpp
  rospy
  actionlib
)
catkin_package()
include_directories(
  ${catkin_INCLUDE_DIRS}
)
add_executable(exploring_house src/exploring_house.cpp)
target_link_libraries(exploring_house
  ${catkin_LIBRARIES}
)
catkin_install_python(PROGRAMS
  scripts/exploring_house.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21

(8)编译和运行

cd ~/catkin_ws
catkin_make -DCATKIN_WHITELIST_PACKAGES="mbot_navigation;mbot_gazebo"
source devel/setup.bash

// 打开仿真环境
roslaunch mbot_gazebo mbot_gazebo.launch
//再开一个窗口,运行有全局地图下的导航规划样例
roslaunch mbot_navigation navigation_with_map.launch
// 再开一个窗口,控制机器人行动
rosrun mbot_navigation exploring_house
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10

在这里插入图片描述

// 打开仿真环境
roslaunch mbot_gazebo mbot_gazebo.launch
//再开一个窗口,运行有无全局地图下的导航规划样例
roslaunch mbot_navigation navigation_with_gmapping.launch
// 再开一个窗口,控制机器人行动
rosrun mbot_navigation exploring_house.py
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

在这里插入图片描述

3 总结

本文代码托管在本人的github上:mbot_navigation

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/人工智能uu/article/detail/805572
推荐阅读
相关标签
  

闽ICP备14008679号