MATLAB模拟移动机器人控制
程序:
clc;close all;clear;
PI = [3;7;0]%PI = [xi;yi;ci]
PT = [120;81;pi/2]%PT = [xt;yt;ct]
VW = [5;pi/12]%VW = [v;w]
P = PI; %P = [x;y;c]
[xi,yi,ci,xt,yt,ct,x,y,c,v,w] = df(PI,PT,P,VW);
D = ((x-xt).^2+(y-yt).^2).^0.5;
i = 1;
L(:,i) = P;
while (D>v)
C = atan((yt-y)*(xt-x).^(-1));
while(c>(C+0.5*w)||c<(C-0.5*w))
if (c>C)
n = -1;
else
n = 1;
end
T = [0 0;0 0;0 n];
P = P+T*VW;
[xi,yi,ci,xt,yt,ct,x,y,c,v,w] = df(PI,PT,P,VW);
end
i = i+1;
L(:,i) = P;
T = [cos(c) 0;sin(c) 0;0 0];
P = P+T*VW;
[xi,yi,ci,xt,yt,ct,x,y,c,v,w] = df(PI,PT,P,VW);
D = ((x-xt).^2+(y-yt).^2).^0.5;
end
while(c>(ct+0.5*w)||c<(ct-0.5*w))
if (c>ct)
n = -1;
else
n = 1;
end
T = [0 0;0 0;0 n];
P = P+T*VW;
[xi,yi,ci,xt,yt,ct,x,y,c,v,w] = df(PI,PT,P,VW);
end
i = i+1;
L(:,i) = P
hold
plot(L(1,:),L(2,:),'ro')
plot(L(1,:),L(2,:))
调用函数df:
function [xi,yi,ci,xt,yt,ct,x,y,c,v,w]=df(PI,PT,P,VW)
xi = PI(1,1);
yi = PI(2,1);
ci = PI(3,1);
xt = PT(1,1);
yt = PT(2,1);
ct = PT(3,1);
x = P(1,1);
y = P(2,1);
c = P(3,1);
v = VW(1,1);
w = VW(2,1);
模拟移动步骤和轨迹:
PI =
4
8
0
PT =
120.0000
81.0000
1.5708
VW =
5.0000
0.2618
L =
Columns 1 through 9
4.0000 4.0000 8.3301 12.6603 16.9904 21.3205 25.6506 29.9808 34.3109
8.0000 8.0000 10.5000 13.0000 15.5000 18.0000 20.5000 23.0000 25.5000
0 0.5236 0.5236 0.5236 0.5236 0.5236 0.5236 0.5236 0.5236
Columns 10 through 18
38.6410 42.9711 47.3013 51.6314 55.9615 60.2917 64.6218 68.9519 73.2820
28.0000 30.5000 33.0000 35.5000 38.0000 40.5000 43.0000 45.5000 48.0000
0.5236 0.5236 0.5236 0.5236 0.5236 0.5236 0.5236 0.5236 0.5236
Columns 19 through 27
77.6122 81.9423 86.2724 90.6025 94.1381 98.4682 102.0037 106.3339 109.8694
50.5000 53.0000 55.5000 58.0000 61.5355 64.0355 67.5711 70.0711 73.6066
0.5236 0.5236 0.5236 0.7854 0.5236 0.7854 0.5236 0.7854 0.5236
Columns 28 through 29
114.1995 117.7351
76.1066 79.6421
0.7854 1.5708