模擬移動機器人控制（三）

原創

2020-07-05 03:03

MATLAB模擬移動機器人跟蹤軌跡的控制

代碼：

clc,close all;clear
X = 1:0.1:10;;
P = rand(1,4)*10;
Y = P(1,1)*X+P(1,2)*sin(0.5*X)+P(1,3)*sin(X)+P(1,4)*sin(2*X);
VW = [2;pi/36];%VW = [v;w]
i = 0;
hold
plot(X,Y)
for s=1:2:90
    PI = [X(1,s);Y(1,s);0];%PI = [xi;yi;ci]
    PT = [X(1,s+2);Y(1,s+2);pi/2]%PT = [xt;yt;ct]
    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 = 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
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
plot(L(1,:),L(2,:),'ro')
plot(L(1,:),L(2,:),'r')

調用函數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);

結果：

L =

Columns 1 through 9

1.0000 1.2000 1.4000 1.6000 1.8000 2.0000 2.2000 2.4000 2.6000
16.6681 17.7861 18.2510 18.2884 18.1670 18.1554 18.4787 19.2840 20.6175
0 0 0 0 0 0 0 0 0

Columns 10 through 18

2.8000 2.8000 3.0000 3.0000 3.2000 3.2000 3.4000 3.6000 3.8000
22.4194 22.4194 24.5352 24.5352 26.7432 26.7432 28.7918 30.4433 31.5142
0 1.4835 0 1.4835 0 1.4835 0 0 0

Columns 19 through 27

4.0000 4.2000 4.4000 4.6000 4.8000 5.0000 5.2000 5.4000 5.6000
31.9077 31.6317 30.8013 29.6219 28.3597 27.3008 26.7069 26.7745 27.6035
0 0 0 0 0 0 0 0 0

Columns 28 through 36

5.8000 5.8000 6.0000 6.0000 6.2000 6.2000 6.4000 6.4000 6.6000
29.1810 29.1810 31.3820 31.3820 33.9875 33.9875 36.7176 36.7176 39.2734
0 1.4835 0 1.4835 0 1.4835 0 1.4835 0

Columns 37 through 45

6.6000 6.8000 7.0000 7.2000 7.4000 7.6000 7.8000 8.0000 8.2000
39.2734 41.3826 42.8401 43.5389 43.4853 42.7973 41.6847 40.4164 39.2782
1.4835 0 0 0 0 0 0 0 0

Columns 46 through 54

8.4000 8.6000 8.8000 9.0000 9.2000 9.2000 9.4000 9.4000 9.6000
38.5288 38.3602 38.8699 40.0479 41.7810 41.7810 43.8740 43.8740 46.0837
0 0 0 0 0 1.4835 0 1.4835 0

Columns 55 through 57

9.6000 9.8000 9.8000
46.0837 48.1609 48.1609
1.4835 0 1.5708