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

一种结合人工势场法（APF）与双向 RRT*算法的无人机路径规划方案。针对传统 RRT 在复杂环境中采样效率低、路径曲折的问题，该混合算法利用 APF 的方向引导优势与 RRT*的渐进最优特性，并通过双向搜索加速收敛。文章详细拆解了环境建模、引力斥力计算及树结构实现等关键模块，提供了改进后的 Python 代码示例，展示了如何在障碍物密集场景下提升规划速度与路径质量。

念念不忘发布于 2026/4/6更新于 2026/4/131 浏览

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

去年夏天，我在一个无人机巡检项目里遇到了一个棘手的问题：传统的 RRT 算法在复杂林地环境中规划路径时，经常'卡'在密集的树木之间，要么采样效率低下导致规划时间过长，要么生成的路径曲折得让无人机像喝醉了一样左右摇摆。团队尝试了各种参数调整，效果都不理想。直到我们把人工势场法的引导机制引入到双向 RRT*算法中，情况才发生了根本性转变——不仅规划速度提升了近 70%，生成的路径也平滑了许多。

这种结合了 APF（人工势场法）和双向 RRT的混合算法，如今已经成为许多无人机开发者解决复杂环境路径规划的秘密武器。它巧妙地将 APF 的方向引导优势与 RRT的渐进最优特性结合起来，同时利用双向搜索大幅提升收敛速度。今天，我就从工程实践的角度，带你一步步实现这个算法，分享我在实际项目中积累的参数调优经验，并提供可直接运行的 Python 代码。

1. 理解 APF-RRT*算法的核心思想

在开始写代码之前，我们需要先弄清楚这个混合算法到底解决了什么问题。传统的 RRT 算法虽然概率完备，但在复杂环境中存在明显的局限性：随机采样导致大量无用的探索，特别是在狭窄通道或障碍物密集区域，算法可能会浪费大量时间在'死胡同'里打转。

人工势场法则提供了另一种思路——将目标点视为引力源，障碍物视为斥力源，通过势场的梯度下降来引导移动。这种方法方向性强，但容易陷入局部极小值，特别是在 U 型或狭窄通道环境中。

APF-RRT*的巧妙之处在于取长补短：

RRT*提供骨架：保证算法的概率完备性和渐进最优性
APF 提供引导：让随机采样不再完全'随机'，而是有方向性地向目标推进
双向搜索加速：从起点和终点同时生长两棵树，加快连接速度

我在实际项目中发现的几个关键优势：

采样效率提升：传统 RRT 有大量采样点落在无意义的区域，而 APF 引导后，约 80% 的采样都集中在有希望的方向上
路径质量改善：初始路径就更接近最优，后续优化迭代次数减少
狭窄通道通过性增强：势场引导帮助算法'找到'狭窄通道的入口

注意：APF-RRT并不是要完全取代传统 RRT，而是在特定场景下（如障碍物密集、通道狭窄的环境）提供更高效的解决方案。在开阔场景中，传统 RRT*可能就足够了。

2. 算法实现的关键模块拆解

让我们把 APF-RRT*算法分解成几个可独立实现和测试的模块。这种模块化的开发方式不仅便于调试，也让你能更清晰地理解每个部分的作用。

2.1 环境建模与障碍物表示

在开始路径规划之前，我们需要一个合适的环境表示方法。对于无人机轨迹规划，我通常使用二维或三维的网格地图，这比连续空间表示更易于碰撞检测。

import numpy as np
import matplotlib.pyplot as plt
from typing import List, Tuple, Optional

class Environment:
    def __init__(self, width: float, height: float, resolution:  = ):
        
        .width = width
        .height = height
        .resolution = resolution
        
        .grid_width = (width / resolution)
        .grid_height = (height / resolution)
        .obstacle_grid = np.zeros((.grid_height, .grid_width), dtype=)

     ():
        
        cx, cy = center
        grid_cx = (cx / .resolution)
        grid_cy = (cy / .resolution)
        grid_r = (radius / .resolution)
        
         i  ((, grid_cy - grid_r), (.grid_height, grid_cy + grid_r + )):
             j  ((, grid_cx - grid_r), (.grid_width, grid_cx + grid_r + )):
                 (i - grid_cy)** + (j - grid_cx)** <= grid_r**:
                    .obstacle_grid[i, j] = 

     () -> :
        
        x1, y1 = point1
        x2, y2 = point2
        
        steps = (np.hypot(x2 - x1, y2 - y1) / .resolution) + 
         i  (steps + ):
            t = i / steps
            x = x1 + (x2 - x1) * t
            y = y1 + (y2 - y1) * t
            grid_x = (x / .resolution)
            grid_y = (y / .resolution)
            
             ( <= grid_x < .grid_width   <= grid_y < .grid_height):
                 .obstacle_grid[grid_y, grid_x]:
                     
            :

相关免费在线工具

加密/解密文本

使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。在线工具，加密/解密文本在线工具，online

RSA密钥对生成器

生成新的随机RSA私钥和公钥pem证书。在线工具，RSA密钥对生成器在线工具，online

Mermaid 预览与可视化编辑

基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。在线工具，Mermaid 预览与可视化编辑在线工具，online

curl 转代码

解析常见 curl 参数并生成 fetch、axios、PHP curl 或 Python requests 示例代码。在线工具，curl 转代码在线工具，online

Base64 字符串编码/解码

将字符串编码和解码为其 Base64 格式表示形式即可。在线工具，Base64 字符串编码/解码在线工具，online

Base64 文件转换器

将字符串、文件或图像转换为其 Base64 表示形式。在线工具，Base64 文件转换器在线工具，online

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

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