优化 arc_turn_around_hori_line 函数，增强圆弧运动的精度和调试信息

- 添加最小半径限制，确保线速度计算的稳定性
- 更新调试信息，显示实际半径与最小半径的使用情况
- 在起点、圆心和目标位置放置标记，便于观察运动轨迹
- 计算实际结束位置与预期目标位置的偏差，输出位置误差信息

base_move/move_base_hori_line.py

@@ -438,14 +438,26 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
         base_w = 0.8  # 原来是0.6
 
     w = base_w if left else -base_w  # 左转为正，右转为负
-    v = abs(w * r)  # 线速度，正负号与角速度方向一致
+    
+    # 确保r有一个最小值，避免速度过低
+    min_radius = 0.2  # 最小半径，单位：米
+    effective_r = max(r, min_radius)
+    
+    # 线速度，必须与角速度和半径的关系一致：v = w * r
+    # 这样才能保证走圆弧
+    v = abs(w * effective_r)  # 线速度，正负号与角速度方向一致
+    
+    if observe:
+        if effective_r != r:
+            print(f"注意: 实际半径 {r:.3f}米 太小，使用最小半径 {effective_r:.3f}米 计算速度")
+    
     t = abs(angle_rad / w)  # 运动时间，取绝对值保证为正
     
     # 应用时间补偿系数
     t *= time_compensation
     
     if observe:
-        print(f"圆弧半径: {r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s")
+        print(f"圆弧半径: {effective_r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s")
         print(f"理论运动时间: {abs(angle_rad / w):.2f}s, 应用补偿系数{time_compensation}后: {t:.2f}s")
 
     # 4. 发送圆弧运动指令
@@ -461,9 +473,42 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
     # 记录起始角度和位置
     start_yaw = ctrl.odo_msg.rpy[2]
     start_position = list(ctrl.odo_msg.xyz)
+    
+    # 在起点放置蓝色标记
+    if observe and hasattr(ctrl, 'place_marker'):
+        # 获取当前姿态
+        yaw = ctrl.odo_msg.rpy[2]
+        ctrl.place_marker(start_position[0], start_position[1], 
+                        start_position[2] if len(start_position) > 2 else 0.0, 
+                        'blue', observe=True)
+        print(f"在起点位置放置蓝色标记: {start_position}")
+    
+    # 计算圆心位置 - 相对于机器人前进方向的左侧或右侧
+    # 圆心与机器人当前位置的距离就是圆弧半径
+    center_x = start_position[0] - effective_r * math.sin(yaw) * (1 if left else -1)
+    center_y = start_position[1] + effective_r * math.cos(yaw) * (1 if left else -1)
+    center_z = start_position[2] if len(start_position) > 2 else 0.0
+    
+    # 在圆心放置绿色标记
+    if observe and hasattr(ctrl, 'place_marker'):
+        ctrl.place_marker(center_x, center_y, center_z, 'green', observe=True)
+        print(f"在旋转圆心放置绿色标记: [{center_x:.3f}, {center_y:.3f}, {center_z:.3f}]")
+    
+    # 计算目标点的位置 - 旋转后的位置
+    # 使用几何关系：目标点是从起点绕圆心旋转angle_rad弧度后的位置
+    target_angle = yaw + (angle_rad if left else -angle_rad)
+    # 计算从圆心到目标点的向量
+    target_x = center_x + effective_r * math.sin(target_angle) * (1 if left else -1)
+    target_y = center_y - effective_r * math.cos(target_angle) * (1 if left else -1)
+    target_z = start_position[2] if len(start_position) > 2 else 0.0
+    
+    # 在目标点放置红色标记
+    if observe and hasattr(ctrl, 'place_marker'):
+        ctrl.place_marker(target_x, target_y, target_z, 'red', observe=True)
+        print(f"在目标位置放置红色标记: [{target_x:.3f}, {target_y:.3f}, {target_z:.3f}]")
 
     # 计算圆弧长度
-    arc_length = abs(angle_rad * r)
+    arc_length = abs(angle_rad * effective_r)
 
     # 进入循环，实时判断
     distance_moved = 0
@@ -535,10 +580,13 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
         if observe:
             print(f"进入减速阶段")
         
-        # 减速到50%
+        # 减速到50%，保持前进速度和角速度的比例关系
+        reduced_w = w * 0.5
+        reduced_v = abs(reduced_w * effective_r)  # 维持圆弧关系
+        
         msg.mode = 11
         msg.gait_id = 26
-        msg.vel_des = [v * 0.5, 0, w * 0.5]
+        msg.vel_des = [reduced_v, 0, reduced_w]  # 维持圆弧关系
         msg.duration = 0
         msg.step_height = [0.06, 0.06]
         msg.life_count += 1
@@ -664,6 +712,21 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
     final_yaw = ctrl.odo_msg.rpy[2]
     final_angle_turned = final_yaw - start_yaw
     
+    # 记录实际结束位置
+    end_position = list(ctrl.odo_msg.xyz)
+    
+    # 在实际结束位置放置黄色标记，与预计的目标位置进行对比
+    if observe and hasattr(ctrl, 'place_marker'):
+        ctrl.place_marker(end_position[0], end_position[1], 
+                        end_position[2] if len(end_position) > 2 else 0.0, 
+                        'yellow', observe=True)
+        print(f"在实际结束位置放置黄色标记: {end_position}")
+        
+        # 计算实际位置与预期位置的偏差
+        position_error = math.sqrt((end_position[0] - target_x)**2 + 
+                                   (end_position[1] - target_y)**2)
+        print(f"位置误差: {position_error:.3f}米")
+    
     # 角度归一化处理
     while final_angle_turned > math.pi:
         final_angle_turned -= 2 * math.pi