已知:
- 正方形5的左下顶点坐标,边长
- 机器人与障碍物的距离至少超过个单位
- 规定机器人的行走路径由直线段和圆弧组成,其中圆弧是机器人转弯路径。
- 机器人不能折线转弯,转弯路径由与直线路径相切的一段圆弧组成,也可以由两个或多个相切的圆弧路径组成,但每个圆弧的半径最小为个单位。
- 机器人直线行走的最大速度为= 5单位/秒
- 机器人转弯时,最大转弯速度为
其中是转弯半径。如果超过该速度,机器人将发生侧翻,无法完成行走。 - A点坐标
需要研究的问题
- 问题:机器人从出发,到达A的最短时间及其路径
-
- 问题1:画出机器人只在正方形左上角拐弯,拐弯半径为10的图
- 问题2:机器人只在正方形左上角拐弯,拐弯半径为10,求路径长度和时间
- 问题3:求出最短时间及其路径
问题:机器人从出发,到达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),'-');
接着求切点
建立切点模型:
设切点为 , 正方形左上角为圆心 圆半径 ;切线上除切点另外一点 ;
即圆外一点引两条切线方程
因为切线和切点到圆心的直线垂直
则有
并且切点在圆上有:
由切点模型和圆外一点建立方程求解
%这里在命令行窗口求解即可
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
由切点模型和圆外一点建立方程求解
%这里在命令行窗口求解即可
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
再求弧线长度和机器人走弧度时间以及总时间
已知圆上弧长公式为:
建立圆上两点弧长模型:
设圆上两点分别为
则弦长为
设圆心角为,则圆周角为
连接两点,连接其中一点和圆心并且延长 交圆上一点 ,连接和另外一点,构成直角三角形
可得
所以
弧长
%求弧度
%圆心角
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:求出最短时间及其路径
由前两问我们得出了圆上两点弧长模型和建立切点模型
这一问就是结合上面模型,求一个求最小值的最优模型
设直线总长度为,弧线总长度为
有
最优的话拐弯半径和圆心肯定会变化
设转弯圆心为,半径为
分别以为圆外一点的切点分别为
由建立切点模型我们可得以下方程
由圆上两点弧长模型和上述切点可得方程
由圆心之间可得直线距离方程
或者有上述切点可得直线距离方程为
由于在左上角的时候是极限情况,圆心连接正方形左上角并延长出去距离必须大于等于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;
94.228254381074020
版权声明:本文为博主作者:晓觉儿原创文章,版权归属原作者,如果侵权,请联系我们删除!
原文链接:https://blog.csdn.net/weixin_72899100/article/details/137650616