做三维无人机路径规划仿真，最常遇到的组合就是“全局搜索算法+轨迹平滑”。我前阵子给一个模拟项目搭航迹规划模块，白天理顺栅格地图，晚上就想着怎么把强化学习塞进去，翻来覆去试了几轮，最后落地的一套方案就是基于Q-learning三次样条曲线求解三维无人机路径规划问题。核心思路并不算新，但组合起来非常稳：先用Q-learning在三维栅格环境里学出一条离散可行路径，再用三次样条把折线平滑成连续可飞的期望轨迹。这篇文章我会直接把完整的Matlab代码放出来，同时把状态空间怎么设计、奖励函数怎么调、样条参数t怎么取、平滑后撞障碍怎么办这些细节讲清楚，适合正在做毕业设计、算法仿真，或者想快速验证强化学习路径规划想法的朋友。

1. 整体思路与问题拆解

1.1 三维路径规划到底在解决什么

先把问题定义清楚。无人机三维路径规划的输入通常是一张环境地图、一个起点、一个终点，外加若干约束条件；输出是一条从起点到终点的无碰撞轨迹。在三维空间里，这条轨迹不仅要求“能走过去”，还得尽量满足无人机本身的运动限制，比如爬升角不能太陡、转弯不能太急、速度不能突变。二维路径加一个高度层之后，很多看似好用的算法立刻不够看了，原因有两个：一是搜索空间暴涨，二是“可通行”的定义变得更复杂——同一个经纬度点上，不同高度层的可通行性往往不一样。

我习惯把这个问题拆成两层看：第一层是“找得到路”，第二层是“飞得出来”。找得到路，指的是搜索算法能在离散网格或连续空间里找出一条连接起点与终点、避开所有障碍的几何路径；飞得出来，指的是这条路径在交给飞控之前，要转成一条时间上连续、空间上平滑的参考轨迹。不少新手只盯着第一层，拿A或RRT跑出结果就收工，但那种折线基本没法直接给飞控用，因为航向不连续、角速度尖峰会触发限幅。这也是我坚持把“规划+平滑”放在同一个方案里的原因。

1.2 为什么选Q-learning而不是A或RRT

我当时选算法，其实先把主流方案过了一遍。A的优点是启发函数引导，找最短路径快，但它需要提前把整个环境建模成栅格图，环境一变就得重算，而且在三维栅格上空间开销增长很快。RRT系列在连续空间和复杂约束下表现不错，但随机采样的特性导致每次跑出来的路径都不太一样，狭窄通道里收敛也比较慢。Q-learning这边是另一个思路：它不显式建模完整环境，只通过“状态-动作-奖励”不断试错，最终在Q表里学到每个状态下最优动作的估计。

不是说我有多偏爱强化学习，而是它带来的容错性和迁移性确实更贴合我当时的需求：环境结构可能微调，起终点也可能变化。用A的话，地图一变全得重来；RRT则是每次结果不稳定；Q-learning训练完之后，Q表本身就是一个策略表，只要起终点能对应当前状态，贪心查表就能出路径。这个特性在“障碍变化不大、又想快速反复出多条路径”的场景里很有用。代价是训练需要时间，网格一大收敛就慢，这个缺点后面会专门讲。

1.3 三次样条曲线要解决什么问题

Q-learning输出的本质上是一串离散栅格坐标，比如 [1,1,2] -> [2,1,2] -> [3,2,3] 这种。把这些点连起来就是折线，折线在转折处有尖角，速度和加速度会出现突变，飞控很难跟踪。三次样条曲线的作用，就是对这些离散点做平滑插值或拟合，生成一条C²连续的曲线。C²连续意味着位置、速度、加速度在整段路径上连续，这是小型多旋翼和固定翼都比较容易接受的参考轨迹形式。

Matlab里做三次样条其实特别省事，spline或者csape都能搞。但这里有一个非常关键的选择：到底做插值还是拟合。如果直接用原始路径点做插值，曲线会严格穿过每个点，点与点之间距离不均匀时，曲线可能出现不自然的波浪；更稳妥的做法是把路径点距离的累积长度作为参数t，再在t上加密采样，必要时用csape做带平滑系数的样条拟合。后面第3章我会把参数化细节写清楚，这一步处理不好，样条曲线很容易“飞”到障碍物外面去。

2. 核心原理：Q-learning与三次样条曲线

2.1 Q-learning的三要素：状态、动作、奖励

Q-learning算法本身没有太多弯弯绕，核心就是一张Q表。假设环境被离散成若干状态s，每个状态可选动作集合A，Q(s,a)表示在状态s下执行动作a的期望回报。训练时不断用贝尔曼方程更新这张表，标准更新公式是：

Q(s,a) ← Q(s,a) + α[ r + γ · max(Q(s',a')) − Q(s,a) ]

其中α是学习率，γ是折扣因子，r是当前转移获得的即时奖励，s'是执行动作后到达的新状态。这个公式最重要的地方，是用“当前奖励 + 未来最大Q值”和“当前Q值”的差值做时间差分更新，本质上是在逐步逼近真实回报。

在三维路径规划里，状态可以取栅格坐标(x,y,z)，动作取六个方向，也就是六邻域模型，前后左右上下。有人会加对角线动作来让路径更短，但三维对角动作容易斜着穿障碍体，还需要额外做线段与障碍的交点检测，对初学者不太友好，六邻域是最稳的起点。

2.2 状态空间离散化与维度控制

Q表的规模是状态数×动作数。如果地图是20×20×20，全部状态8000个，乘以6个动作，只有48000个Q值，在Matlab里就是一个小矩阵的事。但如果把分辨率提到100×100×100，就是100万状态、600万Q值，内存开始吃紧，训练收敛也会慢到让人头疼。

我常用的控制方法有两个。第一，把障碍物所在的栅格直接从状态列表里剔除，Q表里只给可通行状态，能省不少空间。第二，用相对坐标建模状态，比如把状态记为“当前点相对目标点的坐标差”或者“当前点相对起点的坐标差”。这个技巧既能压缩状态空间，又让策略在不同起终点之间具备迁移性。在模拟项目X里，我改成相对坐标之后，训练收敛回合数直接降到原来的三分之一左右。

不过相对坐标有个限制：目标点一变，奖励函数里“到目标距离”那一项也得跟着改。所以真正要做一个通用路径规划脚本时，我更倾向于保留绝对坐标，但把地图裁剪到必要飞行区域，再把不可通行状态剔除，这样代码简单，调试时不容易绕晕。

2.3 奖励函数设计细节与常见误区

奖励函数是整个方案里最需要花心思的部分。设计不好时，Q-learning要么学不到路，要么学出来全是绕路行为。我常用的一组基准奖励如下。

• 到达目标点：+100
• 撞到障碍物或越界：-100
• 每走一步：-1
• 向目标靠近：额外加 0.5 × (d_old − d_new)，其中d_old是当前点到目标的欧氏距离，d_new是下一步到目标的欧氏距离

为什么一定要有“每走一步-1”？因为如果只有目标正奖励和障碍负奖励，Q-learning在空旷区域会倾向于乱转，反正没撞墙就继续耗着。步数惩罚能引导它找更短路径。而“向目标靠近”的奖励是稀疏奖励问题的经典缓解手段——当终点太远、地图障碍分布太散，靠随机探索撞到终点的概率极低，完全依赖终点奖励的话训练基本没法收敛。

但这种“势能型”奖励也有副作用：权重太大，算法会倾向于贴着障碍边缘走，因为离目标近分数就高，学出来的路径擦墙走，安全余量很小。我建议权重控制在0.3到0.8之间，并配合较大的碰撞惩罚。还有一点，计算d_old和d_new时用欧氏距离，不要用曼哈顿距离，否则奖励函数会诱导出奇怪的折线路径。

2.4 三次样条插值的数学模型与平滑逻辑

三次样条的原理可以用一句话概括：把曲线分成多段，每段是一个三次多项式，并且保证在各节点位置函数值、一阶导数、二阶导数连续。这样整条曲线既不会突兀拐弯，也不会有导数跳动。对三维路径来说，一般做法是把它当成参数曲线：先构造参变量t，然后分别对x(t)、y(t)、z(t)做样条插值。

参变量t的选取决定了平滑效果。我见过有人直接用自然数1、2、3当t，结果就是路径点之间时间间隔被强行拉成相等，但实际相邻点距离往往不一样，样条很容易出现“过冲”，在转弯处冒出一个明显偏离原路径的弧线。更合理的办法是用路径点累积折线长度作为t：每个点对应t[k]等于前面所有折线段的长度和，相当于按实际走过的路程均匀参数化。之后再在t的区间内加密采样，比如取200个点，插值出来的曲线非常贴合原始路径，不容易过冲。

如果原始路径点太粗糙，或者希望进一步降低速度波动，可以用csape函数做带平滑系数的拟合。Matlab里csape(x, y, 'variational', 平滑参数)可以控制样条对原始点的贴近程度，本质是在拟合误差和曲率之间做权衡。平滑参数越大，轨迹越光滑，但离原始路径越远，越容易撞障碍。所以平滑之后必须做碰撞校验，不行就回退参数。

3. Matlab实现与代码解析

3.1 程序结构与运行流程

整个程序分四步：环境建模、Q-learning训练、路径提取、三次样条平滑。我建议不管是不是要原样复现这个方案，都保持这个结构，因为它把“找路”和“平滑”两个模块解耦了，后续替换任何一环都方便。我自己的习惯是，环境建模单独写成一段，方便换地图；训练循环单独写，方便观察Q值收敛情况；路径提取和平滑放一起处理，因为两者数据流直接串起来。

3.2 地图建模与状态编号

下面这段代码生成一个12×12×8的栅格环境，包含两组固定障碍物。为了稳定复现，我用固定障碍物；实际做实验时可以把obs改成随机生成，对比着看。

%% 环境参数 GridSize = [12, 12, 8]; % 三维栅格大小 startNode = [1, 1, 2]; % 起点 goalNode = [12, 12, 7]; % 终点 % 固定障碍物 obs = []; for x = 5:6 for y = 5:6 for z = 1:4 obs(end+1, :) = [x, y, z]; end end end for x = 9:10 for y = 3:4 for z = 3:6 obs(end+1, :) = [x, y, z]; end end end obs = unique(obs, 'rows');

这里最关键的思想是建立stateIndex映射。我用全零矩阵表示不可通行，可通行栅格按顺序编号，之后把障碍物从状态空间里剔除掉。Q表的大小只取决于可行栅格数而不是整个地图体积，这在障碍较多的环境里收益很明显。

stateIndex = zeros(GridSize); stateList = []; cnt = 0; for x = 1:GridSize(1) for y = 1:GridSize(2) for z = 1:GridSize(3) if ~ismember([x y z], obs, 'rows') cnt = cnt + 1; stateIndex(x, y, z) = cnt; stateList(cnt, :) = [x, y, z]; end end end end sStart = stateIndex(startNode(1), startNode(2), startNode(3)); sGoal = stateIndex(goalNode(1), goalNode(2), goalNode(3));

有人会问，为什么不直接拿x,y,z当状态维度的下标，还要绕一层stateIndex？因为Q表如果用三维数组做，尺寸是12×12×8×6，看起来还能接受；但地图一旦变成50×50×50，就非常吃力了。把状态线性化之后，Q表是二维的，状态数×动作数，地图越大优势越明显，后面接DQN也容易过渡。

3.3 Q-learning训练主循环

训练主循环我按“回合-步数”两层来写，每回合从起点出发，最多走maxSteps步，到达终点或撞障碍就结束本回合。下面这段是核心训练代码。

alpha = 0.3; gamma = 0.9; epsilon = 0.9; epsilonMin = 0.05; epsilonDecay = 0.995; episodes = 500; maxSteps = 200; Q = zeros(cnt, 6); % 6个动作 for ep = 1:episodes s = sStart; for step = 1:maxSteps if rand < epsilon a = randi(6); % 探索 else [~, a] = max(Q(s, :)); % 利用 end nextCoord = stateList(s, :) + actionSet(a, :); if any(nextCoord < 1) || any(nextCoord > GridSize) || ismember(nextCoord, obs, 'rows') r = -100; nextS = s; else nextS = stateIndex(nextCoord(1), nextCoord(2), nextCoord(3)); if nextS == sGoal r = 100; else dOld = norm(stateList(s, :) - goalNode); dNew = norm(nextCoord - goalNode); r = -1 + 0.5 * (dOld - dNew); end end Q(s, a) = Q(s, a) + alpha * (r + gamma * max(Q(nextS, :)) - Q(s, a)); if nextS == sGoal || r == -100 break; end s = nextS; end epsilon = max(epsilonMin, epsilon * epsilonDecay); end

需要注意几个实现细节。第一，撞障碍时我把nextS设成s，并在原状态写入一个很差的Q值，这样算法能快速学到“这个动作不能碰”，而不是原地罚一遍然后继续绕。第二，越界和撞障碍统一用相同的负奖励，避免边界处出现奇怪的策略偏好。第三，epsilon在每回合结束后衰减，前期多探索、后期多利用，这是强化学习训练里比较通用的经验。

3.4 路径提取与三次样条平滑

训练完之后，Q表就是策略表。提取路径时从起点开始，每次选择Q值最大的动作，把对应栅格坐标追加到path里。这里要处理一个边界情况：如果Q表引导的下一步是障碍，可能是因为奖励权重问题导致Q值估计出现偏差，我加了一个就近选择有效动作的兜底逻辑，避免路径提取在最开始就断掉。

path = startNode; s = sStart; stepCount = 0; while s ~= sGoal && stepCount < maxSteps [~, a] = max(Q(s, :)); sNextCoord = stateList(s, :) + actionSet(a, :); if any(sNextCoord < 1) || any(sNextCoord > GridSize) || ismember(sNextCoord, obs, 'rows') validFlag = false; for aa = 1:6 tempCoord = stateList(s, :) + actionSet(aa, :); if ~any(tempCoord < 1) && ~any(tempCoord > GridSize) && ~ismember(tempCoord, obs, 'rows') a = aa; sNextCoord = tempCoord; validFlag = true; break; end end if ~validFlag break; end end s = stateIndex(sNextCoord(1), sNextCoord(2), sNextCoord(3)); path(end+1, :) = sNextCoord; stepCount = stepCount + 1; end

接下来是三次样条平滑。用累积路径长度作为参数t，再统一采样到200个点，曲线对x、y、z三个坐标分别插值，得到的smoothPath就是平滑后的轨迹点。

t = [0; cumsum(sqrt(sum(diff(path, 1, 1).^2, 2)))]; tq = linspace(0, t(end), 200)'; xq = spline(t, path(:,1)', tq); yq = spline(t, path(:,2)', tq); zq = spline(t, path(:,3)', tq); smoothPath = [xq, yq, zq];

这里提醒一下，spline函数要求原始t必须单调递增，累积长度天然满足。如果想把整段轨迹做得更平滑，可以换成csape拟合，但代价是平滑后的点可能偏离原始折线更远，必须有碰撞校验兜底。

3.5 完整Matlab代码

把上面几段拼成一个完整脚本，方便直接复制运行。代码注释比较详细，建议按段读，不要只看最终结果。整个脚本在Matlab 2019b之后的版本都能跑，12×12×8的栅格环境、500轮训练，常规台式机大概10秒内完成，路径提取和平滑都是瞬间的事，所以在仿真里反复调参完全来得及。

%% Q-learning + 三次样条 三维无人机路径规划 clc; clear; close all; rng(0); %% 1. 环境参数 GridSize = [12, 12, 8]; startNode = [1, 1, 2]; goalNode = [12, 12, 7]; % 障碍物生成 obs = []; for x = 5:6 for y = 5:6 for z = 1:4 obs(end+1, :) = [x, y, z]; end end end for x = 9:10 for y = 3:4 for z = 3:6 obs(end+1, :) = [x, y, z]; end end end obs = unique(obs, 'rows'); %% 2. 状态空间 stateIndex = zeros(GridSize); stateList = []; cnt = 0; for x = 1:GridSize(1) for y = 1:GridSize(2) for z = 1:GridSize(3) if ~ismember([x y z], obs, 'rows') cnt = cnt + 1; stateIndex(x, y, z) = cnt; stateList(cnt, :) = [x, y, z]; end end end end sStart = stateIndex(startNode(1), startNode(2), startNode(3)); sGoal = stateIndex(goalNode(1), goalNode(2), goalNode(3)); % 六邻域动作 actionSet = [1 0 0; -1 0 0; 0 1 0; 0 -1 0; 0 0 1; 0 0 -1]; nActions = size(actionSet, 1); %% 3. Q-learning训练 alpha = 0.3; gamma = 0.9; epsilon = 0.9; epsilonMin = 0.05; epsilonDecay = 0.995; episodes = 500; maxSteps = 200; Q = zeros(cnt, nActions); for ep = 1:episodes s = sStart; for step = 1:maxSteps if rand < epsilon a = randi(nActions); else [~, a] = max(Q(s, :)); end nextCoord = stateList(s, :) + actionSet(a, :); if any(nextCoord < 1) || any(nextCoord > GridSize) || ismember(nextCoord, obs, 'rows') r = -100; nextS = s; else nextS = stateIndex(nextCoord(1), nextCoord(2), nextCoord(3)); if nextS == sGoal r = 100; else dOld = norm(stateList(s, :) - goalNode); dNew = norm(nextCoord - goalNode); r = -1 + 0.5 * (dOld - dNew); end end Q(s, a) = Q(s, a) + alpha * (r + gamma * max(Q(nextS, :)) - Q(s, a)); if nextS == sGoal || r == -100 break; end s = nextS; end epsilon = max(epsilonMin, epsilon * epsilonDecay); end %% 4. 路径提取 path = startNode; s = sStart; stepCount = 0; while s ~= sGoal && stepCount < maxSteps [~, a] = max(Q(s, :)); sNextCoord = stateList(s, :) + actionSet(a, :); if any(sNextCoord < 1) || any(sNextCoord > GridSize) || ismember(sNextCoord, obs, 'rows') validFlag = false; for aa = 1:nActions tempCoord = stateList(s, :) + actionSet(aa, :); if ~any(tempCoord < 1) && ~any(tempCoord > GridSize) && ~ismember(tempCoord, obs, 'rows') a = aa; sNextCoord = tempCoord; validFlag = true; break; end end if ~validFlag break; end end s = stateIndex(sNextCoord(1), sNextCoord(2), sNextCoord(3)); path(end+1, :) = sNextCoord; stepCount = stepCount + 1; if size(path, 1) > 500 break; end end %% 5. 三次样条平滑与可视化 if size(path, 1) >= 2 t = [0; cumsum(sqrt(sum(diff(path, 1, 1).^2, 2)))]; tq = linspace(0, t(end), 200)'; xq = spline(t, path(:,1)', tq); yq = spline(t, path(:,2)', tq); zq = spline(t, path(:,3)', tq); smoothPath = [xq, yq, zq]; figure; scatter3(obs(:,1), obs(:,2), obs(:,3), 60, [0.5 0.5 0.5], 'filled'); hold on; plot3(path(:,1), path(:,2), path(:,3), 'o-', 'LineWidth', 1.5); plot3(smoothPath(:,1), smoothPath(:,2), smoothPath(:,3), 'r-', 'LineWidth', 2); plot3(startNode(1), startNode(2), startNode(3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); plot3(goalNode(1), goalNode(2), goalNode(3), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); legend('障碍物', 'Q-learning路径', '三次样条平滑路径', '起点', '终点'); xlabel('X'); ylabel('Y'); zlabel('Z'); grid on; view(3); else warning('路径长度不足，无法做样条平滑。检查训练是否收敛。'); end

4. 调参经验、常见问题与扩展方向

4.1 关键参数建议表

Q-learning加样条平滑这套流程，真正影响结果的主要是下面几个参数。这组数据是我在12×12×8栅格地图上反复测试后得到的经验范围，不是绝对真理，但它能帮你减少很多盲目尝试。

如果发现训练全程没有提升，先别急着调参，先检查奖励是不是太稀疏，或者起终点是不是被障碍物隔断。把地图画出来看一眼，往往比盯着终端发呆有效得多。

4.2 训练和路径提取常见问题速查表

4.3 平滑后撞障碍的三种处理方式

第一次跑通这套代码的人，大概率会遇到“原始折线不撞障碍，三次样条平滑之后反而撞了”的情况。原因在于样条曲线在拐弯处会朝内侧切，弯曲越急，内切越明显。处理办法我总结成三个方向。

简单的方法是碰撞检测加回退。平滑后逐点检查是否落在障碍栅格内，如果有碰撞，就把采样点数减少，让曲线更贴近原始折线，或者把原始路径点加密后再做拟合而不是插值。第二种方法是扩大安全距离。在地图建模阶段，把障碍物栅格做一个膨胀处理，相当于给每个障碍周围加一圈禁行区，这样即使样条有内切，也有缓冲余量。第三种方法则是把平滑问题交给优化来做，不追求三次样条，而是用最小化Snap这类轨迹优化算法，把碰撞约束直接写成优化条件，效果最好，但复杂度也最高。

从我踩坑的经历看，前期调试用第一种方法就够了，正式做方案再考虑第二种。第三种更适合有飞控实验需求的情况。

4.4 后续扩展方向

这套方案本身就非常适合作为算法框架的起点。如果想把Q-learning换成深度强化学习，地图一大、状态一多，Q表就会迅速膨胀，所以更合理的做法是把状态输入直接改成环境栅格图像，用DQN输出动作价值，训练方式基本不变，只是从查表变成查网络。这个迁移路径非常顺，因为前面我已经把状态线性化了，接神经网络只需要把输入层改成栅格特征向量。

轨迹平滑部分也可以从三次样条升级成minimum snap轨迹优化。三次样条只能保证数学上的C²连续，但没有显式考虑无人机动力学约束；minimum snap可以把加速度、速度、推力边界写进优化目标，生成更符合四旋翼飞行特性的轨迹。推荐顺序是：先把本文里的网格版本跑明白，再去做DQN和minimum snap，基础不牢直接碰深度强化学习会很痛苦。

写到这里，我的感受是：Q-learning加三次样条这套组合，现在看并不算炫技，但它的价值恰恰在组合的完整性。算法好懂、代码能跑、可替换性强。我做模拟项目X时最头疼的不是算法本身，而是奖励函数怎么收敛、样条曲线怎么才能不穿墙，这两个问题搞明白，整个流程就顺了。最后再分享一个小习惯：跑完训练之后，别急着提取路径，先随便挑几个状态看看Q表里最大动作对应的方向是否合理。很多时候路径不对，是因为Q表在某个局部状态上被带偏了，肉眼扫一眼Q值分布就能定位问题。如果你也遇到训练掉坑的情况，按上面的排查表一步步来，基本都能救回来。