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

来源：互联网发布：淘宝我的店铺在哪里找编辑：程序博客网时间：2024/05/16 12:44

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;endwhile(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);endi = i+1;L(:,i) = Pholdplot(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