diff --git a/base_move/go_straight.py b/base_move/go_straight.py
index c5b9b56..d51919f 100644
--- a/base_move/go_straight.py
+++ b/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