机器人避障路径问题 数学建模

承诺书
我们仔细阅读了中国大学生数学建模竞赛的竞赛规则.
我们完全明白，在竞赛开始后参赛队员不能以任何方式（包括电话、电子邮件、网上咨询等）与队外的任何人（包括指导教师）研究、讨论与赛题有关的问题。

我们知道，抄袭别人的成果是违反竞赛规则的, 如果引用别人的成果或其他公开的资料（包括网上查到的资料），必须按照规定的参考文献的表述方式在正文引用处和参考文献中明确列出。

我们郑重承诺，严格遵守竞赛规则，以保证竞赛的公正、公平性。

如有违反竞赛规则的行为，我们将受到严肃处理。

我们授权全国大学生数学建模竞赛组委会，可将我们的论文以任何形式进行公开展示（包括进行网上公示，在书籍、期刊和其他媒体进行正式或非正式发表等）。

我们参赛选择的题号是（从A/B/C/D中选择一项填写）：
我们的参赛报名号为（如果赛区设置报名号的话）：
所属学校（请填写完整的全名）：
参赛队员(打印并签名) ：1.
2.
3.
指导教师或指导教师组负责人(打印并签名)：
日期： 2012 年月日赛区评阅编号（由赛区组委会评阅前进行编号）：
编号专用页
赛区评阅编号（由赛区组委会评阅前进行编号）：
全国统一编号（由赛区组委会送交全国前编号）：全国评阅编号（由全国组委会评阅前进行编号）：
机器人避障问题
摘要
本文研究了机器人避障最短路径及最短时间的问题。

主要解决在一个区域中存在12个不同形状障碍物，机器人由出发点到达目标点、由出发点经过途中的若干目标点到达原点以及到某目标点最短时间路径的三种情形。

首先，我们通过证明具有圆形限定区域的最短路径是由两部分组成的：一部分是平面上的自然最短路径（即直线段），另一部分是限定区域的部分边界。

这两部分是相切的、互相连接的。

我们依据这个结果，可以认为最短路径一定是由线和圆弧组成，因此我们建立了线圆结构，这样无论路径多么复杂，我们都可以将路径划分为若干个这种线圆结构来求解。

其次，我们对于途中经过节点的再到达目标点的状况，我们采用了两种方案，一种是在拐点和节点都采用最小转弯半径的形式，另一种是适当扩大拐点处的转弯半径，使得机器人能够沿直线通过途中的目标点。

然后，建立了最优化模型对两种方案分别进行求解
接下来，我们利用解析几何、解三角形以及线圆结构，结合题目给出的限定因素进行最优求解，建立起具有可操作性的模型，利用穷列法与层次分析法对机器人避障最短路径和最短时间路径进行筛选。

假设机器人从起点O 到到目标点0A ，一定是由圆弧和线段组成，设有m 条线段，n 条圆弧。

那么目标函数可以表示为：
1
1
min m
n
i j
i j L d l ===+∑∑
10
10r k ≥⎧⎨≥⎩
使得
用此模型可对起点到目标点之间的路径进行优化求解得出目标函数。

最后，我们对得出的模型进行综合评价，在评价过程中，我们根据题目给出
的条件以及问题的分析，分别对最短路径和最短时间路径进行系统、合理的解答。

使得出的结果与模型更加合理，达到全局最优。

问题一，我们将其分解成线圆结构来求解，然后把可能路径的最短路径采用穷举法列举出来，最终用层次分析法得出最短路径：
O A →：471.0312 O B →：853.8869 O C →：1082.3445
O A B C O →→→→：2989.3562
问题二，我们利用物理中时间、速度与路程的关系就可以得出O(0,0)到达某目标点的最短的时间路径：
O A →：471.0312
→
O A
起点坐标终点坐标圆心坐标
线段一0，0 70.52，213.13
弧一70.52,213.13 76.61,219.41 80,210 线段二76.61，219.41 300,300
总路径471.03 总时间96.02
→
O B
线段一0,0 50.18,301.91
弧一50.18,301.91 50.05,305.54 60,300 线段二50.05,305.54 141.68,440.55
弧二141.68,440.55 145.28,443.52 150,435 线段三145.28,443.82 222.09,460
弧三222.09,460 230,470 220,470 线段四230,470 230,530
弧四230,530 225.49,538.45 220,530 线段五225.49,538.45 146.35,590.69
弧五146.35,590.69 144.50,591.65 150,600 线段六144.50,591.65 100,700
总路径853.89 总时间177.71
→
O C
线段一0,0 412.22,90.25
弧一412.22,90.25 419.79,97.97 410,100 线段二412.79,97.97 725.94,511,95
弧二725.94,511.95 730,520 720,520 线段三730,520 730,600
弧三730,600 737.72,606.59 720,600 线段四737.72,606.59 700,640
总路径1082.24 总时间219.55
→→→→
O A B C O
线段一0,0 70.52,213.13
弧一70.52,213.13 76.61,219.41 80,210 线段二76.61,219.41 290.59,296.61
弧二290.59,296.61 309.41,296.63 290.41,296.58 线段三309.41,296.63 232.43,536.21
弧三232.43,536.21 225.50,538.36 220,530 线段四225.50,538.36 146.35,590.70
弧四146.35,590.70 144.50,591.65 150,600 线段五144.50,591.65 96.10,690.80
弧五96.10,690.80 109.07,703.91 102.51,696.51 线段六109.07,703.91 360.00,670.02
弧六360,670.02 370,670 370,680 线段七370,670 430,670
弧七430,670 435.52,670.03 430,680 线段八435.52,670.03 534.48,740.00
弧八534.48,740.00 540,740 670,730 线段九540,740 702.43,638.52
弧九702.43,638.52 697.56,641.46 696.43,637.53 线段十697.56,641.46 737.72,606.59
弧十737.72,606.59 730,600 720,600 线段十一730,600 730,520
弧十一730,520 725.94,511.95 720,520 线段十二725.94,511.95 419.79,97.97
弧十二419.79,97.97 412.22，90.25 410,100 线段十三412.22,90.25 0,0
总路径2989.3562 总时间615.21
关键词最优化模型最优路径层次分析避障路径
解析几何解三角形评价模型
一、 问题重述
1.1问题背景
图1是一个800×800的平面场景图，在原点O(0, 0)点处有一个机器人，它只能在该平面场景范围内活动。

图中有12个不同形状的区域是机器人不能与之发生碰
标点与障碍物的距离至少超过10个单位）。

规定机器人的行走路径由直线段和圆弧组成，其中圆弧是机器人转弯路径。

机器人不能折线转弯，转弯路径由与直线路径相切的一段圆弧组成，也可以由两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。

为了不与障碍物发生碰撞，同时要求机器人行走线路与障碍物间的最近距离为10个单位，否则将发生碰撞，若碰撞发生，则机器人无法完成行走。

机器人直线行走的最大速度为50=v 个单位/秒。

机器人转弯时，最大转弯速度为2
1.0100
e
1)(ρρ-+=
=v v v ，其中ρ是转弯半径。

如果超过该速度，机器人将发生
侧翻，无法完成行走。

1.2问题提出
通过建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径
的数学模型。

对场景图中4个点O(0, 0)，A(300, 300)，B(100, 700)，C(700, 640)，具体计算下列问题：
(1) 机器人从O(0, 0)出发，O→A、O→B、O→C和O→A→B→C→O的最短路径。

(2) 机器人从O (0, 0)出发，到达A的最短时间路径。

注：要给出路径中每段直线段或圆弧的起点和终点坐标、圆弧的圆心坐标以及机器人行走的总距离和总时间。

图1 800×800平面场景图
二、问题分析
2.1问题一
首先，要求求定点O（0, 0）按照一定的行走规则绕过障碍物到达目标点的最短路径，我们可以用包络线画出机器人行走的危险区域，这样的话，拐角处就是一个半径为10的四分之一圆弧，那么通过采用拉绳子的方法寻找可能的最短路径（比如求O和A之间的最短路径，我们就可以连接O和A
之间的一段绳子，以拐角处的圆弧为支撑拉紧，那么这段绳子的长度便是O 到A的一条可能的最短路径），然后采用穷举法列出O到每个目标点的可能路径的最短路径，然后比较其大小便可得出O到目标点的最短路径。

然后，要求求定点(0,0)
O经过中间的若干点按照一定的规则绕过障碍物到达目标点，这使我们考虑就不仅仅是经过障碍物拐点的问题，也应该考虑经过路径中的目标点处转弯的问题，这时简单的线圆结构就不能解决这种问题，我们在拐点及途中目标点处都采用最小转弯半径的形式，也可以适当的变换拐点处的拐弯半径，使机器人能够沿直线通过途中的目标点，然后建立优化模型对这两种方案分别进行优化，最终求得最短路径。

2.2问题二
要求求O(0,0)到点A的最短时间路径。

在问题一中，我们根据求得的最短路径算出直线距离与圆弧的距离，再利用机器人在直线与拐弯处的最大速度，利用物理中时间、速度与路程的关系就可以得出O(0,0)到达某目标点的最短的时间路径。

三、模型假设
1、假设障碍物全是规则图形。

2、假设机器人能够抽象成点来处理。

四、符号说明
五、基本信息与数据处理
5.1 最优化模型在机器人避障中的应用
最优化模型在多样化选择性问题上具有优越性，我们所研究的机器人避障问题路径较多，所得最短路径将需要最优化模型来处理。

要找到最短历经及最短时间，最终使用最优化模型综合各方面因素来详细分析。

5.2 最优化模型概述
最优化问题，是指在日常生活中通过适当的规划安排，使得完成一件事所用的费用最少、路线最短、时间最短、产值最高、容积最大等的效率与分配问题，也就是要在各种方案中，寻找一个最节约、合理的方案。

解决这类问题要注意两点：一是明确问题，即通过问题描述中已知的数量关系把活问题转化为单纯的数学问题，我们称之为数学建模的过程；二是建模后的求解问题，即用相关的数学知识求解出最优的处理方案。

5.3 最优化模型的应用程序
穷列法基本原理和步骤 步骤1：将线性规划问题化成矩阵的标准形式，设系数矩阵的秩()R A m =，则对应线性方程组的基础解系自由变量的个数为n m -个。

步骤2：穷举法求解：令()120i i i n m x x x -====，解得对应线行方程组一组解
为(
)
12,,
,n x x x ；对应目标函数值为(
)
12,,
,n i f x x x f =
从n 个变量x 中选n m -个作为自由变量，令它们的值为0，可得到m n m n n C C -=组解。

步骤3：确定最优解：如果最优解存在，则上述求解得到的对应m n m n n C C -=个目标函数中，最小者（或最大者）即为所求最小（或最大）最优值，对应的解为最优解。

步骤4：证明解为最优解：
1）将最优解对应的自由变量看成参数()12,,n m t t t -；解对应线性方程组得
()()012i i i i i n m n m x b b b b t --=+++
+， 1,2,
,i n =。

2）将对应线性方程组解()()01122i i i i i n m n m x b b t b t b t --=+++
+， 1,2,
,i n
=代入目标函数得： ()()01122n m n m f f d t d t d t --=++++。

如果0,i d ≥ 1,2,,i n =，则所求为最小值最优解；否则，线性规划问题无最小
值最优解。

如果0,i d ≤ 1,2,
,i n =，则所求为最大值最优解；否则，线性规划问题无最大
值最优解。

六、模型建立与处理
6.1 先来证明一个猜想：
猜想一：具有圆形限定区域的最短路径是由两部分组成的：一部分是平面上的自然最短路径（即直线段），另一部分是限定区域的部分边界，这两部分是相切的，互相连接的。

（即问题分析中的拉绳子拉到最紧时的状况）
证明：假设在平面中有A （a ，0）和B （-a ，0）两点，中间有一个半圆形的障碍物，证明从A 到B 的最路径为AEFB 。

Y
X
平面上连接两点最短的路径是通过这两点的直线段，但是连接两点的线段于障碍物相交，所以设法尝试折线路径。

在y 轴上取一点C （0，y ），若y 适当大，则折线ACB 与障碍物不相交，折线ACB 的长度为：
22||2a +y ACB =
显然ACB 随着y 的减小而减小，减小y 得1y y →,即1c c →,使得1AC 与1C B 与障碍物相切，切点分别为E 和F,显然1AC B 是这种折线路径中最短的。

由于满足
02
π
α<<
的角满足tan αα<,所以易知弧度EF 小于1EC F 的长， 即1EF EC F <,
从而1AE EF FB AC B ++<,记线段AE 、弧度EF 、线段FB 为AEFB,那么AEFB 比任何折线路径都短。

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

再来比较PQ 之间路径长度PQ 和圆弧EF 的长度的大小。

若PQ 之间的路径可有极坐标方程()ρρθ=,则有1ρ>,可得： 223PQ d d EF π
ρρθθ=+≥--⎰⎰
亦即路径APQB 的长度超过路径AEFB 的长度。

以上证明足以说明了AEFB 是满足条件A 到B 的最短路径。

6.2 模型准备一
有了6.1中的定理，我们就可以这样认为，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个线圆结构所组成。

在本题中存在障碍物的状况，且障碍物在拐点处的危险区域是一个半径为10的圆弧，所以结合定理一，我们易知，求两点之间的最短路径中的转弯半径我们应该按照最小的转弯半径来算才能达到最优。

线圆结构6.21
1）如上图，设A ()11,x y 为起点，B ()22,x y 为目标点，C ()33,x y 和D ()44,x y 别
为机器人经过拐点分别于隔离危险线拐角小圆弧的切点，圆心为O ()55,x y ，圆的半径为r ，AB 的长度为a ，AO 的长度为b ，BO 的长度为c ，角度AOB α∠=,AOC β∠=,BOD γ∠=，COD θ∠=.求ACDB 的长度，设为L. 解法如下：
如上图可得有以下关系：
a b c ⎧=⎪⎪=⎨⎪=⎪⎩
AOB ∆在中：
在Rt AOC ∆： =arccos
r b β
在Rt AOC ∆中： arccos
r
c γ= 所以：
2θπαβγ=---
从而可得：
L r θ=
2）而对于下图两种情况我们不能直接采用线圆的结构来解决，需要做简单的变换。

情况一：
222arccos()2b c a bc
α+-=
线圆结构6.22
我们假设两圆心坐标分别为()11,O x y 和()'22,O x y ,半径均为r ，M 点坐标为()33,x y ，那么我们很容易可以求得: 12
312
322x x x y y y +⎧=⎪⎪⎨+⎪=⎪⎩
这样我们就可以利用1）中的方法，先求A 到M ，再求M 到B ，这样分两段就可以求解。

同理如果有更多的转弯，我们同样可以按照此种方法分解。

情况二：
线圆结构6.23
这里我们依然设圆心坐标分别为11(,)O x y 和22(,)O x y ',半径均为r ，这样我们可以得到： 2121OO y y K x x '-=-
那么OO '直线方程为：
11()OO y K x x y '=-+
因为公切线DE 与OO '平行，那么DE 的直线方程可以表示为：
11()OO y K x x y C '=-++
其中：
2
1OO C r K '=+
那么把公切线的方程于圆的方程联立，渴可以求得切点D 和E 的坐标。

这样用D 和E 任意一点作为分割点都可以将上图分割成两个6.21所示的线圆结构，这样就可以对其进行求解。

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

6.3 模型准备二
一、对于从起点经过若干点然后再到达目标点的状况，因为不能走折线路径，我们就必须考虑在经过路径中的一个目标点时转弯的状况。

为了研究这个问题的方便，我们先来证明一个猜想：
猜想二:如果一个圆环可以绕着环上一个定点转动，那么过圆环外两定点连接一根绳子，并以该圆环为支撑拉紧绳子，达到平衡状态时，圆心与该顶点以及两条切线的延长线的交点共线。

图6.31
证明猜想：
如图6.31所示，E 点就是圆环上的一个顶点，ACDB 就是拉紧的绳子，2O 就是切线AC 和BD 的延长线的交点，证明1O 、E 、2O 三点共线。

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

那么由几何学的知识我们可以知道0F 一定与12OO 共线，而又由力的平衡条件可知：
01F F =-
即12OO 与2EO 共线。

综上所述1O 、E 和2O 三点一定共线。

二、有了以上这个定理我们可以建立以下模型：
如图6.32，要求求出机器人从A 绕过障碍物经过M 点到达目标点B 的最短路径，我们采用以下方法：
用一根钉子使一个圆环定在M 点，使这个圆环能够绕M 点转动。

然后连接A 和B 的绳子并以这些转弯处的圆弧为支撑（这里转弯处圆弧的半径均按照最小转弯半径来计算），拉紧绳子，那么绳子的长度就是A 到B 的最短距离。

我们可以把路径图抽象为以下的几何图形。

下面我们对这段路径求解：
图6.32
如图，A 11(,)x y 是起点，B 22(,)x y 是终点，1O 33(,)x y 和3O 44(,)x y 是两个固定的圆，2O 是一个可以绕M(p ，q)点转动的圆环，三个圆的半径均为r ，C 、D 、E 、F 、G 、H 均为切点。

a 、b 、c 、e ，f 分别是A 1O 、1O 2O 、A 2O 、A 3O 、23O O 的长度。

A 、B 、1O 、3O 均是已知点，2O 是未知点。

那么最短路径就可以表示为：
L AC CD DE EF FG GH HB =++++++
因为2O 点的坐标未知，所以我们就不能用模型一中的线圆结构对其进行求解。

故得先求出2O 点的坐标。

设2O 坐标为（m ，n ），1AO C ∠、12AO O ∠、21AO O ∠、23AO O ∠、32O O F ∠分别为i α（i =1、2、3、4、5），1CO D ∠、2EO F ∠、2EO M ∠分别为1θ、2θ、θ.便有以下关系：
a b c e f ⎧=⎪⎪=⎪⎪=
⎨⎪=
⎪⎪=⎪⎩
在1Rt AO C ∆中： 1arccos
r a α=
在12AO O ∆中： 222
2arccos 2a b c ab α+-=
222
3arccos 2b c a bc α+-=
在23AO O ∆中：
222
4arccos 2c f e cf α+-=
在2Rt NO F ∆中：
52arccos
r f α=
则： 11223453232πθααπθααα⎧=--⎪⎪⎨⎪=---⎪⎩
又因为2MO 一定会在2EO F ∠的角平分线上，所以满足：
2
2θθ=
2θ我们采用向量的形式来求，易知12OO 的一个方向向量：
212(1,)y m x n l -=-
而2O E 与12OO 垂直，故其一个方向向量：
222(1,)x n l y m -=--
而：
2(,)O M p m q n =--
所以： 2222ar ccos ||||l O M
l O M θ•=
综合以上式子可以求得2O 的坐标，从而可以得出路径的长为：
120L r b r l θθ=+++
0l GH HB =+，这可以采用模型一中的线圆结构来求解。

6.4 模型准备三
求解从起点经过若干个点再到达目标点的问题，与6.3不同，我们还可以有另一种方案，即适当扩大障碍物拐点处的拐弯半径使机器人能够沿直线通过路径中的目标点。

这样拐点处拐弯圆弧的半径和圆心都是个变量，对于该题，那么我们可以首先设定三个圆心1O 、2O 、3O ,然后按照以下步骤进行作图：
1） 给定1R ，以1O 为圆心，1R 为半径，画圆，然后过R 点做圆1O 的切线1L ，切
点为D 。

然后过A 点做1O 的切线设为2L ，切点为E 。

2） 然后做2O F 垂直于2L ，垂足为F ，2O F 的长就是2R ，然后以2O 为圆心，2R 为
半径画圆。

很显然2R 能由1R 来确定，即21=f(R )R 。

3） 然后过B 点做2O 的切线为3L ，切点为G ，再过3O F 垂直于3L ，垂足为H ，那
么3O H 的长度就是3R ，然后以3O 为圆心，3R 为半径，画圆。

很显然3R 能
由2R 来确定，即3R =2f(R )。

4） 过C 做3O 的切线。

这就完成了由R 经过A 和B 在到达C 的路径。

5） 然后再变换1O 、2O 、3O 、1R 、2R 、3R ，可得到新的路径。

找出最小者即可。

6.5 模型的建立
假设机器人从起点R 到到目标点0M ，由6.2知路径一定是由圆弧和线段组成，设有m 条线段，n 条圆弧。

那么目标函数可以表示为：
11
min m n
i j i j L d l ===+∑∑
11.r k s t ≥⎧⎨≥⎩
= 用此模型就可以对起点到目标点之间的路径进行优化求解。

七、模型的建立与求解
7.1 问题一
一、以下给出了O 到个目标点的可能路径的最短路径：
1）如图一，解决的就是O 到目标点A 的最短路径问题，通过分析我们找到了两种最短路径，又通过计算得出红色路线为最短路，这样计算出绳子的长度就是O 到A 的最短路径为：471.0312
2）如图二，解决的是O 到目标点B 的最短路径问题，图中给出了可能的三条路径的最短路径，我们可以分别计算出三条可能路径的最短路径的长度，然后进行比较，取最小者就是O 到目标点B 的最优路径为：853.8869
3)如图三，解决的是O 到目标点C 的最短路径问题。

图中给出了两条可能路径的最短路径，我们同样可以分别计算出两条可能的最短路径，取最小者就是O 到目标点C 的最优路为：1082.3445
4）如图四，解决的是O 到目标点A.B.C 再到O 最短路径问题。

图为两种路径，我们采用模型优化进行求解，最终求得最短路径为:2989.3562
图一 图二
图三
图四
7.2 问题二
解决的是最短时间路径，由21.0100
e 1)(ρρ-+==v v v 分析可得O A →路径为
471.03（时间：86.70）与492.38（时间：90.32），故O A →最短路径为：471.03
八、模型的评价与推广
在本题中只有四个障碍物，按照线圆结构画出从起点到达目标点的路径是有限的，我们完全可以采用穷举法把这些路径列出来，然后比较大小取最小者即可，但是我们可以设想如果这个区域内有n 个障碍物，那么按照线圆结构从起点到达目标点的可能路径就有无数多条，这时我们如果在采用穷举法是不现实的。

所以我们必须寻求新的算法来解决这个问题。

由上述分析我们有了这样一个想法：先求出所有的切线，包括出发点和目标点到所有圆弧的切线以及所有圆弧与圆弧之间的切线，然后把这且曲线看成是图
7.11中的，给这些定点赋一个等于切线长度的权值，如果某两条切线有一个公切圆弧，则代表这两条曲线的顶点是一条直线的两个端点，边上的权值等于这两条切线之间的劣弧长度。

然后在这张图中加一个源点和终点，那么在所有代表出发点与其它圆弧之间切线的顶点与源点连成一条边，权值均为0，同理在所有代表目标点到其它圆弧切线的顶点与终点连成一条边，权值均为0，这样题目就转化成了求源点到达终点之间的最短路径问题了，这里最短路径就是指经过所有顶点与边的权值之和最小。

我们可以采用Dijkstra 算法进行求解。

九、参考文献
【1】薛定宇陈阳泉《高等应用数学问题的MATLAB求解》清华大学出版社【2】姜启源谢金星叶俊《数学模型（第三版）》高等教育出版社
【3】姜启源《数学模型（第二版）》高等教育出版社 1993年
【4】韩中庚《数学建模方法及应用》高等教育出版社 2005年
【5】叶其孝《大学生数学建模竞赛辅导教材》湖南教育出版社
附录：
1）%求解一次转弯所经路线总长
%T：初始点 V:转弯圆弧圆心 W:到达点
Function result=longhand(T,W,V,10)
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;
2）%判定是否经过路障,采用跨立实验（计算几何算法）
function m=intersect(H1,H2,G1,G2)
if
min(H1(1),H2(1))<=max(G1(1),G2(1))&min(G1(1),G2(1))<=max(H1(1),H2(1)) &min(G1(2),G2(2))<=max(H1(2),H2(2))&min(H1(2),H2(2))<=max(G1(2),G2(2) )&((G1(1)-H1(1))*(H2(2)-H1(2))-(H2(1)-H1(1))*(G1(2)-H1(2)))*((H2(1)-H 1(1))*(G2(2)-H1(2))-(H2(2)-H1(2))*(G2(1)-H1(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))<=1%保证障碍圆弧部分与两点连线相切
m=1;
else
m=0;
end
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<50;
d2=@acos(@abs((50-x1+(1600-40*x1-40*y1+x1*y1)/(y1-15))/(@sqrt(1+(x1-4 0)^2/(y1-15)^2)*@sqrt((50-x1)^2+(40-y1)^2))));
e=sqrt();。