机器人避障问题的最短路径分析(4)

2019-03-03 22:02

十,附录

附录一;为最短路线O→A 的程序,是利用MATIAB求解的

%0:初始点 D:转弯圆弧圆心 A:到达点 r:表示圆弧半径 function result=zongchang1(r)

O(1)=0;O(2)=0;A(1)=300;A(2)=300; D(1)=80;D(2)=210;

OD = sqrt((O(1)-D(1))^2+(O(2)-D(2))^2); OA=sqrt((O(1)-A(1))^2+(O(2)-A(2))^2); AD=sqrt((D(1)-A(1))^2+(D(2)-A(2))^2);

alpha1=acos((OD^2+AD^2-OA^2)/(2*OD*AD)); alpha2 = acos(r/OD); alpha3 = acos(r/AD);

alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角 OS1=sqrt(OD^2-r^2);%OS1,AS2均为圆弧切线% S2A=sqrt(AD^2-r^2); S1S2hu=r*alpha4;

result=OS1+S1S2hu+S2A; end

zongchang1(10) ans =

471.0372

附录二;为最短路线O→B的程序,是利用MATIAB编程的

O-F

%求解一次转弯所经路线总长

%0:初始点 Q1:转弯圆弧圆心 F:到达点 r:表示圆弧半径ρ function result=zongchangob1(r)

O(1)=0;O(2)=0;F(1)=141.675;F(2)=440.55; Q1(1)=60;Q1(2)=300;

OQ1 = sqrt((O(1)-Q1(1))^2+(O(2)-Q1(2))^2); OF=sqrt((O(1)-F(1))^2+(O(2)-F(2))^2); FQ1=sqrt((Q1(1)-F(1))^2+(Q1(2)-F(2))^2);

alpha1=acos((OQ1^2+FQ1^2-OF^2)/(2*OQ1*FQ1)); alpha2 = acos(r/OQ1); alpha3 = acos(r/FQ1);

alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角

第 16 页

OD=sqrt(OQ1^2-r^2);%OD,FE均为圆弧切线% EF=sqrt(FQ1^2-r^2); DEhu=r*alpha4;

result=OD+DEhu+EF; end

zongchangob1(10) F-P

%求解一次转弯所经路线总长

% E:初始点 Q2:转弯圆弧圆心 P:到达点 r:表示圆弧半径ρ function result=zongchangob1(r)

E(1)=51.675;E(2)=305.5;P(1)=185;P(2)=452.5; Q2(1)=150;Q2(2)=435;

EQ2 = sqrt((E(1)-Q2(1))^2+(E(2)-Q2(2))^2); EP=sqrt((E(1)-P(1))^2+(E(2)-P(2))^2); PQ2=sqrt((Q2(1)-P(1))^2+(Q2(2)-P(2))^2);

alpha1=acos((EQ2^2+PQ2^2-EP^2)/(2*EQ2*PQ2)); alpha2 = acos(r/EQ2); alpha3 = acos(r/PQ2);

alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角 EF=sqrt(EQ2^2-r^2);?,PG均为圆弧切线% PG=sqrt(PQ2^2-r^2); FGhu=r*alpha4; result=FGhu+PG; end P-J

%求解一次转弯所经路线总长

% P:初始点 Q3:转弯圆弧圆心 J:到达点 r:表示圆弧半径ρ function result=zongchangob1(r)

J(1)=230;J(2)=530;P(1)=185;P(2)=452.5; Q3(1)=220;Q3(2)=470;

PQ3 = sqrt((P(1)-Q3(1))^2+(P(2)-Q3(2))^2); JP=sqrt((J(1)-P(1))^2+(J(2)-P(2))^2); JQ3=sqrt((Q3(1)-J(1))^2+(Q3(2)-J(2))^2);

alpha1=acos((JQ3^2+PQ3^2-JP^2)/(2*JQ3*PQ3)); alpha2 = acos(r/PQ3);

第 17 页

alpha3 = acos(r/JQ3);

alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角 JI=sqrt(JQ3^2-r^2);%JI,PH均为圆弧切线% PH=sqrt(PQ3^2-r^2); HIhu=r*alpha4; result=PH+HIhu+JI; end J-q

%求解一次转弯所经路线总长

% P:初始点 Q3:转弯圆弧圆心 J:到达点 r:表示圆弧半径ρ function result=zongchangob1(r)

J(1)=230;J(2)=530;P(1)=185;P(2)=452.5; Q3(1)=220;Q3(2)=470;

PQ3 = sqrt((P(1)-Q3(1))^2+(P(2)-Q3(2))^2); JP=sqrt((J(1)-P(1))^2+(J(2)-P(2))^2); JQ3=sqrt((Q3(1)-J(1))^2+(Q3(2)-J(2))^2);

alpha1=acos((JQ3^2+PQ3^2-JP^2)/(2*JQ3*PQ3)); alpha2 = acos(r/PQ3); alpha3 = acos(r/JQ3);

alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角 JI=sqrt(JQ3^2-r^2);%JI,PH均为圆弧切线% PH=sqrt(PQ3^2-r^2); HIhu=r*alpha4; result=PH+HIhu+JI; end q-B

%求解一次转弯所经路线总长

% q:初始点 Q5:转弯圆弧圆心 B:到达点 r:表示圆弧半径ρ function result=zongchangob1(r)

B(1)=100;B(2)=700;q(1)=185;q(2)=565; Q5(1)=150;Q5(2)=600;

BQ5= sqrt((B(1)-Q5(1))^2+(B(2)-Q5(2))^2); Bq=sqrt((B(1)-q(1))^2+(B(2)-q(2))^2); qQ5=sqrt((Q5(1)-q(1))^2+(Q5(2)-q(2))^2);

alpha1=acos((BQ5^2+qQ5^2-Bq^2)/(2*BQ5*qQ5)); alpha2 = acos(r/qQ5); alpha3 = acos(r/BQ5);

alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角 BM=sqrt(BQ5^2-r^2);%BM,qL均为圆弧切线%

第 18 页

qL=sqrt(qQ5^2-r^2); LMhu=r*alpha4;

result=qL+LMhu+BM; end

function oc (a,b,c,d,e);

sum5 = zongchangob0F(10)+zongchangobFP(10)+zongchangobPJ(10)+zongchangobJq(10)+zongchangobqB(10)

869.4896

附录三;为最短时间路线O→A的程序(圆心固定时),是利用MATIAB编程的

f>%是假设圆心固定所得出的模型

%E:初始点 F:转弯圆弧圆心 G:到达点 for i=1:7000;

r(i)=10+0.01*i;

E(1)=0;E(2)=0;F(1)=80;F(2)=210;G(1)=300;G(2)=300; EF=sqrt((E(1)-F(1))^2+(E(2)-F(2))^2);%b EG=sqrt((E(1)-G(1))^2+(E(2)-G(2))^2);%a FG=sqrt((F(1)-G(1))^2+(F(2)-G(2))^2);%c

alpha1=acos((EF^2+FG^2-EG^2)/(2*EF*FG));% \\alpha1为起始点与圆心连线的夹角 alpha2=acos(r(i)/EF);% \\alpha2为起点到圆心与切点连线的夹角 alpha3=acos(r(i)/FG);% \\alpha3为起点到圆心与切点连线的夹角 alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角 ES1=sqrt(EF^2-r(i)^2);%ES1,ES2均为圆弧切线% S2G=sqrt(FG^2-r(i)^2); S1S2hu=r(i)*alpha4;

L(i)=ES1+S1S2hu+S2G;%总路程

a(i)=ES1/5+S2G/5+S1S2hu*(1+exp(10-0.1*r(i)^2))/5;%总时间 end

mintime=min(a) %最小时间 k=find(a==mintime)

r(k)%最小时间时候的半径 L(k)%最小时间时候的路程

附录四;为最短时间路线O→A的程序(半径固定时),是利用MATIAB编程的

f>%是假设半径固定所得出的模型

%E:初始点 F:转弯圆弧圆心 G:到达点 clear r=11.5;

第 19 页

for i=1:100;

F(1)=80+0.01*i; for j=1:100;

F(2)=210-0.01*j;

E(1)=0;E(2)=0;G(1)=300;G(2)=300; EF=sqrt((E(1)-F(1))^2+(E(2)-F(2))^2);%b EG=sqrt((E(1)-G(1))^2+(E(2)-G(2))^2);%a FG=sqrt((F(1)-G(1))^2+(F(2)-G(2))^2);%c

alpha1=acos((EF^2+FG^2-EG^2)/(2*EF*FG));% \\alpha1为起始点与圆心连线的夹角 alpha2=acos(r/EF);% \\alpha2为起点到圆心与切点连线的夹角 alpha3=acos(r/FG);% \\alpha3为到达点到圆心与切点连线的夹角 alpha4=2*pi-alpha1-alpha2-alpha3;% \\alpha4为转弯圆心角 ES1=sqrt(EF^2-r^2);%ES1,ES2均为圆弧切线% S2G=sqrt(FG^2-r^2); S1S2hu=r*alpha4;

L(i,j)=ES1+S1S2hu+S2G;%总路程

a(i,j)=ES1/5+S2G/5+S1S2hu*(1+exp(10-0.1*r^2))/5;%总时间 end end

m=min(a); %最小时间 mintime=min(m)

[k,l]=find(a==mintime);

F=[80+0.01*k,210-0.01*l] %最小时间时候的圆心坐标

LL=L(k,l) %LL最小时间时候的路程

附录五;为最短时间路线O→A的两个切点C、D,是利用MATIAB编程的

>> syms x

solve('(300-sqrt(56109.75-(300-x)^2))^2-509*(300-sqrt(56109.75-(300-x)^2))+x^2-381*x+87000','x')

y=300-sqrt(56109.75-(300- 85.922520589325167902358822608471)^2)

> syms x3

solve('50109.75-x3^2-209*sqrt(50109.75-x3^2)-81*x3+x3^2=0','x3')

>> y=sqrt(50109.75-70.078048160753442392122465674524^2)

第 20 页


机器人避障问题的最短路径分析(4).doc 将本文的Word文档下载到电脑 下载失败或者文档不完整,请联系客服人员解决!

下一篇:浅谈“微课”在数控车工实训教学中的应用

相关阅读
本类排行
× 注册会员免费下载(下载后可以自由复制和排版)

马上注册会员

注:下载文档有可能“只有目录或者内容不全”等情况,请下载之前注意辨别,如果您已付费且无法下载或内容有问题,请联系我们协助你处理。
微信: QQ: