关闭

模拟移动机器人控制(二)

标签: matlabplotcfunction
569人阅读 评论(0) 收藏 举报
分类:
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

0
0

查看评论
* 以上用户言论只代表其个人观点,不代表CSDN网站的观点或立场
    个人资料
    • 访问:109812次
    • 积分:1215
    • 等级:
    • 排名:千里之外
    • 原创:18篇
    • 转载:16篇
    • 译文:0篇
    • 评论:20条
    文章分类
    最新评论