游晋峰

(晋中师范高等专科学校，山西 晋中 030600)

0引言

机器人避障问题是人工智能领域的研究热点之一。2012年全国大学生数学建模竞赛D题“机器人避障问题”[1]，给出了平面场景中12种静态障碍物，要求建立机器人绕过障碍物达到目的地的最短路径和最短时间路径的数学模型。

蔡志杰[2-3]等人提出了基于Dijkstra算法的机器人避障问题路径规划模型，利用经典的Dijkstra算法搜索得到可行的最短路径；但是由于障碍物较多，算法的搜索时间较长；而且由于转弯圆弧的相互影响，无法将问题转化为一维搜索问题来处理。周玮[4-6]等人提出了将机器人行走路线转化为由若干个线圆结构组成，通过计算各切点坐标和各圆弧长度得到数值结果。这也是大多数参赛队所采用的方法。尽管方法类似，结果却不尽相同。这说明学生应用数学软件的能力有一定的欠缺。

本文对2012年全国大学生数学建模竞赛D题进行分析建模，并给出相应的MATLAB[7]程序，同时利用专业制图软件AutoCAD[8]给出了所有可行路径。

1.线圆结构

由于机器人绕行障碍物时不能折线转弯，故转弯路径由直线和与直线相切的一段圆弧组成。那么，机器人行走线路中可能出现三种线圆结构。

下面，先给出求解三种线圆结构的路径信息模型。

1.1线圆结构一：行走过程中经过一个转弯

图1 线圆结构一

如图1，可以得到以下关系：

该种线圆结构的MATLAB求解程序如下：

function distance=dis1(start,circle,endpoint,r)

%计算线圆结构一的路径信息

%start初始点，circle转弯圆弧圆心，endpoint目标点，r转弯半径，alpha4转弯圆心角

a=sqrt((start(1)-endpoint(1))^2+(start(2)-endpoint(2))^2);

b=sqrt((start(1)-circle(1))^2+(start(2)-circle(2))^2);

c=sqrt((circle(1)-endpoint(1))^2+(circle(2)-endpoint(2))^2);

alpha1=acos((b^2+c^2-a^2)/(2*b*c));alpha2=acos(r/b);alpha3=acos(r/c);

alpha4=2*pi-alpha1-alpha2-alpha3;line1=sqrt(b^2-r^2);line2=sqrt(c^2-r^2);rad=r*alpha4;

distance=sqrt(b^2-r^2)+sqrt(c^2-r^2)+r*alpha4;% A、B两点之间的距离

下面求解两个切点的坐标(t1,t2)(tt1,tt2)，即圆弧的起点终点坐标

s1=start(1);s2=start(2);e1=endpoint(1);e2=endpoint(2);

c1=circle(1);c2=circle(2);b1=b;cc1=c;r1=r;

syms start1 start2 circle1 circle2 endpoint1 endpoint2 b c r

[t1,t2]=solve('(t1-start1)^2+(t2-start2)^2=b^2-r^2','(circle1-t1)^2+(circle2-t2)^2=r^2','t1','t2');

t1=subs(subs(subs(subs(subs(subs(t1,start1,s1),start2,s2),circle1,c1),circle2,c2),b,b1),r,r1)

t2=subs(subs(subs(subs(subs(subs(t2,start1,s1),start2,s2),circle1,c1),circle2,c2),b,b1),r,r1)

[tt1,tt2]=solve('(endpoint1-tt1)^2+(endpoint2-tt2)^2=c^2-r^2','(tt1-circle1)^2+(tt2-circle2)^2=r^2','tt1','tt2');

tt1=subs(subs(subs(subs(subs(subs(tt1,endpoint1,e1),endpoint2,e2),circle1,c1),circle2,c2),c,cc1),r,r1)

tt2=subs(subs(subs(subs(subs(subs(tt2,endpoint1,e1),endpoint2,e2),circle1,c1),circle2,c2),c,cc1),r,r1)

1.2线圆结构二：行走过程中经过两次转弯，且转弯圆弧的圆心连线与公切线有交点

图2 线圆结构二

该种线圆结构的MATLAB求解程序如下：

function distance=dis2(start,circle1,circle2,endpoint,r)

%计算线圆结构二的路径信息，其中endpoint为目标点

middle=(circle1+circle2)/2;%M点的坐标

distance1=dis1(start,circle1,middle,r)%ACDM的长度

distance2=dis1(middle,circle2,endpoint,r)% MEFB的长度

distance=distance1+distance2

1.3线圆结构三：行走过程中经过两次转弯，且转弯圆弧的圆心连线与公切线平行

图3 线圆结构三

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

y=KOO′(x-x6)+y6+Cory=KOO′(x-x7)+y7+C

那么把公切线的方程与圆的方程联立，可以求得切点D和E的坐标。

求切点D、E的坐标的MATLAB求解程序如下：

function [x4,y4,x5,y5]=tangency(start,circle1,circle2,endpoint,r)

%计算第三种线圆结构中的切点坐标

k=(circle2(2)-circle1(2))/(circle2(1)-circle1(1));

c11=circle1(1);c12=circle1(2);c21=circle2(1);c22=circle2(2);r1=r;k1=k;

syms circle11 circle12 circle21 circle22 r k

%与圆O1的切点坐标(x1,y1)

[x1,y1]=solve('y1=k*(x1-circle11)+circle12+r*sqrt(1+k^2)','(x1-circle11)^2+(y1-circle12)^2=r^2','x1','y1');

x1=subs(subs(subs(subs(x1,circle11,c11),circle12,c12),r,r1),k,k1);

y1=subs(subs(subs(subs(y1,circle11,c11),circle12,c12),r,r1),k,k1);

fprintf('与圆O1的切点坐标(x4,y4)为：')

x4=x1(1);y4=y1(1);

%与圆O2的切点坐标(x2,y2)

[x2,y2]=solve('y2=k*(x2-circle21)+circle22+r*sqrt(1+k^2)','(x2-circle21)^2+(y2-circle22)^2=r^2','x2','y2');

x2=subs(subs(subs(subs(x2,circle21,c21),circle22,c22),r,r1),k,k1);

y2=subs(subs(subs(subs(y2,circle21,c21),circle22,c22),r,r1),k,k1);

fprintf('与圆O2的切点坐标(x5,y5)为:')

x5=x2(1);y5=y2(1);

然后将D或E视为目标点或起点，可以将图3分解为两个图1所示的线圆结构，这样就可以对其进行求解。同理多个这样的转弯时，用同样的方法可以进行分割。

这样该段路径的长度为：S=SAE+SDB-DE。

该种线圆结构的MATLAB求解程序如下：

function distance=dis3(start,circle1,circle2,endpoint,r)

%计算线圆结构三的路径信息

[x4,y4,x5,y5]=tangency(start,circle1,circle2,endpoint,r);

tang1=[x4,y4];tang2=[x5,y5];

distance1=dis1(start,circle1,tang2,r);distance2=dis1(tang1,circle2,endpoint,r);

de=sqrt((x4-x5)^2+(y4-y5)^2);

distance=distance1+distance2-de

2.绕过障碍物的最短路径规划模型与求解

利用三种线圆结构求解O→A、O→B、O→C、O→A→B→C→O的最短路径。

2.1绕过障碍物的最短路径规划模型建立

步骤一：包络处理。为满足机器人行走线路与障碍物间的最近距离为10个单位的要求，对障碍物进行包络处理，画出机器人行走的危险区域。对于三角形和四边形的包络方法：边向外平移r个单位；顶点处以顶点为圆心，r为半径画圆。其中，r≥10。对圆的包络方法：以该圆的圆心为圆心，半径增加r个单位进行包络。

步骤二：拉绳子。将障碍物的顶点或节点(A、B、C)处的圆弧视作滑轮，在其上固定一点，过滑轮外两定点连接一根绳子，并以该圆环为支撑拉紧绳子，达到平衡状态时，即为寻找出的可行路径。

需要注意的是：①在确定最优路径的过程中，假设机器人的行走线路尽量沿着包络线的外边缘行走，且不会再在给定的平面场景图中产生新的节点;②滑轮外的两定点为该段线路的起始点和目标点。

步骤三：测量总距离。利用三种线圆结构，执行相应的MATLAB程序进行求解。

步骤四：确定最优路径。根据步骤三的计算结果，从所有可行路径中确定一条总距离最小的路径，即为最优路径。

2.2求解结果

计算时，假设转弯半径r=10。

2.2.1线路O→A的求解结果

利用AutoCAD画出的所有可行线路(图4)。调用线圆结构一的MATLAB函数dis1(start,circle,endpoint,r)计算O→A的最短路径，最终确定O→A的最短路径，如图5。

图4 O→A的所有可行线路

图5 O→A的最短路径

O→A的最短路径的具体信息见表1。

表1 O→A的可行线路的相关信息

2.2.2线路O→B的求解结果

利用AutoCAD画出的所有可行线路(图6)，并调用三种线圆结构的MATLAB函数计算所有可行

图6 O→B的所有可行线路

线路的总距离，最终确定O→B的最短路径，如图7。

图7 线路的最短路径

O→B的最短路径的具体信息见表2。

表2 O→B的最短路径信息

2.2.3线路O→C的求解结果：

同O→B的方法，得到O→C的所有可行线路如图8，最短路径如图9。

图8 O→C的所有可行线路

图9 线路O→C的最短路径

O→C的最短路径的具体信息见表3。

表3 O→C的最短路径信息

2.2.4线路O→A→B→C→O的求解结果：

同样可以得到O→A→B→C→O的最短路径(图10)的总距离为2767.4878，行走总时间为577.6091秒。

图10 线路O→A→B→C→O的最短路径

3.O→A的最短时间路径规划模型与求解

由上述求解结果可知，从O到达A点的路径最短的路线是绕过点(80，210)的路径，如图1，有如下关系：

3.1 边角关系

如图1，可得有以下关系：

3.2 直线段和圆弧的长度的计算公式

3.3 转弯半径的约束

一方面，题目要求转弯半径最小为10个单位，故约束r≥10。

另一方面，以点(80，210)为圆心，r为半径作圆R。以障碍物6右下顶点(235，300)为圆心，10为半径作圆M。当半径r=80时，圆R与左侧边缘相切。利用AutoCAD软件，逐步缩小r的值，找出圆R与圆M的公切线。发现当r为55时几乎相切。为了求解出可行解，取r≤56。

综上，转弯半径r的约束为：10≤r≤56

3.4 行走速度的约束

3.5 目标函数的确定

利用题目所给数据，将各点的坐标代入上述模型中，利用Matlab软件进行求解即可。具体求解程序如下：

start=[0 0];endpoint=[300,300];circle=[80,210];%左上角的坐标r=10:56;%最大取70，v0=5;

%以下计算线路信息

for i=1:length(r)

[distance(i),line1(i),line2(i),rad(i)]=dis1_2(start,circle,endpoint,r(i));

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

t1(i)=(line1(i)+line2(i))./v0;

t2(i)=rad(i)/v(i);

end

t=t1+t2;line=line1+line2;

plot(r,t,'-*')%转弯半径与总行走时间的关系

figure(2);plot(r,v,'-*')%转弯半径与转弯速度的关系

其中，计算路径信息时的调用函数dis1_2.m

function [distance,line1,line2,rad]=dis1_2(start,circle,endpoint,r)

a=sqrt((start(1)-endpoint(1))^2+(start(2)-endpoint(2))^2);

b=sqrt((start(1)-circle(1))^2+(start(2)-circle(2))^2);

c=sqrt((circle(1)-endpoint(1))^2+(circle(2)-endpoint(2))^2);

alpha1=acos((b^2+c^2-a^2)/(2*b*c));alpha2=acos(r/b);alpha3=acos(r/c);

alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4为转弯圆心角

line1=sqrt(b^2-r^2);line2=sqrt(c^2-r^2);rad=r*alpha4;

distance=sqrt(b^2-r^2)+sqrt(c^2-r^2)+r*alpha4;%第一种线圆结构A、B两点之间的距离

s1=start(1);s2=start(2);e1=endpoint(1);e2=endpoint(2);

c1=circle(1);c2=circle(2);b1=b;cc1=c;r1=r;

syms start1 start2 circle1 circle2 endpoint1 endpoint2 b c r

[t1,t2]=solve('(t1-start1)^2+(t2-start2)^2=b^2-r^2','(circle1-t1)^2+(circle2-t2)^2=r^2','t1','t2');

t1=subs(subs(subs(subs(subs(subs(t1,start1,s1),start2,s2),circle1,c1),circle2,c2),b,b1),r,r1)

t2=subs(subs(subs(subs(subs(subs(t2,start1,s1),start2,s2),circle1,c1),circle2,c2),b,b1),r,r1)

[tt1,tt2]=solve('(endpoint1-tt1)^2+(endpoint2-tt2)^2=c^2-r^2','(tt1-circle1)^2+(tt2-circle2)^2=r^2','tt1','tt2');

tt1=subs(subs(subs(subs(subs(subs(tt1,endpoint1,e1),endpoint2,e2),circle1,c1),circle2,c2),c,cc1),r,r1)

tt2=subs(subs(subs(subs(subs(subs(tt2,endpoint1,e1),endpoint2,e2),circle1,c1),circle2,c2),c,cc1),r,r1)

求解发现，当转弯半径r从10逐步增大时，总行走时间T先减小后增大。由图11可以看出，当r=12时，总行走时间T最小。此时，T为94.6001秒，此时总距离S为472.8648。

图11 转弯半径r与总行走时间T的关系

当转弯半径r从10逐步增大时，转弯速度v快速增大。由图12可以看出，当r=15时，转弯速度v达到最大5，之后保持不变。当r从10以步长1增大到15时，v的值依次为2.5000，4.4545，4.9394，4.9950，4.9997，5.0000。

图12 转弯半径r与转弯速度v的关系

4.进一步研究的方向

4.1当障碍物的形状不规则时，模型可进一步改进。问题求解时，对障碍物进行了包络处理。如果遇到不规则的障碍物，需要设计特殊的包络方法。

4.2机器人可以视为一个圆。模型计算时忽略了机器人的体积，将其视为了一点。还可以将机器人视为一个以r为半径的圆。通过圆心在给点区域内的活动来确定机器人的行走路径。这样，将机器人与障碍物之间的距离大于等于r的约束转化为考虑圆心与障碍物边缘的距离问题。