模拟退火-粒子群全局路径规划+DWA局部路径规划

整理了一个路径规划demo，当然图是改进的效果 demo分别有对应的开源 可以在网上搜到，我觉得已经介绍的很详细了，所以不做过多的解释，传送门在下面

（写的不好 轻喷）

粒子群算法

粒子群本质是参数寻优问题，也就是说在运用到路径规划这块需要对规划的路径进行模型建立，这块的demo当时是从一个b站up那块了解的，我记得好像有个up做了这个的讲解 但是我没找到QAQ

传送门

b站up的链接：

粒子群算法，路径规划，星际穿越_哔哩哔哩_bilibili

开源的粒子群路径规划demo链接（要感谢上面up的分享）：

Optimal Robot Path Planning using PSO in MATLAB - Yarpiz

（多说一嘴：咱就是说要有点学术态度吧，别直接照搬照抄一点改进都没有，直接拿来用就发论文，看到了真感觉很恶心 说的是这篇论文：Verification and Analysis of Two-dimensional Path Planning Objective Function Optimization Based on Classical Particle Swarm Optimization Algorithm）

话说回来，对路径建立数学模型（适应度函数），那么首要的任务是确定要量化哪些信息，demo写在了MyCost.m里面 包含两部分：1.路径长度 2.碰撞惩罚 

在确定了适应度函数之后，就可以用粒子群算法进行寻优了，寻优过程在pso.m中进行。

在初始化时需要确定粒子群的维度n，这个维度可以理解为路径中间节点的个数，节点越多需要迭代的次数也就越多，生成最优路径的可能性也就越大。

%% Problem Definition

model=CreateModel(); %环境初始化 
%@(x) 变量名 = @（输入参数列表）运算表达式 既是一种可用于传参和赋值的变量，又可以作为函数名使用。
%这个x是  第x个粒子群
CostFunction=@(x) MyCost(x,model);  % CostFunction 适应度函数 做了一个声明
nVar=model.n;       % Number of Decision Variables  决策变量数量 10个位置点
VarSize=[1 nVar];   %[1 10] % Size of Decision Variables Matrix  决策变量矩阵的大小

%设置边界
VarMin.x=model.xmin;          
VarMax.x=model.xmax;           
VarMin.y=model.ymin;           
VarMax.y=model.ymax;          

%% PSO Parameters

%迭代次数
MaxIt=500;          % Maximum Number of Iterations  500
%种群规模
nPop=150;           % Population Size (Swarm Size)

%粒子群的三个系数 w为惯性权重系数 c1、c2分别为向个体和群体最优飞行速度
%wdamp的设置是为了降低惯性，提升后期局部搜索能力
w=1;                % Inertia Weight
wdamp=0.98;         % Inertia Weight Damping Ratio
c1=1.5;             % Personal Learning Coefficient
c2=1.5;             % Global Learning Coefficient

%定义速度可行域
alpha=0.1;
VelMax.x=alpha*(VarMax.x-VarMin.x);    % Maximum Velocity
VelMin.x=-VelMax.x;                    % Minimum Velocity
VelMax.y=alpha*(VarMax.y-VarMin.y);    % Maximum Velocity
VelMin.y=-VelMax.y;                    % Minimum Velocity

%% Initialization 种群初始化

% Create Empty Particle Structure
empty_particle.Position=[];
empty_particle.Velocity=[];
empty_particle.Cost=[];
empty_particle.Sol=[];
%结构体里的结构体
empty_particle.Best.Position=[];
empty_particle.Best.Cost=[];
empty_particle.Best.Sol=[];

% Initialize Global Best 因为是找最优（短）距离，所以在一开始置为无穷大
GlobalBest.Cost=inf;
% Create Particles Matrix  创建粒子矩阵 维度（nPop,1）
particle=repmat(empty_particle,nPop,1);

% Initialization Loop
for i=1:nPop
    
    %i = 1的时候 等距离生成10个点 后面的就随机生成
    
    % Initialize Position
    if i > 1  %安排后面那些位置信息
        particle(i).Position=CreateRandomSolution(model);
    else 
        % Straight line from source to destination  从源到目标的直线
        %linspace ==> 在初始点目标点之间等距离生成 n+2(12个点包括初始点和目标点)
        xx = linspace(model.xs, model.xt, model.n+2);  
        yy = linspace(model.ys, model.yt, model.n+2);
        
        %把10个点的信息传给particle
        particle(i).Position.x = xx(2:end-1);
        particle(i).Position.y = yy(2:end-1);
    end
    
    % Initialize Velocity 速度赋0 VarSize=[1,10] zeros(VarSize) ==> zeros(1,10)
    % 相当于返回一个1X10的全零矩阵
    particle(i).Velocity.x=zeros(VarSize);
    particle(i).Velocity.y=zeros(VarSize);
    
    %Evaluation返回路径长度 Cost 和 Sol（拟合点 距离 容忍度 可靠性）
    [particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position);
    
    % Update Personal Best  更新个体数据
    particle(i).Best.Position=particle(i).Position;
    particle(i).Best.Cost=particle(i).Cost;
    particle(i).Best.Sol=particle(i).Sol;
    
    % Update Global Best  更新全局数据（选择cost小的值）
    if particle(i).Best.Cost<GlobalBest.Cost
        GlobalBest=particle(i).Best;
    end 
end

% Array to Hold Best Cost Values at Each Iteration  在每次迭代中保持最优值的阵列
BestCost=zeros(MaxIt,1);  %迭代矩阵初始化 每一次的最优值

%% PSO Main Loop 主函数
for it=1:MaxIt（迭代次数）

    for i=1:nPop（粒子个数）

        %x方向
        % 更新速度（粒子群的基本公式）
        particle(i).Velocity.x = w*particle(i).Velocity.x ...
            + c1*rand(VarSize).*(particle(i).Best.Position.x-particle(i).Position.x) ...
            + c2*rand(VarSize).*(GlobalBest.Position.x-particle(i).Position.x);
        
        % Update Velocity Bounds  速度边界约束（如果超过边界 那么等于边界）
        particle(i).Velocity.x = max(particle(i).Velocity.x,VelMin.x);
        particle(i).Velocity.x = min(particle(i).Velocity.x,VelMax.x);
        
        % Update Position 更新位置（上一时刻位置+这这一时刻速度）
        particle(i).Position.x = particle(i).Position.x + particle(i).Velocity.x;
        
        % Velocity Mirroring  速度镜像（我感觉不到这块有啥用）
        OutOfTheRange=(particle(i).Position.x<VarMin.x | particle(i).Position.x>VarMax.x);
        particle(i).Velocity.x(OutOfTheRange)=-particle(i).Velocity.x(OutOfTheRange);
        
        % Update Position Bounds  位置边界约束
        particle(i).Position.x = max(particle(i).Position.x,VarMin.x);
        particle(i).Position.x = min(particle(i).Position.x,VarMax.x);
        
        %y方向（与x方向原理一致）
        % Update Velocity
        particle(i).Velocity.y = w*particle(i).Velocity.y ...
            + c1*rand(VarSize).*(particle(i).Best.Position.y-particle(i).Position.y) ...
            + c2*rand(VarSize).*(GlobalBest.Position.y-particle(i).Position.y);
        
        % Update Velocity Bounds
        particle(i).Velocity.y = max(particle(i).Velocity.y,VelMin.y);
        particle(i).Velocity.y = min(particle(i).Velocity.y,VelMax.y);
        
        % Update Position
        particle(i).Position.y = particle(i).Position.y + particle(i).Velocity.y;
        
        % Velocity Mirroring
        OutOfTheRange=(particle(i).Position.y<VarMin.y | particle(i).Position.y>VarMax.y);
        particle(i).Velocity.y(OutOfTheRange)=-particle(i).Velocity.y(OutOfTheRange);
        
        % Update Position Bounds
        particle(i).Position.y = max(particle(i).Position.y,VarMin.y);
        particle(i).Position.y = min(particle(i).Position.y,VarMax.y);
        

        % Evaluation 评价 循环 返回路径长度Cost和 Sol（拟合点 距离 容忍度 可靠性）
        [particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position);
        
        % Update Personal Best  更新个体最优（如果本次迭代cost小，那么记录该粒子的位置、cost、和sol信息）
        if particle(i).Cost<particle(i).Best.Cost
            
            particle(i).Best.Position=particle(i).Position;
            particle(i).Best.Cost=particle(i).Cost;
            particle(i).Best.Sol=particle(i).Sol;
            
            % Update Global Best 更新全局最优（如果本次迭代cost比全局cost要小，那么保留本次cost为全局cost）
            if particle(i).Best.Cost<GlobalBest.Cost
                GlobalBest=particle(i).Best;
            end
        end
    end
    
    % Update Best Cost Ever Found
    BestCost(it)=GlobalBest.Cost; %每次迭代都把种群最优路径加到BestCost中 以便生成适应度曲线
    
    % Inertia Weight Damping
    w=w*wdamp;  %降低惯性 wdamp = 0.98
 
    % Show Iteration Information 显示迭代信息
    if GlobalBest.Sol.IsFeasible
        Flag=' *';
    else
        Flag=[', Violation = ' num2str(GlobalBest.Sol.Violation)];
    end
    disp(['Iteration ' num2str(it) ': Best Cost = ' num2str(BestCost(it)) Flag]);
    
    % Plot Solution
    figure(1);
    PlotSolution(GlobalBest.Sol,model);
    pause(0.01);
    
end

CreateModel.m

function model=CreateModel()

    % Source  初始点
    xs=-50;
    ys=-50;
    
    % Target  目标点
    xt=45;
    yt=36;
    
    %三个障碍物
%     xobs=[1.5 4.0 1.2 40];
%     yobs=[4.5 3.0 1.5 32];
%     robs=[1.5 1.0 0.8 1.2];

    %随机生成100个障碍物
     xobs=(rand(100,1)*2-1)*50; %障碍物x坐标
     yobs=(rand(100,1)*2-1)*50; %障碍物y坐标
     robs=rand(100,1)*3;        %障碍物半径
     for i=1:size(robs)         %约束一下半径都大于1
     if robs(i) < 2
         robs(i) =robs(i)+ 1;     
     end
      
    %10个中间节点
    n=10;   
    
    %位置的可行域范围
    xmin=-50;
    xmax= 50;
    ymin=-50;
    ymax= 50;
    
    %赋值给model结构体
    model.xs=xs;
    model.ys=ys;
    model.xt=xt;
    model.yt=yt;
    model.xobs=xobs;
    model.yobs=yobs;
    model.robs=robs;
    model.n=n;
    model.xmin=xmin;
    model.xmax=xmax;
    model.ymin=ymin;
    model.ymax=ymax;
    
end

 ParseSolution.m

function sol2=ParseSolution(sol1,model)

    %把生成的10个点的位置信息传进去
    x=sol1.x;
    y=sol1.y;
    %初始点目标点
    xs=model.xs;
    ys=model.ys;
    xt=model.xt;
    yt=model.yt;
    %障碍物
    xobs=model.xobs;
    yobs=model.yobs;
    robs=model.robs;
    
    XS=[xs x xt];
    YS=[ys y yt];
    k=numel(XS);  %numel 返回 数组元素个数
    TS=linspace(0,1,k); %在 0 - 1 之间等距离生成k个点（包括0 1）
    
    tt=linspace(0,1,100);  %（0,1,5000）的意思是通过样条曲线将tt（5个点）转换成5000个点
    xx=spline(TS,XS,tt);  %spline 三次样条插值 就是说每两个节点之间进行一次三次样条插值，插入的个数是tt（tt越大，生成的轨迹越慢，但也越详细）
    yy=spline(TS,YS,tt);
    
    %前后相邻元素之差 维度降1 用于计算距离
    dx=diff(xx);  
    dy=diff(yy);
    
    L=sum(sqrt(dx.^2+dy.^2)); %计算每一小段距离再求和
    
    nobs = numel(xobs); % Number of Obstacles  numel返回数组元素个数 （几个障碍物）
    Violation = 0;
    for k=1:nobs
        d=sqrt((xx-xobs(k)).^2+(yy-yobs(k)).^2); %计算三次样条插值的 每一个点到第k个障碍物的距离之和
        v=max(1-d/robs(k),0);   %从 1-d/robs(k)或0 中返回最大值 返回0说明d>robs(k) 路径可行  
        Violation=Violation+mean(v); %mean 计算每列的平均值 大于0说明路径与障碍物相遇
    end
    %sol的一些数据
    sol2.TS=TS;
    sol2.XS=XS;
    sol2.YS=YS;
    sol2.tt=tt;
    sol2.xx=xx;
    sol2.yy=yy;
    sol2.dx=dx;
    sol2.dy=dy;
    sol2.L=L;
    sol2.Violation=Violation;
    sol2.IsFeasible=(Violation==0);   %Violation==0 返回 1 sol2.IsFeasible=1 证明可靠
    
    
end

也没啥了吧 还有就是障碍物绘制填充什么的，在此基础上在pso中加入模拟退火算法，就形成了模拟退火粒子群算法

模拟退火-粒子群算法：

自适应模拟退火粒子群算法BSAPSO(学习笔记_03)_自适应模拟退火算法_Cloud-Lii的博客-CSDN博客自适应模拟退火粒子群算法BSAPSO（学习笔记_3）https://blog.csdn.net/weixin_49647278/article/details/122549504?utm_medium=distribute.pc_relevant.none-task-blog-2~default~baidujs_baidulandingword~default-1-122549504-blog-127769197.pc_relevant_aa&spm=1001.2101.3001.4242.2&utm_relevant_index=4

标题全局路径规划

DWA算法

DWA算法就是把动态因素考虑进去的，最简单的方法就是在上面的基础上，设置一些动态障碍物，在全局路径规划完成后，进行DWA避障。DWAdemo好像挺久了，这个人写的比较全面，用的也是这个代码，只不过是加到了上面里，所以直接参考他这个自己写一份也是可以的。

DWA算法：

DWA动态窗口法的原理及应用_动态窗口法原理_gophae的博客-CSDN博客看了CSDN博客上面的各种介绍DWA的博客，就这辣鸡点击都能过万？完全是对学术的不尊重吧，还是我来写一下吧。DWA算法的核心：DWA的核心在于所谓的动态窗口，这个东西在sampling based method 中就是sampling. 对于sampling可以是去sample state，也可以sample action. 百度的lattice planner其实就是在sample sta...https://blog.csdn.net/gophae/article/details/105299926

 在完成pso后，进行循环迭代DWA算法

全局+局部路径规划（包含移动障碍物）

%% 机器人的初期状态
x = [-50 -50 pi/4 0 0]'; 
% x下标宏定义 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
POSE_X      = 1;  %坐标 X
POSE_Y      = 2;  %坐标 Y
YAW_ANGLE   = 3;  %机器人航向角
V_SPD       = 4;  %机器人速度
W_ANGLE_SPD = 5;  %机器人角速度 
 
goal=[45 36];  %设置目标位置

global dt; 
dt = 0.1;% 时间[s]

%最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],速度分辨率[m/s],转速分辨率[rad/s]]
Kinematic = [1.5,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];

%定义Kinematic的下标含义
MD_MAX_V    = 1;%   最高速度m/s]
MD_MAX_W    = 2;%   最高角速度[rad/s]
MD_ACC      = 3;%   加速度[m/ss]
MD_VW       = 4;%   旋转加速度[rad/ss]
MD_V_RESOLUTION  = 5;%  速度分辨率[m/s]
MD_W_RESOLUTION  = 6;%  转速分辨率[rad/s]]

% 评价函数参数 航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间
evalParam = [0.045, 0.1 ,0.1, 3.0];

% 模拟实验的结果
result_x=[];   %累积存储走过的轨迹点的状态值
tic; % 估算程序运行时间开始
abc = 0;  %循环第abc次

%% 设置纵向移动障碍物
%初始化障碍物的速度参数 在（-1,1）之间
flag_obstacle = [1-2*rand(1) 1-2*rand(1) 1-2*rand(1) 1-2*rand(1) 1-2*rand(1) 1-2*rand(1) 1-2*rand(1) 1-2*rand(1) 1-2*rand(1)];   
vel_obstacle = 0.1;  %障碍物速度参数权值

%设置纵向障碍物的具体目标（9个）
obstacle=[model.xobs(8) model.yobs(8); 
    model.xobs(65) model.yobs(65);
    model.xobs(63) model.yobs(63);
    model.xobs(42) model.yobs(42);
    model.xobs(30) model.yobs(30);
    model.xobs(45) model.yobs(45);
    model.xobs(37) model.yobs(37);
    model.xobs(46) model.yobs(46);
    model.xobs(10) model.yobs(10);];

%% 设置横向移动障碍物
%% 初始化横向移动障碍物速度参数 在（-1,1）之间
flag_obstacle_1 = [1-2*rand(1) 1-2*rand(1) 1-2*rand(1) 1-2*rand(1) 1-2*rand(1) 1-2*rand(1)];   

%设置横向障碍物具体目标（6个）
obstacle_1=[model.xobs(52) model.yobs(52);   
    model.xobs(29) model.yobs(29);
    model.xobs(3) model.yobs(3);
    model.xobs(2) model.yobs(2);
    model.xobs(18) model.yobs(18);
    model.xobs(61) model.yobs(61);];

psi_obs=ones(numel(flag_obstacle),1);
psi_obs_1=ones(numel(flag_obstacle_1),1);

%% 循环
for i = 1:5000
    
    %对obstcle的移动计算
    for j = 1:numel(flag_obstacle)    %移动障碍物设定
        if obstacle(j,2) > 36 && flag_obstacle(j) > 0 || obstacle(j,2) < -50 && flag_obstacle(j) < 0
            flag_obstacle(j) = -flag_obstacle(j);
        end
        obstacle(j,2)=obstacle(j,2)+flag_obstacle(j)*vel_obstacle;   %flag_obstacle(j)*vel_obstacle 变换速度
        %添加obstcle的航向信息
        if flag_obstacle(j)>0
            psi_obs(j,1)=pi/2;
        else
            psi_obs(j,1)=pi*3/2;
        end
    end
    
    model.yobs(8)=obstacle(1,2);
    model.yobs(65)=obstacle(2,2);
    model.yobs(63)=obstacle(3,2);
    model.yobs(42)=obstacle(4,2);
    model.yobs(30)=obstacle(5,2);
    model.yobs(45)=obstacle(6,2);
    model.yobs(37)=obstacle(7,2);
    model.yobs(46)=obstacle(8,2);
    model.yobs(10)=obstacle(9,2);
    
    %对obstcle_1的移动计算
    for j = 1:numel(flag_obstacle_1)    %移动障碍物设定
        if obstacle_1(j,1) > 45 && flag_obstacle_1(j) > 0 || obstacle_1(j,1) < -50 && flag_obstacle_1(j) < 0
            flag_obstacle_1(j) = -flag_obstacle_1(j);
        end
        obstacle_1(j,1)=obstacle_1(j,1)+flag_obstacle_1(j)*vel_obstacle;   %flag_obstacle(j)*vel_obstacle 变换速度
        %添加obstacle_1的航向信息
        if flag_obstacle_1(j)>0
            psi_obs_1(j,1)=0;
        else
            psi_obs_1(j,1)=pi;
        end
    end
    
    model.xobs(52)=obstacle_1(1,1);
    model.xobs(29)=obstacle_1(2,1);
    model.xobs(3)=obstacle_1(3,1);
    model.xobs(2)=obstacle_1(4,1);
    model.xobs(18)=obstacle_1(5,1);
    model.xobs(61)=obstacle_1(6,1);
    
    %撞上纵向移动障碍物
    for l = 1:numel(flag_obstacle)
        if norm(obstacle(l,:)-x(1:2)')-1 < 0
            disp('==========Hit an obstacle!!==========');  %撞上障碍物
            temp = 1;
            break;
        end
    end
    %撞上横向移动障碍物
    for l = 1:numel(flag_obstacle_1)
        if norm(obstacle_1(l,:)-x(1:2)')-1 < 0
            disp('==========Hit an obstacle!!!==========');  %撞上障碍物
            temp = 1;
            break;
        end
    end
    if temp == 1
        break;
    end
    
    % DWA参数输入 返回控制量 u = [v(m/s),w(rad/s)] 和 轨迹
    [u,traj] = DynamicWindowApproach(x,Kinematic,goal(k,:),evalParam,model,psi_obs,psi_obs_1,obstacle,obstacle_1);%算出下发速度u/当前速度u
    x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度
    abc = abc+1;
    % 历史轨迹的保存
    result_x = [result_x; x'];  %最新结果 以行的形式 添加到result.x， 保存的是所有状态参数值，包括坐标xy、朝向、线速度、角速度，其实应该是只取坐标就OK
    
    % 是否到达目的地
    if norm(x(POSE_X:POSE_Y)-goal(k,:)')<2   % norm函数来求得坐标上的两个点之间的距离
        disp('==========Arrive Goal!!==========');break;
    end
    
    
    %导入PlotSolution_2绘制 动态障碍物  +  机器人模型
    ArrowLength = 0.5;      % 箭头长度
    PlotSolution_2(traj,result_x,x,ArrowLength,GlobalBest.Sol,model);
    
    disp(['time ' num2str(i)]); %打印
end
 
 
%% 航行时间   形式：时间已过 ** 秒。
toc;  

%% 计算航迹总长度
lx_c=[-50;result_x(:,1);45];
ly_c=[-50;result_x(:,2);36];

dlx=diff(lx_c);
dly=diff(ly_c);

L_c=sum(sqrt(dlx.^2+dly.^2));

disp(['DWA航迹长度为： ' num2str(L_c)]); %打印

DynamicWindowApproach.m

function [u,trajDB] = DynamicWindowApproach(x,model,goal,evalParam,model_1,psi_obs,psi_obs_1,obstacle,obstacle_1)
% Dynamic Window [vmin,vmax,wmin,wmax] 最小速度 最大速度 最小角速度 最大角速度速度
Vr = CalcDynamicWindow(x,model);  % 根据当前状态 和 运动模型 计算当前的参数允许范围（最小速度 最大速度 最小角速度 最大角速度速度）

% 评价函数的计算 evalDB N*5  每行一组可用参数 分别为 速度、角速度、航向得分、距离得分、速度得分
%               trajDB      每5行一条轨迹 每条轨迹都有状态x点串组成
[evalDB,trajDB]= Evaluation(x,Vr,goal,model_1,model,evalParam,psi_obs,psi_obs_1,obstacle,obstacle_1);  %evalParam 评价函数参数 [heading,dist,velocity,predictDT]

if isempty(evalDB)
    disp('no path to goal!!');
    u=[0;0];return;
end

% 各评价函数正则化
evalDB = NormalizeEval(evalDB);

% 最终评价函数的计算
feval=[];
for id=1:length(evalDB(:,1))
    feval = [feval;evalParam(1:3)*evalDB(id,3:5)']; %根据评价函数参数 前三个参数分配的权重 计算每一组可用的路径参数信息的得分  评价参数前三个指标 航向得分比重、距离得分比重、速度得分比重 X evalDB的 第三个到第五个指标 航向、距离、速度
end
evalDB = [evalDB feval]; % 最后一组；加最后一列，每一组速度的最终得分

[maxv,ind] = max(feval);% 选取评分最高的参数 对应分数返回给 maxv  对应下标返回给 ind
u = evalDB(ind,1:2)';% 返回最优参数的速度、角速度
end

Evaluation.m(这块把接口写上了以便后续为所欲为 包括障碍物角度以及障碍物位置)

function [evalDB,trajDB] = Evaluation(x,Vr,goal,model_1,model,evalParam,psi_obs,psi_obs_1,obstacle,obstacle_1)
evalDB = [];
trajDB = [];
for vt = Vr(1):model(5):Vr(2)       %根据速度分辨率遍历所有可用速度： 最小速度和最大速度 之间 速度分辨率 递增
    for ot=Vr(3):model(6):Vr(4)     %根据角度分辨率遍历所有可用角速度： 最小角速度和最大角速度 之间 角度分辨率 递增
        % 轨迹推测; 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻 到 预测时刻之间的轨迹（由轨迹点组成）
        [xt,traj] = GenerateTrajectory(x,vt,ot,evalParam(4));  %evalParam(4),前向模拟时间;  得到运动后的位姿 + 这段路径
        % 各评价函数的计算
        heading = CalcHeadingEval(xt,goal); % 前项预测终点的航向得分  偏差越小分数越高
        [dist,Flag] = CalcDistEval(xt,model_1);    % 前项预测终点 距离最近障碍物的间隙得分 距离越远分数越高
        vel = abs(vt);                  % 速度得分 速度越快分越高
        stopDist = CalcBreakingDist(vel,model); % 制动距离的计算
         if dist > stopDist && Flag == 0 % 如果可能撞到最近的障碍物 则舍弃此路径 （到最近障碍物的距离 大于 刹车距离 才取用）
%         if Flag == 0 % 如果可能撞到最近的障碍物 则舍弃此路径 （到最近障碍物的距离 大于 刹车距离 才取用）
            evalDB = [evalDB;[vt ot heading dist vel]];
            trajDB = [trajDB;traj];   % 每5行 一条轨迹
        end
    end
end
end

其他内容基本就跟上面博主写的一样了，算法很简单，一看就懂

这块说一下障碍物移动，也是参考了DWA算法的demo

在PSO随机生成障碍物的基础上，将障碍物信息保存到Excel中，然后读取的Excel数据，以便更改具体障碍物，障碍物的迭代移动demo在PlotSolution_2.m中

function PlotSolution_2(traj,result_x,x,ArrowLength,sol,model)

    xs=model.xs;
    ys=model.ys;
    xt=model.xt;
    yt=model.yt;
    xobs=model.xobs;
    yobs=model.yobs;
    robs=model.robs;
    
    XS=sol.XS;
    YS=sol.YS;
    xx=sol.xx;
    yy=sol.yy;
    
    theta=linspace(0,2*pi,100);
    for k=1:numel(xobs)   %填充障碍物（应该是）
        fill(xobs(k)+robs(k)*cos(theta),yobs(k)+robs(k)*sin(theta),[0.5 0.7 0.8]);
        hold on;
    end
    
  
    fill(xobs(8)+robs(8)*cos(theta),yobs(8)+robs(8)*sin(theta),[0.9373 0.4353 0.4235]);
    hold on;
    fill(xobs(65)+robs(65)*cos(theta),yobs(65)+robs(65)*sin(theta),[0.9373 0.4353 0.4235]);
    hold on;
    fill(xobs(63)+robs(63)*cos(theta),yobs(63)+robs(63)*sin(theta),[0.9373 0.4353 0.4235]);
    hold on;
    fill(xobs(42)+robs(42)*cos(theta),yobs(42)+robs(42)*sin(theta),[0.9373 0.4353 0.4235]);
    hold on;
    fill(xobs(30)+robs(30)*cos(theta),yobs(30)+robs(30)*sin(theta),[0.9373 0.4353 0.4235]);
    hold on;
    fill(xobs(45)+robs(45)*cos(theta),yobs(45)+robs(45)*sin(theta),[0.9373 0.4353 0.4235]);
    hold on;
    fill(xobs(37)+robs(37)*cos(theta),yobs(37)+robs(37)*sin(theta),[0.9373 0.4353 0.4235]);
    hold on;
    fill(xobs(46)+robs(46)*cos(theta),yobs(46)+robs(46)*sin(theta),[0.9373 0.4353 0.4235]);
    hold on;
    fill(xobs(10)+robs(10)*cos(theta),yobs(10)+robs(10)*sin(theta),[0.9373 0.4353 0.4235]);
    hold on;

    
    fill(xobs(52)+robs(52)*cos(theta),yobs(52)+robs(52)*sin(theta),[0.9294, 0.6941, 0.5137]);
    hold on;
    fill(xobs(29)+robs(29)*cos(theta),yobs(29)+robs(29)*sin(theta),[0.9294, 0.6941, 0.5137]);
    hold on;
    fill(xobs(3)+robs(3)*cos(theta),yobs(3)+robs(3)*sin(theta),[0.9294, 0.6941, 0.5137]);
    hold on;
    fill(xobs(2)+robs(2)*cos(theta),yobs(2)+robs(2)*sin(theta),[0.9294, 0.6941, 0.5137]);
    hold on;
    fill(xobs(18)+robs(18)*cos(theta),yobs(18)+robs(18)*sin(theta),[0.9294, 0.6941, 0.5137]);
    hold on;
    fill(xobs(61)+robs(61)*cos(theta),yobs(61)+robs(61)*sin(theta),[0.9294, 0.6941, 0.5137]);
    hold on;
    
    
    POSE_X=x(1);
    POSE_Y=x(2);
    YAW_ANGLE=x(3);
    V_SPD=x(4);
    W_ANGLE_SPD=x(5);
    
    n=0.02;
    long = 150;  %船长
    width = 50;  %船宽
    l = n*long;  %n是比例
    w = n*width;
    % psi = psi*pi/180;
    YAW_ANGLE_1 = YAW_ANGLE ;

    y1 = POSE_X - (l/2)*cos(YAW_ANGLE_1);%1
    x1 = POSE_Y - (l/2)*sin(YAW_ANGLE_1);
    y2 = POSE_X + (l/6)*cos(YAW_ANGLE_1);%2
    x2 = POSE_Y + (l/6)*sin(YAW_ANGLE_1);
    y3 = POSE_X + (l/2)*cos(YAW_ANGLE_1);%3
    x3 = POSE_Y + (l/2)*sin(YAW_ANGLE_1);
    y5 = y1 - (w/2)*sin(YAW_ANGLE_1);%5
    x5 = x1 + (w/2)*cos(YAW_ANGLE_1);
    y6 = y1 + (w/2)*sin(YAW_ANGLE_1);%6
    x6 = x1 - (w/2)*cos(YAW_ANGLE_1);
    y4 = y2 - (w/2)*sin(YAW_ANGLE_1);%4
    x4 = x2 + (w/2)*cos(YAW_ANGLE_1);
    y7 = y2 + (w/2)*sin(YAW_ANGLE_1);%7
    x7 = x2 - (w/2)*cos(YAW_ANGLE_1);


    
    x = [x3 x4 x5 x6 x7 x3];
    y = [y3 y4 y5 y6 y7 y3];
    
    plot(y,x,'g','LineWidth',0.5);  %画船
    quiver(POSE_X, POSE_Y, ArrowLength*cos(YAW_ANGLE), ArrowLength*sin(YAW_ANGLE),'ok'); %画圆
    
    % 绘制走过的所有位置 所有历史数据的 X、Y坐标
    plot(result_x(:,1),result_x(:,2),'-b');
    
    % 探索轨迹 画出待评价的轨迹
    if ~isempty(traj) %轨迹非空
        for it=1:length(traj(:,1))/5    %计算所有轨迹数  traj 每5行数据 表示一条轨迹点
            ind = 1+(it-1)*5; %第 it 条轨迹对应在traj中的下标
            plot(traj(ind,:),traj(ind+1,:),'-g'); hold on;  %根据一条轨迹的点串画出轨迹   traj(ind,:) 表示第ind条轨迹的所有x坐标值  traj(ind+1,:)表示第ind条轨迹的所有y坐标值
        end
    end
    
    
    plot(xx,yy,'k','LineWidth',2);
    plot(XS,YS,'ro');
    plot(xt,yt,'kp','MarkerSize',16,'MarkerFaceColor','g');  %终点
    hold off;
    grid on;
    axis equal;
    drawnow limitrate;

end

整体demo打包到一起发吧，在此再次感谢前面几个博主的分享

代码传送门：

https://download.csdn.net/download/weixin_53293018/87679985?spm=1001.2014.3001.5503

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_53293018/article/details/129802545