基于目标偏置与双向APF-RRT*的无人机动态避障轨迹优化

无人机轨迹规划：传统方法在动态环境中的局限性

无人机在复杂、动态的环境里规划安全高效的飞行路线是物流、巡检、应急救援等领域的关键问题。传统的路径规划方法如快速探索随机树（RRT）算法，通过在空间内随机采样点连接形成路径。这种方法虽然能保证找到路径，但效率较低，路径往往不平滑，且对动态障碍物反应迟钝。

RRT* 算法在 RRT 基础上增加了重布线和父节点重选优化，使路径长度逐渐逼近最优。但在面对动态环境时，其随机采样缺乏目的性，大量计算资源浪费在无用区域，导致规划速度跟不上环境变化。因此，结合目标偏置、双向搜索树与人工势场法（APF）成为改进方向。目标偏置增加向目标生长的趋势；双向搜索同时从起点和终点生长两棵树以提高相遇概率；人工势场法则提供力的引导以规划平滑路径。

核心算法原理

该算法融合了目标偏置策略、双向 RRT* 搜索框架和改进的人工势场（APF）引导。

目标偏置策略

基础 RRT 中，随机采样点在整个自由空间均匀产生，导致大量采样点落在无效区域。目标偏置策略引入概率阈值 p。每次生成新随机采样点时，若产生的随机数小于 p，则直接采样目标点；否则在空间内随机采样。

import random

def sample_with_goal_bias(goal, goal_bias_prob, search_space):
    """带有目标偏置的采样函数"""
    if random.random() < goal_bias_prob:
        return goal
    else:
        x = random.uniform(search_space[0][0], search_space[0][1])
        y = random.uniform(search_space[1][0], search_space[1][1])
        return (x, y)

此改动增加了随机树向目标方向生长的趋势，减少盲目探索。在简单环境中，引入目标偏置后，找到第一条可行路径的迭代次数能显著减少。

双向 RRT*（Bi-RRT*）

传统 RRT 是单棵树从起点向终点生长。双向 RRT* 同时维护两棵树：一棵 Ta 从起点生长，另一棵 Tb 从终点生长。两棵树交替进行扩展。

在每一次迭代中，当前生长的树使用目标偏置采样生成新节点并尝试添加。添加成功后，检查该新节点与另一棵树中最近节点的距离。如果距离小于连接阈值，则认为两棵树'相遇'，通过新节点连接形成完整路径。