简介：这份资源面向机器人自主导航方向的初学者与进阶开发者，聚焦地图环境已知条件下的全局路径规划与运动控制问题。包内以Matlab2018实现的RRT与双向RRT算法为核心，规划结果以点列形式写入txt文件，再由VS2013工程读取点列数据，通过一种较为粗糙的控制方法驱动机器人从起始点运动至目标点，完整串联了算法仿真与工程落地两个环节。资源共89个文件，包含m脚本、cpp与h源码、exe可执行程序、txt数据与说明文档、mat地图数据以及pdb、tlog等编译调试中间文件，压缩包约76.6MB，目录按路径规划与客户端控制两大模块组织。目前已有770人学习下载。读者可据此获得可直接运行的RRT与双向RRT对比实现、规划点列与地图数据、上位机控制示例代码及程序运行说明文档，便于快速复现实验、对比两种算法效果并在此基础上扩展自己的导航方案。
1. 地图已知的机器人自主导航程序：从加载地图到跑完全程的落地路径
很多团队第一次做机器人自主导航，卡住的地方往往不是算法本身，而是地图加载完、定位跳变、路径规划器一启动就报错。标题里的“基于地图已知”意味着环境地图已经提前建好，程序不需要边跑边建图，核心任务变成三件事：让机器人知道自己在哪、让规划器算出一条可通行的路径、让底盘控制器稳定跟踪这条路径。这套方案适合已经拿到二维栅格地图、想在仿真或实机上快速跑通导航栈的开发者，也适合需要把导航模块集成进更大系统的工程团队。我一般会先把整条链路拆成地图服务、定位、全局规划、局部规划、代价地图五块，再逐块验证，而不是一上来就调参。
2. 导航程序的最小系统拆解：五个节点和它们的数据流
2.1 为什么先跑通数据流再谈调参
地图已知的导航程序，本质是一组节点通过话题和服务交换数据。地图服务发布静态栅格，定位模块发布机器人在地图坐标系下的位姿，全局规划器根据目标点算出一条从当前位置到目标点的路径，局部规划器结合实时障碍物生成速度指令，底盘驱动执行。很多新手一上来就改局部规划器的权重，结果发现机器人连动都不动，原因是定位模块根本没发布位姿，或者地图坐标系和机器人坐标系之间的变换树断了。常见做法是先用一个简单的目标点发布工具，观察规划器有没有输出路径，再检查速度指令有没有发到底盘。这一步不需要写代码，但需要把每个话题的发布频率和坐标系关系确认清楚。
2.2 用 launch 文件把五个节点串起来
下面是一个最小可跑的 launch 文件骨架，基于常见的机器人操作系统导航栈。它假设你已经有一张二维栅格地图文件，并且底盘驱动已经能接收速度指令。
<launch> <!-- 地图服务：加载提前建好的栅格地图 --> <node name="map_server" pkg="map_server" type="map_server" args="$(find my_robot_nav)/maps/office.yaml"/> <!-- 定位模块：用自适应蒙特卡洛定位，输入激光和地图 --> <node name="amcl" pkg="amcl" type="amcl" output="screen"> <param name="odom_frame_id" value="odom"/> <param name="base_frame_id" value="base_link"/> <param name="global_frame_id" value="map"/> <param name="laser_topic" value="scan"/> <param name="min_particles" value="500"/> <param name="max_particles" value="3000"/> </node> <!-- 全局规划器：在静态地图上算最短路径 --> <node name="global_planner" pkg="navfn" type="navfn_node" output="screen"> <param name="global_frame" value="map"/> <param name="robot_base_frame" value="base_link"/> <param name="allow_unknown" value="false"/> </node> <!-- 局部规划器：结合实时障碍物生成速度指令 --> <node name="local_planner" pkg="base_local_planner" type="trajectory_planner_ros" output="screen"> <param name="max_vel_x" value="0.6"/> <param name="min_vel_x" value="0.0"/> <param name="max_vel_theta" value="1.0"/> <param name="sim_time" value="1.5"/> </node> <!-- 代价地图：把静态地图和实时障碍物合并 --> <node name="costmap" pkg="costmap_2d" type="costmap_2d_node" output="screen"> <rosparam file="$(find my_robot_nav)/config/costmap_common.yaml" command="load" ns="global_costmap"/> <rosparam file="$(find my_robot_nav)/config/costmap_common.yaml" command="load" ns="local_costmap"/> </node> </launch>这段 launch 的逻辑是：地图服务只发布一次静态地图，定位模块持续订阅激光话题并输出 map 到 odom 的变换，全局规划器订阅地图和定位结果，局部规划器订阅代价地图和当前速度，最终输出 cmd_vel。参数说明上，min_particles 和 max_particles 控制定位粒子数，太小会导致定位跳变，太大会吃满 CPU；max_vel_x 是局部规划器允许的最大前进速度，实机上建议先从 0.3 开始试；sim_time 是局部规划器向前模拟的时间长度，太短会反应迟钝，太长会算得慢。代价地图的配置文件里，inflation_radius 决定障碍物膨胀范围，一般设成机器人内切圆半径加 0.1 米左右。
2.3 验证数据流的三个命令
启动 launch 后，不要急着发目标点。先开三个终端，分别执行下面三条命令，确认数据流是通的。
# 查看地图是否发布，应该看到 map 话题有数据 rostopic echo /map --noarr | head -n 5 # 查看定位输出，应该看到 map 到 odom 的变换持续更新 rosrun tf tf_echo map odom # 查看速度指令，手动发一个目标点后应该有输出 rostopic echo /cmd_vel如果 tf 报错说找不到 map 到 odom 的变换，说明定位模块没起来或者激光话题名字不对。如果 cmd_vel 一直为零，先检查局部规划器有没有收到代价地图，再看全局规划器有没有算出路径。这三个命令能排掉八成“机器人不动”的问题。
3. 全局规划与局部规划的选型：什么时候用 A*，什么时候用 DWA
3.1 全局规划器的三种常见选择
地图已知的场景下，全局规划器负责在静态栅格上算一条从当前位置到目标点的路径。常见做法有三种：基于 Dijkstra 的 navfn、基于 A* 的 global_planner、以及支持更复杂运动学约束的 hybrid A*。如果机器人是差速底盘，navfn 和 global_planner 都够用，差别在于 global_planner 可以切换成 A* 模式，搜索速度更快，但路径可能不够平滑。如果机器人是阿克曼底盘，全局路径必须满足最小转弯半径，这时候 hybrid A* 更合适，但配置复杂度会明显上升。我一般先在仿真里用 global_planner 的 A* 模式跑通，再根据底盘类型决定要不要换。
下面是一个 global_planner 的参数配置片段，放在 costmap 配置同级目录下。
GlobalPlanner: use_dijkstra: false # false 表示用 A*，true 表示用 Dijkstra use_quadratic: true # 用二次近似让路径更平滑 use_grid_path: false # false 表示生成平滑路径，true 表示沿栅格走 allow_unknown: false # 不允许穿过未知区域 default_tolerance: 0.2 # 目标点容差，单位米 cost_factor: 0.8 # 代价因子，影响路径偏向障碍物的程度use_dijkstra 设成 false 后，规划器会用 A* 搜索，在大部分室内地图上比 Dijkstra 快一倍左右。use_quadratic 打开后，路径会做二次插值，机器人走起来更顺，但可能离障碍物更近。allow_unknown 在已知地图场景下建议关掉，避免机器人往没建图的地方跑。default_tolerance 是目标点容差，设太小会导致规划器认为目标点不可达，设太大会让机器人停在离目标点很远的地方。
3.2 局部规划器的参数怎么调
局部规划器负责把全局路径拆成一条条速度指令。差速底盘最常用的是 DWA 和 TEB，DWA 计算量小、参数直观，TEB 路径更优但调参复杂。新手建议先用 DWA 跑通，再考虑换 TEB。DWA 的核心参数有四个：max_vel_x、max_vel_theta、sim_time、path_distance_bias。max_vel_x 和 max_vel_theta 决定机器人最快能跑多快，实机上先从低速开始。sim_time 决定向前模拟多久，室内场景 1.5 到 2.0 秒比较合适。path_distance_bias 决定机器人更愿意贴着全局路径走还是更愿意绕开障碍物，设大了会贴着路径走但容易撞障碍物，设小了会绕来绕去但更安全。
DWAPlannerROS: max_vel_x: 0.6 min_vel_x: 0.0 max_vel_theta: 1.0 min_vel_theta: -1.0 acc_lim_x: 0.5 acc_lim_theta: 1.0 sim_time: 1.5 path_distance_bias: 32.0 goal_distance_bias: 24.0 occdist_scale: 0.02 forward_point_distance: 0.325 scaling_speed: 0.25 max_scaling_factor: 0.2acc_lim_x 和 acc_lim_theta 是加速度限制，设太大机器人会猛冲，设太小会反应迟钝。occdist_scale 是障碍物代价权重，设大了机器人会离障碍物很远，设小了容易擦碰。forward_point_distance 是前视距离，一般设成机器人半径的两倍左右。scaling_speed 和 max_scaling_factor 控制接近目标点时降速，避免冲过头。
3.3 代价地图的膨胀半径怎么定
代价地图把静态地图和实时障碍物合并成一张代价栅格，膨胀半径决定了障碍物周围多大范围内代价升高。膨胀半径设成机器人内切圆半径加 0.1 米，能保证机器人不会撞上障碍物。如果设得太大，窄通道会直接被堵死，规划器认为不可通行。如果设得太小，机器人会贴着障碍物走，实机上容易因为定位误差撞上去。我一般先用机器人内切圆半径加 0.15 米试，跑一圈看窄通道能不能过，再微调。
global_costmap: inflation_radius: 0.55 cost_scaling_factor: 3.0 robot_radius: 0.35 local_costmap: inflation_radius: 0.45 cost_scaling_factor: 5.0 robot_radius: 0.35cost_scaling_factor 控制代价随距离衰减的速度，设大了代价衰减快，机器人更愿意靠近障碍物，设小了代价衰减慢，机器人会离障碍物更远。全局代价地图的膨胀半径一般比局部大一点，因为全局路径不需要那么贴近障碍物。
4. 避坑与排查：导航程序跑不起来时先看这五条
4.1 现象：机器人原地打转，不往目标点走
原因通常是定位模块输出的位姿跳变，或者全局规划器算出的路径方向反了。先看 tf 里 map 到 odom 的变换是不是连续，如果跳变，检查激光话题是不是和定位模块配置的一致。再看全局路径的第一个点和最后一个点，如果方向反了，检查目标点发布时的坐标系是不是 map。解决方法是把定位模块的 laser_topic 参数改成实际激光话题名，并确认目标点是在 map 坐标系下发布的。
4.2 现象：规划器报错说目标点不可达
原因可能是目标点落在障碍物上，或者代价地图的膨胀半径太大把目标点周围堵死了。先在地图上看目标点是不是在黑色栅格上，如果是，换一个空闲栅格。如果目标点在白色区域但规划器仍说不可达，把膨胀半径调小 0.1 米再试。还有一种情况是全局规划器的 default_tolerance 设得太小，目标点稍微偏一点就认为不可达，适当调大到 0.3 米。
4.3 现象：机器人走一半突然停下，速度指令归零
原因通常是局部规划器在代价地图里找不到可行轨迹，或者底盘驱动超时。先看局部代价地图里机器人周围是不是被障碍物填满了，如果是，检查激光是不是被遮挡或者障碍物膨胀半径太大。再看底盘驱动的超时时间，如果速度指令发布频率低于底盘要求的频率，底盘会主动停下。解决方法是提高局部规划器的发布频率，或者把底盘超时时间调大。
4.4 现象：机器人贴着墙走，偶尔擦碰
原因是膨胀半径太小或者 occdist_scale 太小。先把膨胀半径加 0.05 米，再把 occdist_scale 从 0.02 调到 0.05 左右。如果还擦碰，检查定位精度，定位跳变会导致机器人实际位置和规划位置不一致。实机上建议把 max_vel_x 降到 0.3 以下，先保证不撞，再慢慢提速。
4.5 现象：仿真里跑得好，实机上直接失控
原因通常是仿真里的里程计是完美的，实机上里程计有漂移和噪声。先检查实机里程计的标定，轮子直径和轮距参数不对会导致里程计尺度错误。再看定位模块的粒子数，实机上建议把 min_particles 调到 1000 以上。最后检查速度指令的单位，仿真里可能是米每秒，实机上底盘驱动可能要求毫米每秒，单位不对会直接飞车。
5. 进阶技巧：用录制回放验证导航程序的边界
5.1 录一段激光和里程计，反复回放调参
实机上调参最怕的是每次都要重新跑一遍，效率低还容易撞。我一般会先用 rosbag 录一段包含激光、里程计、tf 的数据，然后在回放模式下调参。回放时定位模块和规划器照常运行，但底盘不动，这样可以在不碰实机的情况下反复试参数。
# 录制激光、里程计和 tf rosbag record /scan /odom /tf /tf_static -O nav_test.bag # 回放时用 --clock 让节点使用 bag 里的时间 rosbag play nav_test.bag --clock # 回放的同时启动导航程序，但把底盘驱动换成假驱动 roslaunch my_robot_nav nav_without_base.launch回放模式下，定位模块会重新计算位姿，规划器会重新算路径，你可以观察不同参数下路径的变化。录一段包含窄通道、直角转弯、动态障碍物的数据，能覆盖大部分边界情况。
5.2 用目标点序列测试全局规划器的稳定性
单点测试只能验证一条路径，多点序列能验证规划器在不同起点和终点下的表现。我一般会在地图上标十个目标点，写一个脚本依次发布，观察每次规划的成功率和路径长度。
#!/usr/bin/env python import rospy from geometry_msgs.msg import PoseStamped # 十个测试目标点，坐标基于地图坐标系 waypoints = [ (1.0, 1.0), (3.0, 1.0), (5.0, 2.0), (4.0, 4.0), (2.0, 5.0), (1.0, 3.0), (3.0, 3.0), (5.0, 5.0), (2.0, 2.0), (4.0, 1.0) ] rospy.init_node('waypoint_tester') pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1) rate = rospy.Rate(0.2) # 每 5 秒发一个目标点 for x, y in waypoints: goal = PoseStamped() goal.header.frame_id = 'map' goal.header.stamp = rospy.Time.now() goal.pose.position.x = x goal.pose.position.y = y goal.pose.orientation.w = 1.0 pub.publish(goal) rospy.loginfo('Published goal: %.1f, %.1f' % (x, y)) rate.sleep()这个脚本每 5 秒发一个目标点，你可以观察机器人是否每次都能规划出路径并到达。如果有某个点反复失败，说明那个区域的地图或者膨胀半径有问题。成功率低于八成时，不要急着上路，先把失败的点单独拿出来分析。
5.3 一个我踩过的坑：地图分辨率不是越高越好
刚做导航的时候，我觉得地图分辨率越高越好，把栅格设成 0.01 米，结果规划器算一次路径要好几秒，局部规划器直接超时。后来改成 0.05 米，规划速度上来了，路径精度也够用。地图分辨率要和激光雷达的角分辨率和测距精度匹配，0.05 米是大部分室内场景的甜点值。如果地图是 0.01 米，代价地图的膨胀半径也要跟着调，否则一个障碍物会膨胀成一大片，窄通道直接过不去。
5.4 验证导航程序的三个硬指标
跑通之后，我一般用三个指标判断这套导航程序能不能上实机：第一，从起点到终点的成功率，在十个目标点序列里至少九次成功；第二，单次规划耗时，全局规划不超过 200 毫秒，局部规划不超过 100 毫秒；第三，实机跑一圈不擦碰，速度可以先设 0.3 米每秒，跑完十圈再提速。这三个指标都过了，再考虑加动态障碍物和复杂场景。希望帮到你。
本文还有配套的精品资源，点击获取
