6-枚举法下的避障路径寻优（连续性问题求解）

Halcom

6-枚举法下的避障路径寻优（连续性问题求解）
链接：https://pan.baidu.com/s/1Qyv7FurF5sL_puHO3B6rnw 提取码：pxg6

具体链接在halcom.cn论坛，联系人QQ：3283892722
该论坛是一个学习交流平台，我会逐一的和大家分享学习。
欢迎大家录制视频，你可在论坛进行打赏分享。
视频专用播放器：http://halcom.cn/forum.php?mod=viewthread&tid=258&extra=page%3D1

主程序如下：

1. clc,clear,close all
   
2. warning off
   
3. % 初始化种群
   
4. model = CreateModel();
   
5. itermax = 100;  % 迭代次数
   
6. fitnesszbest = inf;
   
7. for i = 1:itermax
   
8.     disp(['当前迭代次数: ', num2str(i), ' 总迭代次数：', num2str(itermax)])
   
9.     sol = CreateRandomSolution(model);
   
10.     % 计算目标函数
    
11.     [fitness, sol1] = fun(sol, model);
    
12.    
    
13.     if fitness<fitnesszbest
    
14.         fitnesszbest = fitness;
    
15.         fitness_iter(i) = fitnesszbest;
    
16.         zbest = sol1;
    
17.     else
    
18.         fitness_iter(i) = fitnesszbest;
    
19.     end
    
20.     figure(1)
    
21.     PlotSolution(zbest,model)
    
22.     pause(0.1)
    
23. end
    
24. figure(2)
    
25. plot(fitness_iter, '.-');
    
26. axis tight;
    
27. xlabel('迭代次数'); ylabel('适应度曲线');

复制代码

CreateModel脚本如下：

1. function model=CreateModel()
   
2. 
   
3.     % 起点坐标
   
4.     xs=0;
   
5.     ys=0;
   
6.     % 终点坐标
   
7.     xt=4;
   
8.     yt=6;
   
9.     % 3个障碍圆
   
10.     triangle_x1 = [0.35,0.5,2.8];  triangle_y1 = [5.0,3.5,4.7];
    
11.     triangle_x2 = [3.7,3.3,4.8];  triangle_y2 = [3.8,2.4,2.9];
    
12.     triangle_x3 = [1.8,1.3,0.6];  triangle_y3 = [1.3,2.2,1.0];
    
13.    
    
14.     xobs=[mean(triangle_x1),mean(triangle_x2),mean(triangle_x3)];  % 三角形中心横坐标
    
15.     yobs=[mean(triangle_y1),mean(triangle_y2),mean(triangle_y3)];  % 三角形中心纵坐标
    
16.     % 三角形中心 到其它定点 最大距离
    
17.     for i=1:length(xobs)
    
18.         d(1,i) = sqrt( (xobs(1)-triangle_x1(i)).^2+(yobs(1)-triangle_y1(i)).^2 );
    
19.         d(2,i) = sqrt( (xobs(2)-triangle_x2(i)).^2+(yobs(2)-triangle_y2(i)).^2 );
    
20.         d(3,i) = sqrt( (xobs(3)-triangle_x3(i)).^2+(yobs(3)-triangle_y3(i)).^2 );
    
21.     end
    
22.     robs=max(d,[],2)';  % 三角形中心 到其它定点 最大距离
    
23.    
    
24.     n = 3; % 障碍物数量
    
25.    
    
26.     % 地图大小
    
27.     xmin=0;  xmax= 7;
    
28.     ymin=0;  ymax= 7;
    
29.    
    
30.     model.xs=xs;    % 起点横坐标
    
31.     model.ys=ys;    % 起点纵坐标
    
32.     model.xt=xt;    % 终点横坐标
    
33.     model.yt=yt;    % 终点纵坐标
    
34.     model.triangle_x1 = triangle_x1; model.triangle_y1 = triangle_y1;
    
35.     model.triangle_x2 = triangle_x2; model.triangle_y2 = triangle_y2;
    
36.     model.triangle_x3 = triangle_x3; model.triangle_y3 = triangle_y3;
    
37.     model.xobs=xobs;  % 障碍物横坐标
    
38.     model.yobs=yobs;  % 障碍物纵坐标
    
39.     model.robs=robs;  % 障碍物半径
    
40.     model.n=n;        % 障碍物个数
    
41.     model.xmin=xmin;  % 地图最小横坐标
    
42.     model.xmax=xmax;  % 地图最大横坐标
    
43.     model.ymin=ymin;  % 地图最小纵坐标
    
44.     model.ymax=ymax;  % 地图最大纵坐标
    
45.    
    
46. end

复制代码

CreateRandomSolution脚本函数如下：

1. function sol = CreateRandomSolution(model)
   
2. % 中间插值n个点
   
3. n = model.n;
   
4. % 取值范围
   
5. xmin = model.xmin;
   
6. xmax = model.xmax;
   
7. ymin = model.ymin;
   
8. ymax = model.ymax;
   
9. sol.x = unifrnd( xmin,xmax, 1,n );  % 均匀分布，产生n个点的横坐标
   
10. sol.y = unifrnd( ymin,ymax, 1,n );  % 均匀分布，产生n个点的纵坐标

复制代码

适应度函数如下：

游客，如果您要查看本帖隐藏内容请回复

画图函数如下：

1. function PlotSolution(sol,model)
   
2. 
   
3.     xs=model.xs;
   
4.     ys=model.ys;
   
5.     xt=model.xt;
   
6.     yt=model.yt;
   
7.     xobs=model.xobs;
   
8.     yobs=model.yobs;
   
9.     robs=model.robs;
   
10.    
    
11.     XS=sol.XS;
    
12.     YS=sol.YS;
    
13.     xx=sol.xx;
    
14.     yy=sol.yy;
    
15.    
    
16.     % 绘制障碍物
    
17.     fill(model.triangle_x1,model.triangle_y1,[0,1,0]);
    
18.     hold on;
    
19.     fill(model.triangle_x2,model.triangle_y2,[0,1,0]);
    
20.     fill(model.triangle_x3,model.triangle_y3,[0,1,0]);
    
21. 
    
22.     plot(xx,yy,'k','LineWidth',2);  % 插值的曲线坐标
    
23.     plot(XS,YS,'r.');  % 最优解的点坐标
    
24.     plot(xs,ys,'bs','MarkerSize',12,'MarkerFaceColor','y');  % 起点
    
25.     plot(xt,yt,'kh','MarkerSize',16,'MarkerFaceColor','r');  % 终点
    
26.     hold off;
    
27.     grid on;
    
28.     axis equal;
    
29. 
    
30. end

复制代码

参考：【1】结果动态图

侯一心

很实用:victory:

wasd789

学习一下，学习学习。。。。。。

siyecao1996

超级实用,感谢楼主~~

weki

学习路径规划中，谢谢楼主

Reborn

大爱楼主，毕业设计不用愁了

江南哥

太好太好太好

operator321

谢谢分享，学习了

wwmwwmwwm

111111111111111

summer9931

感谢楼主分享，学习了