ROS开发笔记（7）——利用amcl、move_base 进行导航、基于Python编写巡逻机器人导航代码

本文主要是介绍ROS开发笔记（7）——利用amcl、move_base 进行导航、基于Python编写巡逻机器人导航代码，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

在前期构建地图的基础上，本文将利用 amcl（自适应蒙特卡洛定位算法）来在地图中定位机器人，然后在此基础上利用move_base 在地图中对机器人导航。主要内容如下：

1、amcl（自适应蒙特卡洛定位算法）节点基本原理

2、move_base 节点基本工作原理

3、在地图中定位机器人

4、在rivz中导航

5、编写代码导航

1、amcl（自适应蒙特卡洛定位算法）节点基本原理

1.1、粒子云与pose估计值

amcl算法是一种粒子滤波算法，其维护一堆的位姿（pose）值用粒子（下图中机器人周围的绿色点状物的便是粒子云）表示，每个粒子具有一个概率值，概率越大代表机器人处于这个pose的可能性越大，按概率加权平均就得到了机器人pose的估计值。

1.2、运动预测更新

随着机器人的移动，各个粒子也跟随着机器人里程计数据一起运动更新。

1.3、量测更新

实际量测值：实际的机器人的传感器数据（如激光扫描scan数据）。

理想量测值：基于地图模型map数据可以算出如果机器人就处于某个粒子pose时，理想的传感器数据应该是什么样的。

如果某个粒子的|理想量测值-实际量测值| 比较小，那么这个粒子关联的概率值就要大，反之小，这样对粒子云的概率值进行更新，随着时间的推进，概率过小的粒子将被剔除，概率大的保留并重采样，这样粒子云的分布就发生了变化，如果定位算法收敛，其分布会逐步集中，中心收敛到真实pose附近。

amcl 节点信息如下：

wsc@wsc-pc:~$ rosnode list
/amcl
/gazebo
/gazebo_gui
/map_server
/move_base
/robot_state_publisher
/rosout
/rviz
/turtlebot3_teleop_keyboard
wsc@wsc-pc:~$ rosnode info amcl
--------------------------------------------------------------------------------
Node [/amcl]
Publications: * /amcl/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /amcl/parameter_updates [dynamic_reconfigure/Config]* /amcl_pose [geometry_msgs/PoseWithCovarianceStamped]* /particlecloud [geometry_msgs/PoseArray]* /rosout [rosgraph_msgs/Log]* /tf [tf2_msgs/TFMessage]Subscriptions: * /clock [rosgraph_msgs/Clock]* /initialpose [geometry_msgs/PoseWithCovarianceStamped]* /scan [sensor_msgs/LaserScan]* /tf [tf2_msgs/TFMessage]* /tf_static [tf2_msgs/TFMessage]Services: * /amcl/get_loggers* /amcl/set_logger_level* /amcl/set_parameters* /global_localization* /request_nomotion_update* /set_mapcontacting node http://wsc-pc:46609/ ...
Pid: 12958
Connections:* topic: /rosout* to: /rosout* direction: outbound* transport: TCPROS* topic: /tf* to: /amcl* direction: outbound* transport: INTRAPROCESS* topic: /tf* to: /move_base* direction: outbound* transport: TCPROS* topic: /tf* to: /rviz* direction: outbound* transport: TCPROS* topic: /particlecloud* to: /rviz* direction: outbound* transport: TCPROS* topic: /clock* to: /gazebo (http://wsc-pc:33887/)* direction: inbound* transport: TCPROS* topic: /tf* to: /amcl (http://wsc-pc:46609/)* direction: inbound* transport: INTRAPROCESS* topic: /tf* to: /gazebo (http://wsc-pc:33887/)* direction: inbound* transport: TCPROS* topic: /tf* to: /robot_state_publisher (http://wsc-pc:41277/)* direction: inbound* transport: TCPROS* topic: /tf_static* to: /robot_state_publisher (http://wsc-pc:41277/)* direction: inbound* transport: TCPROS* topic: /scan* to: /gazebo (http://wsc-pc:33887/)* direction: inbound* transport: TCPROS* topic: /initialpose* to: /rviz (http://wsc-pc:45675/)* direction: inbound* transport: TCPROSwsc@wsc-pc:~$

2、move_base 节点基本工作原理

参考链接：http://wiki.ros.org/move_base?distro=melodic

move_base综合了地图、定位系统、传感器及里程计数据，规划一条从当前位置到目标位置goal的路径。

1、navigation goal

通过 MoveBaseAction , MoveBaseGoal等实现，MoveBaseGoal封装了目标pose数据，客服端程序将MoveBaseGoal类型数据发送给MoveBaseAction服务端启动导航过程。

2、global planner

全局规划器利用地图与规划算法规划一条从当前位置到目标位置的最优路径。

3、local planner

可以利用传感器数据识别不在地图中障碍物信息，如果本地规划行不通，会请求重新进行全局规划。

move_base与其他组件的相关接口描述如下：

rosnode info move_base
--------------------------------------------------------------------------------
Node [/move_base]
Publications: * /cmd_vel [geometry_msgs/Twist]* /move_base/DWAPlannerROS/cost_cloud [sensor_msgs/PointCloud2]* /move_base/DWAPlannerROS/global_plan [nav_msgs/Path]* /move_base/DWAPlannerROS/local_plan [nav_msgs/Path]* /move_base/DWAPlannerROS/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /move_base/DWAPlannerROS/parameter_updates [dynamic_reconfigure/Config]* /move_base/DWAPlannerROS/trajectory_cloud [sensor_msgs/PointCloud2]* /move_base/NavfnROS/plan [nav_msgs/Path]* /move_base/current_goal [geometry_msgs/PoseStamped]* /move_base/feedback [move_base_msgs/MoveBaseActionFeedback]* /move_base/global_costmap/costmap [nav_msgs/OccupancyGrid]* /move_base/global_costmap/costmap_updates [map_msgs/OccupancyGridUpdate]* /move_base/global_costmap/footprint [geometry_msgs/PolygonStamped]* /move_base/global_costmap/inflation_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /move_base/global_costmap/inflation_layer/parameter_updates [dynamic_reconfigure/Config]* /move_base/global_costmap/obstacle_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /move_base/global_costmap/obstacle_layer/parameter_updates [dynamic_reconfigure/Config]* /move_base/global_costmap/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /move_base/global_costmap/parameter_updates [dynamic_reconfigure/Config]* /move_base/global_costmap/static_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /move_base/global_costmap/static_layer/parameter_updates [dynamic_reconfigure/Config]* /move_base/goal [move_base_msgs/MoveBaseActionGoal]* /move_base/local_costmap/costmap [nav_msgs/OccupancyGrid]* /move_base/local_costmap/costmap_updates [map_msgs/OccupancyGridUpdate]* /move_base/local_costmap/footprint [geometry_msgs/PolygonStamped]* /move_base/local_costmap/inflation_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /move_base/local_costmap/inflation_layer/parameter_updates [dynamic_reconfigure/Config]* /move_base/local_costmap/obstacle_layer/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /move_base/local_costmap/obstacle_layer/parameter_updates [dynamic_reconfigure/Config]* /move_base/local_costmap/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /move_base/local_costmap/parameter_updates [dynamic_reconfigure/Config]* /move_base/parameter_descriptions [dynamic_reconfigure/ConfigDescription]* /move_base/parameter_updates [dynamic_reconfigure/Config]* /move_base/result [move_base_msgs/MoveBaseActionResult]* /move_base/status [actionlib_msgs/GoalStatusArray]* /rosout [rosgraph_msgs/Log]Subscriptions: * /clock [rosgraph_msgs/Clock]* /map [nav_msgs/OccupancyGrid]* /move_base/cancel [unknown type]* /move_base/global_costmap/footprint [geometry_msgs/PolygonStamped]* /move_base/goal [move_base_msgs/MoveBaseActionGoal]* /move_base/local_costmap/footprint [geometry_msgs/PolygonStamped]* /move_base_simple/goal [geometry_msgs/PoseStamped]* /odom [nav_msgs/Odometry]* /scan [sensor_msgs/LaserScan]* /tf [tf2_msgs/TFMessage]* /tf_static [tf2_msgs/TFMessage]Services: * /move_base/DWAPlannerROS/set_parameters* /move_base/NavfnROS/make_plan* /move_base/clear_costmaps* /move_base/get_loggers* /move_base/global_costmap/inflation_layer/set_parameters* /move_base/global_costmap/obstacle_layer/set_parameters* /move_base/global_costmap/set_parameters* /move_base/global_costmap/static_layer/set_parameters* /move_base/local_costmap/inflation_layer/set_parameters* /move_base/local_costmap/obstacle_layer/set_parameters* /move_base/local_costmap/set_parameters* /move_base/make_plan* /move_base/set_logger_level* /move_base/set_parameterscontacting node http://wsc-pc:41569/ ...
Pid: 12968
Connections:* topic: /rosout* to: /rosout* direction: outbound* transport: TCPROS* topic: /cmd_vel* to: /gazebo* direction: outbound* transport: TCPROS* topic: /move_base/goal* to: /move_base* direction: outbound* transport: INTRAPROCESS* topic: /move_base/NavfnROS/plan* to: /rviz* direction: outbound* transport: TCPROS* topic: /move_base/local_costmap/footprint* to: /rviz* direction: outbound* transport: TCPROS* topic: /move_base/DWAPlannerROS/global_plan* to: /rviz* direction: outbound* transport: TCPROS* topic: /move_base/DWAPlannerROS/local_plan* to: /rviz* direction: outbound* transport: TCPROS* topic: /clock* to: /gazebo (http://wsc-pc:33887/)* direction: inbound* transport: TCPROS* topic: /tf* to: /gazebo (http://wsc-pc:33887/)* direction: inbound* transport: TCPROS* topic: /tf* to: /robot_state_publisher (http://wsc-pc:41277/)* direction: inbound* transport: TCPROS* topic: /tf* to: /amcl (http://wsc-pc:46609/)* direction: inbound* transport: TCPROS* topic: /tf_static* to: /robot_state_publisher (http://wsc-pc:41277/)* direction: inbound* transport: TCPROS* topic: /move_base_simple/goal* to: /rviz (http://wsc-pc:45675/)* direction: inbound* transport: TCPROS* topic: /map* to: /map_server (http://wsc-pc:33113/)* direction: inbound* transport: TCPROS* topic: /scan* to: /gazebo (http://wsc-pc:33887/)* direction: inbound* transport: TCPROS* topic: /odom* to: /gazebo (http://wsc-pc:33887/)* direction: inbound* transport: TCPROS* topic: /move_base/goal* to: /move_base (http://wsc-pc:41569/)* direction: inbound* transport: INTRAPROCESS

3、在地图中定位机器人

这一步需要启动以下文件：

需要启动的文件

roslaunch turtlebot3_gazebo turtlebot3_world.launch

roslaunch turtlebot3_navigation turtlebot3_navigation.launch

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

其中 turtlebot3_navigation turtlebot3_navigation.launch 启动文件中启动了map_server 、amcl 、move_base等节点，注意根据自己情况配置好map_file参数（map文件位置），否则会报找不到地图文件。另外，启动move_base节点时一开始报了一个错误“costmap params Invalid argument "/map" passed to canTransform”，根据错误提示，解决方法是，参数配置文件 global_costmap_params.yaml 与 global_costmap_params.yaml 中类似于 “global_frame: /map ” 改为 “global_frame: map ”，两个文件中各需要修改两处地方。

<launch><!-- Arguments --><arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/><arg name="map_file" default="$(find turtlebot3_navigation)/maps/map.yaml"/><arg name="open_rviz" default="true"/><!-- Turtlebot3 --><include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch"><arg name="model" value="$(arg model)" /></include><!-- Map server --><node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/><!-- AMCL --><include file="$(find turtlebot3_navigation)/launch/amcl.launch"/><!-- move_base --><include file="$(find turtlebot3_navigation)/launch/move_base.launch"><arg name="model" value="$(arg model)" /></include><!-- rviz --><group if="$(arg open_rviz)"> <node pkg="rviz" type="rviz" name="rviz" required="true"args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/></group>
</launch>

为了使算法收敛，初始位置误差不要太大，初始误差大小可以看激光传感器数据与地图障碍物信息的位置误差，启动roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch手动驱动机器人行走，可以使得观察数据多样化加速amcl的收敛。具体操作过程看下面的动图。

4、在rivz中导航

以下是在rivz中导航操作过程的动图：

整个系统节点交连图如下：

5、编写代码导航

通过编程实现两个目标点间来回巡逻的机器人导航代码及注释如下：

#!/usr/bin/env python
#-*- coding:utf-8   -*-import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import tf# 巡逻点
waypoints=[[(1.6,0.5,0.0),(0.0,0.0,100.0)],[(-1.8,-0.6,0.0),(0.0,0.0,180.0)]
]
def goal_pose(pose):goal_pose=MoveBaseGoal()goal_pose.target_pose.header.frame_id="map"goal_pose.target_pose.pose.position.x=pose[0][0]goal_pose.target_pose.pose.position.y=pose[0][1]goal_pose.target_pose.pose.position.z=pose[0][2]# r, p, y  欧拉角转四元数x,y,z,w=tf.transformations.quaternion_from_euler(pose[1][0],pose[1][1],pose[1][2])goal_pose.target_pose.pose.orientation.x=xgoal_pose.target_pose.pose.orientation.y=ygoal_pose.target_pose.pose.orientation.z=zgoal_pose.target_pose.pose.orientation.w=wreturn goal_poseif __name__ == "__main__": #节点初始化rospy.init_node('patrol')#创建MoveBaseAction clientclient=actionlib.SimpleActionClient('move_base',MoveBaseAction)#等待MoveBaseAction server启动client.wait_for_server()while not rospy.is_shutdown():for pose in waypoints:goal=goal_pose(pose)client.send_goal(goal)client.wait_for_result()

下面是巡逻时的动图：