现在关于matlab仿真的想法是分几个部分写程序：

目标路径部分、对象建模部分、控制器部分以及参杂在其中的绘图部分。

其中目标路径可以有不同的形式，点、直线、圆、S曲线等；

对象建模从简单的开始，从经典的二轮小车模型开始，在分别对舵机、电机建模，最后统一起来整体建模；

控制器先采用PID算法，再考虑模糊算法。

小车对象为一个多输入多输出的系统。

下午成果：

采用了P控制，对二轮小车的经典模型进行了控制仿真。

当去掉P时，小车路径仅为一点；若P作用越强，小车路径与目标路径偏差越大。去掉偏差，必须加入I作用。

程序如下：

clear all
close all
clc

ts=0.001;
for k=1:1:2000
xd(k)=cos((k-1)*pi*ts);
yd(k)=sin((k-1)*pi*ts);
thd(k)=ts*pi*(k-1)+pi/2;
end

u1(1)=0;u2(1)=0;
e1tmp=0;e2tmp=0;e3tmp=0;
y0=[0;0;pi/2]; 

for k=1:1:2000     
if k==1
    q=y0;
end        
xp(k)=q(1);
yp(k)=q(2);
thp(k)=q(3);

%current error of time k
qd=[xd(k);yd(k);thd(k)];
ce1(k)=qd(1)-q(1);
ce2(k)=qd(2)-q(2);
ce3(k)=qd(3)-q(3);
E=[ce1(k);ce2(k);ce3(k)];
B=ts*[cos(q(3)) 0
      sin(q(3)) 0
      0         1];

Kp=[100 100 100
    100 100 100];
Kd=[0 0 0
    0 0 0];
Ki=[10 10 10
    10 10 10];

if k==1
    Etmp=[e1tmp;e2tmp;e3tmp];       
    Ei=0;
end
Ep=E;Ed=(E-Etmp)/ts;Ei=Ei+E*ts;    
U=Kp*Ep+Kd*Ed+Ki*Ei;
u1(k)=U(1);u2(k)=U(2);
u=[u1(k);u2(k)];

q=q+B*u;

%error of previous time k+1 
e1(k)=cos(k*ts*pi)-q(1);
e2(k)=sin(k*ts*pi)-q(2);
e3(k)=ts*k*pi+pi/2-q(3);


Etmp=E;

end

figure(1);
hold on;
plot(xd,yd,'r',xp,yp,'b');
xlabel('xd xp');ylabel('yd yp');

figure(2)
hold on;
plot(xp,yp,'b');
xlabel('xp');ylabel('yp');