diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index c8725c0..0815d93 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -31,15 +31,32 @@ 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 # 改为更宽松的阈值 + + # 记录初始位置，用于可能的回退 + initial_yaw = ctrl.odo_msg.rpy[2] while attempts < max_attempts and not aligned: section(f"尝试校准横线 {attempts+1}/{max_attempts}", "校准") + # 检测横向线边缘 edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True) if edge_point is None or edge_info is None: error("未检测到横向线，无法进行校准", "失败") - return False + + # 尝试小幅度摇头寻找横线 + if attempts < max_attempts - 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) # 等待稳定 + + attempts += 1 + continue # 获取检测到的斜率和其他信息 slope = edge_info["slope"] @@ -63,36 +80,116 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): # 正的斜率意味着线条从左到右上升，需要逆时针旋转校正 # 负的斜率意味着线条从左到右下降，需要顺时针旋转校正 # 注意旋转方向: 顺时针为负角度，逆时针为正角度 - angle_to_rotate = -angle_deg # 取负值使旋转方向正确 + raw_angle_to_rotate = -angle_deg # 取负值使旋转方向正确 + + # 动态调整补偿系数：对于反复校准同一方向的情况，增加补偿 + 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 = 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"需要旋转的角度: {angle_to_rotate:.2f}度", "角度") + info(f"原始角度: {raw_angle_to_rotate:.2f}度, 补偿后: {angle_to_rotate:.2f}度", "角度") + info(f"当前累积旋转: {accumulated_angle:.2f}度", "累积") - # 执行旋转 - # 如果角度很小，增加一个小的偏移以确保旋转足够 - if abs(angle_to_rotate) < 3.0: - angle_to_rotate *= 1.5 # 对小角度进行放大以确保效果 + # 使用turn_degree函数执行旋转，增加精度参数 + turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False, precision=True) - # 限制旋转角度，避免过度旋转 - angle_to_rotate = max(-30, min(30, angle_to_rotate)) + # 等待稳定 + time.sleep(0.3) - # 使用turn_degree函数执行旋转 - turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False) + # 旋转后实际改变的角度 + 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 - # 在旋转后重新获取图像，这里需要调用获取图像的函数 - # 代码中没有提供获取实时图像的方法，假设每次外部会更新image参数 + # 判断成功条件：基于当前检测到的斜率，而不仅是运动控制的结果 + # 重新获取图像和检测横线 + edge_point_after, edge_info_after = detect_horizontal_track_edge( + ctrl.image_processor.get_current_image(), + observe=observe, + delay=1000 if observe else 0, + save_log=True + ) - # 检查是否已经对齐 - # 对于实际应用，应该在旋转后重新捕获图像并检测横向线 - # 这里简单地根据旋转是否成功和旋转角度是否足够小来判断 - if turn_success and abs(angle_to_rotate) < 5.0: - aligned = True + if edge_info_after and "slope" in edge_info_after: + current_slope = edge_info_after["slope"] + current_angle_deg = math.degrees(math.atan(current_slope)) + + 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倍阈值 + 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, + save_log=True + ) + + if best_edge_info and "slope" in best_edge_info: + 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 return aligned diff --git a/base_move/turn_degree.py b/base_move/turn_degree.py index 02950f3..19e9ca2 100644 --- a/base_move/turn_degree.py +++ b/base_move/turn_degree.py @@ -9,7 +9,7 @@ from utils.log_helper import LogHelper, get_logger, section, info, debug, warnin # 创建本模块特定的日志记录器 logger = get_logger("旋转控制") -def turn_degree(ctrl, msg, degree=90, absolute=False): +def turn_degree(ctrl, msg, degree=90, absolute=False, precision=False): """ 结合里程计实现精确稳定的旋转指定角度 @@ -18,6 +18,7 @@ def turn_degree(ctrl, msg, degree=90, absolute=False): msg: robot_control_cmd_lcmt 对象，用于发送命令 degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度 absolute: 是否绝对角度，默认为 False + precision: 是否启用高精度模式，该模式下会使用更慢的速度和更细致的微调，默认为 False 返回: Bool: 是否成功旋转到指定角度 @@ -41,7 +42,8 @@ def turn_degree(ctrl, msg, degree=90, absolute=False): target_yaw += 2 * math.pi # 定义允许的误差范围（弧度） - limit = 0.04 # 约2.3度 + # 如果是精确模式，使用更小的误差阈值 + limit = 0.03 if precision else 0.04 # 约1.7度或2.3度 # 计算最短旋转方向和距离 def circle_dist(target, location): @@ -63,22 +65,77 @@ def turn_degree(ctrl, msg, degree=90, absolute=False): # 主要转向 const_int = 2470 # 转1.57弧度约需2470的duration值 + # 精确模式下使用更小的旋转速度 + turn_speed = 0.3 if precision else 0.5 + + # 如果角度很小且使用精确模式，进一步降低速度 + if precision and abs(dist) < 0.2: # 约11.5度 + turn_speed = 0.2 + # 设置转向命令 msg.mode = 11 # Locomotion模式 msg.gait_id = 26 # 自变频步态 - msg.vel_des = [0, 0, 0.5 if direction > 0 else -0.5] # 转向速度 - msg.duration = int(const_int * abs(dist)) + msg.vel_des = [0, 0, turn_speed if direction > 0 else -turn_speed] # 转向速度 + + # 精确模式下延长转向时间以保证稳定性 + duration_factor = 1.2 if precision else 1.0 + msg.duration = int(const_int * abs(dist) * duration_factor) msg.step_height = [0.06, 0.06] # 抬腿高度 msg.life_count += 1 # 发送命令 ctrl.Send_cmd(msg) - debug(f"发送旋转命令：方向={'逆时针' if direction > 0 else '顺时针'}, 持续时间={msg.duration}", "旋转") + debug(f"发送旋转命令：方向={'逆时针' if direction > 0 else '顺时针'}, 速度={turn_speed}, 持续时间={msg.duration}", "旋转") # 等待转向完成 - wait_time = 7 * abs(dist) / 1.57 + # 精确模式下增加等待时间 + wait_factor = 1.3 if precision else 1.0 + wait_time = 7 * abs(dist) / 1.57 * wait_factor debug(f"等待旋转完成: {wait_time:.2f}秒", "时间") - time.sleep(wait_time) + + # 精确模式下使用实时监控而不是固定等待时间 + if precision and abs(dist) > 0.1: # 对于较大角度 + start_time = time.time() + last_yaw = current_yaw + stable_count = 0 + timeout = wait_time + 3 # 增加超时保护 + + # 实时监控旋转进度 + while time.time() - start_time < timeout: + time.sleep(0.1) # 频繁检查 + + current_yaw_now = ctrl.odo_msg.rpy[2] + # 计算已旋转角度 + rotated = abs(current_yaw_now - last_yaw) + if rotated > math.pi: + rotated = 2 * math.pi - rotated + + # 如果旋转速度很小（几乎停止） + if rotated < 0.01: # 约0.6度 + stable_count += 1 + else: + stable_count = 0 + + last_yaw = current_yaw_now + + # 如果机器人稳定一段时间，认为旋转完成 + if stable_count >= 3: + debug(f"检测到旋转已稳定，提前结束等待", "旋转") + break + + # 每0.5秒打印一次进度 + elapsed = time.time() - start_time + if int(elapsed * 2) % 2 == 0: + remaining_yaw = target_yaw - current_yaw_now + # 标准化到 [-pi, pi] + if remaining_yaw > math.pi: + remaining_yaw -= 2 * math.pi + if remaining_yaw < -math.pi: + remaining_yaw += 2 * math.pi + debug(f"旋转进度: {elapsed:.1f}s/{wait_time:.1f}s, 剩余角度: {math.degrees(remaining_yaw):.2f}°", "进度") + else: + # 非精确模式使用固定等待时间 + time.sleep(wait_time) # 获取当前角度 current_yaw = ctrl.odo_msg.rpy[2] @@ -94,23 +151,35 @@ def turn_degree(ctrl, msg, degree=90, absolute=False): # 精细调整（如果误差大于限制） if abs(remaining_dist) > limit: warning(f"剩余误差: {math.degrees(remaining_dist):.2f}°, 需要进行微调", "角度") + # 进行微调 - const_int_tiny = 1200 # 微调使用较小的系数 + const_int_tiny = 1200 if not precision else 1000 # 精确模式下使用更小的系数 + + # 精确模式下使用更小的微调速度 + fine_turn_speed = 0.3 if precision else 0.5 + + # 如果剩余误差很小，进一步降低速度 + if abs(remaining_dist) < 0.1: # 约5.7度 + fine_turn_speed = 0.15 # 设置微调命令 msg.mode = 11 msg.gait_id = 26 - msg.vel_des = [0, 0, 0.5 if remaining_dist > 0 else -0.5] - msg.duration = int(const_int_tiny * abs(remaining_dist)) + msg.vel_des = [0, 0, fine_turn_speed if remaining_dist > 0 else -fine_turn_speed] + + # 精确模式下使用更长的微调时间 + duration_factor = 1.25 if precision else 1.0 + msg.duration = int(const_int_tiny * abs(remaining_dist) * duration_factor) msg.step_height = [0.06, 0.06] msg.life_count += 1 # 发送命令 ctrl.Send_cmd(msg) - debug(f"发送微调命令：方向={'逆时针' if remaining_dist > 0 else '顺时针'}, 持续时间={msg.duration}", "旋转") + debug(f"发送微调命令：方向={'逆时针' if remaining_dist > 0 else '顺时针'}, 速度={fine_turn_speed}, 持续时间={msg.duration}", "旋转") - # 等待微调完成 - time.sleep(5) + # 精确模式下等待更长时间 + wait_time = 5 if not precision else 7 + time.sleep(wait_time) # 获取最终角度 final_yaw = ctrl.odo_msg.rpy[2] @@ -126,6 +195,51 @@ def turn_degree(ctrl, msg, degree=90, absolute=False): return True else: warning(f"旋转完成，但误差超出允许范围: {math.degrees(final_error):.2f}° > {math.degrees(limit):.2f}°", "警告") + + # 在精确模式下，如果误差仍然很大，尝试第二次微调 + if precision and final_error > limit * 2: + info("尝试第二次微调", "精确") + + # 计算第二次微调角度 + second_adj = target_yaw - final_yaw + if second_adj > math.pi: + second_adj -= 2 * math.pi + elif second_adj < -math.pi: + second_adj += 2 * math.pi + + # 降低微调速度 + very_fine_speed = 0.1 + + # 设置第二次微调命令 + msg.mode = 11 + msg.gait_id = 26 + msg.vel_des = [0, 0, very_fine_speed if second_adj > 0 else -very_fine_speed] + msg.duration = int(800 * abs(second_adj)) # 使用更小的系数 + msg.step_height = [0.06, 0.06] + msg.life_count += 1 + + # 发送命令 + ctrl.Send_cmd(msg) + debug(f"第二次微调：旋转{math.degrees(second_adj):.2f}°", "精确") + + # 等待第二次微调完成 + time.sleep(3) + + # 获取最终结果 + final_final_yaw = ctrl.odo_msg.rpy[2] + + final_final_error = abs(target_yaw - final_final_yaw) + if final_final_error > math.pi: + final_final_error = 2 * math.pi - final_final_error + + if final_final_error <= limit: + success(f"第二次微调成功，最终误差: {math.degrees(final_final_error):.2f}°", "成功") + return True + else: + warning(f"两次微调后仍有误差: {math.degrees(final_final_error):.2f}°", "警告") + # 如果误差已经比第一次小，算作改进 + return final_final_error < final_error + return False success("旋转成功完成", "完成")