（MATLAB代码）退火PSO融合DWA算法的船舶路径规划MATLAB实现（260915）

1. 引言

粒子群优化算法（Particle Swarm Optimization，PSO）是一种模拟鸟群觅食行为的群体智能优化算法，因其参数少、收敛速度快、实现简单等优点，被广泛应用于路径规划领域。然而，标准PSO算法容易陷入局部最优，且规划出的全局路径无法应对动态障碍物。为此，本文引入模拟退火机制改进PSO算法（退火PSO），提升全局搜索能力，并融合动态窗口法（Dynamic Window Approach，DWA）进行局部避障，实现全局最优与局部动态避障相结合的船舶路径规划方法。

2. 代码创新点

本文代码的核心创新点主要体现在以下三个方面：

退火机制与PSO的深度融合：在标准PSO迭代过程中引入模拟退火策略，以一定概率接受劣质解，有效避免算法陷入局部最优，显著提升全局路径搜索能力，同时保持PSO参数少、收敛快的优势。
全局规划与局部避障的双层融合架构：采用"退火PSO全局路径规划 + DWA局部动态避障"的分层策略，退火PSO负责在静态栅格地图上搜索全局最优路径，DWA负责在船舶沿全局路径行驶时实时规避动态障碍物，兼顾全局最优性与局部安全性。
动态障碍物交互式仿真验证：程序支持通过鼠标交互设置移动障碍物的起点和终点，模拟真实动态航行环境，直观验证DWA局部避障算法在动态场景下的有效性，使整套系统更贴近实际船舶航行需求。

2. 地图建模

路径规划的第一步是建立环境地图。本文采用栅格法对船舶航行环境进行建模，将地图划分为若干正方形网格，每个网格代表一个位置单元。其中，值为1的网格表示障碍物区域，值为0的网格表示可通行区域。

需要注意的是，地图矩阵必须设置为正方形矩阵，即行数和列数相等，否则在后续旋转操作时会出现错误。修改地图时，只需修改MAP矩阵中的数值即可；起点和终点需要在static变量中同步设置。

matlab 复制代码

MAX=MAP();  % 加载地图数据
MAX_X=size(MAX,2);  % 获取列数，即x轴长度
MAX_Y=size(MAX,1);  % 获取行数，即y轴长度

3. 障碍物初始化

在完成地图建模后，需要遍历地图矩阵，将所有障碍物网格标记出来。本文使用黑色方块表示障碍物，并将障碍物的坐标保存到关闭列表CLOSED中，供后续路径搜索算法使用。

matlab 复制代码

n=0;  % 障碍物数量
k=1;
CLOSED=[];  % 关闭列表，存放所有障碍物坐标
for j=1:MAX_X
    for i=1:MAX_Y
        if (MAX(i,j)==1)
            fill([i,i+1,i+1,i],[j,j,j+1,j+1],'k');  % 用黑色方块表示障碍物
            hold on;
            CLOSED(k,1)=i;  % 保存障碍点x坐标
            CLOSED(k,2)=j;  % 保存障碍点y坐标
            k=k+1;
        end
    end
end

4. 设置起始点和目标点

本文采用鼠标交互方式设置起始点和目标点。程序运行后，用户通过鼠标左键在地图上依次点击目标点和起始点，程序会自动获取对应的网格坐标。目标点用绿色圆圈标记，起始点用蓝色三角标记。

matlab 复制代码

% 选择目标点
h=msgbox('请使用鼠标左键选择目标');
uiwait(h,5);
if ishandle(h) == 1
    delete(h);
end
but=0;
while (but ~= 1)
    [xval,yval,but]=ginput(1);
end
xval=floor(xval);
yval=floor(yval);
xTarget=xval;
yTarget=yval;
plot(xval+.5,yval+.5,'go');  % 目标点用绿色圆圈标记
text(xval+1,yval+1,'Target')

5. 退火PSO全局路径规划

在完成地图建模和起止点设置后，调用退火PSO算法进行全局路径搜索。退火PSO在标准PSO的基础上引入模拟退火机制，以一定概率接受劣质解，从而有效跳出局部最优，增强全局搜索能力。算法返回规划得到的全局路径点序列以及路径总长度。

matlab 复制代码

Start=[xStart yStart];
Goal=[xTarget yTarget];
% 退火PSO全局路径规划
[Global_path,global_dist]=SA_PSO(Start, Goal);
global xxone yyone
plot(xxone+0.5,yyone+0.5,'b:','LineWidth',1);
hold on;

6. DWA局部动态避障

退火PSO规划出的全局路径无法应对环境中突然出现的动态障碍物。为此，本文融合动态窗口法（DWA）进行局部路径规划。DWA算法在速度空间中进行采样，生成多组可行的速度组合，并通过评价函数筛选出最优轨迹，实现实时避障。船舶沿全局路径行驶时，DWA根据当前环境动态调整航向和速度，确保安全通过动态障碍区域。

matlab 复制代码

% DWA局部避障参数设置
max_speed = 1.0;      % 最大线速度
max_omega = pi/3;     % 最大角速度
dt = 0.1;             % 采样时间
% 沿全局路径执行DWA局部规划
for i = 1:size(Global_path,1)
    target = Global_path(i,:);
    [v, w] = DWA(current_state, target, obstacle_list, max_speed, max_omega, dt);
    % 更新船舶状态并绘制轨迹
    current_state = update_state(current_state, v, w, dt);
    plot(current_state(1)+0.5, current_state(2)+0.5, 'r.', 'MarkerSize', 6);
    hold on;
end

7. 移动障碍物设置

为了模拟动态环境，程序还支持设置移动障碍物的起点和终点。用户通过鼠标交互指定移动障碍物的运动轨迹，使路径规划更贴近真实船舶航行场景，同时验证DWA局部避障算法的有效性。

matlab 复制代码

h=msgbox('设置移动障碍物的 起点');
hold on;
uiwait(h,5);
if ishandle(h) == 1
    delete(h);
end
but=0;
while (but ~= 1)
    [xval,yval,but]=ginput(1);
end
xval=floor(xval);
yval=floor(yval);
Obst_xS=xval;
Obst_yS=yval;
plot(xval+.5,yval+.5,'k^');  % 移动障碍物起点用黑色三角标记

8. 总结

本文实现了一种基于退火PSO融合DWA算法的船舶路径规划方法。退火PSO算法负责全局路径搜索，通过模拟退火机制有效避免陷入局部最优；DWA算法负责局部动态避障，实时应对环境中的动态障碍物。两者融合兼顾了全局最优性与局部安全性，构建了一套完整的船舶路径规划演示系统，适合作为路径规划算法的进阶学习和二次开发基础。

8.1运行线速度

8.2角速度

8.3航向变化（可根据角度转换为0-360°航向）

8.4规划路径

9.代码获取

（MATLAB代码）退火PSO融合DWA算法的船舶路径规划MATLAB实现（260915）https://mbd.pub/o/bread/YZaVlZpuaw==