2012年高教社杯数学建模D题--机器人避障问题论文

1、机器人避障问题摘要 本文研究了机器人避障最短路径和最短时间路径的问题。主要研究了在一个区域中存在12个不同形状障碍物，由出发点到达目标点以及由出发点经过途中的若干目标点到达最终目标点的多种情形，寻找出一条恰当的从给出发点到目标点的运动路径使机器人在运动中能安全、无碰撞的绕过障碍物而使用的路径和时间最短。由于规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径，机器人不能折线转弯。所以只要给定的出发点到目标点存在至少一个障碍物，我们都可以认为最短路径一定是由线和圆弧所组成，因此我们建立了切线圆结构，这样无论路径多么复杂，我们都可以将路径划分为若干个这种切线圆结构来求解。在没有危险碰撞

2、的情况下，圆弧的半径越小，路径应该越短，因此我们尽量选择最小的圆弧半径以达到最优。对于途中经过节点的再到达目标点的状况，我们采用了两种方案，一种是在拐点和节点都采用最小转弯半径的形式，另一种是适当扩大拐点处的转弯半径，使得机器人能够沿直线通过途中的目标点。然后建立了最优化模型对两种方案分别进行求解，把可能路径的最短路径采用穷举法列举出来，用lingo工具箱求解得出了机器人从o(0, 0)出发，oa、ob、oc和oabco的最短路径；利用matlab中的fminbnd函数求极值的方法求出了机器人从o (0, 0)出发，到达a的最短时间路径。本文提出一种最短切线圆路径的规划方法，其涉及的理论并不高

3、深，只是应用了几何知识和计算机程序、数学工具计算，计算简易，便于实现，能搞提高运行效率。问题一oa 最短路径为：=471.0372ob 最短路径为：853.8014oc 最短路径为：=1054.0oabco最短路径为：问题二机器人从o (0, 0)出发，到达a的最短时间路径：最短时间是94.5649，圆弧的半径是11.5035,路径长关键词 最短路径；避障路径；最优化模型；解析几何；数学工具 一 、问题重述图1是一个800800的平面场景图，在原点o(0, 0)点处有一个机器人，它只能在该平面场景范围内活动。图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物，障碍物的数学描述如下表：编

4、号障碍物名称左下顶点坐标其它特性描述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

5、)边长13011正方形(640, 520)边长8012长方形(500, 140)长300，宽60在图1的平面场景中，障碍物外指定一点为机器人要到达的目标点（要求目标点与障碍物的距离至少超过10个单位）。规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。机器人直线行走的最大速度为个单位/秒。机器人转弯时，最大转弯速度为，其

6、中是转弯半径。如果超过该速度，机器人将发生侧翻，无法完成行走。请建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。对场景图中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 800800平面场景图二、 问题分析1、 要求求定点o(0, 0)按照一定的行走规则绕过障碍物到达目标点的最短路

7、径，我们先可以包络线画出机器人行走的危险区域，在没有危险碰撞的情况下，圆弧的半径越小，路径应该越短。这样的话，拐角处就是一个以方形或三角形障碍物的顶点为圆心半径为10个单位的圆弧，如果是圆形障碍物就应该是以障碍物的圆心为圆心、障碍物的半径长加上10为半径的圆弧。2、 若经过中间的若干点按照一定的规则绕过障碍物到达目标点，这使我们考虑就不仅仅是经过障碍物拐点的问题，也应该考虑经过路径中的目标点处转弯的问题，这时简单的线圆结构就不能解决这种问题，我们在拐点及途中目标点处都采用最小转弯半径的形式，也可以适当的变换拐点处的拐弯半径，使机器人能够沿直线通过途中的目标点，然后建立优化模型对这两种方案分别进

8、行优化，最终求得最短路径。3、 这样机器人行走的路径就是由切线段、内公切线、外公切线以及圆弧组成的这里称之为切线圆路径。三、 模型假设1、假设障碍物只包含长方形、正方形、三角形、圆形。2、假设机器人能够抽象成点来处理。3、路径不考虑走平面场地的边界。五、符号说明在计算机程序输入的原始数据中:(t,v,w,r)表示t是起点坐标,v是圆弧的圆心坐标,w是目标节点坐标,r是圆弧半径.为便于叙述和计算，根据已知条件我们给12个障碍物中的11个方形和三角形顶点用字母或表示，其中t或s表示障碍物，中i表示第i号障碍物，中i表示第9+i号障碍物，j表示从左下角开始按顺时针数起第几个顶点。如下表一所示：表一编

9、号障碍物名称左下顶点坐标左上顶点坐标右上顶点坐标右下顶点坐标1正方形t1t11(300, 400)t12（300，600）t13（500，600）t14（500，400）2圆形t2 圆心坐标t2 (550, 450)，半径703平行四边形t3t31 (360, 240)t32（400，330）t33（540，330）t34（500，240）4三角形t 4t 41(280, 100)t42（345，210）t43（410，100）5正方形t5 t51 (80, 60)t52（80，210）t53（230，210）t54（230，60）6三角形t 6t 61(60, 300)t62（150，435

10、）t64（235，300）7长方形t 7t 71(0, 470)t72（0，530）t73（220，530）t74（220，470）8平行四边形t8t 81(150, 600)t82（180，680）t83（270，680）t84（240，600）9长方形t9t91 (370, 680)t92（370，800）t93（430，800）t94（430，680）10正方形s1s11(540, 600)s12（540，730）s13（670，730）s14（670，600）11正方形s2s21 (640, 520)s22（640，600）s23（720，600）s24（720，520）12长方形s3

11、s31(500, 140)s32（500，200）s33（800，200）s34（800，140）注：各个障碍物顶点如需要设计圆弧，粗略表示路径时圆弧的位置简单用障碍物顶点字母表示（如：表示从点o 经过6号三角形左顶角为圆心的圆弧到6号三角形上顶角为圆心的圆弧到7号方形右下角为圆心的圆弧到7号方形右上角为圆心的圆弧到8号菱形左下角为圆心的圆弧到达点b。标出的经过顶点都是需要设计圆弧的）符号符号说明l路径的总长度第段切线的长度第段圆弧的长度转弯半径障碍物上的任意点与行走路径之间的最短距离以顶点或为圆心的圆弧的两个切点 t路径时间六、模型的建立与求解由于规定机器人的行走路径由直线段和圆弧组成，其中

12、圆弧是机器人转弯路径，机器人不能折线转弯。据此可以这样认为，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个切线圆结构所组成。易知，求两点之间的最短路径中的转弯半径越小路径就越短，我们应该按照最小的转弯半径来算才能达到最优。根据要求机器人行走线路与障碍物间的最近距离为10个单位，因此在方形及三角形顶点转弯的地方圆弧半径，我们尽量取以顶点为圆心半径为r=10个单位的圆弧，如果是圆形障碍物就应该是以障碍物的圆心为圆心、障碍物的半径长加上10为半径的圆弧，只有在必要的时候对半径作适当的加大调整。6.1 模型 求从起点o(0, 0)到目标点a(300, 300)的最短路径。经过观察很显然从o到

13、a有两条选择的路径（其它路径需要经过过多的障碍物路径显然比较长不必考虑）如图6.11所示，一条是从5号障碍物的左上角走，一条是从右下角走。他们的路径结构图是类似的如图6.12所示。图6.11图6.12设为起点，为目标点，和 分别为机器人经过拐点分别于隔离危险线拐角小圆弧的切点，圆心为,圆的半径为，oa的长度为，的长度为，的长度为，角度=, .求的长度，设为.解法如下：如上图可得有以下关系：在：在中：所以：从而可得路径长： （1.1）即模型为：，由已知条件知即，利用计算机计算lingo算法(t是起点坐标v是圆弧圆心坐标w是过程目标点坐标r圆弧是半径)：function result=zongch

14、ang(t,v,w,r)tv=sqrt(t(1)-v(1)2+(t(2)-v(2)2);tw=sqrt(t(1)-w(1)2+(t(2)-w(2)2);vw=sqrt(v(1)-w(1)2+(v(2)-w(2)2);alpha1=acos(tv2+vw2-tw2)/(2*tv*vw);alpha2=acos(r/tv);alpha3=acos(r/vw);alpha4=2*pi-alpha1-alpha2-alpha3;ts1=sqrt(tv2-r2);s2w=sqrt(vw2-r2);s1s2hu=r*alpha4;result=ts1+s1s2hu+s2w;结果算得的长度=471.0372

15、又由 （1）和 （2）计算得点为圆心的圆弧两切点的坐标为。同样，通过计算可得从路径从右下角方向（圆心t的坐标改为5号障碍物右下角顶点坐标（230,60）走时长度498.4259.很显然机器人从5号障碍物左上角走的路径小于机器人5号障碍物右下角走的路径要短。因此从起点o(0, 0)到目标点a(300, 300)的最短路径是，长度=471.0372，圆弧pq的切点坐标为 。同时可以验证6号三角形障碍物右下角顶点到切线aq距离大于10，所以过的路径是安全的也是最短的。下面考虑问题二：从o到a的最短时间路径由式（1.1）可求得时间路径的目标函数为：，利用matlab中的fminbnd函数求极值f1=(

16、x*(2*pi-2.3231-acos(x/224.7221)-acos(x/237.6973)/(5/(1+exp(10-0.1*x2)+ sqrt(224.72212-x2)/5+ sqrt(237.69732-x2)/5;x_min,f_min,flag=fminbnd(f1,10,50)得结果：x_min =11.5035，f_min =94.5649，flag=1即最短时间是94.5649，圆弧的半径是11.5035。利用式（1）（2）算得两切点坐标,路径长. 6.2模型 求从起点o(0, 0)到目标点b(100, 700)的最短路径。根据障碍物的形状与位置，要从点o(0, 0)到点

17、b(100, 700)，经过观察我们给出了可能的4条路径的最短路径:图6.21(1)所示标出了3种路径，路径ob1：从点o 6号三角形左顶角6号三角形上顶角7号方形右下角7号方形右上角8号菱形左下角点b（简写成ob1:，经过各个障碍物顶点时都设计圆弧，这里圆弧的位置简单用顶点字母表示,以下同），路径ob2：,路径ob3：; 图6.21(2)所示标出了1种路径, 路径ob4：,这条路径是为了使（取半径为r=15）到(取半径为r=25)的路径成为直线增大了圆弧半径而考虑的.我们可以分别计算出4条可能路径的最短路径的长度，然后进行比较，取最小者就是o到目标点b的最优路径。图6.21(1)图6.21(

18、2) 分析：从路径观察，求解过程中我们会遇到以下三种情况，我们不能直接采用切线圆6.11（2）的结构来解决，需要做简单的变换。情况一：机器人经过6号三角形障碍物上顶点（150,435）和7号方形障碍物右下顶点（220,470）之间的路径以及7号方形障碍物右上顶点（220,530）和8号障碍物左下顶点（150,600）之间的路径时，其路径如下图6.22所示：6.22 我们假设两圆心坐标分别为和,半径均为r，m点坐标为，那么我们很容易可以求得:这样我们就可以利用1）中的方法，先求a到m，再求m到b，这样将上图分割成两个6.11（2）所示的线圆结构，分两段就可以求解。情况二：机器人经过6号三角形障碍

19、物的左下顶点（60,300）到上顶点（150.435）和经过7号方形障碍物右下顶点（220,470）到右上顶点（220,530）时路径如下图6.12（3）所示：图6.23 这里我们依然设圆心坐标分别为和,半径均为r，这样我们可以得到：那么直线方程为：因为公切线de与平行，那么de的直线方程可以表示为：其中：那么把公切线的方程与圆的方程联立，就可以求得切点d和e的坐标。这样用d和e任意一点作为分割点都可以将上图分割成两个6.12所示的线圆结构，这样就可以对其进行求解。同理多个这样的转弯时，用同样的方法可以进行分割。 情况三，两切线圆的半径不相同如下图6.24所示，只要在6.23的基础上作适当的调

20、整即可。 图6.24 图6.24中大圆弧小圆弧的切点坐标就不能用式（1）（2）来计算，为此我们设大圆的圆心，半径为r,小圆的圆心,半径为r,切点坐标为，则大圆： (3)小圆： (4)利用（3）式和（4）式就可以算出大圆弧和小圆弧的切点的坐标。显然在路径中不管是出现这三种情况中的那种情形，都可以将图形分割成两个6.12所示的线圆结构来解出路径的长。因此我们把图6.12中的模型的一个完整线圆结构路径长（两条切线连一段圆弧）称为一个子任务，那么假设机器人从起点o到目标点，由分析知路径一定是由圆弧和线段组成，就可以看成若干个子任务的和，设从o到路径中有n个障碍物是过程目标节点，那么整个路径长就有n个子

21、任务，那么目标函数可以表示为：表示第i个任务的路径长。用此模型就可以对起点到目标点之间的路径进行优化求解,对计算过程中重复计算的一些切线要减去。这样利用计算机运算时只需重复利用上面的lingo算法然后求和就可以算出结果,非常简便。根据上述分析求路径ob1:，取各顶点圆弧的半径相同均为，路径中包含了6.12切线圆、情况一内公切线和情况二外公切线三种情形；由下面lingo算法function result=zongchang(t,v,w,r)tv=sqrt(t(1)-v(1)2+(t(2)-v(2)2);tw=sqrt(t(1)-w(1)2+(t(2)-w(2)2);vw=sqrt(v(1)-w(

22、1)2+(v(2)-w(2)2);alpha1=acos(tv2+vw2-tw2)/(2*tv*vw);alpha2=acos(r/tv);alpha3=acos(r/vw);alpha4=2*pi-alpha1-alpha2-alpha3;ts1=sqrt(tv2-r2);s2w=sqrt(vw2-r2);s1s2hu=r*alpha4;result=ts1+s1s2hu+s2w;function result=ltt(t,v,w,r)tv=sqrt(t(1)-v(1)2+(t(2)-v(2)2);tw=sqrt(t(1)-w(1)2+(t(2)-w(2)2);vw=sqrt(v(1)-w(

23、1)2+(v(2)-w(2)2);alpha1=acos(tv2+vw2-tw2)/(2*tv*vw);alpha2=acos(r/tv);alpha3=90*pi/180;alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角ts1=sqrt(tv2-r2);%ts1,ts2均为圆弧切线%s2w=vw;s1s2hu=r*alpha4;result=ts1+s1s2hu+s2w;结果:得路径ob1的长为:853.8014.路径ob2:的长可算得得945.9491路径ob3:可算得得917.7443路径ob4: （取半径为r=15, 取半径为r=25, 半径

24、不能取为10，是因为避免与6号三角形右下顶角的位置产生碰撞)：可算得得958.8002可以看出,最短路径为(r=10)，长853.8014下面由式（1）（2）计算最短路径中5段圆弧的切点坐标得： 圆弧圆心切点坐标切点坐标半径r=10以为圆心以为圆心以为圆心以为圆心以为圆心6.3模型 求从起点o(0, 0)到目标点c(700, 640)的最短路径。无论路径多么复杂，根据上面的分析我们都可以将路径划分为若干个这种切线圆结构来求解。因此求从起点o(0, 0)到目标点c(700, 640)的最短路径，目标函数与模型一样可以表示为：表示机器人从起点o到到目标点，要完成n个子任务，表示第i个任务的路径长（

25、两条切线连一段圆弧）。设从o到路径中有n个障碍物是过程目标节点，那么整个路径长就有n个子任务，其和即为整个路径长。 如下图6.31我们给出了从点o到点c的4条路径. ,(这里是2号圆形障碍物的同心圆，半径取r=70+10)(这里是2号圆形障碍物的同心圆，半径取r=70+10)图6.31由lingo算法得：路径oc1: ，路径长=1.1362 e+003=1136.2路径oc2:,路径长=1.0854 +003=1085.4路径oc3:,路径长=1.0639 e+003=1063.9路径oc4:,路径长=1.0540 e+003=1054.0 可以看出路径oc4:,路径长=1054.0是最短的.

26、下面由式（1）（2）计算最短路径中5段圆弧的切点坐标得： 圆弧圆心切点坐标切点坐标半径为圆心(232.1149，50.2262)(239.5323，56.9777)r=10为圆心(395.2092,338.7777)(398.1397，339.8254)r=10为圆心(476.5806，481.7742)(606.3314，393.1953)r=80为圆心(727.0414，512.8994)(730,520)r=10为圆心(730,580)(729.4868,583.1623)r=10 验证此路径跟沿途其它障碍物的安全性, t 41(280, 100),t31 (360, 240)到和之间的

27、圆切线的距离为10,故此路径是切实可行也是最短的路径.6.4模型 求机器人从oabco的最短路径。机器人要经过点a、b、c，这些点要么在路径的切直线段上（根据三点所处的位置这显然不可能），那么只能在a、b、c三点分别设立圆弧使得这三点分别在圆弧上，这样就必须确定三圆弧的圆心。由于oc的最优路径已经由模型iii求出为，由co中的路段可以采用。先给出如下模拟图6.41，得出oabco可能路径为2条： ,另外还可以有路径：图6.41目标函数仍可以表示为： 为了求出点a、b、c所在圆弧的圆心，我们设计了如下图形图6.42 图6.42已知圆，相当于点a在所求的圆弧上，所求的圆弧的圆心为。连接，过点作垂直于，取。1、 用两点式求p1p2的方程：，结合p4求p4p5的方程：；2、 设p3、p4的坐标为、，用p3、p4的距离建立方程，与组成方程组： 从而解出所求的圆心。把相关的数据代入求得过a、b、c的圆的圆心坐标如下：abc(296.8384，299.9327)(101.7541，697.3689)(703，641)(297.1029，298.7325) 用lingo算法得路径的长2726.7341的长2744.066的长2952.1182的长2969.45比较四条路径可以看出 是最短的路径，长为2726.7341 同时用式（1）（2）或（3）（4）可以算出切点所有的切点