机器人避障问题

2014全国大学生数学建模竞赛承诺书我们仔细阅读了中国大学生数学建模竞赛的竞赛规则。我们完全明白，在竞赛开始后参赛队员不能以任何方式（包括电话、电子邮件、网上咨询等）与队外的任何人（包括指导教师）研究、讨论与赛题有关的问题。我们知道，抄袭别人的成果是违反竞赛规则的，如果引用别人的成果或其他公开的资料（包括网上查到的资料），必须按照规定的参考文献的表述方式在正文引用处和参考文献中明确列出。我们郑重承诺，严格遵守竞赛规则，以保证竞赛的公正、公平性。如有违反竞赛规则的行为，我们将受到严肃处理。我们参赛选择的题号是（从A/B/C/D中选择一项填写）：我们的参赛报名号为（如果赛区设置报名号的话）：所属学校（请填写完整的全名）：参赛队员（打印并签名）：1.指导教师或指导教师组负责人（打印并签名）：日期：2014年7月17日赛区评阅编号（由赛区组委会评阅前进行编号）：2014全国大学生数学建模竞赛编号专用页赛区评阅编号（由赛区组委会评阅前进行编号）：赛区评阅记录（可供赛区评阅时使用）：评阅人评分备注全国统一编号（由赛区组委会送交全国前编号）：全国评阅编号（由全国组委会评阅前进行编号）：机器人避障问题摘要本文主要以机器人在有12个障碍物的区域中如何避开障碍物及制定行径路径为研究对象，在研究过程中，运用近似求解的思路，建立简单模型，并有效利用MATLAB.Lingo等软件进行程序的编辑，最终得出了较为准确的结论。首先在本文中，由题目分析可得，机器人的路径是由圆弧和直线段组成，由于直线路径的长度一般来说远大于圆弧路径，所以可以先将经过障碍物顶点时的转弯圆弧看做点，及以折线为路径，运用MATLAB编程，确定出最短距离时所要经过的障碍物的大致路径，再精确求解路径。最后求解得到的结论：O-A、O-B、O-C和O-A-B-C-O的最短路径分别依次为:467.26、853.8890、1086.45、2715.24。由于此问题涉及到最短时间，所以在解题时应考虑到距离和速度。通过观察发现，机器人在圆弧上行走时速度随圆弧半径的增大而不断增大。因此，在。一A的路径尽量短的前提下，应尽量增大圆弧的半径长度。由问题1可知，机器人从O-A的最短路径由两条直线段和一条圆弧组成，故建立最短时间路径优化模型，通过Lingo编程求解得到结论：O-A的最短时间路径总距离为471.1290个单位，总时间为94.22825秒，圆弧的起点坐标为(69.80454,211.9779)，终点坐标为(77.74918,220.1387)，圆心坐标为(82.1414,207.9153)最后利用MATLAB绘制图形，将各坐标代入图形中，验证结果正确。关键词：最短路径关键词：最短路径线圆结构MATLABLINGO一、问题重述1.1问题背景：如图1所示，有一800X800的平面场景图，在原点0(0,0)点处有一个机器人，它只能在该平面场景范围内活动。图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物，障碍物的数学描述如表1所示。在图1的平面场景中，障碍物外指定一点为机器人要到达的目标点(要求目标点与障碍物的距离至少超过10个单位)。规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。机器人直线行走的最大速度为％=5个单位/秒。机器人转弯时，最大转弯速度为：u=v(p)=―—，其中P是转弯半径。如果超过该速度，机器人将发1+eio-0.1p2生侧翻，无法完成行走。1.2问题提出：请建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。对场景图中4个点0(0,0)，A(300,300)，B(100,700)，C(700,640)，具体计算：机器人从0(0,0)出发，Of0-B、0-C和0fA—BfCf0的最短路径。机器人从0(0,0)出发，到达A的最短时间路径。注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总距离和总时间。二、问题分析2.1问题一通过对题目的分析，在处理问题一时，先将经过障碍物顶点时的转弯圆弧看做点，及以折线为路径，运用lingo编程，确定出最短距离时所要经过的障碍物的大致路径，再精确求解路径。题目要求目标点与障碍物的距离至少超过10个单位，所以将障碍物的尺寸扩大10个单位，并用CAD画出扩大后的障碍物，特别注意在障碍物顶点处使用的是圆弧。在处理问题一时，先将经过障碍物顶点时的转弯圆弧看做点，及以折线为路径，运用lingo编程，确定出最短距离时所要经过的障碍物的大致路径，再精确求解路径2.2问题二对于最短时间路径分析，首先需要分析路径的选取，然后根据路径的大体情况通过建立一个以所用时间最少为目标的最优化模型。由问题1可知，机器人由O点到A点的路径需要转弯一次来完成，考虑路径时只需要一次转弯就能完成，并且转弯时不仅增加路程，同时速度将会减少，所以路径应减少转弯，因此可以确定路径是由2条支线路径与1条圆弧路径组成的。然后，根据转弯速度公式分析可知，转弯半径越大，转弯速度越大，因此在转弯时应适当增大转弯半径。所以在解决时，可根据问题1中路径的距离、切点的坐标以及圆弧圆心坐标的位置范围作为约束条件进行分析，建立最优化模型，最后利用Lingo软件进行求解。三、模型假设（1）假设机器人在行走时看做质点，本身大小不计。（2）假设机器人开始行走时不需要经过加速，即小车开始行走时就以最大速度为v5个0单位/秒行走。（3）假设机器人行走过程中，本身不发生故障，即行走顺利。（4）假设机器人行走区域为二维区域。（5）假设机器人有直线行走过渡到曲线路径时无需过度时间。、符号说明v，第/个特征点v机器人转弯行走的速度X，第i个特征点的X坐标v七第i个特征点的V坐标Xi第i个特征点对应的障碍物的顶点的X坐标Y…i第I个特征点对应的障碍物的顶点的V坐标d〜—，」，〜—，—一j第I个特征点到第j个特征点的距离L起点到目标点的路径长度T机器人的行走时间v0机器人直线行走的速度五、模型建立5.1问题一由题意可知，机器人从起点（如O点）到目标点（A）之间不能以直线路径直接到达，在行走过程中必然会经过障碍物，且在经过障碍物时要以弧线过渡，可有几何关系的，机器人的行走路径避开障碍物的顶点才能的到最短路径。由于题目要求目标点与障碍物的距离至少超过10个单位，因此将障碍物的尺寸扩大10个单位，并用CAD画出扩大后的障碍物，特别注意在障碍物顶点处使用的是圆弧。标记处特征点，首先将路径中的圆弧收缩至一个点，以特征点代替，这样机器人行走路径就可以先看做折线组成，这样问题就简化成了求点之间的最短路径。先用这样的方法粗略估计出最短路径要经过的特征点，得知路径后再运用几何关系、勾股定理等来精确求解最短路径。5.1.1特征点的定义对于特征点的定义，在CAD画图扩大障碍物时，顶点部分使用圆弧，对于角形、矩形、平行四边行，以其顶点为圆心做坐标系，将>轴向、轴的负方向旋转「4与圆弧的交点定义为特征点。对于圆形障碍物2,过其圆心左两条分别平行于、轴与>轴的线，将这两条与圆相交的四个点标记为特征点。

iilI图2障碍物扩大图5.1.2u、iilI图2障碍物扩大图根据CAD画图，画出新的障碍物边界，将三角形、矩形、平行四边行的个标记为特征点V,,对于圆形障碍物2,过其圆心左两条分别平行于x轴与y轴的线，将这两条与圆相交的四个点标记为特征点。特征点的取值如下：（1）x=X+10xcos%

y=Y+10xcos%

（1）表2各特征点定点标号及对应x和y值标号X轴Y轴标号X轴Y轴标号X轴Y轴V100V16100700V31727513V27353V17363673V32727607V323753V18437663V33677697V4237217V19293607V34677737V573217V20293393V35700640V6300300V21497390V36393337V7242307V22550370V37353233V853293V23630450V38507233V9150445V24550530V39547337V10227463V25510520V4027393V11227537V26507607V4141793V12143593V27533737V42345217V13247593V28533593V43493133V14277687V29633593V44493207V15173687V306335175.1.3特征点简单的距离根据已知条件，场景图内任意两个特征点之间的距离可有如下公式计算：d=匕:J—x）+G—y）（3）ij'ijij运用MATLAB编程，算出各点间的距离（各个特征点之间的距离和特征点跟目标点之间的距离）。

两点间的距离dij331.267248.4713228.9498241.6982起点VVVVVi1315终点VVVVVj3656两点间的距离dij104635.260674.1485254.0492起点VVVVVi143153终点VVVVVj1514167两点间的距离dij156.719574100.9554115.3170起点VVVVVi7101112终点VVVVVj10111216两点间的距离dij191.4706297.7549180.313679.0759起点VVVVVi51810终点VVVVVj7899两点间的距离dij341.7601160.4307227.547891起点VVVVVi5362231终点VVVVVj36223132两点间的距离dij53.8145271.9118143.3876162.7882起点VVVVVi3364042终点VVVVVj40404238两点间的距离dij356.0899427.2447起点VVVi381终点VVVj3141六、模型求解6.1问题一求解O—A1、确定路径运用直观思维，观察出机器人在到达目标点A可能经过哪几个特征点时路径可能最短。通过CAD绘图如下：□□SOfl600□JOft200□□SOfl600□JOft200u2001。。600800□:1□图3O-A的路径O—七一a的总路径：Q1=228.9498+241.6982=470.648O—七一a的总路径：Q2=331.267+248.4713=579.7383经过比较，发现：Q>Qi,由此可见从°点到A点的最短路径方案应该是O—V3一A2、精确求解由CAD画出由O点到A点的局部如下:

图5O点到A点的局部图设起点O（0.0）路径与圆弧的切点为D，终点路径A（300,300）与圆弧的切点为C，其中P点为障碍物5左上角的的顶点，其坐标为（80,210），由勾股定理有：AC=x；同理：OD=［：:一x)+，一y)_p2o同理：OD=［：又因为：ZCPD=2兀一/APC一/APO一么OPD则:ZCPD=2兀一arccos一ZCPD=2兀一arccos一arccos=2X\«P)()一arccos"一*x—o〃:一x)+G一y)_p2apap+(y,-七\一P2*：'-P2又由弧长定理的：CD=pxZCPD带入数值求得：AC=237.4868,OD=224.4994,带入数值求得：AC=所以总路径L=AC+OD+CD=467.26机器人行走经过的点及总路径见如下表:

起点坐标终点坐标圆弧圆心圆弧半径机器人行走总长(L)O—A线段一(0，0)(71,213)(80,210)=10L=467.26弧线一(71,213)(77,220)线段二(77,220)(300,300)表4机器人行走经过的点及总路径求解O-B1、确定路径运用直观思维，观察出机器人在到达目标点B可能经过哪几个特征点时路径可能最短。通过CAD绘图如下：图7O—B路径与从O到A计算过程一样，带入数据，可以算出:O一,3—"14一"15—B的总路径：L1=331.267+635.2606+104+74.1485=1.1447e+003O—"3一"7一"10一"11一"12—B的总路径：L2=331.267+254.0492+156.7195+74+100.9554+115.3170=1.0323e+003O—V5一V7一V10一V11一V12—B的总路径:L3=228.9498+191.4706+156.7195+74+100.9554+115.3170=867.4123O—V8一V9一V10一V11一V12—B的总路径：L4=297.7549+180.3136+79.0759+74+100.9554+115.3170=847.4168经过比较，发现：L>L>L>L1234由此可见从。点到A点的最短路径方案应该是：O—V8—V9—V10—V11—V12—B3、精确求解由CAD画出由O点到A点的局部图，机器人的路径由6段直线与6段弧线组成，算法与从O点到A点的算法相同。机器人行走总长及经过的其各点坐标见如下表:表5机器人行走经过的点及路径总长起点坐标终点坐标圆弧圆心坐标圆弧半径机器人行走总长LO—B线段一(0,0)(50.5,302)=10853.8890圆弧一(50.5,302)(52,306)(60,300)线段二(52,306)(141.7,439.6)圆弧二(141.7,439.6)(150,444.5)(150,434)线段三(150,444.5)(222.2,460.3)圆弧三(222.2,460.3)(230,470)(220,470)线段四(230,470)(230,530)圆弧四(230,530)(225.5,538.4)(220,530)线段五(225.5,538.4)(144.5,592)圆弧五(144.5,592)(141，596.4)(150,600)线段六(141，596.4)(100,700)求解O—C1、确定路径运用直观思维，观察出机器人在到达目标点B可能经过哪几个特征点时路径可能最短。通过CAD绘图如下：图9到达B可能路径根据公式（3）可以算出:

。一V5一V36一V22一V31一V32—B的总路径：L1=228.9498+341.7601+160.4307+125.6+115.6633+91=1.0634e+003VVVVVV。一3一40一36一22一31一32—B的总路径：L2=331.267+53.8145+271.9118+341.7601+160.4307+227.5478+91=1.4777e+003VVVVVV。一3一40一42一38一31一32—B的总路径：L3=331.267+53.8145+143.3876+162.7882+356.0899+227.5478+91=1.3659e+003O—V41一V38一V31一V32—B的总路径:路径。一BL4=427.2447+162.7882+356.0899+227.5478+91=1.2647e+003路径。一BL4=427.2447+162.7882+356.0899+227.5478+91=1.2647e+003图10机器人到达C点的最短路径机器人的行走过程中经过的点及总路径见如下表:表6机器人的行走过程中经过的点及总路径起点坐标终点坐标圆弧圆心坐标圆弧半径机器人行走总长L0T线段一(0,0)(422.1,90)=10998.0381圆弧一(422.1,90)(428.8,95)(420,100)线段二(428.8,95)(491.3,204.7)圆弧二(491.3,204.7)(492.3,206.3)(500,200)线段三(492.3,206.3)(728,514)圆弧三(728,514)(720,520)(720,520)线段四(720,520)(730,520)圆弧四(730,520)730,600(720,600)线段五730,600(728,606)求解OfA—BfCfO0-A,C-O的最短路径已由6.1.1和6.1.3中求解出来，对于求解O-A-BfC—0的最短路径，只需将A-C和C-B的最短路径分别求解出来，方法与求解O-C相同，先通过直观观察出几条可能的路线，确定路径后再精确求解。1、确定路径运用直观思维，观察出机器人在从0经过A点B点和C点回到0点可能经过哪几个特征点时路径可能最短。通过CAD绘图如下：

图11机器人回到O点路径4、精确求解在从O—A-B-C-O的路径里，机器人所经过的点如下：表6机器人的行走过程中经过的点及总路L起点坐标终点坐标圆弧圆心坐标圆弧半径机器人行走总长LO一A一B一C一线段一(0,0)(72,213)=102714.3圆弧一(72,213)(77,220)(80,210)线段二(77,220)(291,297)圆弧二(291,297)(297,309)(288,306.2)线段三(297,309)(223,533)圆弧三(223,533)(226,539)(220,530)线段四(226,539)(145,592)圆弧四(145,592)(141,596)(150,600)线段五(141,596)(106,686)圆弧五(106,686)(116,700)(115,689)线段六(116,700)(271,690)圆弧六(271,690)(297,689.8)(270,680)O线段七(297,689.8)(367,670)069圆弧七(367,670)(369.3,670)(369.3,680)利用几何关系精确求解的L=2714.30696.2问题二题目要求机器人从O(0,0)出发，到达A的最短时间路径。如图，由第一问求得路径延路径l行走是是最短路径。图12到达A点的路径1、T的确定由路径图12可知，路径由直线路径a、b和圆弧路径c组成，根据问题分析，可得机器人从O点到A点的时间函数为：T=(^+£(4)%v其中，VQ=5个单位/秒，v=v(p)=1——2，a，b,c位路径长度。2、约束条件(1)约束分析利用AUTOCAD画出切线部分局部图，假设圆弧的圆心其坐标为G,y)，半径为p，切线起点坐标为(X,y)，切线终点坐标为(x,y)，因此对以下的方面进aabb行约束。对路径a、b、c的距离进行约束：路径a的距离：a=.((2+y2)—p2(由勾股定理得)利用几何关系将切点(x^,y「表示出来为(x—pxcosm,y+pxsinm)，利用两点间的公式将距离a表示出来：a=%(x-pxcosm》+(y+psinm1路径b的路径距离：b=y'(300—x》+(300—y:—p2(由勾股定理得)同理利用几何关系将切点(xb,yb)表示出来(300—y—pxcosn,300—x+pxsin(n))，利用两点间的公式将距离a表示出来：b=凶00-y—pxcosn2+G00-x+psinn)路径c的距离：c=10x(k/2—m—n)对切点进行约束：首先对切点的坐标进行约束，根据圆弧c的局部示意图(13)进行分析，则两个切点的坐标分别为：图13切点分析图x=x—pxcos(m)y=y—pxcos(m)<ax=x—pxcos(n)b.一，、y=y+pxcos(n)其次由于切点在圆弧上，根据题意机器人距离障碍物的长度不得少于10个单位，再结合局部示意图，对切点的位置进行约束为：](x-80E+(y-2101>10aa土-80》+£-210》>10'x<80ya>210<a…x>230b、yb>210根据题目要求，目标点与障碍物的距离至少超过10个单位，对圆弧半径进行限制，要求转弯时的转弯半径,要大于10个单位：p-G-80》+(y-210*>10根据对问题分析确定圆弧圆心的坐标点在障碍物中，因为圆弧圆心在障碍物内，所以对圆弧圆心做限定：180<x<230[60<y<2103)lingo求解通过上述准备，以机器人从O点到A点所用的最短时间为目标，根据以上约束分析，建立一个最优化模型为：T(a+b)+J1+10-0邛2xc(~~¥—a=tx2+y2，一p2b=\侦0-x*+G00-y*a=、；Cx-P2+psinm*\J300-y-pxcosn*+600-x+psinn2-pxcosm*+(yc=10x(K/2-m-n)t'G-80*+(y-210*>10VJaa山-80*+《-210*>10-80*+(y-210*>10P-x=x-pxcos(m)ay=y-pxcos(m)x=x-pxcos(n)by=y+pxcos(n)80<x<23060<y<210%<80y>210lb将目标函数和约束条件输入lingo软件中，利用LINGO进行求解，解得最短时间为94.22825，3段路径长度分别为223.1755、236.1636、11.78992，圆弧半径为12.98854,圆心坐标为（82.14140，207.9153），切点的坐标为（69.80454,211.9779）（77.74918，220.1387）。七、模型评价⑴模型的优点：在此题中建立的模型有效地利用了众多约束条件，且在模型建立过程中，着重考虑了直线路径与圆弧路径的切点位置、圆弧圆心的位置以及路径长度等方面，使通过模型求得的结果更加准确，同时很好的运用了MATLAB以及LINGO这些软件，给问题的解决提供了帮助。⑵模型的缺点：在模型建立的过程中，还是存在一些小问题，例如为了使模型建立起来变得简单，做了许多假设，假设机器人从两种状态过渡时没有加速度，可直接达到应具有的速度，但在实际生活中，这根本是理想状态，所以，我们仍需和实际紧密联合，才能做得完美。参考文献戴光明.避障路径规划的算法研究[D].华中科技大学.计算机科学与技术学院，2004孙翔.MATLAB7.0基础教程[M].北京:清华大学出版社，2005

九、附录表1障碍物的数学描述:编号障碍物名称左下顶点坐标其它特性描述1正方形(300,400)边长2002圆形圆心坐标(550,450)，半径703平行四边形(