PSO-DWA无人机三维动态避障路径规划与Matlab仿真实现

直接切入主题：把无人机从A点飞到B点，听起来像是入门级任务，但一旦把你扔进一片随时可能冒出新障碍物的三维空域，而且要同时保证安全、平稳、不绕远，这个问题立刻就从“会飞”变成了“怎么飞才好”。我最近围绕“基于PSO-DWA无人机三维动态避障路径规划”跑了一轮完整的Matlab仿真，从算法公式推导到代码实现，再到不断调参踩坑，整个过程走下来感触很深：DWA反应快、能实时避障，但它本质是个“近视眼”，只看得到眼前几步；PSO擅长全局搜索，但把它直接丢进动态环境里做实时规划又显得太重，所以两者结合刚好互补。这篇文章我就把这套方案的原理、Matlab实现框架、仿真结果和常见的调参问题完整拆开讲清楚，适合正在做无人机路径规划研究、或准备用Matlab动手验证避障算法的同学参考。

这项研究解决的核心矛盾，就是无人机在未知动态环境中“既要全局找路，又要局部避让”的双重需求。直接说结论：我实现的这套PSO-DWA融合方案，在三维静态障碍物场景下路径长度比纯DWA缩短了大约15%，面对移动障碍物时的安全避让成功率也显著提升，而且整体计算量控制在单步50毫秒以内，具备不错的实时潜力。

1. 项目定位：为什么三维动态避障要选PSO和DWA的组合

1.1 三维动态避障真正难在哪里

无人机路径规划和地面移动机器人有个本质区别：地面机器人大多数时候就是二维平面运动，转个弯、刹个车基本就能解决避障问题。到了无人机这里，自由度一下子从两个变成至少四个——三维空间里的位置加偏航姿态，更严格来说还包括爬升率、偏航角速度这些控制约束。三维避障意味着你在规划路径时不仅要考虑“往前走还是往左拐”，还要同时决定“要不要抬升高度、爬升率多大、俯仰和偏航怎么配合”。

举个我常常用来和学生解释的例子：地面上遇到一堵墙，你减速转个向就过去了；无人机遇到一个高大的障碍物，你可以选择左右绕行，也可以选择拉升越障，还可以贴地低空穿越。每一条路都有不同的飞行代价，这就把“避障”从一个几何问题变成了一个带约束的优化问题。

再加上“动态”两个字，问题复杂度又上一个台阶。动态环境里的障碍物不是静止的，可能迎面飞过来，可能从侧面切入航线，甚至可能突然出现在传感器探测范围内。这种不确定性要求路径规划算法必须具备“滚动时域”的思维方式：每时每刻重新评估当前状态，快速给出下一步动作，而不是一次性算完好几百步的路径然后就闷头执行。

1.2 单靠DWA或单靠PSO都不够，融合思路是怎么来的

我先分别说一下这两个算法的性格，这样你就能理解为什么要把它们揉在一起。

DWA，动态窗口法，诞生于移动机器人领域，后来被广泛移植到无人机上。它的核心思想非常工程化：在速度空间里采样一组可行的速度和角速度组合，对每一组速度做前向模拟，预测未来一小段时间内无人机会走出什么轨迹，然后根据轨迹的安全性和目的性打分，选得分最高的那组速度作为当前控制指令。

DWA的优点很突出：计算量小、实时性强、实现简单，而且反应速度快，特别适合用来处理突发障碍物。但它的缺点同样明显——它是一个局部规划器，只考虑眼前几步的轨迹，没有全局视野。遇到U形障碍物或者大片密集障碍区，DWA很容易把自己绕进死胡同里，表现为无人机在原地打转。

PSO，粒子群优化算法，是一个经典的群体智能全局优化算法。它的思路可以这样理解：你放出去一群粒子，每个粒子代表一条可能的路径，粒子们根据自己的历史经验和群体中其他粒子的优秀经验不断调整飞行方向，最终收敛到一条比较优的路径上。

PSO的优势是全局搜索能力强，不会轻易卡在局部最优解里。但它的弱点也很致命：迭代收敛需要时间，而且随着问题维度升高，计算量呈指数级增长，很难保证在无人机需要立即做出避障动作的几十毫秒内完成优化。

所以很自然地想到一个互补方案：用PSO去弥补DWA的“近视”，用DWA去弥补PSO的“慢反应”。在我的实现里，这个互补主要体现在两个层面——离线层面用PSO去优化DWA评价函数的权重参数，使DWA在不同环境特征下能有更合理的决策偏好；在线层面借助全局规划得到一条参考路径，在DWA的评价函数里加入对参考路径的贴合程度惩罚项，让局部避障不跑偏。

2. 算法核心：DWA动力学窗口机制与PSO参数寻优逻辑

2.1 三维DWA是怎么把速度空间“开窗”的

理解DWA的关键点在于“动态窗口”这四个字。它不是说在最大允许速度范围内任意采样，而是根据无人机当前的速度和加速度限制，动态地计算出一个“下一时刻实际能达到的速度集合”，就像给采样开了一个可移动的窗口。

放在无人机的动力学模型里，状态量一般取为：

状态向量: [x, y, z, ψ] 分别代表位置坐标和偏航角 控制输入: [v, w_ψ, w_z] 分别代表前向速度、偏航角速度、爬升角速度

动态窗口的计算公式如下：

V_dynamic = min(V_max, V_current + a_max * Δt) = max(V_min, V_current - a_max * Δt)

这里的思路很直白：无人机有惯性，受最大加速度约束，下一时刻速度不可能瞬间跳变到任意值。当前速度是2m/s，最大加速度是1m/s²，采样间隔是0.1s，那下一时刻速度最大只能是2.1m/s，最小只能是1.9m/s。这个窗口里的速度才是物理上能实现的，采样才有意义。

三维DWA和二维DWA一个很大的区别在于，除了前向速度v和偏航角速度w，还需要把爬升角速度w_z纳入采样空间。这意味着动态窗口从二维变成一个三维的立方体区域，每次采样要同时确定：

v_new = v + rand * (v_max - v_min) w_new = w + rand * (w_max - w_min) wz_new = wz + rand * (wz_max - wz_min)

对每一组采样速度，做一个前向模拟，预测未来T秒内无人机的运动轨迹。轨迹长度由采样数决定，我这里一般模拟20到30个点，每个点间隔0.1秒，然后对这条轨迹进行安全性和目标性评估。

评价函数是DWA的灵魂，一般包含三项：

score = α * heading_score + β * obstacle_score + γ * velocity_score

• heading_score：轨迹末端方向与目标点方向的夹角，越接近目标方向得分越高
• obstacle_score：轨迹与所有障碍物的最小距离，距离越大越安全
• velocity_score：轨迹对应的速度大小，速度越快越高效

三项的前面分别乘上权重系数α、β、γ。问题就出在这个地方：这三个权重怎么定？很多人直接用经验值，但实测下来，一个在开阔场景下表现很好的权重组合，放到密集障碍场景里可能根本走不动道。这正是PSO介入的理由。

2.2 PSO迭代寻优的三个核心更新公式

PSO本身不复杂，核心就是速度和位置两个更新公式。每一个粒子代表一组候选权重配置，粒子群在搜索空间里不断迭代，目标是让评价函数给出的综合表现最好。

迭代更新的标准公式是：

v_id_new = ω * v_id + c1 * rand() * (pbest_id - x_id) + c2 * rand() * (gbest_id - x_id) x_id_new = x_id + v_id_new

拆开讲一下这里面每一项的语义：

• 惯性项ω * v_id：保持粒子原来的运动趋势，相当于“惯性”。ω大，全局搜索能力强，不容易早熟；ω小，局部开发能力强，收敛快。

• 个体认知项c1 * rand() * (pbest_id - x_id)：粒子向自己历史上最好的位置飞行。这一项保证每个粒子不会盲目跟风，保留个体经验。

• 社会认知项c2 * rand() * (gbest_id - x_id)：粒子向群体中最好的位置飞行。这一项保证信息共享，让好解能被其他粒子快速发现。

在我这个项目里，粒子维度就是3，对应DWA的三个权重系数。每个粒子跑一遍DWA仿真，返回路径长度、碰撞次数等指标，然后根据这些指标计算出适应度值，不断更新pbest和gbest，直到收敛。

值得注意的是，PSO搜索的不是“路径”本身，而是“参数”本身。这大大降低了搜索空间维度，也保证了收敛速度。如果你让PSO直接搜一条上百个航迹点构成的完整路径，维度太高，迭代几百轮都不一定收敛，实时性完全跟不上。

2.3 融合方案设计：参数优化加路径引导，两层互补

融合方案可以有多种设计思路，我梳理一下常见的三种，你就能理解我最后为什么选了其中一种。

第一种方案：离线的PSO权重寻优。在预先设定好的典型场景中运行PSO，找出表现最好的权重组合，然后把这一组权重固定下来用于DWA。优点是实现简单，计算开销小；缺点是环境一旦变化，固定权重仍然可能失效。

第二种方案：在线的PSO权重自适应。在每次DWA决策之前，根据无人机当前的环境感知信息，动态调用PSO去重新优化权重。说白了就是“每走一步都要算一组新权重”。这种方案适配性最好，但计算量很大，对实时性要求很高的动态避障场景不太友好。

第三种方案：PSO做全局参考路径规划，DWA做局部跟踪避障。先用PSO在已知或半已知的地图上规划出一条从起点到终点的全局参考路径，然后在DWA的评价函数里加入一个偏离惩罚项，让DWA生成的局部轨迹既避开动态障碍物，又不偏离全局方向。

我最终采用的是第一种和第三种融合的综合实现：PSO离线优化DWA权重参数，同时PSO规划出一条全局参考路径，DWA实时跟随路径并避让动态障碍物。这样既保证了动态避障的实时反应能力，又弥补了DWA缺少全局视野的短板。

具体落地上，评价函数变成了四项：

score = α * goal_score + β * obstacle_score + γ * velocity_score + δ * path_score

其中path_score表示当前轨迹偏离全局参考路径的程度，越贴近参考路径得分越高。δ这个权重系数也纳入PSO的优化范围，所以粒子维度升级为4。

3. Matlab工程结构：从零搭建可复用的仿真系统

3.1 程序模块怎么划分才能既清晰又好调试

做Matlab仿真的最大教训就是不要把所有代码塞进一个脚本里。尤其是这种涉及多个算法模块的程序，如果全部堆在主文件里，改一个参数就要翻半天，而且容易改出一堆低级错误。

我最终把程序拆成了六个模块，各自职责单一：

1. 主控制脚本：负责初始化场景、启动主循环、调用各个模块、显示和控制动画
2. 无人机运动模型：根据当前控制输入，更新无人机的位置和姿态
3. 障碍物生成模块：在场景中布置静态障碍物，同时生成移动障碍物的运动轨迹
4. DWA核心模块：动态窗口计算、速度采样、轨迹模拟、评价函数计算
5. PSO优化模块：实现粒子群迭代寻优，优化DWA权重参数
6. 可视化模块：绘制三维路径、障碍物模型、无人机轨迹动画

模块之间的调用关系也很简单：主循环里调用DWA模块得到控制指令，运动模型更新状态，可视化模块实时绘图，而PSO模块在主循环开始前先离线跑完，把最优权重传给DWA模块。

3.2 DWA核心函数的Matlab实现细节

DWA的核心实现并不复杂，但有几个地方很容易写错。我贴一段关键代码，说明每一步在干什么。

function [best_vel] = dwa_control(state, goal, obstacles, weights, param) % state: [x, y, z, psi] 当前状态 % goal: [gx, gy, gz] 目标点 % obstacles: Nx3矩阵，每行一个障碍物坐标 % weights: [alpha, beta, gamma, delta] 评价权重 % param: 包含速度边界、加速度边界、采样步长等参数 % 第一步：计算动态窗口 v_min = max(param.v_min, state.v - param.a_max * param.dt); v_max = min(param.v_max, state.v + param.a_max * param.dt); w_min = max(param.w_min, state.w - param.aw_max * param.dt); w_max = min(param.w_max, state.w + param.aw_max * param.dt); wz_min = max(param.wz_min, state.wz - param.az_max * param.dt); wz_max = min(param.wz_max, state.wz + param.az_max * param.dt); % 第二步：在动态窗口内均匀采样速度组合 num_v = 30; num_w = 30; num_wz = 10; best_score = -inf; best_vel = [0, 0, 0]; for v = linspace(v_min, v_max, num_v) for w = linspace(w_min, w_max, num_w) for wz = linspace(wz_min, wz_max, num_wz) % 第三步：前向模拟轨迹 traj = simulate_trajectory(state, v, w, wz, param); % 第四步：计算各评价项 goal_score = calc_goal_score(traj, goal); obs_score = calc_obstacle_score(traj, obstacles, param.min_radius); vel_score = v / param.v_max; % 归一化速度评分 path_score = calc_path_score(traj, param.ref_path); total = weights(1) * goal_score + ... weights(2) * obs_score + ... weights(3) * vel_score + ... weights(4) * path_score; if total > best_score best_score = total; best_vel = [v, w, wz]; end end end end end

第三步模拟轨迹的函数，本质就是基于运动学方程做数值积分。每一次迭代更新位置和姿态，代码是这样的：

function traj = simulate_trajectory(state, v, w, wz, param) traj = zeros(param.sim_steps, 3); x = state(1); y = state(2); z = state(3); psi = state(4); for i = 1:param.sim_steps x = x + v * cos(psi) * param.dt; y = y + v * sin(psi) * param.dt; z = z + wz * param.dt; psi = psi + w * param.dt; traj(i, :) = [x, y, z]; end end

这里有个细节值得注意：三维DWA里的偏航角速度w和爬升角速度wz是独立采样的，但实际飞行中两者并非完全独立——大幅度加减速和大幅度转向同时发生时，无人机会有姿态耦合。在更精细化的仿真里要加上动力学约束，比如升力限制、最大转弯坡度对应的最小转弯半径等。我第一版仿真偷懒没加，结果出现了一些很“不真实”的急转弯轨迹，后面才把约束补上。

评价函数里的归一化处理也很重要。三个评价项的数值量级可能差出几个量级，比如障碍物距离可能是米级，速度可能是米每秒级，如果不做归一化，某个权重就会因为数值量级大而掩盖其他项的影响，最终退化成单目标优化。我一般在每个评价项里除以其最大可能值，让结果落在0到1之间，再乘权重加权。

3.3 PSO权重优化模块的编码与适应度设计

PSO模块的编码并不难，难在适应度函数怎么设计。我的做法是：把一组权重参数输入到DWA仿真器里，让无人机从起点飞行到目标点，统计这块飞行过程中的表现指标，然后根据这些指标计算适应度。

适应度函数的设计直接决定了最终权重好不好用。我用的公式是：

fitness = λ1 * (1 / path_length) + λ2 * (1 / time_cost) * safety_penalty * smoothness_penalty

其中safety_penalty是一个乘法因子：如果飞行过程中发生碰撞，这个惩罚因子会大幅降低适应度，比如直接乘0.1；绕行路程过长也会适当降低得分。这样设计的好处是惩罚是“相乘”而不是“相加”，碰撞一次就直接让适应度掉一个数量级，粒子会倾向于远离会导致碰撞的参数组合。

核心的PSO迭代代码大致是这样的：

function [best_weights] = pso_optimize(param, obstacles, start, goal) num_particles = 20; max_iter = 50; dim = 4; % alpha, beta, gamma, delta % 初始化粒子位置和速度 positions = rand(num_particles, dim) .* param.w_bound; velocities = zeros(num_particles, dim); pbest = positions; pbest_score = zeros(num_particles, 1); gbest = positions(1, :); gbest_score = -inf; for iter = 1:max_iter for i = 1:num_particles % 用当前权重跑一次DWA仿真 score = evaluate_weights(positions(i, :), obstacles, start, goal); if score > pbest_score(i) pbest(i, :) = positions(i, :); pbest_score(i) = score; end if score > gbest_score gbest = positions(i, :); gbest_score = score; end end % 更新速度和位置 w = 0.9 - 0.5 * (iter / max_iter); % 线性递减惯性权重 c1 = 1.5; c2 = 1.5; for i = 1:num_particles r1 = rand(1, dim); r2 = rand(1, dim); velocities(i, :) = w * velocities(i, :) + ... c1 * r1 .* (pbest(i, :) - positions(i, :)) + ... c2 * r2 .* (gbest - positions(i, :)); positions(i, :) = positions(i, :) + velocities(i, :); % 边界处理，超出边界的粒子拉回边界 positions(i, :) = min(max(positions(i, :), param.w_lb), param.w_ub); end end best_weights = gbest; end

这里用了一个线性的惯性权重递减策略，从0.9慢慢降到0.4。前期惯性大，粒子跑得远，搜索范围广；后期惯性小，粒子在局部区域精细搜索，加速收敛。实测下来这个策略比固定权重效果好很多，不容易早熟，也不容易在最优解附近震荡。

3.4 关键参数配置表：一份可以直接用的参考值

把参数配表放在这里，是我在多次实验中反复调整后觉得比较稳的一组值，可以作为你仿真的起点。注意这些不是绝对真理，你的场景和动力学模型不同时，参数要跟着变。

4. 仿真场景与结果分析：从数据看PSO加持后的改进

4.1 场景设计：静态障碍物加移动障碍物的组合压力测试

只跑一个简单场景说明不了问题，所以我设计了三组难度递增的场景来验证算法的表现。

场景一是开阔地形加少量静态障碍物，主要验证基本路径规划能力。场景二在场景一的基础上穿插了多个移动障碍物，让它们以不同速度和角度穿越无人机的航线，检验动态避障能力。场景三最狠，设计了一个类似“峡谷穿越”的地形，两侧有高大的障碍墙，堵死了绕行的可能，无人机必须从中间狭窄通道穿过，同时通道里还有一个移动障碍物不停来回巡视。

障碍物在Matlab里用三维球体表示，这样碰撞检测最简单：计算轨迹点到所有障碍物球心的距离，如果小于球半径加安全裕度，就判定碰撞。移动障碍物的位置更新规则也很简单，每个障碍物有各自的运动速度向量：

obs_pos = obs_pos + obs_vel * dt;

为了让仿真更接近真实，我让移动障碍物碰到场景边界时反弹，这样它们不会跑出仿真区域。

4.2 评价指标：别只盯着路径长度看

评价一个路径规划算法好不好，不能只看路径长度，至少要看四个维度：

安全性指标是最基本的，统计整个飞行过程中有没有发生碰撞，以及最小离障碍物距离是多少。效率指标看路径总长度和总飞行时间，路径短不一定代表时间短，因为可能出现走很多急转弯导致速度一直上不去的情况。平滑性指标看轨迹的曲率变化，曲率变化太剧烈说明无人机一直在做剧烈的姿态调整，实际飞行中会带来很大的能耗和姿态稳定问题。计算开销指标看单帧决策的平均耗时，这对判断算法能不能上真机非常关键。

这四个维度有些互相矛盾：路径越短往往意味着贴着障碍物飞，安全裕度就小；走得太稳又可能绕远路。所以我的结果分析里会把这些指标综合起来看，而不是拿一个指标说事。

4.3 对比实验：固定权重DWA与PSO-DWA的实测数据

我在三个场景里分别跑了固定权重DWA和PSO-DWA各50次取平均，结果如下表：

数据反映出的信息很清晰：在简单静态场景下，两个算法表现差别不大，PSO-DWA也只是路径略微短一点，因为这种情况下固定权重已经够用了。但随着场景复杂度上升，固定权重DWA的问题逐渐暴露，在场景三里碰撞次数达到12次，基本就是不可用的状态。PSO-DWA在动态场景里的碰撞次数降到0，在峡谷高压场景也做到了只碰撞一次，这已经是非常显著的改善了。

单步耗时方面，PSO-DWA比固定权重DWA多了大约3到5毫秒，这是因为要在评价函数里额外计算参考路径偏离项。这个开销完全可以接受，38毫秒的决策周期对大多数无人机飞控系统来说毫无压力。

我在场景三里观察到一个特别有意思的现象：固定权重DWA经常会“贪快”冲进狭窄通道，结果迎面碰到移动障碍物时来不及刹车，直接撞上去。而PSO优化后的权重对障碍物安全距离赋予了更高重视，无人机在进入通道之前会先稍微减速、悬停观察，等移动障碍物掠过之后再快速通过。虽然单看决策有点“保守”，但整体飞行时间反而更短了，因为减少了碰撞后的重新规划次数。

5. 调试避坑：动态环境下反复踩过的真实问题

5.1 权重互锁现象：改一个参数带崩全局

我最早调试时有个误区，觉得障碍物权重β越大越安全，于是直接把它从1.0调到3.0。结果无人机确实离障碍物远了，但路径变得极其绕，还经常出现明明终点就在前方，无人机却偏要绕一大圈的情况。问题出在权重之间的“互锁”效应：β调大后，障碍物评价项主导了总评分，安全优先完全压过了目标优先和速度优先，无人机近乎偏执地远离所有障碍物，哪怕威胁程度很低也大幅绕行。

后来我总结出一套联动调参的经验：三个基础权重的比例关系比绝对值重要，一般优先确定障碍物权重β，因为它决定了安全底线；再根据β调节目标权重α，保证无人机有明确的目的性；速度权重γ是微调项，主要用来控制飞行效率。每次改权重后，最少跑三个不同难度的场景验证，不能只在一个场景里调好就完事。

5.2 动态障碍物突然出现时，无人机来不及反应怎么办

场景二测试中有一个让我很头疼的问题：固定权重DWA在移动障碍物速度超过2m/s时，经常会出现“看到障碍了但刹不住”的情况。原因是DWA的前向模拟时间太短，只有1.5秒，以无人机当前速度5m/s来说，1.5秒只能看7.5米远的距离。障碍物迎面飞过来，相对速度接近10m/s，留给无人机的反应时间不到0.75秒，这时再减速已经晚了。

解决办法有两个方向：一是把前向模拟时间从1.5秒加到3秒，无人机看得更远，自然能提前规避；二是增加采样数量，让速度空间中低速度区域的采样更密集，这样紧急情况下更容易找到可用的减速方案。我两个方向都试了，最终选择把模拟时间加到2.5秒，采样数调到30，单步耗时约35毫秒，仍然在可接受范围。

但前向模拟时间也不是越长越好。模拟时间过长，轨迹预测要考虑更远未来，而动态障碍物的运动预测误差会随时间快速累积，太远的预测反而不可靠。这需要在“看得远”和“看得准”之间取平衡。

5.3 PSO在线优化的实时性陷阱

我一朋友拿着这套代码想直接改成在线优化版本，每走一步都用PSO重新算一组权重，结果单步决策耗时直接飙到400多毫秒，完全跑不动。这个坑值得提一下：PSO优化权重本身是离线任务，一旦放进在线循环，20个粒子、每个粒子要跑一次完整DWA仿真，相当于每次决策做了20次路径规划，耗时自然爆炸。

我的建议是，除非你的应用场景计算平台很强，否则别轻易做纯在线的PSO权重重优化。更务实的方案是分层处理：离线阶段在多种典型场景下跑PSO，得到一组在不同环境中表现都稳定的折中权重，或者存一个“场景特征到最优权重”的映射表；在线阶段DWA只根据当前环境特征快速匹配权重，而不实时运行PSO。这样既保留了PSO的优化能力，又不拖累实时性。

5.4 三维可视化的调试技巧

路径规划仿真里可视化不是锦上添花，是刚需。看不到无人机飞行轨迹，出了bug连问题在哪都说不清楚。我调试贯穿始终的一招是用Matlab的plot3实时绘制轨迹，同时用scatter3绘制障碍物位置，最后把全局参考路径和DWA实际飞行路径叠加在一起画，一眼就能看出局部规划有没有偏离全局脉络。

移动障碍物的可视化有个技巧：不要每帧都删除旧句柄重建新图形，这样会导致动画卡顿。正确做法是先创建句柄，每帧更新XData、YData、ZData，这样Matlab只是更新数据而不重建图形对象，流畅度会好很多。

还有一个小细节是碰撞检测的可视化：一旦检测到轨迹点与障碍物距离小于安全半径，不仅要在数据层面记录碰撞，还要在三维图里用红色标记碰撞点。这种视觉冲击力会让你更快意识到问题的严重性，比看一行数据输出直观得多。

最后再分享一个我个人的习惯：每次调试完一个版本，都会把这一版的参数和对应的仿真结果截图存下来。路径规划算法的参数调整非常依赖经验积累，今天这个场景用这组参数表现好，不代表明天换个环境还好用。把这些“参数-结果”对应关系记录下来，慢慢就能形成自己的调试直觉，下次碰到类似的问题，脑子里立马就有反应该怎么调。

这套PSO-DWA方案目前我已经跑了接近两百轮仿真，总体稳定性相当不错。后续我还在考虑两个扩展方向：一个是给无人机加上更精细的动力学约束模型，让仿真轨迹更贴近真实飞行特性；另一个是把PSO全局参考路径也做成动态更新的，让全局规划器能够感知到环境变化。如果你也在折腾路径规划，欢迎从我这套基础框架开始，按自己的场景往里加东西。