From 76c00337edfd9326ead3e5d91036cae4236700ed Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Mon, 26 May 2025 01:16:20 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96follow=5Fleft=5Fside=5Ftrack?= =?UTF-8?q?=E5=87=BD=E6=95=B0=EF=BC=8C=E8=B0=83=E6=95=B4=E6=9C=80=E5=A4=A7?= =?UTF-8?q?=E6=97=B6=E9=97=B4=E5=8F=82=E6=95=B0=E8=87=B330=E7=A7=92?= =?UTF-8?q?=EF=BC=8C=E5=A2=9E=E5=BC=BA=E8=BD=A8=E8=BF=B9=E8=B7=9F=E9=9A=8F?= =?UTF-8?q?=E7=9A=84=E7=A8=B3=E5=AE=9A=E6=80=A7=E5=92=8C=E5=93=8D=E5=BA=94?= =?UTF-8?q?=E9=80=9F=E5=BA=A6=EF=BC=9B=E6=94=B9=E8=BF=9B=E6=A3=80=E6=B5=8B?= =?UTF-8?q?=E9=80=BB=E8=BE=91=EF=BC=8C=E7=A1=AE=E4=BF=9D=E5=9C=A8=E6=9C=AA?= =?UTF-8?q?=E6=A3=80=E6=B5=8B=E5=88=B0=E8=BD=A8=E8=BF=B9=E7=BA=BF=E6=97=B6?= =?UTF-8?q?=E8=83=BD=E5=A4=9F=E7=BB=A7=E7=BB=AD=E4=BD=BF=E7=94=A8=E4=B8=8A?= =?UTF-8?q?=E4=B8=80=E6=AC=A1=E6=8E=A7=E5=88=B6=E5=91=BD=E4=BB=A4=EF=BC=9B?= =?UTF-8?q?=E6=9B=B4=E6=96=B0=E9=80=9F=E5=BA=A6=E8=AE=A1=E7=AE=97=E6=96=B9?= =?UTF-8?q?=E5=BC=8F=EF=BC=8C=E7=A1=AE=E4=BF=9D=E6=9B=B4=E5=B9=B3=E6=BB=91?= =?UTF-8?q?=E7=9A=84=E8=B0=83=E6=95=B4=E5=92=8C=E5=81=9C=E6=AD=A2=E8=BF=87?= =?UTF-8?q?=E7=A8=8B=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- base_move/move_base_hori_line.py | 303 +++++++------------------------ 1 file changed, 69 insertions(+), 234 deletions(-) diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 689c566..81f9048 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -1222,7 +1222,7 @@ 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=540, speed=0.3, max_time=3, observe=False): +def follow_left_side_track(ctrl, msg, target_distance=540, speed=0.3, max_time=30, observe=False): """ 控制机器狗向左侧移动并靠近左侧的黄色轨迹线，只进行侧向移动，不进行前进 @@ -1248,255 +1248,90 @@ def follow_left_side_track(ctrl, msg, target_distance=540, speed=0.3, max_time=3 # 记录起始时间 start_time = time.time() - # 记录起始位置 - start_position = list(ctrl.odo_msg.xyz) + # 记录初始检测 + image = ctrl.image_processor.get_current_image() + track_info, _ = detect_left_side_track(image, observe=observe, save_log=True) + + if track_info is None: + error("无法检测到左侧轨迹线，无法开始跟踪", "失败") + return False + + initial_distance = track_info["distance_to_left"] 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) + info(f"初始距离: {initial_distance:.1f}px, 目标距离: {target_distance}px", "初始") - # 最大侧向速度限制与动态调整 - max_side_velocity = speed - min_side_velocity = 0.05 # 最小侧向速度，确保能够缓慢移动 + # 最大允许误差(像素) + error_threshold = 30 - # 检测成功计数器 - detection_success_count = 0 - detection_total_count = 0 - - # 保存上一次有效的检测结果，用于检测失败时的平滑过渡 - last_valid_track_info = None - - # 滤波队列 - 增大滤波窗口以提高稳定性 - filter_size = 7 - distance_queue = [] - - # 设置差值阈值 - 当距离与目标的差值小于此阈值时，认为已达到目标 - distance_threshold = 25 # 像素误差阈值 - - # 记录目标到达状态和稳定计数 - target_reached = False - stable_count = 0 - required_stable_count = 8 # 连续多帧满足条件才算稳定到达目标 + # 简单比例控制参数 + kp = 0.0005 # 开始跟踪循环 while time.time() - start_time < max_time: - # 获取当前图像 - image = ctrl.image_processor.get_current_image() - # 检测左侧轨迹线 - detection_total_count += 1 - track_info, tracking_point = detect_left_side_track(image, observe=observe, delay=500 if observe else 0, save_log=True) + image = ctrl.image_processor.get_current_image() + track_info, _ = detect_left_side_track(image, observe=observe, save_log=True) - if track_info is not None: - detection_success_count += 1 - - # 保存有效的轨迹信息 - last_valid_track_info = track_info - - # 获取当前与左侧线的距离 - current_distance = track_info["distance_to_left"] - - # 添加到滤波队列 - distance_queue.append(current_distance) - if len(distance_queue) > filter_size: - distance_queue.pop(0) - - # 计算滤波后的距离值 - 使用中值滤波，对抗异常值 - if len(distance_queue) >= 3: - # 中值滤波更能抵抗异常值 - 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"左侧距离: {filtered_distance:.1f}px, 目标距离: {target_distance}px, 差值: {difference:.1f}px", "距离") - - # 检查是否达到目标位置（差值小于阈值） - if abs(difference) < distance_threshold: - stable_count += 1 - 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", "调整") - - # 确保最小移动速度 - 当需要移动但计算值太小时 - 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 observe and time.time() % 0.5 < 0.02: - debug(f"控制指令: 侧向={side_velocity:.2f}m/s", "速度") - - # 设置速度命令 - [前进速度, 侧向速度, 角速度] - # 侧向速度为正表示向左移动，为负表示向右移动 - # 前进速度保持为0，只进行侧向移动 - msg.vel_des = [0, side_velocity, 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 - # 将角度校正应用为小的角速度，并根据距离调整强度 - angle_factor = 1.0 - if target_reached: - angle_factor = 0.5 # 接近目标时减弱角度校正 - angular_velocity = angle_correction * 0.2 * angle_factor - # 限制角速度范围 - angular_velocity = max(-0.2, min(0.2, angular_velocity)) - - 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}", "校正") - - else: - warning("未检测到左侧轨迹线", "警告") - - # 如果之前有有效检测结果，使用上一次的控制值但降低强度 - if last_valid_track_info is not None and len(distance_queue) > 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("上次距离接近目标，保持当前位置", "保持") - else: - # 根据差值方向移动，但速度降低 - direction = 1 if last_difference > 0 else -1 - side_velocity = 0.08 * direction - if observe: - 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", "恢复") - else: - # 如果从未检测到轨迹线，默认向左侧移动一点，尝试寻找 - default_side_velocity = 0.08 - msg.vel_des = [0, default_side_velocity, 0] - if observe: - warning(f"无法检测到左侧轨迹线，向左缓慢移动尝试寻找: {default_side_velocity:.2f}m/s", "警告") + if track_info is None: + warning("未检测到左侧轨迹线，继续使用上一次控制", "警告") + # 继续执行最后一次的控制命令 + msg.life_count += 1 + ctrl.Send_cmd(msg) + time.sleep(0.1) + continue - # 发送命令 + # 获取当前距离 + current_distance = track_info["distance_to_left"] + + # 计算误差 (正值表示需要向左移动，负值表示需要向右移动) + error = target_distance - current_distance + + if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 + debug(f"当前距离: {current_distance:.1f}px, 目标距离: {target_distance}px, 误差: {error:.1f}px", "跟踪") + + # 检查是否已达到目标 + if abs(error) < error_threshold: + success(f"已达到目标距离，当前距离: {current_distance:.1f}px, 目标距离: {target_distance}px", "完成") + break + + # 计算侧向速度 (简单比例控制) + lateral_velocity = kp * error + + # 限制侧向速度 + lateral_velocity = max(-speed, min(speed, lateral_velocity)) + + # 设置速度命令 + msg.vel_des = [0, lateral_velocity, 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() % 2.0 < 0.02: # 每2秒左右打印一次 - info(f"已移动: {distance_moved:.3f}米, 已用时间: {time.time() - start_time:.1f}秒", "移动") - # 显示检测成功率 - 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})", "统计") + time.sleep(0.1) - # 平滑停止 - if observe: - info("开始平滑停止", "停止") - - # 更平滑的停止 - slowdown_steps = 4 - for i in range(slowdown_steps, 0, -1): - slowdown_factor = i / slowdown_steps - 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.12) # 稍微延长每步的等待时间，使停止更平滑 - - # 最后完全停止 + # 停止移动 + msg.vel_des = [0, 0, 0] + msg.life_count += 1 + ctrl.Send_cmd(msg) + time.sleep(0.2) 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) + # 最终检查 + image = ctrl.image_processor.get_current_image() + track_info, _ = detect_left_side_track(image, observe=observe, save_log=True) - 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) + if track_info is not None: + final_distance = track_info["distance_to_left"] + distance_error = abs(final_distance - target_distance) + final_success = distance_error < error_threshold - success(f"左侧轨迹跟随完成，总移动距离: {final_distance:.3f}米", "完成") + if observe: + if final_success: + success(f"成功达到目标位置，最终距离: {final_distance:.1f}px, 目标: {target_distance}px", "成功") + else: + warning(f"未能精确达到目标位置，最终距离: {final_distance:.1f}px, 目标: {target_distance}px", "警告") - # 显示检测成功率 - 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})", "统计") - - return target_reached - -# 用法示例 -if __name__ == "__main__": - move_to_hori_line(None, None, observe=True) - # 90度左转 - arc_turn_around_hori_line(None, None, angle_deg=90, observe=True) - # 180度右转 - arc_turn_around_hori_line(None, None, angle_deg=-90, observe=True) - # 双轨道跟随 - follow_dual_tracks(None, None, observe=True) - # 左侧轨迹跟随 - follow_left_side_track(None, None, observe=True) - \ No newline at end of file + return final_success + else: + if observe: + warning("无法检测最终位置的左侧轨迹线", "警告") + return False