import math import time import sys 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