人工势场引导双向RRT*：三维无人机航迹规划与Matlab实践

无人机三维路径规划这件事，真正上手之后你会发现，它核心不在“无人机”三个字，而在“路径规划”这四个字。三维空间的路线搜索和二维迷宫寻路完全不是一个量级，多出来的那个维度，会让采样效率、碰撞检测、内存开销都出现指数级的上涨。这个项目标题拆开看其实就三样东西：人工势场、双向RRT*、三维航迹规划。它解决的典型问题是：在已知环境地图、给定起点和终点的情况下，让无人机在布满障碍物的三维空间里找出一条安全、可飞、长度尽量短的路径。适合需要做无人机避障航迹规划的学生、想快速验证算法效果的工程师，以及正在对比不同规划算法性能的研究者。

这篇内容我会直接基于这套“改进双向人工势场引导RRT*”的代码方案来展开，把设计思路、算法原理、Matlab实现细节、参数调优和踩坑经验一次性讲清楚。代码我不会整段贴完，但关键模块和最容易写错的部分会讲得很细，照着复现是可行的。

1. 项目设计思路：为什么会把人工势场“塞进”双向RRT*

1.1 单一算法各自的痛点和互补逻辑

先说RRT*。RRT*本身是RRT（快速搜索随机树）的渐进最优改进版，它通过重新选择父节点（chooseParent）和重连接（rewire）两步操作，让搜索树不断优化路径代价。单纯RRT能找到一条可行路径，但大概率是折线、冗余段多、甚至绕远路，因为它的采样是完全随机的。在三维空间里，随机采样的问题会被放大：采样点散布在球体空间里，树扩张的方向非常“散”，搜索到终点附近的概率低，收敛速度慢。

人工势场法（APF）思路相反，它不采样，而是把目标点看作吸引力源，把障碍物看作斥力源。无人机在势场中被“推着”走，方向性强、实时性好。但它有个知名毛病：局部极小值。某个平衡点附近引力和斥力抵消，无人机就会停住或者绕圈，而且这个状态在复杂障碍环境中几乎必然出现，只是时间早晚的问题。

双向RRT*则是另一个方向上的改进，它同时从起点和终点生长两棵树，每轮交替扩展，并尝试连接两棵树。一旦两棵树成功接上，就得到一条完整路径。双向版本比单向版本搜索速度快很多，原因很朴素：两棵树各自搜索一半距离，缩短了有效搜索深度。

把三者结合起来，思路就明朗了：用人工势场来“引导”RRT的采样方向，让随机树更多地向终点和目标区域生长，而不是无头苍蝇一样乱撞；再用双向树结构降低单树搜索深度。APF不直接驱动机器人移动，它的角色变成“采样导向器”，既保留了RRT的完备性和渐进最优性，又把APF的方向优势吸收进来。

1.2 改进后算法的整体流程

整个流程按顺序是这么走的：

1. 读取或生成三维网格地图，设置起点、终点、障碍物位置和尺寸。
2. 初始化两棵搜索树：T1以起点为根，T2以终点为根。
3. 在每次迭代中，从两棵树中选一棵进行扩展。
4. 扩展时，采样点不完全是随机的。按照设定概率，一部分采样点直接用终点坐标，一部分通过人工势场函数生成“偏置采样点”（这个点是沿势场下降方向靠近目标的点），剩余部分才做均匀随机采样。
5. 从当前这棵树上找到距离采样点最近的节点，然后沿方向以固定步长生成新节点。
6. 检查新节点与最近节点之间的连线是否与障碍物碰撞。无碰撞则加入树，并执行RRT*的chooseParent和rewire优化。
7. 尝试用新节点连接另一棵树，判断两棵树之间的连线是否无碰撞、路径代价是否合理。
8. 若两棵树成功连接，提取完整路径。
9. 对路径做简化和平滑处理，去掉冗余转折点，输出最终三维航迹。

这套流程在Matlab里写起来并不复杂，真正费心思的是第4步和第6步，一个关系到采样效率，一个关系到路径质量，后面我会详细展开。

2. 三维环境建模与碰撞检测：写代码前必须先定的四个基础

2.1 障碍物与地图的离散化表达

三维路径规划的第一步不是写算法，而是把“地图”这个东西定义清楚。Matlab里面三维地图常见的表达方式有两种：体素栅格（voxel grid）和球体集合。体素栅格适合用occupancy map工具生成，直观但内存开销大，300×300×300的栅格就是2700万个格点，无论是存储还是遍历都很吃力。球体集合则轻量得多，把障碍物抽象为若干个球体，每个球体只需要圆心坐标和半径，存储简洁，碰撞检测也快。

我在实际仿真中倾向于先用球体障碍物做算法验证，因为球体的碰撞检测数学上非常干净。如果地形复杂，再用栅格地图或者把地形表面离散为凸包，不过那属于工程化的后处理了。最开始写代码，球体障碍物足够。

地图的坐标系我统一用三维笛卡尔坐标，单位设为米，起点和终点都放在一个长方体空域内，空域的边界就是采样范围：x、y、z各有一个下限和上限。障碍物数量不固定，但数量决定了算法难度，一般用10到30个随机生成的球体来做实验比较有意义。

2.2 球体包围盒与线段插值碰撞检测

碰撞检测是路径规划里的核心操作，每一轮扩展都要做一次，所以它必须快，否则整体效率会被拖垮。对于球体障碍物，碰撞检测的数学表达式很直接：判断节点到球心的距离是否大于半径。但路径规划里检测的不是“点是否在障碍物里”，而是“线段是否和障碍物相交、是否离障碍物太近”。

线段碰撞检测常用插值法，把线段按步长切成若干小段，逐一检查每个采样点到球心的距离。步长如果太小，计算量激增；步长太大，可能“跨过”一个薄障碍物却没检测到。实际调试经验是把检测间隔设为地图对角线的0.5%到1%之间，既稳又不太慢。

另一种更优雅的方法是用点到线段的距离公式，直接计算线段与球心的最近距离，然后判断这个最近距离是否小于球半径。这样不需要分段采样，精确度和效率更高。Matlab代码写起来大概是：

function collisionFlag = isCollision(p1, p2, obstacles, safetyMargin) % p1, p2: 线段两端点坐标 % obstacles: 结构体数组，含 center 和 radius collisionFlag = false; for i = 1:length(obstacles) center = obstacles(i).center; r = obstacles(i).radius + safetyMargin; d = distancePointToSegment(center, p1, p2); if d < r collisionFlag = true; return; end end end

这里我加了一个safetyMargin安全余量，相当于给障碍物“膨胀”了一圈。无人机不同于质点，它有自己的尺寸，路径点离障碍物太近在物理执行时非常危险，所以规划阶段就留出余量是最省事的方案。

2.3 起点、终点与可行空间的约定

起点和终点本身必须落在自由空间里，不能放在障碍物内部。这个看似废话，但很多第一次写代码的人没检查就运行，结果搜索树一开始就被判定碰撞，程序卡在死循环。写代码先跑一个前置检查：

assert(~isInObstacle(start, obstacles), '起点在障碍物内部'); assert(~isInObstacle(goal, obstacles), '终点在障碍物内部');

另外，可行空间的边界也要在采样函数里体现，随机采样坐标必须包含在空域边界内。如果采样点落在边界外，直接丢弃或者重新采样都行，但千万不要不加限制地生成，否则树的节点会发散到地图之外，后面的路径提取会变得很难看。

还有一个小细节：起点和终点之间的距离也会影响算法表现。如果两点距离太近，双向树很快就相接了，看不出算法差异；距离太远，双向树也容易绕路。做对比实验时，起点和终点距离固定在一个适中的范围，通常设置在空域对角线的50%到70%之间比较合理。

3. 核心算法模块拆解：APF引导采样与双向树的扩展逻辑

3.1 人工势场的作用方式：不是直接移动，而是“带节奏”

人工势场在经典APF算法里是直接作用于运动体的，但在改进RRT中，它换了个角色。它不直接生成路径点，而是帮助生成“有方向的采样点”。这就解决了RRT最让人头疼的问题：随机采样太盲目。

具体做法是：在每次需要生成采样点时，按照设定的概率进入三种情况之一。第一种，以一定概率直接取终点作为采样点，这叫目标偏置采样（goal bias），它保证树不会长时间完全偏离终点。第二种，调用势场函数生成一个引导采样点。这个引导采样点怎么算？以当前搜索树上最新节点为起点，计算目标点对它的引力方向，以及周围障碍物对它的斥力方向，两者矢量叠加得到一个合力方向，然后从当前节点沿合力方向移动一段距离，得到一个新的采样点。第三种，完全均匀随机采样。

这里有个关键理解点：势场引导采样并不是直接把RRT变成了APF，它只是让随机树更倾向于往势场下降的方向探索。即使势场采样点所在位置路径走不通，RRT的随机性仍然能保证探索能力，这就是组合的优势。

力场的计算公式比较常规。引力方向是目标点坐标减当前节点坐标，引力大小可以恒定也可以随距离变化。斥力场则对障碍物周围的节点产生排斥作用，每个障碍物都对采样点产生一个斥力，方向是从障碍物中心指向采样点，大小和采样点到障碍物表面的距离成反比。距离越近，斥力越大。

3.2 双向树的交替扩展与最近点选择

双向RRT*的实现细节比单向版本多几个坑。首先是两棵树的交替策略。最简单的实现是每次迭代选一棵树扩展一次，两棵树轮流来，这就是RRG（Rapidly-exploring Random Graph）里的标准做法。但使用过之后你会发现，如果终点附近空间非常复杂，反向树的扩展速度会拖慢整体。一个改进是动态选择扩展哪棵树：比较两棵树的节点数量，扩展节点少的树，让两棵树规模尽量平衡，从概率上提升连接成功率。

最近点选择是另一个容易忽略的瓶颈。在三维空间里，如果树里已经有几千个节点，寻找距离采样点最近的节点如果写成一重循环遍历所有节点，每轮迭代耗时很长。这里我用的是KD树或者干脆Matlab自带的KDTreeSearcher，把最近邻搜索从O(N)降到O(logN)。节点数量超过500个时，这个优化的效果肉眼可见。

两棵树连接时，我倾向于在反向树（终点树）上寻找距离正向树新节点最近的节点，然后判断这段连线是否无碰撞、代价是否合理。如果满足条件，就说明两棵树已连通，路径就出来了。这里还有一个重要细节：连接成功后不要立刻退出，可以再继续迭代若干轮，尝试用rewire优化进一步降低路径代价。不过从实际效果来看，后期优化的空间已经不大，RRT*的渐进最优性需要靠增量rewire逐步体现，所以我会在代码里设置一个“连接成功后继续迭代N轮再退出”的开关。

3.3 目标偏置与势场引导的概率分配策略

采样策略里概率的设定，直接影响算法的探索能力和收敛速度。目标偏置概率alpha太大会让树总是直接冲向终点，遇到复杂障碍时容易在局部反复碰壁；偏置概率太小又失去了引导意义。势场引导概率beta同样需要权衡：beta太大，树会变得过于“贪婪”，容易陷入APF的局部极小值问题。

我采用的参数组合是alpha = 0.1、beta = 0.3、随机概率 = 0.6。这个组合不是拍脑袋定的，实测下来的逻辑是：10%的采样点直接指向终点，保证整体的方向感；30%的采样点沿势场引导方向分布，让树能比较聪明地绕过障碍物；60%的采样点完全随机，维持RRT*的探索能力。三者配合，既不会丢失采样完备性，又能明显加速收敛。

当然这组参数不是万能的。障碍物越密集、分布越复杂，alpha和beta就应该适当调低，给随机采样留出更多空间。如果环境空旷，alpha和beta可以调高一些。调参时要观察树的节点分布：节点如果明显集中在某个障碍物附近，说明势场引导概率太高了；如果节点分布散乱、迟迟不靠近终点目标，说明目标偏置不够。

3.4 Raw路径生成、冗余点去除与轨迹平滑

当两棵树成功连接后，从连接节点向两边回溯，可以得到一组从起点到终点的路径节点。但这组节点直接拿来用会很难看，原因在于RRT*的采样是离散的，路径会呈现明显的折线形态，转折点多，甚至有回折现象。这显然不适合无人机直接飞行。

第一步是冗余点去除。从起点开始，依次尝试连接后续更远的节点，如果连线不碰撞，就跳过中间节点。这个操作也叫路径简化，相当于把路径压缩成尽量少的直线段。第二步是轨迹平滑。可以用B样条曲线或者均匀B样条对路径节点做拟合，让航迹变得连续光滑。无人机飞行时，尖锐折线段意味着巨大的过载，平滑处理是必须的。

Matlab里做B样条平滑比较顺手，核心代码大概是：

% pts: 简化后的路径点 t = 1:size(pts, 1); tt = 1:0.1:size(pts, 1); % 插值密度 smoothX = spline(t, pts(:, 1), tt); smoothY = spline(t, pts(:, 2), tt); smoothZ = spline(t, pts(:, 3), tt);

平滑之后的路径还需要再做一次碰撞检查，因为插值点可能在拐弯处侵入障碍物膨胀区域。这个检查很多人第一次做都会忘，不是路径点不碰撞就万事大吉，路径段中间的位置同样需要确认。

4. Matlab实现：从数据结构到仿真画面的落地细节

4.1 数据结构与函数模块划分

Matlab写这种算法，最忌讳的就是堆一堆脚本、变量满天飞。我的习惯是把功能拆成独立函数，每个函数干一件事。整个工程至少要有这么几个模块：

• 地图生成与可视化模块：生成障碍物、绘制三维地图
• 采样器模块：负责随机采样、目标偏置采样、势场引导采样
• RRT*扩展模块：扩展函数、chooseParent、rewire
• 碰撞检测模块：点到线段距离、障碍物判断
• 路径提取与平滑模块：路径回溯、简化、样条插值
• 主控脚本：把所有模块串起来，跑仿真并输出结果

数据结构上，树节点建议用结构体数组或一个矩阵来存节点坐标。我实际用矩阵存坐标（N×3），另用一个数组存父节点索引（N×1），再用一个数组存从根节点到当前节点的路径代价（N×1）。这样chooseParent和rewire操作可以直接基于数组索引进行，效率高。

每条树的节点信息没必要存成结构体嵌套结构体，Matlab的结构体数组如果动态扩充会很慢，因为每次增加节点都要重新分配内存。用数值矩阵预分配一块空间，比如预先分配5000行，如果节点超了再扩充，速度会快很多。这个细节在仿真规模大时差距非常明显。

4.2 采样器和扩展函数的代码实现

采样器是整条改进链路里最关键的部分，代码上我建议直接封装成一个函数，每次调用返回一个采样点。核心框架如下：

function x_rand = samplePoint(map, tree, q_goal, alpha, beta) r = rand(); if r < alpha % 目标偏置采样 x_rand = q_goal; elseif r < alpha + beta % 势场引导采样 x_current = tree(end, :); % 最近节点或最新节点 F = computePotentialForce(x_current, map, q_goal); step = clamp(norm(F), map.stepMin, map.stepMax); x_rand = x_current + step * F / norm(F); else % 均匀随机采样 x_rand = map.randPosition(); end % 限制在边界内 x_rand = clampToBounds(x_rand, map.bounds); end

势场力computePotentialForce里面就是标准APF的引力斥力叠加，不再赘述。

扩展函数则是从树上最近节点向采样点方向生长固定步长：

function x_new = extendTree(tree, x_rand, stepLen) [~, idx] = min(sum((tree - x_rand).^2, 2)); x_near = tree(idx, :); dir = (x_rand - x_near) / norm(x_rand - x_near); x_new = x_near + stepLen * dir; end

注意这里算最近节点用欧氏距离平方比较，省去开方，速度略微提升。如果节点很多，换成KDTreeSearcher更稳。

chooseParent和rewire操作是需要特别仔细的部分。chooseParent的思路是：以x_new为中心，在半径R内搜索已有树上的候选父节点集合，对每个候选父节点，检查x_new和它的连线是否碰撞，如果碰撞就跳过，不碰撞就比较“当前最近节点作为父节点的代价”和“候选父节点作为父节点的代价”，取代价更小的那个作为新父节点。代价不是两点间直线距离这么简单，而是从起点到候选父节点的累计代价加上两点间的距离代价。

rewirepermutation则是对x_new附近的已有节点做反向优化：如果把某个邻居节点的父节点换成x_new，路径代价会变低，且连线不碰撞，那就执行替换。这两个操作是RRT之所以叫RRT的原因，也是代码最容易写错的地方。写错之后一个典型症状就是：算法跑得飞快，但路径质量非常差，长度明显不是最优甚至次优。

4.3 仿真参数选择的经验参考值

仿真参数的设定直接决定算法能不能收敛、收敛多快、路径多好。我常用的参数参考值如下，可以作为起点再微调：

步长控制着整个算法的“分辨率”。步长太大，新节点可能一步跨过窄通道，导致搜索树根本进不去某些凹形区域，路径质量会大打折扣。步长太小，树的节点暴增，整个搜索过程变得非常慢。比较好的做法是先用地图已知障碍物分布估计一下窄通道宽度，把步长设置成窄通道宽度的1/3左右。

最大迭代次数也别拍脑袋选。我见过有人设5000次迭代，结果跑了很久没收敛，最后发现是步长太小导致节点数爆炸。迭代次数的选择要结合步长来估计：两棵树的平均扩展距离乘以迭代次数，至少要等于起点到终点的直线距离的1.5倍，否则没有足够的探索空间。

4.4 仿真结果怎么看：路径长度、搜索时间与节点数

代码跑通之后，不要只盯着那一条路径看，还要把三个指标记录下来：路径总长度、算法运行时间、搜索树总节点数。这三个指标最能说明改进算法和基础算法之间的差异。

路径总长度的计算方式是累加简化前路径上所有相邻节点之间的欧氏距离。注意要在简化和平滑之前算，因为平滑后长度会变化。运行时间可以用tic/toc包住主循环，但注意排除掉绘图的时间，绘图往往是性能大头。节点数则统计两棵树的节点总和。

做对比实验时，建议在同一地图上分别跑基础RRT*和加入APF引导+双向结构的改进版本，各跑20到50次取平均值。随机算法单次结果波动很大，只看一次结果毫无意义。平均下来，改进版本搜索时间通常会明显缩短，路径长度也会更短。如果发现改进版和基础版差别不明显，多半是势场引导概率太小，或者目标偏置概率设置不合理。

5. 常见问题与排错技巧实录

5.1 最容易踩的四个坑

第一个坑：势场引导采样点直接落在障碍物内部。原因很简单，引力斥力叠加后的方向可能正好把采样点推向障碍物。这个问题在代码里几乎必然出现，所以生成引导采样点之后，一定要先检查它是否在自由空间，如果在障碍物内部就把这次采样降级为均匀随机采样。这个操作并不影响引导效果，因为引导采样点本来就是大概率帮助而非必须帮助。

第二个坑：两棵树从彼此区域穿过，却没有连接成功。原因通常是连接条件判断太严格，或者碰撞检测的安全余量太大，使得两棵树最近节点之间的连线刚好被判定为碰撞。这里建议连接检测时使用和普通扩展一样的碰撞检测函数，但不要额外放大安全余量。路径提取出来后可以再做一次整体检查，如果有问题就重新规划。

第三个坑：rewire导致树结构破坏，出现“断链”。这是一个非常隐蔽的bug。rewire执行时，如果邻居节点的新父节点路径代价计算错误，会出现某个节点的父节点链变成了环状，导致回溯路径时死循环或路径缺失。排查方法：每次执行rewire后，从根节点出发做一次遍历，确认所有节点都能通过父节点链追溯到根节点。

第四个坑：绘图和分析阶段严重拖慢代码。Matlab的plot3每帧都重新绘制整个树会非常耗时，尤其是节点上千之后，仿真画面几乎和PPT翻页一样卡。解决方案有三：一是每隔N次迭代才更新一次绘图；二是把plot3的句柄保存下来，用set更新数据而不是重新plot；三是仿真结束后一次性绘制最终结果，不做实时动画。做算法验证优先选第三种，把实时可视化关闭，速度能提升一个数量级。

5.2 我的调试顺序与工作习惯

我拿到一套改进算法代码后的调试顺序基本固定：先关掉平滑和rewire，只跑扩展和碰撞检测，确认整个搜索能够从起点反向连通到终点；通了之后，打开rewire，对比路径长度；再打开势场引导，看搜索时间是否下降；最后加平滑和可视化。一步一步打开功能，出问题时定位范围很小。

调试过程中，日志输出是必要的。我习惯每500次迭代输出一条记录：当前迭代次数、两棵树的节点数、最近连接距离。当最近连接距离持续下降但速度很慢时，大概率是步长太大在窄通道附近反复碰壁；如果最近连接距离震荡不降，大概率是碰撞检测或势场参数出了问题。

参数调优时，我习惯用控制变量法。比如固定alpha = 0.1，只调beta从0到0.5，观察搜索时间和路径长度两个指标。这样做的原因是参数之间存在耦合，同时乱调很难定位是谁生效。

还要特别提醒一点：随机数种子。在做对比实验时，如果每次都让算法使用不同的随机序列，那么实验差异里会混入随机波动，导致对比结果不可信。Matlab里可以在主循环前用rng(1)固定种子，或者使用stream来管理随机数。我的习惯是每组参数跑20次，每次用不同的种子，最后统计均值和方差，这样既能保证公平又能反映算法的稳定性。

最后一个工作习惯：路径规划结束后，把路径节点保存成csv或mat文件，供后续做无人机动力学仿真、轨迹跟踪控制时直接导入。很多人在规划阶段花费大量精力，却忘了和后续模块衔接。路径规划的输出不该只是一张仿真图，而应该是一个干净的、时间标记的航迹点序列。

最后再分享一点实际体会

调试这套算法真正花时间的地方，不是RRT*本身的结构，而是三个改进点之间的平衡。APF引导的力度太大，算法会暴露出势场法的局部极小值问题，树卡在某个障碍物附近不进不退；引导力度太小，随机树的采样又回到原生态，双向结构带来的优势被浪费。我个人调试时，APF相关的引力增益和斥力增益参数并不会在一开始就调得特别精确，而是先把增益设成中间值，让整个系统跑通，再根据树节点分布去微调。先让它“能跑”，再让它“跑得聪明”，这个顺序很重要。

这套方案后续还可以扩展的地方也不少。比如加入无人机的动力学约束，让偏航角、俯仰角、最大过载都体现在路径可行性里；再比如把环境从静态换成动态，引入速度障碍物概念，让RRT树可以实时更新避障。本质上，改进双向人工势场引导RRT提供了一个很好的框架，在这个框架之上做任何扩展，都会比从零开始实现简单得多。