Webots中NAO机器人Python寻路避障实战：从A*到动态窗口法

简介：这份资源面向机器人学初学者与Webots仿真爱好者，聚焦在Webots平台上用Python实现NAO人形机器人的寻路避障功能。包内共23个文件，以13个motion动作文件、4个Python脚本、3个xls数据表及wbproj、wbt工程文件为主，涵盖NAO的关节动作序列、传感器数据处理脚本与仿真场景配置，压缩包约73KB，结构紧凑便于直接导入Webots运行调试。资源围绕超声波与红外传感器读数判断障碍物、设定阈值触发转向或停止、循环更新路径规划等核心环节展开，并预留了向A*、Dijkstra等高级算法扩展的空间。已有1352人学习下载，适合希望理解类人机器人运动控制、传感器融合与避障策略实现细节的读者参考，可据此快速搭建可运行的仿真实验环境并观察算法效果。

1. 从一次仿真翻车说起：这套 NAO 寻路避障附件到底能干什么

去年帮某高校实验室调一个仿真项目，NAO 机器人在 Webots 里走直线没问题，一遇到动态障碍物就直接原地抽搐——激光雷达数据读到了，但避障逻辑和路径规划各跑各的，谁也不理谁。后来换了一套带完整寻路避障实现的附件包，把感知、决策、执行三层串起来，才把这个问题按住。这套附件就是围绕 Webots 平台上的 NAO 双足机器人，用 Python 实现从 A 点到 B 点的自主导航，核心解决三件事：全局路径怎么规划、局部障碍怎么绕、双足步态怎么配合转向。它适合正在做机器人仿真课程设计的学生、需要快速验证导航算法的开发者，以及想把 Webots 当测试床但不想从零搭框架的从业者。Python 在 Webots 里的控制器接口不算复杂，但要把寻路和避障揉成一个稳定跑通的闭环，里面有不少参数和时序上的门道，这套附件省掉的正是这部分反复试错的时间。

2. 环境搭建与控制器骨架：把 NAO 塞进 Webots 的正确姿势

2.1 Webots 版本选择与 Python 控制器绑定

Webots 的版本迭代对 Python API 影响不小，尤其是controller模块里Robot类的初始化方式。我一般建议用 R2023b 及之后的版本，原因是这个版本之后 Python 控制器的step()阻塞行为更稳定，不会出现仿真时间跳变导致传感器数据错帧的问题。安装完 Webots 后，第一件事是确认 Python 解释器路径被正确识别——在首选项 > Python command里填你本地的 python 可执行文件绝对路径，别用默认的python字符串，Windows 上很容易指到 Microsoft Store 的占位程序上。

控制器文件放在controllers/nao_nav/nao_nav.py，入口结构如下：

from controller import Robot, Motor, Camera, GPS, InertialUnit import math class NaoNavigator: def __init__(self): self.robot = Robot() self.timestep = int(self.robot.getBasicTimeStep()) # 双足关节电机：NAO 每条腿有 6 个自由度 self.l_hip_yaw = self.robot.getDevice("LHipYawPitch") self.l_hip_roll = self.robot.getDevice("LHipRoll") self.l_hip_pitch = self.robot.getDevice("LHipPitch") self.l_knee = self.robot.getDevice("LKneePitch") self.l_ankle_pitch = self.robot.getDevice("LAnklePitch") self.l_ankle_roll = self.robot.getDevice("LAnkleRoll") # 传感器：GPS 给全局坐标，惯性单元给姿态角 self.gps = self.robot.getDevice("gps") self.gps.enable(self.timestep) self.imu = self.robot.getDevice("inertial unit") self.imu.enable(self.timestep) # 红外距离传感器阵列，NAO 头部和胸部共 8 个 self.distance_sensors = [] for i in range(8): s = self.robot.getDevice(f"ds{i}") s.enable(self.timestep) self.distance_sensors.append(s)

这段代码做了三件事：拿机器人实例、绑定腿部电机、开启导航必需的传感器。timestep必须和 Webots 世界文件里的basicTimeStep一致，否则step()的节奏会乱。GPS 返回的是世界坐标系下的绝对位置，惯性单元返回的是 roll/pitch/yaw 三个姿态角，这两个是后面路径跟踪的基准。红外传感器ds0到ds7的朝向分布需要对照 NAO 的官方模型文档确认，不同版本的 Webots 里传感器命名可能差一个前缀。

2.2 世界文件配置与机器人初始位姿

世界文件.wbt里要确认两处：NAO 节点的translation和rotation决定了初始位姿，controller字段要填"nao_nav"和控制器目录名一致。地面材质如果设成高摩擦的grass，双足行走的步态参数需要相应调大关节力矩，否则会出现脚底打滑导致里程计漂移。常见做法是在 NAO 的physics节点里把damping设为 0.1 左右，减少仿真中的高频抖动。

障碍物建议用RectangleArena加若干WoodenBox或Cylinder，高度不要超过 NAO 的胸部传感器平面，否则红外测距会被地面反射干扰。我一般会在场景里放三到五个静态障碍加一个沿固定轨迹移动的动态障碍，用来验证避障逻辑的响应速度。

提示：每次改完世界文件后，先在 Webots 里手动运行一次空控制器，确认机器人不会因为初始位姿穿模而直接弹飞。

3. 全局路径规划：A* 在栅格地图上的落地细节

3.1 栅格地图构建与代价函数设计

Webots 本身不提供现成的占据栅格地图，需要自己从传感器数据或已知障碍物位置生成。这套附件里的做法是：在仿真开始前，读取世界文件中所有障碍物的包围盒，投影到二维平面，按 0.1 米分辨率离散成栅格。每个栅格有三种状态：空闲、占据、未知。未知区域默认按占据处理，避免规划出穿墙路径。

代价函数不是简单的欧氏距离，而是加了转向惩罚项：

import heapq def heuristic(a, b): # 对角距离启发式，比曼哈顿距离更贴近实际移动代价 dx = abs(a[0] - b[0]) dy = abs(a[1] - b[1]) return (dx + dy) + (math.sqrt(2) - 2) * min(dx, dy) def a_star(grid, start, goal): open_set = [(0, start)] came_from = {} g_score = {start: 0} while open_set: _, current = heapq.heappop(open_set) if current == goal: # 回溯路径 path = [] while current in came_from: path.append(current) current = came_from[current] return path[::-1] for dx, dy in [(-1,0),(1,0),(0,-1),(0,1),(-1,-1),(-1,1),(1,-1),(1,1)]: neighbor = (current[0]+dx, current[1]+dy) if not (0 <= neighbor[0] < len(grid) and 0 <= neighbor[1] < len(grid[0])): continue if grid[neighbor[0]][neighbor[1]] == 1: continue # 对角移动代价为 sqrt(2)，直行为 1 move_cost = math.sqrt(2) if dx != 0 and dy != 0 else 1.0 tentative_g = g_score[current] + move_cost if neighbor not in g_score or tentative_g < g_score[neighbor]: came_from[neighbor] = current g_score[neighbor] = tentative_g f = tentative_g + heuristic(neighbor, goal) heapq.heappush(open_set, (f, neighbor)) return []

heuristic用对角距离而不是曼哈顿距离，是因为 NAO 允许斜向移动，用曼哈顿距离会高估代价导致路径偏保守。move_cost里对角移动乘sqrt(2)是标准做法，但实际双足机器人斜向行走的能耗比直行高不少，如果追求能耗最优，可以把对角代价调到 1.6 左右。grid是二维列表，0 表示空闲，1 表示占据，构建时注意坐标系和 Webots 世界坐标的映射关系——Webots 里 X 轴向前、Z 轴向上，栅格地图通常取 X-Z 平面投影。

3.2 路径平滑与航点抽取

A* 出来的路径是栅格序列，直接送给双足执行会变成锯齿状行走，每走一步都要微调方向，效率极低。附件里用了一个简单的视线检测做平滑：从起点开始，尝试连接更远的航点，如果连线不穿过任何占据栅格，就跳过中间点。

def line_of_sight(grid, p1, p2): # Bresenham 直线算法检查两点间是否穿过障碍 x0, y0 = p1 x1, y1 = p2 dx = abs(x1 - x0) dy = abs(y1 - y0) sx = 1 if x0 < x1 else -1 sy = 1 if y0 < y1 else -1 err = dx - dy while True: if grid[x0][y0] == 1: return False if x0 == x1 and y0 == y1: return True e2 = 2 * err if e2 > -dy: err -= dy x0 += sx if e2 < dx: err += dx y0 += sy def smooth_path(grid, path): if len(path) <= 2: return path smoothed = [path[0]] i = 0 while i < len(path) - 1: j = len(path) - 1 while j > i + 1: if line_of_sight(grid, path[i], path[j]): break j -= 1 smoothed.append(path[j]) i = j return smoothed

平滑后的航点数量通常只有原始路径的十分之一到五分之一，NAO 走起来会连贯很多。line_of_sight用的是 Bresenham 整数算法，比浮点采样快，适合在控制器循环里反复调用。注意平滑后的航点间距如果超过 0.5 米，需要在中间插值补点，否则双足步态会跨太大导致失衡。

注意：平滑不能改变路径的拓扑结构，如果原始路径是绕过一个 U 形障碍，平滑后不能把 U 形开口封死，否则会撞墙。

4. 局部避障与步态控制：让 NAO 真的能绕开动态障碍

4.1 红外传感器数据处理与障碍物聚类

NAO 的红外传感器返回的是距离值，单位是米，有效范围大概 0.1 到 0.8 米。原始数据噪声不小，尤其是黑色障碍物吸红外，读数会偏大。附件里先做中值滤波，再做阈值分割：

def filter_and_cluster(sensor_values, threshold=0.35): # 中值滤波窗口为 3 filtered = [] for i in range(len(sensor_values)): window = [ sensor_values[(i-1) % len(sensor_values)], sensor_values[i], sensor_values[(i+1) % len(sensor_values)] ] filtered.append(sorted(window)[1]) # 低于阈值的视为障碍，聚类成扇区 obstacles = [] current_cluster = [] for i, val in enumerate(filtered): if val < threshold: current_cluster.append((i, val)) else: if current_cluster: obstacles.append(current_cluster) current_cluster = [] if current_cluster: obstacles.append(current_cluster) return obstacles

threshold设 0.35 米是经验值，对应 NAO 一步的步长。低于这个距离如果不转向，下一步就会撞上。聚类的作用是把连续触发传感器归为一个障碍物，避免同一个障碍被重复计算导致机器人来回摆头。filtered数组的索引对应传感器编号，需要提前标定每个编号的朝向角度，附件里用了一个sensor_angles列表来映射。

4.2 动态窗口法与步态参数耦合

局部避障用的是简化版动态窗口法：在速度空间里采样若干组(线速度, 角速度)，预测未来 0.5 秒的轨迹，选一条不撞障碍且最接近全局路径方向的。NAO 的线速度上限大概 0.3 m/s，角速度上限 1.2 rad/s，采样时在这个范围内离散。

def dynamic_window(current_pose, goal_direction, obstacles): v_samples = [0.0, 0.1, 0.2, 0.3] w_samples = [-1.2, -0.6, 0.0, 0.6, 1.2] best_score = -float('inf') best_vw = (0.0, 0.0) for v in v_samples: for w in w_samples: # 预测轨迹：简化为圆弧 predicted = predict_trajectory(current_pose, v, w, dt=0.5) if check_collision(predicted, obstacles): continue # 评分：方向对齐 + 速度奖励 - 障碍距离惩罚 heading = math.atan2(predicted[-1][1] - current_pose[1], predicted[-1][0] - current_pose[0]) heading_score = math.cos(heading - goal_direction) velocity_score = v / 0.3 obstacle_score = min_distance_to_obstacle(predicted, obstacles) score = 0.6 * heading_score + 0.2 * velocity_score + 0.2 * obstacle_score if score > best_score: best_score = score best_vw = (v, w) return best_vw

权重0.6 / 0.2 / 0.2是调出来的，方向对齐占主导，速度和障碍距离各占两成。如果场景里动态障碍多，把障碍距离权重提到 0.3，方向权重降到 0.5，响应会更灵敏。predict_trajectory用圆弧近似，因为 NAO 转向时不是原地转，而是边走边转，圆弧模型比直线模型更贴近实际。

步态控制这块，附件里没有用复杂的 ZMP 或 MPC，而是用了一个预定义的行走周期函数，根据(v, w)调整步幅和步频：

def walk_cycle(v, w, phase): # phase 在 [0, 2*pi] 之间循环 step_length = 0.04 + 0.06 * (v / 0.3) # 步幅随速度线性增加 step_frequency = 1.5 + 0.5 * (v / 0.3) # 步频也随速度增加 turn_amplitude = 0.3 * (w / 1.2) # 转向幅度随角速度缩放 # 髋关节俯仰角：正弦驱动 hip_pitch = 0.2 * math.sin(phase * step_frequency) # 髋关节偏航角：叠加转向分量 hip_yaw = turn_amplitude * math.sin(phase * step_frequency + math.pi/2) return hip_pitch, hip_yaw

step_length和step_frequency的系数是实测调出来的，速度 0.3 m/s 时步幅约 0.1 米、步频约 2 Hz，再快步态就散了。turn_amplitude限制在 0.3 弧度以内，超过这个值 NAO 的脚踝侧向力矩不够，会侧翻。

提示：调步态参数时先把v固定为 0，只调w，确认原地转向稳定后再加线速度，否则两个变量耦合很难定位问题。

5. 避坑与排查：那些让仿真跑不下去的细节

5.1 传感器数据跳变导致避障抖动

现象：NAO 在障碍物前反复左右摆头，路径跟踪完全失效。原因：红外传感器读数在阈值附近波动，障碍物一会儿被判定存在、一会儿消失，动态窗口法每次采样选出的最优速度方向来回跳。解决：在filter_and_cluster之后加一个滞后计数器，障碍物连续 3 帧被检测到才加入障碍列表，连续 5 帧消失才移除。滞后窗口的长度和timestep有关，20 ms 步长下 3 帧约 60 ms，不会明显影响响应速度。

5.2 全局路径与局部避障的目标冲突

现象：NAO 沿着全局路径走，遇到动态障碍绕开后，不再回到原路径，而是直接朝终点方向斜穿，结果撞上另一侧的静态障碍。原因：局部避障的goal_direction直接用了终点方向，而不是当前航点方向。解决：把goal_direction改成指向当前正在跟踪的航点，只有到达航点半径 0.15 米以内才切换到下一个航点。这个半径不能太小，否则 NAO 因为步态误差永远到不了航点，会卡在原地。

5.3 双足里程计漂移导致定位失准

现象：仿真跑了两三分钟后，NAO 的实际位置和 GPS 读数偏差越来越大，路径跟踪开始画龙。原因：双足行走的脚底打滑在仿真里无法完全消除，GPS 虽然有绝对坐标，但更新频率和timestep一致，中间过程靠航迹推算，误差累积。解决：在控制器里加一个简单的粒子滤波，用 GPS 观测值修正航迹推算的位姿。附件里给了一个 50 个粒子的轻量实现，计算量不大，能把漂移控制在 0.1 米以内。

5.4 步态周期与传感器采样周期不同步

现象：NAO 行走时偶尔会突然顿一下，像是被什么东西绊住了。原因：步态周期函数的phase累加用的是timestep，但传感器数据读取也在同一个循环里，如果某次循环因为计算量大导致step()实际耗时超过timestep，phase就会跳变。解决：用robot.getTime()的实际仿真时间来计算phase，而不是自己累加timestep。这样即使某帧慢了，下一帧的phase也能自动追上。

5.5 世界文件里障碍物材质影响传感器读数

现象：同样的距离，木箱和金属圆柱的红外读数差了一倍。原因：Webots 的红外传感器模拟考虑了材质反射率，深色吸光材质的回波弱，读数偏大。解决：在障碍物的Material节点里把shininess调到 0.5 以上，或者统一用同一种材质做障碍物。如果必须用多种材质，在filter_and_cluster里按材质类型分别设阈值，别用一个全局阈值打天下。

6. 进阶调参与验证：把仿真结果变成可复现的数据

6.1 批量跑仿真与指标采集

单次跑通不算数，得能批量复现。附件里带了一个batch_runner.py，用 Webots 的命令行模式webots --mode=fast --no-rendering跑无头仿真，每次随机生成障碍物布局，跑 50 次取统计值。关键指标有三个：到达终点成功率、平均路径长度比（实际路径 / 全局路径）、最小障碍距离。成功率低于 90% 说明避障参数太保守或太激进，路径长度比超过 1.5 说明绕路太多，最小障碍距离低于 0.1 米说明安全裕度不够。

# 批量运行示例：50 次仿真，结果输出到 results.csv python batch_runner.py --runs 50 --output results.csv --seed 42

--seed固定随机种子，保证障碍物布局可复现。--runs根据机器性能调，无渲染模式下每次仿真大概 10 到 20 秒，50 次在普通笔记本上十分钟以内能跑完。

6.2 参数敏感度分析与调参顺序

调参别一上来就网格搜索，先做敏感度分析。我一般固定其他参数，单独调一个，看指标变化曲线。从经验看，敏感度从高到低是：障碍距离阈值 > 动态窗口方向权重 > 步频系数 > 平滑航点间距。障碍距离阈值每变 0.05 米，成功率能差 15 个百分点。调参顺序建议：先定障碍距离阈值，再调动态窗口权重，然后微调步态参数，最后动平滑间距。

6.3 从仿真到实机的参数迁移边界

这套附件是纯仿真环境下的实现，迁移到实体 NAO 上有几个硬边界得说清楚。实体 NAO 的红外传感器响应延迟比仿真高一个数量级，动态窗口法的预测时域得从 0.5 秒缩到 0.2 秒，否则避障动作会滞后。实体机器人的关节力矩限制更严格，turn_amplitude得从 0.3 降到 0.15 左右。另外实体 NAO 的 GPS 是外部动捕系统提供的，更新频率通常只有 100 Hz 左右，粒子滤波的观测更新要相应降频。

从那以后我每次调完仿真参数，都会强制走一遍批量验证，确认成功率、路径长度比、最小障碍距离三个指标都在阈值内，才敢把参数写进配置文件。希望帮到你。

本文还有配套的精品资源，点击获取