clear all; close all; ts=0.001; for k=1:1:2001 xd(k)=cos((k-1)pits); yd(k)=sin((k-1)pits); thd(k)=tspi(k-1)+pi/2; end for k=1:1:2001 u1(k)=0;u2(k)=0; e1(k)=0;e2(k)=0; e3(k)=0; end y0=[1;0;pi/2]; M=20; for i=0:1:M i pause(0.01); for k=1:1:2001 if k==1 q=y0; end xp(k)=q(1); yp(k)=q(2); th(k)=q(3); 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); u=[u1(k);u2(k)]; B=ts*[cos(q(3)) 0 sin(q(3)) 0 0 1]; L1=10*[cos(q(3)) sin(q(3)) 0; 0 0 1]; L2=L1; cond=norm(eye(2)-L1B); U=u+L1+L2*[ce1(k);ce2(k);ce3(k)]; u1(k)=U(1); u2(k)=U(2); u=[u1(k);u2(k)]; q=q+Bu; e1(k)=cos(ktspi)-q(1); e2(k)=sin(ktspi)-q(2); e3(k)=tskpi+pi/2-q(3); end figure(1); hold on; plot(xd,yd,'r',xp,yp,'b'); xlabel('xd xp');ylabel('yd,yp'); j=i+1; times(j)=j-1; e1i(j)=max(abs(ce1)); e2i(j)=max(abs(ce2)); e3i(j)=max(abs(ce3)); end figure(2); plot(xd,yd,'r',xp,yp,'b'); xlabel('xd xp');ylabel('yd,yp'); figure(3); plot(times,e1i,'-r',times,e2i,'o-b',times,e3i,'o-k'); title('Change of maximum absolute value of error1,error2 and angle with times i'); xlabel('times');ylabel('e1,e2 and angle');

这段代码是MATLAB代码，它包含了一些变量、循环和计算。其中，B是一个包含运动学参数的矩阵，它用于计算机器人的运动学方程。在代码中，B用于计算控制输入的增益。具体来说，B是一个6x1的矩阵，其中前三个元素是cos(q3), 0和sin(q3)，后三个元素是0, 0和1。该矩阵与机器人状态向量q相乘，得到一个6x1的结果向量，其中前三个元素是机器人的位置和方向，后三个元素是机器人的角速度。