matlab lingo 【数学建模】机器人避障问题

已知：

正方形5的左下顶点坐标

(

80

,

60

)

(80,60)

(80,60)，边长

150

150

150机器人与障碍物的距离至少超过

10

10

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

10

10

10个单位。机器人直线行走的最大速度为

v

0

v_0

v0​= 5单位/秒机器人转弯时，最大转弯速度为

v

=

v

(

p

)

=

v

0

/

(

1

+

e

10

−

0.1

p

2

)

v = v(p) = v_0 /( {1+ e^{10-0.1p^2}})

v=v(p)=v0​/(1+e10−0.1p2) 其中

p

p

p是转弯半径。如果超过该速度，机器人将发生侧翻，无法完成行走。A点坐标

(

300

,

300

)

(300,300)

(300,300)

需要研究的问题

问题:机器人从

O

(

0

,

0

)

O(0,0)

O(0,0)出发，到达A的最短时间及其路径问题1：画出机器人只在正方形左上角拐弯，拐弯半径为10的图问题2：机器人只在正方形左上角拐弯，拐弯半径为10，求路径长度和时间问题3：求出最短时间及其路径

问题:机器人从

O

(

0

,

0

)

O(0,0)

O(0,0)出发，到达A的最短时间及其路径

问题1：画出机器人只在正方形左上角拐弯，拐弯半径为10的图

第一问其实是为整个问题服务的 ， 也算是先进行一个预测，写一个样例先

MATLAB 基本数据定义：

x = 80;

y = 210;

r = 10;

theta = 0:pi/20:2*pi; %角度[0,2*pi]

hold on;

画一个正方形：

%定义x,y轴范围

xlim([0,300]);

ylim([0,300]);

%四条线形成一个正方形

line([80,80],[60,210]);

line([80,230],[210,210]);

line([80,230],[60,60]);

line([230,230],[60,210]);

在正方形左上角为圆心画圆

plot(x+r*cos(theta),y+r*sin(theta),'-');

接着求切点 建立切点模型： 设切点为

(

x

1

,

x

2

)

(x_1,x_2)

(x1​,x2​) , 正方形左上角为圆心

(

x

,

y

)

,

x

=

80

;

y

=

210

;

(x,y) , x = 80;y = 210;

(x,y),x=80;y=210; 圆半径

r

=

10

r = 10

r=10 ；切线上除切点另外一点

(

x

2

,

y

2

)

(x_2,y_2)

(x2​,y2​) ; 即圆外一点

(

x

2

,

y

2

)

(x_2,y_2)

(x2​,y2​)引两条切线方程 因为切线和切点到圆心的直线垂直 则有

(

x

−

x

2

)

2

+

(

y

−

y

2

)

2

=

r

2

+

(

x

1

−

x

2

)

2

+

(

y

1

−

y

2

)

2

(x-x_2)^2+(y-y_2)^2 = r^2 + (x_1-x_2)^2 + (y_1-y_2)^2

(x−x2​)2+(y−y2​)2=r2+(x1​−x2​)2+(y1​−y2​)2 并且切点在圆上有：

（

x

1

−

x

）

2

+

(

y

1

−

y

)

2

=

r

2

（x_1-x）^2 + (y_1-y)^2 = r^2

（x1​−x）2+(y1​−y)2=r2

由切点模型和圆外一点

O

(

0

,

0

)

O(0,0)

O(0,0)建立方程求解

%这里在命令行窗口求解即可

solve('(px-80)^2+(py-210)^2=100','px^2+py^2+100=210^2+80^2');

排除不符合要求的点

px = 8064/101 - (252*14^(1/2))/101;

%(252*14^(1/2))/101 + 8064/101

%8064/101 - (252*14^(1/2))/101

py = (96*14^(1/2))/101 + 21168/101;

%21168/101 - (96*14^(1/2))/101

%(96*14^(1/2))/101 + 21168/101

由切点模型和圆外一点

A

(

300

,

300

)

A(300,300)

A(300,300)建立方程求解

%这里在命令行窗口求解即可

solve('(px-80)^2+(py-210)^2=100','(px-300)^2+(py-300)^2+100=220^2+90^2')

排除不符合要求的点

px2 =9084/113 - (36*141^(1/2))/113;

%(36*141^(1/2))/113 + 9084/113

%9084/113 - (36*141^(1/2))/113

py2 =(88*141^(1/2))/113 + 23748/113;

%23748/113 - (88*141^(1/2))/113

%(88*141^(1/2))/113 + 23748/113

将切线画出来

line([0,px],[0,py]);

line([px2,300],[py2,300]);

问题2：机器人只在正方形左上角拐弯，拐弯半径为10，求路径长度和时间

由问题一的图可知，路径分三段 先求最简单的两段直线长度

pdist([[0,0];[px,py]],'euclidean')

pdist([[300,300];[px2,py2]],'euclidean')

%直线总距离

L2 = pdist([[0,0];[px,py]],'euclidean') + pdist([[300,300];[px2,py2]],'euclidean');

机器人走直线时间

ans1 = L2/v0

再求弧线长度和机器人走弧度时间以及总时间 已知圆上弧长公式为:

l

=

∣

θ

∣

r

,

(

θ

为圆心角，弧度

)

l=|\theta|r , (\theta为圆心角，弧度)

l=∣θ∣r,(θ为圆心角，弧度)

建立圆上两点弧长模型: 设圆上两点分别为

(

x

1

,

y

1

)

,

(

x

2

,

y

2

)

(x_1,y_1),(x_2,y_2)

(x1​,y1​),(x2​,y2​) 则弦长为

d

=

(

x

1

−

x

2

)

2

+

(

y

1

−

y

2

)

2

d= \sqrt{(x_1-x_2)^2+(y_1-y_2)^2}

d=(x1​−x2​)2+(y1​−y2​)2

​

设圆心角为

θ

\theta

θ，则圆周角为

θ

/

2

\theta/2

θ/2 连接两点，连接其中一点和圆心并且延长 交圆上一点

(

x

3

,

y

3

)

(x_3,y_3)

(x3​,y3​) ，连接

(

x

3

,

y

3

)

(x_3,y_3)

(x3​,y3​)和另外一点，构成直角三角形 可得

s

i

n

(

θ

/

2

)

=

d

/

(

2

r

)

sin(\theta/2) = d / (2r)

sin(θ/2)=d/(2r) 所以

θ

=

2

∗

arcsin

⁡

(

d

/

(

2

r

)

)

\theta = 2*\arcsin{(d/(2r))}

θ=2∗arcsin(d/(2r)) 弧长

l

=

θ

∗

r

l = \theta * r

l=θ∗r

%求弧度

%圆心角

d=sqrt((px-px2)^2+(py-py2)^2);

therta=2*asin(d/20);

%弧长

L=10*therta;

总时间

%求弧度

%圆心角

d=sqrt((px-px2)^2+(py-py2)^2);

therta=2*asin(d/20);

%弧长

L=10*therta;

%直线总距离

L2 = pdist([[0,0];[px,py]],'euclidean') + pdist([[300,300];[px2,py2]],'euclidean');

v0 = 5;

vp = v0/(1+(exp(1)^(10-0.1*10*10)));

ans1 = L2/v0 + L/vp;

求得为 96.017639004032700

问题3：求出最短时间及其路径

由前两问我们得出了圆上两点弧长模型和建立切点模型 这一问就是结合上面模型，求一个求最小值的最优模型

设直线总长度为

s

1

s_1

s1​,弧线总长度为

s

2

s_2

s2​ 有

min

⁡

a

n

s

=

s

1

/

v

0

+

s

2

/

v

\min{ans = s_1/v0 + s_2/v}

minans=s1​/v0+s2​/v

最优的话拐弯半径和圆心肯定会变化 设转弯圆心为

(

x

,

y

)

(x,y)

(x,y)，半径为

r

r

r 分别以

(

0

,

0

)

和

(

300

,

300

)

(0,0)和(300,300)

(0,0)和(300,300)为圆外一点的切点分别为

(

x

1

,

y

1

)

,

(

x

2

,

y

2

)

(x_1,y_1),(x_2,y_2)

(x1​,y1​),(x2​,y2​) 由建立切点模型我们可得以下方程

{

x

2

+

y

2

=

r

2

+

x

1

2

+

y

1

2

（

x

1

−

x

）

2

+

(

y

1

−

y

)

2

=

r

2

(

x

−

300

)

2

+

(

y

−

300

)

2

=

r

2

+

(

x

1

−

300

)

2

+

(

y

1

−

300

)

2

（

x

2

−

x

）

2

+

(

y

2

−

y

)

2

=

r

2

\begin{cases} x^2+y^2 = r^2 + x_1^2 + y_1^2 \\ （x_1-x）^2 + (y_1-y)^2 = r^2 \\ (x-300)^2+(y-300)^2 = r^2 + (x_1-300)^2 + (y_1-300)^2\\ （x_2-x）^2 + (y_2-y)^2 = r^2 \end{cases}

⎩

⎨

⎧​x2+y2=r2+x12​+y12​（x1​−x）2+(y1​−y)2=r2(x−300)2+(y−300)2=r2+(x1​−300)2+(y1​−300)2（x2​−x）2+(y2​−y)2=r2​

由圆上两点弧长模型和上述切点

(

x

1

,

y

1

)

,

(

x

2

,

y

2

)

(x_1,y_1),(x_2,y_2)

(x1​,y1​),(x2​,y2​)可得方程

s

2

=

2

∗

(

arcsin

⁡

(

x

1

−

x

2

)

2

+

(

y

1

−

y

2

)

2

/

(

2

r

)

)

∗

r

s_2 = 2 * (\arcsin{\sqrt{(x_1-x_2)^2+(y_1-y_2)^2}/(2r)})*r

s2​=2∗(arcsin(x1​−x2​)2+(y1​−y2​)2

​/(2r))∗r

由圆心之间可得直线距离方程

s

1

=

x

2

+

y

2

−

r

2

/

v

+

(

x

−

300

)

2

+

(

y

−

300

)

2

−

r

2

/

v

s_1 = \sqrt{x^2+y^2-r^2}/v + \sqrt{(x-300)^2+(y-300)^2-r^2}/v

s1​=x2+y2−r2

​/v+(x−300)2+(y−300)2−r2

​/v 或者有上述切点可得直线距离方程为

s

1

=

x

1

2

+

y

1

2

/

v

+

(

x

2

−

300

)

2

+

(

y

2

−

300

)

2

/

v

s_1 = \sqrt{x_1^2+y_1^2}/v + \sqrt{(x_2-300)^2+(y_2-300)^2}/v

s1​=x12​+y12​

​/v+(x2​−300)2+(y2​−300)2

​/v

由于在左上角的时候是极限情况，圆心连接正方形左上角并延长出去距离必须大于等于10可得

r

−

(

x

−

80

)

2

+

(

y

−

210

)

2

≥

10

r - \sqrt{(x-80)^2 + (y-210)^2}\ge10

r−(x−80)2+(y−210)2

​≥10

LINGO求解

data:

v0 = 5;

e = 2.71828;

enddata

s = 2*r * @asin(@sqrt((x1-x2)^2 + (y1-y2)^2)/(2*r));

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

min = @sqrt(x^2 + y^2 - r^2)/5 + @sqrt((x-300)^2 + (y-300)^2 - r^2 )/5 + s/v;

x1^2 + y1^2 + r^2 = x^2 + y^2;

(x2 - 300)^2 + (y2-300)^2 + r^2 = (x-300)^2 + (y-300)^2;

(x1-x)^2 + (y1-y)^2 = r^2;

(x2-x)^2 + (y2-y)^2 = r^2;

r-@sqrt((x-80)^2+(y-210)^2)>=10;

x1<80;

y2>210;

x>=80 ; x<=230;

y>=60; y<=210;

得出

Objective value: 94.22825

Objective bound: 94.22825

Variable Value Reduced Cost

V0 5.000000 0.000000

E 2.718280 0.000000

S 11.78994 0.000000

R 12.98856 0.000000

X1 69.80452 0.000000

X2 77.74917 0.000000

Y1 211.9779 0.000000

Y2 220.1387 0.000000

V 4.994814 0.000000

X 82.14139 0.000000

Y 207.9153 0.000000

将数据带入前两问已经写好的MATLAB中可得图像

x = 82.14139;

y = 207.9153;

r = 12.98856;

theta = 0:pi/20:2*pi; %角度[0,2*pi]

plot(x+r*cos(theta),y+r*sin(theta),'-');

hold on;

xlim([0,300]);

ylim([0,300]);

line([80,80],[60,210]);

line([80,230],[210,210]);

line([80,230],[60,60]);

line([230,230],[60,210]);

px = 69.80452;

py = 211.9779;

px2 =77.74917;

py2 =220.1387;

line([0,px],[0,py]);

line([px2,300],[py2,300]);

%求距离

pdist([[0,0];[px,py]],'euclidean')

pdist([[300,300];[px2,py2]],'euclidean')

%求弧度

%圆心角

d=sqrt((px-px2)^2+(py-py2)^2);

therta=2*asin(d/(2*r));

%弧长

L=r*therta;

%直线总距离

L2 = pdist([[0,0];[px,py]],'euclidean') + pdist([[300,300];[px2,py2]],'euclidean');

v0 = 5;

vp = v0/(1+(exp(1)^(10-0.1*r*r)));

ans1 = L2/v0 + L/vp;

MATLAB数据算出来 ， 验证成功

94.228254381074020

精彩文章

 您阅读本篇文章共花了：