基于 APF-RRT*算法的无人机避障与轨迹规划实现

这个环境类提供了基础的障碍物管理和碰撞检测功能。在实际项目中，你可能需要根据具体的传感器数据（如激光雷达点云）来构建更复杂的环境表示。

2.2 人工势场法的实现

人工势场法的核心是计算引力和斥力。这里我实现了一个改进版本，解决了传统 APF 在狭窄通道中斥力过强的问题。

class ArtificialPotentialField:
    def __init__(self, goal: Tuple[float, float], attractive_gain: float = 1.0, repulsive_gain: float = 0.5, influence_distance: float = 3.0):
        """
        初始化人工势场
        :param goal: 目标点坐标
        :param attractive_gain: 引力增益系数
        :param repulsive_gain: 斥力增益系数
        :param influence_distance: 障碍物影响距离
        """
        self.goal = np.array(goal)
        self.ka = attractive_gain
        self.kr = repulsive_gain
        self.d0 = influence_distance

    def attractive_force(self, position: np.ndarray) -> np.ndarray:
        """计算引力"""
        distance = np.linalg.norm(position - self.goal)
        # 改进的引力函数：在接近目标时减小引力，避免振荡
        if distance <= 1.0:
            force = self.ka * (position - self.goal)
        else:
            force = self.ka * (position - self.goal) / distance
        return -force  # 负梯度方向

    def repulsive_force(self, position: np.ndarray, obstacles: List[Tuple[float, float, float]]) -> np.ndarray:
        """计算所有障碍物的总斥力"""
        total_force = np.zeros(2)
        pos_array = np.array(position)
        for obs in obstacles:
            obs_pos = np.array([obs[0], obs[1]])
            obs_radius = obs[2]
            # 计算到障碍物边缘的距离
            distance = np.linalg.norm(pos_array - obs_pos) - obs_radius
            if distance < self.d0:
                # 改进的斥力函数：避免在目标附近斥力过大
                if distance <= 0.1:  # 防止除零
                    distance = 0.1
                # 斥力大小
                magnitude = self.kr * (1.0/distance - 1.0/self.d0) * (1.0/(distance**2))
                # 斥力方向（远离障碍物）
                direction = (pos_array - obs_pos) / np.linalg.norm(pos_array - obs_pos)
                total_force += magnitude * direction
        return total_force

    def total_force(self, position: np.ndarray, obstacles: List[Tuple[float, float, float]]) -> np.ndarray:
        """计算总力（引力 + 斥力）"""
        att_force = self.attractive_force(position)
        rep_force = self.repulsive_force(position, obstacles)
        # 限制最大力的大小，避免数值不稳定
        max_force = 5.0
        total = att_force + rep_force
        force_norm = np.linalg.norm(total)
        if force_norm > max_force:
            total = total / force_norm * max_force
        return total

我在这里做了两个重要改进：

1. 距离相关的引力调节：接近目标时减小引力，避免在目标点附近振荡
2. 斥力函数改进：使用(1/distance - 1/d0) * (1/distance²)的形式，避免在障碍物附近斥力无限大

2.3 双向 RRT*树结构的实现

RRT*算法的核心是树结构。在双向版本中，我们需要维护两棵树，并实现它们的生长和连接逻辑。