模拟移动机器人控制（二）

最新推荐文章于 2018-12-09 20:32:42 发布

nevermoredanny

最新推荐文章于 2018-12-09 20:32:42 发布

阅读量1k

点赞数

分类专栏： MATLAB 文章标签： matlab plot c function

本文链接：https://blog.csdn.net/nevermoredanny/article/details/7401123

版权

MATLAB 专栏收录该内容

20 篇文章 1 订阅

订阅专栏

MATLAB模拟移动机器人控制

程序：

clc;close all;clear;
PI = [3;7;0]%PI = [xi;yi;ci]
PT = [120;81;pi/2]%PT = [xt;yt;ct]
VW = [7;pi/50]%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));
    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);
    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 =

3
7
0

PT =

120.0000
81.0000
1.5708

VW =

9.0000
0.0628

L =

Columns 1 through 9

3.0000 3.0000 11.9822 20.9113 29.7519 38.4691 47.0286 55.3966 63.5400
7.0000 7.0000 7.5651 8.6931 10.3795 12.6178 15.3989 18.7120 22.5440
0 0.0628 0.1257 0.1885 0.2513 0.3142 0.3770 0.4398 0.5027

Columns 10 through 18

71.4268 79.0258 86.3069 93.2415 99.8022 105.9632 111.7000 116.9901 121.8125
26.8798 31.7023 36.9923 42.7291 48.8901 55.4508 62.3854 69.6666 77.2655
0.5655 0.6283 0.6912 0.7540 0.8168 0.8796 0.9425 1.0053 1.5708