基于弹性绳索拉伸的机器人避障问题

1、基于弹性绳索拉伸的机器人避障问题摘要本文研究了机器人避障的相关问题。在一个已知区域中存在12个障碍物，使用基于弹性绳索拉伸的方法，求解了由出发点到目标点的最短路径和最短时间路径。我们在禁区顶点以最小转弯半径转向为最优的前提下，对障碍物进行了加工，即将限定区域向外扩展并将顶点圆角化。那么最短路径就由两部分组成：一部分是平面上的直线段，另一部分是限定区域上部分弧构成。由于最短路径一定是由直线线段和圆弧做组成，而弹性绳索紧贴障碍物时，弹性绳索与直线线段和圆弧重合，并且弹性绳索有自然缩短的趋势，弹性绳处于紧绷状态，此时弹性绳长就是最短路径。问题一，将绳索系与起点和终点，使用拉伸弹性绳索的方法，找到所有

2、符合要求的绳索连结成的路径并计算路径长度，最终最短的绳长即为所求。由于符合要求的路径可能比较多，我们又使用了尺规作图进行简化了以及一般情况下的Dijkstra求解最短路径的方法。最终求得：OA最短路径长度为471.037OB最短路径长度为 853.13OC最短路径长度为 1092.82OABCO最短路径长度为2714.31问题二，由于机器人转弯时所行走的速度和转弯半径有关。而当转弯半径最小时相应的速度也最小。就必须平衡转弯半径和转弯时速度的这一对矛盾。本文通过极限状态的求解，计算出可能的最短时间路径。关键字：最短路径 切线长 弧长一、问题的重述图1是一个800800的平面场景图，在原点O(0,

3、 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平行四

4、边形(150, 600)底边长90，左上顶点坐标(180, 680)9长方形(370, 680)长60，宽12010正方形(540, 600)边长13011正方形(640, 520)边长8012长方形(500, 140)长300，宽60在图1的平面场景中，障碍物外指定一点为机器人要到达的目标点(要求目标点与障碍物的距离至少超过10个单位)。规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最

5、近距离为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的最短时间路径。注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人

6、行走的总距离和总时间。图1 800800平面场景图二、模型假设1、机器人可以抽象成为点；2、未到达终点前机器人行走过程不会意外停止；3、忽略影响机器人行走的外界因素，机器人可以准确转弯。4、假设路径是一条带有弹性的橡皮绳，有自动缩短的趋势3、 符号说明符号符号说明S起点G终点D机器人离障碍物的最小距离转弯半径Pi切点四、问题分析问题一：根据题中的限定条件，机器人的行走路线一定为直线和与直线相切的圆弧，我们对障碍物进行改造，使得障碍物也为直线和圆弧构成。求出所有符合条件的切线和直线段的长度，并寻求最短的路径即为我们需要的最短路径。另外这条路径还可以看做是栓在起点的一条弹性绳索，经由各个障碍物，并

7、以障碍物的拐角处的圆弧为支撑拉紧最终到达终点的路径，此时这段绳子的长度便是一条可能的最短路径。具体在搜索路径时，我们知道两点确定的直线段最短。那么就将起点和终点用弹性绳索连结，分别向上方和下方拉伸绳索，让绳索绕过障碍物。由于绳索带有弹性，将自动贴紧改造后的障碍物。此时的绳长就是需要的自然直线段和弧长的总和。问题二：由于机器人转弯时所行走的速度和转弯半径有关。转弯速度是转弯半径的增函数，当转弯半径最小时相应的速度也最小。就必须平衡转弯半径和转弯时速度的这一对矛盾。通过考虑各种极限状态，求解最短时间路径。五、模型的建立5.1相关定理证明1. 禁区顶点转弯最优，以最小转弯半径转向最优1如图所示：假设

8、在平面中有A(a，0)和B(-a，0)两点，中间有一个半圆形的障碍物，证明从A到B的最优路径为AB。平面上连接两点最短的路径是通过这两点的直线段，但是连接两点的线段于障碍物相交，所以设法尝试折线路径。在y轴上取一点C(0，y)，若y适当大，则折线ACB与障碍物不相交，折线ACB的长度为： 显然随着y的减小而减小，减小得，即,使得与与障碍物相切，切点分别为E和F,显然是这种折线路径中最短的。由于满足0的角满足,所以易知弧度EF小于的长， 即E,从而+FB|AP|,又由AEEO,所以|AP|AE,从而AE,同理可得BF。再来比较PQ之间路径长度和圆弧EF的长度的大小。若PQ之间的路径可有极坐标方程

9、,则有,可得：=- 亦即路径APQB的长度超过路径AB的长度。以上证明足以说明了AB是满足条件A到B的最短路径。2. 在中间目标点附近转向，且中间目标点位于圆弧中点处最优1如图所示：E点就是圆环上的一个顶点，AB就是拉紧的绳子，就是切线AC和BD的延长线的交点，证明、E、三点共线。我们可以用力学的知识进行证明，因为是拉紧的绳子，所以两边的绳子拉力相等，设为，它们的合力设为，定点对圆环的作用力设为。那么由几何学的知识我们可以知道一定与共线，而又由力的平衡条件可知：=-即与共线。综上所述、和三点一定共线。 5.2 障碍区域扩展题中要求机器人与障碍物距离d个单位，转弯半径最小为个单位，那么我们将障碍

10、物区域向外扩展形成新的禁区。如下图：d5.3 弹性绳索拉伸构造路径两点确定的直线段最短。将起点和终点用弹性绳索连结，分别向上方和下方拉伸绳索，让绳索绕过障碍物。由于绳索带有弹性，将自动贴紧改造后的障碍物。下图为示意图：5.4 禁区类型分析由于禁区顶点转弯最优，以最小转弯半径转向最优。我们将弹性绳索绕过圆角矩形的障碍区域，绳索与圆角矩形的顶点关系有3种。分别计算这三种情况的直线段与弧长。计算过程中涉及到的解析几何的公式见文献3情况一：起点和终点之间只有一个障碍区域，此情况可以抽象为下图所示：起点到终点的路径长度为：。通过几何的方法我们可以计算出切点、路径长度，具体计算见附录中算法LengthA。

11、情况二：起点和终点之间经过两个障碍区域，并且这两个障碍区域在弹性绳索的两侧，此种情况如下图所示：通过几何方法分析，可以得出两圆心连线的中点为公切线的中点。那么就可以转化为两个情况一的组合，分别按照情况一计算并求和即可。起点到终点的路径长度为：，其中F为的中点。这种情况下的中点坐标F(x，y)计算较简单。路径长度见算法LengthC。情况三：起点和终点之间两个障碍区域，并且这两个障碍区域在弹性绳索的同侧，此种情况如下图所示：P此种情况中，可以计算出的直线方程，该直线方程联立圆的方程即可算出切点。再由中点坐标公式计算出的中点P。P做为左侧图形的终点，右侧图形的起点，此时该情况就转化成了两个情况一。

12、分别计算并求和即可。起点到终点的路径长度为：，其中P为的中点。中点坐标计算方法见附录中算法M。路径长度见算法LengthB。5.5 中间结点的弧化处理机器人由O点出发经由A、B、C到达O点时，因为机器人不能折现转弯，因此机器人在到达中间结点时就应该提前转向，即中间结点的弧化处理。我们可以想象，在中间节点A上，套上半径为R的圆环。弹性绳索经过节点A的最佳拉法即为最优解。直观的看绳索绷紧后，A点与目标点的连线在两条切线的角平分线上时，目标点的位置最优。设中间目标点(a,b),最小转弯半径为。圆的中心位于轨迹上。圆的方程为。那么最优路径中的机器人中间目标点位于圆弧的中点，圆心与中间目标点的连线为圆环

13、的两条切线的角平分线上。即：，为圆的两条切线的斜率。这样就可以算出两条切线在圆上的切点，从而转换为已知的禁区类型。5.6 尺规作图处理路径从目标起点经由障碍物到达终点的路径可能很多，一一去计算工作量较大。由于机器人离障碍物的距离相比整个障碍物的长度来说很小，我们简化机器人经过障碍物不需要离开障碍物，并且可以折现转弯，这样做的路线比机器人实际走的路线要短。我们把所有可能的路径用尺规作图的方法4，由曲线转换为直线映射到坐标轴上。取得直线最短且相近的几条计算路径，通过前面建立的模型计算路径长度(由于忽略了离障碍物的距离和转弯转弧线弯，会存在误差)，取实际计算最短的一条路径。以搜索题中的OB为例，示意

14、图如下（实际绘制的图，和转换为电子版的图）：通过尺规作图的方式，将路径的各个线段搬到坐标轴横轴上，可以很清楚的看到红色的路径1即为所求。5.7 Dijkstra路径处理更一般的情况下，在有若干个障碍物的区域中，我们把按照线和圆弧结构画出从出发点到目标点的路径图抽象成数据结构图。结点和结点之间的权值为出发点和目标点到圆弧的切线长和弧长的和。通过Dijkstra算法2求得最短路径即可。示意图如下：其中：为起点，为终点。弧上的数字为切线长和弧长构成的权值。5.8 模型的建立假设机器人从起点S到到目标点G，由上文中证明路径一定是由圆弧和线段组成，设共有n条线段，m条圆弧。那么目标函数可以表示为：用此模

15、型就可以对起点到目标点之间的路径进行优化求解。6、 模型的求解6.1 问题一（1） OA。通过弹性绳索拉伸的办法，连结起点和终点OA。将弹性绳向下拉伸，可以绕过障碍物5的右下角，此为一种情况。继续拉伸经过障碍物4的右下角，显然此种情况的绳长更长，我们忽略不考虑。其它向下搜索的障碍物同理；将弹性绳向上拉伸，可以绕过障碍5的左上角，此为第二种情况。继续拉伸向上搜索所得的路径更长，忽略不予考虑。示意图如下：O到A的路径有两条，分别计算这两条路径的长度。将各点的数据通过程序的方法计算出路径长度为498.4259和471.0372。通过比较可知上行线路的路径较短。最短路径为：417.04，总时间为96.

16、02。具体数据如下表：起点终点圆弧圆心直线长或弧长时间线段一(0,0)(70.51，213.14)224.5044.90圆弧一（70.51，213.14）（76.61，219.41）(80,200)9.053.62线段二（76.61，219.41）（300，300）237.4947.50（2） OB。同样通过拉伸绳索的办法找到所有可能的路径。如图所示：通过尺规作图的方法，可以求得最靠近纵轴的路径为最短路径。通过计算求得最短路径为：853.13，总时间为179.36。具体数据如下表：起点终点圆弧圆心直线长或弧长时间线段一(0，0)(50.14，301.64)305.7861.16圆弧一(50.1

17、4，301.64)(51.70，305.57)(60，300)4.271.71线段二(51.70，305.57)(141.70，439.61)161.4532.29圆弧二(141.70，439.61)(150，435)(150，435)9.793.92线段三(150，435)(222.19，460.24)73.9914.80圆弧三(222.19，460.24)(230，470)(230，470)13.505.40线段四(230，470)(230，530)6012圆弧四(230，530)(225.50，538.35)(230，530)9.893.96线段五(225.50，538.35)(144.

18、50，591.65)96.9519.39圆弧五(144.50，591.65)(140.69，596.35)(150，600)6.152.46线段六(140.69，596.35)(100，700)111.3622.27（3） OC。同(2)，最短路径为1092.82，机器人行走总时间为223.34数据如下表：起点终点标圆弧圆心直线长或弧长时间线段一(0，0)(422.09，90.08)431.5986.32圆弧一(422.09，90.08)(428.69，94.91)(420，100)8.433.37线段二(428.69，94.91)(491.12，204.60)126.2125.24圆弧二(4

19、91.12，204.60)(492.20，206.26)(500，200)1.980.79线段三(492.20，206.26)(727.94，513.92)387.5977.52圆弧三(727.94，513.92)(720，520)(720，520)6.542.62线段四(730，520)(720，600)8016圆弧四(730，600)(727.72，606.36)(720，600)6.892.76线段五(727.72，606.36)(700，640)43.598.72（4） OABCO这种情况需要考虑经过A、B、C结点的提前转弯。通过障碍物向中间结点引切线，计算出两个切点的坐标，转换为已知

20、的禁区类型。通过类似(1)(2)(3)问的方法即可求出路径长度。下图为最优路径，经过计算知最短路径为2714.31，机器人行走总时间为565.96。具体数据如下表：圆弧圆心坐标直线长或弧长时间线段一224.5044.90圆弧一(80,210)9.053.62线段二228.0045.60圆弧二 (287,306)15.186.07线段三233.8246.76圆弧三(220,530)6.952.78线段四96.9519.39圆弧四(150,600)6.152.46线段五95.7219.14圆弧五(115,689)20.048.01线段六155.2531.05圆弧六(270,680)1.440.58

21、线段七96.7119.34圆弧七(370,680)2.611.04线段八6012圆弧八（430,680）6.172.47线段九119.1423.83圆弧九（540，730）5.922.37线段十13026圆弧十（670，730）13.505.40线段十一92.0918.42圆弧十一（709，644）4.691.87线段十二41.208.24圆弧十二（720，600）6.892.76线段十三8016圆弧十三（720，520）6.542.62线段十四387.5977.52圆弧十四（500，200）1.980.79线段十五126.2125.24圆弧十五（410，100）8.433.37线段十六431

22、.5986.326.2 问题二我们将的图形画出，如图下图。从图形的趋势上看，该函数为一增函数，当转弯半径在13开始逐渐稳定，并变为机器人行走的最大速度。 通过计算可知当，速度。通过构造所有可能的极限路径，并一一计算，算出花费时间最短的即为所求。七、模型的评价与应用7.1 模型评价1.模型的优点在本模型中，机器人可以看到区域中的全局障碍物。求解使用弹性绳索拉伸绕过障碍物的绳长。使用这种方法主要有以下的优点：(1)对障碍物的形状、大小没有要求，适应性强；(2)通过解析几何的方法，求解坐标、切线长、线段长等结果精确度高；(3)在障碍物较多的情况下，使用尺规作图寻找最短路径较为简便。2.模型的不足(1

23、)在给定区域障碍物较多的情况下，由弹性绳索组成的路径较多。计算路径时计算复杂度较大。(2)此模型获得的路径结果精确较高，但效率较低。7.2 模型应用在5.7中我们讨论了Dijkstra路径处理，并分析了可能遇到的障碍物区域，通过编写计算机程序，自动判别障碍区域，可以将这个模型应用到更一般的环境中。该模型可以用于机器人作业，以及游戏中NPC寻路的设计中。b8rp8 r_b8,I6 Rx7QlA;八、参考文献1佚名，行走机器人避障问题， 99399a.html，2012年9月8日2佚名，Dijkstra算法, .html,2012年9月8日3佚名,高中数学概念总结-解析几何公式,1/1childr

24、en9883.html,2012年9月7日4佚名,尺规作图在运输优化问题中的应用，b84ae45c3b358cf8.html，2012年9月8日附录相关程序使用Matlab编写程序1 LengthA 计算S经由O到G点，半径为R的距离function Path=LengthA(S,G,O,R) %计算S经由O到G点，半径为R的距离SO=sqrt(S(1)-O(1)2+(S(2)-O(2)2); %起点到圆心距离SG=sqrt(S(1)-G(1)2+(S(2)-G(2)2); %起点到终点距离OG=sqrt(O(1)-G(1)2+(O(2)-G(2)2);%圆心到终点的距离alphaa=acos

25、(SO2+OG2-SG2)/(2*SO*OG);alphab=acos(R/SO);alphac=acos(R/OG);alpha=2*pi-alphaa-alphab-alphac; %圆心角Path=sqrt(SO2-R2)+sqrt(OG2-R2)+R*alpha;程序2 LengthB计算S经由OA,OB同侧相切到G点，半径为R的距离function result=LengthB(S,G,OA,OB,R) %计算S经由OA,OB同侧相切到G点，半径为R的距离k=(OB(2)-OA(2)/(OB(1)-OA(1); %公切线斜率d=sqrt(1+k2)*R; %公切线截距syms x y

26、;eq1=(x-OA(1)2+(y-OA(2)2-R2; %OA圆的方程% eq1=subs(eq1);eq2=k*(x-OA(1)+OA(2)+R*sqrt(1+k2)-y;% eq2=subs(eq2);xa,ya=solve(eq1,eq2); %xa,ya为切线与OA的切点eq3=(x-OB(1)2+(y-OB(2)2-R2;% eq3=subs(eq3);xb,yb=solve(eq2,eq3);%xb,yb为切线与OB的切点xaa=xa(1);yaa=ya(1);xbb=xb(1);ybb=yb(1);XM=(xaa+xbb)/2;YM=(yaa+ybb)/2;M=XM,YM; %中点坐标result=eval(LengthA(S,M,OA,R)+LengthA(M,G,OB,R);程序3 LengthC计算S经由OA,OB异侧相切到G点，半径为R的距离function result=LengthC(S,