diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 0815d93..a51b53b 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -31,12 +31,11 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): attempts = 0 aligned = False image = ctrl.image_processor.get_current_image() - last_angle_to_rotate = 0 - accumulated_angle = 0 # 添加累积角度跟踪 - max_total_rotation = 45 # 防止过度累积旋转 - angle_accuracy_threshold = 2.0 # 改为更宽松的阈值 + accumulated_angle = 0 + max_total_rotation = 45 + angle_accuracy_threshold = 2.0 - # 记录初始位置，用于可能的回退 + # 记录初始位置 initial_yaw = ctrl.odo_msg.rpy[2] while attempts < max_attempts and not aligned: @@ -50,15 +49,14 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): # 尝试小幅度摇头寻找横线 if attempts < max_attempts - 1: - small_angle = 5 * (1 if attempts % 2 == 0 else -1) # 左右交替摇头 + small_angle = 5 * (1 if attempts % 2 == 0 else -1) info(f"尝试摇头 {small_angle}度 寻找横线", "校准") turn_degree(ctrl, msg, small_angle, absolute=False) - time.sleep(0.5) # 等待稳定 + time.sleep(0.5) attempts += 1 continue - # 获取检测到的斜率和其他信息 slope = edge_info["slope"] is_horizontal = edge_info["is_horizontal"] @@ -66,80 +64,54 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): debug(f"检测到横向线，斜率: {slope:.6f}", "检测") info(f"是否足够水平: {is_horizontal}", "检测") - # 如果已经水平，则无需旋转 if is_horizontal: success("横向线已经水平，无需校准", "完成") return True # 计算需要旋转的角度 - # 斜率 = tan(θ)，因此 θ = arctan(斜率) angle_rad = math.atan(slope) angle_deg = math.degrees(angle_rad) + angle_to_rotate = -angle_deg # 取负值使旋转方向正确 - # 调整角度方向 - # 正的斜率意味着线条从左到右上升，需要逆时针旋转校正 - # 负的斜率意味着线条从左到右下降，需要顺时针旋转校正 - # 注意旋转方向: 顺时针为负角度，逆时针为正角度 - raw_angle_to_rotate = -angle_deg # 取负值使旋转方向正确 + # 设置最小旋转阈值 + if abs(angle_to_rotate) < 0.5: # 降低最小旋转阈值 + angle_to_rotate = math.copysign(0.5, angle_to_rotate) - # 动态调整补偿系数：对于反复校准同一方向的情况，增加补偿 - if last_angle_to_rotate * raw_angle_to_rotate > 0: # 如果方向一致 - # 原来的角度太小，需要更大的补偿 - compensation_factor = 1.3 + min(0.3, attempts * 0.1) # 随着尝试次数增加补偿 - else: - # 方向不一致，可能过度补偿，降低系数 - compensation_factor = 0.8 + # 限制单次旋转角度 + angle_to_rotate = max(-15, min(15, angle_to_rotate)) - # 应用补偿 - angle_to_rotate = raw_angle_to_rotate * compensation_factor - - # 避免角度过小导致效果不明显 - if abs(angle_to_rotate) < 2.0: - # 对小角度增加最小旋转阈值 - angle_to_rotate = math.copysign(2.0, angle_to_rotate) - - # 限制单次旋转角度，避免过度旋转 - angle_to_rotate = max(-20, min(20, angle_to_rotate)) - - # 检查累积旋转是否过大 + # 检查累积旋转 if abs(accumulated_angle + angle_to_rotate) > max_total_rotation: warning(f"累积旋转角度({accumulated_angle + angle_to_rotate:.2f}°)过大，限制旋转", "警告") - # 如果过大，只旋转一半的角度 angle_to_rotate = angle_to_rotate / 2 if observe: - info(f"原始角度: {raw_angle_to_rotate:.2f}度, 补偿后: {angle_to_rotate:.2f}度", "角度") + info(f"计算旋转角度: {angle_to_rotate:.2f}度", "角度") info(f"当前累积旋转: {accumulated_angle:.2f}度", "累积") - # 使用turn_degree函数执行旋转，增加精度参数 + # 执行旋转 turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False, precision=True) - - # 等待稳定 time.sleep(0.3) - # 旋转后实际改变的角度 + # 计算实际旋转角度 current_yaw = ctrl.odo_msg.rpy[2] actual_rotation = math.degrees(current_yaw - initial_yaw - math.radians(accumulated_angle)) - # 规范化到 -180 到 180 度范围 + # 规范化角度 while actual_rotation > 180: actual_rotation -= 360 while actual_rotation < -180: actual_rotation += 360 - # 更新累积角度 accumulated_angle += actual_rotation if observe: info(f"请求旋转: {angle_to_rotate:.2f}度, 实际旋转: {actual_rotation:.2f}度", "旋转") info(f"旋转结果: {'成功' if turn_success else '失败'}", "成功" if turn_success else "失败") - # 增加尝试次数 attempts += 1 - last_angle_to_rotate = angle_to_rotate - # 判断成功条件：基于当前检测到的斜率，而不仅是运动控制的结果 - # 重新获取图像和检测横线 + # 检查校准结果 edge_point_after, edge_info_after = detect_horizontal_track_edge( ctrl.image_processor.get_current_image(), observe=observe, @@ -154,28 +126,25 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): if observe: info(f"校准后斜率: {current_slope:.6f}, 角度: {current_angle_deg:.2f}度", "检测") - # 使用更宽松的阈值判断成功 if abs(current_angle_deg) < angle_accuracy_threshold: success(f"校准成功，当前角度: {current_angle_deg:.2f}度", "完成") aligned = True else: warning(f"校准后角度仍超出阈值: {abs(current_angle_deg):.2f}° > {angle_accuracy_threshold}°", "警告") - # 如果到达最大尝试次数且未对齐，但最后一次接近成功，也返回成功 + # 最后一次尝试的妥协处理 if attempts == max_attempts and not aligned and edge_info_after: current_slope = edge_info_after["slope"] current_angle_deg = math.degrees(math.atan(current_slope)) - # 如果接近水平，放宽标准接受这个结果 - if abs(current_angle_deg) < angle_accuracy_threshold * 1.5: # 允许1.5倍阈值 + if abs(current_angle_deg) < angle_accuracy_threshold * 1.5: warning(f"达到最大尝试次数，接受近似结果，当前角度: {current_angle_deg:.2f}度", "妥协") aligned = True - # 如果无法对齐但已累积大量旋转，尝试恢复到最佳状态 + # 恢复最佳状态的处理 if not aligned and abs(accumulated_angle) > 15: warning(f"校准失败，累积旋转{accumulated_angle:.2f}度，尝试恢复到最佳状态", "恢复") - # 重新检测一次 best_edge_point, best_edge_info = detect_horizontal_track_edge( ctrl.image_processor.get_current_image(), observe=observe, @@ -186,7 +155,6 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): best_slope = best_edge_info["slope"] best_angle_deg = math.degrees(math.atan(best_slope)) - # 如果当前状态已经是最好的，不进行调整 if abs(best_angle_deg) < angle_accuracy_threshold * 1.5: info(f"当前状态已经接近水平，角度: {best_angle_deg:.2f}度", "保持") return True