轮式移动机器人基础及滑模控制仿真

一、机器人坐标系

全局参考坐标系：以点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,\theta_e/rad');%x y轴坐标标注
h_legend=legend('x_e','y_e','\theta_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,\theta_e/rad');%x y轴坐标标注
h_legend=legend('x_e','y_e','\theta_e',1); %右上角标注

    

现阶段，结合PID控制轮式机器人尚不能很好地成图，接下来会继续更新的