diff --git a/base_move/center_on_dual_tracks.py b/base_move/center_on_dual_tracks.py index 629c670..8e4c2dd 100644 --- a/base_move/center_on_dual_tracks.py +++ b/base_move/center_on_dual_tracks.py @@ -200,6 +200,7 @@ def center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=Fa 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], stone_path_mode=None): diff --git a/logs/res/1/dual_track_20250531_012429_159526.jpg b/logs/res/1/dual_track_20250531_012429_159526.jpg new file mode 100644 index 0000000..9d2a710 Binary files /dev/null and b/logs/res/1/dual_track_20250531_012429_159526.jpg differ diff --git a/logs/res/1/dual_track_20250531_012429_250427.jpg b/logs/res/1/dual_track_20250531_012429_250427.jpg new file mode 100644 index 0000000..32a27cf Binary files /dev/null and b/logs/res/1/dual_track_20250531_012429_250427.jpg differ diff --git a/logs/res/1/dual_track_20250531_012429_336806.jpg b/logs/res/1/dual_track_20250531_012429_336806.jpg new file mode 100644 index 0000000..5aa2ba5 Binary files /dev/null and b/logs/res/1/dual_track_20250531_012429_336806.jpg differ diff --git a/logs/res/1/dual_track_orig_20250531_012429_159526.jpg b/logs/res/1/dual_track_orig_20250531_012429_159526.jpg new file mode 100644 index 0000000..8add27b Binary files /dev/null and b/logs/res/1/dual_track_orig_20250531_012429_159526.jpg differ diff --git a/logs/res/1/dual_track_orig_20250531_012429_250427.jpg b/logs/res/1/dual_track_orig_20250531_012429_250427.jpg new file mode 100644 index 0000000..4e3e2f4 Binary files /dev/null and b/logs/res/1/dual_track_orig_20250531_012429_250427.jpg differ diff --git a/logs/res/1/dual_track_orig_20250531_012429_336806.jpg b/logs/res/1/dual_track_orig_20250531_012429_336806.jpg new file mode 100644 index 0000000..528251c Binary files /dev/null and b/logs/res/1/dual_track_orig_20250531_012429_336806.jpg differ diff --git a/logs/res/2/dual_track_20250530_170029_494229.jpg b/logs/res/2/dual_track_20250530_170029_494229.jpg new file mode 100644 index 0000000..9994876 Binary files /dev/null and b/logs/res/2/dual_track_20250530_170029_494229.jpg differ diff --git a/logs/res/2/dual_track_20250530_170029_578646.jpg b/logs/res/2/dual_track_20250530_170029_578646.jpg new file mode 100644 index 0000000..e44c6ad Binary files /dev/null and b/logs/res/2/dual_track_20250530_170029_578646.jpg differ diff --git a/logs/res/2/dual_track_20250530_170029_667280.jpg b/logs/res/2/dual_track_20250530_170029_667280.jpg new file mode 100644 index 0000000..a4cbafa Binary files /dev/null and b/logs/res/2/dual_track_20250530_170029_667280.jpg differ diff --git a/logs/res/2/dual_track_20250530_170036_885372.jpg b/logs/res/2/dual_track_20250530_170036_885372.jpg new file mode 100644 index 0000000..5d845dd Binary files /dev/null and b/logs/res/2/dual_track_20250530_170036_885372.jpg differ diff --git a/logs/res/2/dual_track_20250530_170036_964867.jpg b/logs/res/2/dual_track_20250530_170036_964867.jpg new file mode 100644 index 0000000..4c6b4eb Binary files /dev/null and b/logs/res/2/dual_track_20250530_170036_964867.jpg differ diff --git a/logs/res/2/dual_track_20250530_170037_053387.jpg b/logs/res/2/dual_track_20250530_170037_053387.jpg new file mode 100644 index 0000000..76956b9 Binary files /dev/null and b/logs/res/2/dual_track_20250530_170037_053387.jpg differ diff --git a/logs/res/2/dual_track_20250530_170040_324842.jpg b/logs/res/2/dual_track_20250530_170040_324842.jpg new file mode 100644 index 0000000..85714b7 Binary files /dev/null and b/logs/res/2/dual_track_20250530_170040_324842.jpg differ diff --git a/logs/res/2/dual_track_20250530_170040_395634.jpg b/logs/res/2/dual_track_20250530_170040_395634.jpg new file mode 100644 index 0000000..99f250f Binary files /dev/null and b/logs/res/2/dual_track_20250530_170040_395634.jpg differ diff --git a/logs/res/2/dual_track_20250530_170040_471020.jpg b/logs/res/2/dual_track_20250530_170040_471020.jpg new file mode 100644 index 0000000..437c971 Binary files /dev/null and b/logs/res/2/dual_track_20250530_170040_471020.jpg differ diff --git a/logs/res/2/dual_track_orig_20250530_170029_494229.jpg b/logs/res/2/dual_track_orig_20250530_170029_494229.jpg new file mode 100644 index 0000000..84d8b7c Binary files /dev/null and b/logs/res/2/dual_track_orig_20250530_170029_494229.jpg differ diff --git a/logs/res/2/dual_track_orig_20250530_170029_578646.jpg b/logs/res/2/dual_track_orig_20250530_170029_578646.jpg new file mode 100644 index 0000000..b0c5328 Binary files /dev/null and b/logs/res/2/dual_track_orig_20250530_170029_578646.jpg differ diff --git a/logs/res/2/dual_track_orig_20250530_170029_667280.jpg b/logs/res/2/dual_track_orig_20250530_170029_667280.jpg new file mode 100644 index 0000000..2c7ac0a Binary files /dev/null and b/logs/res/2/dual_track_orig_20250530_170029_667280.jpg differ diff --git a/logs/res/2/dual_track_orig_20250530_170036_885372.jpg b/logs/res/2/dual_track_orig_20250530_170036_885372.jpg new file mode 100644 index 0000000..3910534 Binary files /dev/null and b/logs/res/2/dual_track_orig_20250530_170036_885372.jpg differ diff --git a/logs/res/2/dual_track_orig_20250530_170036_964867.jpg b/logs/res/2/dual_track_orig_20250530_170036_964867.jpg new file mode 100644 index 0000000..d8ecc31 Binary files /dev/null and b/logs/res/2/dual_track_orig_20250530_170036_964867.jpg differ diff --git a/logs/res/2/dual_track_orig_20250530_170037_053387.jpg b/logs/res/2/dual_track_orig_20250530_170037_053387.jpg new file mode 100644 index 0000000..68f96d0 Binary files /dev/null and b/logs/res/2/dual_track_orig_20250530_170037_053387.jpg differ diff --git a/logs/res/2/dual_track_orig_20250530_170040_324842.jpg b/logs/res/2/dual_track_orig_20250530_170040_324842.jpg new file mode 100644 index 0000000..7dec9ca Binary files /dev/null and b/logs/res/2/dual_track_orig_20250530_170040_324842.jpg differ diff --git a/logs/res/2/dual_track_orig_20250530_170040_395634.jpg b/logs/res/2/dual_track_orig_20250530_170040_395634.jpg new file mode 100644 index 0000000..337df87 Binary files /dev/null and b/logs/res/2/dual_track_orig_20250530_170040_395634.jpg differ diff --git a/logs/res/2/dual_track_orig_20250530_170040_471020.jpg b/logs/res/2/dual_track_orig_20250530_170040_471020.jpg new file mode 100644 index 0000000..125d65d Binary files /dev/null and b/logs/res/2/dual_track_orig_20250530_170040_471020.jpg differ diff --git a/main.py b/main.py index 66a4092..a85e106 100644 --- a/main.py +++ b/main.py @@ -59,13 +59,15 @@ def main(): arrow_direction = 'right' # TEST - # if arrow_direction == 'left': - # run_task_4(Ctrl, msg) - # else: - # run_task_3(Ctrl, msg) + if arrow_direction == 'left': + run_task_4(Ctrl, msg) + else: + run_task_3(Ctrl, msg) - # turn_degree_v2(Ctrl, msg, degree=90, absolute=True) - # run_task_5(Ctrl, msg, direction=arrow_direction) + return + + turn_degree_v2(Ctrl, msg, degree=90, absolute=True) + run_task_5(Ctrl, msg, direction=arrow_direction) if arrow_direction == 'left': # run_task_3_back(Ctrl, msg) diff --git a/task_3/task_3.py b/task_3/task_3.py index f7270d0..2c8e698 100644 --- a/task_3/task_3.py +++ b/task_3/task_3.py @@ -253,7 +253,7 @@ def pass_up_down(ctrl, msg): stable_threshold = 10 # 连续10次检测z轴速度接近零则认为已经到达平地 z_speed_threshold = 0.005 # z轴速度阈值，小于这个值认为已经停止下降 descent_speed_threshold = -0.05 # 检测到开始下坡的速度阈值（负值表示下降） - max_iterations = 120 # 最大循环次数，作为安全保障 + max_iterations = 100 # 最大循环次数，作为安全保障 min_iterations = 80 # 最小循环次数，确保有足够的时间开始动作 start_height = ctrl.odo_msg.xyz[2] # 记录起始高度 @@ -501,16 +501,17 @@ def run_task_3(ctrl, msg): section('任务3：上下坡', "启动") info('开始执行任务3...', "启动") - turn_degree_v2(ctrl, msg, 90, absolute=True) - go_lateral(ctrl, msg, distance=0.2, speed=0.1, observe=True) + # turn_degree_v2(ctrl, msg, 90, absolute=True) + # go_lateral(ctrl, msg, distance=0.2, speed=0.1, observe=True) - section('任务3-1：up', "开始") - pass_up_down(ctrl, msg) + # section('任务3-1：up', "开始") + # pass_up_down(ctrl, msg) - section('任务3-2：center on dual tracks', "开始") turn_degree_v2(ctrl, msg, 90, absolute=True) center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False, stone_path_mode=False) + return + section('任务3-2：yellow stop', "开始") go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3) diff --git a/task_4/task_4.py b/task_4/task_4.py index 25bda9c..952d38a 100644 --- a/task_4/task_4.py +++ b/task_4/task_4.py @@ -3,6 +3,7 @@ import sys import os import cv2 import numpy as np +import math # 添加父目录到路径，以便能够导入utils sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) @@ -12,6 +13,7 @@ from base_move.go_straight import go_straight, go_lateral from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing from utils.gray_sky_analyzer import analyze_gray_sky_ratio from utils.detect_track import detect_horizontal_track_edge +from utils.detect_dual_track_lines import detect_dual_track_lines from base_move.move_base_hori_line import calculate_distance_to_line from task_4.pass_bar import pass_bar @@ -50,18 +52,21 @@ def run_task_4_back(ctrl, msg): # 向右移动0.5秒 section('任务4-回程：向右移动', "移动") - go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True) # DEBUG + # go_lateral(ctrl, msg, distance=-0.3, speed=0.1, observe=True) # DEBUG section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测") go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.35, speed=0.5) section('任务4-2：通过栏杆', "移动") + turn_degree_v2(ctrl, msg, degree=-90, absolute=True) pass_bar(ctrl, msg) section('任务4-3：stone', "移动") go_straight(ctrl, msg, distance=1, speed=2) - go_straight(ctrl, msg, distance=4.5, speed=0.35, mode=11, gait_id=3, step_height=[0.21, 0.21]) + # Use enhanced calibration for better Y-axis correction on stone path + go_straight_with_enhanced_calibration(ctrl, msg, distance=4.5, speed=0.35, + mode=11, gait_id=3, step_height=[0.21, 0.21], observe=True) section('任务4-3：前进直到遇到黄线 - 石板路', "移动") # 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处 @@ -155,3 +160,304 @@ def go_straight_until_sky_ratio_below(ctrl, msg, warning(f"已达到最大移动距离 {max_distance} 米，但未找到天空比例小于 {sky_ratio_threshold:.2%} 的位置", "超时") return success_flag + +def go_straight_with_enhanced_calibration(ctrl, msg, distance, speed=0.5, observe=False, + mode=11, gait_id=3, step_height=[0.21, 0.21]): + """ + 控制机器人在石板路上沿直线行走，使用视觉校准和姿态传感器融合来保持直线 + + 参数: + ctrl: Robot_Ctrl 对象 + msg: 控制消息对象 + distance: 要行走的距离(米) + speed: 行走速度(米/秒) + observe: 是否输出调试信息 + mode: 运动模式 + gait_id: 步态ID + step_height: 摆动腿高度 + + 返回: + 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'): + warning("机器人控制器没有提供图像处理器，将使用姿态传感器辅助校准", "警告") + + # 限制速度范围 + 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"开始石板路增强直线移动，距离: {abs_distance:.3f}米，速度: {abs(move_speed):.2f}米/秒", "移动") + + # 设置移动命令 + msg.mode = mode + msg.gait_id = gait_id + msg.step_height = step_height + msg.duration = 0 # wait next cmd + + # 根据需要移动的距离动态调整移动速度 + if abs_distance > 1.0: + actual_speed = move_speed # 距离较远时用设定速度 + else: + actual_speed = move_speed * 0.8 # 较近距离略微降速 + + # 设置移动速度和方向 + msg.vel_des = [actual_speed, 0, 0] # [前进速度, 侧向速度, 角速度] + msg.life_count += 1 + + # 发送命令 + ctrl.Send_cmd(msg) + + # 估算移动时间，但实际上会通过里程计控制 + estimated_time = abs_distance / abs(actual_speed) + timeout = estimated_time + 5 # 增加超时时间 + + # 使用里程计进行实时监控移动距离 + distance_moved = 0 + start_time = time.time() + last_check_time = start_time + position_check_interval = 0.1 # 位置检查间隔(秒) + + # 计算移动方向单位向量（世界坐标系下） + direction_vector = [math.cos(start_yaw), math.sin(start_yaw)] + if not forward: + direction_vector = [-direction_vector[0], -direction_vector[1]] + + # 视觉跟踪相关变量 + vision_check_interval = 0.2 # 视觉检查间隔(秒) + last_vision_check = start_time + vision_correction_gain = 0.006 # 视觉修正增益系数 + + # 用于滤波的队列 + deviation_queue = [] + filter_size = 5 + last_valid_deviation = 0 + + # PID控制参数 - 用于角度修正 + kp_angle = 0.6 # 比例系数 + ki_angle = 0.02 # 积分系数 + kd_angle = 0.1 # 微分系数 + + # PID控制变量 + angle_error_integral = 0 + last_angle_error = 0 + + # 偏移量累计 - 用于检测持续偏移 + y_offset_accumulator = 0 + + # 动态调整参数 + slow_down_ratio = 0.85 # 当移动到目标距离的85%时开始减速 + completion_threshold = 0.95 # 当移动到目标距离的95%时停止 + + # 监控移动过程 + 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] + + # 累积y方向偏移量，检测持续偏移趋势 + y_offset_accumulator = y_offset_accumulator * 0.8 + y_offset * 0.2 + + # 根据前进或后退确定期望方向 + 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 + + # 使用PID控制计算角速度修正 + # 比例项 + p_control = kp_angle * yaw_error + + # 积分项 (带衰减) + angle_error_integral = angle_error_integral * 0.9 + yaw_error + angle_error_integral = max(-1.0, min(1.0, angle_error_integral)) # 限制积分范围 + i_control = ki_angle * angle_error_integral + + # 微分项 + d_control = kd_angle * (yaw_error - last_angle_error) + last_angle_error = yaw_error + + # 计算总的角速度控制量 + angular_correction = -(p_control + i_control + d_control) + # 限制最大角速度修正 + angular_correction = max(min(angular_correction, 0.3), -0.3) + + # 根据持续的y偏移趋势增加侧向校正 + lateral_correction = 0 + if abs(y_offset_accumulator) > 0.05: # 如果累积偏移超过5厘米 + lateral_correction = -y_offset_accumulator * 0.8 # 反向校正 + lateral_correction = max(min(lateral_correction, 0.15), -0.15) # 限制最大侧向速度 + + if observe and abs(lateral_correction) > 0.05: + warning(f"累积Y偏移校正: {y_offset_accumulator:.3f}米，应用侧向速度 {lateral_correction:.3f}m/s", "偏移") + + # 计算完成比例 + 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.3, 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}%)", "移动") + + actual_speed = new_speed + + # 应用修正 - 同时应用角速度和侧向速度修正 + msg.vel_des = [actual_speed, lateral_correction, angular_correction] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if observe 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}米, 累积偏移: {y_offset_accumulator:.3f}米", "偏移") + debug(f"朝向偏差: {math.degrees(yaw_error):.1f}度, 角速度修正: {angular_correction:.3f}rad/s", "角度") + debug(f"PID: P={p_control:.3f}, I={i_control:.3f}, D={d_control:.3f}", "控制") + + # 更新检查时间 + last_check_time = current_time + + # 定期进行视觉检查和修正 + if hasattr(ctrl, 'image_processor') and current_time - last_vision_check >= vision_check_interval: + try: + # 获取当前相机帧 + frame = ctrl.image_processor.get_current_image() + if frame is not None: + # 检测轨道线 - 使用特定的石板路模式 + center_info, _, _ = detect_dual_track_lines( + frame, observe=False, save_log=False, stone_path_mode=True) + + # 如果成功检测到轨道线，使用它进行修正 + if center_info is not None: + # 获取当前偏差 + 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 + + # 计算视觉侧向修正速度 + vision_lateral_correction = -filtered_deviation * vision_correction_gain + # 限制最大侧向速度修正 + vision_lateral_correction = max(min(vision_lateral_correction, 0.2), -0.2) + + # 与当前的侧向校正进行融合 (加权平均) + if msg.vel_des[1] != 0: + # 如果已经有侧向校正，与视觉校正进行融合 + fused_lateral = msg.vel_des[1] * 0.3 + vision_lateral_correction * 0.7 + else: + # 如果没有侧向校正，直接使用视觉校正 + fused_lateral = vision_lateral_correction + + if observe and abs(vision_lateral_correction) > 0.05: + warning(f"视觉修正: 偏差 {filtered_deviation:.1f}像素，应用侧向速度 {vision_lateral_correction:.3f}m/s", "视觉") + + # 应用视觉修正，保留当前前进速度和角速度 + msg.vel_des = [msg.vel_des[0], fused_lateral, msg.vel_des[2]] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + if observe and current_time % 1.0 < vision_check_interval: + debug(f"视觉检测: 原始偏差 {current_deviation:.1f}像素, 滤波后 {filtered_deviation:.1f}像素", "视觉") + debug(f"融合侧向速度: {fused_lateral:.3f}m/s", "视觉") + except Exception as e: + if observe: + error(f"视觉检测异常: {e}", "错误") + + # 更新视觉检查时间 + last_vision_check = current_time + + # 短暂延时 + time.sleep(0.01) + + # 平滑停止 + 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 = math.sqrt(dx*dx + dy*dy) + + # 计算最终y方向偏移 + final_y_offset = -dx * direction_vector[1] + dy * direction_vector[0] + + if observe: + success(f"石板路增强直线移动完成，实际移动距离: {actual_distance:.3f}米", "完成") + info(f"最终Y方向偏移: {final_y_offset:.3f}米", "偏移") + + # 判断是否成功完成 + distance_error = abs(actual_distance - abs_distance) + y_offset_error = abs(final_y_offset) + + go_success = distance_error < 0.1 and y_offset_error < 0.1 # 距离误差和y偏移都小于10厘米视为成功 + + if observe: + if go_success: + success(f"移动成功", "成功") + else: + warning(f"移动不够精确，距离误差: {distance_error:.3f}米, Y偏移: {y_offset_error:.3f}米", "警告") + + return go_success