优化 go_straight 函数中的方向计算逻辑，使用 IMU 数据来确定角度偏差，提升了机器人在前进和后退时的运动精度。

base_move/go_straight.py

@@ -104,10 +104,11 @@ def go_straight(ctrl, msg, distance, speed=0.5, observe=False):
             dy = current_position[1] - start_position[1]
             distance_moved = math.sqrt(dx*dx + dy*dy)
             
-            # 计算移动方向与初始朝向的偏差
-            movement_direction = math.atan2(current_position[1] - last_position[1], 
-                                           current_position[0] - last_position[0]) if distance_moved > 0.05 else start_yaw
-            yaw_error = movement_direction - start_yaw
+            # 根据前进或后退确定期望方向
+            expected_direction = start_yaw if forward else (start_yaw + math.pi) % (2 * math.pi)
+            
+            # 使用IMU朝向数据计算角度偏差
+            yaw_error = current_yaw - expected_direction
             # 角度归一化
             while yaw_error > math.pi:
                 yaw_error -= 2 * math.pi