机器人避障问题

D题机器人避障问题

摘要

本文针对机器人的避障问题，建立了两个相应的数学模型。

模型一：针对机器人避障最短路径的问题。研究了机器人从出发点到目标点，以及从出发点经过若干目标点最终回到出发点的两种情况。

首先，证明了具有圆形限定区域的最短路径是由限定区域的部分边界（部分圆弧）以及与之相切的直线段组成；

其次，依据证明结果，最短路径一定是由线和圆弧组成，以线圆结构建立了最短路径与时间的通用优化模型，解决了无论路径多么复杂都可以将路径划分为若干个这种线圆结构来求解的问题；

再次，对于途中经过若干目标点最终再回到出发点的问题，采用在拐点和节点都用最小转弯半径的的方案进行计算；

最后，对机器人所走最短路径可能性较大的几条路径进行分割，再用通用优化模型进行求解，得到机器人行走的最短路径如下：

路径总距离（单位）总时间（秒）

→ 471.0372 96.0176

O A

→853.7001 179.5340

O B

→1095.1 224.7865

O C

→→→→2762.5 581.4193 O A B C O

模型二：针对机器人避障最短时间路径的问题。研究了行走总时间（即机器人走直线和圆弧所用的时间之和）会随转弯圆弧的圆心和半径的变动而改变的情况。

首先，分析在半径一定、圆心在直线OE上运动的情况，得到半径一定时的最短时间路径的最优方案；

然后，以转弯圆弧过E点为条件，通过调整半径的大小，得出最短时间路径的最优方案；

最后，以以上两种方案为依据，得到O A

→的最短时间路径为：圆心为(82,208)，T=(秒)。

12.828

r=（单位），94.2284

本文还对模型做了进一步的推广，对于智能设备的研究有较高的参考价值。

关键词：最短路径最短时间路径线圆结构最优化模型

1问题重述

1.1 背景资料

（1）图1（见附录B ）在原点O(0,0)处有一个机器人，它只能在一个800×800的平面场景范围内活动。而图中有12个不同形状的区域是机器人不能与之发生碰撞的障碍物，其障碍物的数学描述如表（见附录A ）。

（2）在图1（见附录B ）的平面场景中：

a.在障碍物外选一点作为机器人将到的目标点（目标点与障碍物的距离至少超过10个单位）。

b.规定机器人的行走路径由直线段和圆弧（机器人的转弯路径），而机器人不能折线转弯，转弯路径由直线路径相切的一段圆弧组成，也可以是两个或多个相切的圆弧路径组成，但每个圆弧的半径最小为10个单位。

c.为了不与障碍物发生碰撞，要求满足机器人行走路线与障碍物的最近距离为10个单位，否则将发生碰撞，若发生碰撞，那么机器人将无法完成行走。 （3）机器人直线行走的最大速度为05v =个单位/秒。机器人转弯时，最大转弯速度

为2

100.1()1e

v v

v ρ

ρ-==

+，其中ρ是转弯半径。如果超过该速度，机器人

将发生侧翻，无法完成行走。

1.2 问题提出

问题一 建立机器人从区域中一点到达另一点的避障最短路径和最短时间路径的数学模型。对场景图中4个点O(0, 0)，A(300, 300)，B(100, 700)，C(700, 640)中，计算机器人从O(0, 0)出发，O →A 、O →B 、O →C 和O →A →B →C →O 的最短路径。

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

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

2 模型假设与符号说明

2.1模型假设

1.假设机器人为一个质点。

2.假设障碍物的数学描述准确无误。

3.假设机器人的速度不受其他外部因素影响。

2.2 符号说明

L ：路径的总长度

i d ：第i 条路径的总长度（1,2,3i = ） j l ：第j 段圆弧的长度（1,2,3j = ）

r ：转弯圆弧所在圆的半径

k ：障碍物上的任意点与行走路径之间的最短距离 T ：机器人走完路径所用的时间

3 问题分析

3.1问题一的分析：

问题一中要求求机器人从定点O (0,0)按照一定的行走规则绕过障碍物到达目标点的最短路径。

首先，平面上两点间的最短距离是两点间所连直线段，当其所连直线段与障碍物相交时，必须要有折线路径，因此我们要猜想并证明当走折线时两点的最短路径。

其次，画出机器人行走的危险区域，则其拐点处为半径为10个单位的四分之一圆弧。

再次，列出机器人经过拐点处的所有可能的情况，并分别分析出机器人经过不同种拐点时的，最短路径的计算方法，这里可采用拉绳子的方法（比如求R和A之间的最短路径，那么R和A点之间的距离就可以用一段绳子连接来表示，以拐角处的圆弧为支撑拉紧，即这段绳子的长度便是R到A的一条可能的最短路径）。

最后，列出机器人从定点O到各目标点可能性较大的最短路径，然后比较其大小，从而得到其最短路径。

3.2问题二的分析

问题二中要求求机器人从定点O(0,0)出发，按照一定的行走规则绕过障碍物到达目标点的最短时间路径。

首先，我们把机器人行走的危险区域用包络线画出，那么在拐角的四分之一的圆弧是半径为10个单位的圆。

然后，由题知最短行走时间与半径和圆心的变化相联系。所以先确定半径，分析当圆心符合什么关系时的行走时间最短。再分析半径的变化范围，求在什么条件下时间符合最短。

其次，将半径和圆心的关系联合起来进行综合分析，建立最短时间模型。

最后，根据最短时间路径模型，利用MATABL编程求解机器人从O(0,0)出发到A的最短时间路径。

4 模型建立与求解

4.1猜想证明

首先证明一个猜想，一个圆形限定区域的最短路径是由两部分组成的：一部分是平面上的最短路径（即直线段），另一部分是限定区域的部分边界（即圆的部分弧长），这两部分是相切的。

证明：如图4-1，假设在平面中有(,0)

A a

B a两点，中间有一个半圆形的障

和(,0)

碍物，证明从A到B的最短路径为

A EF B。

图4-1 猜想示意图

平面上两点最近距离为两点间所连直线，但连接两点的线段于障碍物相交，所以设

法尝试折线路径。在y 轴上取一点(0,)C y ，若y 适当大，则折线ACB 与障碍物不相交，折线ACB 的长度为：22

||2a +y =AC B 。

由上式可知，||ACB 随着y 的减小而减小，即1y y →、1→C C ,当无限趋近时1AC 、

1C B

与障碍物相切，切点分别为E 和F ,显然1A C B 是这种折线路径中最短的。由于满足

02

πα<<

的角满足tan α

α

<,所以易知 EF 小于1E C F 的长， 即 1

EF EC F <,从而 1

AE EF FB AC B ++<，记线段A E 、 EF 、线段F B 为 A EF B ,那么 A EF B 比任何折线路径都短。

下面再考察一条不穿过障碍物的任何一条路径，设其分别于OE 和OF 的延长线交与P 、Q 两点，记A 和P 之间的路径长度为A P ，因为A E E O ⊥,所以AP AE >,从而

AP AE >,同理可得BQ BF >。

再来比较PQ 之间路径长度错误！未找到引用源。和

E F 的长度的大小。若PQ 之间的路径可有极坐标方程()ρρθ=,则有错误！未找到引用源。10ρ>,可得：

π

ρ

ρθθ=

+≥

?

?

2

2

d d -

-3

PQ EF

即路径 A PQ

B 的长度超过路径 A EF B 的长度。 综上证明，足以说明了 A EF

B 满足条件A 到B 的最短路径。

4.2模型一的建立

有了4.1中的定理，我们就可以这样认为，起点到目标点无论中间障碍物有多少，最短路径都应该是若干个线圆结构所组成。在本题中存在障碍物的状况，且障碍物在拐点处的危险区域是一个半径为10的圆弧，所以结合定理，我们易知，求两点之间的最短路径中的转弯半径我们应该按照最小的转弯半径来算才能达到最优。

线圆结构4-21

（1）如上图，设11(,)A x y 为起点，22(,)B x y 为目标点，33(,)C x y 和44(,)D x y 分别为机

器人经过拐点分别于隔离危险线拐角小圆弧的切点，圆心为55(,)O x y ，圆的半径为r ，

A B 的长度为a ，A O 的长度为b ，B O 的长度为c ，角度α∠=A O B , β∠=AOC , γ∠=BOD ，

θ

∠=C O D .求 AC D B 的长度，设为L

. 解法如下：

如上图可得有以下关系：

22

21212

2

515122

5252a ()()b ()()

c ()()

?=-+-??=-+-??=-+-??

x x y y x x y y x x y y

AOB ?在中：

=arccos

βr b

在Rt AOC ?中：

arccos

γ=r c

所以：

2θπαβγ=---

从而可得：

22

22

θ=

-+

-+L b r c r r

时间： 2

22

2

2

100.15551r

T b r c r r e

θ-=

-÷+

-÷+÷

+

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

情况一：

线圆结构4-22

如图4-22， 假设两圆心坐标分别为11(,)O x y 和22(,)'O x y ,半径均为r ，则很容易求

222

arc cos(

)

2α+-=b c a

bc

出M 点的坐标为（

12

2

x x +，

12

2

y y +）。

这样就可以用点M 将A 到B 的路径分为两段，便可利用（1）中的方法，先求A 到

M 的路径的长度，再求M 到B 的路径的长度，两段之和即为A 到B 的路径的长度。同理如果有更多的转弯，同样可以按照此种方法分解。

情况二：

线圆结构4-23

这里依然设圆心坐标分别为11(,)O x y 和22(,)'O x y ,半径均为r ，便可以得到：

2121

'-=

-O O y y K x x

那么OO '直线方程为：

11()'=-+O O y K x x y

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

11()'=-++O O y K x x y C 其中：

2

1'

=+O O C r K

圆O 的方程：

2

2

11()()10

x x y y -+-=

圆O`的方程：

2

2

22()()10

x x y y -+-=

则把公切线的方程与圆的方程联立，可以求得切点D 和E 的坐标。通过D 和E 的坐标可以求出D E 中点G 的坐标，即用点G 作为分割点可以将上图分割成两个4.21所示的线圆结构，那么就可以对其进行求解。同理多个类似的转弯时，用同样的方法可以进行分割。

4.3 模型二的建立

对于从起点经过若干点然后再到达目标点的状况，因为不能走折线路径，因此就必 须考虑在经过路径中的一个目标点时转弯的情况。为了使这个问题研究更加方便，再来证明另一个猜想：如果一个圆环可以绕着环上一个定点转动，那么过圆环外两定点连接一根绳子，并以该圆环为支撑拉紧绳子，达到平衡状态时，圆心与该顶点以及两条切线的延长线的交点共线。

图 4-31

证明猜想：

如图4-31所示，E 点就是圆环上的一个顶点， AC D B 就是拉紧的绳子，2

O 就是切线A C 和B D 的延长线的交点，证明1O 、E 、2O 三点共线。

我们可以用力学的知识进行证明，因为是拉紧的绳子，所以两边的绳子拉力相等，

设为

F ，它们的合力设为0 F ，定点对圆环的作用力设为1 F 。

那么由几何学的知识可以知道0

F 一定与12 O O 共线，而又由力的平衡条件可知：

F =-1F

即12

O O 与2 EO 共线。

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

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

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

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

图4-32

如图4-32，11(,)A x y 是起点，22(,)B x y 是终点，133(,)O x y 和344(,)O x y 是两个固定的圆，2O 是一个可以绕(,)M p q 点转动的圆环，三个圆的半径均为r ，C 、D 、E 、F 、G 、

H 均为切点。a 、b 、c 、e 、f ，分别是1A O 、12O O 、2A O 、3A O 、23O O 的长度。A 、

B 、1O 、3O 均是已知点，2O 是未知点。那么最短路径就可以表示为：

L AC CD

DE EF FG GH HB =++++++ 因为2O 点的坐标未知，所以我们就不能用模型一中的线圆结构对其进行求解。故得先求出2O 点的坐标。设2O 坐标为(,)m n ，1∠A O C 、12∠AO O 、21∠AO O 、23∠AO O 、32∠O O F

分别为αi （1,2,3,4,5i =），1∠C

O D 、2∠EO F 、2∠E O M 分别为1θ、2θ、θ。这样便有以下关系：

2

2

13132

2

3322

1122

14142

2

44()()()()()()

()()()()

?=-+-??=-+-??=-+-??=-+-??=-+-??

a x x y y

b x m y n

c x m y n e x x y y f x m y n

在1Rt AO C ?中：

1arccos

α=r a

在12?AO O 中：

2

2

2

2arccos

2α+-=a b c

ab 2

2

2

3arccos

2α+-=b c a

bc

在23?A O O 中：

22

2

4arccos

2α+-=c f

e

cf

在2?Rt N O F 中：

52arccos

α=r f

则：

1122345

32

32

πθααπθααα?=--???

?=---?? 又因为2M O 一定会在2∠EO F 的角平分线上，所以满足：

2

2

θθ=

2θ我们采用向量的形式来求，易知12

O O 的一个方向向量：

212(1,)-=- y m x n

l

而2 O E 与12

O O 垂直，故其一个方向向量：

222(1,)x n l y m

-=--

而： 2(,)=--

O M p m q n

所以：

2222ar c cos ||||

θ=

l O M

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

2

2

22

1202(

)2

f L a r r b r r l θθ=

-++++-+

0l GH

HB =+，这可以采用模型一中的线圆结构来求解。

4.4问题一的求解

（1）如图4-41，质点从O 点到达目标点A 有两条路径，即1d 、2d 。

图4-41 O A →的路径图

用MATLAB （程序见附录C ）求解得：1 471.0372d =

2 498.4259d =

其中，每一条路径我们划分为几个由线圆结构组成的部分，每个部分的起点、终点、圆弧的圆心坐标以及行走的总距离如表1：

表1 线段或圆弧的起点、终点坐标，圆弧的圆心坐标以及总距离和时间表

路径

起点坐标、终点坐标 圆心坐标 总距离（单位） 总时间（秒） 1d （0,0） （300,300） （80,210） 471.0372 96.0176 2d

（0,0） （300,300）

（230,60）

498.4259

101.9130

由12d d <，则质点从O 到达目标点A 的最短路径为471.0372。 （2）如图4-42，质点从O 到达B 有四条路径1d 、2d 、3d 、4d 。

图4-42 O B →的路径图

用MATLAB 求解得：11063.458d = 21046.662d = 3877.3840d = 4 853.7001d =

路径所过的线段或圆弧的起点、终点坐标，圆弧的圆心坐标以及总距离和总时间如表2所示：

表2 线段或圆弧的起点、终点坐标，圆弧的圆心坐标以及总距离和时间表

路径

起点坐标、终点坐标 圆心坐标 总距离（单位） 总时间（秒）

1d

（0,0） （240.02，370.64） （230,60）

1063.458

115.0560

217.9254

（240.02，370.64）（100,700） （270,680） 102.8694

2d （0,0） （222.5,180.2）

（230，60） 1046.662

77.5135

196.4425 （222.5,180.2） （237.48,415.65） （235,300） 48.0644 （237.48,415.65） （185,565）

（220,530） 36.4392 （185,565） （100,700）

（150,600） 34.4254

3d （0,0） （175.5,255）

（80,210） 877.3840

69.5426

182.7327 （175.5,255） （237.48,415.65） （235,300） 42.3255 （237.48,415.65） （185,565）

（220,530） 36.4392 （185,565） （100,700）

（150,600） 34.4254

4d

（0,0） （96.68,373.05） （60,300） 853.7001

79.0737 179.5340 （96.68,373.05） （185,452.5） （150,435）

26.1278 （185,452.5） （230,500）

（220,470） 20.2564 （230,500） （185,565） （220,530） 19.6507 （185,565） （100,700）

（150,600）

34.4254

由4321d d d d <<<，则质点从O 到达目标点B 的最短路径为853.7001。

（3）如图4-43，质点从O 到达C 有两条路径1d 、2d

。

图4-43 O C →的路径图

用MATLAB 求解得：11217.96d =

21095.1d =

路径所过的线段或圆弧的起点、终点坐标，圆弧的圆心坐标以及总距离和总时间如表3所示：

表3 线段或圆弧的起点、终点坐标，圆弧的圆心坐标以及总距离和时间表

路径 起点坐标、终点坐标

圆心坐标 总距离（单位） 总时间（秒）

1

d （0,0） （300,292.5）

（80,210）

1217.96 95.5899

234.5964 （300,292.5） （ 416.67，343.33） （400,330）

28.6260

（416.67，343.33）（650,436.4632） （550,450） 58.0431 （650,436.4632） （730,560） （720,520） 32.8630 （730,560） （700,640） （720,600） 19.4744

2

d

（0,0） （317.83,89.762） （230,60）

1095.1 66.8231

224.7865

（317.83,89.762） （455,150） （410,100）

35.6817 （455,150） （610,360） （500,200） 53.4106 （610,360） （730,560） （720,520） 49.3967 （730,560） （700,640）

（720,600）

19.4744

由21d d <，则质点从O 到达目标点C 的最短路径为1090.8。

（4）由上面计算对比可知0经过A、B、C再回到O的最短路径只有一条，如图4-44所示：

图4-41 O A B C O

→→→→的路径图

用MATLAB求解得：2762.5

d=

路径所过的线段或圆弧的起点、终点坐标，圆弧的圆心坐标以及总距离和总时间如表4所示：

表4 线段或圆弧的起点、终点坐标，圆弧的圆心坐标以及总距离和时间表起点坐标、终点坐标圆心坐标总距离总时间（秒）

（0,0 ）（181.465,258,.536）（80,210）

2762.5 70.8826

581.4193

（181.46,258.54）（265.969,421.64）（292.92,307.07）53.9222 （265.969,421.64）（185,565）（220,530）35.8415 （185,565）（137.61,650.66）（150,600）23.6798 （137.61,650.66）（189.33,696.43）（107.07,692.93）36.4552 （189.33,696.43）（320,680）（270,680）28.2384 （320,680）（400,670）（370,680）19.1774 （400,670）（460,705）（430,680）20.2880 （460,705）（605,740）（540,730）27.2880 （605,740）（730,560）（670,730）28.1195 （730,560）（685,685）（707.07,647.07）16.5454 （685,685）（710,620）（720,600）66.8231 （730,560）（610,360）（720,520）35.6817 （610,360）（455,150）（500,200）53.4106 （455,150）（317.83,89.762）（410,100）49.3967 （317.83,89.762）（0,0）（230,60）15.7092

4.5 最短时间路径模型建立与求解： 4.

5.1模型的建立

在求解从起点到达目标点的最短时间路径的状况中，圆心和半径是变化的。

首先，当半径一定时，根据绳子拉紧法可知，圆弧与障碍物的外弧（即E 点）相切时为最优方案。如图4-51：

图4-51

当半径变化时可得出不同半径的最优方案如图4-52：

图 4-52

由绳子拉紧法可知O 、1O 、2O 会有一个公共的切点（即直线A B 到圆O 的最远点E 点） 设11(,)A x y ,22(,)B x y ，33(,)O x y ,圆O 的半径为r ，则直线A B 的方程为：

211121

()y y y x x y x x -=

-+-

则直线''A B 的方程为 ：

211121

()y y y x x y C x x -=

-++- （1）

圆O 的标准方程为：22233()()x x y y r -+-= （2） 联立方程（1）、（2）可以得出E 点坐标44(,)x y 。

根据模型准备二可以得出O 、E 和1O 三点一定共线，同理O 、E 和2O 三点一定共线，由此可以得出圆心在OE 这条线上运动，不断改变半径的大小，求出最优解。

4.5.2模型的求解：

机器人从O 点绕过障碍物5到达A 点的最短时间路径如图 4-53

图 4-53

利用模型建立的方法，对路径1d 、2d 求解，可以求出E 点坐标为(72.928,217.07)，1E 点的坐标为（237.07,52.928）

，那么圆心运动所在直线方程为：290y x =-+，改变半径大小，求出与做出对各方案的进行进一步优化，再将各值代入程序（程序见附录

C ）中，分别计算得出路径1d 、2d 的最优方案为： 94.2284=T ，199.7219T = 路径所过的线段或圆弧的起点、终点坐标，圆弧的圆心坐标以及总距离和总时间如表5所示：

表4 线段或圆弧的起点、终点坐标，圆弧的圆心坐标以及总距离和时间表

路径

起点坐标、终点坐标 圆心坐标 总距离（单位） 总时间（秒） 1d （0,0） （300,300） （82,208） 471.1239 94.2284 2d

（0,0） （300,300）

（228,62）

498.5869

99.7219

由<1T T ，则质点从O 到达目标点C 的最短时间为94.2284（秒）。

5模型评价与推广

5．1模型的评价

优点：1）在建立模型前做了预备知识，对怎样选取才为最短路径进行了证明和说明，为路径在优化过程中提供了依据。

2）模型列出了机器人从定点O到各目标点可能性较大的最短路径，并且采用编程来解出各路径的距离，使得模型的求解更加完善和精准。

3）模型一的的建立中运用解析几何的方法进行求解，简单易懂且便于推广。

4）本文通过利用数学工具，对建立的最短路径模型进行求解，具有科学性。缺点：1）该模型不适用于较多的障碍物，否则计算就显得较为繁琐，耗时太多，不能达到全局最优。

2）当障碍物为形状不规则的物体时，就不能用此计算，具有一定的局限性。

6．2模型的推广：

1）本模型的建立解决了机器人避障问题，采用了解析几何和拉紧绳子的模型解决问题的方法。因此，该模型的方法和思想还可应用与其他类似问题，如：无人汽车、遥控机器自行躲避障碍物的设计等方面的问题,只需稍微改动模型即可。

2）模型方便、直观，可以实现计算机模拟。

6参考文献

【1】吴振奎王全文主编《运筹学》北京中国人民大学出版社 2006

【2】吴建国主编《数学建模案例精编》中国水利水电出版社 2005.5

【3】姜启源谢金星叶俊编著《数学模型》（第三版），高等教育出版社 2003.8 【4】孙祥等编著《MATLAB7.0基础教程》，北京：清华大学出版社，2005.5 【5】复旦大学数学系《概率论与数理统计》上海科技出版社1961

【6】周培德，计算几何—算法与设计，北京清华大学出版社，2005

7 附录

附录A

障碍物的数学描述

编号障碍物名称左下顶点坐标其它特性描述

1 正方形(300, 400) 边长200

2 圆形圆心坐标(550, 450)，半径70

3 平行四边形(360, 240) 底边长140，左上顶点坐标(400, 330)

4 三角形(280, 100) 上顶点坐标(345, 210)，右下顶点坐标(410, 100)

5 正方形(80, 60) 边长150

6 三角形(60, 300) 上顶点坐标(150, 435)，右下顶点坐标(235, 300)

7 长方形(0, 470) 长220，宽60

8 平行四边形(150, 600) 底边长90，左上顶点坐标(180, 680)

9 长方形(370, 680) 长60，宽120

10 正方形(540, 600) 边长130

11 正方形(640, 520) 边长80

12 长方形(500, 140) 长300，宽60

附录B

图1 包含12个障碍物的800×800的平面场景图

附录C

程序1：

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

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;%弧长

L=TS1+S1S2hu+S2W;L%求解一次转弯所经路线总长

T=TS1/5+S1S2hu/2.5+S2W/5;T%求解一次转弯所经路线总时间

程序2：

%求当两圆公切线与两圆圆心连线相平行时，两圆公切线的中点坐标function f=qingkuang2(O,O1,r)%两原点的坐标O,O1,以及圆的半径syms x y y1;

k=(O1(2)-O(2))/(O1(1)-O(1));%求出两原点间线段的斜率k

c=r*sqrt(1+k^2);%与两原点构成线段平行直线平移的距离c

y1=k*(x-O(1))+O(2)+c;

y2=vpa(simple(y1),5);%化简以后的表达式

f1=r^2-(x-O(1))^2-(y-O(2))^2;

f2=r^2-(x-O1(1))^2-(y-O1(2))^2;

f3=y-k*(x-O(1))-O(2)-c;

s=solve(f1,f3);

x1=vpa(s.x,7);

y3=vpa(s.y,7);

s1=solve(f2,f3);

x2=vpa(s1.x,7);

y4=vpa(s1.y,7);

x=vpa((x1(1)+x2(1))/2,5)

y=vpa((y3(1)+y4(1))/2,5)

程序3：

%求解最短时间

function result=sj(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;

v=5/(1+exp(10-0.1*r^2));

T=(TS1+S2W)/5+S1S2hu/v%最短时间