机器人避障问题论文.doc

机器人避障问题摘要本文主要研究了在特定的平面场景中机器人为了避开障碍物从某定点到另一个定点的最短路径和最短时间路径建立了两个优化模型，并给以了各种情况下的最短路径和最短时间路径。问题一，我们很容易分解成线圆结构来求解，然后利用组合把可能路径的最短路径采用穷举法列举出来，最终得出最短路径：最短路径的值为：479.1252；所用时间为：99.25276最短路径的值为：926.99； 所用时间为：224.5684最短路径的值为：1211.8 所用时间为：311.5432最短路径的值为：2924；所用时间为：645.3072问题二，适当扩大机器人的转弯半径，建立优化模型，得出结果：半径：11.5035路径：500.7212时间：96.5939关键词 MATLAB 线圆结构 避障路径 解析几何一、问题重述1.1 题目背景：在一个800800的平面场景图，在原点O(0,0)点处有一个机器人，它只能在该平面场景范围内活动。图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物。障碍物的坐标和平面图如下：编号障碍物名称左下顶点坐标其它特性描述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，宽601.2 题目条件：(1) 在平面场景图中，障碍物外指定一点为机器人要到达的目标点（要求目标点与障碍物的距离至少超过10个单位）；规定机器人的行走路径由直线段和圆弧组成（圆弧是机器人转弯路径）；(2) 机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位；(3) 为了不与障碍物发生碰撞，机器人行走的线路与障碍物间的最近距离为10个单位。否则将发生碰撞，若发生碰撞，则机器人无法完成行走；(4) 机器人直线行走的最大速度为个单位/秒；机器人转弯时，最大转弯速度为，其中是转弯半径；(5) 如果超过转弯速度，机器人将发生侧翻，无法完成行走。1.3 所求问题： 对场景图中4个点O(0,0),A(300,300),B(100,700),C(700,640),具体计算：(1) 机器人从O(0,0)出发，OA、OB、OC和OABCO的最短路径。(2) 机器人从O (0,0)出发，到达A的最短时间路径。1.4 题目目的：建立机器人从区域中一点到达另一点的避障最短路径和最短时间的数学模型。二、问题分析2.1、问题一中要求求机器人从O(0, 0)出发，OA、OB、OC和OABCO的最短路径。我们先用包络线画出机器人行走路线中的危险区域，这样拐角处就是一个半径为10个单位的圆弧（因为机器人不能折线转弯，所以路径中，其中A，B，C三点也要作一半径为10个单位的圆弧）。通过采用拉绳子的方法寻找到目标点可能的最短路径（如：求的最短路径，我们可以用绳子连接，以拐角处的圆弧为支撑拉紧，那么绳子的长度便是的一条可能的最短路径），然后采用穷举法列出到每个目标点的可能最短的路径，然后比其大小便可得出到目标点的最短路径。2.2机器人从O (0, 0)出发，到达A的最短时间路径。对于这个问题我们可以适当扩大障碍物拐点处的拐弯半径使机器人能够以较快的速度通过障碍物（当半径越大，它的某段圆弧就越接近直线），另外在求坐标问题中我们可以直线与直线之间的关系求出切点坐标。2.3、问题二中要求机器人从O(0,0)出发到A(300,300)的最短时间路径。机器人通过适当的扩大拐弯半径来加快通过障碍物。这是圆的半径就是一个动态变量，可以通过改变圆的半径来调整机器人的运行时间，以尽可能的缩短耗时。三、模型假设3.1、假设当机器人穿过图中A,B,C点时，可将A,B,C三点看做是障碍物来处理。3.2、假设机器人能够抽象成点来处理。四、符号说明x1起点O的横坐标x2圆弧的圆心的横坐标x3目标点的横坐标y1起点O的纵坐标y2圆弧的圆心的纵坐标y3目标点的纵坐标R圆弧所在圆的半径Di某段路径的长度Li某圆弧的长度五、模型的建立5.1、证明猜想 猜想一：具有圆弧危险区域的最短路径是由两部分组成的：一部分是平面上的自然最短路径（即直线段），另一部分是圆弧，这两部分是相切的，互相连接的。（即问题分析中的拉绳子拉到最紧时的状况）证明：假设在平面中有A（a，0）和B（-a，0）两点，中间有一个半圆形的障碍物，证明从A到B的最路径为AEFB。 平面上连接两点最短的路径是通过这两点的直线段，但是连接两点的线段于障碍物相交，所以设法尝试折线路径。在y轴上取一点C（0，y），若y适当大，则折线ACB与障碍物不相交，折线ACB的长度为： 显然随着y的减小而减小，减小得，即,使得与与障碍物相切，切点分别为E和F,显然是这种折线路径中最短的。由于满足0的角满足tan,所以易知弧度EF小于的长， 即EFEC1F,从而EF+FB|AP|,又由AEEO,所以|AP|AE,从而APAE,同理可得BQBF。再来比较PQ之间路径长度PQ和圆弧EF的长度的大小。若PQ之间的路径可有极坐标方程=(),则有1,可得：PQ=2+2dd-EF 亦即路径APQB的长度超过路径AEFB的长度。以上证明足以说明了AEFB是满足条件A到B的最短路径。猜想二:如果一个圆环可以绕着环上一个定点转动，那么过圆环外两定点连接一根绳子，并以该圆环为支撑拉紧绳子，达到平衡状态时，圆心与该顶点以及两条切线的延长线的交点共线。 证明猜想：如上图所示，E点就是圆环上的一个顶点，ACDB就是拉紧的绳子，就是切线AC和BD的延长线的交点，证明、E、三点共线。我们可以用力学的知识进行证明，因为是拉紧的绳子，所以两边的绳子拉力相等，设为，它们的合力设为，定点对圆环的作用力设为。那么由几何学的知识我们可以知道一定与共线，而又由力的平衡条件可知：=-即与共线。综上所述、和三点一定共线。5.2、模型分析一我们就可以这样认为，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个线圆结构所组成。在本题中存在障碍物的状况，且障碍物在拐点处的危险区域是一个半径为10个单位的圆弧，所以结合定理一，我们易知，求两点之间的最短路径中的转弯半径我们应该按照最小的转弯半径来算才能达到最优。5.2.1）如上图，设, , ,圆O的半径为R，则由猜想一及函数知识可得是关于半径R的函数，：=，即对求导可得：0; 可知函数在区间10,min(AB,DB)上为单调递减函数，故函数将在半径R等于10时，取的最小值。5.2.2）而对于下图两种情况我们不能直接采用线圆的结构来解决，需要做简单的变换。情况一： 我们假设两圆心坐标分别为和,半径均为r，M点坐标为，那么我们很容易可以求得:这样我们就可以利用5.2.1）中的方法，先求A到M，再求M到B，这样分两段就可以求解。同理如果有更多的转弯，我们同样可以按照此种方法分解。情况二： 这里我们依然设圆心坐标分别为和,半径均为r，这样我们可以得到：那么直线方程为：因为公切线DE与平行，那么DE的直线方程可以表示为：其中：那么把公切线的方程于圆的方程联立，可以求得切点D和E的坐标。这样用D和E任意一点作为分割点都可以将上图分割成两个线圆结构，这样就可以对其进行求解。同理多个这样的转弯时，用同样的方法可以进行分割。5.3模型分析二5.3.1 对于从起点经过若干点然后再到达目标点的状况，因为不能走折线路径，我们就必须考虑在经过路径中的一个目标点时转弯的状况。为了研究这个问题的方便，我们先来证明一个猜想：5.2.4）求解从起点经过若干个点再到达目标点的问题，我们还可以有另一种方案，即适当扩大障碍物拐点处的拐弯半径使机器人能够沿直线通过路径中的目标点。这样拐点处拐弯圆弧的半径和圆心都是个变量，对于该题，那么我们可以首先设定三个圆心、,然后按照以下步骤进行作图：1） 给定，以为圆心，为半径，画圆，然后过R点做圆的切线，切点为D。然后过A点做的切线设为，切点为E。2） 然后做F垂直于，垂足为F，F的长就是，然后以为圆心，为半径画圆。很显然能由来确定，即。3） 然后过B点做的切线为，切点为G，再过F垂直于，垂足为H，那么H的长度就是，然后以为圆心，为半径，画圆。很显然能由来确定，即=。4） 过C做的切线。这就完成了由R经过A和B在到达C的路径。5） 然后再变换、，可得到新的路径。找出最小者即可。5.3.2最短时间路径问题分析如上图所示，令d1=OA,d2=AB，则；有因，知直行速度为V0，弧形速度为，故有要时间最短，则对R关于时间T求导，求R的最大值即可；确定半径后即可求出路径。5.4、模型的建立5.4.1线圆结构模型： 假设机器人从起点O到目标点B，其中绕弧圆心为A，则典型线圆结构模型的最短路径为：机器人避障行进最短路径通用模型为： （m为直行路径的数量，n为弧长路径的数量） 5.4.2 最短时间路径模型： 假设机器人从起点O到目标点A，其中圆弧圆心为B，则最短时间模型路径为： 六、模型的求解6.1问题一1）如下图机器人从O(0, 0)出发到A的最短路径。从图中可以看出两条路径相差不大，所以我们采用对比数据的方法选出最优途径。项目D1D2LDT路径1224.4994237.486817.1389479.125299.252762237.4868249.799929.5551516.8419109.27938路线1起点起点终点终点圆点圆点D10070.506213.1406D284.1724200.91230030080210L70.506213.140684.1724200.912路线2起点起点终点终点圆点圆点D100227.070969.5614D2239.704357.586230030023060L227.070969.5614239.704357.5862程序：1、A=0 0;B=80,210;C=300,300;x=daojiao(A,B,C);x1,D1,C1=jiaobian(A,B)x2,D2,C2=jiaobian(C,B)D=juli(A,B,C)L=D-D1-D2t=(D1+D2)/5+(L)/2.52、A=0 0;B=230 60;C=300,300;x=daojiao(A,B,C)x1,D1,C1=jiaobian(A,B)x2,D2,C2=jiaobian(C,B)D=juli(A,B,C)L=D-D1-D2t=(D1+D2)/5+L/2.52）如下图机器人从O(0, 0)出发到B的最短路径 项目起点坐标终点坐标长度总时间编号224.56841D10,0 50.1353 , 301.6396305.7777D268.9408, 295.5209105.0000, 367.500080.5062L1 50.1353 , 301.639668.9408, 295.520922.9438D3105.0000, 367.5000141.0592,439.47980.5062D4156.6094 ,427.4956 185.0000, 452.500037.8319L2141.0592,439.479156.6094 ,427.495636.6074D5 185.0000, 452.5000 213.3906,417.504430D6211.6462 404.5033211.6462,434.503330L3 213.3906,417.5044211.6462 404.5033D7211.6462,434.5033211.6462,464.503337.8319D8211.6462,524.5033 185 ,56548.4768L4211.6462,464.5033211.6462,524.503343.9064D9 185 ,565158.3538,605.496748.4768D10159.195,603.9308100700151.7399L5158.3538,605.4967159.195,603.930832.3897总长926.992D10,0 50.1353 , 301.6396305.7777261.3450D268.9408, 295.5209105.0000, 367.500080.5062L1 50.1353 , 301.639668.9408, 295.520922.9438D3105.0000, 367.5000141.0592,439.47980.5062D4156.6094 ,427.4956 185.0000, 452.500037.8319L2141.0592,439.479156.6094 ,427.495636.6074D5 185.0000, 452.5000213.3906 ,477.504437.8319D6229.1299,465.9201 260 ,53575.6637L3213.3906 ,477.5044229.1299,465.920157.2769D7 260 ,535290.8701 ,604.079975.6637D8290.0749,598.7781 285 ,64041.5331L4290.8701 ,604.0799290.0749,598.778153.7276D9 285 ,640 279.9251 ,681.221941.5331D10267.6792,670.273010070085L5 279.9251 ,681.2219267.6792,670.273051.8827总长935.59程序：1、n=1sum1=0;sum2=0;sum3=0;sum4=0;A=0 0;B=60 300 150 435 220 470 300 600 270 680 100 700;for i=1:4 B(i,1:2); C(1)=(B(i,1)+B(i+1,1)/2 C(2)=(B(i,2)+B(i+1,2)/2 x=daojiao(A,B(i,:),C);x1,D1,d1=jiaobian(A,B(i,:)x2,D2,d2=jiaobian(C,B(i,:)D=juli(A,B(i,:),C);L=D-D1-D2; A=C n=n+1 sum1=sum1+D1 sum2=sum2+D2 sum3=sum3+D sum4=sum4+Lend t=(sum1+sum2)/5+(sum4)/2.52、n=1sum1=0;sum2=0;sum3=0;sum4=0;A=0 0;B=60 300 150 435 220 470 300 600 270 680 100 700;for i=1:4 B(i,1:2); C(1)=(B(i,1)+B(i+1,1)/2 C(2)=(B(i,2)+B(i+1,2)/2 x=daojiao(A,B(i,:),C);x1,D1,d1=jiaobian(A,B(i,:)x2,D2,d2=jiaobian(C,B(i,:)D=juli(A,B(i,:),C);L=D-D1-D2; A=C n=n+1 sum1=sum1+D1 sum2=sum2+D2 sum3=sum3+D sum4=sum4+Lendt=(sum1+sum2)/5+(sum4)/2.53）如下图机器人从O(0, 0)出发到C的最短路径 项目起点坐标终点坐标长度时间编号1D10,0 227.0709， 69.5614237.4868311.5432D2233.2153,50.5310 320，8091.6515L1 227.0709， 69.5614 233.2153 ，50.531020.4153D3320， 80233.2153 ， 50.531091.6515D4418.3448，94.489745515066.5207L2406.7847 ， 109.4690418.3448，94.489756.6387D5455150491.6552，205.510366.5207D6508.5213 ，194.7666 610 ， 360193.9072L3491.6552 ，205.5103508.5213 ，194.766651.229D7 610 ， 360711.4787,525.2334193.9072D872052072060020L4711.4787,525.233472052041.8749D9720610700640255.78总长1211.82D10,0 50.1353 ,301.6396305.7777333.8906D268.9408, 295.5209 105.0000 ,367.500080.5062L1 50.1353 ,301.639668.9408, 295.520922.9438D3 105.0000 ,367.5000141.0592 ,439.479180.5062D4156.6094 ,427.4956185.0000 ,452.500037.8319L2141.0592 ,439.4791156.6094 ,427.495636.6074D5185.0000 ,452.5000213.3906,477.504437.8319D6 229.1299 , 465.9201 260 ,53575.6637L3213.3906,477.5044 229.1299 , 465.920157.2796D7 260 ,535 290.8701,604.079975.6637D8 306.3117 , 592.243536564075.6637L4 290.8701,604.0799 306.3117 , 592.243536.0908D9365640423.6883 ,687.756575.6637D10435.5878 ,671.7068 485 ,70559.5819L5423.6883 ,687.7565435.5878 ,671.706832.6662D11 485 ,705 534.4122,738.293259.5819D12541.5385,739.8809 685 , 73064.2262L6 534.4122,738.2932541.5385,739.880935.6822D13 685 , 730668.4615,720.119164.2262D14679.9403 ,731.091270064046.3681L7668.4615,720.1191679.9403 ,731.091243.9064总长1404.3程序：1、n=1sum1=0;sum2=0;sum3=0;sum4=0;A=0 0B=230 60 410 100 500 200 720 520 700 560;for i=1:4 B(i,1:2) C(1)=(B(i,1)+B(i+1,1)/2 C(2)=(B(i,2)+B(i+1,2)/2 x=daojiao(A,B(i,:),C)x1,D1,d1=jiaobian(A,B(i,:)x2,D2,d2=jiaobian(C,B(i,:)D=juli(A,B(i,:),C)L=D-D1-D2 A=C n=n+1 sum1=sum1+D1 sum2=sum2+D2 sum3=sum3+D sum4=sum4+Lendt=(sum1+sum2)/5+sum4/2.52、n=1sum1=0;sum2=0;sum3=0;sum4=0;A=0 0B=60 300 150 435 220 470 300 600 430 680 540 730 670 730 700 640;for i=1:6 B(i,1:2) C(1)=(B(i,1)+B(i+1,1)/2 C(2)=(B(i,2)+B(i+1,2)/2 x=daojiao(A,B(i,:),C)x1,D1,d1=jiaobian(A,B(i,:)x2,D2,d2=jiaobian(C,B(i,:)D=juli(A,B(i,:),C)L=D-D1-D2 A=C n=n+1 sum1=sum1+D1 sum2=sum2+D2 sum3=sum3+D sum4=sum4+Lendt=(sum1+sum2)/5+sum4/2.54）如下图机器人OABCO的最短路径。项目起点坐标终点坐标长度时间编号324.84D10,070.5060，213.1406224.4994D284.5517， 201.0959190 ， 255118.4272L170.5060，213.140684.5517， 201.095922.7955D3190 ， 255295.4483，308.9041118.4272第一段D4 290.3171，297.5016260415121.3466L2295.4483，308.9041 290.3171，297.501635.8795D5260415229.6829 ， 532.4984121.3466D6211.6462 ，524.5033185 ， 56548.4768L3229.6829 ， 532.4984211.6462 ，524.503352.2542D7185 ， 565158.3538 ，605.496748.4768D8 140.4000，597.2000125 ， 65055L4158.3538 ，605.4967 140.4000，597.200034.6334D9125 ， 650109.6000,702.800055D10102.3208 ,709.7270185 , 69085L5109.6000,702.8000102.3208 ,709.727052.9315D11185 , 690267.6792,670.273085D12274.0000,689.165232068022.9129L6267.6792,670.2730274.0000,689.165261.6608第一段总长和时间1364.1D1320680429.0909，670.0414109.544587.9066D2 435.5878，671.706848570559.5819第二段2L1429.0909，670.0414 435.5878，671.706854.2686D3485705534.4122，738.293259.5819D4543.0769 ，739.5149605.000， 730.000030.9233L2534.4122，738.2932543.0769 ，739.514935.6822第二段总长和时间349.58D1605.000， 730.0000668.4615，720.119164.226277.5202D2679.9403，731.091268568546.3681第三段L1668.4615，720.1191679.9403，731.091243.9064D368568572061046.3681D472060072052020L272061072060060第三段总长和时间282.28D1720510508.3045，194.4291380208.51D2 491.6552， 205.510345515066.5207L1508.3045，194.4291 491.6552， 205.510319.5262D3455150 418.3448，94.489766.5207第四段D4406.7847，109.4690 320 ， 8091.6515L2 418.3448，94.4897406.7847，109.469037.609D5 320 ， 80233.2153 ，50.531091.6515D6226.6706 ，69.42950,0118.4272L3233.2153 ，50.5310226.6706 ，69.429556.7576第四段总长和时间928.66路线总长和总时间2924645.3072程序：n=1sum1=0;sum2=0;sum3=0;sum4=0;A=0 0B=80 210 300 300 220 530 150 600 100 700 270 680 320 680;for i=1:6 B(i,1:2) C(1)=(B(i,1)+B(i+1,1)/2 C(2)=(B(i,2)+B(i+1,2)/2 x=daojiao(A,B(i,:),C)x1,D1,d1=jiaobian(A,B(i,:)x2,D2,d2=jiaobian(C,B(i,:)D=juli(A,B(i,:),C)L=D-D1-D2 A=C n=n+1 sum1=sum1+D1 sum2=sum2+D2 sum3=sum3+D sum4=sum4+Lendt=(sum1+sum2)/5+(sum4)/2.52、n=1sum1=0;sum2=0;sum3=0;sum4=0;A=320 680B=430 680 540 730 605 730;for i=1:2 B(i,1:2) C(1)=(B(i,1)+B(i+1,1)/2 C(2)=(B(i,2)+B(i+1,2)/2 x=daojiao(A,B(i,:),C)x1,D1,d1=jiaobian(A,B(i,:)x2,D2,d2=jiaobian(C,B(i,:)D=juli(A,B(i,:),C)D=D-D1-D2 A=C n=n+1 sum1=sum1+D1 sum2=sum2+D2 sum3=sum3+D sum4=sum4+Lendt=(sum1+sum2)/5+(sum4)/2.56.1问题二项目起点坐标终点坐标长度时间d10,069.0546,213.5395224.4274d284.8658,199.5762300300237.4188L169.0546,213.539584.8658,199.576210.5617总长500.721296.5939机器人从O (0, 0)出发，到达A的最短时间路径。程序：A=0 0;B=80,210;C=300,300;x=daojiao(A,B,C);x1,D1,C1=jiaobian(A,B)x2,D2,C2=jiaobian(C,B)D=juli(A,B,C)L=D-D1-D2t=(D1+D2)/5+(L)/(5/(1+exp(10-0.1*(11.5035)七、模型评价一、模型优点1、 运用多个方案对最短路径进行优化，在相对优化中取最优值。2、 模型大部分采用几何解析，精确度高。3、 模型思路简单易懂，便于实际操作和应用模型。二、模型缺点1、 在建模时对各种情况理想化，失去了实际机器人操作性。2、 在障碍去较多或障碍物不规则时此模型需要改动。八、参考文献1 刘来福，曾文艺，数学模型与数学建模，北京：北京师范大学出版社，20022 姜启源，邢文训等，大学数学实验，北京：清华大学出版社，20053 周建兴，岂兴明等，matlab从入门到精通，北京：人名邮电出版社，20084 枫铃草1207，行走机器人避障问题，/view/59fd857aa26925c52cc5bf4c.html，2012-9-85 尤承业，解析几何，北京，北京大学出版社，2004九、附录1、function x=daojiao(A,B,C)k1=(B(2)-A(2)/(B(1)-A(1);k2=(C(2)-B(2)/(C(1)-B(1);x=atan(k2-k1)/(1+k1*k2);if x0 k3=(k1+k2)/(1-k1*k2); k4=-1/k3;else k3=(k1-k2)/(1+k1*k2) k4=-1/k3endC(1)=(A(2)-k3*A(1)+k4*B(1)-B(2)/(k4-k3);C(2)=(k3*k4*B(1)-k3*B(2)+k4*A(2)-k3*k4*A(1)/(k4-k3);3、function d=juli(A,B,C)x1,d1=jiaobian(A,B);x2,d2=jiaobian(B,C);x=daojiao(A,B,C);d=d1+d2+10*(2*pi-x-x1-x2);4、function x=daojiao(A,B,C)k1=(B(2)-A(2)/(B(1)-A(1);k2=(C(2)-B(2)/(C(1)-B(1);x=atan(k2-k1)/(1+k1*k2);if x0 k3=(k1+k2)/(1-k1*k2); k4=-1/k3;else k3=(k1-k2)/(1+k1*k2) k4=-1/k3endC(1)=(A(2)-k3*A(1)+k4*B(1)-B(2)/(k4-k3);C(2)=(k3*k4*B(1)-k3*B(2)+k4*A(2)-k3*k4*A(1)/(k4-k3);6、function d=juli(A,B,C)x1,d1=jiaobian(A,B);x2,d2=jiaobian(B,C);x=daojiao(A,B,C);d=d1+d2+10*(2*pi-x-x1-x2);7、function x=daojiao(A,B,C)k1=(B(2)-A(2)/(B(1)-A(1);k2=(C(2)-B(2)/(C(1)-B(1);x=atan(k2-k1)/(1+k1*k2);if x0 k3=(k1+k2)/(1-k1*k2); k4=-1/k3;else k3=(k1-k2)/(1+k1*k2) k4=-1/k3endC(1)=(A(2)-k3*A(1)+k4*B(1)-B(2)/(k4-k3);C(2)=(k3*k4*B(1)-k3*B(2)+k4*A(2)-k3*k4*A(1)/(k4-k3);9、function d=juli(A,B,C)x1,d1=jiaobian(A,B);x2,d2=jiaobian(B,C);x=daojiao(A,B,C);d=d1+d2+10*(2*pi-x-x1-x2);10、function x=daojiao(A,B,C)k1=(B(2)-A(2)/(B(1)-A(1);k2=(