3、行人路径规划：全局路径规划（A*算法）、局部路径规划（DWA算法）、行人路径规划与车辆路径规划的差异
好，咱们进入第三章。这一章聊的是行人的路径规划。
说实话，行人路径规划跟车辆路径规划，虽然名字都叫“路径规划”，但骨子里的逻辑差别很大。我最早做自动驾驶仿真时，直接把车辆的规划算法套到行人身上，结果跑出来的行人像“鬼畜”一样——要么原地转圈，要么突然急刹。嗯，后来我才明白，行人的行为逻辑跟车完全不是一回事。
3.1 全局路径规划：A*算法
全局路径规划，说白了就是给行人画一条从起点到终点的“大路线”。在仿真里，我们通常用A*算法来做这件事。
A*算法的核心公式很简单：F = G + H。G是起点到当前点的实际代价，H是当前点到终点的预估代价。我个人习惯用曼哈顿距离作为H值，因为行人走的路网大多是网格化的，曼哈顿距离算起来快，效果也够用。
我在项目中遇到过一个问题：直接用A*算出来的路径，行人走起来太“机械”了。比如明明可以斜穿广场，A*非要让人沿着人行道绕一圈。后来我加了一个“偏好因子”，让算法在空旷区域倾向于走直线，在障碍物密集区域才严格走网格。效果好了很多。
下面是一个简化的A*伪代码，我通常用它做原型验证：
function AStar(start, end, grid):
openList = [start]
closedList = []
while openList is not empty:
current = node with lowest F in openList
if current == end:
return reconstructPath(current)
openList.remove(current)
closedList.add(current)
for each neighbor of current:
if neighbor in closedList or not walkable:
continue
tentativeG = current.G + distance(current, neighbor)
if neighbor not in openList or tentativeG < neighbor.G:
neighbor.G = tentativeG
neighbor.H = heuristic(neighbor, end)
neighbor.parent = current
if neighbor not in openList:
openList.add(neighbor)
return null // 无路径
你想想看，这个算法在行人仿真里有个天然缺陷——它假设环境是静态的。但行人周围有车、有其他行人、有临时路障。所以A*算出来的路径，只能作为“参考线”。
3.2 局部路径规划：DWA算法
全局路径定好了，接下来就是局部路径规划。这里我推荐DWA算法（Dynamic Window Approach）。
DWA的核心思想很简单：在当前时刻，行人能走的速度和方向是有限的——这就是“动态窗口”。在这个窗口里，采样一组速度组合，然后选一个最好的。
我曾经踩过一个坑：DWA的代价函数里，如果“朝向目标”的权重设得太高，行人会直愣愣地撞向障碍物。如果“避障”的权重设得太高，行人又会原地打转。后来我总结了一个经验公式：
- 朝向目标权重：0.4
- 避障权重：0.4
- 速度平滑权重：0.2
这个比例在大多数场景下都表现不错。当然，具体数值还是要根据你的仿真场景微调。
DWA的伪代码长这样：
function DWA(currentPose, goal, obstacles, dynamicWindow):
bestTrajectory = null
bestCost = INF
for each (v, w) in dynamicWindow:
trajectory = simulateTrajectory(currentPose, v, w, dt)
cost = alpha * headingCost(trajectory, goal)
+ beta * obstacleCost(trajectory, obstacles)
+ gamma * velocityCost(v, w)
if cost < bestCost:
bestCost = cost
bestTrajectory = trajectory
return bestTrajectory
注意看，这里的dynamicWindow是实时计算的。行人的最大加速度、最大减速度、最大转向速度，这些参数决定了窗口的大小。我建议你在仿真里把这些参数设成随机值，模拟不同年龄、不同体力的行人。
3.3 行人路径规划与车辆路径规划的差异
好，这是本章的重点。我直接列个表格，对比一下两者的核心差异：
| 对比维度 | 行人路径规划 | 车辆路径规划 |
|---|---|---|
| 运动模型 | 全向移动（可侧移、后退） | 非完整约束（只能前进、转弯） |
| 速度范围 | 0~2m/s，可瞬间停止 | 0~30m/s，加减速有延迟 |
| 路径精度 | 允许偏差0.5~1米 | 要求偏差<0.1米 |
| 避障策略 | 灵活绕行、可等待 | 严格车道保持、换道 |
| 全局规划频率 | 每5~10秒一次 | 每0.1~1秒一次 |
| 局部规划频率 | 每0.5~1秒一次 | 每0.05~0.1秒一次 |
为什么会这样？说白了，行人的自由度更高，但精度要求更低。车辆正好相反。
我记得有一次做仿真，车辆规划算法要求行人必须走“完美路径”，结果行人一直在微调方向，看起来像在跳机械舞。后来我把行人的路径容忍度放宽到0.8米，效果自然多了。
另外，行人的路径规划还有一个车辆没有的特点——社交力。行人会考虑“这个人是不是要给我让路”、“前面那个人走得太慢了我要超过去”。这些在车辆规划里是不存在的。我建议你在仿真里加入一个“社交距离”参数，当两个行人距离小于1.5米时，自动触发避让行为。
嗯，这一章就到这里。下一章我们会聊行人的行为决策——什么时候过马路、什么时候停下来、什么时候跑起来。这些才是仿真里最有趣的部分。