搜索
机器人运动控制算法推导及其检验如图假设321eee分别为轮1轮2轮3速度的单位方向矢量，),();,();1,0(21231212321eee，假设机器人经过时间t（0t）则机器人三个轮子和机器人中心的位移分别为0321;;;ssss；假设机器人三个轮子和中心的速度为0321;;;vvvv机器人的角速度为。则有几何关系可得tviteavsiii*)3,2,1(;*0；机器人三个轮子的驱动速度iiiestV所以轮子的驱动速度为iiieeavV)(0由上式可得（计算程序见附录一）viViavvVavvVavVyxyxy21233212321解线性方程可得（见附录二）aVVVyxVVVvVVv3331231132233333321对于地面坐标，由坐标变换得tyxydyxxddtvvvvvv0cossinsincos为了验证上面的结论正确性，对其进行仿真，给定一组一定的速度如果机器人能按照期望运动则说明推导是正确的。（见附录三，四）附录一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.5-0.5a;0.5*3^0.5-0.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*v1-v2-v3)/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([-55-55]);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*v1-v2-v3)/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(-2*pi/3+sd)/n;forj=1:4axis([-55-55]);plot(xy(j,1),xy(j,2),'.');holdonendendholdoffendtoc