机器人避障问题.doc

机器人避障问题摘要：本文研究了机器人避障最短路径与最短时间路径的问题。问题（1）：首先利用几何知识证明了机器人在两个定点间绕一个障碍物的最短路径是由直线和圆弧组成的，并可将这种线弧结构应用所有路径之中，根据这个结论建立了机器人从区域中一点到达另一点的避障最短路径的最短路问题数学模型，此模型可用中的算法求解。在此基础之上，过原线弧结构中的圆弧中点，作与圆弧内切、半径为的圆，得到新的线弧结构，用规划的方法求出最短时间路径，此时圆的半径为11.97。将新线弧结构应用所有路径之中建立最短时间路径的最短路问题数学模型。问题（2）：该题是研究机器人由原点到达目标点所用最短时间的问题。根据所设的转弯圆弧半径 以及所转弯圆弧的圆心坐标，先计算出机器人在该区域行进中所用时间的方程一般通式，然后考虑到的特殊路径，通过软件求出所用时间的最小值。 关键词： 线弧 最短路径 软件 规划1、问题的重述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，宽60在图1的平面场景中，障碍物外指定一点为机器人要到达的目标点（要求目标点与障碍物的距离至少超过10个单位）。规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。机器人直线行走的最大速度为个单位/秒。机器人转弯时，最大转弯速度为，其中是转弯半径。如果超过该速度，机器人将发生侧翻，无法完成行走。请建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。对场景图中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的最短时间路径。注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总距离和总时间。2、模型的假设与符号说明2.1模型的假设（1）假设机器人看作是质点；（2）假设机器人由直线运动到圆弧的过程中没有加速度；（3）假设机器人正常运行。2.2符号的约定：机器人所走路径的总长度；：障碍物上任意点与行走路径之间的最短距离；：转弯圆弧的半径。3、模型的准备3.1模型准备一假设在平面中有任意与两点，中间的半圆是躲避障碍物时的不可接触区域，其中圆心为障碍物的顶点。、为圆弧的两条切线，相交于。、为圆弧的两条切线。显而易见从到的路径中，圆弧半径越小所走的路径最短。因此，机器人在躲避障碍物时，沿半径最小的弧线路径走（即），所用的路程最短。最短路径为：，（其中是机器人走圆弧所对的圆心角）区域中无论障碍物有多少，路径是由若干个线弧结构组成的，转弯半径最小时路径才能达到总路程最短。3.2模型准备二由于机器人需要躲避的障碍物形状不同，运动路线路较多，必然遇到两个线弧结构相连的情况。 情况1： 设出发点，目标点，圆心坐标，半径为，为到经过弧的路程长。由上图可以得到：在中，在中，在中，所以，所以可以得出：情况2：针对上面的问题，设圆心坐标分别为和,半径均为，这样我们可以得到：因为与平行，故设CD的直线方程可以表示为：由点到直线的距离公式求得将的直线方程分别与圆、的方程联立，求得切点与和的坐标。这样用、 任意一点作为分割点都可以将上图分割成两个线弧结构，这样就可以对其进行求解。情况3:假设两圆心坐标分别为和,半径均为，点坐标为，那么根据欧氏几何知识求得: 先求到，再求到，这样分为两部分进行求解。设出发点，目标点，圆心坐标，半径为，为A到B经过弧EF的路程长。由上图可以得到：在中，在中，在中，所以，所以可以得出：情况4：设出发点，目标点，圆心坐标，半径为，为到的路程。同问题3一样，我们同样可以计算出：3.3模型准备三要求该区域中机器人从出发点到目标点的最短时间，由条件知机器人转弯半径越大速度越大，适当增加转弯半径可提高机器人的速度，从而根据几何知识构造新的转弯圆弧得到从出发点到目标点的最短时间，方法如下：过最短路径中线弧结构中的圆弧中点，作与圆弧内切、半径为的圆，得到新的线弧结构。设区域中任意两点，且ab两点之间只需避让一个障碍区域，转弯圆弧的中点为，半径为10，转弯圆弧对应的圆心的坐标为,切点为、,过点作与ULV圆弧内切、半径为的圆,分别过点作圆的切线，切点为如图。设切线的方程：根据直线到圆心的距离公式求得和。同理可得切线的方程： 求得交点坐标为，进而可以求出直线方程：直线方程联立圆方程求得。 设圆弧所在圆的方程为： 段的圆弧方程为： 所以，所用的最少时间： 其中，4、模型的建立与求解4.1模型的建立：在区域中任意两点的最短避障路径和最短时间路径是由两部分组成的，一部分是直线段，另一部分是转弯时所经过的圆弧，而直线与圆弧是相切的。模型一：最短避障路径模型先求出所有的切线，包括出发点和目标点到所有圆弧的合法切线以及所有圆弧与圆弧之间的合法切线（即机器人能顺利通过的切线），然后求出所有合法切点，分别用来表示，用来表示切点与之间的合法切线的长度，用来表示切点与之间的合法圆弧的长度。以为节点，以或为边。做一个有n个节点，m条边的连通网络图，这样题目就转化成了求源点到达终点之间的最短路径问题了，这里最短路径就是指经过所有顶点与边的权值之和最小。那么目标函数可以表示为：该模型可以在中采用算法对路径进行优化求解。模型二：最短时间路径的模型，在上个模型基础上，根据模型准备3在最短时间路径下的所有合法切线与合法圆弧的长度，所有合法切点分别用来表示，用来表示切点与之间的合法切线的长度，用来表示切点与之间的合法圆弧的长度。以为节点，以或为边。做一个有n个节点，m条边的连通网络图，这样题目就转化成了求源点到达终点之间的最短时间下最短路径问题。那么目标函数可以表示为：同样该模型可以在MATLAB中采用Dijkstra算法对路径进行优化求解。4.2模型的求解根据模型准备与模型一可分别计算机器人从O(0, 0)出发，OA、OB、OC和OABCO的最短路径。1. 从O到A的路径其可能走的路径如下图4.2-1所示:图4.2-1依照其路线图，可以得出机器人行走的树状图4.2-2图4.2-2表4.2-1OAH4H2O0224.4994237.49A0246.5378264.2H4224.4994246.53780H2237.49264.20用可以求出（详见附录二）：最终我们求出的线路为线路一。表4.2-2起点坐标终点坐标圆弧圆心坐标圆弧半径直线长或弧长机器人行走总时间O到A线段一0,070.506,213.1406224.499444.9圆弧一70.506,213.140676.602，219.406680,210109.0513.62线段二76.60，219.4066300,300237.486847.49736机器人行走的总距离471.037296.017464从到，其可能走的路径如下图4.2-3图4.2-3求得机器人行走路径的树状图4.2-4为：图4.2-4表4.2-3OH4H2I2I1I3J2J3K1K3BO0224.5237.49305.78H4224.50178.1292.2H2237.490240.26621.29I2178.12240.260175159.53170.66230.49381.61I1305.781750162.25I3159.53162.25075.66J2170.6675.66060215.87J3230.4960096.95158.11K196.950111.36K2621.29381.61215.87158.110170.88B111.36170.880用可以求出（详见附录三）：最终我们确定机器人行走的线路为线路一。表4.2-4起点坐标终点坐标圆弧圆心坐标圆弧半径直线长或弧长机器人行走总时间O到B线段一0,050.1353,301.6396305.77761.1556圆弧一50.1353,301.639651.6979,305.574660,300104.26621.70648线段二51.6979,305.5746141.6979,439.6089161.447232.289圆弧二141.6979,439.6089105,444.0343150,434.03109.79483.918线段三105,444.0343222.1907,460.242973.98814.7976圆弧三222.1907,460.2429230,470220,4701013.49935.39972线段四230,470230,5306012圆弧四230,530225.4967,538.3538220,530109.88833.955线段五225.4967,538.3538144.5033,59.646296.953619.39圆弧五144.5033,59.6462140.6916,596.3458150,600106.14742.459线段六140.6916,596.3458100，700111.355322.271机器人行走的总距离853.1178179.34从到，其可能走的路径如下图图4.2-5图4.2-6表4.2-5OH4H2F2F4G3G2P4EN2N3CO0224.5237.49H4224.50341.76420.59H2237.490318.43187.95184.39F2341.76318.430169.71F4420.590156.636.64204.21356.09G3187.95156.60G2184.390133.04P436.64133.040238.54387.81E169.71204.21238.540170N2356.09387.81170080N380043.59C43.590用可以求出（详见附录四）：最终我们确定机器人行走的路线为线路一。表4.2-6起点坐标终点坐标圆弧圆心坐标圆弧半径直线长或弧长机器人行走总时间O到C线段一0,0422.0872,90.0817431.592886.319圆弧一422.0872,90.0817428.6909,94.9149420,99.86108.43093.37线段二428.6909,94.9149491.1217,204.6016126.209325.24圆弧二491.1217,204.6016492.2017,206.2599500,200101.98220.79线段三492.2017,206.2599727.9377,513.9179387.588677.5圆弧三727.9377,513.9179730,520720,520106.53812.615线段四730,520730,6008016圆弧四730,600727.7178,606.3589720,600106.89162.76线段五727.7178,606.3589700,64043.5898.72机器人行走的总距离1092.82233.33起点坐标终点坐标圆弧圆心坐标圆弧半径直线长或弧长机器人行走总时间O到A到B到C到O线段一0，070.506,213.14224.544.9圆弧一70.506,213.1476.6064,219.406680，210109.053.62线段二76.6064,219.4066291.0754,296.7803227.99945.6圆弧二291.0754,296.7803297.2537,309.0814287.7，3061015.186线段三297.2537,309.0814229.5719,532.8946233.8246.76圆弧三229.5719,532.8946225.4967,538.3538220，530106.952.78线段四225.4967,538.3538144.5033,591.646296.9519.39圆弧四144.5033,591.6462140.6916,596.3458150，600106.152.46线段五140.6916,596.3458105.7133，685.44795.7219.144圆弧五105.7133，685.447115.3079,699.0835115，6891020.048.014线段六115.3079,699.0835270.5862,689.9828155.231.049圆弧六270.5862,689.9828272.0119,689.7955270，680101.4390.576线段七272.0119,689.7955366.744,670.396.7119.342圆弧七366.744,670.3369.3239,670369.3，680102.0691.044线段八369.3239,670429.3239,6706012圆弧八429.3239,670435.109,671.8432429.3，680106.16920468线段九435.109,671.8432533.9915,738.3119.139423.83圆弧九533.9915,738.3539.57,740539.57，730105.9172.367线段十539.57,740539.57,74013026圆弧十539.57,740679.47,642.3669.57，7301013.505.4线段十一679.47,642.3699.47,642.392.0918.42圆弧十一699.47,642.3701.518,638.16709.2，644.5104.691.874线段十二701.518,638.16727.7178，606.3641.2028.24圆弧十二727.7178，606.36730，600720，600106.89162.757线段十三730，600730，5208016圆弧十三730，520727.94，513.9179720，520106.53812.6线段十四727.94，513.9179492.202，206.26387.5977.5圆弧十四492.202，206.26491.12，204.60500，200101.980.79线段十五491.12，204.60428.6909，94.91126.2125.24圆弧十五428.6909，94.91422.0872，90.0817420，99.86108.433.37线段十六422.0872，90.08176，04314.5986.32机器人行走的总距离2714.3565.968从到到到到其可能行走的路线：图4.2-7用matlab可以求出：最终机器人行走的路线为线路一。起点坐标终点坐标圆弧圆心坐标圆弧半径直线长或弧长机器人行走总时间O到A到B到C到O线段一0，070.506,213.14224.544.9圆弧一70.506,213.1476.6064,219.406680，210109.053.62线段二76.6064,219.4066291.0754,296.7803227.99945.6圆弧二291.0754,296.7803297.2537,309.0814287.7，3061015.186线段三297.2537,309.0814229.5719,532.8946233.8246.76圆弧三229.5719,532.8946225.4967,538.3538220，530106.952.78线段四225.4967,538.3538144.5033,591.646296.9519.39圆弧四144.5033,591.6462140.6916,596.3458150，600106.152.46线段五140.6916,596.3458105.7133，685.44795.7219.144圆弧五105.7133，685.447115.3079,699.0835115，6891020.048.014线段六115.3079,699.0835270.5862,689.9828155.231.049圆弧六270.5862,689.9828272.0119,689.7955270，680101.4390.576线段七272.0119,689.7955366.744,670.396.7119.342圆弧七366.744,670.3369.3239,670369.3，680102.0691.044线段八369.3239,670429.3239,6706012圆弧八429.3239,670435.109,671.8432429.3，680106.16920468线段九435.109,671.8432533.9915,738.3119.139423.83圆弧九533.9915,738.3539.57,740539.57，730105.9172.367线段十539.57,740539.57,74013026圆弧十539.57,740679.47,642.3669.57，7301013.505.4线段十一679.47,642.3699.47,642.392.0918.42圆弧十一699.47,642.3701.518,638.16709.2，644.5104.691.874线段十二701.518,638.16727.7178，606.3641.2028.24圆弧十二727.7178，606.36730，600720，600106.89162.757线段十三730，600730，5208016圆弧十三730，520727.94，513.9179720，520106.53812.6线段十四727.94，513.9179492.202，206.26387.5977.5圆弧十四492.202，206.26491.12，204.60500，200101.980.79线段十五491.12，204.60428.6909，94.91126.2125.24圆弧十五428.6909，94.91422.0872，90.0817420，99.86108.433.37线段十六422.0872，90.08176，04314.5986.32机器人行走的总距离2714.3565.968从到最短时间路径求解：计算点到点的最短时间路径，根据3.3模型准备三可以表示出总时间的方程：由几何知识解得点的坐标为。其中，段的路径所用的时间为： 弧段所用的时间为： 段的路径所用的时间为： 通过软件（详见附录一），求出机器人由到的最短时间为94.75秒，转弯半径为11.97个单位。5、模型的优化与推广对于该模型，在计算最短时间时我们假设了走直线到转弯时没有加速度，速度直接从跳变到。在实际行走中，机器人的速度不会发生跳变，所以我们计算出的最短时间要大于实际所花费的时间。为此我们可以在加速度这一点对模型进一步优化。在实际生活中我们还可以应用于其他领域。例如：在管道铺设等城市规划领域；山路铺设等建筑领域等等。6、参考文献1邦迪，图论及其应用，西安，西安科学出版社，19842谭永基，数学模型，上海，复旦大学出版社，20113周建兴，matlab从入门到精通，北京，人民邮电出版社，20067、附录附录一：e=2.71828;pi=3.1415926;t1=(x2+y2-r2)(1/2)/5;t2=(300-x)2+(300-y)2-r2)(1/2)/5;h1=cos(x2+y2-r2)/(x2+y2)(1/2)*(x2+y2-r2)(1/2);h2=cos(300-x)2+(300-y)2-r2)/(300-x)2+(300-y)2)(1/2)*(300-x)2+(300-y)2-r2)(1/2);h3=cos(300-x)2+(300-y)2+x2+y2-180000)/(2*(300-x)2+(300-y)2)(1/2)*(x2+y2)(1/2);h=pi-h1-h2-h3;t3=r*h/(5/(1+e(10-0.1*r2);min=t1+t2+t3;r2-(75-x)2-(215-y)2=0;(215-y)/(75-x)=(210-y)/(80-x);r=10;x=80; Local optimal solution found. Objective value: 94.75295 Total solver iterations: 4 Variable Value Reduced Cost E 2.718280 0.000000 PI 3.141593 0.000000 T1 44.48834 0.000000 X 83.46318 0.000000 Y 206.5368 0.000000 R 11.96875 0.000000 T2 47.10852 0.000000 H1 0.5415172 0.000000 H2 0.5413861 0.000000 H3 0.7574371 0.000000 H 1.301252 0.000000 T3 3.156089 0.000000 Row Slack or Surplus Dual Price 1 0.000000 0.6556567E-01 2 0.000000 -2.425424 3 0.000000 -1.000000 4 0.000000 -1.000000 5 0.000000 2.425424 6 0.000000 2.425424 7 0.000000 2.425424 8 0.000000 -2.425424 9 0.000000 -1.000000 10 94.75295 -1.000000 11 0.000000 -0.5981185E-02 12 0.000000 -0.6379778E-02 13 1.968746 0.000000 14 3.463181 0.000000附录二： w=0inf224.4994237.49;inf0246.5378264.2;224.4994246.53780inf;237.49264.2inf0w = 0 Inf 224.4994 237.4900 Inf 0 246.5378 264.2000 224.4994 246.5378 0 Inf 237.4900 264.2000 Inf 0 d,DD=dijkstra1(w,1)d = 0 471.0372 224.4994 237.4900DD = 1 0 2 0 0 0 0 4 2 0 0 3 0 4 3 0附录三：w = 0 224.5000 237.4900 Inf 305.7800 Inf Inf Inf Inf Inf Inf 224.5000 0 Inf 178.1200 92.2000 Inf Inf Inf Inf Inf Inf 237.4900 Inf 0 240.2600 Inf Inf Inf Inf Inf 621.2900 Inf Inf 178.1200 240.2600 0 175.0000 159.5300 170.6600 230.4900 Inf 381.6100 Inf 305.7800 Inf Inf 175.0000 0 162.2500 Inf Inf Inf Inf Inf Inf Inf Inf 159.5300 162.2500 0 75.6600 Inf Inf Inf Inf Inf Inf Inf 170.6600 Inf 75.6600 0 60.0000 Inf 215.8700 Inf Inf Inf Inf 230.4900 Inf Inf 60.0000 0 96.9500 158.1100 Inf Inf Inf Inf Inf Inf Inf Inf 96.9500 0 Inf 111.3600 Inf Inf 621.2900 381.6100 Inf Inf 215.8700 158.1100 Inf 0 170.8800 Inf Inf Inf Inf Inf Inf Inf Inf 111.3600 170.8800 0 d,DD=dijkstra1(w,1)d = 0 224.5000 237.4900 402.6200 305.7800 468.0300 543.6900 603.6900 700.6400 759.5600 812.0000DD = 1 2 0 0 0 0 0 0 0 0 0 2 0 3 0 0 0 0 0 0 0 0 0 3 0 0 4 0 0 0 0 0 0 0 0 0 0 5 6 0 0 0 0 0 0 0 4 5 0 0 0 0 0 0 0 0 0 0 6 0 0 7 0 0 0 0 0 0 0 0 0 7 0 8 0 0 0 0 0 0 0 0 0 8 0 9 0 0 0 0 0 0 0 0 0 9 0 10 0 0 0 0 0 0 0 0 0 10 0 11 0 0 0 0 0 0 0 0 0 11 0附录四：w=224.5237.49infinfinfinfinfinfinfinfinf;224.50inf341.76420.59infinfinfinfinfinfinf;237.49inf0318.43inf187.95184.39infinfinfinfinf;inf341.76318.430infinfinfinf169.71infinfinf;inf420.59infinf0156.6inf36.64204.21356.09infinf;infinf187.95inf156.60infinfinfinfinfinf;infinf184.39infinfinf0133.04infinfinfinf;infinfinfinf36.64inf133.040238.54387.81infinf;infinfinf169.71204.21infinf238.540170infinf;infinfinfinf356.09infinf387.81170080inf;infinfinfinfinfinfinfinfinf80043.59;infinfinfinfinfinfinfinfinfinf43.590? Error using = vertcatAll rows in the bracketed expression must have the same number of columns. w=0 224.5237.49infinfinfinfinfinfinfinfinf;224.50inf341.76420.59infinfinfinfinfinfinf;237.49inf0318.43inf187.95184.39infinfinfinfinf;inf341.76318.430infinfinfinf169.71infinfinf;inf420.59infinf0156.6inf36.6420