行走机器人避障问题

机器人行走问题摘要本文研究了机器人避障最短路径的问题。主要研究了在一个区域中存在四个障碍物，由出发点到达目标点以及由出发点经过途中的若干目标点到达最终目标点的两种情形。我们通过证明具有圆形限定区域的最短路径是由两部分组成的：一部分是平面上的自然最短路径（即直线段），另一部分是限定区域的部分边界，这两部分是相切的，互相连接的。依据这个结果，我们可以认为最短路径一定是由线和圆弧做组成，因此我们建立了线圆结构，这样无论路径多么复杂，我们都可以将路径划分为若十个这种线圆结构来求解。对于途中经过节点的再到达目标点的状况，我们采用了两种方案，一种是在拐点和节点都采用最小转弯半径的形式，另一种是适当扩大拐点处的转弯半径，使得机器人能够沿直线通过途中的目标点。然后建立了最优化模型对两种方案分别进行求解。问题一，我们很容易分解成线圆结构来求解，然后把可能路径的最短路径采用穷举法列举出来，最终得出最短路径：RTA最短路径为：70.5076RTB最短路径为：107.9587RTC最短路径为：102.0514问题二，我们方案都进行优化，求得最终结果：第一种方案最短路径为：156.471第二种方案最短路径为：157.752关键词最短路径最优化模型避障路径解析几何一、问题重述下图是一个100X80的平面场景图，在R(0,0)点处有一个机器人，机器人只能在该100X80的范围内活动，图中四个矩形区域是机器人不能与之发生碰撞的障碍物，障碍物的数学描述分别为B1(20,40；5,10)、B2(30,30；10,15)、B3(70,50；15,5)、B4(85,15；5,10),其中B1(20,40；5,10)表示一个矩形障碍物，其中心坐标为(20,40),5表示从中心沿横轴方向左右各5个单位，即矩形沿横轴方向长5X2=10个单位，10表示从中心沿纵轴方向上下各10个单位，即矩形沿纵轴方向长10X2=20个单位，所以，障碍物B1的中心在(20,40)，大小为10X20个单位的矩形，其它三个障碍物的描述完全类似。1ie11(20/)1ie11(20/))8060(75,6。)B3(70,50;15,5)20B2(30,30;1Q15)204060SO__20B2(30,30;1Q15)204060SO__I..10040A(50,40)64(85,15;5,10)■•i■C(95,啊在平面场景中、障碍物外指定一点为机器人要到达的目标点(要求目标点与障碍物的距离至少超过1个单位)，为此，须要确定机器人的最优行走路线——由直线段和圆弧线段组成的光滑曲线，其中圆弧线段是机器人转弯路线，机器人不能折线转弯，转弯路径是与直线相切的一圆形曲线段，也可以是两个或多个相切的圆弧曲线段组成，但每个圆形路线的半径都必须大于某个最小转弯半径，假设为1个单位。另外，为了不与障碍物发生碰撞，要求机器人行走线路与障碍物间的最短距离为1个单位，越远越安全，否则将发生碰撞，若碰撞发生，则机器人无法到达目标点，行走失败。请回答如下问题：场景图中有三个目标点A(50,40)、B(75,60)、C(95,20),请用数学建模的方法给出机器人从R(0,0)出发安全到达每个目标点的最短路线。求机器人从R(0,0)出发，依次安全通过A、B到达C的最短路线。二、问题分析1、问题一中要求求定点R（0,0）按照一定的行走规则绕过障碍物到达目标点的最短路径，我们先可以包络线画出机器人行走的危险区域，这样的话，拐角处就是一个半径为1的四分之一圆弧，通过那么然后采用拉绳子的方法寻找可能的最短路径（比如求R和A之间的最短路径，我们就可以连接R和A之间的一段绳子，以拐角处的圆弧为支撑拉紧，那么这段绳子的长度便是R到A的一条可能的最短路径），然后采用穷举法列出R到每个目标点的可能路径的最短路径，然后比较其大小便可得出日到目标点的最短路径。2、问题二中要求求定点R（0，0）经过中间的若干点按照一定的规则绕过障碍物到达目标点，这使我们考虑就不仅仅是经过障碍物拐点的问题，也应该考虑经过路径中的目标点处转弯的问题，这时简单的线圆结构就不能解决这种问题，我们在拐点及途中目标点处都采用最小转弯半径的形式，也可以适当的变换拐点处的拐弯半径，使机器人能够沿直线通过途中的目标点，然后建立优化模型对这两种方案分别进行优化，最终求得最短路径。三、模型假设1、假设障碍物全是矩形。2、假设机器人能够抽象成点来处理。四、符号说明符号符号说明L路径的总长度diljr第i段切线的长度第j段圆弧的长度转弯半径k障碍物上的任意点与行走路径之间的最短距离五、模型的建立5.1先来证明一个猜想：猜想一：具有圆形限定区域的最短路径是由两部分组成的：一部分是平面上的自然最短路径（即直线段），另一部分是限定区域的部分边界，这两部分是相切的，互相连接的。（即问题分析中的拉绳子拉到最紧时的状况）证明：假设在平面中有A(a,0)和B(-a,0)两点，中间有一个半圆形的障碍物，证明从A到B的最路径为A^B。平面上连接两点最短的路径是通过这两点的直线段，但是连接两点的线段于障碍物相交，所以设法尝试折线路径。在y轴上取一点C(0，y)，若y适当大，则折线ACB与障碍物不相交，折线ACB的长度为：IACB\=2<a2+y2显然IACBI随着y的减小而减小，减小y得y-y，即C—C，使得AC与CB与1111障碍物相切，切点分别为E和F,显然ACB是这种折线路径中最短的。由于满足10<。<；的角满足cc<Mnct,所以易知弧度EF小于ECF的长，即ST<^F,从而AE+m+FB<ACB,记线段AE、弧度EF、线段FB为AEFB,那么AEFB比任何折线1路径都短。下面在考察一条不穿过障碍物的任何一条路径，设其分别于OE和OF的延长线交与P、Q两点，记A和P之间的路径长度为二，显然二>|AP|,又由AE±EO,所以|AP|>AE,从而二〉AE,同理可得m；>BF。再来比较PQ之间路径长度；一；和圆弧EF的长度的大小。若PQ之间的路径可有极坐标方程"〜，则有：：:',可得:：—"二•「二一-M3亦即路径APQB的长度超过路径AEFB的长度。以上证明足以说明了AEFB是满足条件A到B的最短路径。5.2模型准备一有了4.1中的定理，我们就可以这样认为，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个线圆结构所组成。在本题中存在障碍物的状况，且障碍物在拐点处的危险区域是一个半径为1的圆弧，所以结合定理一，我们易知，求两点之间的最短路径中的转弯半径我们应该按照最小的转弯半径来算才能达到最优。线圆结构5.211）如上图，设A（二_，为起点，B（二.二）为目标点，C（二.二）和D（二一，「_）分别为机器人经过拐点分别于隔离危险线拐角小圆弧的切点，圆心为O（二.，圆的半径为r，AB的长度为a，AO的长度为b，BO的长度为c，角度ZAOB=cc,ZAOC邓，ZBOD=「，/COD=0.求ACBB的长度，设为L.解法如下：如上图可得有以下关系:a=寸(x-x)2+(y-y)2TOC\o"1-5"\h\z*2121*b=q(x-x)2+(y-y)25151c=J(x-x)2+(y-y)2〔¥5252在AA08中:,b2+c2-a2、a=arccos()2bc在RtAAOC:rP=arccos-b在RtAAOC中:ry=arccos-c所以:从而可得：L=i：'b2-r2+\，'c2-r2+r02）而对于下图两种情况我们不能直接采用线圆的结构来解决，需要做简单的变换。情况一：

线圆结构5.22我们假设两圆心坐标分别为03,J）和,J），半径均为r,M点坐标为1122（%,七），那么我们很容易可以求得:X=匚232这样我们就可以利用1）中的方法，先求A到M，再求M到B，这样分两段就可以求解。同理如果有更多的转弯，我们同样可以按照此种方法分解。情况二：线圆结构5.231122这里我们依然设圆心坐标分别为0（x,j）和0'（x,j），半径均为r，这样我1122K00K00—X-X那么00'直线方程为:"K00'（X-X1）+j1因为公切线DE与00平行，那么DE的直线方程可以表示为:其中:C=尸\：'1+七2那么把公切线的方程于圆的方程联立，渴可以求得切点D和E的坐标。这样用D和E任意一点作为分割点都可以将上图分割成两个4.21所示的线圆结构，这样就可以对其进行求解。同理多个这样的转弯时，用同样的方法可以进行分割。5・3模型准备二一、对于从起点经过若干点然后再到达目标点的状况，因为不能走折线路径，我们就必须考虑在经过路径中的一个目标点时转弯的状况。为了研究这个问题的方便，我们先来证明一个猜想：猜想二：如果一个圆环可以绕着环上一个定点转动，那么过圆环外两定点连接一根绳子，并以该圆环为支撑拉紧绳子，达到平衡状态时，圆心与该顶点以及两条切线的延长线的交点共线。图5.31证明猜想：如图4.31所示，E点就是圆环上的一个顶点，A赤B就是拉紧的绳子，02就是切线AC和BD的延长线的交点，证明O「E、O2三点共线。我们可以用力学的知识进行证明，因为是拉紧的绳子，所以两边的绳子拉力相等，设为F，它们的合力设为孔，定点对圆环的作用力设为F1。那么由几何学的知识我们可以知道F一定与qq共线，而又由力的平衡条件可知:F=-FTOC\o"1-5"\h\z01>>-即OO与EO共线。22

综上所述q、e和q三点一定共线。二、有了以上这个定理我们可以建立以下模型：如图4.32，要求求出机器人从A绕过障碍物经过M点到达目标点B的最短路径，我们采用以下方法：用一根钉子使一个圆环定在M点，使这个圆环能够绕M点转动。然后连接A和B的绳子并以这些转弯处的圆弧为支撑（这里转弯处圆弧的半径均按照最小转弯半径来计算），拉紧绳子，那么绳子的长度就是A到B的最短距离。我们可以把路径图抽象为以下的几何图形。下面我们对这段路径求解：图5.32图5.32O(x,j)和O(x,j)是两个固定如图，A3,J）是起点，B3,J）是终点，的圆，O2是一个可以绕M（p，q）点转动的圆环，三个圆的半径均为r，C、D、E、F、G、H均为切点。a、b、c、e，f分别是AO、OO、AO、AO、OO的长1122323度。O(x,j)和O(x,j)是两个固定L=|AC|+E+|de|+M+|fg|+M+|hb|因为O2点的坐标未知，所以我们就不能用模型一中的线圆结构对其进行求解。故得先求出O点的坐标。设O坐标为（m,n）,AAC、ZAOO、ZAOO、211221ZAOO、ZOOF分别为a(i=1、2、3、4、5),ZCD、ZEOF、ZEOM2332i122分别为气、七、9。这样便有以下关系:a=J(x—x)2+(y—y)2*1313b=扣③—m)2+(y3—n)2<c=J(x—m)2+(y—n)2e=q(x—x)2+(y—y)2414f=\l(x4—m)2+(y4—n)2在RtAAOC中:1ra1=arccos—在AAOO中:12a2+b2一c2a=arccos^^b2+c2一a2a=arccos妥在AAOO中:23c2+f2—e2a=arccos2-f在RtANOF中:22ra=arccosf则：9=空—a—a12121A3兀9=—a—a—a22345又因为MO2一定会在ZEO2F的角平分线上，所以满足:七我们采用向量的形式来求，易知qq的一个方向向量:I=(i,L)1x-n»»而OE与OO垂直，故其一个方向向量:212而:>OM-(p-m,q-n)2所以:—►*-l•OM0=arccos~2,112IIO2MI综合以上式子可以求得O2的坐标，从而可以得出路径的长为:L=Ja2—r2+0r+b+0r+2,气）2—r2+11。二丽+HB，这可以采用模型一中的线圆结构来求解。5.4模型准备三求解从起点经过若干个点再到达目标点的问题，与4.4不同，我们还可以有另一种方案，即适当扩大障碍物拐点处的拐弯半径使机器人能够沿直线通过路径中的目标点。这样拐点处拐弯圆弧的半径和圆心都是个变量，对于该题，那么我们可以首先设定三个圆心q、O2、O3，然后按照以下步骤进行作图：1）给定R，以O为圆心，R为半径，画圆，然后过R点做圆O的切线L，切TOC\o"1-5"\h\z11111点为D。然后过A点做O］的切线设为L2，切点为E。2）然后做OF垂直于L，垂足为F，OF的长就是R，然后以O为圆心，R为222222半径画圆。很显然R能由R来确定，即R=f（R）。2121

3）然后过B点做O的切线为L，切点为G,再过OF垂直于L，垂足为H,那TOC\o"1-5"\h\z2333么OH的长度就是R，然后以O为圆心，R为半径，画圆。很显然R能3333由R2来确定，即R3=f（R2）。4）过C做O3的切线。这就完成了由R经过A和B在到达C的路径。5）然后再变换O、O、O、R、R、R，可得到新的路径。找出最小者即可。1231235.5模型的建立假设机器人从起点R到到目标点M0，由4.2知路径一定是由圆弧和线段组成，设有m条线段，n条圆弧。那么目标函数可以表示为：minL=£d.+»i=1j=1s.t=<r>1s.t=<用此模型就可以对起点到目标点之间的路径进行优化求解。六、模型的求解6.1问题一一、以下给出了日到个目标点的可能路径的最短路径：1）如图一，解决的就是R到目标点A的最短路径问题，很显然的一个问题是机器人从三：的上方走的最短路径肯定是大于机器人从三-下方走的最短路径，所以机器人从《方向走的最优路线我们在图一中没有给出。图一中，蓝线就可以为机器人隔出了危险区域，红线表示的就是通过"点的圆为支撑而拉紧的绳子，这样计算出绳子的长度就是R到A的最短路径。2）如图二，解决的是R到目标点B的最短路径问题，图中给出了可能的三条路径的最短路径（图中的红线所示），我们可以分别计算出三条可能路径的最短路径的长度，然后进行比较，取最小者就是R到目标点B的最优路径。3）如图三，解决的是R到目标点C的最短路径问题。图中给出了两条可能路径的最短路径（图中的红线所示），我们同样可以分别计算出两条可能的最短路径，取最小者就是R到目标点C的最优路径。

图6.13二、然后用matlab求解结果如下：R到目标点A的最短距离为70.5076R到目标点B的可能路径有三条，即就有三条可能的最短路径。如图二，机器人走最上面这条路径到达B，直接用matlab求得最短路径为114.1611而机器人经过中间一条路径到达B,这条路径由三条线段和两段圆弧组成，直接用三中的解法是结不出来的。于是我们做了如下变换，先求出中间一条直线的中点设为F，那么可以采用三中的解法，分别求R到F和F到B的最短路径，然后两段相加，便可求出R到B的最短路径。求解结果为107.9587机器人经过最下边一条路径，同理这条路径由四条直线和三个圆弧组成，同样可以采取②中的变换，分三部分求解。求解结果如下为116.1256综合①②③所述，日到目标点B的最短路径为107.9587R到目标点C的可能路径由两条，和2中的方法一样，最终求解结果R到目标点C的最短路径为102.05146.2问题二一、我们先按照4.3中的思想来进行求解，这样我们可以做出5.21所示的示意图:显然运用模型准备二中的方法求解的结果小于用模型准备三中的方法求解的结果，说明模型准备二的方法优于模型准备三的方法。七、模型推广7.1问题深入分析在本题中只有四个障碍物，按照线圆结构画出从起点到达目标点的路径是有限的，我们完全可以采用穷举法把这些路径列出来，然后比较大小取最小者即可，但是我们可以设想如果这个区域内有n个障碍物，那么按照线圆结构从起点到达目标点的可能路径就有无数多条，这时我们如果在采用穷举法是不现实的。所以我们必须寻求新的算法来解决这个问题。由上述分析我们有了这样一个想法：先求出所有的切线，包括出发点和目标点到所有圆弧的切线以及所有圆弧与圆弧之间的切线，然后把这且曲线看成是图6.11中的，给这些定点赋一个等于切线长度的权值，如果某两条切线有一个公切圆弧，则代表这两条曲线的顶点是一条直线的两个端点，边上的权值等于这两条切线之间的劣弧长度。然后在这张图中加一个源点和终点，那么在所有代表出发点与其它圆弧之间切线的顶点与源点连成一条边，权值均为0，同理在所有代表目标点到其它圆弧切线的顶点与终点连成一条边，权值均为0，这样题目就转化成了求源点到达终点之间的最短路径问题了，这里最短路径就是指经过所有顶点与边的权值之和最小。我们可以采用Dijkstra算法进行求解。7.2模型的进一步求解根据6.1的分析，在有若干个障碍物的区域中，我们把按照线圆结构画出从出发点到目标点的路径图依据6.1中的想法转换成了下面这张图，图中的A和G点就是添加的源点和终点，其它节点均是出发点和目标点到圆弧的切线和圆弧与圆弧之间的切线转化而成。图7.21对于最短路径的求解，有以下步骤：1）我们画出出发点和目标点和各个圆弧的切线，以及圆弧与圆弧之间的切线，但是切线有可能经过障碍物的内部或危险区域，也可能出现切线重复的状况，既有很多不合法的切线。于是我们对模型做了以下修正：1、检验切线两个端点是否在障碍物内部。2、检验切线是否障碍物的对角线相交。3、检验圆弧所对应的圆心，即障碍物的顶点到切线的距离是否小于1。如果以上三种情况满足其一，我们规定对应这段切线的顶点为M（M为无穷大）。4、另外还有如下图所示的一种特殊情况：两个大小相同在同一水平或者竖直位置上，不考虑切线满足1、2、3的状况它们由2条内公切线，8条外公切线，但是有6条外公切线是重复的。因此我们作如下规定：如果某条切线与某段圆弧相切，且切点不在切线的端点上，则该切线为不合法。权值矩阵中表示它的顶点也为M。ABCD图7.222）然后把合法的切线与这些切线之间的劣弧转化成如7.21所示的形式。假设转化过后有m条合法切线，那么就有m个顶点，设这些点的权值凡（1<i<m），即第i条合法曲线的长度。。/.为边的权值，即第/条弧的长度。3）然后把路径图转化成如图6.21所示，按照求得权值矩阵给图中的顶点及边长赋值。4）最后依据Dijkstra算法求得最短路径。八、模型评价一、模型优点1、运用多个方案对路径进行优化，在相对优化之中取得最优解。2、模型优化后用解析几何进行求解，精确度较高。3、模型简单易懂，便于实际检验及应用。二、模型缺陷1、此模型适于全局规划，获得精确解却失去了效率。2、在障碍物较多时，且形状不规则时，模型需要进一步改进。九、参考文献谭永基，数学模型，上海，复旦大学出版社，2011邦迪，图论及其应用，西安，西安科学出版社1984胡海星，RPG游戏中精灵的移动问题，杂志《程序员》2011；尤承业，解析几何，北京，北京大学出版社，2004周培德，计算几何一算法与设计，北京清华大学出版社，2005十、附录%求解一次转弯所经路线总长%T：初始点V:转弯圆弧圆心W:到达点functionresult=zongchang(T,W,V,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((TV"2+VW"2-TW”2)/(2*TV*VW));alpha2=acos(r/TV);alpha3=acos(r/VW);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角TS1二sqrt(TV"2-