最近开发了一个新的群智能优化算法，想看看在三维无人机航迹优化效果。自己不想花时间写，所以找了很多公众号和网络相关资料，要么需要付费要么就太复杂了。突然，发现一个https://ww2.mathworks.cn/matlabcentral/fileexchange/90292-spherical-vector-based-particle-swarm-optimization-spso
网站中可以找到MATLAB代码，而且看起来不是很复杂。
必须强调作者哈：
Spherical Vector-based Particle Swarm Optimization (SPSO)
版本 1.0.3 (2.9 MB) 作者: Manh Duong Phung
Implementation of the Spherical Vector-based Particle Swarm Optimization (SPSO) algorithm for UAV path planning
GitHub - duongpm/SPSO: Spherical Vector-based Particle Swarm Optimization
为了防止部分可能进不了，我就斗胆把代码搬过来了，不过如果代码对你有用，一定要去给代码原作者点个关注收藏啥的：一共这么几个模块，我将按顺序给出！不过我没运行哈，具体可不可以我不知道，反正评论表示还行。兄弟们等我几天，我把这个翻译成PYTHON语言。然后就是文章类型选的是原创，因为选另外两个挺麻烦的。
%{
This model is generated by:
- Loading terrain map
- Creating threats as cylinders
- Creating start and finish points
- Setting ranges and limits
%}
function model=CreateModel()
H = imread('ChrismasTerrain.tif'); % Get elevation data
H (H < 0) = 0;
MAPSIZE_X = size(H,2); % x index: columns of H
MAPSIZE_Y = size(H,1); % y index: rows of H
[X,Y] = meshgrid(1:MAPSIZE_X,1:MAPSIZE_Y); % Create all (x,y) points to plot
% Threats as cylinders
R1=80; % Radius
x1 = 400; y1 = 500; z1 = 100; % center
R2=70; % Radius
x2 = 600; y2 = 200; z2 = 150; % center
R3=80; % Radius
x3 = 500; y3 = 350; z3 = 150; % center
R4=70; % Radius
x4 = 350; y4 = 200; z4 = 150; % center
R5=70; % Radius
x5 = 700; y5 = 550; z5 = 150; % center
R6=80; % Radius
x6 = 650; y6 = 750; z6 = 150; % center
% Map limits
xmin= 1;
xmax= MAPSIZE_X;
ymin= 1;
ymax= MAPSIZE_Y;
zmin = 100;
zmax = 200;
% Start and end position
start_location = [200;100;150];
end_location = [800;800;150];
% Number of path nodes (not including the start position (start node))
n=10;
% Incorporate map and searching parameters to a model
model.start=start_location;
model.end=end_location;
model.n=n;
model.xmin=xmin;
model.xmax=xmax;
model.zmin=zmin;
model.ymin=ymin;
model.ymax=ymax;
model.zmax=zmax;
model.MAPSIZE_X = MAPSIZE_X;
model.MAPSIZE_Y = MAPSIZE_Y;
model.X = X;
model.Y = Y;
model.H = H;
model.threats = [x1 y1 z1 R1;x2 y2 z2 R2; x3 y3 z3 R3; x4 y4 z4 R4; x5 y5 z5 R5;x6 y6 z6 R6];
PlotModel(model);
end
%
% Create random paths (solutions)
%
function sol=CreateRandomSolution(VarSize,VarMin,VarMax)
% Random path nodes
sol.r=unifrnd(VarMin.r,VarMax.r,VarSize);
sol.psi=unifrnd(VarMin.psi,VarMax.psi,VarSize);
sol.phi=unifrnd(VarMin.phi,VarMax.phi,VarSize);
end% Calculate the minimum Distance between a Point to a Segment
function dist = DistP2S(x,a,b)
d_ab = norm(a-b);
d_ax = norm(a-x);
d_bx = norm(b-x);
if d_ab ~= 0
if dot(a-b,x-b)*dot(b-a,x-a)>=0
A = [b-a;x-a];
dist = abs(det(A))/d_ab; % Formula of point - line distance
else
dist = min(d_ax, d_bx);
end
else % if a and b are identical
dist = d_ax;
end%
% Calculate path cost
%
function cost=MyCost(sol,model)
J_inf = inf;
n = model.n;
H = model.H;
% Input solution
x=sol.x;
y=sol.y;
z=sol.z;
% Start location
xs=model.start(1);
ys=model.start(2);
zs=model.start(3);
% Final location
xf=model.end(1);
yf=model.end(2);
zf=model.end(3);
x_all = [xs x xf];
y_all = [ys y yf];
z_all = [zs z zf];
N = size(x_all,2); % Full path length
% Altitude wrt sea level = z_relative + ground_level
z_abs = zeros(1,N);
for i = 1:N
z_abs(i) = z_all(i) + H(round(y_all(i)),round(x_all(i)));
end
%============================================
% J1 - Cost for path length
J1 = 0;
for i = 1:N-1
diff = [x_all(i+1) - x_all(i);y_all(i+1) - y_all(i);z_abs(i+1) - z_abs(i)];
J1 = J1 + norm(diff);
end
%==============================================
% J2 - threats/obstacles Cost
% Threats/Obstacles
threats = model.threats;
threat_num = size(threats,1);
drone_size = 1;
danger_dist = 10*drone_size;
J2 = 0;
for i = 1:threat_num
threat = threats(i,:);
threat_x = threat(1);
threat_y = threat(2);
threat_radius = threat(4);
for j = 1:N-1
% Distance between projected line segment and threat origin
dist = DistP2S([threat_x threat_y],[x_all(j) y_all(j)],[x_all(j+1) y_all(j+1)]);
if dist > (threat_radius + drone_size + danger_dist) % No collision
threat_cost = 0;
elseif dist < (threat_radius + drone_size) % Collision
threat_cost = J_inf;
else % danger
threat_cost = (threat_radius + drone_size + danger_dist) - dist;
end
J2 = J2 + threat_cost;
end
end
%==============================================
% J3 - Altitude cost
% Note: In this calculation, z, zmin & zmax are heights with respect to the ground
zmax = model.zmax;
zmin = model.zmin;
J3 = 0;
for i=1:n
if z(i) < 0 % crash into ground
J3_node = J_inf;
else
J3_node = abs(z(i) - (zmax + zmin)/2);
end
J3 = J3 + J3_node;
end
%==============================================
% J4 - Smooth cost
J4 = 0;
turning_max = 45;
climb_max = 45;
for i = 1:N-2
% Projection of line segments to Oxy ~ (x,y,0)
for j = i:-1:1
segment1_proj = [x_all(j+1); y_all(j+1); 0] - [x_all(j); y_all(j); 0];
if nnz(segment1_proj) ~= 0
break;
end
end
for j = i:N-2
segment2_proj = [x_all(j+2); y_all(j+2); 0] - [x_all(j+1); y_all(j+1); 0];
if nnz(segment2_proj) ~= 0
break;
end
end
climb_angle1 = atan2d(z_abs(i+1) - z_abs(i),norm(segment1_proj));
climb_angle2 = atan2d(z_abs(i+2) - z_abs(i+1),norm(segment2_proj));
turning_angle = atan2d(norm(cross(segment1_proj,segment2_proj)),dot(segment1_proj,segment2_proj));
if abs(turning_angle) > turning_max
J4 = J4 + abs(turning_angle);
end
if abs(climb_angle2 - climb_angle1) > climb_max
J4 = J4 + abs(climb_angle2 - climb_angle1);
end
end
%============================================
% Weight coeffcients
b1 = 5;
b2 = 1;
b3 = 10;
b4 = 1;
% Overall cost
cost = b1*J1 + b2*J2 + b3*J3 + b4*J4;
end% Plot the terrain model and threats
function PlotModel(model)
mesh(model.X,model.Y,model.H); % Plot the data
colormap summer; % Default color map.
set(gca, 'Position', [0 0 1 1]); % Fill the figure window.
axis equal vis3d on; % Set aspect ratio and turn off axis.
shading interp; % Interpolate color across faces.
material dull; % Mountains aren't shiny.
camlight left; % Add a light over to the left somewhere.
lighting gouraud; % Use decent lighting.
xlabel('x [m]');
ylabel('y [m]');
zlabel('z [m]');
hold on
% Threats as cylinders
threats = model.threats;
threat_num = size(threats,1);
h=250; % Height
for i = 1:threat_num
threat = threats(i,:);
threat_x = threat(1);
threat_y = threat(2);
threat_z = threat(3);
threat_radius = threat(4);
[xc,yc,zc]=cylinder(threat_radius); % create a unit cylinder
% set the center and height
xc=xc+threat_x;
yc=yc+threat_y;
zc=zc*h+threat_z;
c = mesh(xc,yc,zc); % plot the cylinder
set(c,'edgecolor','none','facecolor','#FF0000','FaceAlpha',.3); % set color and transparency
end
end%{
This function will plot:
- model with a terrain map and obstacles
- solutions with different views
%}
function PlotSolution(sol,model,smooth)
%% Plot 3D view
figure(1)
PlotModel(model)
x=sol.x;
y=sol.y;
z=sol.z;
% Start location
xs=model.start(1);
ys=model.start(2);
zs=model.start(3);
% Final location
xf=model.end(1);
yf=model.end(2);
zf=model.end(3);
x_all = [xs x xf];
y_all = [ys y yf];
z_all = [zs z zf];
N = size(x_all,2); % real path length
% Path height is relative to the ground height
for i = 1:N
z_map = model.H(round(y_all(i)),round(x_all(i)));
z_all(i) = z_all(i) + z_map;
end
% given data in a point matrix, xyz, which is 3 x number of points
xyz = [x_all;y_all;z_all];
[ndim,npts]=size(xyz);
xyzp=zeros(size(xyz));
for k=1:ndim
xyzp(k,:)=ppval(csaps(1:npts,xyz(k,:),smooth),1:npts);
end
plot3(xyzp(1,:),xyzp(2,:),xyzp(3,:),'k','LineWidth',2);
% plot start point
plot3(x_all(1),y_all(1),z_all(1),'ks','MarkerSize',7,'MarkerFaceColor','k');
% plot target point
plot3(x_all(N),y_all(N),z_all(N),'ko','MarkerSize',7,'MarkerFaceColor','k');
hold off;
%% Plot top view
figure(3)
mesh(model.X,model.Y,model.H); % Plot the data
colormap summer; % Default color map.
set(gca, 'Position', [0 0 1 1]); % Fill the figure window.
axis equal vis3d on; % Set aspect ratio and turn off axis.
shading interp; % Interpolate color across faces.
material dull; % Mountains aren't shiny.
camlight left; % Add a light over to the left somewhere.
lighting gouraud; % Use decent lighting.
xlabel('x [m]');
ylabel('y [m]');
zlabel('z [m]');
hold on
% Threats as cylinders
threats = model.threats;
threat_num = size(threats,1);
for i = 1:threat_num
threat = threats(i,:);
threat_x = threat(1);
threat_y = threat(2);
threat_z = max(max(model.H))+1; % choose z to be the highest peak
threat_radius = threat(4);
for j=1:3
% Define circle parameters:
% Make an array for all the angles:
theta = linspace(0, 2 * pi, 2000);
% Create the x and y locations at each angle:
x = threat_radius * cos(theta) + threat_x;
y = threat_radius * sin(theta) + threat_y;
% Need to make a z value for every (x,y) pair:
z = zeros(1, numel(x)) + threat_z;
% Do the plot:
% First plot the center:
plot3(threat_x, threat_y, threat_z, 'o', 'color', '#752e29', 'MarkerSize', 3, 'MarkerFaceColor','#752e29');
% Next plot the circle:
plot3(x, y, z, '-', 'color', '#752e29', 'LineWidth', 1);
% Repeat for a smaller radius
threat_radius = threat_radius - 20;
end
end
% plot path
plot3(xyzp(1,:),xyzp(2,:),xyzp(3,:),'k','LineWidth',2);
% plot start point
plot3(x_all(1),y_all(1),z_all(1),'ks','MarkerSize',7,'MarkerFaceColor','k');
% plot target point
plot3(x_all(N),y_all(N),z_all(N),'ko','MarkerSize',7,'MarkerFaceColor','k');
% Set top view
view(0,90)
hold off;
%% Plot side view
figure(5)
mesh(model.X,model.Y,model.H); % Plot the data
colormap summer; % Default color map.
set(gca, 'Position', [0 0 1 1]); % Fill the figure window.
axis equal vis3d on; % Set aspect ratio and turn off axis.
shading interp; % Interpolate color across faces.
material dull; % Mountains aren't shiny.
camlight left; % Add a light over to the left somewhere.
lighting gouraud; % Use decent lighting.
xlabel('x [m]');
ylabel('y [m]');
zlabel('z [m]');
hold on
% plot path
plot3(xyzp(1,:),xyzp(2,:),xyzp(3,:),'k','LineWidth',2);
% plot start point
plot3(x_all(1),y_all(1),z_all(1),'ks','MarkerSize',7,'MarkerFaceColor','k');
% plot target point
plot3(x_all(N),y_all(N),z_all(N),'ko','MarkerSize',7,'MarkerFaceColor','k');
view(90,0);
hold off;
end%_________________________________________________________________________________%
% Spherical Vector-based Particle Swarm Optimization (SPSO) source codes demo 1.0%
% %
% Developed in MATLAB 2020b %
% %
% Author and programmer: Manh Duong Phung %
% %
% e-Mail: [email protected] %
% [email protected] %
% %
% Homepage: https://uet.vnu.edu.vn/~duongpm/ %
% %
% Main paper: Manh Duong Phung, Quang Phuc Ha %
% "Safety-enhanced UAV Path Planning with %
% Spherical Vector-based Particle Swarm Optimization", %
% Applied soft computing %
% %
% %
%_________________________________________________________________________%
%
% Find a path that maximizes the probability of finding object
%
clc;
clear;
close all;
%% Problem Definition
model = CreateModel(); % Create search map and parameters
CostFunction=@(x) MyCost(x,model); % Cost Function
nVar=model.n; % Number of Decision Variables = searching dimension of PSO = number of path nodes
VarSize=[1 nVar]; % Size of Decision Variables Matrix
% Lower and upper Bounds of particles (Variables)
VarMin.x=model.xmin;
VarMax.x=model.xmax;
VarMin.y=model.ymin;
VarMax.y=model.ymax;
VarMin.z=model.zmin;
VarMax.z=model.zmax;
VarMax.r=2*norm(model.start-model.end)/nVar;
VarMin.r=0;
% Inclination (elevation)
AngleRange = pi/4; % Limit the angle range for better solutions
VarMin.psi=-AngleRange;
VarMax.psi=AngleRange;
% Azimuth
% Determine the angle of vector connecting the start and end points
dirVector = model.end - model.start;
phi0 = atan2(dirVector(2),dirVector(1));
VarMin.phi=phi0 - AngleRange;
VarMax.phi=phi0 + AngleRange;
% Lower and upper Bounds of velocity
alpha=0.5;
VelMax.r=alpha*(VarMax.r-VarMin.r);
VelMin.r=-VelMax.r;
VelMax.psi=alpha*(VarMax.psi-VarMin.psi);
VelMin.psi=-VelMax.psi;
VelMax.phi=alpha*(VarMax.phi-VarMin.phi);
VelMin.phi=-VelMax.phi;
%% PSO Parameters
MaxIt=200; % Maximum Number of Iterations
nPop=500; % Population Size (Swarm Size)
w=1; % Inertia Weight
wdamp=0.98; % Inertia Weight Damping Ratio
c1=1.5; % Personal Learning Coefficient
c2=1.5; % Global Learning Coefficient
%% Initialization
% Create Empty Particle Structure
empty_particle.Position=[];
empty_particle.Velocity=[];
empty_particle.Cost=[];
empty_particle.Best.Position=[];
empty_particle.Best.Cost=[];
% Initialize Global Best
GlobalBest.Cost=inf; % Minimization problem
% Create an empty Particles Matrix, each particle is a solution (searching path)
particle=repmat(empty_particle,nPop,1);
% Initialization Loop
isInit = false;
while (~isInit)
disp("Initialising...");
for i=1:nPop
% Initialize Position
particle(i).Position=CreateRandomSolution(VarSize,VarMin,VarMax);
% Initialize Velocity
particle(i).Velocity.r=zeros(VarSize);
particle(i).Velocity.psi=zeros(VarSize);
particle(i).Velocity.phi=zeros(VarSize);
% Evaluation
particle(i).Cost= CostFunction(SphericalToCart(particle(i).Position,model));
% Update Personal Best
particle(i).Best.Position=particle(i).Position;
particle(i).Best.Cost=particle(i).Cost;
% Update Global Best
if particle(i).Best.Cost < GlobalBest.Cost
GlobalBest=particle(i).Best;
isInit = true;
end
end
end
% Array to Hold Best Cost Values at Each Iteration
BestCost=zeros(MaxIt,1);
%% PSO Main Loop
for it=1:MaxIt
% Update Best Cost Ever Found
BestCost(it)=GlobalBest.Cost;
for i=1:nPop
% r Part
% Update Velocity
particle(i).Velocity.r = w*particle(i).Velocity.r ...
+ c1*rand(VarSize).*(particle(i).Best.Position.r-particle(i).Position.r) ...
+ c2*rand(VarSize).*(GlobalBest.Position.r-particle(i).Position.r);
% Update Velocity Bounds
particle(i).Velocity.r = max(particle(i).Velocity.r,VelMin.r);
particle(i).Velocity.r = min(particle(i).Velocity.r,VelMax.r);
% Update Position
particle(i).Position.r = particle(i).Position.r + particle(i).Velocity.r;
% Velocity Mirroring
% If a particle moves out of the range, it will moves backward next
% time
OutOfTheRange=(particle(i).Position.r<VarMin.r | particle(i).Position.r>VarMax.r);
particle(i).Velocity.r(OutOfTheRange)=-particle(i).Velocity.r(OutOfTheRange);
% Update Position Bounds
particle(i).Position.r = max(particle(i).Position.r,VarMin.r);
particle(i).Position.r = min(particle(i).Position.r,VarMax.r);
% psi Part
% Update Velocity
particle(i).Velocity.psi = w*particle(i).Velocity.psi ...
+ c1*rand(VarSize).*(particle(i).Best.Position.psi-particle(i).Position.psi) ...
+ c2*rand(VarSize).*(GlobalBest.Position.psi-particle(i).Position.psi);
% Update Velocity Bounds
particle(i).Velocity.psi = max(particle(i).Velocity.psi,VelMin.psi);
particle(i).Velocity.psi = min(particle(i).Velocity.psi,VelMax.psi);
% Update Position
particle(i).Position.psi = particle(i).Position.psi + particle(i).Velocity.psi;
% Velocity Mirroring
OutOfTheRange=(particle(i).Position.psi<VarMin.psi | particle(i).Position.psi>VarMax.psi);
particle(i).Velocity.psi(OutOfTheRange)=-particle(i).Velocity.psi(OutOfTheRange);
% Update Position Bounds
particle(i).Position.psi = max(particle(i).Position.psi,VarMin.psi);
particle(i).Position.psi = min(particle(i).Position.psi,VarMax.psi);
% Phi part
% Update Velocity
particle(i).Velocity.phi = w*particle(i).Velocity.phi ...
+ c1*rand(VarSize).*(particle(i).Best.Position.phi-particle(i).Position.phi) ...
+ c2*rand(VarSize).*(GlobalBest.Position.phi-particle(i).Position.phi);
% Update Velocity Bounds
particle(i).Velocity.phi = max(particle(i).Velocity.phi,VelMin.phi);
particle(i).Velocity.phi = min(particle(i).Velocity.phi,VelMax.phi);
% Update Position
particle(i).Position.phi = particle(i).Position.phi + particle(i).Velocity.phi;
% Velocity Mirroring
OutOfTheRange=(particle(i).Position.phi<VarMin.phi | particle(i).Position.phi>VarMax.phi);
particle(i).Velocity.phi(OutOfTheRange)=-particle(i).Velocity.phi(OutOfTheRange);
% Update Position Bounds
particle(i).Position.phi = max(particle(i).Position.phi,VarMin.phi);
particle(i).Position.phi = min(particle(i).Position.phi,VarMax.phi);
% Evaluation
particle(i).Cost=CostFunction(SphericalToCart(particle(i).Position,model));
% Update Personal Best
if particle(i).Cost < particle(i).Best.Cost
particle(i).Best.Position=particle(i).Position;
particle(i).Best.Cost=particle(i).Cost;
% Update Global Best
if particle(i).Best.Cost < GlobalBest.Cost
GlobalBest=particle(i).Best;
end
end
end
% Inertia Weight Damping
w=w*wdamp;
% Show Iteration Information
disp(['Iteration ' num2str(it) ': Best Cost = ' num2str(BestCost(it))]);
end
%% Plot results
% Best solution
BestPosition = SphericalToCart(GlobalBest.Position,model);
disp("Best solution...");
BestPosition
smooth = 0.95;
PlotSolution(BestPosition,model,smooth);
% Best cost
figure;
plot(BestCost,'LineWidth',2);
xlabel('Iteration');
ylabel('Best Cost');
grid on;% Convert the solution from spherical space to Cartesian coordinates
function position = SphericalToCart(sol,model)
% Start location
xs = model.start(1);
ys = model.start(2);
zs = model.start(3);
% Solution in Sperical space
r = sol.r;
psi = sol.psi;
phi = sol.phi;
% First Cartesian coordinate
x(1) = xs + r(1)*cos(psi(1))*sin(phi(1));
% Check limits
if x(1) > model.xmax
x(1) = model.xmax;
end
if x(1) < model.xmin
x(1) = model.xmin;
end
y(1) = ys + r(1)*cos(psi(1))*cos(phi(1));
if y(1) > model.ymax
y(1) = model.ymax;
end
if y(1) < model.ymin
y(1) = model.ymin;
end
z(1) = zs + r(1)*sin(psi(1));
if z(1) > model.zmax
z(1) = model.zmax;
end
if z(1) < model.zmin
z(1) = model.zmin;
end
% Next Cartesian coordinates
for i = 2:model.n
x(i) = x(i-1) + r(i)*cos(psi(i))*sin(phi(i));
if x(i) > model.xmax
x(i) = model.xmax;
end
if x(i) < model.xmin
x(i) = model.xmin;
end
y(i) = y(i-1) + r(i)*cos(psi(i))*cos(phi(i));
if y(i) > model.ymax
y(i) = model.ymax;
end
if y(i) < model.ymin
y(i) = model.ymin;
end
% z(i) = z(i-1) + r(i)*cos(psi(i));
z(i) = z(i-1) + r(i)*sin(psi(i));
if z(i) > model.zmax
z(i) = model.zmax;
end
if z(i) < model.zmin
z(i) = model.zmin;
end
end
position.x = x;
position.y = y;
position.z = z;
end