turns-00015.parquet:3207
44cd23f01a6e4c5099fa1c71degenerate_repetitionAbsentFinal dense release
USER
请加入球体障碍物
ASSISTANT
以下是在示例代码中加入球体障碍物的修改版：
```matlab
clear;
clc;
close all;
% 定义三维地图大小和障碍物坐标
mapSize = [100, 100, 100]; % 三维地图大小
startPoint = [10, 10, 10]; % 起点坐标
endPoint = [90, 90, 90]; % 终点坐标
obstacles_cube = [20, 20, 20; 30, 30, 30; 40, 40, 40; ...]; % 长方体障碍物坐标
obstacles_sphere = [60, 60, 60, 5; 70, 70, 70, 7; ...]; % 球体障碍物坐标和半径
% 设置鲸鱼优化算法的参数
numWhales = 50; % 鲸鱼数量
maxIterations = 100; % 最大迭代次数
a = 2; % 慢速收敛的参数
b = 0.5; % 鲸鱼个体的觅食半径
stepSize = 0.1; % 鲸鱼移动的步长
% 生成初始的鲸鱼个体位置
whales = rand(numWhales, 3) .* repmat(mapSize, numWhales, 1);
% 鲸鱼优化算法主循环
fitnessHistory = zeros(maxIterations, 1);
bestFitness = Inf;
bestPath = [];
for iter = 1:maxIterations
% 计算每个鲸鱼的适应度值
fitnessValues = zeros(numWhales, 1);
for i = 1:numWhales
whalePos = whales(i, :);
fitnessValues(i) = fitnessFunction(whalePos, startPoint, endPoint, obstacles_cube, obstacles_sphere);
% 更新最优适应度和最优路径
if fitnessValues(i) < bestFitness
bestFitness = fitnessValues(i);
bestPath = whalePos;
end
end
% 更新路径规划适应度变化图
fitnessHistory(iter) = bestFitness;
% 更新每个鲸鱼的位置
for i = 1:numWhales
whalePos = whales(i, :);
whalePos_new = whalePos;
% 以最优路径为目标移动鲸鱼
if rand() > 0.5
whalePos_new = whalePos_new + (bestPath - whalePos_new) * a;
else
randomWhaleIndex = randi(numWhales);
randomWhalePos = whales(randomWhaleIndex, :);
whalePos_new = whalePos_new + (randomWhalePos - whalePos_new) * b;
end
whalePos_new = whalePos_new + stepSize * randn(1, 3);
% 避免越界
whalePos_new = min(whalePos_new, mapSize);
whalePos_new = max(whalePos_new, [0 0 0]);
whales(i, :) = whalePos_new;
end
% 显示路径规划适应度变化图
plot(fitnessHistory, 'b');
xlabel('Iteration');
ylabel('Fitness');
drawnow;
end
% 绘制三维空间中最优路线图
figure;
plot3(startPoint(1), startPoint(2), startPoint(3), 'ro', 'MarkerSize', 10);
hold on;
plot3(endPoint(1), endPoint(2), endPoint(3), 'ro', 'MarkerSize', 10);
for i = 1:size(obstacles_cube, 1)
plotCube(obstacles_cube(i, :), 1, [0 0 1]);
end
for i = 1:size(obstacles_sphere, 1)
plotSphere(obstacles_sphere(i, :), [1 0 0]);
end
plot3(bestPath(:, 1), bestPath(:, 2), bestPath(:, 3), 'r', 'LineWidth', 2);
axis equal;
xlabel('X');
ylabel('Y');
zlabel('Z');
% 定义适应度函数
function fitness = fitnessFunction(position, startPoint, endPoint, obstacles_cube, obstacles_sphere)
fitness = norm(position - endPoint);
% 判断是否碰撞长方体障碍物
for i = 1:size(obstacles_cube, 1)
obstaclePos = obstacles_cube(i, :);
if checkCollisionCube(position, obstaclePos)
fitness = fitness + norm(position - obstaclePos);
end
end
% 判断是否碰撞球体障碍物
for i = 1:size(obstacles_sphere, 1)
obstaclePos = obstacles_sphere(i, 1:3);
radius = obstacles_sphere(i, 4);
if checkCollisionSphere(position, obstaclePos, radius)
fitness = fitness + norm(position - obstaclePos);
end
end
% 判断是否超出地图范围
if ~CheckBoundaries(position)
fitness = fitness + 100; % 惩罚超出边界的路径
end
end
% 判断路径是否与长方体障碍物碰撞
function collision = checkCollisionCube(position, obstacle)
distance = norm(position - obstacle);
collision = (distance <= sqrt(3)); % 假设障碍物是边长为1的长方体
end
% 判断路径是否与球体障碍物碰撞
function collision = checkCollisionSphere(position, obstacle, radius)
distance = norm(position - obstacle);
collision = (distance <= radius);
end
% 判断路径是否超出地图边界
function boundary = CheckBoundaries(position)
boundary = all(position >= 0) && all(position <= mapSize);
end
% 绘制长方体障碍物
function plotCube(position, size, color)
vertices = [...
position;...
position + [size/2, 0, 0];...
position + [size/2, size/2, 0];...
position + [0, size/2, 0];...
position + [0, 0, size];...
position + [size/2, 0, size];...
position + [size/2, size/2, size];...
position + [0, size/2, size]...
];
faces = [...
1, 2, 3, 4;... % 底面
1, 2, 6, 5;... % 竖面1
2, 3, 7, 6;... % 竖面2
3, 4, 8, 7;... % 竖面3
4, 1, 5, 8;... % 竖面4
5, 6, 7, 8;... % 顶面
];
patch('Vertices', vertices, 'Faces', faces, 'FaceColor', color);
end
% 绘制球体障碍物
function plotSphere(position, color)
[x, y, z] = sphere(20); % 球的细分级别为20
x = x * position(4) + position(1);
y = y * position(4) + position(2);
z = z * position(4) + position(3);
surf(x, y, z, 'FaceColor', color, 'EdgeColor', 'none');
end
```
在修改后的代码中，新增了`obstacles_sphere`变量，用于定义球体障碍物的坐标和半径。适应度函数`fitnessFunction`也相应地新增了对球体障碍物的碰撞判断。另外，新增了绘制球体障碍物的`plotSphere`函数。
通过这些修改，代码可以同时处理长方体障碍物和球体障碍物，并在绘制三维空间中最优路线图时将这两种障碍物都标示出来。请根据具体情况调整障碍物的坐标和半径，以及其他参数，以实现你的路径规划需求。