diff --git a/base_move/center_on_dual_tracks.py b/base_move/center_on_dual_tracks.py index 19ccab3..f2f15d7 100644 --- a/base_move/center_on_dual_tracks.py +++ b/base_move/center_on_dual_tracks.py @@ -5,4 +5,282 @@ import os sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing +from utils.detect_dual_track_lines import detect_dual_track_lines, auto_detect_dual_track_lines +def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=False, + mode=11, gait_id=26, step_height=[0.06, 0.06]): + """ + 控制机器狗仅使用Y轴移动调整到双轨道线的中间位置 + + 参数: + ctrl: Robot_Ctrl 对象，包含里程计信息 + msg: robot_control_cmd_lcmt 对象，用于发送命令 + max_time: 最大调整时间(秒)，默认为15秒 + max_deviation: 允许的最大偏差(像素)，当偏差小于此值时认为已居中，默认为8像素 + observe: 是否输出中间状态信息和可视化结果，默认为False + mode: 控制模式，默认为11 + gait_id: 步态ID，默认为26 + step_height: 抬腿高度，默认为[0.06, 0.06] + + 返回: + bool: 是否成功调整到中心位置 + """ + section("开始双轨道居中", "轨道居中") + + # 设置移动命令基本参数 + msg.mode = mode + msg.gait_id = gait_id + msg.duration = 0 # wait next cmd + msg.step_height = step_height + + # 记录起始时间 + start_time = time.time() + + # 记录起始位置 + start_position = list(ctrl.odo_msg.xyz) + if observe: + debug(f"起始位置: {start_position}", "位置") + # 在起点放置绿色标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True) + + # PID控制参数 - 增加比例系数提高响应速度 + kp = 0.005 # 比例系数增加 + + # 帧间滤波参数 - 减少滤波长度以提高响应性 + filter_size = 3 # 滤波队列大小 + deviation_queue = [] # 偏差值队列 + + # 统计变量 + detection_success_count = 0 + detection_total_count = 0 + + # 稳定计数器 - 连续几次在中心位置才认为稳定 + stable_count = 0 + required_stable_count = 2 # 降低稳定条件，更快响应 + + # 变量记录最后有效的偏差，用于处理检测失败的情况 + last_valid_deviation = 0 + + # 开始调整循环 + while time.time() - start_time < max_time: + # 获取当前图像 + image = ctrl.image_processor.get_current_image() + + # 检测双轨道线，尝试使用自动检测方法，可能更稳定 + detection_total_count += 1 + + center_info, left_info, right_info = detect_dual_track_lines(image, observe=observe, save_log=True) + + if center_info is not None: + detection_success_count += 1 + + # 获取当前偏差 + current_deviation = center_info["deviation"] + last_valid_deviation = current_deviation # 记录有效偏差 + + # 添加到队列 + deviation_queue.append(current_deviation) + if len(deviation_queue) > filter_size: + deviation_queue.pop(0) + + # 计算滤波后的偏差值 (去除最大和最小值后的平均) + if len(deviation_queue) >= 3: + filtered_deviations = sorted(deviation_queue)[1:-1] if len(deviation_queue) > 2 else deviation_queue + filtered_deviation = sum(filtered_deviations) / len(filtered_deviations) + else: + filtered_deviation = current_deviation + + if observe: + debug(f"原始偏差: {current_deviation:.1f}px, 滤波后: {filtered_deviation:.1f}px", "偏差") + + # 判断是否已经居中 + if abs(filtered_deviation) <= max_deviation: + stable_count += 1 + if observe: + info(f"已接近中心，稳定计数: {stable_count}/{required_stable_count}", "居中") + + # 即使在稳定过程中，也要保持小幅调整以保持居中 + if stable_count < required_stable_count: + # 使用非常小的调整速度 + lateral_velocity = kp * filtered_deviation * 0.5 + max_lateral_velocity = 0.1 # 减小稳定阶段的最大速度 + lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity)) + + msg.vel_des = [0, lateral_velocity, 0] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if stable_count >= required_stable_count: + # 已经稳定在中心位置 + if observe: + success(f"成功居中，最终偏差: {filtered_deviation:.1f}px", "完成") + break + else: + # 不在中心，重置稳定计数 + stable_count = 0 + + # 计算横向移动速度 (只使用y轴移动) + # 注意：偏差为正表示需要向左移动（y轴正方向），偏差为负表示需要向右移动（y轴负方向） + + # 使用动态kp：偏差越大，移动越快；偏差越小，移动越慢 + dynamic_kp = kp + if abs(filtered_deviation) > 50: + dynamic_kp = kp * 1.5 # 大偏差时增加响应 + elif abs(filtered_deviation) < 20: + dynamic_kp = kp * 0.8 # 小偏差时减小响应，避免过冲 + + lateral_velocity = dynamic_kp * filtered_deviation + + # 限制横向移动速度 + max_lateral_velocity = 0.35 # 增加最大横向速度 (米/秒) + lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity)) + + if observe: + debug(f"横向移动速度: {lateral_velocity:.3f}m/s", "控制") + + # 设置速度命令 - 只使用y轴移动，不前进和转向 + msg.vel_des = [0, lateral_velocity, 0] # [前进速度, 侧向速度, 角速度] + + # 发送命令 + msg.life_count += 1 + ctrl.Send_cmd(msg) + else: + warning("未检测到双轨道线", "警告") + + # 如果已经有了一些有效的检测，使用最后一次有效的偏差继续移动 + if len(deviation_queue) > 0: + # 使用衰减的最后偏差值继续移动 + lateral_velocity = kp * last_valid_deviation * 0.7 # 衰减系数，降低未检测到时的移动速度 + max_lateral_velocity = 0.2 # 降低未检测情况下的移动速度 + lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity)) + + msg.vel_des = [0, lateral_velocity, 0] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if observe: + warning(f"使用最后偏差值继续移动: {lateral_velocity:.3f}m/s", "暂停") + else: + # 如果一开始就没有检测到，可以尝试小范围搜索 + if detection_total_count < 10: + if detection_total_count % 2 == 0: + # 向右搜索，增加搜索速度 + msg.vel_des = [0, -0.15, 0] + else: + # 向左搜索，增加搜索速度 + msg.vel_des = [0, 0.15, 0] + + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if observe: + debug("搜索轨道线...", "搜索") + else: + # 超过一定次数仍未检测到，停止搜索 + msg.vel_des = [0, 0, 0] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if observe: + error("无法检测到轨道线，放弃调整", "失败") + break + + # 短暂延时 + time.sleep(0.05) + + # 停止移动 + ctrl.base_msg.stop() + + # 计算最终位置与起始位置的变化 + final_position = ctrl.odo_msg.xyz + dx = final_position[0] - start_position[0] + dy = final_position[1] - start_position[1] + + if observe: + # 在终点放置红色标记 + if hasattr(ctrl, 'place_marker'): + ctrl.place_marker(final_position[0], final_position[1], final_position[2] if len(final_position) > 2 else 0.0, 'red', observe=True) + + info(f"横向移动距离: {abs(dy):.3f}米", "统计") + + # 显示检测成功率 + if detection_total_count > 0: + detection_rate = (detection_success_count / detection_total_count) * 100 + info(f"轨迹检测成功率: {detection_rate:.1f}% ({detection_success_count}/{detection_total_count})", "统计") + + # 判断是否成功 + success = False + if time.time() - start_time >= max_time: + if observe: + warning("超过最大调整时间", "超时") + else: + # 如果因为已稳定在中心而退出循环，则认为成功 + if stable_count >= required_stable_count: + success = True + + return success + +def center_and_follow_dual_tracks(ctrl, msg, distance, speed=0.5, max_centering_time=15, observe=False, + mode=11, gait_id=26, step_height=[0.06, 0.06]): + """ + 先居中到双轨道线中间，然后沿轨道线行走指定距离 + + 参数: + ctrl: Robot_Ctrl 对象，包含里程计信息 + msg: robot_control_cmd_lcmt 对象，用于发送命令 + distance: 目标前进距离(米) + speed: 前进速度(米/秒)，默认为0.5米/秒 + max_centering_time: 最大居中调整时间(秒)，默认为15秒 + observe: 是否输出中间状态信息和可视化结果，默认为False + mode: 控制模式，默认为11 + gait_id: 步态ID，默认为26 + step_height: 抬腿高度，默认为[0.06, 0.06] + + 返回: + bool: 是否成功完成居中和轨道跟随 + """ + section("开始双轨道居中和跟随", "轨道任务") + + # 第一步：居中到轨道中间 + if observe: + info("步骤1: 调整到轨道中间", "居中") + + centering_success = center_on_dual_tracks( + ctrl, msg, + max_time=max_centering_time, + observe=observe, + mode=mode, + gait_id=gait_id, + step_height=step_height, + ) + + if not centering_success: + if observe: + error("居中调整失败，无法继续跟随轨道", "失败") + return False + + # 第二步：沿轨道跟随指定距离 + if observe: + info(f"步骤2: 沿轨道前进 {distance:.2f}米", "跟随") + + # 导入轨道跟随函数 + from base_move.follow_dual_tracks import follow_dual_tracks + + following_success = follow_dual_tracks( + ctrl, msg, + speed=speed, + target_distance=distance, + observe=observe, + mode=mode, + gait_id=gait_id, + step_height=step_height + ) + + if observe: + if following_success: + success("成功完成轨道居中和跟随任务", "完成") + else: + warning("轨道跟随未完全成功", "警告") + + return following_success diff --git a/logs/res/3/dual_track_20250531_060458_547428.jpg b/logs/res/3/dual_track_20250531_060458_547428.jpg new file mode 100644 index 0000000..654e8d0 Binary files /dev/null and b/logs/res/3/dual_track_20250531_060458_547428.jpg differ diff --git a/logs/res/3/dual_track_orig_20250531_060458_547428.jpg b/logs/res/3/dual_track_orig_20250531_060458_547428.jpg new file mode 100644 index 0000000..619308c Binary files /dev/null and b/logs/res/3/dual_track_orig_20250531_060458_547428.jpg differ diff --git a/task_3/task_3.py b/task_3/task_3.py index 2c8e698..f21996c 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -508,7 +508,7 @@ def run_task_3(ctrl, msg): # pass_up_down(ctrl, msg) turn_degree_v2(ctrl, msg, 90, absolute=True) - center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False) + center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False) return diff --git a/utils/detect_dual_track_lines.py b/utils/detect_dual_track_lines.py index d142cdb..e54dd0f 100644 --- a/utils/detect_dual_track_lines.py +++ b/utils/detect_dual_track_lines.py @@ -72,7 +72,7 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True, cv2.waitKey(delay) # 裁剪底部区域重点关注近处的黄线 - bottom_roi_height = int(height * 0.6) # 增加关注区域到图像底部60% + bottom_roi_height = int(height * 0.4) # 增加关注区域到图像底部60% bottom_roi = mask[height-bottom_roi_height:, :] if observe: