机器人避障问题

摘要

本文主要运用直线逼近法、直线圆弧相切连接最优定理等方法来解决机器人避障问题。对于问题一：

运用直线逼近法证得直线圆弧相切连接最优定理，得出结论：若一大圆弧角三角形完全包括另一小圆弧角三角形，则该三角形曲线周长必大于小的三角形周长。

据此定理，在曲线过弯时，选择最小半径可满足路径最短，即为10个单位半径，与各障碍顶角的圆相切。首先根据两点之间障碍物的顶角分布，排列符合要求的路线。由于圆弧较小，而且加上圆弧会使得问题复杂化，所以先忽略圆弧和限制条件，计算出直线的最短路径，大致筛选选出总长较小、长度相近（之差小于100）的路线，再利用平面几何知识具体求出各切点，进而利用matlab计算各直线、圆弧的长度，求和得出各路线实际长度，比较得到最短路径。

建模结果：

1、O→A的最短路径长为:471.0372；

圆弧的圆心坐标为：(80,210)。2、O→B的最短路径长为：853.7；

圆弧的圆心坐标为：(60,300),(150,435),(150,600),(220,470),(220,530)。3、O→C的最短路径长为：1088.15；

圆弧的圆心坐标为：(230,60),(410,100),(500,200),(720,520),(720,600)。4、O→A→B→C→O最短路径长为：2724.896。

圆弧的圆心坐标为：

(80,210)(220,530),(150,600),(270,680),(370,680),(430,680),(540,730),(670,730),(230,60),(410,100),(500,200),(720,520),(720,600)对于问题二：

通过对机器人过弯规律的分析可知，针对最短时间路径问题：(1)在障碍物拐点处的圆弧半径为临界半径;

(2)因为直线速度大于vv()1e100.1转弯速度,所以在不转弯的地方尽可能走直线;

所以A至B最速路径仍由2段直线和一段圆弧相切构成。而走最短距离的时间并不一定是最短的到达时间，此时应对转弯半径和转弯时所走的圆弧的圆心进行重新判断。再通过寻找圆弧段半径大于最小转弯半径等约束条件，列出非线性方程利用LINGO得出最优解，具体建立了从O点出发到A的最短时间路径的非线性规划模型，获得了机器人从O点出发，到达A的最短时间路径。

建模结果：转弯半径：12.9885；

O→A的最短时间路径时间长：94.2283；

相应圆弧的圆心坐标为（82.1414，207.9153）。

关键词：机器人避障、最短路径、直线圆弧连接相切最优、优化。

问题重述

图1是一个800×800的平面场景图，在原点O(0,0)点处有一个机器人，它只能在该平面场景范围内活动。图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物，障碍物的数学描述如下表：编号[**************]

障碍物名称正方形圆形平行四边形三角形正方形三角形长方形平行四边形长方形正方形正方形长方形

(360,240)(280,100)(80,60)(60,300)(0,470)(150,600)(370,680)(540,600)(640,520)(500,140)

表1:障碍物数学描述表

在图1的平面场景中，障碍物外指定一点为机器人要到达的目标点（要求目标点与障碍物的距离至少超过10个单位）。规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。

机器人直线行走的最大速度为v05个单位/秒。机器人转弯时，最大转弯速度为

vv()

100.1

1e

左下顶点坐标(300,400)

其它特性描述边长200

圆心坐标(550,450)，半径70底边长140，左上顶点坐标(400,330)上顶点坐标(345,210)，右下顶点坐标(410,100)

边长150

上顶点坐标(150,435)，右下顶点坐标(235,300)

长220，宽60

底边长90，左上顶点坐标(180,680)

长60，宽120边长130边长80长300，宽60

其中是转弯半径。如果超过该速度，机器人将发生侧翻，无

法完成行走。

请建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。对场景图中4个点O(0,0)，A(300,300)，B(100,700)，C(700,640)，具体计算：(1)机器人从O(0,0)出发，O→A、O→B、O→C和O→A→B→C→O的最短路径。(2)机器人从O(0,0)出发，到达A的最短时间路径。注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总距离和总时间。

图1：原始障碍物图

问题分析

针对问题一：为达到要求,我们按照以下原则选择路径:

(1)由直线圆弧相切连接最优可知过弯时在障碍物拐点处的圆弧半径为临界半径，则可认为以最小半径10的圆过弯;

(2)因为直线速度大于转弯速度,所以在不转弯的地方尽可能走直线;按照上述原则,我们选取以下步骤求最短路径:

(1)根据圆弧路线应与顶角处以10为半径的圆相切，确定出问题可能的几个最短路径，先忽略圆弧和限制条件，计算出直线的最短路径，大致筛选选出总长较小、长度相近（之差小于100）的路线；

(2)针对上述第一遍筛选出来的几条最短直线路径,在障碍物拐点处加入弧线转弯,应用相关几何知识，计算出具体的实际路径长度，比较取其中最短的一条。

针对问题二：由问题一知，A至B用时最少的路径仍由2段直线和1段圆弧相切构成，但需要满足的条件是圆弧段半径大于最小转弯半径。通过对转弯速度与圆弧长度关系的推算，得到用时最短时的弯道半径。

模型假设

1、假设机器人为一可运动的质点，即质点机器人不考虑其外形尺寸；

2、假设半径不变时，机器人在行进、转弯过程中能一直保持最大速度；3、假设机器人可准确执行运动轨道，无任何偏差；

4、假设机器人的行进速度可瞬时加减变化，不受条件限制。

符号说明

hijDijDhjTR

（h、i、j定义同上）

机器人的行走路径上的障碍物的顶点。

（i、j定义同上）

机器人在O→A→B→C→O环道中的各线

切点。（h、j定义同上）

从O到A的时间

大圆半径

模型准备

1、建立机器人运动坐标系：

以O为原点，两对应坐标轴，水平方向为X轴，垂直方向为Y轴。2、建立机器人可安全运动到达的区域图：由于保持安全距离10个单位，则机器人的实际可到达到区域应由各障碍物的外延10个单位的区域组成如下（图一）所示。

图一

3、证明“直线圆弧相切连接最优”这一猜想。

DOPS

AEQS

图二

由以上证明得：1）、在对应同一圆心角的情况下，大圆弧走得距离大于小圆弧的距离。2）、又结合图二知：因为DO、AE是切线，所以同理。从而得到两切线较其它曲线段相对最短。“直线圆弧相切连接最优”猜想得证。

4、障碍物为圆的情况

在前面我们已经证明了障碍物有顶点的情况下，机器人沿禁区边缘圆弧转弯路径最小。下面就机器人通过圆形障碍物的情况是否符合以上结论进行讨论。

如图三，分别是机器人通过圆形禁区的3种情况，其中a图为机器人人沿禁区边缘进行转弯的。b图为机器人沿以圆形障碍物边上一点为圆形，最小转弯半径为转弯半径的弧转弯图，c图为机器人沿一个刚好绕过禁区的圆弧转弯的情况。

图三

由以上用弹性绳的方法进行证明发现，b图转弯路径最小，但是经过了禁区，因此不可行，另外两种方案a图路径明显要比c图小。因此机器人过禁区时沿禁区边缘圆弧转弯路径最小得结论同样适用于障碍物位圆的情况。转弯半径为：半径，d为机器人距离障碍物的最小安全距离）

rd

（其中r为障碍物

模型的建立与求解

一、对问题一模型的建立与求解

该问题要求路径最短，即不要求速度与时间，同时由直线圆弧相切连接最优可得：

本论文问题一求路径最短可采用10单位过弯半径，即以半径为10个单位的圆弧过弯可满足两点避障过弯最短问题。

1、机器人从O(0,0)出发，O→A的最短路径

已知机器人所走路线为直线或圆弧，通过规划可得实际得如下（图四）四种避障行进方

案：

图四

1）当机器人以一个连续圆弧过弯，即选择路线二或路线四；

2）当机器人以直线—圆弧—直线的方式过弯，即有以10单位半径过弯模式的线路一

和三；

由直线圆弧相切连接最优可筛选出相对较短路径：路线一和路线三。通过matlab可以得出两条路径直线与圆弧相加的具体实际长度：

图五、路线一图六、路线三

圆弧的圆心坐标为(80,210)。路线一（Da11、Da12）和路线三Da31、Da32）的切点坐标：

点号

坐标

Da1170.5060213.1406

Da1276.6064219.4066

表1

Da31232.114950.2262

Da32239.704357.5862

路线一和路线三的实际具体长度：

线号长度

La11La12La13

总长度

224.4994237.4868

9.051011.1391

表2

237.4868249.7999

471.0372498.4259

由此可见：

O→A的最短路径长为：471.0372；圆弧的圆心坐标为(80,210)；路径坐标：

(0,0)→(70.5060,213.1406)→(76.6064,219.4066)→(300,300)。2、机器人从O(0,0)出发，O→A的最短路径

由已知机器人所走路线为直线或圆弧，通过规划可得实际得如下（图七）三种避障路线方案：

图七

由于方案较多，可预先进行粗略筛选（表三）：

路线名称路线一路线二路线三

段一305.7

8224.50235.37

段二162.25175.70240.05

段三73.65230.50230.50

表3

段四6096.9596.95

段五96.95111.36111.36

段六111.36//

总长810839914.22

由表格可以看出路线一和路线二相差不大，且路线较短，则可进一步仅考虑该两路线的精确长度。由C++编程可以得出两条路线的实际长度以及相应坐标：

Lb11

长度

305.78

Lb12

4.20

Lb13

166.36

Lb14

7.58

Lb15

75.66

Lb16

12.62

表4

Lb1760

Lb18

9.89

Lb19

94.2

Lb110

6.05

Lb111

111.36

总长853.70

Lb21

长度

224.5

Lb22

8.37

Lb3

178.12

Lb24

12.22

Lb25

230.49

表

Lb26

9.89

Lb2796.95

Lb28

6.15

Lb29

111.36

总长878.05

所以可以得到O→B的最短路径长为：853.7。

圆弧的圆心坐标为：(60,300),(150,435),(150,600),(220,470),(220,530)路径坐标：

(0,0)→(50.1354,301.6396)→(51.6795,305.547)→(141.6795,440.547)→

(147.9621,444.79)→(222.038,460.2096)→(230,470)→(230,530)→(229.9563,530.9242)→(229.5746,532.8954)→(229.2564,533.7791)→(229.1263,534.0782)→

(225.4971,538.3544)→(144.5036,591.6465)→(140.6922,596.346)→(100,700)3、机器人从O(0,0)出发，O→C的最短路径

如图八所示，机器人从O到C点有五条可能最短路线，在路径途中经过多次转弯到达目标点。先通过对简化模型即对直线段的直接求和筛选出较短的路径，如图九。

图八图九

由C++编程可得路径一和路径二的实际长度：

Lc11

长度

237.49

Lc12

0.06

Lc13

184.39

Lc14

7.44

Lc15

133.07

Lc16

0.69

表6

Lc17387.82

Lc18

Lc19

6.76

Lc110

43.6

总长1088.15

Lc21

长度

237.49

Lc22

7.85

Lc23

187.96

Lc24

9.15

Lc25

156.63

Lc26

8.12

表7

Lc27

356.09

Lc28

Lc29

6.76

Lc210

43.6

总长1101.23

所以可以得到O→C的最短路径长为：1088.15。

圆弧的圆心坐标为：(230,60),(410,100),(500,200),(720,520),(720,600)路径坐标：

(0,0)→(232.115,50.2262)→(232.169,50.2381)→(412.169,90.2381)→

(418.245,94.5361)→(491.755,205.464)→(492.072,206.074)→(727.928,513.926)→(730,520)→(730,600)→(727.718,606.359)→(700,640)

4、机器人从O(0,0)出发，O→A→B→C→O的最短路径

已经由一、三小问得到了O→A、C→O的最短路径及其距离。现只需讨论A→B、B→C路径的最短距离路径。其路径实现步骤与前问相同，可以得到最终路径（图十）：

图十

由C++编程可得O→A→B→C→O最短路径的实际长度：2724.896。圆弧的圆心坐标为：

(80,210)(220,530),(150,600),(270,680),(370,680),(430,680),(540,730),(670,730),(230,60),(410,100),(500,200),(720,520),(720,600)；路径坐标：

(0,0)→(70.5060,213.1406)→(76.6064,219.4066)→(300,300)→(229.572，532.895)→(225.421,538.166)→(144.579,591.834)→(140.692,596.346)→(100,700)→

(270.586,689.983)→(271.923,689.615)→(368.077,670.385)→(370,670)→(430,670)→(435.494,671.806)→(534.506,738.194)→(540,740)→(670,740)→(679.767,732.145)→(700,640)→(727.718,606.359)→(730,600)→(730,520）→(727.928,513.926)→

(492.072,206.074)→(491.755,205.464)→(418.245,94.5361)→(412.169,90.2381)→

(232.169,50.2381)→(232.115,50.2262)→(0,0)

5、问题一模型的总结

图十一：OA最短路径图图十二：OB最短路径

图十三：OC最短路径图图十四：OABCO最短路径图

二、对问题二模型的建立与求解

1、模型分析：

图十五：圆心与半径的确定

1）确定目标

由于问题二目标所求为最短时间路径，所以应以机器人从原点出发到达A点的时间T最少为目标建立优化模型：

minTO1O2OO1O2A00

1e100.1*其中，O1O2为所经路线圆弧长度，OO1和AO2为原点到圆弧、圆弧到点A的直线段长度。

2）条件约束

(1)走最短距离的时间并不一定是最短的到达时间，此时应对转弯半径，和转弯时所走圆弧的圆心进行重新搜索。圆弧的圆心和半径应满足的条件为：机器人转弯时应与障碍物保持不小于10个单位的距离：

s.t.

R

1080xE230,0y0210(2)再根据图示的关系得出下列约束条件：

d

d1

d2

L1

L2

arccos1Rarccos2222d1d2darccos212SR(2)

2、模型确立：

minTO1O2OO1O2A00100.1*1e

dd1

d2

L1

L2

arccosd1Rarccosd2

222d1d2darccos2d1d2SR(2)

3、模型求解：

根据上述建立的模型和所列直线段和弧长长度计算方法，用LINGO程序，可最终求解出：机器人过点A的圆弧圆心为（82.14140，207.9153）；

圆弧半径为12.98854；

花费的最短时间为：94.22825秒。

LINGO运行结果如下：

模型评价

模型优点：

1、针对本题障碍物较少的问题，简化了线圆相间路径的复杂结构，通过对直线圆弧相切连接最优定理的证明，使得求最短路径的问题能直接简化为寻求路线与障碍物顶点处以半径为10的圆相切时的切点问题，利用几何关系公式，求出相应切点，从而得到各路线的具体长度；

2、先通过对弧线段长度的忽略，直接由直线段长度筛选出相对较短的几条路径，可以简化后续具体切点的计算；

3、本模型能够具体得到实际的路线长度，每段小圆弧的具体路径长度都有计算在内，使模型计算的结果能够精确的体现实际情况，结果也更加准确。

模型缺点：

1、如果在实际问题中障碍物较多，出现的拐点较多，那么切点的计算和线段长度的计算就比较繁琐；

2、实际问题障碍物较多时，人工对路线的规划就会有比较大的工作量，不能体现模型的问题简洁化特点；

3、本文将机器人看成了质点，但会与实际不符；

附录：

Matlab代码汇总:

1、求解O-->A的最短路径:

x0=0;

y0=0;

x1=80;

y1=210;

a1=((x1-x0)*(x1-x0)-100);

b1=-2*(y1-y0)*(x1-x0);

c1=(y1-y0)*(y1-y0)-100;

k1=(-b1+sqrt(b1*b1-4*a1*c1))/(2*a1);

k2=(-b1-sqrt(b1*b1-4*a1*c1))/(2*a1);

ifk1

k=k2;

else

k=k1;

end

qx1=(x1+y1*k+k*k*x0-y0*k1)/(k*k+1);

qy1=k*(qx1-x0)+y0;

l1=sqrt((qx1-x0)*(qx1-x0)+(qy1-y0)*(qy1-y0));

%O点与转角

x0=300;

y0=300;

x1=80;

y1=210;

a=((x1-x0)*(x1-x0)-100);

b=-2*(y1-y0)*(x1-x0);

c=(y1-y0)*(y1-y0)-100;

k1=(-b+sqrt(b*b-4*a*c))/(2*a);

k2=(-b-sqrt(b*b-4*a*c))/(2*a);

ifk1>k2

k=k2;

else

k=k1;

end

qx2=(x1+y1*k+k*k*x0-y0*k)/(k*k+1);

qy2=k*(qx2-x0)+y0;

l2=sqrt((qx2-x0)*(qx2-x0)+(qy2-y0)*(qy2-y0));

%转角与A点

d=sqrt((qx1-qx2)*(qx1-qx2)+(qy1-qy2)*(qy1-qy2));

j=2*asin(d/(2*10));

l3=j*10;

%圆弧长

l=l1+l2+l3;

C++代码汇总:

voidoxo(doublex1,doubley1,doublexo,doubleyo,inti)

{

intchoose=0;

doublek,b1,b2,b;

if(x1==xo)

{

xq2[i]=x[i]+10;

yq2[i]=y[i];

xq1[i+1]=x[i]+10;

yq1[i+1]=yo;

return;

}

cout

cin>>choose;

if(choose==2)

{

doublek0,k1,k2,k3,l,b1,b2,c,a;

k0=(yo-y1)/(xo-x1);

l=sqrt((xo-x1)*(xo-x1)+(yo-y1)*(yo-y1));

k1=tan(atan(k0)+atan(2*10/l));//圆切圆

k2=tan(atan(k0)-atan(2*10/l));

cout

cin>>choose;

if(choose==1)k3=k1;

elsek3=k2;

b1=(yo+y1)/2-k3*(xo+x1)/2;

b2=2*k3*(b1-yo)-2*xo;//x0圆切点

a=k3*k3+1;

c=(b1-yo)*(b1-yo)+xo*xo-100;

xq1[i+1]=-b2/(2*a);

yq1[i+1]=k3*xq1[i+1]+b1;

b2=2*k3*(b1-y1)-2*x1;//x1圆切点

a=k3*k3+1;

c=(b1-y1)*(b1-y1)+x1*x1-100;

xq2[i]=-b2/(2*a);

yq2[i]=k3*xq2[i]+b1;

return;

}

k=(yo-y1)/(xo-x1);

b1=10*sqrt(1+k*k)-(k*x1-y1);

b2=-10*sqrt(1+k*k)-(k*x1-y1);

cout

cin>>choose;

if(choose==1)b=b1;

elseb=b2;

xq2[i]=(x1+y1*k-b*k)/(1+k*k);

yq2[i]=k*xq2[i]+b;

xq1[i+1]=(xo+yo*k-b*k)/(1+k*k);

yq1[i+1]=k*xq1[i+1]+b;

}

voidpxo(doublex1,doubley1,doublex0,doubley0,inti,intj)//从起始点

或终点切圆。

{

doublek1,k2,a,b,c,qx,qy;

intchoose=0;

a=((x1-x0)*(x1-x0)-100);

b=-2*(y1-y0)*(x1-x0);

c=(y1-y0)*(y1-y0)-100;

k1=(-b+sqrt(b*b-4*a*c))/(2*a);

k2=(-b-sqrt(b*b-4*a*c))/(2*a);

cout

cin>>choose;

if(choose==1)

{

qx=(x1+y1*k1+k1*k1*x0-y0*k1)/(k1*k1+1);

qy=k1*(qx-x0)+y0;

}

else

{

qx=(x1+y1*k2+k2*k2*x0-y0*k2)/(k2*k2+1);

qy=k2*(qx-x0)+y0;

}

cout

if(j==1)

{

xq1[i]=qx;

yq1[i]=qy;

}

else

{

xq2[i]=qx;

yq2[i]=qy;

}

问题二求最短时间路径lingo程序

min=@sqrt((x-300)^2+(y-300)^2-r^2)/5+@sqrt(x^2+y^2-r^2)/5+2*r*@asin(d/(2*r))/5*(1+@exp(10-0.1*r^2));

d=@sqrt((xc-xb)^2+(yc-yb)^2);

@sqrt((xb)^2+(yb)^2+r^2)=@sqrt(x^2+y^2);

@sqrt((xc-300)^2+(yc-300)^2+r^2)=@sqrt((x-300)^2+(y-300)^2);

init:

x=80;

y=210;

endinit

yc>210;

r=@sqrt((xb-x)^2+(yb-y)^2);r=@sqrt((xc-x)^2+(yc-y)^2);r>@sqrt((80-x)^2+(210-y)^2)+10;x>80;

摘要

本文主要运用直线逼近法、直线圆弧相切连接最优定理等方法来解决机器人避障问题。对于问题一：

建模结果：

1、O→A的最短路径长为:471.0372；

圆弧的圆心坐标为：(80,210)。2、O→B的最短路径长为：853.7；

圆弧的圆心坐标为：(60,300),(150,435),(150,600),(220,470),(220,530)。3、O→C的最短路径长为：1088.15；

圆弧的圆心坐标为：(230,60),(410,100),(500,200),(720,520),(720,600)。4、O→A→B→C→O最短路径长为：2724.896。

圆弧的圆心坐标为：

(80,210)(220,530),(150,600),(270,680),(370,680),(430,680),(540,730),(670,730),(230,60),(410,100),(500,200),(720,520),(720,600)对于问题二：

通过对机器人过弯规律的分析可知，针对最短时间路径问题：(1)在障碍物拐点处的圆弧半径为临界半径;

(2)因为直线速度大于vv()1e100.1转弯速度,所以在不转弯的地方尽可能走直线;

建模结果：转弯半径：12.9885；

O→A的最短时间路径时间长：94.2283；

相应圆弧的圆心坐标为（82.1414，207.9153）。

关键词：机器人避障、最短路径、直线圆弧连接相切最优、优化。

问题重述

障碍物名称正方形圆形平行四边形三角形正方形三角形长方形平行四边形长方形正方形正方形长方形

(360,240)(280,100)(80,60)(60,300)(0,470)(150,600)(370,680)(540,600)(640,520)(500,140)

表1:障碍物数学描述表

机器人直线行走的最大速度为v05个单位/秒。机器人转弯时，最大转弯速度为

vv()

100.1

1e

左下顶点坐标(300,400)

其它特性描述边长200

圆心坐标(550,450)，半径70底边长140，左上顶点坐标(400,330)上顶点坐标(345,210)，右下顶点坐标(410,100)

边长150

上顶点坐标(150,435)，右下顶点坐标(235,300)

长220，宽60

底边长90，左上顶点坐标(180,680)

长60，宽120边长130边长80长300，宽60

其中是转弯半径。如果超过该速度，机器人将发生侧翻，无

法完成行走。

图1：原始障碍物图

问题分析

针对问题一：为达到要求,我们按照以下原则选择路径:

(1)由直线圆弧相切连接最优可知过弯时在障碍物拐点处的圆弧半径为临界半径，则可认为以最小半径10的圆过弯;

(2)因为直线速度大于转弯速度,所以在不转弯的地方尽可能走直线;按照上述原则,我们选取以下步骤求最短路径:

模型假设

1、假设机器人为一可运动的质点，即质点机器人不考虑其外形尺寸；

2、假设半径不变时，机器人在行进、转弯过程中能一直保持最大速度；3、假设机器人可准确执行运动轨道，无任何偏差；

4、假设机器人的行进速度可瞬时加减变化，不受条件限制。

符号说明

hijDijDhjTR

（h、i、j定义同上）

机器人的行走路径上的障碍物的顶点。

（i、j定义同上）

机器人在O→A→B→C→O环道中的各线

切点。（h、j定义同上）

从O到A的时间

大圆半径

模型准备

1、建立机器人运动坐标系：

图一

3、证明“直线圆弧相切连接最优”这一猜想。

DOPS

AEQS

图二

4、障碍物为圆的情况

图三

rd

（其中r为障碍物

模型的建立与求解

一、对问题一模型的建立与求解

该问题要求路径最短，即不要求速度与时间，同时由直线圆弧相切连接最优可得：

本论文问题一求路径最短可采用10单位过弯半径，即以半径为10个单位的圆弧过弯可满足两点避障过弯最短问题。

1、机器人从O(0,0)出发，O→A的最短路径

已知机器人所走路线为直线或圆弧，通过规划可得实际得如下（图四）四种避障行进方

案：

图四

1）当机器人以一个连续圆弧过弯，即选择路线二或路线四；

2）当机器人以直线—圆弧—直线的方式过弯，即有以10单位半径过弯模式的线路一

和三；

由直线圆弧相切连接最优可筛选出相对较短路径：路线一和路线三。通过matlab可以得出两条路径直线与圆弧相加的具体实际长度：

图五、路线一图六、路线三

圆弧的圆心坐标为(80,210)。路线一（Da11、Da12）和路线三Da31、Da32）的切点坐标：

点号

坐标

Da1170.5060213.1406

Da1276.6064219.4066

表1

Da31232.114950.2262

Da32239.704357.5862

路线一和路线三的实际具体长度：

线号长度

La11La12La13

总长度

224.4994237.4868

9.051011.1391

表2

237.4868249.7999

471.0372498.4259

由此可见：

O→A的最短路径长为：471.0372；圆弧的圆心坐标为(80,210)；路径坐标：

(0,0)→(70.5060,213.1406)→(76.6064,219.4066)→(300,300)。2、机器人从O(0,0)出发，O→A的最短路径

由已知机器人所走路线为直线或圆弧，通过规划可得实际得如下（图七）三种避障路线方案：

图七

由于方案较多，可预先进行粗略筛选（表三）：

路线名称路线一路线二路线三

段一305.7

8224.50235.37

段二162.25175.70240.05

段三73.65230.50230.50

表3

段四6096.9596.95

段五96.95111.36111.36

段六111.36//

总长810839914.22

Lb11

长度

305.78

Lb12

4.20

Lb13

166.36

Lb14

7.58

Lb15

75.66

Lb16

12.62

表4

Lb1760

Lb18

9.89

Lb19

94.2

Lb110

6.05

Lb111

111.36

总长853.70

Lb21

长度

224.5

Lb22

8.37

Lb3

178.12

Lb24

12.22

Lb25

230.49

表

Lb26

9.89

Lb2796.95

Lb28

6.15

Lb29

111.36

总长878.05

所以可以得到O→B的最短路径长为：853.7。

圆弧的圆心坐标为：(60,300),(150,435),(150,600),(220,470),(220,530)路径坐标：

(0,0)→(50.1354,301.6396)→(51.6795,305.547)→(141.6795,440.547)→

(147.9621,444.79)→(222.038,460.2096)→(230,470)→(230,530)→(229.9563,530.9242)→(229.5746,532.8954)→(229.2564,533.7791)→(229.1263,534.0782)→

(225.4971,538.3544)→(144.5036,591.6465)→(140.6922,596.346)→(100,700)3、机器人从O(0,0)出发，O→C的最短路径

图八图九

由C++编程可得路径一和路径二的实际长度：

Lc11

长度

237.49

Lc12

0.06

Lc13

184.39

Lc14

7.44

Lc15

133.07

Lc16

0.69

表6

Lc17387.82

Lc18

Lc19

6.76

Lc110

43.6

总长1088.15

Lc21

长度

237.49

Lc22

7.85

Lc23

187.96

Lc24

9.15

Lc25

156.63

Lc26

8.12

表7

Lc27

356.09

Lc28

Lc29

6.76

Lc210

43.6

总长1101.23

所以可以得到O→C的最短路径长为：1088.15。

圆弧的圆心坐标为：(230,60),(410,100),(500,200),(720,520),(720,600)路径坐标：

(0,0)→(232.115,50.2262)→(232.169,50.2381)→(412.169,90.2381)→

(418.245,94.5361)→(491.755,205.464)→(492.072,206.074)→(727.928,513.926)→(730,520)→(730,600)→(727.718,606.359)→(700,640)

4、机器人从O(0,0)出发，O→A→B→C→O的最短路径

图十

由C++编程可得O→A→B→C→O最短路径的实际长度：2724.896。圆弧的圆心坐标为：

(80,210)(220,530),(150,600),(270,680),(370,680),(430,680),(540,730),(670,730),(230,60),(410,100),(500,200),(720,520),(720,600)；路径坐标：

(0,0)→(70.5060,213.1406)→(76.6064,219.4066)→(300,300)→(229.572，532.895)→(225.421,538.166)→(144.579,591.834)→(140.692,596.346)→(100,700)→

(492.072,206.074)→(491.755,205.464)→(418.245,94.5361)→(412.169,90.2381)→

(232.169,50.2381)→(232.115,50.2262)→(0,0)

5、问题一模型的总结

图十一：OA最短路径图图十二：OB最短路径

图十三：OC最短路径图图十四：OABCO最短路径图

二、对问题二模型的建立与求解

1、模型分析：

图十五：圆心与半径的确定

1）确定目标

由于问题二目标所求为最短时间路径，所以应以机器人从原点出发到达A点的时间T最少为目标建立优化模型：

minTO1O2OO1O2A00

1e100.1*其中，O1O2为所经路线圆弧长度，OO1和AO2为原点到圆弧、圆弧到点A的直线段长度。

2）条件约束

s.t.

R

1080xE230,0y0210(2)再根据图示的关系得出下列约束条件：

d

d1

d2

L1

L2

arccos1Rarccos2222d1d2darccos212SR(2)

2、模型确立：

minTO1O2OO1O2A00100.1*1e

dd1

d2

L1

L2

arccosd1Rarccosd2

222d1d2darccos2d1d2SR(2)

3、模型求解：

根据上述建立的模型和所列直线段和弧长长度计算方法，用LINGO程序，可最终求解出：机器人过点A的圆弧圆心为（82.14140，207.9153）；

圆弧半径为12.98854；

花费的最短时间为：94.22825秒。

LINGO运行结果如下：

模型评价

模型优点：

2、先通过对弧线段长度的忽略，直接由直线段长度筛选出相对较短的几条路径，可以简化后续具体切点的计算；

3、本模型能够具体得到实际的路线长度，每段小圆弧的具体路径长度都有计算在内，使模型计算的结果能够精确的体现实际情况，结果也更加准确。

模型缺点：

1、如果在实际问题中障碍物较多，出现的拐点较多，那么切点的计算和线段长度的计算就比较繁琐；

2、实际问题障碍物较多时，人工对路线的规划就会有比较大的工作量，不能体现模型的问题简洁化特点；

3、本文将机器人看成了质点，但会与实际不符；

附录：

Matlab代码汇总:

1、求解O-->A的最短路径:

x0=0;

y0=0;

x1=80;

y1=210;

a1=((x1-x0)*(x1-x0)-100);

b1=-2*(y1-y0)*(x1-x0);

c1=(y1-y0)*(y1-y0)-100;

k1=(-b1+sqrt(b1*b1-4*a1*c1))/(2*a1);

k2=(-b1-sqrt(b1*b1-4*a1*c1))/(2*a1);

ifk1

k=k2;

else

k=k1;

end

qx1=(x1+y1*k+k*k*x0-y0*k1)/(k*k+1);

qy1=k*(qx1-x0)+y0;

l1=sqrt((qx1-x0)*(qx1-x0)+(qy1-y0)*(qy1-y0));

%O点与转角

x0=300;

y0=300;

x1=80;

y1=210;

a=((x1-x0)*(x1-x0)-100);

b=-2*(y1-y0)*(x1-x0);

c=(y1-y0)*(y1-y0)-100;

k1=(-b+sqrt(b*b-4*a*c))/(2*a);

k2=(-b-sqrt(b*b-4*a*c))/(2*a);

ifk1>k2

k=k2;

else

k=k1;

end

qx2=(x1+y1*k+k*k*x0-y0*k)/(k*k+1);

qy2=k*(qx2-x0)+y0;

l2=sqrt((qx2-x0)*(qx2-x0)+(qy2-y0)*(qy2-y0));

%转角与A点

d=sqrt((qx1-qx2)*(qx1-qx2)+(qy1-qy2)*(qy1-qy2));

j=2*asin(d/(2*10));

l3=j*10;

%圆弧长

l=l1+l2+l3;

C++代码汇总:

voidoxo(doublex1,doubley1,doublexo,doubleyo,inti)

{

intchoose=0;

doublek,b1,b2,b;

if(x1==xo)

{

xq2[i]=x[i]+10;

yq2[i]=y[i];

xq1[i+1]=x[i]+10;

yq1[i+1]=yo;

return;

}

cout

cin>>choose;

if(choose==2)

{

doublek0,k1,k2,k3,l,b1,b2,c,a;

k0=(yo-y1)/(xo-x1);

l=sqrt((xo-x1)*(xo-x1)+(yo-y1)*(yo-y1));

k1=tan(atan(k0)+atan(2*10/l));//圆切圆

k2=tan(atan(k0)-atan(2*10/l));

cout

cin>>choose;

if(choose==1)k3=k1;

elsek3=k2;

b1=(yo+y1)/2-k3*(xo+x1)/2;

b2=2*k3*(b1-yo)-2*xo;//x0圆切点

a=k3*k3+1;

c=(b1-yo)*(b1-yo)+xo*xo-100;

xq1[i+1]=-b2/(2*a);

yq1[i+1]=k3*xq1[i+1]+b1;

b2=2*k3*(b1-y1)-2*x1;//x1圆切点

a=k3*k3+1;

c=(b1-y1)*(b1-y1)+x1*x1-100;

xq2[i]=-b2/(2*a);

yq2[i]=k3*xq2[i]+b1;

return;

}

k=(yo-y1)/(xo-x1);

b1=10*sqrt(1+k*k)-(k*x1-y1);

b2=-10*sqrt(1+k*k)-(k*x1-y1);

cout

cin>>choose;

if(choose==1)b=b1;

elseb=b2;

xq2[i]=(x1+y1*k-b*k)/(1+k*k);

yq2[i]=k*xq2[i]+b;

xq1[i+1]=(xo+yo*k-b*k)/(1+k*k);

yq1[i+1]=k*xq1[i+1]+b;

}

voidpxo(doublex1,doubley1,doublex0,doubley0,inti,intj)//从起始点

或终点切圆。

{

doublek1,k2,a,b,c,qx,qy;

intchoose=0;

a=((x1-x0)*(x1-x0)-100);

b=-2*(y1-y0)*(x1-x0);

c=(y1-y0)*(y1-y0)-100;

k1=(-b+sqrt(b*b-4*a*c))/(2*a);

k2=(-b-sqrt(b*b-4*a*c))/(2*a);

cout

cin>>choose;

if(choose==1)

{

qx=(x1+y1*k1+k1*k1*x0-y0*k1)/(k1*k1+1);

qy=k1*(qx-x0)+y0;

}

else

{

qx=(x1+y1*k2+k2*k2*x0-y0*k2)/(k2*k2+1);

qy=k2*(qx-x0)+y0;

}

cout

if(j==1)

{

xq1[i]=qx;

yq1[i]=qy;

}

else

{

xq2[i]=qx;

yq2[i]=qy;

}

问题二求最短时间路径lingo程序

min=@sqrt((x-300)^2+(y-300)^2-r^2)/5+@sqrt(x^2+y^2-r^2)/5+2*r*@asin(d/(2*r))/5*(1+@exp(10-0.1*r^2));

d=@sqrt((xc-xb)^2+(yc-yb)^2);

@sqrt((xb)^2+(yb)^2+r^2)=@sqrt(x^2+y^2);

@sqrt((xc-300)^2+(yc-300)^2+r^2)=@sqrt((x-300)^2+(y-300)^2);

init:

x=80;

y=210;

endinit

yc>210;

r=@sqrt((xb-x)^2+(yb-y)^2);r=@sqrt((xc-x)^2+(yc-y)^2);r>@sqrt((80-x)^2+(210-y)^2)+10;x>80;

相关内容

热门内容

标签