diff --git a/base_move/go_straight.py b/base_move/go_straight.py index 660d939..f418054 100644 --- a/base_move/go_straight.py +++ b/base_move/go_straight.py @@ -196,6 +196,12 @@ def go_straight(ctrl, msg, distance, speed=0.5, observe=False): return go_success +def go_straight_until_bar(ctrl, msg, distance, speed=0.5, observe=False): + """ + 控制机器人沿直线行走指定距离，直到检测到栏杆 + """ + pass + # 用法示例 if __name__ == "__main__": # 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象 diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 82e645b..c48a8b1 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -1224,13 +1224,13 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None, def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=30, observe=False): """ - 控制机器狗向左侧移动并靠近左侧的黄色轨迹线 + 控制机器狗向左侧移动并靠近左侧的黄色轨迹线，只进行侧向移动，不进行前进 参数: ctrl: Robot_Ctrl 对象，包含里程计信息 msg: robot_control_cmd_lcmt 对象，用于发送命令 target_distance: 目标与左侧线的像素距离，默认为150像素（保持一定间距） - speed: 移动速度(米/秒)，默认为0.3米/秒 + speed: 侧向移动的最大速度(米/秒)，默认为0.3米/秒 max_time: 最大执行时间(秒)，默认为30秒 observe: 是否输出中间状态信息和可视化结果，默认为False @@ -1256,22 +1256,22 @@ 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.002 # 比例系数 + # PID控制参数 - 侧向移动的PID参数，根据近距离跟踪需求调整 + kp_side = 0.0025 # 比例系数 ki_side = 0.0001 # 积分系数 - kd_side = 0.0005 # 微分系数 + kd_side = 0.001 # 微分系数 # 记录目标到达状态和稳定计数 target_reached = False stable_count = 0 - required_stable_count = 10 # 需要连续稳定的检测次数 + required_stable_count = 8 # 需要连续稳定的检测次数 # PID控制变量 previous_error = 0 integral = 0 # 最大侧向速度限制 - max_side_velocity = 0.3 # m/s + max_side_velocity = speed # 使用参数传入的速度作为最大侧向速度 # 检测成功计数器 detection_success_count = 0 @@ -1284,6 +1284,9 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 filter_size = 5 distance_queue = [] + # 动态调整目标距离的阈值 + target_reached_threshold = 25 # 在目标距离±25像素范围内视为达到目标 + # 开始跟踪循环 while time.time() - start_time < max_time: # 获取当前图像 @@ -1321,7 +1324,7 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 error = target_distance - filtered_distance # 检查是否达到目标位置 - if abs(error) < 20: # 误差小于20像素视为达到目标 + if abs(error) < target_reached_threshold: # 误差小于阈值视为达到目标 stable_count += 1 if stable_count >= required_stable_count: if not target_reached: @@ -1348,38 +1351,41 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 # 计算侧向速度 side_velocity = p_control + i_control + d_control + # 应用非线性控制，加快大距离靠近，减缓小距离微调 + if abs(error) > 100: + # 大误差时，增强响应 + side_velocity *= 1.2 + elif abs(error) < 40: + # 小误差时，减弱响应，增加稳定性 + side_velocity *= 0.7 + # 限制侧向速度范围 side_velocity = max(-max_side_velocity, min(max_side_velocity, side_velocity)) - # 根据目标状态调整速度 - forward_speed = speed + # 如果已达到目标，进一步降低侧向速度以减少振荡 if target_reached: - # 如果已达到目标，降低侧向速度以减少振荡 - side_velocity *= 0.5 - # 降低前进速度让侧向移动更平稳 - forward_speed *= 0.7 + 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"控制指令: 前进={forward_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "速度") + debug(f"控制指令: 侧向={side_velocity:.2f}m/s", "速度") # 设置速度命令 - [前进速度, 侧向速度, 角速度] # 侧向速度为正表示向左移动，为负表示向右移动 - msg.vel_des = [forward_speed, side_velocity, 0] + # 前进速度保持为0，只进行侧向移动 + msg.vel_des = [0, side_velocity, 0] # 使用轨迹线斜率进行小角度调整，确保机器人方向与轨迹线平行 - # 斜率大于0表示向右倾斜，需要顺时针旋转(负角速度) - # 斜率小于0表示向左倾斜，需要逆时针旋转(正角速度) if not track_info["is_vertical"]: # 对于非垂直线，进行角度校正 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.2 # 角速度控制系数 + angular_velocity = angle_correction * 0.25 # 限制角速度范围 - angular_velocity = max(-0.2, min(0.2, angular_velocity)) + angular_velocity = max(-0.25, min(0.25, angular_velocity)) - if abs(angular_velocity) > 0.02: # 只在需要明显校正时应用 + if abs(angular_velocity) > 0.01: # 只在需要明显校正时应用 msg.vel_des[2] = angular_velocity if observe and time.time() % 0.5 < 0.02: debug(f"应用角度校正，斜率: {slope:.2f}, 角速度: {angular_velocity:.3f}", "校正") @@ -1389,9 +1395,6 @@ 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: - # 使用上一次的距离，但降低侧向速度 - reduced_speed = speed * 0.5 # 降低前进速度 - # 如果上次距离已经接近目标，维持当前位置 last_error = target_distance - distance_queue[-1] if abs(last_error) < 50: # 如果接近目标距离 @@ -1400,19 +1403,19 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 info("接近目标位置，保持当前侧向位置", "保持") else: # 向目标方向缓慢移动 - side_velocity = 0.05 * (1 if last_error > 0 else -1) + side_velocity = 0.08 * (1 if last_error > 0 else -1) if observe: info(f"距离目标较远，缓慢向{'左' if side_velocity > 0 else '右'}移动", "调整") - msg.vel_des = [reduced_speed, side_velocity, 0] + msg.vel_des = [0, side_velocity, 0] if observe: - warning(f"使用降低强度的控制: 前进={reduced_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "恢复") + warning(f"使用降低强度的控制: 侧向={side_velocity:.2f}m/s", "恢复") else: - # 如果从未检测到轨迹线，降低速度直接向前 - reduced_speed = speed * 0.3 - msg.vel_des = [reduced_speed, 0, 0] + # 如果从未检测到轨迹线，默认向左侧移动一点，尝试找到轨迹线 + default_side_velocity = 0.1 + msg.vel_des = [0, default_side_velocity, 0] if observe: - warning(f"无法检测到左侧轨迹线，降低速度前进: {reduced_speed:.2f}m/s", "警告") + warning(f"无法检测到左侧轨迹线，向左移动尝试寻找: {default_side_velocity:.2f}m/s", "警告") # 发送命令 msg.life_count += 1 @@ -1438,11 +1441,11 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3 if observe: info("开始平滑停止", "停止") - # 先降低速度再停止，实现平滑停止 - slowdown_steps = 5 + # 平滑停止 - 由于没有前进速度，只需要停止侧向移动 + slowdown_steps = 3 for i in range(slowdown_steps, 0, -1): slowdown_factor = i / slowdown_steps - msg.vel_des = [speed * slowdown_factor, 0, 0] + msg.vel_des = [0, 0, 0] # 完全停止所有移动 msg.life_count += 1 ctrl.Send_cmd(msg) time.sleep(0.1) diff --git a/logs/robot_2025-05-25.log b/logs/robot_2025-05-25.log new file mode 100644 index 0000000..4fcf520 --- /dev/null +++ b/logs/robot_2025-05-25.log @@ -0,0 +1,37 @@ +2025-05-25 19:50:01 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码 +2025-05-25 19:50:02 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码 +2025-05-25 19:50:03 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测 +2025-05-25 19:50:04 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线 +2025-05-25 19:50:05 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 8 条垂直线 +2025-05-25 19:50:07 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250525_195007_428255.jpg +2025-05-25 19:50:07 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250525_195007_428255', 'center_point': (834, 1080), 'deviation': -126, 'left_track_mid_x': 397.0, 'right_track_mid_x': 1351.5, 'track_width': 954.5, 'center_slope': -2.8529411764705883} +2025-05-25 19:50:15 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-25 19:50:16 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-25 19:50:17 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 +2025-05-25 19:50:18 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-25 19:50:19 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线 +2025-05-25 19:50:20 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线 +2025-05-25 19:50:21 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 +2025-05-25 19:50:22 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_195022_290308.jpg +2025-05-25 19:50:22 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_195022_290308.jpg +2025-05-25 19:50:22 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_195022_290308', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0} +2025-05-25 19:52:13 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-25 19:52:14 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-25 19:52:15 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 +2025-05-25 19:52:16 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-25 19:52:17 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 74 条直线 +2025-05-25 19:52:18 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 2 条垂直线 +2025-05-25 19:52:19 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 +2025-05-25 19:52:20 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_195220_395525.jpg +2025-05-25 19:52:20 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_195220_395525.jpg +2025-05-25 19:52:20 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_195220_395525', 'tracking_point': (549, 1071), 'ground_intersection': (543, 1080), 'distance_to_left': 584.5, 'slope': -1.619718309859155, 'line_mid_x': 584.5} +2025-05-25 19:52:26 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载 +2025-05-25 19:52:27 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码 +2025-05-25 19:52:28 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码 +2025-05-25 19:52:29 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测 +2025-05-25 19:52:31 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 79 条直线 +2025-05-25 19:52:32 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线 +2025-05-25 19:52:33 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点 +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} diff --git a/res/path/test/left_track_results/result_image_20250513_162556.png b/res/path/test/left_track_results/result_image_20250513_162556.png index bb43db2..59e7f07 100644 Binary files a/res/path/test/left_track_results/result_image_20250513_162556.png and b/res/path/test/left_track_results/result_image_20250513_162556.png differ diff --git a/res/path/test/left_track_results/result_image_20250525_113607.png b/res/path/test/left_track_results/result_image_20250525_113607.png new file mode 100644 index 0000000..5b07e69 Binary files /dev/null and b/res/path/test/left_track_results/result_image_20250525_113607.png differ diff --git a/res/path/test/left_track_results/result_image_20250525_113635.png b/res/path/test/left_track_results/result_image_20250525_113635.png new file mode 100644 index 0000000..e9a3d16 Binary files /dev/null and b/res/path/test/left_track_results/result_image_20250525_113635.png differ diff --git a/res/path/test/result_image_20250514_024347.png b/res/path/test/result_image_20250514_024347.png new file mode 100644 index 0000000..04fd57f Binary files /dev/null and b/res/path/test/result_image_20250514_024347.png differ diff --git a/utils/detect_track.py b/utils/detect_track.py index 88163b0..f34521f 100644 --- a/utils/detect_track.py +++ b/utils/detect_track.py @@ -1248,11 +1248,10 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True): # 计算图像中间和左侧区域的范围 center_x = width // 2 - # 主要关注视野的左半部分 - left_region_width = center_x - left_region_height = height + # 主要关注视野的左半部分，但稍微扩大一点以确保捕捉到左侧线 + left_region_width = int(center_x * 1.2) # 扩大左侧搜索区域 left_bound = 0 - right_bound = center_x + right_bound = min(width, left_region_width) bottom_bound = height top_bound = 0 @@ -1268,17 +1267,17 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True): # 转换到HSV颜色空间以便更容易提取黄色 hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) - # 黄色的HSV范围 - 扩大范围以更好地捕捉不同光照条件下的黄色 - lower_yellow = np.array([15, 80, 80]) # 更宽松的黄色下限 - upper_yellow = np.array([35, 255, 255]) # 更宽松的黄色上限 + # 黄色的HSV范围 - 进一步扩大范围以适应不同光照条件 + lower_yellow = np.array([12, 70, 70]) # 更宽松的黄色下限 + upper_yellow = np.array([38, 255, 255]) # 更宽松的黄色上限 # 创建黄色的掩码 mask = cv2.inRange(hsv, lower_yellow, upper_yellow) # 形态学操作以改善掩码 - kernel = np.ones((5, 5), np.uint8) # 增大kernel尺寸 + kernel = np.ones((5, 5), np.uint8) mask = cv2.dilate(mask, kernel, iterations=1) - mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1) # 添加腐蚀操作去除噪点 + mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1) if observe: debug("步骤2: 创建黄色掩码", "处理") @@ -1301,9 +1300,9 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True): cv2.imshow("边缘检测", edges) cv2.waitKey(delay) - # 霍夫变换检测直线 - lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25, - minLineLength=height*0.15, maxLineGap=50) # 调整参数以检测更长的线段 + # 霍夫变换检测直线 - 降低minLineLength以更好地检测近距离的线段 + lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=20, + minLineLength=height*0.1, maxLineGap=60) # 调整参数以检测更短的线段 if lines is None or len(lines) == 0: error("未检测到直线", "失败") @@ -1318,7 +1317,7 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True): cv2.imshow("检测到的直线", lines_img) cv2.waitKey(delay) - # 筛选左侧区域内的近似垂直线 + # 筛选左侧区域内的近似垂直线，放宽条件以捕获更多可能的线 left_vertical_lines = [] for line in lines: x1, y1, x2, y2 = line[0] @@ -1333,8 +1332,8 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True): else: slope = (y2 - y1) / (x2 - x1) - # 筛选接近垂直的线 (斜率较大) - if abs(slope) > 0.7: # 设置较宽松的垂直线斜率阈值 + # 放宽垂直线的斜率范围，以适应近距离时线的倾斜 + if abs(slope) > 0.5: # 降低垂直线斜率阈值 line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2) # 计算线的中点坐标 mid_x = (x1 + x2) / 2 @@ -1360,15 +1359,24 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True): cv2.imshow("左侧垂直线", left_lines_img) cv2.waitKey(delay) - # 按线段长度和位置进行评分，优先选择更长且更靠近图像左边的线 + # 修改评分函数，优先选择最左侧的线 def score_left_line(line_info): - _, mid_x, _, _, length = line_info - # 线段越长分数越高 + _, mid_x, mid_y, slope, length = line_info + + # 线段越长分数越高，但降低权重 length_score = min(1.0, length / (height * 0.3)) - # 越靠近左边分数越高 + + # 越靠近左边分数越高，增加权重 position_score = 1.0 - (mid_x / center_x) - # 综合评分 - return length_score * 0.7 + position_score * 0.3 + + # 优先选择在图像下半部分的线段 + 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 # 对线段进行评分并排序 left_vertical_lines = sorted(left_vertical_lines, key=score_left_line, reverse=True) @@ -1387,9 +1395,6 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True): tracking_point = (x2, y2) if y2 > y1 else (x1, y1) # 计算线与地面的交点 - # 使用线段的方程: (y - y1) = slope * (x - x1) - # 地面对应图像底部: y = height - # 解这个方程得到交点的x坐标 if abs(slope) < 0.01: # 几乎垂直 ground_intersection_x = x1 else: