移动机器人避障问题

移动机器人避障问题

摘要

本文研究的是移动机器人避障最短路径问题。首先，把机器人看作为一个点，将障碍全部当作矩形，抽象成为一个简单的几何问题，由于机器人的行走路径必须是有由直线段与圆弧线段组成的光滑曲线，而且通过证明，最优行走路线是由切线-圆弧-切线结构组成的，并且机器人的行走路径都可看成有若干上述线圆线结构组成的，因此，机器人的行走路径的长度即可通过计算若干上述结构的长度取得，基于这一理论，我们进行了对问题一与问题二的研究。

针对问题一，要求两点间的最短路径，可因为存在障碍物，我们首先用包络线画出机器人不可行走的危险区域，其形状为圆角矩形，然后对本题附图和求解目标进行分析，可以发现，在起点和终点一定的情况下，转弯处的圆弧位置以及圆弧长度决定了行进路线的实际情况，也就是说：在寻求最佳路线时，核心的问题就是确定圆弧的相关信息，然后可以根据圆弧的位置及大小，确定行进方向上的切线，并最终得出最短路径。虽然行进的路线存在很多可能，情况复杂，但是经过我们猜想证明，当圆弧圆心位于矩形顶点时，且圆弧半径最小时可使得问题一大大简化。由此，于是我们建立单源最短路径几何模型，在计算R点到A点的距离时我们运用解析几何的办法，分别求出切线的长度，然后运用三角形余弦定理求出圆心角的大小，从而求出圆弧长度，随即可得R点到A点的最短路径。R点到其他目标点的最短路径同理可得。最终得出最短路径：

RA 最短路径为：70.5076

RB 最短路径为：107.9587

RC 最短路径为：102.0514

针对问题二，机器人需要依次经过目标点A、B再到达目标点C，首先基于方案一时的猜想证明，若单纯考虑每个转弯的路径最优，容易造成行进路线在中间节点处出现折线，故首先整体规划无折线行进路径，然后利用基于线圆线结构的最优化模型给出了两种方案，方案一：适当扩大拐点处的转弯半径，使得机器人能够沿直线通过途中的目标点。方案二：在节点套上以最小转弯半径为半径的圆环，采用紧拉绳子的方法使节点位于圆环上，并且在拐点采用最小转弯半径，然后建立了最优化模型对两种方案分别运用Lingo求解。求的最终结果，第一种方案最短路径为： 157.752；第二种方案最短路径为：156.471.第二种方案的结果比第一种方案的结果小，说明了按照第二种方案要比第一种方案行走路径更优。

最后我们将模型进行了推广，假设平面内存在N个障碍物，求任意两点间最短路径，同样利用切线-圆弧-切线的结构，我们把切线看作顶点，切线长度作为定点权值，把圆弧长度看作两顶点连线的权值，然后运用Dijkstra进行求解。

关键词 ： 最短路径 最优化模型 避障路径 解析几何

一 、问题重述

下图是一个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、假设机器人能够抽象成点来处理。

四、 符号说明

五、模型的建立

5.1 先来证明一个猜想：

猜想一：具有圆形限定区域的最短路径是由两部分组成的：一部分是平面上的自然最短路径（即直线段），另一部分是限定区域的部分边界，这两部分是相切的，互相连接的[1]。（即问题分析中的绳子绷紧时的状况）

证明：假设在平面中有A（a，0）和B（-a，0）两点，中间有一个半径为1半圆形障碍物，证明从A到B的最短路径为AB。

图5.11

平面上连接两点最短的路径是通过这两点的直线段，但是连接两点的线段于障碍物相交，所以设法尝试折线路径。在y轴上取一点C（0，y），若y适当大，则折线ACB与障碍物不相交，折线ACB的长度为：

|ACB|显然|ACB|随着y的减小而减小，减小y得yy1，即CC1,使得AC1与C1B与障碍物相切，切点分别为E和F,显然AC1B是这种折线路径中最短的。由于满足0

AE

的角满足

下面在考察一条不穿过障碍物的任何一条路径，设其分别于OE和OF的延长线交与P、Q两点，记A和P之间的路径长度为所以|AP|>AE,从而>AE,同理可得>BF。 ，显然>|AP|,又由AEEO,

再来比较PQ之间路径长度

和圆弧EF的长度的大小。若PQ之间的路径可

有极坐标方程,则有

=

,可得：

=2 其中 为角POC的大小。

亦即路径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，

B的长度，设为L. 角度AOB=, 

AOC=, BOD=，COD.求A

解法如下：

如上图可得有以下关系：

ab

c

在AOB中：

在RtAOC： b2c2a2arccos()2bc

=arccos

在RtAOC中：

r

crbarccos

所以：

2

从而可得：

Lr

2）而对于下图两种情况我们不能直接采用线圆的结构来解决，需要做简单的变换。 情况一：

线圆结构5.22

我们假设两圆心坐标分别为O(x1,y1)和O(x2,y2),半径均为r，M点坐标为(x3,y3)，那么我们很容易可以求得:

x1x2x32 yy1y2

32

这样我们就可以利用1）中的方法，先求A到M，再求M到B，这样分两段就可以求解。同理如果有更多的转弯，我们同样可以按照此种方法分解。

情况二：

线圆结构5.23

这里我们依然设圆心坐标分别为O(x1,y1)和O(x2,y2),半径均为r，这样我们可以得到：

KOOy2y1 x2x1

那么OO直线方程为：

yKOO(xx1)y1

因为公切线DE与OO平行，那么DE的直线方程可以表示为：

yKOO(xx1)y1C

其中：

C那么把公切线的方程于圆的方程联立，可以求得切点D和E的坐标。这样用D和E任意一点作为分割点都可以将上图分割成两个4.21所示的线圆结构，这样

就可以对其进行求解。同理多个这样的转弯时，用同样的方法可以进行分割。

5.3 模型准备二

一、对于从起点经过若干点然后再到达目标点的状况，因为不能走折线路径，我们就必须考虑在经过路径中的一个目标点时转弯的状况。为了研究这个问题的方便，我们先来证明一个猜想：

猜想二:如果一个圆环可以绕着环上一个定点转动，那么过圆环外两定点连 接一根绳子，并以该圆环为支撑拉紧绳子，达到平衡状态时，圆心与该顶点以及两条切线的延长线的交点共线[3]。

图5.31

证明猜想：

如图4.31所示，E点就是圆环上的一个顶点，AB就是拉紧的绳子，O2就是切线AC和BD的延长线的交点，证明O1、E、O2三点共线。

我们可以用力学的知识进行证明，因为是拉紧的绳子，所以两边的绳子拉力相等，设为F，它们的合力设为F0，定点对圆环的作用力设为F1。

那么由几何学的知识我们可以知道F0一定与O1O2共线，而又由力的平衡条件可知：

F0=-F1

即O1O2与EO2共线。

综上所述O1、E和O2三点一定共线。

二、有了以上这个定理我们可以建立以下模型：

如图4.32，要求求出机器人从A绕过障碍物经过M点到达目标点B的最短路径，我们采用以下方法：

用一根钉子使一个圆环定在M点，使这个圆环能够绕M点转动。然后连接A和B的绳子并以这些转弯处的圆弧为支撑（这里转弯处圆弧的半径均按照最小转弯半径来计算），拉紧绳子，那么绳子的长度就是A到B的最短距离。我们可以把路径图抽象为以下的几何图形。下面我们对这段路径求解：

图5.32

如图，A(x1,y1)是起点，B(x2,y2)是终点，O1(x3,y3)和O3(x4,y4)是两个固定的圆，O2是一个可以绕M(p，q)点转动的圆环，三个圆的半径均为r，C、D、E、F、G、H均为切点。a、b、c、e，f分别是AO1、O1O2、AO2、AO3、O2O3的长度。A、B、O1、O3均是已知点，O2是未知点。那么最短路径就可以表示为： L=|AC|++|DE|++|FG|++|HB|

因为O2点的坐标未知，所以我们就不能用模型一中的线圆结构对其进行求解。

故得先求出O2点的坐标。设O2坐标为（m，n），AOC1、AO1O2、AO2O1、

，COD1、EO2F、EO2MAO2O3、O3O2F分别为i（i=1、2、3、4、5）

分别为1、2、。这样便有以下关系：

abc

e

f

在RtAO1C中： 1arccos

在AO1O2中： ra

a2b2c2

2arccos 2ab

b2c2a2

3arccos 2bc

在AO2O3中：

c2f2e2

4arccos 2cf

在RtNO2F中：

5arccos

则： 2r f

31212 334522

又因为MO2一定会在EO2F的角平分线上，所以满足：

2

2

2我们采用向量的形式来求，易知O1O2的一个方向向量：

l1(1,y2m) x2n

而O2E与O1O2垂直，故其一个方向向量：

l2(1,而：

x2n) y2m

O2M(pm,qn)

所以： arccosl2

O2M

|l2||O2M|

综合以上式子可以求得O2的坐标，从而可以得出路径的长为：

L1rb2rl0 l0=+HB，这可以采用模型一中的线圆结构来求解。

5.4 模型准备三

求解从起点经过若干个点再到达目标点的问题，与4.4不同，我们还可以有另一种方案，即适当扩大障碍物拐点处的拐弯半径使机器人能够沿直线通过路径中的目标点。这样拐点处拐弯圆弧的半径和圆心都是个变量，对于该题，那么我们可以首先设定三个圆心O1、O2、O3,然后按照以下步骤进行作图：

1） 给定R1，以O1为圆心，R1为半径，画圆，然后过R点做圆O1的切线

L1，切点为D。然后过A点做O1的切线设为L2，切点为E。

2） 然后做O2F垂直于L2，垂足为F，O2F的长就是R2，然后以O2为圆

心，R2为半径画圆。很显然R2能由R1来确定，即R2=f(R1)。

3） 然后过B点做O2的切线为L3，切点为G，再过O3F垂直于L3，垂足

为H，那么O3H的长度就是R3，然后以O3为圆心，R3为半径，画圆。

很显然R3能由R2来确定，即R3=f(R2)。

4）

5） 过C做O3的切线。这就完成了由R经过A和B在到达C的路径。 然后再变换O1、O2、O3、R1、R2、R3，可得到新的路径。找出最

小者即可。

5.5 模型的建立

假设机器人从起点R到到目标点M0，由4.2知路径一定是由圆弧和线段组

成，设有m条线段，n条圆弧。那么目标函数可以表示为：

mn

minLdilj

i1j1

r1s.t k1

用此模型就可以对起点到目标点之间的路径进行优化求解。

六、模型的求解

6.1 问题一

一、以下给出了R到个目标点的可能路径的最短路径：

1）如图一，解决的就是R到目标点A的最短路径问题，很显然的一个问题

是机器人从

所以机器人从的上方走的最短路径肯定是大于机器人从下方走的最短路径，方向走的最优路线我们在图一中没有给出。图一中，蓝线就可以点的圆为支撑而拉紧的绳子，为机器人隔出了危险区域，红线表示的就是通过

这样计算出绳子的长度就是R到A的最短路径。

2）如图二，解决的是R到目标点B的最短路径问题，图中给出了可能的三条路径的最短路径（图中的红线所示），我们可以分别计算出三条可能路径的最短路径的长度，然后进行比较，取最小者就是R到目标点B的最优路径。

3) 如图三，解决的是R到目标点C的最短路径问题。图中给出了两条可能路径的最短路径（图中的红线所示），我们同样可以分别计算出两条可能的最短路径，取最小者就是R到目标点C的最优路径。

图6.11

图6.12

图6.13

二、 然后用matlab求解

结果如下：

1) R到目标点A的最短距离为70.5076

2）R到目标点B的可能路径有三条，即就有三条可能的最短路径。

1、如图二，机器人走最上面这条路径到达B，直接用matlab求得最短

路径为114.1611

2、而机器人经过中间一条路径到达B,这条路径由三条线段和两段圆弧

组成，直接用三中的解法是结不出来的。于是我们做了如下变换，先

求出中间一条直线的中点设为F，那么可以采用三中的解法，分别求R

到F和F到B的最短路径，然后两段相加，便可求出R到B的最短路

径。求解结果为107.9587

3、机器人经过最下边一条路径，同理这条路径由四条直线和三个圆弧

组成，同样可以采取②中的变换，分三部分求解。求解结果如下为

116.1256

3）R到目标点C的可能路径由两条，和2中的方法一样，最终求解结果R

到 目标点C的最短路径为102.0514

6.2 问题二

一、我们先按照4.3中的思想来进行求解，这样我们可以做出5.21所示的示意图：

图6.21

采用模型准备二中的计算方法，用lingo求解的结果最短路径为：156.471

二、我们在采用模型准备三中的算法进行求解，可以画出5.22所示的示意图：

图6.22

采用模型进行优化，运用lingo求解的最短路径为：157.752

显然运用模型准备二中的方法求解的结果小于用模型准备三中的方法求解的结果，说明模型准备二的方法优于模型准备三的方法。

七、模型推广

7.1 问题深入分析

在本题中只有四个障碍物，按照线圆结构画出从起点到达目标点的路径是有限的，我们完全可以采用穷举法把这些路径列出来，然后比较大小取最小者即可，但是我们可以设想如果这个区域内有n个障碍物，那么按照线圆结构从起点到达目标点的可能路径就有无数多条，这时我们如果在采用穷举法是不现实的。所以我们必须寻求新的算法来解决这个问题。

由上述分析我们有了这样一个想法：先求出所有的切线，包括出发点和目标点到所有圆弧的切线以及所有圆弧与圆弧之间的切线，然后把这且曲线看成是图

6.11中的，给这些定点赋一个等于切线长度的权值，如果某两条切线有一个公切圆弧，则代表这两条曲线的顶点是一条直线的两个端点，边上的权值等于这两条切线之间的劣弧长度。然后在这张图中加一个源点和终点，那么在所有代表出发点与其它圆弧之间切线的顶点与源点连成一条边，权值均为0，同理在所有代表目标点到其它圆弧切线的顶点与终点连成一条边，权值均为0，这样题目就转化成了求源点到达终点之间的最短路径问题了，这里最短路径就是指经过所有顶点与边的权值之和最小[2]。我们可以采用Dijkstra[5]算法进行求解。

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。

图7.22

2）然后把合法的切线抽象为顶点，以这些切线之间的劣弧为边，转化成如

7.21所示的形式。假设转化过后有m条合法切线，那么就有m个顶点。

E为1*m的顶点权值矩阵：

E[e1,e2,,em]

其中ei为第i顶点的权值。

邻接矩阵为：

dij为第i个顶点和第j个顶点之间变得权值。

3）然后把路径图转化成如图6.21所示，按照求得权值矩阵给图中的顶点及边

长赋值。

4）最后依据Dijkstra算法求得最短路径。

一、模型优点 八、模型评价

1、运用多个方案对路径进行优化，在相对优化之中取得最优解。

2、模型优化后用解析几何进行求解，精确度较高。

3、模型简单易懂，便于实际检验及应用。

二、模型缺陷

1、此模型适于全局规划，获得精确解却失去了效率。

2、在障碍物较多时，且形状不规则时，就无法进行求解。

九、参考文献

[1]谭永基，数学模型，上海，复旦大学出版社，2011

[2]胡海星，RPG游戏中精灵的移动问题，第11期：102页，《程序员》， 2001；

[3]尤承业，解析几何，北京，北京大学出版社，2004

[4]周培德，计算几何—算法与设计，北京清华大学出版社，2005

[5]邦迪，图论及其应用，西安，西安科学出版社 1984

十、附录

1）%判定是否经过路障,采用跨立实验（计算几何算法）[4]

function m=intersect(H1,H2,G1,G2)

if

min(H1(1),H2(1))=0

m=1;

else

if

(abs((G2(2)*H2(1)-G2(2)*G1(1)-G1(2)*H2(1)+G1(1)*G1(2))/(G2(1)-G1(1))+G1(2)-H2(2)))/(sqrt(1+(G2(2)-G1(2))^2/(G2(1)-G1(1))^2))

m=1;%表示经过路障

else

m=0;%表示不经过路障

end

2）%求解一次转弯所经路线总长

%T：初始点 V:转弯圆弧圆心 W:到达点 r：转弯圆弧半径

function result=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-r^2);%TS1,TS2均为圆弧切线%

S2W=sqrt(VW^2-r^2);

S1S2hu=r*alpha4;

result=TS1+S1S2hu+S2W;

3）%求解R经过中间节点到达目标点的最短路径（方案一）

model:

min=d1+b+d3+u+v+t;

a=@sqrt(40^2+15^2);

b=@sqrt((x1-40)^2+(y1-15)^2);

c=@sqrt(x1^2+y1^2);

d1=3*3.1415926/2-@acos((a^2+b^2-c^2)/(2*a*b))-@acos(1/a);

(x1-50)^2+(y1-40)^2=1;

x1>49;

x1

d2=@acos(@abs((50-x1+(1600-40*x1-40*y1+x1*y1)/(y1-15))/(@sqrt(1+(x1-40)^2/(y1-15)^2)*@sqrt((50-x1)^2+(40-y1)^2))));

e=@sqrt(55^2+55^2);

f=@sqrt((x1-55)^2+(y1-55)^2);

d3=3*3.1415926/2-@acos((c^2+f^2-e^2)/(2*c*f))-@acos((b^2+c^2-a^2)/(2*b*c))-@acos(2/f);

d3=2*d2;

h=@sqrt((x2-55)^2+(y2-55)^2);

g=@sqrt((x2-x1)^2+(y2-y1)^2);

d4=3*3.1415926/2-@acos(2/f)-@acos((f^2+h^2-g^2)/(2*f*h));

j=@sqrt((85-x2)^2+(55-y2)^2);

d5=3.1415926-@acos((h^2+j^2-900)/(2*h*j));

d6=@acos(@abs((75-x2+(3300-55*y2-60*x2+x2*y2)/(y2-55))/(@sqrt(1+(55-x

2)^2/(y2-55)^2)*@sqrt((75-x2)^2+(60-y2)^2))));

d5=2*d6;

k=@sqrt(1324);

m=@sqrt(1325);

n=@sqrt((95-x2)^2+(20-y2)^2);

d7=3*3.1415926/2-@acos(1/k)-@acos((j^2+m^2-n^2)/(2*m*j));

(75-x2)^2+(60-y2)^2=1;

u=2*@sqrt(1/4*f*f-1)+d4+h+d5+j+d7;

v=@sqrt(1324);

t=@sqrt(40^2+15^2-1);

end

4）%求解R经过中间节点到达目标点的最短路径（方案二）

model:

min=@sqrt(x1^2+y1^2-r1^2)+k+@sqrt((x1-x2)^2+(y1-y2)^2)+s+@sqrt((x2-x3

)^2+(y2-y3)^2)+p+@sqrt((x3-95)^2+(y3-20)^2-r3^2);

b=@sqrt(x1^2+y1^2);

d=@sqrt((x1-50)^2+(y1-40)^2);

t1=2*3.1415926-@acos(r1/b)-@acos(r1/d)-@acos(((b^2+d^2-4100)/(2*b*d))

);

k=r1*t1;

@sqrt(d^2-r1^2)+@sqrt((x2-50)^2+(y2-40)^2-r2^2)=@sqrt((x1-x2)^2+(y1-y

2)^2);

g=@sqrt((x2-50)^2+(y2-40)^2);

i=@sqrt((x2-75)^2+(y2-60)^2);

t2=2*3.1415926-@acos(r2/g)-@acos(r2/i)-@acos((g^2+i^2-1025)/(2*g*i)); @sqrt((x2-75)^2+(y2-60)^2-r2^2)+@sqrt((x3-75)^2+(y3-60)^2-r3^2)=@sqrt

((x2-x3)^2+(y2-y3)^2);

s=r2*t2;

v=@sqrt((x3-75)^2+(y3-60)^2);

m=@sqrt((x3-95)^2+(y3-20)^2);

t3=2*3.141592-@acos(r3/v)-@acos(r3/m)-@acos(((m^2+v^2-2000)/(2*mv))); p=r3*t3;

r1-@sqrt((x1-40)^2+(y1-15)^2)>=1;

r2-@sqrt((x2-55)^2+(y2-55)^2)>=1;

r3-@sqrt((x3-85)^2+(y3-55)^2)>=1;

end

5）%顶点有权值的特殊dijikstra算法

%a 表示图的权值矩阵

%m为1*n的矩阵，代表n个顶点的权值矩阵

%d 表示所求最短路的权和

%index1 表示标号顶点顺序

%index2 表示标号顶点索引

function [d index1 index2]=Dijkf(a,m)

M=max(max(a));

pb(1:length(a))=0;

pb(1)=1;

index1=1;

index2=ones(1,length(a));

d(1:length(a))=M;d(1)=m(1);temp=1;

while sum(pb)

tb=find(pb==0);

d(tb)=min(d(tb),d(temp)+m(tb)+a(temp,tb));

tmpb=find(d(tb)==min(d(tb)));

temp=tb(tmpb(1));

pb(temp)=1;

index1=[index1,temp];

index=index1(find(d(index1)==d(temp)-a(temp,index1)-m(temp))); if length(index)>=2

index=index(1);

end

index2(temp)=index;

end

d;

index1;

index2;

移动机器人避障问题

摘要

本文研究的是移动机器人避障最短路径问题。首先，把机器人看作为一个点，将障碍全部当作矩形，抽象成为一个简单的几何问题，由于机器人的行走路径必须是有由直线段与圆弧线段组成的光滑曲线，而且通过证明，最优行走路线是由切线-圆弧-切线结构组成的，并且机器人的行走路径都可看成有若干上述线圆线结构组成的，因此，机器人的行走路径的长度即可通过计算若干上述结构的长度取得，基于这一理论，我们进行了对问题一与问题二的研究。

针对问题一，要求两点间的最短路径，可因为存在障碍物，我们首先用包络线画出机器人不可行走的危险区域，其形状为圆角矩形，然后对本题附图和求解目标进行分析，可以发现，在起点和终点一定的情况下，转弯处的圆弧位置以及圆弧长度决定了行进路线的实际情况，也就是说：在寻求最佳路线时，核心的问题就是确定圆弧的相关信息，然后可以根据圆弧的位置及大小，确定行进方向上的切线，并最终得出最短路径。虽然行进的路线存在很多可能，情况复杂，但是经过我们猜想证明，当圆弧圆心位于矩形顶点时，且圆弧半径最小时可使得问题一大大简化。由此，于是我们建立单源最短路径几何模型，在计算R点到A点的距离时我们运用解析几何的办法，分别求出切线的长度，然后运用三角形余弦定理求出圆心角的大小，从而求出圆弧长度，随即可得R点到A点的最短路径。R点到其他目标点的最短路径同理可得。最终得出最短路径：

RA 最短路径为：70.5076

RB 最短路径为：107.9587

RC 最短路径为：102.0514

针对问题二，机器人需要依次经过目标点A、B再到达目标点C，首先基于方案一时的猜想证明，若单纯考虑每个转弯的路径最优，容易造成行进路线在中间节点处出现折线，故首先整体规划无折线行进路径，然后利用基于线圆线结构的最优化模型给出了两种方案，方案一：适当扩大拐点处的转弯半径，使得机器人能够沿直线通过途中的目标点。方案二：在节点套上以最小转弯半径为半径的圆环，采用紧拉绳子的方法使节点位于圆环上，并且在拐点采用最小转弯半径，然后建立了最优化模型对两种方案分别运用Lingo求解。求的最终结果，第一种方案最短路径为： 157.752；第二种方案最短路径为：156.471.第二种方案的结果比第一种方案的结果小，说明了按照第二种方案要比第一种方案行走路径更优。

最后我们将模型进行了推广，假设平面内存在N个障碍物，求任意两点间最短路径，同样利用切线-圆弧-切线的结构，我们把切线看作顶点，切线长度作为定点权值，把圆弧长度看作两顶点连线的权值，然后运用Dijkstra进行求解。

关键词 ： 最短路径 最优化模型 避障路径 解析几何

一 、问题重述

下图是一个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、假设机器人能够抽象成点来处理。

四、 符号说明

五、模型的建立

5.1 先来证明一个猜想：

猜想一：具有圆形限定区域的最短路径是由两部分组成的：一部分是平面上的自然最短路径（即直线段），另一部分是限定区域的部分边界，这两部分是相切的，互相连接的[1]。（即问题分析中的绳子绷紧时的状况）

证明：假设在平面中有A（a，0）和B（-a，0）两点，中间有一个半径为1半圆形障碍物，证明从A到B的最短路径为AB。

图5.11

平面上连接两点最短的路径是通过这两点的直线段，但是连接两点的线段于障碍物相交，所以设法尝试折线路径。在y轴上取一点C（0，y），若y适当大，则折线ACB与障碍物不相交，折线ACB的长度为：

|ACB|显然|ACB|随着y的减小而减小，减小y得yy1，即CC1,使得AC1与C1B与障碍物相切，切点分别为E和F,显然AC1B是这种折线路径中最短的。由于满足0

AE

的角满足

下面在考察一条不穿过障碍物的任何一条路径，设其分别于OE和OF的延长线交与P、Q两点，记A和P之间的路径长度为所以|AP|>AE,从而>AE,同理可得>BF。 ，显然>|AP|,又由AEEO,

再来比较PQ之间路径长度

和圆弧EF的长度的大小。若PQ之间的路径可

有极坐标方程,则有

=

,可得：

=2 其中 为角POC的大小。

亦即路径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，

B的长度，设为L. 角度AOB=, 

AOC=, BOD=，COD.求A

解法如下：

如上图可得有以下关系：

ab

c

在AOB中：

在RtAOC： b2c2a2arccos()2bc

=arccos

在RtAOC中：

r

crbarccos

所以：

2

从而可得：

Lr

2）而对于下图两种情况我们不能直接采用线圆的结构来解决，需要做简单的变换。 情况一：

线圆结构5.22

我们假设两圆心坐标分别为O(x1,y1)和O(x2,y2),半径均为r，M点坐标为(x3,y3)，那么我们很容易可以求得:

x1x2x32 yy1y2

32

这样我们就可以利用1）中的方法，先求A到M，再求M到B，这样分两段就可以求解。同理如果有更多的转弯，我们同样可以按照此种方法分解。

情况二：

线圆结构5.23

这里我们依然设圆心坐标分别为O(x1,y1)和O(x2,y2),半径均为r，这样我们可以得到：

KOOy2y1 x2x1

那么OO直线方程为：

yKOO(xx1)y1

因为公切线DE与OO平行，那么DE的直线方程可以表示为：

yKOO(xx1)y1C

其中：

C那么把公切线的方程于圆的方程联立，可以求得切点D和E的坐标。这样用D和E任意一点作为分割点都可以将上图分割成两个4.21所示的线圆结构，这样

就可以对其进行求解。同理多个这样的转弯时，用同样的方法可以进行分割。

5.3 模型准备二

一、对于从起点经过若干点然后再到达目标点的状况，因为不能走折线路径，我们就必须考虑在经过路径中的一个目标点时转弯的状况。为了研究这个问题的方便，我们先来证明一个猜想：

猜想二:如果一个圆环可以绕着环上一个定点转动，那么过圆环外两定点连 接一根绳子，并以该圆环为支撑拉紧绳子，达到平衡状态时，圆心与该顶点以及两条切线的延长线的交点共线[3]。

图5.31

证明猜想：

如图4.31所示，E点就是圆环上的一个顶点，AB就是拉紧的绳子，O2就是切线AC和BD的延长线的交点，证明O1、E、O2三点共线。

我们可以用力学的知识进行证明，因为是拉紧的绳子，所以两边的绳子拉力相等，设为F，它们的合力设为F0，定点对圆环的作用力设为F1。

那么由几何学的知识我们可以知道F0一定与O1O2共线，而又由力的平衡条件可知：

F0=-F1

即O1O2与EO2共线。

综上所述O1、E和O2三点一定共线。

二、有了以上这个定理我们可以建立以下模型：

如图4.32，要求求出机器人从A绕过障碍物经过M点到达目标点B的最短路径，我们采用以下方法：

用一根钉子使一个圆环定在M点，使这个圆环能够绕M点转动。然后连接A和B的绳子并以这些转弯处的圆弧为支撑（这里转弯处圆弧的半径均按照最小转弯半径来计算），拉紧绳子，那么绳子的长度就是A到B的最短距离。我们可以把路径图抽象为以下的几何图形。下面我们对这段路径求解：

图5.32

如图，A(x1,y1)是起点，B(x2,y2)是终点，O1(x3,y3)和O3(x4,y4)是两个固定的圆，O2是一个可以绕M(p，q)点转动的圆环，三个圆的半径均为r，C、D、E、F、G、H均为切点。a、b、c、e，f分别是AO1、O1O2、AO2、AO3、O2O3的长度。A、B、O1、O3均是已知点，O2是未知点。那么最短路径就可以表示为： L=|AC|++|DE|++|FG|++|HB|

因为O2点的坐标未知，所以我们就不能用模型一中的线圆结构对其进行求解。

故得先求出O2点的坐标。设O2坐标为（m，n），AOC1、AO1O2、AO2O1、

，COD1、EO2F、EO2MAO2O3、O3O2F分别为i（i=1、2、3、4、5）

分别为1、2、。这样便有以下关系：

abc

e

f

在RtAO1C中： 1arccos

在AO1O2中： ra

a2b2c2

2arccos 2ab

b2c2a2

3arccos 2bc

在AO2O3中：

c2f2e2

4arccos 2cf

在RtNO2F中：

5arccos

则： 2r f

31212 334522

又因为MO2一定会在EO2F的角平分线上，所以满足：

2

2

2我们采用向量的形式来求，易知O1O2的一个方向向量：

l1(1,y2m) x2n

而O2E与O1O2垂直，故其一个方向向量：

l2(1,而：

x2n) y2m

O2M(pm,qn)

所以： arccosl2

O2M

|l2||O2M|

综合以上式子可以求得O2的坐标，从而可以得出路径的长为：

L1rb2rl0 l0=+HB，这可以采用模型一中的线圆结构来求解。

5.4 模型准备三

求解从起点经过若干个点再到达目标点的问题，与4.4不同，我们还可以有另一种方案，即适当扩大障碍物拐点处的拐弯半径使机器人能够沿直线通过路径中的目标点。这样拐点处拐弯圆弧的半径和圆心都是个变量，对于该题，那么我们可以首先设定三个圆心O1、O2、O3,然后按照以下步骤进行作图：

1） 给定R1，以O1为圆心，R1为半径，画圆，然后过R点做圆O1的切线

L1，切点为D。然后过A点做O1的切线设为L2，切点为E。

2） 然后做O2F垂直于L2，垂足为F，O2F的长就是R2，然后以O2为圆

心，R2为半径画圆。很显然R2能由R1来确定，即R2=f(R1)。

3） 然后过B点做O2的切线为L3，切点为G，再过O3F垂直于L3，垂足

为H，那么O3H的长度就是R3，然后以O3为圆心，R3为半径，画圆。

很显然R3能由R2来确定，即R3=f(R2)。

4）

5） 过C做O3的切线。这就完成了由R经过A和B在到达C的路径。 然后再变换O1、O2、O3、R1、R2、R3，可得到新的路径。找出最

小者即可。

5.5 模型的建立

假设机器人从起点R到到目标点M0，由4.2知路径一定是由圆弧和线段组

成，设有m条线段，n条圆弧。那么目标函数可以表示为：

mn

minLdilj

i1j1

r1s.t k1

用此模型就可以对起点到目标点之间的路径进行优化求解。

六、模型的求解

6.1 问题一

一、以下给出了R到个目标点的可能路径的最短路径：

1）如图一，解决的就是R到目标点A的最短路径问题，很显然的一个问题

是机器人从

所以机器人从的上方走的最短路径肯定是大于机器人从下方走的最短路径，方向走的最优路线我们在图一中没有给出。图一中，蓝线就可以点的圆为支撑而拉紧的绳子，为机器人隔出了危险区域，红线表示的就是通过

这样计算出绳子的长度就是R到A的最短路径。

2）如图二，解决的是R到目标点B的最短路径问题，图中给出了可能的三条路径的最短路径（图中的红线所示），我们可以分别计算出三条可能路径的最短路径的长度，然后进行比较，取最小者就是R到目标点B的最优路径。

3) 如图三，解决的是R到目标点C的最短路径问题。图中给出了两条可能路径的最短路径（图中的红线所示），我们同样可以分别计算出两条可能的最短路径，取最小者就是R到目标点C的最优路径。

图6.11

图6.12

图6.13

二、 然后用matlab求解

结果如下：

1) R到目标点A的最短距离为70.5076

2）R到目标点B的可能路径有三条，即就有三条可能的最短路径。

1、如图二，机器人走最上面这条路径到达B，直接用matlab求得最短

路径为114.1611

2、而机器人经过中间一条路径到达B,这条路径由三条线段和两段圆弧

组成，直接用三中的解法是结不出来的。于是我们做了如下变换，先

求出中间一条直线的中点设为F，那么可以采用三中的解法，分别求R

到F和F到B的最短路径，然后两段相加，便可求出R到B的最短路

径。求解结果为107.9587

3、机器人经过最下边一条路径，同理这条路径由四条直线和三个圆弧

组成，同样可以采取②中的变换，分三部分求解。求解结果如下为

116.1256

3）R到目标点C的可能路径由两条，和2中的方法一样，最终求解结果R

到 目标点C的最短路径为102.0514

6.2 问题二

一、我们先按照4.3中的思想来进行求解，这样我们可以做出5.21所示的示意图：

图6.21

采用模型准备二中的计算方法，用lingo求解的结果最短路径为：156.471

二、我们在采用模型准备三中的算法进行求解，可以画出5.22所示的示意图：

图6.22

采用模型进行优化，运用lingo求解的最短路径为：157.752

显然运用模型准备二中的方法求解的结果小于用模型准备三中的方法求解的结果，说明模型准备二的方法优于模型准备三的方法。

七、模型推广

7.1 问题深入分析

在本题中只有四个障碍物，按照线圆结构画出从起点到达目标点的路径是有限的，我们完全可以采用穷举法把这些路径列出来，然后比较大小取最小者即可，但是我们可以设想如果这个区域内有n个障碍物，那么按照线圆结构从起点到达目标点的可能路径就有无数多条，这时我们如果在采用穷举法是不现实的。所以我们必须寻求新的算法来解决这个问题。

由上述分析我们有了这样一个想法：先求出所有的切线，包括出发点和目标点到所有圆弧的切线以及所有圆弧与圆弧之间的切线，然后把这且曲线看成是图

6.11中的，给这些定点赋一个等于切线长度的权值，如果某两条切线有一个公切圆弧，则代表这两条曲线的顶点是一条直线的两个端点，边上的权值等于这两条切线之间的劣弧长度。然后在这张图中加一个源点和终点，那么在所有代表出发点与其它圆弧之间切线的顶点与源点连成一条边，权值均为0，同理在所有代表目标点到其它圆弧切线的顶点与终点连成一条边，权值均为0，这样题目就转化成了求源点到达终点之间的最短路径问题了，这里最短路径就是指经过所有顶点与边的权值之和最小[2]。我们可以采用Dijkstra[5]算法进行求解。

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。

图7.22

2）然后把合法的切线抽象为顶点，以这些切线之间的劣弧为边，转化成如

7.21所示的形式。假设转化过后有m条合法切线，那么就有m个顶点。

E为1*m的顶点权值矩阵：

E[e1,e2,,em]

其中ei为第i顶点的权值。

邻接矩阵为：

dij为第i个顶点和第j个顶点之间变得权值。

3）然后把路径图转化成如图6.21所示，按照求得权值矩阵给图中的顶点及边

长赋值。

4）最后依据Dijkstra算法求得最短路径。

一、模型优点 八、模型评价

1、运用多个方案对路径进行优化，在相对优化之中取得最优解。

2、模型优化后用解析几何进行求解，精确度较高。

3、模型简单易懂，便于实际检验及应用。

二、模型缺陷

1、此模型适于全局规划，获得精确解却失去了效率。

2、在障碍物较多时，且形状不规则时，就无法进行求解。

九、参考文献

[1]谭永基，数学模型，上海，复旦大学出版社，2011

[2]胡海星，RPG游戏中精灵的移动问题，第11期：102页，《程序员》， 2001；

[3]尤承业，解析几何，北京，北京大学出版社，2004

[4]周培德，计算几何—算法与设计，北京清华大学出版社，2005

[5]邦迪，图论及其应用，西安，西安科学出版社 1984

十、附录

1）%判定是否经过路障,采用跨立实验（计算几何算法）[4]

function m=intersect(H1,H2,G1,G2)

if

min(H1(1),H2(1))=0

m=1;

else

if

(abs((G2(2)*H2(1)-G2(2)*G1(1)-G1(2)*H2(1)+G1(1)*G1(2))/(G2(1)-G1(1))+G1(2)-H2(2)))/(sqrt(1+(G2(2)-G1(2))^2/(G2(1)-G1(1))^2))

m=1;%表示经过路障

else

m=0;%表示不经过路障

end

2）%求解一次转弯所经路线总长

%T：初始点 V:转弯圆弧圆心 W:到达点 r：转弯圆弧半径

function result=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-r^2);%TS1,TS2均为圆弧切线%

S2W=sqrt(VW^2-r^2);

S1S2hu=r*alpha4;

result=TS1+S1S2hu+S2W;

3）%求解R经过中间节点到达目标点的最短路径（方案一）

model:

min=d1+b+d3+u+v+t;

a=@sqrt(40^2+15^2);

b=@sqrt((x1-40)^2+(y1-15)^2);

c=@sqrt(x1^2+y1^2);

d1=3*3.1415926/2-@acos((a^2+b^2-c^2)/(2*a*b))-@acos(1/a);

(x1-50)^2+(y1-40)^2=1;

x1>49;

x1

d2=@acos(@abs((50-x1+(1600-40*x1-40*y1+x1*y1)/(y1-15))/(@sqrt(1+(x1-40)^2/(y1-15)^2)*@sqrt((50-x1)^2+(40-y1)^2))));

e=@sqrt(55^2+55^2);

f=@sqrt((x1-55)^2+(y1-55)^2);

d3=3*3.1415926/2-@acos((c^2+f^2-e^2)/(2*c*f))-@acos((b^2+c^2-a^2)/(2*b*c))-@acos(2/f);

d3=2*d2;

h=@sqrt((x2-55)^2+(y2-55)^2);

g=@sqrt((x2-x1)^2+(y2-y1)^2);

d4=3*3.1415926/2-@acos(2/f)-@acos((f^2+h^2-g^2)/(2*f*h));

j=@sqrt((85-x2)^2+(55-y2)^2);

d5=3.1415926-@acos((h^2+j^2-900)/(2*h*j));

d6=@acos(@abs((75-x2+(3300-55*y2-60*x2+x2*y2)/(y2-55))/(@sqrt(1+(55-x

2)^2/(y2-55)^2)*@sqrt((75-x2)^2+(60-y2)^2))));

d5=2*d6;

k=@sqrt(1324);

m=@sqrt(1325);

n=@sqrt((95-x2)^2+(20-y2)^2);

d7=3*3.1415926/2-@acos(1/k)-@acos((j^2+m^2-n^2)/(2*m*j));

(75-x2)^2+(60-y2)^2=1;

u=2*@sqrt(1/4*f*f-1)+d4+h+d5+j+d7;

v=@sqrt(1324);

t=@sqrt(40^2+15^2-1);

end

4）%求解R经过中间节点到达目标点的最短路径（方案二）

model:

min=@sqrt(x1^2+y1^2-r1^2)+k+@sqrt((x1-x2)^2+(y1-y2)^2)+s+@sqrt((x2-x3

)^2+(y2-y3)^2)+p+@sqrt((x3-95)^2+(y3-20)^2-r3^2);

b=@sqrt(x1^2+y1^2);

d=@sqrt((x1-50)^2+(y1-40)^2);

t1=2*3.1415926-@acos(r1/b)-@acos(r1/d)-@acos(((b^2+d^2-4100)/(2*b*d))

);

k=r1*t1;

@sqrt(d^2-r1^2)+@sqrt((x2-50)^2+(y2-40)^2-r2^2)=@sqrt((x1-x2)^2+(y1-y

2)^2);

g=@sqrt((x2-50)^2+(y2-40)^2);

i=@sqrt((x2-75)^2+(y2-60)^2);

t2=2*3.1415926-@acos(r2/g)-@acos(r2/i)-@acos((g^2+i^2-1025)/(2*g*i)); @sqrt((x2-75)^2+(y2-60)^2-r2^2)+@sqrt((x3-75)^2+(y3-60)^2-r3^2)=@sqrt

((x2-x3)^2+(y2-y3)^2);

s=r2*t2;

v=@sqrt((x3-75)^2+(y3-60)^2);

m=@sqrt((x3-95)^2+(y3-20)^2);

t3=2*3.141592-@acos(r3/v)-@acos(r3/m)-@acos(((m^2+v^2-2000)/(2*mv))); p=r3*t3;

r1-@sqrt((x1-40)^2+(y1-15)^2)>=1;

r2-@sqrt((x2-55)^2+(y2-55)^2)>=1;

r3-@sqrt((x3-85)^2+(y3-55)^2)>=1;

end

5）%顶点有权值的特殊dijikstra算法

%a 表示图的权值矩阵

%m为1*n的矩阵，代表n个顶点的权值矩阵

%d 表示所求最短路的权和

%index1 表示标号顶点顺序

%index2 表示标号顶点索引

function [d index1 index2]=Dijkf(a,m)

M=max(max(a));

pb(1:length(a))=0;

pb(1)=1;

index1=1;

index2=ones(1,length(a));

d(1:length(a))=M;d(1)=m(1);temp=1;

while sum(pb)

tb=find(pb==0);

d(tb)=min(d(tb),d(temp)+m(tb)+a(temp,tb));

tmpb=find(d(tb)==min(d(tb)));

temp=tb(tmpb(1));

pb(temp)=1;

index1=[index1,temp];

index=index1(find(d(index1)==d(temp)-a(temp,index1)-m(temp))); if length(index)>=2

index=index(1);

end

index2(temp)=index;

end

d;

index1;

index2;