基于子路径数组的每步均衡方向避障导航求解

先梳理你之前方案的核心问题：

1. 仅取距离≤1的点作为样本，数据量太少，方向计算易受局部噪声影响，缺乏全局连贯性。
2. 按单元格计算均衡方向时，未结合终点引导，容易出现绕路或方向迷失的情况。

针对这些问题，给出以下优化思路：

• 预处理子路径数据：将所有子路径按网格单元格划分（单元格边长可设为1），记录每个单元格内所有子路径的走向（方向向量），同时标记障碍物单元格，后续导航直接跳过。
• 加权均衡方向计算：以当前位置所在单元格为中心，取3x3邻域范围内的单元格扩大样本池；对收集到的方向向量做加权平均——权重结合两个维度：子路径长度（越长的路径越具参考性）、方向与终点的契合度（和当前点到终点的向量夹角越小，权重越高）。
• 增量式导航+避障 fallback：每一步按均衡方向移动，若移动后进入障碍物区域，从8个基础方向中选择与均衡方向夹角最小的可行方向作为替代；到达终点区域或超出最大步数时停止。

import numpy as np

def preprocess_paths(paths, bbox, cell_size=1, obstacle_mask=None):
    """
    预处理子路径，按单元格存储方向向量及对应权重
    参数:
        paths: 子路径数组，每个路径格式为 [[x1,y1], [x2,y2], ...]
        bbox: 边界框，格式为 [x_min, y_min, x_max, y_max]
        cell_size: 单元格边长
        obstacle_mask: 障碍物掩码，二维布尔数组，True表示该单元格为障碍物
    返回:
        cell_directions: 字典，键为单元格坐标(i,j)，值为(方向向量, 路径权重)的列表
    """
    cell_directions = {}
    x_min, y_min, x_max, y_max = bbox
    num_cols = int((x_max - x_min) / cell_size) + 1
    num_rows = int((y_max - y_min) / cell_size) + 1

    for path in paths:
        path_len = len(path)
        if path_len < 2:
            continue
        # 用对数缩放路径长度，避免长路径权重过度占优
        path_weight = np.log(path_len)

        for i in range(path_len - 1):
            curr_point = np.array(path[i])
            next_point = np.array(path[i+1])
            # 计算当前点所属单元格
            curr_col = int((curr_point[0] - x_min) / cell_size)
            curr_row = int((curr_point[1] - y_min) / cell_size)
            # 跳过边界外或障碍物单元格
            if curr_col < 0 or curr_col >= num_cols or curr_row < 0 or curr_row >= num_rows:
                continue
            if obstacle_mask is not None and obstacle_mask[curr_row][curr_col]:
                continue
            # 归一化方向向量
            dir_vec = next_point - curr_point
            vec_norm = np.linalg.norm(dir_vec)
            if vec_norm < 1e-6:
                continue
            dir_vec = dir_vec / vec_norm
            # 存入对应单元格
            cell_key = (curr_row, curr_col)
            if cell_key not in cell_directions:
                cell_directions[cell_key] = []
            cell_directions[cell_key].append((dir_vec, path_weight))
    
    return cell_directions

def calculate_balanced_direction(curr_pos, end_pos, cell_directions, bbox, cell_size=1, obstacle_mask=None):
    """
    计算当前位置的均衡导航方向
    参数:
        curr_pos: 当前位置 [x,y]
        end_pos: 终点位置 [x,y]
        cell_directions: 预处理后的单元格方向数据
        bbox: 边界框
        cell_size: 单元格边长
        obstacle_mask: 障碍物掩码
    返回:
        balanced_dir: 归一化的均衡方向向量，无有效路径时返回指向终点的方向
    """
    x_min, y_min, _, _ = bbox
    curr_col = int((curr_pos[0] - x_min) / cell_size)
    curr_row = int((curr_pos[1] - y_min) / cell_size)
    end_vec = np.array(end_pos) - np.array(curr_pos)
    end_vec_norm = np.linalg.norm(end_vec)
    if end_vec_norm < 1e-6:
        return np.array([0, 0])
    
    end_dir = end_vec / end_vec_norm
    total_weight = 0.0
    weighted_dir = np.array([0.0, 0.0])

    # 遍历3x3邻域单元格
    for dr in (-1, 0, 1):
        for dc in (-1, 0, 1):
            cell_key = (curr_row + dr, curr_col + dc)
            if cell_key not in cell_directions:
                continue
            if obstacle_mask is not None and obstacle_mask[cell_key[0]][cell_key[1]]:
                continue
            # 累加加权方向向量
            for dir_vec, path_weight in cell_directions[cell_key]:
                # 方向与终点方向的余弦相似度作为额外权重
                direction_weight = np.dot(dir_vec, end_dir) + 1
                combined_weight = path_weight * direction_weight
                weighted_dir += dir_vec * combined_weight
                total_weight += combined_weight
    
    # 无有效路径时直接指向终点
    if total_weight < 1e-6:
        return end_dir
    
    balanced_dir = weighted_dir / total_weight
    # 检查方向是否指向障碍物，若有则切换备选方向
    if obstacle_mask is not None:
        next_pos = curr_pos + balanced_dir * cell_size
        next_col = int((next_pos[0] - x_min) / cell_size)
        next_row = int((next_pos[1] - y_min) / cell_size)
        if 0 <= next_row < obstacle_mask.shape[0] and 0 <= next_col < obstacle_mask.shape[1]:
            if obstacle_mask[next_row][next_col]:
                # 从8个基础方向中选最优可行方向
                candidate_dirs = [
                    np.array([1,0]), np.array([-1,0]), np.array([0,1]), np.array([0,-1]),
                    np.array([0.707,0.707]), np.array([0.707,-0.707]),
                    np.array([-0.707,0.707]), np.array([-0.707,-0.707])
                ]
                valid_candidates = []
                for dir in candidate_dirs:
                    temp_next_pos = curr_pos + dir * cell_size
                    temp_col = int((temp_next_pos[0] - x_min) / cell_size)
                    temp_row = int((temp_next_pos[1] - y_min) / cell_size)
                    if 0 <= temp_row < obstacle_mask.shape[0] and 0 <= temp_col < obstacle_mask.shape[1]:
                        if not obstacle_mask[temp_row][temp_col]:
                            valid_candidates.append(dir)
                if valid_candidates:
                    angles = [np.arccos(np.dot(balanced_dir, dir)) for dir in valid_candidates]
                    best_idx = np.argmin(angles)
                    balanced_dir = valid_candidates[best_idx]
    return balanced_dir

def navigate(start_pos, end_pos, paths, bbox, obstacle_mask=None, step_size=1, max_steps=1000):
    """
    执行从起点到终点的导航
    参数:
        start_pos: 起点 [x,y]
        end_pos: 终点 [x,y]
        paths: 子路径数组
        bbox: 边界框
        obstacle_mask: 障碍物掩码
        step_size: 每步移动距离
        max_steps: 最大步数，防止死循环
    返回:
        nav_path: 导航路径数组
    """
    cell_directions = preprocess_paths(paths, bbox, obstacle_mask=obstacle_mask)
    curr_pos = np.array(start_pos, dtype=np.float64)
    nav_path = [curr_pos.copy()]
    end_threshold = step_size * 2  # 到达终点的判定阈值

    for _ in range(max_steps):
        if np.linalg.norm(curr_pos - end_pos) < end_threshold:
            nav_path.append(np.array(end_pos))
            break
        # 计算均衡方向并移动
        balanced_dir = calculate_balanced_direction(curr_pos, end_pos, cell_directions, bbox, obstacle_mask=obstacle_mask)
        next_pos = curr_pos + balanced_dir * step_size
        # 边界检查，超出则调整为指向终点
        x_min, y_min, x_max, y_max = bbox
        if next_pos[0] < x_min or next_pos[0] > x_max or next_pos[1] < y_min or next_pos[1] > y_max:
            end_vec = np.array(end_pos) - curr_pos
            end_vec_norm = np.linalg.norm(end_vec)
            if end_vec_norm > 1e-6:
                end_dir = end_vec / end_vec_norm
                next_pos = curr_pos + end_dir * step_size
        nav_path.append(next_pos.copy())
        curr_pos = next_pos
    
    return np.array(nav_path)

• 扩大样本范围：用3x3邻域单元格的路径方向数据，避免单单元格样本不足导致的方向偏差。
• 加权逻辑：同时考虑路径合理性（长度）和导航目标（终点方向），让均衡方向更贴合实际需求。
• 避障 fallback：当均衡方向不可行时，自动切换到最优可行方向，保证导航的连续性。

内容的提问来源于stack exchange，提问作者May