Enhance dual track centering functionality by adding a new method for centering and following dual tracks. Update parameters for improved control and response. Adjust detection region in dual track line detection for better accuracy.

base_move/center_on_dual_tracks.py

@@ -5,4 +5,282 @@ 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 center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=8.0, observe=False,
+                         mode=11, gait_id=26, step_height=[0.06, 0.06]):
+    """
+    控制机器狗仅使用Y轴移动调整到双轨道线的中间位置
+    
+    参数:
+    ctrl: Robot_Ctrl 对象，包含里程计信息
+    msg: robot_control_cmd_lcmt 对象，用于发送命令
+    max_time: 最大调整时间(秒)，默认为15秒
+    max_deviation: 允许的最大偏差(像素)，当偏差小于此值时认为已居中，默认为8像素
+    observe: 是否输出中间状态信息和可视化结果，默认为False
+    mode: 控制模式，默认为11
+    gait_id: 步态ID，默认为26
+    step_height: 抬腿高度，默认为[0.06, 0.06]
+    
+    返回:
+    bool: 是否成功调整到中心位置
+    """
+    section("开始双轨道居中", "轨道居中")
+    
+    # 设置移动命令基本参数
+    msg.mode = mode
+    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 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  # 比例系数增加
+    
+    # 帧间滤波参数 - 减少滤波长度以提高响应性
+    filter_size = 3  # 滤波队列大小
+    deviation_queue = []  # 偏差值队列
+    
+    # 统计变量
+    detection_success_count = 0
+    detection_total_count = 0
+    
+    # 稳定计数器 - 连续几次在中心位置才认为稳定
+    stable_count = 0
+    required_stable_count = 2  # 降低稳定条件，更快响应
+    
+    # 变量记录最后有效的偏差，用于处理检测失败的情况
+    last_valid_deviation = 0
+    
+    # 开始调整循环
+    while time.time() - start_time < max_time:
+        # 获取当前图像
+        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
+            
+            # 获取当前偏差
+            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:
+                debug(f"原始偏差: {current_deviation:.1f}px, 滤波后: {filtered_deviation:.1f}px", "偏差")
+            
+            # 判断是否已经居中
+            if abs(filtered_deviation) <= max_deviation:
+                stable_count += 1
+                if observe:
+                    info(f"已接近中心，稳定计数: {stable_count}/{required_stable_count}", "居中")
+                
+                # 即使在稳定过程中，也要保持小幅调整以保持居中
+                if stable_count < required_stable_count:
+                    # 使用非常小的调整速度
+                    lateral_velocity = kp * filtered_deviation * 0.5
+                    max_lateral_velocity = 0.1  # 减小稳定阶段的最大速度
+                    lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity))
+                    
+                    msg.vel_des = [0, lateral_velocity, 0]
+                    msg.life_count += 1
+                    ctrl.Send_cmd(msg)
+                
+                if stable_count >= required_stable_count:
+                    # 已经稳定在中心位置
+                    if observe:
+                        success(f"成功居中，最终偏差: {filtered_deviation:.1f}px", "完成")
+                    break
+            else:
+                # 不在中心，重置稳定计数
+                stable_count = 0
+                
+                # 计算横向移动速度 (只使用y轴移动)
+                # 注意：偏差为正表示需要向左移动（y轴正方向），偏差为负表示需要向右移动（y轴负方向）
+                
+                # 使用动态kp：偏差越大，移动越快；偏差越小，移动越慢
+                dynamic_kp = kp
+                if abs(filtered_deviation) > 50:
+                    dynamic_kp = kp * 1.5  # 大偏差时增加响应
+                elif abs(filtered_deviation) < 20:
+                    dynamic_kp = kp * 0.8  # 小偏差时减小响应，避免过冲
+                
+                lateral_velocity = dynamic_kp * filtered_deviation
+                
+                # 限制横向移动速度
+                max_lateral_velocity = 0.35  # 增加最大横向速度 (米/秒)
+                lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity))
+                
+                if observe:
+                    debug(f"横向移动速度: {lateral_velocity:.3f}m/s", "控制")
+                
+                # 设置速度命令 - 只使用y轴移动，不前进和转向
+                msg.vel_des = [0, lateral_velocity, 0]  # [前进速度, 侧向速度, 角速度]
+                
+                # 发送命令
+                msg.life_count += 1
+                ctrl.Send_cmd(msg)
+        else:
+            warning("未检测到双轨道线", "警告")
+            
+            # 如果已经有了一些有效的检测，使用最后一次有效的偏差继续移动
+            if len(deviation_queue) > 0:
+                # 使用衰减的最后偏差值继续移动
+                lateral_velocity = kp * last_valid_deviation * 0.7  # 衰减系数，降低未检测到时的移动速度
+                max_lateral_velocity = 0.2  # 降低未检测情况下的移动速度
+                lateral_velocity = max(-max_lateral_velocity, min(max_lateral_velocity, lateral_velocity))
+                
+                msg.vel_des = [0, lateral_velocity, 0]
+                msg.life_count += 1
+                ctrl.Send_cmd(msg)
+                
+                if observe:
+                    warning(f"使用最后偏差值继续移动: {lateral_velocity:.3f}m/s", "暂停")
+            else:
+                # 如果一开始就没有检测到，可以尝试小范围搜索
+                if detection_total_count < 10:
+                    if detection_total_count % 2 == 0:
+                        # 向右搜索，增加搜索速度
+                        msg.vel_des = [0, -0.15, 0]
+                    else:
+                        # 向左搜索，增加搜索速度
+                        msg.vel_des = [0, 0.15, 0]
+                    
+                    msg.life_count += 1
+                    ctrl.Send_cmd(msg)
+                    
+                    if observe:
+                        debug("搜索轨道线...", "搜索")
+                else:
+                    # 超过一定次数仍未检测到，停止搜索
+                    msg.vel_des = [0, 0, 0]
+                    msg.life_count += 1
+                    ctrl.Send_cmd(msg)
+                    
+                    if observe:
+                        error("无法检测到轨道线，放弃调整", "失败")
+                    break
+        
+        # 短暂延时
+        time.sleep(0.05)
+    
+    # 停止移动
+    ctrl.base_msg.stop()
+    
+    # 计算最终位置与起始位置的变化
+    final_position = ctrl.odo_msg.xyz
+    dx = final_position[0] - start_position[0]
+    dy = final_position[1] - start_position[1]
+    
+    if observe:
+        # 在终点放置红色标记
+        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)
+        
+        info(f"横向移动距离: {abs(dy):.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})", "统计")
+    
+    # 判断是否成功
+    success = False
+    if time.time() - start_time >= max_time:
+        if observe:
+            warning("超过最大调整时间", "超时")
+    else:
+        # 如果因为已稳定在中心而退出循环，则认为成功
+        if stable_count >= required_stable_count:
+            success = True
+    
+    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]):
+    """
+    先居中到双轨道线中间，然后沿轨道线行走指定距离
+    
+    参数:
+    ctrl: Robot_Ctrl 对象，包含里程计信息
+    msg: robot_control_cmd_lcmt 对象，用于发送命令
+    distance: 目标前进距离(米)
+    speed: 前进速度(米/秒)，默认为0.5米/秒
+    max_centering_time: 最大居中调整时间(秒)，默认为15秒
+    observe: 是否输出中间状态信息和可视化结果，默认为False
+    mode: 控制模式，默认为11
+    gait_id: 步态ID，默认为26
+    step_height: 抬腿高度，默认为[0.06, 0.06]
+    
+    返回:
+    bool: 是否成功完成居中和轨道跟随
+    """
+    section("开始双轨道居中和跟随", "轨道任务")
+    
+    # 第一步：居中到轨道中间
+    if observe:
+        info("步骤1: 调整到轨道中间", "居中")
+    
+    centering_success = center_on_dual_tracks(
+        ctrl, msg, 
+        max_time=max_centering_time, 
+        observe=observe,
+        mode=mode, 
+        gait_id=gait_id, 
+        step_height=step_height,
+    )
+    
+    if not centering_success:
+        if observe:
+            error("居中调整失败，无法继续跟随轨道", "失败")
+        return False
+    
+    # 第二步：沿轨道跟随指定距离
+    if observe:
+        info(f"步骤2: 沿轨道前进 {distance:.2f}米", "跟随")
+    
+    # 导入轨道跟随函数
+    from base_move.follow_dual_tracks import follow_dual_tracks
+    
+    following_success = follow_dual_tracks(
+        ctrl, msg, 
+        speed=speed, 
+        target_distance=distance, 
+        observe=observe,
+        mode=mode, 
+        gait_id=gait_id, 
+        step_height=step_height
+    )
+    
+    if observe:
+        if following_success:
+            success("成功完成轨道居中和跟随任务", "完成")
+        else:
+            warning("轨道跟随未完全成功", "警告")
+    
+    return following_success

task_3/task_3.py

@@ -508,7 +508,7 @@ def run_task_3(ctrl, msg):
     # pass_up_down(ctrl, msg)
 
     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)
+    center_on_dual_tracks(ctrl, msg, max_time=15, max_deviation=10.0, observe=False)
 
     return
 

utils/detect_dual_track_lines.py

@@ -72,7 +72,7 @@ def detect_dual_track_lines(image, observe=False, delay=1000, save_log=True,
         cv2.waitKey(delay)
     
     # 裁剪底部区域重点关注近处的黄线
-    bottom_roi_height = int(height * 0.6)  # 增加关注区域到图像底部60%
+    bottom_roi_height = int(height * 0.4)  # 增加关注区域到图像底部60%
     bottom_roi = mask[height-bottom_roi_height:, :]
     
     if observe: