算法概述
本方案利用Q-learning 离线强化学习，在三维栅格环境下实现无人机的无碰撞、最短路径及最少步数规划。无人机支持 1 格/2 格三维全向移动、对角线飞行及悬停，通过与环境交互迭代学习最优策略，核心目标是在满足约束的前提下输出最优飞行路径。
环境与核心建模
1. 三维状态空间
将飞行空间离散化为三维栅格地图，状态定义为无人机坐标： $$S={(x,y,z) \mid 1 \le x \le X_{max},\ 1 \le y \le Y_{max},\ 1 \le z \le Z_{max}}$$ 其中 $(x,y,z)$ 为空间位置，$X_{max},Y_{max},Z_{max}$ 为地图边界（示例中设为 $10\times10\times10$）。
2. 动作空间
系统共定义54 种动作，涵盖轴向与斜向的 1 格/2 格移动，以及悬停：
- 6 个轴向 1 格移动、12 个平面斜向 1 格移动、8 个空间斜向 1 格移动
- 6 个轴向 2 格移动、12 个平面斜向 2 格移动、8 个空间斜向 2 格移动
- 1 个悬停动作 $(0,0,0)$
动作集合表示为： $$A={ (\Delta x,\Delta y,\Delta z) \mid \Delta x,\Delta y,\Delta z \in {-2,-1,0,1,2},\ \text{非全零} } \cup { (0,0,0) }$$
3. 约束条件
- 边界约束：坐标必须严格位于地图范围内。
- 避障约束：禁止进入障碍物坐标区域。
- 目标约束：从起点 $(2,2,2)$ 到达终点 $(9,9,8)$。
Q-learning 核心数学原理
1. Q 值函数更新
Q 值函数 $Q(s,a)$ 代表状态 $s$ 下执行动作 $a$ 的长期累积奖励期望，是决策的核心依据： $$Q(s,a) \leftarrow Q(s,a) + \alpha \cdot \left[ R(s,a) + \gamma \cdot \max_{a'} Q(s',a') - Q(s,a) \right]$$ 参数说明：
- $\alpha$：学习率，控制更新步长（代码中动态衰减：$0.25 \to 0.05$）。
- $\gamma$：折扣因子，权衡远期奖励（代码中 $\gamma=0.99$）。
- $R(s,a)$：即时奖励。
- $s'$：执行动作后的新状态。
2. 动作选择策略
采用ε-贪心策略配合动态衰减，平衡探索与利用： $$\pi(a|s)= \begin{cases} \arg\max_a Q(s,a) & \text{概率 } 1-\varepsilon \text{（利用最优策略）} \ \text{随机合法动作} & \text{概率 } \varepsilon \text{（探索未知环境）} \end{cases}$$ $\varepsilon$ 随训练轮次指数衰减：$\varepsilon = \max(\varepsilon_{min},\ \varepsilon_0 \cdot e^{-0.007 \cdot \text{episode}})$。
奖励函数 Reward 设计
算法以路径最短、无碰撞、必达终点、步数最少为目标，奖励函数分为四类场景：
1. 数学表达式
$$R(s,a)= \begin{cases} 3500 - 15 \cdot d_{step} - 5 \cdot N_{step} & \text{到达终点}\ -1500 & \text{碰撞障碍物/越界}\ 30 \cdot (d_{cur}-d_{next}) -10 \cdot d_{step} -2 \cdot N_{step} -10 & \text{悬停}\ 30 \cdot (d_{cur}-d_{next}) -10 \cdot d_{step} -2 \cdot N_{step} & \text{正常飞行} \end{cases}$$
2. 设计意图
- 到达终点：给予高额基础奖励（+3500），同时根据单步距离和总步数进行惩罚，强制引导最短路径。
- 碰撞/越界：施加最高惩罚（-1500），确保智能体彻底学会避障。
- 悬停：额外惩罚（-10），避免无效停留，提升效率。
- 正常飞行：靠近终点时给予正向启发式奖励，远离则产生负奖励；飞行距离越长、步数越多，惩罚越高。
算法实现流程
初始化阶段
首先构建三维地图、设定起点终点及障碍物坐标，定义包含 54 种移动加 1 种悬停的动作集。初始化四维 Q 表 $Q(X_{max},Y_{max},Z_{max},A)$ 为零矩阵，并配置超参数如学习率、折扣因子、探索率及训练轮数。
训练迭代
在每一轮训练中，动态衰减学习率和探索率。无人机重置至起点后，循环执行以下步骤：
- 动作剪枝：剔除越界或碰撞障碍物的无效动作。
- 策略选择：依据 ε-贪心策略选取最优或随机动作。
- 状态转移：执行动作获取新状态 $s'$。
- 奖励计算：根据当前位置与目标关系计算即时奖励。
- Q 值更新：应用贝尔曼方程更新 Q 表。
- 终止判断：若到达终点、发生碰撞或超出最大步长则结束当前回合。
路径规划与可视化
训练完成后加载 Q 表，从起点出发每一步选择 Q 值最大的动作，严格校验边界与障碍物，直至抵达终点。最终输出最优路径、总步数及总距离，并绘制奖励收敛曲线、路径距离曲线及三维飞行轨迹图。
关键代码片段
以下是训练结束后计算最终指标及可视化的部分逻辑，展示了如何统计步数、距离并生成对比图表：
% 计算最终结果
final_steps = size(path,1)-1;
final_dist = 0;
for i = 1:final_steps
final_dist = final_dist + norm(path(i+1,:)-path(i,:));
end
fprintf('\n=========================================\n');
fprintf(' 最优步数：%d 步\n', final_steps);
fprintf(' 最短距离：%.4f\n', final_dist);
fprintf(' 避障状态：无碰撞\n');
fprintf('=========================================\n');
%% 绘图展示
figure('Color','w','Position',[80,80,1200,380]);
subplot(1,3,1);
plot(1:max_episode, reward_curve, 'b-','LineWidth',1.6);
xlabel('训练轮数'); ylabel('总奖励'); title('奖励收敛曲线'); grid on;
subplot(1,3,2);
plot(1:max_episode, episode_path_distance, 'r-','LineWidth',1.6);
xlabel('训练轮数'); ylabel('路径总距离'); title('距离收敛曲线'); grid on;
subplot(1,3,3);
hold on; grid on; axis equal; view(3);
xlabel('X'); ylabel('Y'); zlabel('Z');
title(sprintf('最优路径 | 步数：%d | 距离：%.2f | 无碰撞',final_steps,final_dist));
xlim([1 x_max]); ylim([1 y_max]); zlim([1 z_max]);
plot3(start_state(1),start_state(2),start_state(3),'rs','MarkerSize',11,'LineWidth',2);
plot3(end_state(1),end_state(2),end_state(3),'gd','MarkerSize',11,'LineWidth',2);
scatter3(obstacle(:,1),obstacle(:,2),obstacle(:,3),100,'k','filled');
plot3(path(:,1),path(:,2),path(:,3),'m-','LineWidth',2.8,'MarkerSize',5);
legend('起点','终点','障碍物','最优路径','Location','best');
通过上述流程，我们成功实现了在复杂三维环境中无人机的自主路径规划，验证了 Q-learning 算法在处理此类多约束优化问题上的有效性。
