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

原创 2012年03月28日 12:59:08

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


轨迹取样周期较大,直线速度较大的情况:


版权声明:本文为博主原创文章,未经博主允许不得转载。

相关文章推荐

模拟移动机器人控制(四)(修改)

解决了大于360°时机器人无限调整角度的问题; clc;close all;clear hold on axis([0,200,0,200]); t= 0.01; [X,Y] = ginput(1...

模拟移动机器人控制(四)(再次修改)

环境:WIN7;MATLAB2010 加入了模拟光电编码器的计数显示 clc;close all;clear subplot(121);title('模拟机器人坐标'); hold on a...

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

MATLAB模拟移动机器人控制 程序: clc;close all;clear; PI = [3;7;0]%PI = [xi;yi;ci] PT = [120;81;pi/2]%PT = [xt;...

基于ROS平台的移动机器人-2-小车底盘控制

基于ROS平台的移动机器人-2-小车底盘控制说明本博文将介绍小车底盘控制的原理,如PID控制,控制程序的编写等。小车控制思想 控制电机转动。电机的控制我们分为两部分,一部分为电机转动方向的控制,另一个...

基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动

基于ROS平台的移动机器人-4-通过ROS利用键盘控制小车移动准备工作1.下载串口通信的ROS包(1)cd ~/catkin_ws/src (2)git@github.com:Forrest-Z/se...
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:深度学习:神经网络中的前向传播和反向传播算法推导
举报原因:
原因补充:

(最多只允许输入30个字)