diff --git a/.DS_Store b/.DS_Store index 918d474..d4d83e6 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 13e0d6a..6dafff4 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -413,7 +413,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc elif angle_deg <= 90: time_compensation = 0.85 # 90度左右使用稍大的系数 else: - time_compensation = 0.92 # 大角度保持原有补偿 + time_compensation = 0.75 # 大角度(如180度)减小补偿系数，防止过度旋转 # 调整实际目标角度，针对不同角度应用不同的缩放比例 actual_angle_deg = angle_deg @@ -421,6 +421,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc actual_angle_deg = angle_deg * 0.85 # 70度及以下角度减少到85% elif angle_deg <= 90: actual_angle_deg = angle_deg * 0.9 # 90度减少到90% + elif angle_deg >= 170: # 处理大角度旋转，特别是180度情况 + actual_angle_deg = angle_deg * 0.75 # 减少到75%，防止过度旋转 if observe and actual_angle_deg != angle_deg: print(f"应用角度补偿: 目标{angle_deg}度 -> 实际指令{actual_angle_deg:.1f}度") @@ -429,7 +431,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 设定角速度（rad/s），可根据实际调整 # 减小基础角速度，增加精度 - base_w = 0.8 # 原来是0.6 + if angle_deg >= 170: + base_w = 0.6 # 大角度旋转时使用更小的基础角速度，提高精度 + else: + base_w = 0.8 # 原来是0.6 w = base_w if left else -base_w # 左转为正，右转为负 v = abs(w * r) # 线速度，正负号与角速度方向一致 @@ -470,16 +475,22 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc current_v = v current_w = w - # 旋转精度监控 - target_angle = angle_rad if left else -angle_rad # 目标角度（弧度） # 对小角度提前开始减速 if angle_deg <= 70: slowdown_threshold = 0.65 # 当达到目标角度的65%时开始减速 + elif angle_deg >= 170: # 针对大角度(如180度)采用更早的减速时机 + slowdown_threshold = 0.6 # 当达到目标角度的60%时开始减速 else: slowdown_threshold = 0.75 # 对于大角度，75%时开始减速（原来是80%） + # 根据角度大小调整旋转停止阈值 + if angle_deg >= 170: + rotation_completion_threshold = 0.85 # 大角度旋转提前停止，避免惯性导致过度旋转 + else: + rotation_completion_threshold = 0.95 # 默认旋转到95%时停止 + # 监控旋转进度并自行控制减速 - while abs(angle_turned) < abs(angle_rad) * 0.95 and time.time() - start_time < timeout: + while abs(angle_turned) < abs(angle_rad) * rotation_completion_threshold and time.time() - start_time < timeout: # 计算已移动距离 current_position = ctrl.odo_msg.xyz dx = current_position[0] - start_position[0] @@ -510,10 +521,23 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 当完成一定比例时开始减速 if completion_ratio > slowdown_threshold: - # 剩余比例作为速度系数 - speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold) - # 确保速度系数不会太小，但可以更慢一些 - speed_factor = max(0.15, speed_factor) # 原来是0.2 + # 实现多阶段减速，特别针对大角度旋转 + if angle_deg >= 170: + if completion_ratio < 0.7: # 第一阶段：60%-70% + speed_factor = 0.7 + elif completion_ratio < 0.8: # 第二阶段：70%-80% + speed_factor = 0.4 + else: # 第三阶段：80%以上 + speed_factor = 0.2 + else: + # 标准减速逻辑保持不变 + # 剩余比例作为速度系数 + speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold) + + # 根据角度调整最小速度因子 + min_speed_factor = 0.15 # 默认最小速度因子 + # 确保速度系数不会太小，但可以更慢一些 + speed_factor = max(min_speed_factor, speed_factor) # 应用减速 current_v = v * speed_factor @@ -528,6 +552,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc msg.life_count += 1 ctrl.Send_cmd(msg) + # 针对180度旋转，在减速命令后稍作等待，以确保减速效果 + if angle_deg >= 170: + time.sleep(0.03) # 短暂延迟，让减速命令更有效 + if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 当前角速度: {current_w:.3f}rad/s") else: @@ -575,7 +603,13 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 检查是否需要微调 angle_error = abs(final_angle_deg - target_angle_deg) - if angle_error > 3.0: # 如果误差超过3度 + + # 针对不同角度使用不同的微调阈值 + adjustment_threshold = 3.0 # 默认微调阈值 + if angle_deg >= 170: + adjustment_threshold = 5.0 # 大角度旋转使用更大的微调阈值 + + if angle_error > adjustment_threshold: # 如果误差超过阈值 if observe: print(f"角度误差: {angle_error:.2f}度，进行微调") @@ -585,16 +619,53 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 增加补偿系数，因为实际旋转常会超出理论计算值 if angle_deg <= 70: compensation_factor = 0.7 # 小角度使用更小的补偿因子 + elif angle_deg >= 170: # 单独处理180度附近的大角度 + compensation_factor = 0.5 # 大角度使用更小的补偿因子，防止过度校正 else: - compensation_factor = 0.85 # 只旋转计算值的85% + compensation_factor = 0.85 # 一般角度只旋转计算值的85% - adjust_angle *= compensation_factor - - if observe: - print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度") - - # 使用turn_degree函数进行微调 - turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False) + # 对于特别大的误差进行多次微调 + if angle_deg >= 170 and angle_error > 20: + # 先进行粗略校正，只校正70% + first_adjust_angle = adjust_angle * 0.7 + if observe: + print(f"误差较大，进行两阶段校正，第一阶段调整: {first_adjust_angle:.2f}度") + + turn_success = turn_degree(ctrl, msg, first_adjust_angle, absolute=False) + + # 等待稳定后再次检查角度 + time.sleep(0.8) + + # 计算剩余误差 + current_yaw = ctrl.odo_msg.rpy[2] + current_angle_turned = current_yaw - start_yaw + # 角度归一化处理 + while current_angle_turned > math.pi: + current_angle_turned -= 2 * math.pi + while current_angle_turned < -math.pi: + current_angle_turned += 2 * math.pi + + current_angle_deg = math.degrees(current_angle_turned) + remaining_error = target_angle_deg - current_angle_deg + + if observe: + print(f"第一阶段校正后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度") + + # 对剩余误差进行更精确的校正 + if abs(remaining_error) > 2.0: + second_adjust_angle = remaining_error * 0.5 # 只校正一半 + if observe: + print(f"进行第二阶段微调: {second_adjust_angle:.2f}度") + + turn_success = turn_degree(ctrl, msg, second_adjust_angle, absolute=False) + else: + adjust_angle *= compensation_factor + + if observe: + print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度") + + # 使用turn_degree函数进行微调 + turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False) if observe: print(f"微调结果: {'成功' if turn_success else '失败'}")