一、机器人坐标系 全局参考坐标系：以点o为原点，相互正交的x、y轴建立全局参考坐标系 局部参考坐标系：为确定机器人位置，选择C点为位置参考点，{Xr,Yr}定义机器人底盘相对于C的两个轴。 机器人姿态描述为ξ1=[x,y,Θ]T,为了根据分量的移动描述机器人的移动，需要从全局坐标系中映射成局部参考坐标系 即：ξR=R（Θ）ξ1，其中正交旋转矩阵R（Θ）为 二、双轮差速移动机器人运动学模型
假设C点与机器人重心重合，机器人有两个主动轮子，直径r，轮间距为l； 左右轮运动速度分别为VL和VR，则角速度w(t)=(VR-VL)/l, 线速度v(t)=(VR+VL)/2; 那么由局部参考坐标系到全局参考坐标系的映射为：
三、机器人轨迹跟踪控制
上图为机器人运动控制系统的方框图。首先给定期望轨迹，确定机器人控制输入v和w，然后识别出机器人位资坐标(x,y,Θ)，将当前位资坐标和期望位资进行比较得到全局位资偏差，然后通过变换矩阵从全局位资偏差映射到局部位资偏差。在经过控制控制算法矫正得到控制输入v和w，反复过程直到实际位资镇定到期望位资为止。 机器人位资误差方程为：
Xe=(Xr-X)cosΘ+(Yr-Y)sinΘ Ye=-(Xr-X)sinΘ+(Yr-Y)cosΘ Θe=Θr-Θ
位资误差微分方程为：
为实现轨迹跟踪就是寻找一个合适的输入q=(v w)T ，让系统无初始误差要求条件下Pe=(Xe Ye Θe)T有界，且lim || (Xe Ye Θe)T ||=0; 四、切换函数的设计以及滑模控制器的设计 点击打开链接

五、仿真效果


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% t=0:0.1:100; n=length(t); k1=3;k2=3;ky=10;ks=3; xr(1)=1;yr(1)=0;vc=0.2;wc=0; x(1)=1.2;y(1)=-2;se(1)=pi/2;w(1)=0.3;v(1)=0.4; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:n %%期望轨迹直线 ser(i)=pi/3; %期望角度 xr(i+1)=xr(i)+0.1*vc*cos(pi/3);%采样周期0.1s; yr(i+1)=yr(i)+0.1*vc*sin(pi/3); %期望轨迹 %%实际轨迹 se(i+1)=se(i)+0.1*w(i); %实际角度 x(i+1)=x(i)+0.1*v(i)*cos(se(i)); y(i+1)=y(i)+0.1*v(i)*sin(se(i)); %实际轨迹 %%局部位资误差方程Pe=(Xe,Ye,Θe)T e=[cos(se(i))*(xr(i)-x(i))+sin(se(i))*(yr(i)-y(i)); %局部位资误差x_e -sin(se(i))*(xr(i)-x(i))+cos(se(i))*(yr(i)-y(i)); %局部位资误差y_e ser(i)-se(i)]; %局部位资误差theta_e xe(i)=e(1);ye(i)=e(2);see(i)=e(3);% x轴y轴 Θ误差 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%Ux,Uy,Utheta jiaosd=wc+2*vc*ky*ye(i)*cos(see(i)/2)+ks*sin(see(i)/2);%角速度 yed=-(wc+2*ky*ye(i)*vc*cos(see(i))+ks*sin(see(i)/2))*xe(i)+sin(see(i)); sed=-2*ky*ye(i)*vc*cos(see(i)/2)-ks*sin(see(i)/2); jiaosdd=2*ky*vc*yed*cos(see(i)/2)-ky*vc*ye(i)*sin(see(i)/2)*sed+0.5*ks*cos(see(i)/2)*sed; xiansd=vc*cos(see(i))+k1*sin(atan(jiaosd))*jiaosd*xe(i)-k1*vc*sin(atan(jiaosd))*sin(see(i))... +k2*(xe(i)-k1*sin(atan(jiaosd))*ye(i))-k1*cos(atan(jiaosd))*(1/(1+jiaosd^2))*jiaosdd*ye(i);%线速度 w(i+1)=jiaosd;v(i+1)=xiansd; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%轨迹跟踪图示 figure; plot(x(1:300),y(1:300),':k'); hold on; plot(xr(1:300),yr(1:300),'r'); h_legend=legend('实际轨迹','期望轨迹',2); %legend(' ',' ',2)指的是文字说明在图左侧 h_xlabel=xlabel('x/m');h_ylabel=ylabel('y/m'); %坐标轴x轴进行标注 legend boxoff; %背景透明 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%v,w速度图示 figure; plot(v(1:200),'r'); hold on; plot(w(1:200),'g'); h_legend=legend('线速度','角速度',1); %legend(' ',' ',1)指的是文字说明在图右侧 legend boxoff; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%误差图 figure; plot(xe(1:120),'r'); hold on; plot(ye(1:120),':k'); hold on; plot(see(1:120),'--b'); h_xlabel=xlabel('t/s'); h_ylabel=ylabel('x_e/m,y_e/m, heta_e/rad');%x y轴坐标标注 h_legend=legend('x_e','y_e',' heta_e',1); %右上角标注 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %增量式PID控制器 ts=0.001; sys=tf(400,[1,50,0]); dsys=c2d(sys,ts,'z'); [num,den]=tfdata(dsys,'v'); u_1=0.0;u_2=0.0; y_1=0;y_2=0; x=[0,0,0]'; error_1=0;%要两个误差 error_2=0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for k=1:1:1000 time(k)=k*ts; rin(k)=1.0;%阶跃函数 kp=8.8; ki=0.10; kd=10; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %PID控制增量 du(k)=kp*x(1)+kd*x(2)+ki*x(3); u(k)=u_1+du(k); %限幅控制 if u(k)>=10 u(k)=10; end if u(k)<=-10 u(k)=-10; end %输出 yout(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2; error(k)=rin(k)-yout(k); x(1)=error(k)-error_1; %Calculating P x(2)=error(k)-2*error_1+error_2; %Calculating D x(3)=error(k); %Calculating I %参数更新 u_2=u_1;u_1=u(k); y_2=y_1;y_1=yout(k); error_2=error_1; error_1=error(k); end plot(time,rin,'b',time,yout,'r'); xlabel('time(s)');ylabel('rin,yout'); figure; plot(time,error) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %PID控制轮式机器人 t=0:0.1:100; n=length(t); T=0.01; kp=10;ki=4;kd=0; xr(1)=0;yr(1)=1;vr=1;wr=0; x(1)=0;y(1)=0;se(1)=pi/2;w(1)=0.2;v(1)=5; u1_1=0;u1_2=0;y1_1=0;y1_2=0;error1_1=0;error1_2=0;x1=[0,0,0]'; u2_1=0;u2_2=0;y2_1=0;y2_2=0;error2_1=0;error2_2=0;x2=[0,0,0]'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for i=1:n %%期望轨迹直线 ser(i)=pi/3; %期望角度 xr(i+1)=xr(i)+0.1*vr*cos(pi/3);%采样周期0.1s; yr(i+1)=yr(i)+0.1*vr*sin(pi/3); %期望轨迹 %%实际轨迹 se(i+1)=se(i)+0.1*w(i); %实际角度 x(i+1)=x(i)+0.1*v(i)*cos(se(i)); y(i+1)=y(i)+0.1*v(i)*sin(se(i)); %实际轨迹 %%局部位资误差方程Pe=(Xe,Ye,Θe)T e=[cos(se(i))*(xr(i)-x(i))+sin(se(i))*(yr(i)-y(i)); %局部位资误差x_e -sin(se(i))*(xr(i)-x(i))+cos(se(i))*(yr(i)-y(i)); %局部位资误差y_e ser(i)-se(i)]; %局部位资误差theta_e xe(i)=e(1);ye(i)=e(2);see(i)=e(3);% x轴y轴 Θ误差 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% du1(k)=kp*x1(1)+ki*x1(2)+kd*x1(3); u1(k)=u1_1+du1(k); yout1(k)=w(i); error1(k)=wr-yout1(k); x1(1)=error1(k)-error1_1; x1(2)=error1(k)-2*error1_1+error1_2; x1(3)=error1(k); u1_2=u1_1;u1_1=u1(k); y1_2=y1_1;y1_1=yout1(k); error1_2=error1_1; error1_1=error1(k); w(i+1)=u1(k); du2(k)=kp*x2(1)+ki*x2(2)+kd*x2(3); u2(k)=u2_1+du2(k); yout2(k)=v(i); error2(k)=vr-yout2(k); x2(1)=error2(k)-error2_1; x2(2)=error2(k)-2*error2_1+error2_2; x2(3)=error2(k); u2_2=u2_1;u2_1=u2(k); y2_2=y2_1;y2_1=yout2(k); error2_2=error2_1; error2_1=error2(k); v(i+1)=u2(k); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%轨迹跟踪图示 figure; plot(x(1:1000),y(1:1000),':k'); hold on; plot(xr(1:1000),yr(1:1000),'r'); h_legend=legend('实际轨迹','期望轨迹',2); %legend(' ',' ',2)指的是文字说明在图左侧 h_xlabel=xlabel('x/m');h_ylabel=ylabel('y/m'); %坐标轴x轴进行标注 legend boxoff; %背景透明 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%v,w速度图示 figure; plot(v(1:1000),'r'); hold on; plot(w(1:1000),'g'); h_legend=legend('线速度','角速度',1); %legend(' ',' ',1)指的是文字说明在图右侧 legend boxoff; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%误差图 figure; plot(xe(1:1000),'r'); hold on; plot(ye(1:1000),':k'); hold on; plot(see(1:1000),'--b'); h_xlabel=xlabel('t/s'); h_ylabel=ylabel('x_e/m,y_e/m, heta_e/rad');%x y轴坐标标注 h_legend=legend('x_e','y_e',' heta_e',1); %右上角标注 现阶段，结合PID控制轮式机器人尚不能很好地成图，接下来会继续更新的