Webots平台Firebird6机器人实现指定坐标点导航问题求助

先修复现有代码的明显错误 #

• 函数名不统一：你定义的前进函数是foward()，但主循环调用的是avanzar()；定义的停止函数是stop()，主循环调用的是detener()，先把命名统一，否则基础功能无法正常运行。
• 不要用Python原生的time.time()计算时间，Webots的仿真时间和系统真实时间不同步，要用robot.getTime()获取仿真时间，否则转向计时逻辑完全不准。

修正Compass传感器的使用逻辑 #

你现在只打印了Compass的原始三维向量，没有转成可用的航向角，参考转换代码：

import math

# 在主循环里获取航向角，范围为[-π, π]
compass_val = compassXY.getValues()
# 注意坐标轴映射要和你Webots世界的正北方向对齐，根据实际情况调整参数顺序
heading = math.atan2(compass_val[0], compass_val[1])
# 转角度的话用 math.degrees(heading) 即可

如果嫌Compass校准麻烦，建议直接启用Firebird6内置的GPS传感器，直接获取机器人当前全局坐标，误差比自己算里程计小很多，初始化代码：

gps = robot.getDevice('gps')
gps.enable(timestep)
# 获取当前坐标
current_x = gps.getValues()[0]
current_y = gps.getValues()[1]

新增目标导航逻辑 #

参考简易的BUG导航算法，实现避障+目标点导航的融合：

1. 先预设目标点坐标(target_x, target_y)
2. 每帧根据当前坐标和航向角，计算到目标点的方位角偏差：

# 计算目标方位角
target_heading = math.atan2(target_y - current_y, target_x - current_x)
# 计算角度偏差，归一化到[-π, π]区间
angle_diff = target_heading - heading
angle_diff = (angle_diff + math.pi) % (2 * math.pi) - math.pi

3. 状态切换逻辑：

• 无障碍物时：如果角度偏差大于0.1弧度，根据偏差方向调整左右轮速度转向对准目标；偏差足够小时直接前进
• 检测到障碍物时：暂停导航逻辑，进入避障模式，转向直到前方无障碍物，再退出避障模式重新对准目标
• 当当前坐标和目标点的距离小于阈值（比如0.05m）时，停止机器人，判定到达目标

完善避障逻辑 #

你现在只检测了左前方的距离传感器，右前方的ps4、ps5也需要加判断，触发时右转，否则右侧来的障碍物会直接撞上。

内容的提问来源于stack exchange，提问作者Thazze