做三维无人机路径规划仿真最常遇到的组合就是“全局搜索算法轨迹平滑”。我前阵子给一个模拟项目搭航迹规划模块白天理顺栅格地图晚上就想着怎么把强化学习塞进去翻来覆去试了几轮最后落地的一套方案就是基于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每个状态可选动作集合AQ(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(end1, :) [x, y, z]; end end end for x 9:10 for y 3:4 for z 3:6 obs(end1, :) [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(end1, :) 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(end1, :) [x, y, z]; end end end for x 9:10 for y 3:4 for z 3:6 obs(end1, :) [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(end1, :) 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(路径长度不足无法做样条平滑。检查训练是否收敛。); end4. 调参经验、常见问题与扩展方向4.1 关键参数建议表Q-learning加样条平滑这套流程真正影响结果的主要是下面几个参数。这组数据是我在12×12×8栅格地图上反复测试后得到的经验范围不是绝对真理但它能帮你减少很多盲目尝试。参数建议范围调试方向学习率 alpha0.1 ~ 0.5太小收敛慢太大会震荡折扣因子 gamma0.85 ~ 0.99越大越看重长远回报epsilon 初始值0.8 ~ 1.0前期不探索策略学不出来epsilon 衰减速率0.99 ~ 0.999衰减太快会过早进入纯利用阶段靠近目标奖励权重0.3 ~ 0.8权重太大会擦墙走训练回合数200 ~ 800地图越大需要回合越多如果发现训练全程没有提升先别急着调参先检查奖励是不是太稀疏或者起终点是不是被障碍物隔断。把地图画出来看一眼往往比盯着终端发呆有效得多。4.2 训练和路径提取常见问题速查表现象可能原因解决方式训练很多回合路径仍然绕圈步数惩罚不够或epsilon衰减太快增大每步惩罚绝对值降低衰减速度路径贴着障碍物走靠近目标奖励权重过大权重降到0.3附近或把碰撞惩罚加大到-200Q表陷入局部最优始终找不到终点起终点被障碍物隔断或奖励太稀疏检查地图连通性给起点附近增加探索奖励三次样条平滑后路径穿墙原始路径点间距离不均匀样条过冲参数t用累积长度降低采样点数或改用csape拟合平滑后曲线出现尖刺某个轴插值时原始点重叠检查path是否有重复坐标重复点去重4.3 平滑后撞障碍的三种处理方式第一次跑通这套代码的人大概率会遇到“原始折线不撞障碍三次样条平滑之后反而撞了”的情况。原因在于样条曲线在拐弯处会朝内侧切弯曲越急内切越明显。处理办法我总结成三个方向。简单的方法是碰撞检测加回退。平滑后逐点检查是否落在障碍栅格内如果有碰撞就把采样点数减少让曲线更贴近原始折线或者把原始路径点加密后再做拟合而不是插值。第二种方法是扩大安全距离。在地图建模阶段把障碍物栅格做一个膨胀处理相当于给每个障碍周围加一圈禁行区这样即使样条有内切也有缓冲余量。第三种方法则是把平滑问题交给优化来做不追求三次样条而是用最小化Snap这类轨迹优化算法把碰撞约束直接写成优化条件效果最好但复杂度也最高。从我踩坑的经历看前期调试用第一种方法就够了正式做方案再考虑第二种。第三种更适合有飞控实验需求的情况。4.4 后续扩展方向这套方案本身就非常适合作为算法框架的起点。如果想把Q-learning换成深度强化学习地图一大、状态一多Q表就会迅速膨胀所以更合理的做法是把状态输入直接改成环境栅格图像用DQN输出动作价值训练方式基本不变只是从查表变成查网络。这个迁移路径非常顺因为前面我已经把状态线性化了接神经网络只需要把输入层改成栅格特征向量。轨迹平滑部分也可以从三次样条升级成minimum snap轨迹优化。三次样条只能保证数学上的C²连续但没有显式考虑无人机动力学约束minimum snap可以把加速度、速度、推力边界写进优化目标生成更符合四旋翼飞行特性的轨迹。推荐顺序是先把本文里的网格版本跑明白再去做DQN和minimum snap基础不牢直接碰深度强化学习会很痛苦。写到这里我的感受是Q-learning加三次样条这套组合现在看并不算炫技但它的价值恰恰在组合的完整性。算法好懂、代码能跑、可替换性强。我做模拟项目X时最头疼的不是算法本身而是奖励函数怎么收敛、样条曲线怎么才能不穿墙这两个问题搞明白整个流程就顺了。最后再分享一个小习惯跑完训练之后别急着提取路径先随便挑几个状态看看Q表里最大动作对应的方向是否合理。很多时候路径不对是因为Q表在某个局部状态上被带偏了肉眼扫一眼Q值分布就能定位问题。如果你也遇到训练掉坑的情况按上面的排查表一步步来基本都能救回来。