机器人避障问题

D题 机器人避障问题

摘要

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

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

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

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

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

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

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

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

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

最后，以以上两种方案为依据，得到OA的最短时间路径为：圆心为(82,208)，r12.828（单位），T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）机器人直线行走的最大速度为v0为v

v()

5个单位/秒。机器人转弯时，最大转弯速度

v0

1e

100.1

2

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

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

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：路径的总长度

di：第i条路径的总长度（i1,2,3） lj：第j段圆弧的长度（j1,2,3）

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，假设在平面中有A(a,0)和B(a,0)两点，中间有一个半圆形的障



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

图4-1 猜想示意图

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

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

的长度为：|ACB|。

由上式可知，即yy1、CC1,当无限趋近时AC1|ACB|随着y的减小而减小，、

C1B

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

2

0

的角满足

tan

小于ECF的长， 即EFECF,从而,所以易知EF11

、线段FB为AEFFBACB，记线段AE、EFB,那么AEFB比任何折线路AEEF1

径都短。

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

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



再来比较PQ之间路径长度错误！未找到引用源。和EF的长度的大小。若PQ之间的路径可有极坐标方程(),则有错误！未找到引用源。10,可得：

PQ







d-



3



-EF

B的长度。 B的长度超过路径AEF即路径APQ

B满足条件A到B的最短路径。 综上证明，足以说明了AEF

4.2模型一的建立

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

线圆结构4-21

（1）如上图，设A(x1,y1)为起点，B(x2,y2)为目标点，C(x3,y3)和D(x4,y4)分别为机

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

AO的长度为b，BO的长度为c，的长度为a，角度AOB, AOC, BOD，

DB的长度，设为. .求ACL

解法如下：

如上图可得有以下关系：

COD

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

在AOB中：

arccos(

bca

2bc

222

)

在RtAOC中：

arccos

=arccos

rb

rc

所以：

2

从而可得：

L

r

时间：

T

5

5r

51e

100.1r

2

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

情况一：

线圆结构4-22

如图4-22， 假设两圆心坐标分别为O(x1,y1)和O(x2,y2),半径均为r，则很容易求

出M点的坐标为（

x1x2

2

，

y1y2

2

）。

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

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

情况二：

线圆结构4-23

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

KOO

y2y1x2x1

那么OO直线方程为：

yKOO(xx1)y1

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

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

C圆O的方程：

圆O`的方程：

10

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

10

4.3 模型二的建立

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

图 4-31

证明猜想：

DB就是拉紧的绳子，O就是切如图4-31所示，E点就是圆环上的一个顶点，AC2

线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的最短距离。可以把路径图抽象为以下的几何图形。以下为对这段路径的求解：

图4-32

如图4-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是未知点。那么最短路径就可以表示为：

DEEFFGGHHB LACCD

因为O2点的坐标未知，所以我们就不能用模型一中的线圆结构对其进行求解。故得先求出O2点的坐标。设O2坐标为(m,n)，AO1C、AO1O2、AO2O1、AO2O3、O3O2F

OD1、EO2F、EO2M分别为1、2、。这样便分别为i（i1,2,3,4,5），C

有以下关系：

abcef

在RtAO1C中：

1arccos

ra

2

在AO1O2中：

2arccos

abc

2abbca

2bc

2

2

2

2

2

3arccos

在AO2O3中：

4arccos

cf

22

e

2

2cf

在RtNO2F中：

5arccos

2rf

则：

31212



3345

22

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

2



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



2

yml1(1,2)

x2n



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

xnl2(1,2)

y2m

而： 所以：



O2M(pm,qn)



lO2M

arccos2|l2||O2M|

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

L

1rb2rl0

HB，这可以采用模型一中的线圆结构来求解。 l0GH

4.4问题一的求解

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

图4-41 OA的路径图

用MATLAB（程序见附录C）求解得：d1 471.0372

d2 498.4259

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

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

由d1d2，则质点从O到达目标点A的最短路径为471.0372。 （2）如图4-42，质点从O到达B有四条路径d1、d2、d3、d4。

图4-42 OB的路径图

用MATLAB求解得：d11063.458 d21046.662 d3877.3840 d4 853.7001

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

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

由d4d3d2d1，则质点从O到达目标点B的最短路径为853.7001。

（3）如图4-43，质点从O到达C有两条路径d1、d2。

图4-43 OC的路径图

用MATLAB求解得：d11217.96

d21095.1

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

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

由d2d1，则质点从O到达目标点C的最短路径为1090.8。

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

图4-41 OABCO的路径图

用MATLAB求解得：d2762.5

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

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

4.5 最短时间路径模型建立与求解： 4.5.1模型的建立

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

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

图4-51

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

图 4-52

由绳子拉紧法可知O、（即直线AB到圆O的最远点E点） O1、O2会有一个公共的切点设A(x1,y1),B(x2,y2)，O(x3,y3),圆O的半径为r，则直线AB的方程为：

y

y2y1x2x1

(xx1)y1

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

y

y2y1x2x1

(xx1)y1C （1）

圆O的标准方程为：(xx3)2(yy3)2r2 （2） 联立方程（1）、（2）可以得出E点坐标(x4,y4)。

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

4.5.2模型的求解：

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

图 4-53

利用模型建立的方法，对路径d1、d2求解，可以求出E点坐标为(72.928,217.07)，，那么圆心运动所在直线方程为：yx290，改E1点的坐标为（237.07,52.928）

变半径大小，求出与做出对各方案的进行进一步优化，再将各值代入程序（程序见附录

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

TT1

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

障碍物的数学描述

附录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%最短时间