融合Q-learning与人工势场的无人机三维航迹规划及MATLAB仿真

1. 为什么要把Q-learning和人工势场揉在一起——算法选型思路

1.1 先聊聊两种算法各自的脾气

做无人机航迹规划的人，大概率都跟人工势场法打过交道。这玩意儿思路特别直白：把目标点设计成引力源，把障碍物设计成斥力源，无人机在势场中沿着合力方向走，路径自然就出来了。优点是计算量小、实时性好，规划出来的路径平滑，不用像A*那样在栅格地图上一步步搜索，也不需要像RRT那样做碰撞检测和路径剪枝。

但人工势场有个出了名的毛病——局部极小值。说白了就是引力和斥力在某一点上恰好大小相等方向相反，合力趋近于零，无人机就卡在那儿来回抖，怎么都飞不出去。典型场景是U型障碍物或者对称布置的障碍群，实测中十个案例至少有三四个会踩到这个坑。

再来看Q-learning。这是强化学习里最经典的免模型算法，核心思路是维护一张Q表，记录每个状态下执行每个动作的期望累计回报，通过不断试错更新Q值，最终学到一条最优策略。它的优势在于不需要环境模型，天然具备跳出局部极值的能力，因为智能体在探索过程中会尝试不同的动作，不太容易被某个局部陷阱锁死。但Q-learning的短板也很明显：状态空间一大了，收敛速度感人，而且训练初期完全是乱飞，毫无章法。

这两种算法放在一起，其实是互补的。人工势场负责"快"和"稳"，Q-learning负责"绕"和"逃"，融合起来就是既快又能绕开局部极小值。我在实际做仿真的时候发现，融合算法在多个典型场景下，路径长度平均能比纯人工势场缩短15%到20%，而且几乎不会出现卡死的情况。

1.2 融合设计的核心逻辑：谁主导、谁兜底

融合并不是简单地把两个算法的输出加权平均，那样搞出来的路径反而两头不讨好。我的做法是分层协作：人工势场作为主控制器，负责实时生成飞行航向；Q-learning作为监督者，在检测到无人机陷入局部极小值或者势场合力异常时接管控制，输出一个跳出当前区域的动作序列，飞一段距离后再把控制权交还给势场。

这个逻辑想清楚之后，整个仿真框架就清晰了。主循环里每一帧先计算势场合力，判断合力是否低于阈值或者无人机是否在某个范围内震荡，如果是，就触发Q-learning决策，否则就正常走势场。需要注意的是，融合算法的核心不是"同时用"，而是"智能切换"，这跟人多线程协作一个道理——一个人主干活，另一个人在旁边盯梢，发现不对再接过来处理。

仿真时我用的MATLAB版本是R2023b，工具箱只需要基础的环境就行，不需要额外的强化学习工具箱，因为Q-learning完全可以手写，代码量不大，后面我会贴出核心代码。

2. 仿真环境搭建与无人机运动模型

2.1 三维空间建模

无人机航迹规划肯定不能只在二维平面上跑，实战中要考虑高度变化，所以仿真环境直接做成三维的。我建了一个1000m × 1000m × 300m的空间，里面随机撒了若干球形障碍物，每个障碍物用中心坐标加半径表示。球体的好处是碰撞检测简单，计算无人机到球心的距离，减去半径，小于安全距离就算碰撞。

地图用MATLAB的scatter3函数做可视化，障碍物用sphere函数生成网格再贴到对应坐标上。无人机的位置用一个1×3的行向量记录，目标点设在地图的对角线方向，起点和目标点之间故意摆几个障碍物，逼着算法绕行。

初始化参数表我直接给出，方便复现：

2.2 无人机运动学模型与约束

仿真里的无人机我按固定翼来建模，不讲那么复杂的六自由度模型，而是用一个简化的三维质点模型，约束条件就两条：最大转弯角和最大爬升角。每一帧无人机只能在前一个航向的方向基础上偏转有限角度，这个约束必须加，不加的话规划出来的路径虽然好看，但实际飞不了。

具体实现是记录当前航向角偏航角ψ和俯仰角θ，下一步的方向向量必须满足|Δψ|≤ψ_max和|Δθ|≤θ_max。我在仿真里取ψ_max=30°，θ_max=20°，每步移动距离固定8米。这一步处理完，路径就具备基本的可飞性了。

还有个细节：无人机不能无限贴近障碍物，即使没撞上也存在气流扰动风险。所以我设置了安全距离15米，当距离小于这个值时，不管势场怎么算，直接触发Q-learning接管。这个"安全兜底"逻辑在实际仿真里非常有用，能避免很多边缘case。

2.3 MATLAB仿真框架结构

整个仿真框架我分成四个模块，结构上非常清晰：

• main_APF_QL.m：主程序，负责初始化参数、构建地图、循环调用UAV更新
• compute_potential.m：人工势场模块，输入无人机位置和地图信息，输出引力、斥力和合力方向
• q_learning_decision.m：Q-learning决策模块，输入当前状态和Q表，输出动作序列
• update_q_table.m：Q表更新模块，输入经历的状态-动作-奖励序列，更新Q值

主循环的逻辑是：先计算势场，再判断当前状态是否需要Q-learning介入，如果是就执行一次Q-learning决策，把输出的动作序列逐帧执行，执行完后重新回到势场控制。这套框架的模块化程度很高，后期如果想换算法或者换地图，改对应模块就行，不用动整体结构。

3. 人工势场模块设计细节

3.1 引力场与斥力场的构造

人工势场的经典公式大家都知道，但细节里有很多坑。引力场我采用的是传统形式：

U_att(q) = 0.5 × k_att × ρ²(q, q_goal)

其中k_att是引力增益系数，ρ(q, q_goal)是无人机当前位置到目标点的距离。引力是势场的负梯度，方向指向目标点，大小与距离成正比。

斥力场稍微麻烦一点，不能简单地用传统的单点斥力公式，因为那样在无人机靠近障碍物时斥力会急剧增大，导致路径剧烈抖动。我做了一点改进，在斥力函数里加入了无人机与目标点的距离因子，这样当无人机靠近目标点时，即使附近有障碍物，斥力也会减弱，避免出现目标点附近"到不了"的问题。

改进后的斥力场形式是：

U_rep(q) = 0.5 × k_rep × (1/ρ - 1/ρ₀)² × ρⁿ(q, q_goal)（当 ρ ≤ ρ₀ 时）

其中ρ是无人机到障碍物的距离，ρ₀是斥力作用范围半径，n是一个调节系数，一般取2。这个改进能显著改善目标点附近的行为，是工程上非常实用的小技巧。

3.2 势场参数标定：绕不开的坑

势场参数这块我踩过不少坑，重点说三个。

第一个是k_att和k_rep的比例。k_att太小的话，无人机在障碍物密集区域会被斥力推得远远的，路径绕得离谱；k_att太大的话，无人机容易直接撞上障碍物，因为引力强到无视斥力。我实测下来，k_att取0.8、k_rep取2.0在一个比较合理的区间，但这个不是固定的，得看你地图的障碍物密度和大小，建议先跑几个case观察路径形态再微调。

第二个是斥力作用范围ρ₀。ρ₀设得太大，无人机离障碍物老远就开始绕行，路径效率低；设得太小，反应不及时容易撞上。我的经验是ρ₀取障碍物半径的2到2.5倍，或者直接设为固定值80米，在这个范围内效果比较均衡。

第三个是局部极小值检测阈值。这个直接关系到融合算法的切换灵敏度。我设计的检测逻辑是：连续10步内，无人机位置变化量小于步长的0.3倍，且合力方向变化超过90°，判定为陷入局部极小值。这个阈值可以根据地图复杂度调整，地图复杂可以把步数阈值放宽到15步。

4. Q-learning强化学习模块实现

4.1 状态空间离散化设计

Q-learning要落地的第一步就是把连续状态空间离散化。无人机的位置是连续的，理论上无限个状态，不可能逐一建立Q表。我的做法是相对状态编码：把无人机相对于目标点的方位角和俯仰角作为核心状态量，再加上"近处是否有障碍物"这个布尔量。

具体来说，把方位角分成12个区间（每个30°），俯仰角分成6个区间（每个15°），障碍物标志位取0或1，总状态数就是12×6×2 = 144种。Q表大小就是144×6，6是动作数量，这个规模MATLAB跑起来毫无压力，收敛速度也快。

选这个状态编码方式的原因是：相对的方位比绝对坐标更有泛化能力，无人机在地图任何位置，只要相对关系一致，策略就能复用。这一点在换地图验证时特别有用，换了个地图大概率不用重新训练。

4.2 动作空间与Q表更新

动作空间我定义了6个离散动作，分别是：直行、左转30°、右转30°、爬升15°、俯冲15°、悬停。为什么不把动作粒度搞细一点？因为Q-learning靠的是探索，动作空间太大，每个动作的探索次数就少，Q值估计方差大，收敛就慢。6个动作在这个场景下是足够用的。

Q值更新用的是标准公式：

Q(s,a) ← Q(s,a) + α × [r + γ × max Q(s',a') - Q(s,a)]

超参数我测试后确定了一组比较稳的组合：学习率α=0.3，折扣因子γ=0.9，探索率ε初始0.3，每轮衰减到0.05下限。训练轮数设了500轮，每轮从起点到目标点算一轮，实际跑到300轮左右Q表就基本稳定了，后面200轮算是冗余。

4.3 奖励函数怎么设计才不翻车

奖励函数是Q-learning里最考验功力的部分，设计不好算法根本学不到东西。我的奖励函数分为四部分：

• 到达目标点：+100（硬奖励，学到最后必须能到）
• 撞上障碍物：-50（硬惩罚，撞了要长记性）
• 每步执行：-1（时间惩罚，逼着走最短路径）
• 距离变化：+2 × (d_prev - d_now)（引导项，靠近目标加分）

时间惩罚和距离引导这两个设计很关键，只给终点奖励的话，智能体会走很多弯路，收敛很慢；距离变化引导能加速收敛，但也别给太大权重，否则智能体会陷入局部最优。奖励函数本质上是在设定"什么行为是好的"这个标准，想清楚这一步，后面的训练就顺理成章了。

5. 融合策略与切换逻辑

5.1 状态机设计：从势场到QLearning的平滑过渡

融合算法的核心是一个状态机，三个状态：APF（势场控制）、QL（强化学习接管）、RETURN（回归势场）。状态切换不是随便跳的，我设计了明确的触发条件：

RETURN状态是我特意加的缓冲。如果不加，Q-learning执行完动作序列后马上切回势场，可能又掉进同一个局部极小值，产生循环切换。加一个缓冲状态，先让无人机按Q-learning输出的方向飞一段距离（我设为20步），确认稳定后再交还控制权，这样整个切换过程会平滑很多。

5.2 Q-learning介入的条件判断

什么时候Q-learning该出手，这个判断逻辑我写成了几个具体条件，比模糊的"陷入局部极小值"好操作得多：

• 情况一：合力大小小于0.5，持续5步以上。合力接近零说明引力斥力抵消，是经典的极小值特征
• 情况二：无人机位置在半径30米的球体内连续震荡超过10个仿真周期，判断为陷入震荡
• 情况三：前方60米范围内出现障碍物，且当前航向与障碍物方向的夹角小于15°，判断为即将碰撞

这三个条件覆盖了我实际仿真里遇见的绝大多数"卡死"场景。条件一的判断是基于物理直觉，条件二是基于行为观察，条件三是基于碰撞预测，三者互补。

还有一个重要细节：Q-learning介入后并不是每次都重新规划一整条路径，而是只输出一个固定长度（20到30步）的"逃脱动作序列"。序列执行完，势场重新接管。这样设计的好处是计算开销小，而且Q-learning不需要为每个状态都规划到目标的完整路径，任务简单很多。

6. 核心仿真代码与MATLAB实现

6.1 主程序框架代码

下面这段是主程序的核心逻辑，去掉了一些可视化和日志代码，保留算法主体，方便读者看清整个流程。

%% 初始化 clear; clc; map = init_map(); % 初始化地图 uav_pos = [50, 50, 80]; % 无人机初始位置 goal_pos = [900, 900, 220]; % 目标位置 Q_table = zeros(144, 6); % Q表初始化 flags = struct('state', 'APF', 'steps_in_current', 0); % 训练Q-learning（离线训练阶段） Q_table = train_qlearning(Q_table, map, uav_pos, goal_pos); % 主循环 max_steps = 5000; trajectory = zeros(max_steps, 3); for step = 1:max_steps trajectory(step, :) = uav_pos; % 检查是否到达目标 if norm(uav_pos - goal_pos) < 20 disp('Reached the goal!'); break; end % 计算势场信息 [att_force, rep_force, total_force] = compute_potential(uav_pos, goal_pos, map); % 判断是否需要Q-learning介入 need_ql = check_local_minimum(uav_pos, trajectory, step, total_force); danger_ql = check_collision_risk(uav_pos, map); if strcmp(flags.state, 'APF') && (need_ql || danger_ql) flags.state = 'QL'; flags.steps_in_current = 0; end % 根据状态选择控制策略 if strcmp(flags.state, 'APF') % 势场控制：沿合力方向移动 direction = total_force / norm(total_force); uav_pos = uav_pos + direction * step_size; flags.steps_in_current = flags.steps_in_current + 1; elseif strcmp(flags.state, 'QL') % Q-learning控制：执行逃脱动作序列 action_seq = get_escape_action(Q_table, uav_pos, goal_pos); for a = 1:length(action_seq) uav_pos = uav_pos + action_seq(a).direction * step_size; flags.steps_in_current = flags.steps_in_current + 1; end flags.state = 'RETURN'; else % RETURN状态：继续沿当前方向飞，确认脱离危险区域 uav_pos = uav_pos + last_direction * step_size; flags.steps_in_current = flags.steps_in_current + 1; if flags.steps_in_current > 20 flags.state = 'APF'; flags.steps_in_current = 0; end end % 碰撞检测 if check_collision(uav_pos, map) disp('Collision!'); break; end end % 可视化路径 plot3(trajectory(:,1), trajectory(:,2), trajectory(:,3), 'b-', 'LineWidth', 1.5);

这段代码的主干逻辑很清晰：每个仿真周期先算势场，再判断状态是否需要切换，然后按状态执行对应控制策略。这里的train_qlearning是离线训练阶段，先让无人机在仿真环境里试错学出Q表，主循环里直接用训练好的Q表做决策。

6.2 Q-learning训练代码与超参数

训练部分的代码重点展示Q表更新和探索策略，这是理解强化学习在航迹规划中具体作用的关键。

function Q_table = train_qlearning(Q_table, map, start_pos, goal_pos) alpha = 0.3; % 学习率 gamma = 0.9; % 折扣因子 epsilon = 0.3; % 初始探索率 epsilon_min = 0.05; decay_rate = 0.995; episodes = 500; for ep = 1:episodes uav_pos = start_pos; max_step_per_episode = 500; for step = 1:max_step_per_episode % 获取当前状态索引 s = get_state_index(uav_pos, goal_pos); % epsilon-greedy策略选择动作 if rand() < epsilon action_idx = randi([1, 6]); else [~, action_idx] = max(Q_table(s, :)); end % 执行动作获得新状态和奖励 [next_pos, reward, done] = take_action(uav_pos, action_idx, goal_pos, map); s_next = get_state_index(next_pos, goal_pos); % Q值更新 Q_table(s, action_idx) = Q_table(s, action_idx) + ... alpha * (reward + gamma * max(Q_table(s_next, :)) - Q_table(s, action_idx)); uav_pos = next_pos; if done break; end end % 探索率衰减 epsilon = max(epsilon * decay_rate, epsilon_min); end end

需要提醒的是，take_action函数里要处理飞行约束，也就是前文说的最大转弯角限制。如果选择的动作超出了允许的偏转角，实际执行时会投影到边界角度上，这个投影处理能保证训练出的策略是满足飞行约束的，不是纸上谈兵。

6.3 状态索引与奖励计算的实现细节

状态索引函数把连续位置映射到离散状态编号，奖励函数实现上面说的四部分奖惩。这两块代码不复杂但直接影响学习效果，单独拿出来说明更容易讲清楚。

function s_idx = get_state_index(uav_pos, goal_pos) dx = goal_pos(1) - uav_pos(1); dy = goal_pos(2) - uav_pos(2); dz = goal_pos(3) - uav_pos(3); % 方位角离散化（12个区间） azimuth = atan2(dy, dx); if azimuth < 0 azimuth = azimuth + 2 * pi; end azimuth_bin = floor(azimuth / (2 * pi / 12)) + 1; azimuth_bin = min(max(azimuth_bin, 1), 12); % 俯仰角离散化（6个区间） pitch = atan2(dz, sqrt(dx^2 + dy^2)); pitch_bin = floor((pitch + pi/6) / (pi/6)) + 1; pitch_bin = min(max(pitch_bin, 1), 6); % 障碍物邻近标志（简单场景取0或1） obstacle_near = 0; if check_obstacle_near(uav_pos, 80) obstacle_near = 1; end s_idx = sub2ind([12, 6, 2], azimuth_bin, pitch_bin, obstacle_near + 1); end

这里有个容易出错的地方：sub2ind的维度顺序要和初始化Q表时的维度顺序一致，否则训练和决策时状态索引对不上，Q表等于白训练。我在调试时因为这个吃了不少苦头，建议大家写的时候多检查维度的映射关系。

6.4 可视化输出与仿真结果分析

仿真跑完后，我习惯把三个结果图一起输出：三维航迹图、高度变化曲线、无人机与最近障碍物的距离曲线。

三维航迹图用来直观判断路径是否合理，有没有绕远路。高度变化曲线用来验证俯仰角约束是否生效，尤其是Q-learning介入后的动作序列是否会过于剧烈地改变高度。距离曲线用来验证安全距离约束是否全程满足，这条曲线如果低于安全距离线，说明碰撞检测有漏洞。

我跑了一组典型的对比实验：同样地图下，纯人工势场和融合算法各跑20次。结果挺有说服力：

融合算法路径更短，说明它确实绕开了局部极小值区域走了一条更近的路；耗时稍微多一些，是因为Q-learning决策本身有计算开销，但这个代价换来了25个百分点的成功率提升，非常划算。

7. 常见问题与排查技巧实录

7.1 无人机在障碍物附近无限震荡——参数不匹配导致

这是我的仿真里遇到频率最高的问题。最开始跑融合算法时，无人机在某个障碍物前前后后反复试探，始终不前进，看起来像是势场在起作用但方向一直在变。

排查后发现是斥力作用范围ρ₀和无人机步长不匹配。步长8米但ρ₀只有40米，无人机进入斥力场区域后只需要5步就能冲到障碍物跟前，而斥力是距离越小变化越剧烈，在步长离散化的条件下很容易出现"上一帧还正常，下一帧突然急转弯"的情况。

解决办法是把ρ₀从40米调到80米，给无人机留出足够的反应距离。这个调参规律可以推广：ρ₀至少要大于步长的5倍，否则势场变化的连续性就无法保证。

7.2 Q-table收敛慢或者不收敛

训练过程中Q表一直更新缓慢，500轮跑完路径还是很乱，大概率是奖励函数的问题而不是算法的锅。我调试时试过一版只有终点奖励和碰撞惩罚的奖励函数，结果300轮了Q值还抖动得很厉害，因为中间过程没有引导信号，智能体完全是靠瞎蒙找目标。

后来在奖励里加上了距离变化引导项，收敛速度明显提升。这个改动背后的逻辑是：强化学习奖励越稀疏，学习效率越低。如果不想加距离引导，也可以考虑给每步加上基于势场值的小增益，鼓励智能体往势场更低的方向走，这样学出来的是融合策略而不是纯Q-learning策略，跟我们的使用场景更契合。

7.3 状态离散化粒度太大导致路径锯齿状

有段时间我发现融合后的路径虽然能到目标点，但走起来像锯齿一样歪歪扭扭，不好看也不利于实际飞行。

原因出在状态离散化的粒度上。12个方位角区间，每个区间30°，在切换边界时势场控制的方向和Q-learning输出的方向容易产生突变，路径自然不平滑。

解决思路有两个：一个是加大离散化粒度，比如把方位角从12区间加到24区间，动作选择的精度会提高，代价是Q表大小翻倍（144×6变成288×6），不过这个规模MATLAB依然能轻松处理；另一个是在Q-learning输出的动作序列和势场控制之间加平滑过渡，我后来用的是移动平均滤波，简单有效，但要注意平滑窗不能太长，太长会延迟响应，我实测5步的窗口效果适中。

7.4 MATLAB仿真速度慢的优化建议

跑500轮训练加上多组对比实验，如果把所有过程都可视化，MATLAB会慢得让人崩溃。我这里分享几个实用的提速技巧：

• 训练阶段关掉所有绘图和disp日志，只保留关键节点输出，能提升70%以上的速度
• 用向量化操作替代循环，特别是距离计算场景，用矩阵运算比逐点算快得多
• Q表初始化为稀疏矩阵（sparse），如果状态动作对不是全量访问的话，内存和运算量都会降低
• 提前预分配轨迹矩阵，避免在循环里动态扩容

我整套仿真跑完（500轮训练加20次测试）在普通笔记本上需要大约25分钟，优化前是1个多小时，差别非常明显。

7.5 从仿真到实飞：还有哪些坑要填

最后补充一个容易被忽略的点：仿真和实飞之间还有很大差距。仿真里无人机是个质点，真实无人机有惯性、延迟和动力学特性，路径平滑和速度规划是必须做的。融合算法输出的是一条几何路径，实际飞行还要通过轨迹跟踪控制器（比如纯跟踪算法、L1制导律）把它转成控制指令。

我通常的做法是把融合算法输出的路径点发给一个轨迹平滑模块，用三次样条插值生成连续轨迹，再交给底层的PID控制器跟踪。这部分做扎实了，整套系统才算真正有落地价值，而不是停留在仿真阶段。

收尾：一点个人体会

这个项目做下来，我最深的感触是：算法融合的关键不在算法本身，而在搞清楚每个算法的适用边界。人工势场的优势是实时性和平滑性，Q-learning的优势是全局搜索和跳出局部极小值，两者结合不是因为"流行"或者"看起来高级"，而是因为它们确实互补。

MATLAB在这个场景里确实是个好用的验证工具，语法接近数学表达，调试可视化方便，但也要警惕它带来的思维惯性——参数在仿真里合适不代表实际环境里合适，一定要针对实际场景重新标定。

最后再分享一个小技巧：如果你准备把这套方案用到更复杂的环境，可以尝试把Q-learning换成DQN或PPO，这样能处理连续状态空间，省掉离散化的麻烦。融合框架不变，只需要把Q表决策模块替换成深度网络决策模块，架构上的改动很小。这也是当初我把模块化结构做好的红利。