简介这份资源面向机器人路径规划方向的研究者与开发者聚焦ROS环境下人工势场法与A算法结合的混合规划方案用于解决单一人工势场法易陷入局部极小值、难以稳定抵达目标的问题。压缩包共54个文件约78KB以cpp与h源码为主体配合yaml参数、pgm地图、launch启动文件、xml插件描述及rviz可视化配置构成可编译运行的规划器工程。内容涵盖势场模型定义、A搜索中节点代价评估、ROS节点与传感器及运动控制的交互以及测试调参等环节核心规划器源码结构完整便于读者理解吸引与排斥势场的计算方式、启发式搜索的融合逻辑和插件化封装思路。目前已有6331人学习下载适合希望掌握混合路径规划实现细节、提升机器人自主导航能力的中高级开发者参考借鉴。1. 从一次小车在走廊里“抖”到停不下来讲起如果你在 ROS 里跑过全局 A* 加局部人工势场法的组合大概率见过这个画面小车在 Gazebo 里本来走得好好的一靠近墙角或者窄门速度突然开始高频抖动原地打转最后卡死在离目标不到半米的地方。这不是你参数调得不够玄学而是两套算法在“谁说了算”这件事上没谈拢。A* 负责给一条全局最优的折线人工势场法负责在这条线附近做局部避障但势场本身有个天生缺陷——局部极小值一旦引力场和斥力场在某点抵消机器人就以为到了终点。这份资源要解决的就是把这个组合从“能跑”推到“能稳定跑完”。它适合已经在 Ubuntu 上装好 ROS、写过简单导航节点、但被局部极小值和抖动反复折磨的从业者。下面我按自己拆包复现的顺序把选型理由、参数含义和翻车点一次讲透。2. 为什么是 A* 打底、势场法收尾组合逻辑与工程取舍2.1 全局与局部的分工边界A* 算法在栅格地图上做全局搜索优点是完备性——只要路径存在它一定能找到缺点是它把机器人当质点不考虑运动学约束也不处理动态障碍。人工势场法正好相反它用引力场把机器人拉向目标用斥力场把机器人推离障碍计算量小、响应快适合做局部微调。但单独用势场法做全局规划遇到 U 型障碍就会在凹槽里来回震荡这就是经典的局部极小值问题。把两者串起来的常见做法是A* 先算出一条从起点到终点的全局路径然后在这条路径上取一系列“局部目标点”让势场法去追踪。这样势场法不需要面对整个地图的复杂地形只需要在全局路径附近做小范围避障局部极小值的触发概率大幅降低。但注意这只是降低不是消除。如果全局路径本身贴着障碍走势场法的斥力场会把机器人推离路径反而制造新的震荡。我一般会在 A* 的代价函数里加一个“离障碍距离”的惩罚项让全局路径主动远离障碍物至少一个机器人半径给势场法留出调整空间。这一步不做后面调参就是无底洞。2.2 势场函数的参数怎么定人工势场法的核心是引力场和斥力场的叠加。引力场通常用二次函数斥力场用指数或反比函数。下面是我在 ROS 里常用的一个势场计算片段基于nav_msgs/Path和sensor_msgs/LaserScan做局部更新import numpy as np def attractive_force(robot_pos, goal_pos, k_att1.0): 引力场二次函数k_att 控制引力强度 diff np.array(goal_pos) - np.array(robot_pos) dist np.linalg.norm(diff) if dist 0.1: # 防止目标点附近力突变 return np.zeros(2) return k_att * diff def repulsive_force(robot_pos, obstacles, k_rep0.5, rho_01.5): 斥力场反比函数rho_0 是斥力影响半径 force np.zeros(2) for obs in obstacles: diff np.array(robot_pos) - np.array(obs) dist np.linalg.norm(diff) if dist rho_0 and dist 0.01: # 斥力大小随距离减小而增大 mag k_rep * (1.0/dist - 1.0/rho_0) * (1.0/dist**2) force mag * (diff / dist) return force def total_force(robot_pos, goal_pos, obstacles): return attractive_force(robot_pos, goal_pos) repulsive_force(robot_pos, obstacles)这段代码里三个参数最关键k_att决定机器人多“想”去目标k_rep决定它多“怕”障碍rho_0决定多远开始怕。常见翻车是k_rep给太大机器人在窄道里被两侧斥力夹住直接僵直或者rho_0设得比激光雷达量程还大远处一个噪点就触发斥力。我一般先把k_att固定为 1.0调k_rep从 0.1 开始往上加每次加 0.1直到机器人能贴着障碍绕过去但不抖。rho_0通常设成机器人半径的 2 到 3 倍激光雷达的range_max要留出余量。2.3 全局路径如何喂给局部势场A* 算出的路径是一串离散的栅格中心点直接拿来做局部目标会导致机器人频繁切换目标点速度指令跳变。常见做法是对路径做插值和平滑然后按固定距离间隔取局部目标。下面是一个简单的路径重采样逻辑def resample_path(path, step0.3): 把 A* 路径按 step 米间隔重采样返回局部目标点列表 resampled [path[0]] for i in range(1, len(path)): prev np.array(resampled[-1]) curr np.array(path[i]) dist np.linalg.norm(curr - prev) if dist step: # 在两点之间线性插值 direction (curr - prev) / dist new_point prev direction * step resampled.append(new_point.tolist()) if resampled[-1] ! path[-1]: resampled.append(path[-1]) return resampledstep这个参数直接决定局部目标的密度。太小机器人频繁换目标速度指令抖太大势场法来不及避障就冲过头。我一般设成 0.2 到 0.5 米具体看机器人最大速度和激光雷达更新频率。如果激光雷达是 10Hz机器人最大速度 0.5m/s那step至少 0.05 米才够但实际用 0.3 米左右更稳因为势场法本身有惯性。提示重采样后的路径点要发布成nav_msgs/Path让局部规划器订阅而不是直接塞进势场函数。这样你可以在 RViz 里看到局部目标点的分布调参时心里有数。3. 在 ROS 里把两个算法接起来节点、话题与参数配置3.1 节点划分与话题设计我习惯把全局规划和局部规划拆成两个独立节点中间用nav_msgs/Path通信。全局节点订阅/map和/move_base_simple/goal发布/global_path局部节点订阅/global_path、/scan和/odom发布/cmd_vel。这样做的原因是全局路径不需要频繁重算局部势场需要高频更新拆开后各自的频率可以独立控制。全局节点用 A* 在栅格地图上搜索栅格分辨率通常取 0.05 米和map_server加载的地图一致。搜索时把机器人半径膨胀到障碍物上膨胀半径至少等于机器人内切圆半径。局部节点用势场法计算合力然后通过一个简单的比例控制器把力映射成线速度和角速度def force_to_cmd(force, max_linear0.5, max_angular1.0): 把势场合力转成速度指令 linear np.linalg.norm(force) if linear 0.01: return 0.0, 0.0 # 限制最大速度 linear min(linear, max_linear) # 角速度取合力方向与机器人朝向的偏差 angle np.arctan2(force[1], force[0]) angular np.clip(angle, -max_angular, max_angular) return linear, angularmax_linear和max_angular要根据机器人底盘的实际能力设别直接抄网上的 1.0 和 2.0。我见过有人给差速小车设max_linear1.5结果势场一抖小车直接侧翻。稳妥做法是先设 0.3 和 0.8跑通后再往上加。3.2 参数文件与 launch 组织ROS 里参数最好放在 YAML 文件里launch 文件只负责加载和启动。下面是我常用的参数结构# config/planner_params.yaml global_planner: grid_resolution: 0.05 robot_radius: 0.25 inflation_radius: 0.35 a_star_heuristic: manhattan # 可选 euclidean local_planner: k_att: 1.0 k_rep: 0.4 rho_0: 1.2 step: 0.3 max_linear: 0.4 max_angular: 0.9 goal_tolerance: 0.15a_star_heuristic选manhattan还是euclidean取决于地图是否允许对角移动。如果允许八方向搜索用euclidean更准如果只允许四方向manhattan更快。goal_tolerance是局部目标的到达阈值设太小机器人会在目标点附近来回蹭设太大路径跟踪会提前切点。我一般设成step的一半。launch 文件里用rosparam加载 YAML然后分别启动两个节点。注意global_planner和local_planner的命名空间要和 YAML 里的层级一致否则参数读不到节点会用默认值跑现象就是“怎么调都没反应”。3.3 仿真环境搭建与验证步骤在 Gazebo 里验证时我一般用turtlebot3_world或自己搭一个带窄道和 U 型障碍的场景。步骤是先启动 Gazebo 和机器人模型再启动map_server加载地图然后启动全局和局部节点最后在 RViz 里用2D Nav Goal给目标点。验证时重点看三件事全局路径是否避开障碍、局部目标点是否沿路径均匀分布、/cmd_vel是否连续无跳变。如果/cmd_vel出现高频正负跳变先查势场合力是否在零点附近震荡。常见原因是k_rep太大或rho_0太小导致机器人在两个障碍之间被反复推拉。把k_rep减半、rho_0加 0.3 米通常能压住。如果机器人卡在 U 型障碍里说明全局路径本身穿过了凹槽需要在 A* 的代价函数里加障碍距离惩罚或者把膨胀半径加大到能封住凹槽入口。4. 避坑与排查局部极小值、抖动和参数失配4.1 现象机器人在目标点附近原地打转原因势场引力在目标点附近趋近于零但斥力场如果受到地面噪点或动态障碍影响会产生一个非零合力机器人就绕着目标点画圈。解决在目标点附近切换成纯位置控制或者给引力场加一个最小阈值。我一般在goal_tolerance范围内直接发零速度并停止势场计算。4.2 现象窄道里机器人左右抖动速度指令正负交替原因两侧障碍的斥力场在机器人中心对称合力方向在正负之间跳变。解决把rho_0减小到窄道宽度的一半以下或者对斥力做低通滤波。更彻底的做法是在窄道区域临时降低k_rep让引力占主导。我通常会在局部节点里加一个简单的滑动平均滤波对/cmd_vel的角速度做 5 帧平均抖动会明显改善。4.3 现象全局路径贴墙局部规划直接撞上去原因A* 的代价函数只考虑路径长度没有考虑离障碍的距离。解决在代价里加一项w * (1 / dist_to_obstacle)w取 0.5 到 2.0 之间。这样 A* 会主动选择离墙远的路径。注意dist_to_obstacle要用膨胀后的地图算否则贴墙路径的代价还是零。4.4 现象参数改了但节点行为没变原因YAML 文件没被正确加载或者节点启动顺序导致参数被覆盖。解决用rosparam list和rosparam get确认参数是否生效。如果参数在 launch 里被重复设置后设置的会覆盖先设置的。我习惯在 launch 里只加载一次 YAML节点内部不再设默认值避免“两套参数打架”。4.5 现象仿真里跑得好实车上一动就抖原因仿真里的激光雷达没有噪声实车雷达有噪点势场斥力被噪点触发。解决对/scan做中值滤波或距离阈值过滤把小于机器人半径的读数直接丢弃。另外实车的里程计有累积误差局部目标点会漂移需要定期用全局路径重采样修正。我一般每 2 秒重新取一次局部目标而不是一次性把整条路径都喂进去。5. 进阶技巧用动态窗口法兜底势场法的盲区势场法有个绕不过去的盲区当机器人速度较快时斥力场来不及在碰撞前把速度降下来。我后来在局部节点里加了一层动态窗口法DWA做速度采样兜底思路是势场法给出一个期望速度方向DWA 在这个方向附近采样多组线速度和角速度用前向仿真预测轨迹选一条既不撞障碍又最接近期望方向的速度。这样势场法负责“往哪走”DWA 负责“走多快、转多急”。具体实现时我在局部节点里维护一个速度采样空间线速度从 0 到max_linear分 5 档角速度从-max_angular到max_angular分 7 档每组速度前向仿真 1.5 秒用激光雷达数据检查碰撞。评价函数里加三项与势场期望方向的夹角、与障碍的最小距离、速度大小。夹角越小、距离越大、速度越快得分越高。def dwa_evaluate(v, w, force_dir, scan_data, dt0.1, predict_time1.5): DWA 评价函数返回得分越高越好 score 0.0 # 方向一致性 angle_diff abs(np.arctan2(force_dir[1], force_dir[0]) - w * predict_time) score 1.0 / (1.0 angle_diff) # 障碍距离 min_dist min(scan_data) if scan_data else 10.0 score 0.5 * min_dist # 速度奖励 score 0.3 * v return score这个评价函数的权重需要根据机器人调整。我一般先让方向一致性占主导跑通后再慢慢加障碍距离的权重。注意predict_time不要超过激光雷达的更新周期太多否则预测轨迹和实际偏差大反而容易撞。验证 DWA 是否生效可以在 RViz 里把采样轨迹可视化出来看机器人是否在势场方向附近选了一条更安全的速度。如果 DWA 选的速度和势场期望方向偏差太大说明障碍距离权重过高机器人变得过于保守在窄道里会停滞。这时候把障碍距离权重降到 0.2 左右或者把predict_time缩短到 1.0 秒。从那以后我每次调势场法参数都强制先关掉 DWA 跑一遍纯势场确认局部极小值和抖动在可接受范围再开 DWA 做速度兜底。这样出问题时能快速定位是势场本身的问题还是 DWA 评价函数的问题。希望帮到你。本文还有配套的精品资源点击获取
