4.3 局部规划与避障：势场与 DWA
4.3 局部规划与避障：势场与 DWA
本节摘要：全局地图是几秒前的快照，动态障碍不会等它更新。局部规划在机器人周边滚动工作：人工势场用一句物理直觉给出反应式避让，DWA 在速度空间里直接采样可执行动作。本节讲清两者机制、势场的局部极小陷阱与 DWA 的代价。
凌晨三点的人形障碍
凌晨三点，配送机器人在医院走廊执行夜班任务。一位加班的护士端着器材从侧门出来，正好走在全局规划的走廊中线上。全局图上这格是空的——几秒前的快照里没有人。此刻救场的必须是局部层：在几百毫秒内，机器人要发现障碍、评估各方向可走性、减速、绕出、回线、恢复速度。局部规划的两个流派就此分野：势场法在位置空间造力场，DWA 在速度空间做采样。前者像被环境"推着走"，后者像"先想清楚自己能做什么"。
人工势场：谷底吸引，山峰排斥
把目标点设成吸引谷，障碍设成排斥山，机器人像一颗小球在势面上滚：
U_att(q) = ½·k_att·|q − q_goal|² 引力势：离目标越远力越大 U_rep(q) = ½·k_rep·(1/ρ − 1/ρ0)² (ρ < ρ0) 斥力势：只在影响半径 ρ0 内生效 F(q) = −∇U_att − ∇U_rep 合力即期望运动方向
引力随距离线性增长，斥力在障碍影响半径内按倒数平方陡增。合成的力场让机器人自然"离障碍远、朝目标去"。它快——每个控制周期只需算几个梯度——但有一个教科书级的缺陷：局部极小。当引力与斥力共线反向（障碍正挡在目标与机器人之间），合力为零，机器人停住不前；凹形障碍（U 形货架、凹槽）会把机器人"吸"进去出不来。
图：势场的局部极小陷阱
三个经典补丁：检测到速度长期为零就注入随机扰动逃逸；给斥力加垂直分量，让机器人"贴墙滑"而非正对障碍顶死；或者干脆承认势场只适合"开阔环境的小幅避让"，重活交给 DWA。
DWA：在速度空间里做选择题
动态窗口法（Dynamic Window Approach）换了个问法：不问"往哪走"，问"下一瞬间我可能怎么动"。步骤四条：
- 采样速度对 (v, ω)：只采当前速度能在一个控制周期内达到的范围（受电机加速度限制）——这就是"动态窗口"名字的来历，保证采出来的全是物理上立刻可执行的指令；
- 前向仿真：每个速度对按运动学推演未来 1–2 秒的轨迹；
- 评分：按"朝目标靠近程度、离障碍距离、速度（快加分）"加权打分，落入障碍区直接淘汰；
- 执行最高分，下个周期重新采样。
DWA 的先天优势是物理可行性内建：采样窗口本身就受动力学限制，输出的永远是底盘做得到的动作；对动态障碍的反应也自然——前向仿真时目标若在移动，未来轨迹立即评分下降。代价是要调的参数多（仿真时长、各权重、速度上限），权重之间互相牵扯：距离权重太高机器人变得胆小黏墙，目标权重太高又贴着障碍钻。
def dwa_score(v, w, pose, goal, laser_pts, dt=0.1, T=1.5): """单个速度对的评分：目标项 + 障碍项 + 速度项""" x, y, th = pose collision = False for _ in range(int(T/dt)): x += v * np.cos(th) * dt y += v * np.sin(th) * dt th += w * dt d = np.min(np.hypot(laser_pts[:,0]-x, laser_pts[:,1]-y)) if d < ROBOT_RADIUS: # 撞上，一票否决 collision = True; break if collision: return -float('inf') d_goal = np.hypot(goal[0]-x, goal[1]-y) d_obs = np.min(np.hypot(laser_pts[:,0]-x, laser_pts[:,1]-y)) return w_goal*d_goal*-1 + w_obs*min(d_obs, 1.0) + w_speed*v
两派怎么分工
工程系统的常见架构是"全局 A* + 局部 DWA"：全局层管宏观路线（走哪条通道、过哪扇门），局部层管微观执行（贴着路线走、躲开临时障碍）。势场法则退化成最轻量的反应保险丝——比如在安全停障里用斥力直接输出减速。选型的经验判据：环境动态程度低、走廊开阔，势场够用；人流密集、需要流畅避让，DWA 或更先进的 TEB（时间弹性带）值得引入。
⚠️ 常见坑：局部最优不是势场的专利，DWA 也会困在凹形障碍前——它只看未来一两秒。局部层的本职是"反应"，发现反复绕不出时就该上报全局层重规划，而不是自己硬扛。这个升级逻辑要显式写进状态机。
本节要点
- 势场法一句话机制（引谷斥山）决定其快而贪，局部极小是结构缺陷，补丁治标不治本。
- DWA 在速度空间采样、前向仿真评分，物理可行性内建，代价是参数耦合需细调。
- 全局+局部的双层架构是主流：全局管路线，局部管反应；局部反复失败要升级全局重规划。
- 选型看动态程度与走廊结构：开阔简单用势场保险丝，人流密集用 DWA/TEB。
TEB 与 MPC：局部规划的进阶选项
DWA 之后，局部规划还有两级进阶。**TEB（时间弹性带）**把路径表达为带时间戳的控制点序列，同时优化几何与时间：约束显式处理（最大速度、最大角速度、最小转弯半径、与障碍的最小距离），目标函数平衡"路径长度、时间、平滑"。它输出的是带速度指令的完整轨迹而非单点指令，对差速与阿克曼底盘都有成熟适配。MPC 进局部层则把"滚动优化"用在速度空间：预测时域内优化一串控制输入，约束直接写进求解器。两者的共同代价是每个周期要解一个优化问题——TEB 典型毫秒到几十毫秒级，MPC 取决于预测时域与约束数。选择的经验线：DWA 够用就 DWA（简单、快、好调），需要更顺滑的轨迹与更强的约束处理上 TEB，需要显式动力学约束与预测时域优化再上 MPC。
局部层参数的调试台账
DWA 类算法的参数耦合让很多人栽跟头，立台账是解药。每调一个参数记录四栏：参数名、改动值、可观察的行为变化、反向案例。核心参数的典型影响：前向仿真时长加大（典型 1.5 到 2.5 秒），对远处障碍的预判变好但动态响应变钝；速度权重加大，穿越开阔区更快但转弯变"冲"；障碍距离权重加大，过窄通道的能力下降。台账的价值在于暴露"参数互相拉扯"的关系——当你发现 A 参数的改善总被 B 参数的恶化抵消，就该意识到问题不在参数而在权重结构，退回到"先用全局层保证路线质量，局部层只管反应"的架构分工上。
问题：机器人明明有路却频繁急停
先分辨急停的来源：是局部规划器判"无解"（采样窗口内全部速度对都碰撞），还是安全停障直接触发（感知层面判定碰撞风险）？两者修法不同。前者看膨胀半径与通道宽度的关系（4.1 节的老问题）与仿真时长（太短看不见出路）；后者看安全区判定用的是原始障碍还是膨胀障碍——安全层用膨胀图会"提前"急停，把局部层本来能解的局面掐死。分层清晰的原则在此生效：安全层用物理尺寸加小裕度，规划层用大膨胀，各管各的。