diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 968ce8f..dfb2eb1 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -1222,14 +1222,14 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None, return True -def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=30, observe=False): +def follow_left_side_track(ctrl, msg, target_distance=540, speed=0.3, max_time=30, observe=False): """ 控制机器狗向左侧移动并靠近左侧的黄色轨迹线，只进行侧向移动，不进行前进 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 - target_distance: 目标与左侧线的像素距离，默认为150像素（保持一定间距） + target_distance: 目标与左侧线的像素距离，默认为540像素（考虑机器狗视角、高度的合适距离） speed: 侧向移动的最大速度(米/秒)，默认为0.3米/秒 max_time: 最大执行时间(秒)，默认为30秒 observe: 是否输出中间状态信息和可视化结果，默认为False @@ -1256,22 +1256,9 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 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控制参数 - 侧向移动的PID参数，根据近距离跟踪需求调整 - kp_side = 0.0025 # 比例系数 - ki_side = 0.0001 # 积分系数 - kd_side = 0.001 # 微分系数 - - # 记录目标到达状态和稳定计数 - target_reached = False - stable_count = 0 - required_stable_count = 8 # 需要连续稳定的检测次数 - - # PID控制变量 - previous_error = 0 - integral = 0 - - # 最大侧向速度限制 - max_side_velocity = speed # 使用参数传入的速度作为最大侧向速度 + # 最大侧向速度限制与动态调整 + max_side_velocity = speed + min_side_velocity = 0.05 # 最小侧向速度，确保能够缓慢移动 # 检测成功计数器 detection_success_count = 0 @@ -1280,12 +1267,17 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 # 保存上一次有效的检测结果，用于检测失败时的平滑过渡 last_valid_track_info = None - # 滤波队列 - filter_size = 5 + # 滤波队列 - 增大滤波窗口以提高稳定性 + filter_size = 7 distance_queue = [] - # 动态调整目标距离的阈值 - target_reached_threshold = 25 # 在目标距离±25像素范围内视为达到目标 + # 设置差值阈值 - 当距离与目标的差值小于此阈值时，认为已达到目标 + distance_threshold = 25 # 像素误差阈值 + + # 记录目标到达状态和稳定计数 + target_reached = False + stable_count = 0 + required_stable_count = 8 # 连续多帧满足条件才算稳定到达目标 # 开始跟踪循环 while time.time() - start_time < max_time: @@ -1310,64 +1302,69 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 if len(distance_queue) > filter_size: distance_queue.pop(0) - # 计算滤波后的距离值 (去除最大和最小值后的平均) + # 计算滤波后的距离值 - 使用中值滤波，对抗异常值 if len(distance_queue) >= 3: - filtered_distances = sorted(distance_queue)[1:-1] if len(distance_queue) > 2 else distance_queue - filtered_distance = sum(filtered_distances) / len(filtered_distances) + # 中值滤波更能抵抗异常值 + filtered_distance = sorted(distance_queue)[len(distance_queue) // 2] else: filtered_distance = current_distance + # 直接计算当前距离与目标距离的差值 + difference = target_distance - filtered_distance # 正值：需要向左移动；负值：需要向右移动 + if observe and time.time() % 0.5 < 0.02: - debug(f"原始左侧距离: {current_distance:.1f}px, 滤波后: {filtered_distance:.1f}px, 目标: {target_distance}px", "距离") + debug(f"左侧距离: {filtered_distance:.1f}px, 目标距离: {target_distance}px, 差值: {difference:.1f}px", "距离") - # 计算误差 - 正误差表示需要向左移动，负误差表示需要向右移动 - error = target_distance - filtered_distance - - # 检查是否达到目标位置 - if abs(error) < target_reached_threshold: # 误差小于阈值视为达到目标 + # 检查是否达到目标位置（差值小于阈值） + if abs(difference) < distance_threshold: stable_count += 1 - if stable_count >= required_stable_count: - if not target_reached: - target_reached = True - if observe: - success(f"已达到目标位置，误差: {abs(error):.1f}px", "成功") + if stable_count >= required_stable_count and not target_reached: + target_reached = True + if observe: + success(f"已达到目标位置，当前距离: {filtered_distance:.1f}px, 与目标差值: {abs(difference):.1f}px", "成功") + + # 在目标位置附近时，使用非常小的速度进行微调 + # 根据差值比例计算速度，使调整更平滑 + adjustment_factor = difference / distance_threshold # 归一化到[-1,1]范围 + side_velocity = min_side_velocity * adjustment_factor * 0.5 # 乘以0.5使调整更微小 + + # 如果计算出的速度非常小，直接设为0 + if abs(side_velocity) < 0.01: + side_velocity = 0 + + if observe and time.time() % 0.5 < 0.02 and side_velocity != 0: + info(f"接近目标位置，微调: {side_velocity:.3f}m/s", "调整") else: + # 不在目标位置，根据差值计算速度 stable_count = 0 target_reached = False + + # 计算速度系数 - 差值越大，速度越大，但有上限 + # 使用非线性映射，使速度变化更平滑 + if abs(difference) > 200: + # 差值大于200像素时，使用最大速度 + speed_factor = 1.0 + else: + # 差值在0-200像素之间，速度从0增长到最大 + speed_factor = min(1.0, abs(difference) / 200.0) + # 应用平方根函数使速度增长曲线更合理（开始缓慢，后面加速） + speed_factor = math.sqrt(speed_factor) + + # 应用方向（差值为正向左移动，为负向右移动） + side_velocity = max_side_velocity * speed_factor * (1 if difference > 0 else -1) + + if observe and time.time() % 0.5 < 0.02: + direction = "左" if side_velocity > 0 else "右" + info(f"距离目标较{direction}远({difference:.1f}px)，向{direction}移动: {side_velocity:.3f}m/s", "调整") - # 比例项 - p_control = kp_side * error - - # 积分项 (添加积分限制以防止积分饱和) - integral += error - integral = max(-500, min(500, integral)) # 限制积分范围 - i_control = ki_side * integral - - # 微分项 - derivative = error - previous_error - d_control = kd_side * derivative - previous_error = error - - # 计算侧向速度 - side_velocity = p_control + i_control + d_control - - # 应用非线性控制，加快大距离靠近，减缓小距离微调 - if abs(error) > 100: - # 大误差时，增强响应 - side_velocity *= 1.2 - elif abs(error) < 40: - # 小误差时，减弱响应，增加稳定性 - side_velocity *= 0.7 + # 确保最小移动速度 - 当需要移动但计算值太小时 + if 0 < abs(side_velocity) < min_side_velocity: + side_velocity = min_side_velocity * (1 if side_velocity > 0 else -1) # 限制侧向速度范围 side_velocity = max(-max_side_velocity, min(max_side_velocity, side_velocity)) - # 如果已达到目标，进一步降低侧向速度以减少振荡 - if target_reached: - side_velocity *= 0.4 - if observe and time.time() % 0.5 < 0.02: - debug(f"误差: {error:.1f}px, P: {p_control:.4f}, I: {i_control:.4f}, D: {d_control:.4f}", "控制") debug(f"控制指令: 侧向={side_velocity:.2f}m/s", "速度") # 设置速度命令 - [前进速度, 侧向速度, 角速度] @@ -1380,10 +1377,13 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 slope = track_info["slope"] # 计算旋转角度 - 垂直线的ideal_angle应该是0 angle_correction = -math.atan(1/slope) if abs(slope) > 0.01 else 0 - # 将角度校正应用为小的角速度 - angular_velocity = angle_correction * 0.25 + # 将角度校正应用为小的角速度，并根据距离调整强度 + angle_factor = 1.0 + if target_reached: + angle_factor = 0.5 # 接近目标时减弱角度校正 + angular_velocity = angle_correction * 0.2 * angle_factor # 限制角速度范围 - angular_velocity = max(-0.25, min(0.25, angular_velocity)) + angular_velocity = max(-0.2, min(0.2, angular_velocity)) if abs(angular_velocity) > 0.01: # 只在需要明显校正时应用 msg.vel_des[2] = angular_velocity @@ -1395,27 +1395,34 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 # 如果之前有有效检测结果，使用上一次的控制值但降低强度 if last_valid_track_info is not None and len(distance_queue) > 0: - # 如果上次距离已经接近目标，维持当前位置 - last_error = target_distance - distance_queue[-1] - if abs(last_error) < 50: # 如果接近目标距离 - side_velocity = 0 # 停止侧向移动 + # 获取上次的滤波距离 + last_filtered_distance = distance_queue[-1] + + # 计算上次距离与目标的差值 + last_difference = target_distance - last_filtered_distance + + # 根据上次差值决定行为 + if abs(last_difference) < distance_threshold: + # 上次接近目标，保持位置 + side_velocity = 0 if observe: - info("接近目标位置，保持当前侧向位置", "保持") + info("上次距离接近目标，保持当前位置", "保持") else: - # 向目标方向缓慢移动 - side_velocity = 0.08 * (1 if last_error > 0 else -1) + # 根据差值方向移动，但速度降低 + direction = 1 if last_difference > 0 else -1 + side_velocity = 0.08 * direction if observe: - info(f"距离目标较远，缓慢向{'左' if side_velocity > 0 else '右'}移动", "调整") + info(f"根据上次距离差值({last_difference:.1f}px)，向{'左' if direction > 0 else '右'}缓慢移动", "调整") msg.vel_des = [0, side_velocity, 0] if observe: - warning(f"使用降低强度的控制: 侧向={side_velocity:.2f}m/s", "恢复") + warning(f"使用上次距离估计控制: 侧向={side_velocity:.2f}m/s", "恢复") else: - # 如果从未检测到轨迹线，默认向左侧移动一点，尝试找到轨迹线 - default_side_velocity = 0.1 + # 如果从未检测到轨迹线，默认向左侧移动一点，尝试寻找 + default_side_velocity = 0.08 msg.vel_des = [0, default_side_velocity, 0] if observe: - warning(f"无法检测到左侧轨迹线，向左移动尝试寻找: {default_side_velocity:.2f}m/s", "警告") + warning(f"无法检测到左侧轨迹线，向左缓慢移动尝试寻找: {default_side_velocity:.2f}m/s", "警告") # 发送命令 msg.life_count += 1 @@ -1441,14 +1448,21 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 if observe: info("开始平滑停止", "停止") - # 平滑停止 - 由于没有前进速度，只需要停止侧向移动 - slowdown_steps = 3 + # 更平滑的停止 + slowdown_steps = 4 for i in range(slowdown_steps, 0, -1): slowdown_factor = i / slowdown_steps - msg.vel_des = [0, 0, 0] # 完全停止所有移动 + if i > 1: + # 前几步只降低速度 + last_vel = msg.vel_des[1] + reduced_vel = last_vel * slowdown_factor + msg.vel_des = [0, reduced_vel, 0] + else: + # 最后一步完全停止 + msg.vel_des = [0, 0, 0] msg.life_count += 1 ctrl.Send_cmd(msg) - time.sleep(0.1) + time.sleep(0.12) # 稍微延长每步的等待时间，使停止更平滑 # 最后完全停止 ctrl.base_msg.stop() diff --git a/logs/robot_2025-05-25.log b/logs/robot_2025-05-25.log index 4fcf520..da45394 100644 --- a/logs/robot_2025-05-25.log +++ b/logs/robot_2025-05-25.log @@ -35,3 +35,23 @@ 2025-05-25 19:52:34 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_195234_036434.jpg 2025-05-25 19:52:34 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_195234_036434.jpg 2025-05-25 19:52:34 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_195234_036434', 'tracking_point': (133, 1077), 'ground_intersection': (128, 1080), 'distance_to_left': 246.0, 'slope': -0.7035398230088495, 'line_mid_x': 246.0} +2025-05-25 21:00:13 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-25 21:00:15 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-25 21:00:16 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 +2025-05-25 21:00:17 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-25 21:00:18 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线 +2025-05-25 21:00:19 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线 +2025-05-25 21:00:20 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 +2025-05-25 21:00:21 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_210021_481763.jpg +2025-05-25 21:00:21 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_210021_481763.jpg +2025-05-25 21:00:21 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_210021_481763', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0} +2025-05-25 21:20:33 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-25 21:20:34 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-25 21:20:35 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 +2025-05-25 21:20:36 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-25 21:20:37 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线 +2025-05-25 21:20:38 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线 +2025-05-25 21:20:39 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 +2025-05-25 21:20:40 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_212040_471018.jpg +2025-05-25 21:20:40 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_212040_471018.jpg +2025-05-25 21:20:40 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_212040_471018', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0} diff --git a/logs/robot_2025-05-26.log b/logs/robot_2025-05-26.log new file mode 100644 index 0000000..c68d2ee --- /dev/null +++ b/logs/robot_2025-05-26.log @@ -0,0 +1,30 @@ +2025-05-26 00:20:28 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-26 00:20:29 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-26 00:20:30 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 +2025-05-26 00:20:31 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-26 00:20:32 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线 +2025-05-26 00:20:33 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线 +2025-05-26 00:20:34 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 +2025-05-26 00:20:35 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250526_002035_779025.jpg +2025-05-26 00:20:35 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250526_002035_779025.jpg +2025-05-26 00:20:35 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250526_002035_779025', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0} +2025-05-26 00:21:27 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-26 00:21:28 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-26 00:21:29 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 +2025-05-26 00:21:30 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-26 00:21:31 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线 +2025-05-26 00:21:32 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线 +2025-05-26 00:21:33 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 +2025-05-26 00:21:34 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250526_002134_938675.jpg +2025-05-26 00:21:34 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250526_002134_938675.jpg +2025-05-26 00:21:34 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250526_002134_938675', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0} +2025-05-26 00:25:05 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-26 00:25:06 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-26 00:25:07 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 +2025-05-26 00:25:08 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-26 00:25:10 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 74 条直线 +2025-05-26 00:25:11 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 2 条垂直线 +2025-05-26 00:25:12 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 +2025-05-26 00:25:13 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250526_002513_050661.jpg +2025-05-26 00:25:13 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250526_002513_050661.jpg +2025-05-26 00:25:13 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250526_002513_050661', 'tracking_point': (549, 1071), 'ground_intersection': (543, 1080), 'distance_to_left': 584.5, 'slope': -1.619718309859155, 'line_mid_x': 584.5} diff --git a/res/path/left/1.png b/res/path/left/1.png new file mode 100644 index 0000000..3a4add7 Binary files /dev/null and b/res/path/left/1.png differ diff --git a/res/path/left/2.png b/res/path/left/2.png new file mode 100644 index 0000000..f18f608 Binary files /dev/null and b/res/path/left/2.png differ diff --git a/res/path/test/left_track_results/result_2.png b/res/path/test/left_track_results/result_2.png new file mode 100644 index 0000000..5b07e69 Binary files /dev/null and b/res/path/test/left_track_results/result_2.png differ diff --git a/test/task-path-track/left_track_demo.py b/test/task-path-track/left_track_demo.py index dacc369..041c20b 100644 --- a/test/task-path-track/left_track_demo.py +++ b/test/task-path-track/left_track_demo.py @@ -196,7 +196,7 @@ def process_video(video_path, save_dir=None, show_steps=False): def main(): parser = argparse.ArgumentParser(description='左侧黄色轨迹线检测演示程序') - parser.add_argument('--input', type=str, default='res/path/image_20250513_162556.png', help='输入图像或视频的路径') + parser.add_argument('--input', type=str, default='res/path/left/2.png', help='输入图像或视频的路径') parser.add_argument('--output', type=str, default='res/path/test/left_track_results/', help='输出结果的保存目录') parser.add_argument('--type', type=str, choices=['image', 'video'], help='输入类型，不指定会自动检测') parser.add_argument('--show', default=True, help='显示处理步骤') diff --git a/utils/detect_track.py b/utils/detect_track.py index f34521f..770546f 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -1372,11 +1372,8 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True): # 优先选择在图像下半部分的线段 height_score = min(1.0, mid_y / (height * 0.6)) - # 斜率得分，垂直度越高越好 - slope_score = min(1.0, abs(slope) / 20) if abs(slope) < 100 else 1.0 - # 综合评分，加大位置权重 - return length_score * 0.2 + position_score * 0.5 + height_score * 0.2 + slope_score * 0.1 + return length_score * 0.2 + position_score * 0.5 + height_score * 0.2 # 对线段进行评分并排序 left_vertical_lines = sorted(left_vertical_lines, key=score_left_line, reverse=True)