机器人运动控制算法推导及其检验-刘述亮

机器人运动控制算法推导及其检验ViviVivi如图假设分别为轮1轮2轮3速度的单位方向矢量，，假设机器人经过时间（）则机器人三个轮子和机器人中心的位移分别为；假设机器人三个轮子和中心的速度为机器人的角速度为。则有几何关系可得；机器人三个轮子的驱动速度所以轮子的驱动速度为由上式可得（计算程序见附录一）解线性方程可得（见附录二）对于地面坐标，由坐标变换得为了验证上面的结论正确性，对其进行仿真，给定一组一定的速度如果机器人能按照期望运动则说明推导是正确的。（见附录三，四）附录一clcclearsymsvxvyaw;v0=[vxvy];e=zeros(3,2);vi=zeros(3,2);e(1,:)=[01];e(2,:)=[0.5*sqrt(3)0.5];e(3,:)=[0.5*sqrt(3)0.5];fori=1:3forj=1:2(v0(j)+a*w*e(i,j))*e(i,j)endend附录二symsv1v2v3aw;A=[01a;0.5*3^0.50.5a;0.5*3^0.50.5a];b=[v1v2v3]';c=[Ab];rref(c)附录三clearclctic;v1=2;v2=1.5;v3=0.51;a=0.6;dt=0.03;sd=0;x=0;y=0;h=plot(x,y,'.');set(h,'EraseMode','Xor','MarkerSize',20);vx1=v3/sqrt(3)v2/sqrt(3);vy1=(2*v1v2v3)/3;t=0;w1=1;w2=1;fori=1:100000drawnowv1=2*sin(w1*sin(0.5*t)*t)+0.5;v2=2*sin(w2*t);v3=2.51*sin(w1*t+1);xy=zeros(4,2);w=(v1+v2+v3)/3/a;sd=sd+w*dt;t=t+dt;vx=vx1*cos(sd)vy1*sin(sd);vy=vx1*sin(sd)+vy1*cos(sd);x=x+vx*dt;y=y+vy*dt;xy(1,1)=x;xy(1,2)=y;xy(2,1)=x+a*cos(sd);xy(2,2)=y+a*sin(sd);xy(3,1)=x+a*cos(2*pi/3+sd);xy(3,2)=y+a*sin(2*pi/3+sd);xy(4,1)=x+a*cos(2*pi/3+sd);xy(4,2)=y+a*sin(2*pi/3+sd);forj=1:4axis([5555]);plot(xy(j,1),xy(j,2),'.');holdonendholdoffendtoc附录四仿真动画clearclctic;v1=2;v2=1.5;v3=0.51;a=0.6;dt=0.05;sd=0;x=0;y=0;h=plot(x,y,'.');set(h,'EraseMode','Xor','MarkerSize',20);vx1=v3/sqrt(3)v2/sqrt(3);vy1=(2*v1v2v3)/3;t=0;w1=1;w2=1;fori=1:10000drawnowv1=2*sin(w1*sin(0.5*t)*t)+0.5;v2=2*sin(w2*t);v3=2.51*sin(w1*t+1);w=(v1+v2+v3)/3/a;sd=sd+w*dt;t=t+dt;vx=vx1*cos(sd)vy1*sin(sd);vy=vx1*sin(sd)+vy1*cos(sd);x=x+vx*dt;y=y+vy*dt;xy=zeros(4,2);n=10;fork=1:nxy(1,1)=x;xy(1,2)=y;xy(2,1)=x+k*a*cos(sd)/n;xy(2,2)=y+k*a*sin(sd)/n;xy(3,1)=x+k*a*cos(2*pi/3+sd)/n;xy(3,2)=y+k*a*sin(2*pi/3+sd)/n;xy(4,1)=x+k*a*cos(2*pi/3+sd)/n;xy(4,2)=y+k*a*sin(