《实验2路径规划》由会员分享，可在线阅读，更多相关《实验2路径规划（6页珍藏版）》请在金锄头文库上搜索。
1、实验 2：移动机器人的路径规划一、实验目的：1、掌握移动机器人路径曲线规划方法，通过MATLAB设计目标点及控制点，实 现小车的路径规划与自动避障；2、理解机器人运动的物理空间到路径规划运算的位姿空间的转换；3、掌握并应用人工势场法；了解迭代学习控制法实验环境：软件：MATLAB2018a及Simulink相应组件硬件：电脑一台二、实验内容：1. 采用人工势场法编写程序，设置起点与终点，布置障碍物，合理设置一个至几 个中间控制点（临时目标点），使移动机器人规划出合理的运动路径。（1）实验原理：背景：机器人运动的物理空间到路径规划运算的位姿空间的转换。算法思路：将障碍物按机器人车体宽度的一半或者
2、外接圆半径进行扩张，同时将 机器人理想成为一个点。将机器人在周围环境中的运动，设计成一种在抽象的人 造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产 生“斥力”，最后通过求合力来控制移动机器人的运动。（2）实验程序：clearm=1;fx_sta=0;fy_sta=0;fa_des=5,14,20;fb_des=6,13,0;a1=0:4;b1=3*ones(size(a1);%初值（循环）%起点横坐标%起点纵坐标%两个中间控制点及终点坐标（20， 0）%设置障碍物的位置，由于点数过多，故采取此 种方法节省工作量，增加程序的可阅读性。a2=0:10;b2=11*ones(
3、size(a2);a3=7:13;b3=6*ones(size(a3); a4=12:20;b4=14*ones(size(a4);b5=0:8;a5=7*ones(size(b5);b6=0:12;a6=14*ones(size(b6);%障碍物设置完毕fx_obs=cat(2,a1,a2,a3,a4,a5,a6);fy_obs=cat(2,b1,b2,b3,b4,b5,b6);%通过 cat 函数将矩阵合并num_obs=length(fx_obs); nx(1)=fx_sta;ny(1)=fy_sta; x=fx_sta;y=fy_sta; d=zeros(1,num_obs+1);k=
4、1;for mid=1:length(fa_des)%障碍物数目%机器人初始位置%循环次数%for 函数进行循环d(num_obs+1)=sqrt(fa_des(mid)-x)*(fa_des(mid)-x)+(fb_des(mid)-y)*(fb_des(mid)-y);while(d(num_obs+1)0.1)%到目标点距离%防止找不到路时进入无限循环k=k+1;if (k3000)%3000 次以内不能找到路break;%强制跳出循环endf=zeros(1,num_obs+1);ffx=zeros(1,num_obs+1);ffy=zeros(1,num_obs+1); d(num_
5、obs+1)=sqrt(fa_des(mid)-x)*(fa_des(mid)-x)+(fb_des(mid)-y)*(fb_des(mid)-y ); %到目标点距离f(num_obs+1)=10; %目标点引力ffx(num_obs+1)=f(num_obs+1)*(fa_des(mid)-x)/d(num_obs+1);%X 方向分力ffy(num_obs+1)=f(num_obs+1)*(fb_des(mid)-y)/d(num_obs+1);%Y 方向分力for i=1:num_obs%计算障碍物的斥力xx=fx_obs(i);yy=fy_obs(i); d(i)=sqrt(xx-x
6、)*(xx-x)+(yy-y)*(yy-y);if (d(i)1.8)%比较接近障碍物：斥力系数为 3(斥力较小)f(i)=-3/(xx-x)*(xx-x)+(yy-y)*(yy-y);if (d(i)0.7)%十分接近障碍物：斥力系数 6(斥力较大)f(i)=-6/(xx-x)*(xx-x)+(yy-y)*(yy-y);endelsef(i)=-0.01/(xx-x)*(xx-x)+(yy-y)*(yy-y);%远离障碍物：斥力系数 0.01(可以忽略不计) end ffx(i)=f(i)*(xx-x)/d(i); ffy(i)=f(i)*(yy-y)/d(i);endsumx=0;sumy
7、=0;sumz=0;for i=1:num_obs+1%计算 x、y 各方向合力sumx=sumx+ffx(i);sumy=sumy+ffy(i);endsumz=sqrt(sumx*sumx+sumy*sumy);dx=sumx/sumz; dy=sumy/sumz; m=m+1;nx(m)=nx(m-1)+0.2*dx; ny(m)=ny(m-1)+0.2*dy; if (m=2000)%x 方向受力%y 方向受力%x 方向位移%y 方向位移%跳出“力平衡陷阱”nx(2000)=nx(2000)-1.2*rand(1);ny(2000)=ny(2000)-1.8*rand(1); %通过随
8、机数使机器人跳出陷阱x=nx(m);%位置更新y=ny(m);endenda1_plot=0:0.2:4;b1_plot=3*ones(size(a1_plot);%绘制地图中的障碍物a2_plot=0:0.2:10;b2_plot=11*ones(size(a2_plot);a3_plot=7:0.2:13;b3_plot=6*ones(size(a3_plot); a4_plot=12:0.2:20;b4_plot=14*ones(size(a4_plot);b5_plot=0:0.2:8;a5_plot=7*ones(size(b5_plot); b6_plot=0:0.2:12;a6_
9、plot=14*ones(size(b6_plot);x_plot=cat(2,a1_plot,a2_plot,a3_plot,a4_plot,a5_plot,a6_plot); y_plot=cat(2,b1_plot,b2_plot,b3_plot,b4_plot,b5_plot,b6_plot); plot(nx,ny,r,x_plot,y_plot,bo,fa_des,fb_des,*);%绘制路径与中间控制点axis(0,20,0,20)%坐标系绘制grid(3)运行结果：1&161410862810161820140上0斗-图 2.3 实验结果截图H Figure 1-X文件旧编辑
10、旧査看(V)插入(1工具桌面D窗口 (W)帮助(H)七曰p| n纵2 5跟X 0202. 迭代学习控制法(1)实验原理：它通过反复应用先前试验得到的信息来获得能够产生期望输出轨迹的控制输入， 以改善控制质量。与传统的控制方法不同的是，迭代学习控制能以非常简单的方 式处理不确定度相当高的动态系统，且仅需较少的先验知识和计算量，同时适应 性强，易于实现；更主要的是，它不依赖于动态系统的精确数学模型，是一种以 迭代产生优化输入信号，使系统输出尽可能逼近理想值的算法。(2)实验程序：T=1;h=0.01;n=T/h+1;m=20;A=1,-1;0 -2;B=0;1;C=0 1;D=0;G, H=c2d
11、(A,B,h);L=5;D=1;x1(1:m,1:n)=0;x2(1:m,1:n)=0;u=zeros(m,n);y=zeros(m,n);e=zeros(m,n);for i=1:nt(i)=(i-1)*h;yd(i)=12*(i-1)*h)T*(1-(i-1)*h);endfor k=2:mfor r=1:n-1x1(k,r+1)=G(1,1)*x1(k,r)+G(1,2)*x2(k,r)+H(1,1)*u(k,r);%计算状态量 x1 的变化x2(k,r+1)=G(2,1)*x1(k,r)+G(2,2)*x2(k,r)+H(2,1)*u(k,r); y(k,r+1)=C(1,1)*x1(
12、k,r+1)+C(1,2)*x2(k,r+1);%计算输出的变化%计算偏差%采样周期%时间周期%时间点数%迭代点数%系统模型%状态方程变传递函数%增益%控制量%时间周期离散化%期望轨迹e(k,r+1)=yd(r+1)-y(k,r+1);endfor r=1:n u(k+1,r)=u(k,r)+L*e(k,r)+D*(e(k,r)-e(k-1,r);%PD 型迭代学习控制算法endendfigure(1);plot(t,yd,r*-);xlabel(时间t);ylabel(期望轨迹yd和实际输出y); hold on;for k=1:mdisp(正在进行地,n um2str(k-1),次迭代);
13、 pause(0.01);plot(t,y(k,:),g-);hold on;disp(迭代完成);for k=1:m err(k)=norm(e(k,:),2)/sqrt(n);%平方根end figure(2)plot(1:m-2,err(1,3:20),bo-);lege nd(迭代误差);xlabel(迭代次数);ylabel(误差均方根);运行结果 DA k-| S Ojjfl FigMre- 2 D X宜賊F)寓辆:E 査怕粗我山 E A3iD) fin(W) WflWH?壬.-Jrr.-:-.-冷0曰y aV X 動沱乳*(5L| S p图 2.4 实验结果截图三、总结及心得(1)遇到的问题及解决在实验过程中，在对于中间控制点进行设置时出现了问题。最开始取的中间控制点为（5，6）与（13，13），实验结果是移动机器人迷路 在规定步数内没有正确规划路径。ia16141210862268101214182D*| I igun? 1-_x立什離査看（V）捶入1工ME桌頁D）窗口 （W）帮助（H）竺 asl k o z - a d= q20图 3.1 中间控制点选取不当 当时第一时间想到的是在（13，13）点处，引力与斥力没有很好的合成引导机器 人走向正确的道路，于是便修改了斥力系数，可经过多次修改，仍然无法规划出 正确的路径，走入了我设置障碍的“迷宫”。此时我发现
