智能机器人技术 课件 项目17智能机器人自主导航

授课老师：连国云项目17：智能机器人自主导航《智能机器人技术》课程任务要求知识导入任务实施任务评价利用上一个任务中通过SLAM构建的地图，实现智能机器人从出发点到目标位置的自主导航。任务要求知识导入任务实施任务评价机器人的定位机器人要能够自主的实现从出发位置到达目标位置，主要涉及三个关键问题：第一个是如何对外部环境构建出准确的地图问题第二个是机器人自身的实时定位问题第三个是机器人根据地图进行导航规划问题任务要求知识导入任务实施任务评价机器人的定位对于机器人的定位问题，根据面向的不同场景，可以分为局部定位(locallocation)、全局定位(globallocation)、机器人绑架问题(kidnappedrobotproblem)三种类型目前这三种定位问题的解决主要是依靠概率仿真的方法进行解决任务要求知识导入任务实施任务评价机器人的定位在SLAM中，机器人一般是通过里程计(odom)的提供的运动参数解决定位问题的。但在实际中里程计提供的相关数据存在一定的误差，此外里程计的误差会随着持续运动进行累积，因此这种定位方式只适合在初期建图时使用，而无法在导航过程中提供精确的、实时的定位信息，因此需要鲁棒性更高的定位算法来满足机器人自主导航的需要任务要求知识导入任务实施任务评价AMCL定位算法AMCL(AdaptiveMonteCarlolocalization)，自适应蒙特卡洛定位算法，是一种基于概率的定位算法，可以实现机器人在二维环境中有效定位，它采用自适应(或者KLD采样)蒙特卡洛方法进行定位，并且它使用粒子滤波对机器人在已知的地图中进行位姿跟踪任务要求知识导入任务实施任务评价AMCL定位算法

任务要求知识导入任务实施任务评价AMCL定位算法

任务要求知识导入任务实施任务评价导航规划原理导航规划也就是路径规划，路径规划算法经常被认为是一种图形搜索算法，智能机器人要进行导航规划必须首先完成对工作环境的建模，即路径规划的第一步是构图，构图完成后，第二步则是规划。最经典的一类规划方法是图搜索方法，如Dijkstra算法，A*算法任务要求知识导入任务实施任务评价导航规划原理智能机器人的环境地图常见的描述方法主要有栅格地图、可视地图等几种方式。栅格地图是将机器人所处在的工作环境进行栅格化，将环境按照一定的长度划分为一个个小栅格，由栅格的状态表示环境信息的可通过或者有障碍物可视地图将环境中的障碍物进行凸多边形建模表示，然后将凸多边形的各个顶点相互连接，对每条连线进行判断：智能机器人是否可以安全通过赋予不同的权值任务要求知识导入任务实施任务评价导航规划原理任务要求知识导入任务实施任务评价导航规划原理智能机器人的全局路径规划是基于对环境的先验地图在运动空间内利用某种评价方法寻找到最优路径的过程。最优路径的选择首要条件是保证安全，即机器人运动时与障碍物无碰前提下的评价最优路径常见的全局路径算法主要有迪杰斯特拉算法(Dijkstra)、A*算法(AStar）、蚁群算法、人工势场法等以及对这些算法的改进任务要求知识导入任务实施任务评价Dijkstra算法Dijkstra算法是一种经典的求解最短路径的算法，它是一种广度优先的搜索算法，用于计算一个节点去往其他各个不相关节点的最小移动代价。此算法的思想是把在图中无论如何出现的所有节点分离维2组，第1组包含被准确认定是最短路径的节点，第2组存放待检查的不确定的节点任务要求知识导入任务实施任务评价Dijkstra算法根据最小移动代价逐渐增大的顺序，逐个将第2组需要检查的节点经过检验后加入到第1组中，从起始点开始移动机器人目标方向所能前往的所有路径点全被收纳于第1组中任务要求知识导入任务实施任务评价A*算法A*算法是启发式搜索算法，用于在静态二维构型空间中计算最优路径。它结合了经典的Dijkstra算法和启发式最佳搜索算法(BFS)。依据成本评价函数进行搜索，将搜索到满足成本评价函数的点作为下一次要搜索的点，并重复此过程，直到找到目标点，形成最优路径任务要求知识导入任务实施任务评价A*算法

任务要求知识导入任务实施任务评价导航中的代价地图机器人在仅含有布尔信息的栅格地图中运动并不是十分安全，为了改善路径规划的环境而对原始地图进行加工生成的新地图称为代价地图(costmap)任务要求知识导入任务实施任务评价导航中的代价地图按照构造方式的不同，costmap可以分为单源costmap和多层costmap单源costmap，将所有的静态地图和传感器的数据都存在一个单元格中，所有的路径规划的信息也在同一张costmap上进行任务要求知识导入任务实施任务评价导航中的代价地图多层costmap借助每个子层有规律地协同维护一个层级列表，追踪与本层有关的特定功能的数据，将相关的数据进行更新，然后存储在本层私有的costmap中然后将各个子层级按照特定的顺序和方式将本层的子层级costmap整合到主costmap中任务要求知识导入任务实施任务评价导航功能包从导航功能实现的空间范围来讲，可以分为二维空间导航和三维空间导航，目前较为成熟的是二维空间导航ROS系统主要提供了针对二维空间的导航功能包，其基本原理就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令任务要求知识导入任务实施任务评价导航功能包在ROS中，进行导航需要使用到的三个包是：(1)SLAM相关功能包：根据不同算法，ROS系统提供不同的功能包，根据激光数据(或者深度数据模拟的激光数据)建立地图(2)move_base功能包：根据参照的消息进行路径规划，使移动机器人到达指定的位置(3)amcl功能包：根据已经有的地图进行定位任务要求知识导入任务实施任务评价导航功能包任务要求知识导入任务实施任务评价导航功能包机器人将传感器数据和导航的目标位置信息发送到ROS系统，ROS系统通过相应的计算，转换为实时的运动控制信号，实现机器人的自主导航。在该框架中，move_base功能包提供导航的主要运行、交互接口。为了保障导航路径的准确性，ROS系统还需使用amcl功能包实现对机器人自身位置的精确定位任务要求知识导入任务实施任务评价导航功能包ROS系统涉及多个与导航功能相关的功能包，可以直接通过以下命令直接按照ROS系统的导航框架，实现多个功能包的安装：$sudoapt-getinstallros-melodic-navigationROS系统导航框架中，涉及建图、导航、定位三种功能包任务要求知识导入任务实施任务评价amcl功能包ROS系统中基于amcl定位算法提供了amcl功能包，该功能包可以实现机器人在任意状态下推算自己在地图中的定位任务要求知识导入任务实施任务评价amcl功能包中的topictopic名称msgs类型功能topicpublishamcl_posegeometry_msgs/PoseWithCovarianStamped机器人在地图中的位姿估计，带有协方差信息particlecloudgeometry_msgs/PoseArray粒子滤波器维护的位姿估计集合Tftf/tfMessage发布从odom(可以使用参数~odom_frame_id进行重映射)到map的转换topicsubscriberScansensor_msgs/LaserScan激光雷达数据Tftf/tfMessage坐标变换信息initiposegeometry_msgs/PoseWithCovarianceStamped用来初始化粒子滤波器的均值和协方差mapnav_msgs/OccupancyGrid设置use_map_topic参数时，amcl订阅map话题以获取地图数据，用于激光定位任务要求知识导入任务实施任务评价amcl功能包中的serviceservice名称srv类型功能serviceserverglobal_localizationstd_srvs/Empty初始化全局定位，所有粒子被随机撒在地图上的空闲区域request_nomotion_updatestd_srvs/Empty手动执行更新并发布更新的粒子serviceclientstatic_mapnav_msgs/GetMapamcl调用该服务来获取地图数据任务要求知识导入任务实施任务评价amcl功能包中的parameterparameter名数据类型默认值功能描述总体过滤器参数~min_particlesint100允许的最少粒子数~max_particlesint5000允许的最多粒子数~kld_errdouble0.01真实分布与估计分布之间的最大误差~kld_zdouble0.99(1-p)的上标准正常分位数，其中p是估计分布误差小于kld_eer的概率~update_min_ddouble0.2(m)执行一次滤波器更新所需的平移距离任务要求知识导入任务实施任务评价amcl功能包中的parameterparameter名数据类型默认值功能描述~update_min_adoubleπ/6.0(rad)执行一次滤波器更新所需的旋转角度~resample_intervalint2重采样之前滤波器的更新次数~transform_tolerancedouble0.1(s)发布变换的时间，以指示此变换在未来有效~recovery_alpha_slowdouble0.0慢速平均权重滤波器的指数衰减率，用于决定何时通过添加随机姿态进行恢复操作，0.0表示禁用~recovery_alpha_fastdouble0.0快速平均权重滤波器的指数衰减率，用于决定何时通过添加随机姿态进行恢复操作，0.0表示禁用任务要求知识导入任务实施任务评价amcl功能包中的parameterparameter名数据类型默认值功能描述~initial_pose_xdouble0.0(m)初始姿态平均值(x)，用于初始化高斯分布滤波器~initial_pose_ydouble0.0(m)初始姿态平均值(y)，用于初始化高斯分布滤波器~initial_pose_adouble0.0(m)初始姿态平均值(yaw)，用于初始化高斯分布滤波器~initial_cov_xxdouble0.5*0.5(m)初始姿态协方差(x*x)，用于初始化高斯分布滤波器~initial_cov_yydouble0.5*0.5(m)初始姿态协方差(y*y)，用于初始化高斯分布滤波器~initial_cov_aadouble(π/12)*(π/12)(rad)初始姿态协方差(yaw*yaw)用于初始化高斯分布滤波器任务要求知识导入任务实施任务评价amcl功能包中的parameterparameter名数据类型默认值功能描述~gui_publish_ratedouble-1.0(Hz)可视化时，发布信息的最大速率-1.0表示禁用~save_pose_ratedouble0.5(Hz)参数服务器中的存储姿态估计(~initial_pose_)和协方差(~initial_cov_)的最大速率，用于后续初始化过滤器-1.0表示禁用~use_map_topicboolfalse当设置为true时，amcl将订阅地图话题，而不是通过服务调用接收地图~first_map_onlyboolfalse当设置为true时，amcl将只使用它订阅的第一个地图，而不是每次更新接收到的地图任务要求知识导入任务实施任务评价amcl功能包中的parameterparameter名数据类型默认值功能描述激光模型参数~laser_min_rangedouble-1.0最小扫描范围~laser_max_rangedouble-1.0最大扫描范围~laser_max_beamsint30更新过滤器时要在每次扫描中使用多少均匀间隔的光束~laser_z_hitdouble0.95模型z_hit部分的混合参数~laser_z_shortdouble0.1模型z_short部分的混合参数~laser_z_maxdouble0.05模型z_max部分的混合参数任务要求知识导入任务实施任务评价amcl功能包中的parameterparameter名数据类型默认值功能描述~laser_z_randdouble0.05模型z_rand部分的混合参数~laser_sigma_hitdouble0.2(m)模型z_hit部分中使用的高斯模型的标准偏差~laser_lambda_shortdouble0.1模型z_short部分的指数衰减参数~laser_likelihood_max_distdouble2.0m地图上测量障碍物膨胀的最大距离~laser_model_typestring“likelihood_field”模型选择，beam、likelihood_field或likelihood_field_prob任务要求知识导入任务实施任务评价amcl功能包中的parameterparameter名数据类型默认值功能描述里程计模型参数~odom_model_typestring“diff”模型选择，diff、omni、diff-corrected或omni-corrected~odom_alpha1double0.2根据机器人运动的旋转分量指定里程计旋转估计中的预期噪声~odom_alpha2double0.2根据机器人运动的平移分量指定里程计旋转估计中的预期噪声~odom_alpha3double0.2根据机器人运动的平移分量指定里程计平移估计中的预期噪声~odom_alpha4double0.2根据机器人运动的旋转分量指定里程计平移估计中的预期噪声任务要求知识导入任务实施任务评价amcl功能包中的parameterparameter名数据类型默认值功能描述~odom_alpha5double0.2平移相关的噪声参数(仅在模型omni中使用)~odom_frame_idstring“odom”里程计的坐标系~base_frame_idstring“base_link”机器人底盘的坐标系~global_frame_idstring“map”定位系统发布的坐标系~tf_broadcastbooltrue设置为false时，amcl不会发布map与odom之间的坐标变换任务要求知识导入任务实施任务评价amcl功能包不同的传感器提供了机器人的运动信息，从而根据这些信息可以获知机器人本体的位置，但由于不同传感器的安装位置以及机器人本体的中心位置直接存在差别，因此需要将这些信息进行tf变换任务要求知识导入任务实施任务评价move_base功能包move_base是ROS系统中完成路径规划的功能包，主要包括全局路径规划器(global_planner)和本地实时规划器(local_planner)组成此外move_base功能包还将根据计算得到路径和机器人实时的运动控制数据，通过发布cmd_vel话题将计算得到的实时运动控制数据发送到机器人的运动控制器中，使机器人按照规划得到的轨迹路径产生实际的运动任务要求知识导入任务实施任务评价move_base功能包发布/订阅的topictopic名称msgs类型功能topicpublishcmd_velgeometry_msgs/Twist输出到机器人底盘的速度命令topicsubscribermove_base_simple/goalgeometry_msgs/PoseStamped为无需追踪目标执行状态的用户，提供一个非action接口任务要求知识导入任务实施任务评价move_base功能包有关的serviceservice名称srv类型功能service~make_plannav_msgs/GetPlan允许用户从move_base获取给定目标的路径规划，但不会执行该路径规划~clear_unknown_spacestd_srvs/Empty允许用户直接清除机器人周围的未知空间。适合costmap停止很长时间后，在一个全新环境中重新启动时使用~clear_costmapsstd_srvs/Empty允许用户命令move_base节点清除costmap中的障碍。这可能会导致机器人撞上障碍物，请谨慎使用任务要求知识导入任务实施任务评价move_base功能包相关的actionaction名称类型功能actionpublishmove_base/feedbackmove_base_msgs/MoveBaseActionFeedback反馈信息，含有机器人底盘的坐标move_base/statusactionlib_msgs/GoalStatusArray发送到move_base的目标状态信息move_base/resultmove_base_msgs/MoveBaseActionResult此处move_base操作的结果为空actionsubscribermove_base/goalmove_base_msgs/MoveBaseActionGoalmove_base的运动规划目标move_base/cancelactionlib_msgs/GoalID取消特定目标的请求任务要求知识导入任务实施任务评价move_base功能包相关的actionparameter名数据类型默认值功能描述~base_global_plannerstring“navfn/NavfnROS”设置move_base的全局路径规划器的插件名称~base_local_plannerstring“base_local_planner/Tra-jectoryPlannerROS”设置move_base的局部路径规划器的插件名称~recovery_benaviorslist[{name:conservative_reset,type:clear_costmap_recovery/ClearCostmapRecovery},{name:rotate_recovery/Rotate-Recovery},{name:aggressive_reset,type:clear_costmap_recovery/ClearCostmapRecovery}]设置move_base的恢复操作插件列表，当move_base不能找到有效的路径规划时，将按照这里指定的顺序执行操作任务要求知识导入任务实施任务评价move_base功能包相关的actionparameter名数据类型默认值功能描述~controller_frequencydouble20.0(Hz)发布控制指令的循环周期以该周期向机器人底盘发送命令~planner_patiencedouble5.0(s)空间清理操作执行前路径规划器等待有效规划的时间~controller_patiencedouble15.0(s)空间清理操作执行前控制器等待有效控制命令的时间~conservative_reset_distdouble3.0(m)在地图中清理空间时距机器人该范围的障碍将从costmap中清除~recovery_behavior_enabledbooltrue是否启用move_base恢复机制来清理空间任务要求知识导入任务实施任务评价move_base功能包相关的actionparameter名数据类型默认值功能描述~clearing_rotation_allowedbooltrue清理空间操作时，机器人是否采用原地旋转的运动方式~shutdown_costmapsboolfalse当move_base进入inactive状态时，是否停用节点的costmap~oscillation_timeoutdouble0.0(s)执行恢复操作之前允许的震荡时间，0代表永不超时~oscillation_distancedouble0.5机器人需要移动该距离才可当作没有震荡移动完毕后重置定时器参数~oscillation_timeout~planner_frequencydouble0.0全局路径规划器循环速率。如果设置为0.0，当收到新目标点或者局部路径规划器上报路径不通时全局路径规划器才会启动~max_planning_retriesint-1恢复操作之前尝试规划的次数，-1代表无上限的不断尝试任务要求知识导入任务实施任务评价导航功能实现amcl功能包和move_base功能包是ROS系统中实现导航功能核心功能包，这两个功能包提供实现导航功能必要的计算方法和软件接口，因此在ROS系统实现导航功能主要就是调用这两个功能包任务要求知识导入任务实施任务评价amcl的调用在amcl功能包中实现定位的node是amcl，因此实现amcl定位最核心的就是调用该node。amcl定位的实现除了启动amcl节点之外，还需要对AMCL算法中的大量参数进行设置，主要包括最大粒子数max_particles，最小粒子数min_particles，真实分布与估计分布之间的最大误差kld_err，滤波器更新所需的平移距离update_min_d，滤波器更新所需的旋转角度update_min_a等参数任务要求知识导入任务实施任务评价move_base的调用在move_base功能包中实现导航的node是move_base，因此实现导航与路径规划最核心的就是调用该node。在调用move_base功能包时除了要像amcl功能包调用一样，对相关参数进行设置之外，还需要使用参数服务器(parameterserver)，实现参数的全局调用任务要求知识导入任务实施任务评价位置标识操作机器人进行自主导航除了要进行路径规划和定位之外，明确机器人的初始位置和导航目标位置也是必不可少的操作环节。ROS系统中已经提供了便利的可视化工具，使用RVIZ进行机器人的初始位置和导航目标位置标识是目前较为主要的操作方法任务要求知识导入任务实施任务评价位置标识操作任务要求知识导入任务实施任务评价位置标识操作启动ROS系统的导航节点之后，系统本身会默认机器人放在SLAM建图时的原点上，这时机器人的位置是不准确的，为了尽快使机器人定位，可以手动的设定机器人的初始位置任务要求知识导入任务实施任务评价位置标识操作在RVIZ软件的左上角菜单栏上有一个2DPoseEstimate

菜单按钮，点击该按钮，将其移动到给定地图中实际机器人所在的位置，并调整机器人的正确朝向，然后释放鼠标，这样就完成了一次手动设置机器人初始位置的操作可以通过机器人的键盘控制节点等方式，移动机器人位置和调整