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 follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None, observe=False, mode=11, gait_id=26, step_height=[0.06, 0.06], ): """ 控制机器狗在双轨道线中间行走 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 speed: 前进速度(米/秒)，默认为0.5米/秒 max_time: 最大行走时间(秒)，默认为30秒 target_distance: 目标移动距离(米)，如果设置，则达到此距离后停止 observe: 是否输出中间状态信息和可视化结果，默认为False 返回: bool: 是否成功完成双轨道跟随 """ section("开始双轨道跟随", "轨道跟随") # 设置移动命令基本参数 msg.mode = mode # Locomotion模式 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 target_distance: info(f"目标距离: {target_distance}米", "目标") # 在起点放置绿色标记 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 # 比例系数 ki = 0.0002 # 积分系数 kd = 0.001 # 微分系数 # PID控制变量 previous_error = 0 integral = 0 # 最大角速度限制 max_angular_velocity = 0.5 # rad/s # 记录成功检测到双轨道的次数和总次数，用于计算检测成功率 detection_success_count = 0 detection_total_count = 0 # 帧间滤波参数 filter_size = 5 # 滤波队列大小 deviation_queue = [] # 偏差值队列 last_valid_deviation = 0 # 上一次有效的偏差值 last_valid_center_info = None # 上一次有效的中心信息 # 开始跟随循环 distance_moved = 0 while time.time() - start_time < max_time: # 检查距离条件 if target_distance and distance_moved >= target_distance: if observe: success(f"已达到目标距离 {target_distance}米，停止移动", "完成") break # 获取当前图像 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 # 保存有效的中心信息 last_valid_center_info = center_info # 获取当前偏差 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 and time.time() % 0.5 < 0.02: debug(f"原始偏差: {current_deviation}px, 滤波后: {filtered_deviation}px", "滤波") # 计算PID控制 error = -filtered_deviation # 负值表示需要向右转，正值表示需要向左转 # 比例项 p_control = kp * error # 积分项 (添加积分限制以防止积分饱和) integral += error integral = max(-1000, min(1000, integral)) # 限制积分范围 i_control = ki * integral # 微分项 derivative = error - previous_error d_control = kd * derivative previous_error = error # 计算总的角速度控制量 angular_velocity = p_control + i_control + d_control # 限制角速度范围 angular_velocity = max(-max_angular_velocity, min(max_angular_velocity, angular_velocity)) # 动态调整前进速度 - 偏差越大，速度越慢 adaptive_speed = speed if abs(filtered_deviation) > 100: # 如果偏差较大 # 根据偏差大小动态调整速度，最低降至60% deviation_factor = max(0.6, 1.0 - abs(filtered_deviation) / 1000.0) adaptive_speed = speed * deviation_factor if observe and time.time() % 0.5 < 0.02: debug(f"偏差较大({filtered_deviation:.1f}px)，调整速度至 {adaptive_speed:.2f}m/s", "速度") if observe and time.time() % 0.5 < 0.02: debug(f"偏差: {filtered_deviation:.1f}px, P: {p_control:.4f}, I: {i_control:.4f}, D: {d_control:.4f}, 角速度: {angular_velocity:.4f}", "控制") # 设置速度命令 msg.vel_des = [adaptive_speed, 0, angular_velocity] else: warning("未检测到双轨道线", "警告") # 如果有之前的有效检测结果，使用上一次的控制值来平滑过渡 if last_valid_center_info is not None and len(deviation_queue) > 0: # 使用上一次的有效偏差，但降低其影响（乘以衰减因子） decay_factor = 0.8 decayed_deviation = last_valid_deviation * decay_factor # 计算PID控制 error = -decayed_deviation # 更新PID控制值但不更新积分项 p_control = kp * error i_control = ki * integral d_control = 0 # 不使用微分项以避免抖动 # 计算总的角速度控制量，但降低其幅度 angular_velocity = (p_control + i_control) * 0.7 # 限制角速度范围 angular_velocity = max(-max_angular_velocity/2, min(max_angular_velocity/2, angular_velocity)) # 降低速度以增加安全性 reduced_speed = speed * 0.8 if observe: warning(f"使用上一帧数据，偏差: {decayed_deviation:.1f}px, 角速度: {angular_velocity:.4f}, 速度: {reduced_speed:.2f}m/s", "恢复") # 设置速度命令 msg.vel_des = [reduced_speed, 0, angular_velocity] else: # 如果连续多次无法检测到轨道，降低速度直线行走 if detection_total_count > 5 and detection_success_count / detection_total_count < 0.3: reduced_speed = speed * 0.6 # 降低至60%的速度 msg.vel_des = [reduced_speed, 0, 0] if observe: warning(f"检测成功率低，降低速度至 {reduced_speed:.2f}m/s", "警告") else: # 如果刚开始无法检测，使用原速度直线行走 msg.vel_des = [speed, 0, 0] # 发送命令 msg.life_count += 1 ctrl.Send_cmd(msg) # 短暂延时 time.sleep(0.05) # 计算已移动距离 current_position = ctrl.odo_msg.xyz dx = current_position[0] - start_position[0] dy = current_position[1] - start_position[1] distance_moved = math.sqrt(dx*dx + dy*dy) if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 info(f"已移动: {distance_moved:.3f}米", "移动") if target_distance: progress = (distance_moved / target_distance) * 100 info(f"进度: {progress:.1f}%", "进度") # 平滑停止 if observe: info("开始平滑停止", "停止") # 先降低速度再停止，实现平滑停止 slowdown_steps = 5 for i in range(slowdown_steps, 0, -1): slowdown_factor = i / slowdown_steps msg.vel_des = [speed * slowdown_factor, 0, 0] msg.life_count += 1 ctrl.Send_cmd(msg) time.sleep(0.1) # 最后完全停止 ctrl.base_msg.stop() # 计算最终移动距离 final_position = ctrl.odo_msg.xyz dx = final_position[0] - start_position[0] dy = final_position[1] - start_position[1] final_distance = math.sqrt(dx*dx + dy*dy) if observe: # 在终点放置红色标记 end_position = list(final_position) if hasattr(ctrl, 'place_marker'): ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True) success(f"双轨道跟随完成，总移动距离: {final_distance:.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})", "统计") # 如果有目标距离，判断是否达到目标 if target_distance: is_success = abs(final_distance - target_distance) < 0.2 # 误差在20cm内算成功 if observe: if is_success: success(f"成功到达目标距离，误差: {abs(final_distance - target_distance):.2f}米", "成功") else: warning(f"未能精确到达目标距离，误差: {abs(final_distance - target_distance):.2f}米", "警告") return is_success return True def go_straight_with_visual_track(ctrl, msg, distance, speed=0.5, observe=False, mode=11, gait_id=26, step_height=[0.06, 0.06], stone_path_mode=False): """ 控制机器人沿直线行走指定距离，同时利用视觉检测黄色轨道线进行自动校准 参数: ctrl: Robot_Ctrl 对象，包含里程计信息和相机 msg: robot_control_cmd_lcmt 对象，用于发送命令 distance: 要行走的距离(米)，正值为前进，负值为后退 speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5 observe: 是否输出中间状态信息，默认为False mode: 控制模式，默认为11 gait_id: 步态ID，默认为26 step_height: 抬腿高度，默认为[0.06, 0.06] stone_path_mode: 是否为石板路模式，默认为False 返回: bool: 是否成功完成行走 """ section("开始视觉引导直线移动", "视觉直行") # 参数验证 if abs(distance) < 0.01: info("距离太短，无需移动", "信息") return True # 检查相机是否可用 if not hasattr(ctrl, 'image_processor') or not hasattr(ctrl.image_processor, 'get_current_image'): error("机器人控制器没有提供图像处理器，无法进行视觉跟踪", "错误") return False # 限制速度范围 speed = min(max(abs(speed), 0.1), 1.0) # 确定前进或后退方向 forward = distance > 0 move_speed = speed if forward else -speed abs_distance = abs(distance) # 获取起始位置 start_position = list(ctrl.odo_msg.xyz) start_yaw = ctrl.odo_msg.rpy[2] # 记录起始朝向，用于保持直线 if observe: debug(f"起始位置: {start_position}", "位置") info(f"开始{'前进' if forward else '后退'} {abs_distance:.3f}米，速度: {abs(move_speed):.2f}米/秒", "移动") # 在起点放置标记 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) # 设置移动命令 msg.mode = mode # 控制模式 msg.gait_id = gait_id # 步态ID # 根据需要移动的距离动态调整移动速度 if abs_distance > 1.0: actual_speed = move_speed # 距离较远时用设定速度 elif abs_distance > 0.5: actual_speed = move_speed * 0.8 # 中等距离略微降速 elif abs_distance > 0.2: actual_speed = move_speed * 0.6 # 较近距离降低速度 else: actual_speed = move_speed * 0.4 # 非常接近时用更慢速度 # 设置移动速度和方向 msg.vel_des = [actual_speed, 0, 0] # [前进速度, 侧向速度, 角速度] msg.duration = 0 # wait next cmd msg.step_height = step_height # 抬腿高度 msg.life_count += 1 # 发送命令 ctrl.Send_cmd(msg) # 估算移动时间，但实际上会通过里程计控制 estimated_time = abs_distance / abs(actual_speed) timeout = estimated_time + 3 # 增加超时时间为预计移动时间加3秒 # 使用里程计进行实时监控移动距离 distance_moved = 0 start_time = time.time() last_position = start_position # 动态调整参数 angle_correction_threshold = 0.05 # 角度偏差超过多少弧度开始修正 vision_check_interval = 0.3 # 视觉检查间隔(秒) last_vision_check = start_time vision_correction_gain = 0.005 # 视觉修正增益系数，根据像素偏差换算 slow_down_ratio = 0.85 # 当移动到目标距离的85%时开始减速 completion_threshold = 0.95 # 当移动到目标距离的95%时停止 position_check_interval = 0.1 # 位置检查间隔(秒) last_check_time = start_time # 计算移动方向单位向量（世界坐标系下） direction_vector = [math.cos(start_yaw), math.sin(start_yaw)] if not forward: direction_vector = [-direction_vector[0], -direction_vector[1]] # 视觉跟踪相关变量 last_valid_center_info = None last_valid_deviation = 0 deviation_queue = [] # 用于滤波 filter_size = 5 detection_success_count = 0 detection_total_count = 0 # 监控移动距离和视觉轨迹 while distance_moved < abs_distance * completion_threshold and time.time() - start_time < timeout: current_time = time.time() # 按固定间隔检查位置，减少计算负担 if current_time - last_check_time >= position_check_interval: # 获取当前位置和朝向 current_position = ctrl.odo_msg.xyz current_yaw = ctrl.odo_msg.rpy[2] # 计算在移动方向上的位移 dx = current_position[0] - start_position[0] dy = current_position[1] - start_position[1] # 计算在初始方向上的投影距离（实际前进距离） distance_moved = dx * direction_vector[0] + dy * direction_vector[1] distance_moved = abs(distance_moved) # 确保距离为正值 # 计算垂直于移动方向的偏移量（y方向偏移） y_offset = -dx * direction_vector[1] + dy * direction_vector[0] # 根据前进或后退确定期望方向 expected_direction = start_yaw if forward else (start_yaw + math.pi) % (2 * math.pi) # 使用IMU朝向数据计算角度偏差 yaw_error = current_yaw - expected_direction # 角度归一化 while yaw_error > math.pi: yaw_error -= 2 * math.pi while yaw_error < -math.pi: yaw_error += 2 * math.pi # 计算完成比例 completion_ratio = distance_moved / abs_distance # 根据距离完成情况调整速度 if completion_ratio > slow_down_ratio: # 计算减速系数 slow_factor = 1.0 - (completion_ratio - slow_down_ratio) / (1.0 - slow_down_ratio) # 确保不会减速太多 slow_factor = max(0.2, slow_factor) new_speed = actual_speed * slow_factor if observe and abs(new_speed - msg.vel_des[0]) > 0.05: info(f"减速: {msg.vel_des[0]:.2f} -> {new_speed:.2f} 米/秒 (完成: {completion_ratio*100:.1f}%)", "移动") msg.vel_des[0] = new_speed # 根据角度偏差进行角速度修正 angular_correction = 0 if abs(yaw_error) > angle_correction_threshold: # 计算角速度修正值，偏差越大修正越强 angular_correction = -yaw_error * 0.5 # 比例系数可调整 # 限制最大角速度修正 angular_correction = max(min(angular_correction, 0.2), -0.2) if observe and abs(angular_correction) > 0.05: warning(f"方向修正: 偏差 {math.degrees(yaw_error):.1f}度，应用角速度 {angular_correction:.3f}rad/s", "角度") # 更新最后检查时间和位置 last_check_time = current_time last_position = current_position if observe and current_time - start_time > 1 and (current_time % 1.0 < position_check_interval): debug(f"已移动: {distance_moved:.3f}米, 目标: {abs_distance:.3f}米 (完成: {completion_ratio*100:.1f}%)", "距离") debug(f"Y偏移: {y_offset:.3f}米, 朝向偏差: {math.degrees(yaw_error):.1f}度", "偏移") # 定期进行视觉检查和修正 if current_time - last_vision_check >= vision_check_interval: try: # 获取当前相机帧 frame = ctrl.image_processor.get_current_image() if frame is not None: # 检测轨道线 detection_total_count += 1 if stone_path_mode: # 使用特定的石板路模式 center_info, left_info, right_info = detect_dual_track_lines( frame, observe=False, save_log=False, stone_path_mode=True) else: # 使用自动检测模式 center_info, left_info, right_info = auto_detect_dual_track_lines( frame, observe=False, save_log=False) # 如果成功检测到轨道线，使用它进行修正 if center_info is not None: detection_success_count += 1 # 保存有效的中心信息 last_valid_center_info = center_info # 获取当前偏差 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 # 计算侧向修正速度 lateral_correction = -filtered_deviation * vision_correction_gain # 限制最大侧向速度修正 lateral_correction = max(min(lateral_correction, 0.2), -0.2) if observe and abs(lateral_correction) > 0.05: warning(f"视觉修正: 偏差 {filtered_deviation:.1f}像素，应用侧向速度 {lateral_correction:.3f}m/s", "视觉") # 应用视觉修正，保留当前前进速度和角速度修正 msg.vel_des = [msg.vel_des[0], lateral_correction, msg.vel_des[2]] msg.life_count += 1 ctrl.Send_cmd(msg) if observe and current_time % 0.5 < 0.05: debug(f"视觉检测: 原始偏差 {current_deviation:.1f}像素, 滤波后 {filtered_deviation:.1f}像素", "视觉") debug(f"轨道宽度: {center_info['track_width']:.1f}像素, 石板路模式: {center_info.get('stone_path_mode', False)}", "视觉") debug(f"修正后速度: [{msg.vel_des[0]:.2f}, {msg.vel_des[1]:.2f}, {msg.vel_des[2]:.2f}]", "移动") else: # 如果有之前的有效检测结果，使用上一次的控制值来平滑过渡 if last_valid_center_info is not None and len(deviation_queue) > 0: # 使用上一次的有效偏差，但降低其影响（乘以衰减因子） decay_factor = 0.8 decayed_deviation = last_valid_deviation * decay_factor # 计算侧向修正速度 lateral_correction = -decayed_deviation * vision_correction_gain * 0.7 # 限制最大侧向速度修正 lateral_correction = max(min(lateral_correction, 0.15), -0.15) # 应用衰减后的视觉修正 msg.vel_des = [msg.vel_des[0], lateral_correction, msg.vel_des[2]] msg.life_count += 1 ctrl.Send_cmd(msg) if observe: warning(f"使用上一帧数据，偏差: {decayed_deviation:.1f}像素, 侧向速度: {lateral_correction:.3f}m/s", "恢复") elif observe: warning("未检测到轨道线", "视觉") except Exception as e: error(f"视觉处理异常: {str(e)}", "错误") last_vision_check = current_time time.sleep(0.01) # 小间隔检查位置 # 平滑停止 if observe: info("开始平滑停止", "停止") # 先降低速度再停止，实现平滑停止 slowdown_steps = 5 for i in range(slowdown_steps, 0, -1): slowdown_factor = i / slowdown_steps msg.vel_des = [actual_speed * slowdown_factor, 0, 0] msg.life_count += 1 ctrl.Send_cmd(msg) time.sleep(0.1) # 最后完全停止 if hasattr(ctrl.base_msg, 'stop_smooth'): ctrl.base_msg.stop_smooth() else: ctrl.base_msg.stop() # 获取最终位置和实际移动距离 final_position = ctrl.odo_msg.xyz dx = final_position[0] - start_position[0] dy = final_position[1] - start_position[1] # 计算在初始方向上的投影距离（实际前进距离） actual_distance = dx * direction_vector[0] + dy * direction_vector[1] actual_distance = abs(actual_distance) # 确保距离为正值 # 计算垂直于移动方向的最终偏移量（y方向偏移） final_y_offset = -dx * direction_vector[1] + dy * direction_vector[0] # 计算总移动距离（直线距离） total_distance = math.sqrt(dx*dx + dy*dy) if observe: success(f"移动完成，从里程计计算的移动距离: {actual_distance:.3f}米", "完成") info(f"最终Y偏移: {final_y_offset:.3f}米", "偏移") # 在终点放置标记 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) # 显示检测成功率 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})", "统计") # 判断是否成功完成 distance_error = abs(actual_distance - abs_distance) y_error = abs(final_y_offset) go_success = distance_error < 0.1 and y_error < 0.1 # 如果距离误差和y偏移都小于10厘米，则认为成功 if observe: info(f"目标距离: {abs_distance:.3f}米, 实际距离: {actual_distance:.3f}米, 误差: {distance_error:.3f}米", "距离") info(f"Y偏移: {final_y_offset:.3f}米", "偏移") if go_success: success(f"移动成功", "成功") else: warning(f"移动{'部分成功' if distance_error < 0.1 or y_error < 0.1 else '失败'}", "结果") if distance_error >= 0.1: warning(f"距离误差过大: {distance_error:.3f}米", "距离") if y_error >= 0.1: warning(f"Y偏移过大: {y_error:.3f}米", "偏移") return go_success