行走机器人避障问题[1]

一 、问题重述下图是一个100×80的平面场景图，在R（0，0）点处有一个机器人，机器人只能在该100×80的范围内活动，图中四个矩形区域是机器人不能与之发生碰撞的障碍物，障碍物的数学描述分别为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个单位，即矩形沿横轴方向长5×2=10个单位，10表示从中心沿纵轴方向上下各10个单位，即矩形沿纵轴方向长10×2=20个单位，所以，障碍物B1的中心在（20，40），大小为10×20个单位的矩形，其它三个障碍物的描述完全类似在平面场景中、障碍物外指定一点为机器人要到达的目标点（要求目标点与障碍物的距离至少超过1个单位），为此，须要确定机器人的最优行走路线——由直线段和圆弧线段组成的光滑曲线，其中圆弧线段是机器人转弯路线，机器人不能折线转弯，转弯路径是与直线相切的一圆形曲线段，也可以是两个或多个相切的圆弧曲线段组成，但每个圆形路线的半径都必须大于某个最小转弯半径，假设为1个单位。

另外，为了不与障碍物发生碰撞，要求机器人行走线路与障碍物间的最短距离为1个单位，越远越安全，否则将发生碰撞，若碰撞发生，则机器人无法到达目标点，行走失败请回答如下问题：1. 场景图中有三个目标点A（50，40）、B（75，60）、C（95，20），请用数学建模的方法给出机器人从R（0，0）出发安全到达每个目标点的最短路线2. 求机器人从R（0，0）出发，依次安全通过A、B到达C的最短路线二、 问题分析1、问题一中要求求定点R（0, 0）按照一定的行走规则绕过障碍物到达目标点的最短路径，我们先可以包络线画出机器人行走的危险区域，这样的话，拐角处就是一个半径为1的四分之一圆弧，通过那么然后采用拉绳子的方法寻找可能的最短路径（比如求R和A之间的最短路径，我们就可以连接R和A之间的一段绳子，以拐角处的圆弧为支撑拉紧，那么这段绳子的长度便是R到A的一条可能的最短路径），然后采用穷举法列出R到每个目标点的可能路径的最短路径，然后比较其大小便可得出R到目标点的最短路径2、问题二中要求求定点R（0， 0）经过中间的若干点按照一定的规则绕过障碍物到达目标点，这使我们考虑就不仅仅是经过障碍物拐点的问题，也应该考虑经过路径中的目标点处转弯的问题，这时简单的线圆结构就不能解决这种问题，我们在拐点及途中目标点处都采用最小转弯半径的形式，也可以适当的变换拐点处的拐弯半径，使机器人能够沿直线通过途中的目标点，然后建立优化模型对这两种方案分别进行优化，最终求得最短路径。

三、 模型假设1、假设障碍物全是矩形2、假设机器人能够抽象成点来处理四、符号说明符号符号说明L路径的总长度第段切线的长度第段圆弧的长度转弯半径障碍物上的任意点与行走路径之间的最短距离五、模型的建立5.1 先来证明一个猜想：猜想一：具有圆形限定区域的最短路径是由两部分组成的：一部分是平面上的自然最短路径（即直线段），另一部分是限定区域的部分边界，这两部分是相切的，互相连接的即问题分析中的拉绳子拉到最紧时的状况）证明：假设在平面中有A（a，0）和B（-a，0）两点，中间有一个半圆形的障碍物，证明从A到B的最路径为AB 平面上连接两点最短的路径是通过这两点的直线段，但是连接两点的线段于障碍物相交，所以设法尝试折线路径在y轴上取一点C（0，y），若y适当大，则折线ACB与障碍物不相交，折线ACB的长度为： 显然随着y的减小而减小，减小得，即,使得与与障碍物相切，切点分别为E和F,显然是这种折线路径中最短的由于满足0<<的角满足<,所以易知弧度EF小于的长， 即

下面在考察一条不穿过障碍物的任何一条路径，设其分别于OE和OF的延长线交与P、Q两点，记A和P之间的路径长度为，显然>|AP|,又由AEEO,所以|AP|>AE,从而>AE,同理可得>BF再来比较PQ之间路径长度和圆弧EF的长度的大小若PQ之间的路径可有极坐标方程,则有,可得：=-- 亦即路径APQB的长度超过路径AEFB的长度以上证明足以说明了AEFB是满足条件A到B的最短路径5.2 模型准备一 有了4.1中的定理，我们就可以这样认为，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个线圆结构所组成在本题中存在障碍物的状况，且障碍物在拐点处的危险区域是一个半径为1的圆弧，所以结合定理一，我们易知，求两点之间的最短路径中的转弯半径我们应该按照最小的转弯半径来算才能达到最优线圆结构5.21 1）如上图，设A（为起点，B（为目标点，C（和D（分别为机器人经过拐点分别于隔离危险线拐角小圆弧的切点，圆心为O（，圆的半径为r，AB的长度为a，AO的长度为b，BO的长度为c，角度=, =, =，.求AB的长度，设为L.解法如下：如上图可得有以下关系：：在：在中：所以：从而可得：2）而对于下图两种情况我们不能直接采用线圆的结构来解决，需要做简单的变换。

情况一：线圆结构5.22 我们假设两圆心坐标分别为和,半径均为r，M点坐标为，那么我们很容易可以求得:这样我们就可以利用1）中的方法，先求A到M，再求M到B，这样分两段就可以求解同理如果有更多的转弯，我们同样可以按照此种方法分解情况二：线圆结构5.23 这里我们依然设圆心坐标分别为和,半径均为r，这样我们可以得到：那么直线方程为：因为公切线DE与平行，那么DE的直线方程可以表示为：其中：那么把公切线的方程于圆的方程联立，渴可以求得切点D和E的坐标这样用D和E任意一点作为分割点都可以将上图分割成两个4.21所示的线圆结构，这样就可以对其进行求解同理多个这样的转弯时，用同样的方法可以进行分割5.3 模型准备二 一、对于从起点经过若干点然后再到达目标点的状况，因为不能走折线路径，我们就必须考虑在经过路径中的一个目标点时转弯的状况为了研究这个问题的方便，我们先来证明一个猜想：猜想二:如果一个圆环可以绕着环上一个定点转动，那么过圆环外两定点连接一根绳子，并以该圆环为支撑拉紧绳子，达到平衡状态时，圆心与该顶点以及两条切线的延长线的交点共线 图5.31证明猜想：如图4.31所示，E点就是圆环上的一个顶点，AB就是拉紧的绳子，就是切线AC和BD的延长线的交点，证明、E、三点共线。

我们可以用力学的知识进行证明，因为是拉紧的绳子，所以两边的绳子拉力相等，设为，它们的合力设为，定点对圆环的作用力设为那么由几何学的知识我们可以知道一定与共线，而又由力的平衡条件可知：=-即与共线综上所述、和三点一定共线二、有了以上这个定理我们可以建立以下模型：如图4.32，要求求出机器人从A绕过障碍物经过M点到达目标点B的最短路径，我们采用以下方法：用一根钉子使一个圆环定在M点，使这个圆环能够绕M点转动然后连接A和B的绳子并以这些转弯处的圆弧为支撑（这里转弯处圆弧的半径均按照最小转弯半径来计算），拉紧绳子，那么绳子的长度就是A到B的最短距离我们可以把路径图抽象为以下的几何图形下面我们对这段路径求解：图5.32如图，A是起点，B是终点，和是两个固定的圆，是一个可以绕M(p，q)点转动的圆环，三个圆的半径均为r，C、D、E、F、G、H均为切点a、b、c、e，f分别是A、、A、A、的长度A、B、、均是已知点，是未知点那么最短路径就可以表示为：L=|AC|++|DE|++|FG|++|HB|因为点的坐标未知，所以我们就不能用模型一中的线圆结构对其进行求解故得先求出点的坐标设坐标为（m，n），、、、、分别为（=1、2、3、4、5），、、分别为、、。

这样便有以下关系：在中：在中：在中：在中：则：又因为一定会在的角平分线上，所以满足：我们采用向量的形式来求，易知的一个方向向量：而与垂直，故其一个方向向量：而：所以：综合以上式子可以求得的坐标，从而可以得出路径的长为：=+HB，这可以采用模型一中的线圆结构来求解5.4 模型准备三求解从起点经过若干个点再到达目标点的问题，与4.4不同，我们还可以有另一种方案，即适当扩大障碍物拐点处的拐弯半径使机器人能够沿直线通过路径中的目标点这样拐点处拐弯圆弧的半径和圆心都是个变量，对于该题，那么我们可以首先设定三个圆心、、,然后按照以下步骤进行作图：1） 给定，以为圆心，为半径，画圆，然后过R点做圆的切线，切点为D然后过A点做的切线设为，切点为E2） 然后做F垂直于，垂足为F，F的长就是，然后以为圆心，为半径画圆很显然能由来确定，即3） 然后过B点做的切线为，切点为G，再过F垂直于，垂足为H，那么H的长度就是，然后以为圆心，为半径，画圆很显然能由来确定，即=4） 过C做的切线这就完成了由R经过A和B在到达C的路径5） 然后再变换、、、、、，可得到新的路径找出最小者即可5.5 模型的建立 假设机器人从起点R到到目标点，由4.2知路径一定是由圆弧和线段组成，设有m条线段，n条圆弧。

那么目标函数可以表示为：用此模型就可以对起点到目标点之间的路径进行优化求解六、模型的求解6.1 问题一一、以下给出了R到个目标点的可能路径的最短路径：1）如图一，解决的就是R到目标点A的最短路径问题，很显然的一个问题是机器人从的上方走的最短路径肯定是大于机器人从下方走的最短路径，所以机器人从方向走的最优路线我们在图一中没有给出图一中，蓝线就可以为机器人隔出了危险区域，红线表示的就是通过点的圆为支撑而拉紧的绳子，这样计算出绳子的长度就是R到A的最短路径2）如图二，解决的是R到目标点B的最短路径问题，图中给出了可能的三条路径的最短路径（图中的红线所示），我们可以分别计算出三条可能路径的最短路径的长度，然后进行比较，取最小者就是R到目标点B的最优路径3)如图三，解决的是R到目标点C的最短路径问题图中给出了两条可能路径的最短路径（图中的红线所示），我们同样可以分别计算出两条可能的最短路径，取最小者就是R到目标点C的最优路径图5.11图6.12图6.13二、 然后用matlab求解结果如下：1)R到目标点A的最短距离为70.50762)R到目标点B的可能路径有三条，即就有三条可能的最短路径① 如图二，机器人走最上面这条路径到达B，直接用matlab求得最短路径为114.1611② 而机器人经过中间一条路径到达B,这条路径由三条线段和两段圆弧组成，直接用三中的解法是结不出来的。

于是我们做了如下变换，先求出中间一条直线的中点设为F，那么可以采用三中的解法，分别求R到F和F到B的最短路径，然后两段相加，便可求出R到B的最短路径求解结果为107.9587③ 机器人经过最下边一条路径，同理这条路径由四条直线和三个圆弧组成，同样可以采取②中的变换，分三部分求解求解结果如下为1。