机器人避障问题论文

18/20机器人避障问题摘要本文通过在给定的平面场景范围内对机器人就如何躲避12个不同形状障碍物区域的避障行走问题进行探究，在出发点到目的点的多种情形中进行选择，并根据要求，保证所走的路线为直线段和圆弧。继而探究避障的最短路径及最短时间路径的数学模型，在此探究过程中，运用穷举法，进行各种行走路线的CAD绘图，利用平面几何的点、线、圆的关系求解行走路径所经过点的坐标、线段长度、和弧长，在各总长度中进行比较，找出最短路径。最终，根据机器人速度的数据，建立最短时间路径的数学模型，运用LINGO软件最终求出最短时间的路径。针对问题一，根据题意，为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，可分别以障碍物的边界处绘制以10为半径的圆，从而确定安全的可行走的活动范围。利用平面几何知识，在给定的障碍物的坐标的基础上，求解各路径下的直线段和圆弧的长度并加总求和，利用平面几何的知识，假设未知切点坐标和圆心，以及根据固定点的坐标，建立模型，求解路线的距离。进而，比较同一目的地不同路径的总长度，最终，求得最短路径，结果如下所示：OA:471.03;OB:853.77;OC:1055.063;OABCO:2701.932对于求解OABCO路径时，将路径划分为若干个切线圆结构来求解，建立目标函数，并利用目标函数建立优化方程组，运用LINGO软件，求解确定过A,B,C点圆弧不同圆心的坐标针对问题二，由于转弯速度的不同，在问题一的基础上求解出转弯半径的取值范围，建立以转弯半径为变量的最短时间路径模型，并通过LINGO软件求解，并通过CAD软件做出求解路径的具体图形。关键词：避障最短路径穷举法CADLINGO平面几何优化模型目录摘要 11问题重述 12问题分析 23模型假设 24符号说明 25模型的建立与求解 35.1问题一的模型 35.1.1模型建立 3模型I 35.1.2模型求解 75.2问题二的模型 145.2.1模型建立 14模型II 165.2.2模型求解 166模型的评价与改进 177参考文献 171问题重述在一个800*800的平面场景图中，如图，在已知的12个不同形状障碍物的坐标区域，机器人从以原点（0,0）出发前往不同的目标点，并且不能与障碍物发生碰撞。障碍物的数学描述如下表所述：编号障碍物名称左下顶点坐标其它特性描述1正方形(300,400)边长2002圆形圆心坐标(550,450)，半径703平行四边形(360,240)底边长140，左上顶点坐标(400,330)4三角形(280,100)上顶点坐标(345,210)，右下顶点坐标(410,100)5正方形(80,60)边长1506三角形(60,300)上顶点坐标(150,435)，右下顶点坐标(235,300)7长方形(0,470)长220，宽608平行四边形(150,600)底边长90，左上顶点坐标(180,680)9长方形(370,680)长60，宽12010正方形(540,600)边长13011正方形(640,520)边长8012长方形(500,140)长300，宽60在机器人的行进过程中，规定机器人所走的路径为直线和圆弧所组成（不可有折线转弯），其中与直线相切的圆弧为不与障碍物发生碰撞的转弯路径，也可以由多个相切的圆弧路径组成，每个圆弧的半径最小为10个单位，否则将发生碰撞，导致机器人无法完成行走。机器人直线行走的最大速度为个单位/秒，转弯时，最大的弯速为，其中是转弯半径。若超过该速度，则机器人侧翻，无法完成行走。根据以上所给信息，建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。对场景中的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的最短时间路径。注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总距离和总时间。2问题分析本文研究的是以（0,0）为起点，以800*800为平面场景的机器人避障行走问题，在给定的明确的障碍物的坐标位置的前提下，按照一定的行走路径绕过障碍物达到目的点的最短路径进行分析，且给定的路径行走方式为仅可以直线段与圆弧，对最短路径的行走给出方案，建立可行的避障定位最短路径和最短时间路径的数学模型。问题1中，首先根据给定的10个单位的障碍距离，绘制出机器人允许行走的活动区域。接着，利用平面几何的知识以及导数原理，找出最短路径的制定方法，得出圆弧的半径越小，得到的路径也相应的最短的结论，据此证明所绘制的路径为最短路径。其次，将到不同目标点,O（0,0），A（300,300），B(100，700)，C(700，640),四个点的最短路径的情况绘制出，在拐角处绘制以方形或圆形的障碍物的顶点为10单位半径的圆弧（圆形的障碍物即以障碍物的圆心为圆心，障碍物的半径加10单位为半径的圆弧）。在此，制定求解路径长度及起始点的坐标，分4种情况进行分析。最终，对各条路径的直线段和圆弧进行加总求和，求得总长度，比较同一目标点下不同路径，最短的即为最短路径。依据此方法，建立最短路径的数学模型，并求解直线，圆弧起点，终点坐标，圆弧的圆心坐标。问题2中，OA的最短路径由距离和速度决定，而直线和转弯的速度不同。因此需建立转弯半径为变量的优化模型，此时，建立将转弯半径的取值范围为约束条件，由此，建立模型求解最短时间距离。3模型假设1、假设障碍物的位置固定不变，且只包含长方形，正方形，平行四边形，三角形，圆形，题目中所给的数据准确无误。2、假设机器人是一个不被考虑大小的点，即可作为质点，本身宽度不计。3、假设机器人的性能足够好，在行走的时候不出现故障，且能准确的沿着圆弧转弯。4、假设机器人可以无限接近边缘行走，忽略机器人转弯的时间。4符号说明表SEQ表\*ARABIC1符号说明表符号描述原点到直线A的长度A到下一切点直线段的长度圆弧半径机器人过圆弧长度i表示第i个障碍物，j表示由左下角逆时针旋转的第j个角5模型的建立与求解5.1问题一（求解最短路径）5.1.1模型分析1确定机器人可行区域由于要求的目标点与障碍物的距离至少超过10个单位的距离，且行走路径为直线段与圆弧组成，无直线转弯，可绘制出机器人的活动区域，利用CAD制图软件，绘制图形。2绘制路径的原则题目要求在机器人允许的活动区域内，绘制出机器人到达不同目标点的线路图。规定机器人的行走路径由直线段和圆弧组成(转弯路径由与直线路径相切的一段圆弧组成，机器人不能折线转弯)。所以将模型简化为在已知两个固定点和圆弧圆心的情况下，确定圆弧半径为何值时，才能使机器人行走布局路径最短。局部简化路线图如图1：图11）圆弧半径与路径长度关系假设图中固定点的坐标分别为、，圆弧圆心为，圆弧半径为。线和线分别相切于圆于点、，各圆心角分别为，，，，线段、、、的长度分别为。根据勾股定理可以分别计算的长度为：（1）且在中满足如下三角函数关系：（2）（由于三条线段均可由已知条件计算出，所以也为已知角）再根据机器人所走路线结构和公式（2）计算出的角度，可建立路径长度关于圆弧半径的函数关系：（3）2）求解最短路径下的根据路径与圆弧半径的关系式（3），对未知量求导，求出最短路径下的。求导结果为：（4）均在直角三角形中，所以，则求导结果可以近似看为：（）（5）（路程函数对于求导的结果大于0，所以函数关系为单调递增函数）得出绘制路径原则制定机器人行走路线时，圆弧半径应为要求中的最小值10个单位。理由：根据求导结果（4），可知当圆弧半径逐渐增大时，机器人行走弧线结构的路程最大，逐渐减小时，路程逐渐减小。题目要求离障碍物最近距离为10个单位，所以当圆弧半径为10时，路程最短。所走圆弧的圆心为障碍物定点。理由：两点之间直线最短，所以尽量要直线靠近目标点，当无法直线行进时要尽量保证躲避障碍物所走弧线最短，即以障碍物顶点为圆心下最小要求距离为半径的圆弧最短，才不会产生绕远现象（障碍物2本身为圆，所以圆弧圆心为障碍物2的圆心，半径为障碍物2的半径）。3求解路径长度以及点坐标（路径起始点和终点坐标）的方法在进行绘制路径的过程中，经归纳可以将路径长度的计算分为三钟类型，第一，（点到弧）已知点到圆弧，第二，（弧到弧）公切线与两圆心的连线平行；公切线与两圆心的连心相交。三，弧长的求解（1）已知点到圆弧图2如图2中固定点的坐标分别为、，圆弧圆心为，圆弧半径为（为最小半径），线和线分别相切于圆于点、，切点坐标未知，设坐标分别为、。根据已知坐标和数据可以求出线段的长度为：求长度（6）求长度（7）在求出线段后，可以根据方程求解确切的切点坐标：求切点坐标（8）求切点坐标（9）由于方程组（8）、（9）均为二元二次方程组，可以用MATLAB软件进行求解。（2）切点连线与圆心平行图3如图3，已知圆心、，两圆弧的半径均为最短半径。未知点为切点，设其坐标分别为、。图3中四边形为矩形，所以切线段的长度等于线段的长度：在结合的长度，和三角形勾股定理建立方程，求出切点坐标求切点坐标（10）求切点坐标（11）（3）切点连线与圆心连线相交图4如图4，已知圆心、，两圆弧的半径均为最短半径。未知点为切点，设其坐标分别为、。切点连线与圆心连线的交点为。根据三角形全等规律，可以证明，此时为线段的中点，切线的长度计算公式为：根据所求长度和已知坐标点，列出关于切点的函数方程组并求解出（计算机求解）切点具体坐标。函数方程组具体如下：求切点坐标（12）求切点坐标（13）（4）求弧长图5在以上3种情况中我们均可求出切点坐标，、，圆弧圆心为，圆弧半径为（为最小半径）。根据余弦定理可得（14）（15）并且有已知条件可得：（16）弧长为：（17）5)特别注意由于障碍物2本身为圆，所以求解其上行走的切点坐标时，应对圆弧半径进行相应改正。将半径改为（80为障碍物本身半径70与障碍物和机器人之间最短距离10之和）。切求解点的坐标时，不能运用求交叉点的方法，需另行计算。5.1.2求解最短路径根据前面已证明绘图原则和求解路径长度和起始点坐标的方法，在求得线路的最短路径的基础上求得路径。1）最短路径（1）根据所制定的绘制路径的原则，在场景图中，绘制全部的路径，有两条线路1,2,如下图2所示：图6（2）根据制定的求解路径长度和起始点坐标的方法进行求解，可得出所走路径长度的具体坐标如下表1所示：表2：OA路线1O→A路线1O→T52→A路线1起点坐标终点坐标圆弧角度路线距离（弧长）O→T51(0,0)(70.5,213.1)224.4994圆弧1(70.5,213.1)(76.6,29.4)52°9.051T51→A(76.6,29.4)(300,300)237.486机器人行走总路程471.03表3:OA路线2O→A路线2O→T54→A路线1起点坐标终点坐标圆弧角度路线距离（弧长）O→T54(0,0)(232.1,50.2)237.486圆弧1(232.1,50.2)(239.7,57.6)52°9.051T51→A(239.7,57.6)(300,300)249.799机器人行走总路程498.4258由表2，表3，比较可得出OA线路1是其最短路径的长度，为471.032）最短路径（1）根据所制定的绘制路径的原则，在场景图中，绘制全部的路径,如下图X所示图7（2）根据制定的求解路径长度和起始点坐标的方法进行求解，可得出所走路径长度的具体坐标如下表2所示：表4：OB线路1O→B路线1O→T61→T62→T72→T73→T73→T81→B路线1起点坐标终点坐标圆弧角度路线距离（弧长）O→T61(0,0)(50.2,301,6)305.777圆弧1(50.2,301.6)(51.7,305.5)24°4.239T61→T62(51.7,305.5)(141.7,440.5)162.249圆弧2(141.7,440.5)(147.9,444.8)45°7.776T62→T72(147.9,444.8)(220.04,460.2)75.663圆弧3(220.04,460.2)(230,470)78°13.655T72→T73(230,470)(230,530)60圆弧4(230,530)(225.5,538.35)57°9.884T73→T81(225.5,538.35)(144.5,591.6)96.95圆弧5(144.5,591.6)(140.7,596.3)35°6.1473T81→B(140.7,596.3)(100,700)111.44机器人行走总路程853.77表5：OB线路2O→B路线2O→T51→T63→T73→T81→B路线1起点坐标终点坐标圆弧角度路线距离（弧长）O→T51(0,0)(70.3,210.05)224.4992圆弧1(70.3,210.05)(76.01,219.03)48°8.46T51→T63(76.01,219.03)(239.02,290.84)178.11圆弧2(239.02,290.84)(244.98,300.65)70°12.217T63→T73(244.98,300.65)(229.97,530.65)230.488圆弧3(229.97,530.65)(225.55,538.41)57°9.94T73→T81(225.55,538.41)(144.51,591.64)96.95圆弧4(144.51,591.64)(140.69,596.38)35°6.108T81→B(140.69,596.38)(100,700)111.44机器人行走总路程878.21表6:OB线路3O→B路线3O→T53→T63→T73→T81→B路线1起点坐标终点坐标圆弧角度路线距离（弧长）O→T53(0,0)(232.11,50.22)237.4868圆弧1(232.11,50.22)(239.84,59.611)77°13.43T51→T63(239.84,59.611)(244.99,299.79)240.05圆弧2(244.99,299.79)(244.98,300.65)5°0.87T63→T73(244.98,300.65)(229.97,530.65)230.488圆弧3(229.97,530.65)(225.55,538.41)57°9.94T73→T81(225.55,538.41)(144.51,591.64)96.95圆弧4(144.51,591.64)(140.69,596.38)35°6.108T81→B(140.69,596.38)(100,700)111.44机器人行走总路程946.76由表4，表5，表6，比较可得OB的三条路径中，最短的路径为路线1，为853.773）最短路径（1）根据所制定的绘制路径的原则，在场景图中，绘制全部的路径,如下图X所示图8（2）根据制定的求解路径长度和起始点坐标的方法进行求解，可得出所走路径长度的具体坐标如下表1所示：表7：OC线路1O→C路线1O→T61→T62→T74→T12→T94→T102→T103→C路线1圆弧角度路线距离（弧长）O→T61305.77圆弧124°4.188T61→T62162.2698圆弧245°7.8539T62→T7475.66圆弧354°9.4247T74→T12151.32圆弧442°7.3303T12→T94151.327圆弧510°1.7453T94→T102119.16圆弧634°5.9341T102→T103130圆弧777°13.439T103→C94.38机器人行走总路程1239.802O→C路线2O→T52→T32→T2→T114→T113→C路线1圆弧角度路线距离（弧长）O→T52224.49圆弧151°4.188T52→T32341.76圆弧210°7.8539T32→T2169.70圆弧334°9.4247T2→T114170圆弧445°7.3303T114→T11380圆弧539°1.7453T113→C43.5机器人行走总路程1059.992表8：OC线路3O→C路线3O→T54→T32→T2→T114→T113→C路线1圆弧角度路线距离（弧长）O→T54237.48圆弧149°8.552T54→T32318.43圆弧251°8.9011T32→T2169.70圆弧334°9.4247T2→T114170圆弧445°7.3303T114→T11380圆弧539°1.7453T113→C43.5机器人行走总路程1055.063由表7，表8比较分析可得，在三条备选路径中，路线3是最短的，路径长为1055.0634）确定过点A、B、C时的圆弧圆心位置和圆弧半径无论路径多么复杂，我们都可以将路径划分为若干个切线圆结构来求解，目标函数可以表示为：（18）其中表示总路径长，表示各分段路径中每一段的长度，因此利用该目标函数，建立优化方程组，再借助LINGO对优化方程进行求解。例如在障碍物5经过再到障碍物7的路径中，设经过的转弯圆弧的圆心坐标为，经过障碍5和障碍7的转弯圆心坐标为（80,210），（220,530），根据关系建立优化方程为：（19）（20）利用LINGO软件编程求解：min=l1+l2+EF;l1=@sqrt((x-80)^2+(@sqrt(100-(x-300)^2)+300-210)^2);l2=@sqrt((x-220)^2+(@sqrt(100-(x-300)^2)+300-530)^2);EF=10*(3.1415926-@acos((l1^2+l2^2-140^2-320^2)/(2*l1*l2)));x<300;y=@sqrt(100-(x-300)^2)+300;解得圆心坐标为（290.8854，304.1140），半径长度为10。绘制过A点的线路图，如图9所示图9同理可证，过点B的圆弧圆心为（108.1715,694.0966）；过点C的圆弧圆心为（709.7939,642.0198），圆弧半径均为10。5）根据所确定的过点A,B,C圆弧圆心坐标，求解最短路径（O→A→B→C→O）1、先求从O到A的最短路径，可能成为O到A的最短路径的有两条路径，如图10：图10从O到A的两条路径的距离如下表9所示：表9OA的线路1路线1距离(弧长)路线2距离(弧长)O→T52224.4994O→T54237.4868圆弧19.25圆弧110.64T52→A230.0239T54→A251.4854圆弧215.707圆弧25.7595总距离479.4803总距离505.3717所以我们选择路线1：O→T52→A2、如图可知从A→B的最短路径只有一条：A→T73→T81→B3、由于无法从9号障碍物上方行走，因此从B→C的路径也只有一条：B→T91→T94→T102→T103→C4、上文已求O→C的最短路径，所以C→O的最短路径为C→T113→T114→S2→T32→T54→O所以从O→A→B→C→O的最短路径为：O→T52→A→T73→T81→B→T83→T91→T94→T102→T103→C→T113→T114→S2→T32→T54→O距离及弧长如下表10：表10 OA线路2O→T52224.4994圆弧19.2522T52→A230.0239圆弧215.7073A→T73236.5184圆弧36.6986T73→T8196.9543圆弧45.4894T81→B103.0155圆弧520.7680B→T83162.3420圆弧61.1451T83→T9197.9790圆弧72.0135T91→T9460圆弧85.9200T94→T102119.163圆弧95.9291T102→T103130圆弧1013.5462T103→C94.4673C→T11338.3383圆弧127.1911T113→T11480圆弧1311.8013T114→S2196.7232圆弧1412.6136S2→T32135.6466圆弧1513.6802T32→T54318.4337圆弧168.5852T54→O237.4861总路程2701.932所以，最短路径：O→T52→A→T73→T81→B→T83→T91→T94→T102→T103→C→T113→T114→S2→T32→T54→O的总长度为2701.9325.2问题二（求解最短时间路径）5.2.1模型的建立由问题1可知从O→A的最短路径为O→5(左上角)→A，当速度一定时，时间与路程成正比，因此只需先在最短路径上方画出安全行走范围，在安全范围内对转弯半径讨论即可得到行走时间最短的转弯半径。为了求出安全行走范围，以5号障碍物左上角为圆心，半径R需要满足：（1）最小转弯半径R=10；（2）最大转弯半径必须保证机器人在6号障碍物右下角的安全行走。通过分析，如图所示，只要保证机器人在酒红色区域行走即可，通过计算可知最大转弯半径为55.0824。所以，机器人以5号障碍物左上角顶点为圆心，在半径为[10,55.0824]区域内行走是安全的，在这个范围内可以找到最短时间路径。设机器人转弯半径为r，以5号障碍物左上角点F为圆心，设A点与O点与圆F相切与E、D两点，根据几何关系可以求得OD，AE，DE弧的长度分别为：QUOTEdOD=(80-42525QUOTEdAE=(-220+112825rQUOTEdDE=r(π-arccos⁡(-1+1所以因此该问题的模型为：其中，直线行走速度QUOTEv1=5m/s,转弯速度为QUOTEv2=v11+e10-0.1r图115.2.2模型求解有lingo编程求解该优化模型，程序如下：min=(OE+AF)/5+EF/v;OE=@sqrt((80-4/2525*r^2-21/5050*r*@sqrt(50500-r^2))^2+(210-21/5050*r^2+4/2525*r*@sqrt(50500-r^2))^2);AF=@sqrt((-220+11/2825*r^2-9/5650*r*@sqrt(50500-r^2))^2+(-90+9/5650*r^2+11/2825*r*@sqrt(50500-r^2))^2);EF=r*(3.1415926-@acos(-1+1/2*((1563/28532