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

机器人运动控制算法推导及其检验

Vivi

Vi

vi

如图假设分别为轮1轮2轮3速度的单位方向矢量，，假设机器人经过时间（）

则机器人三个轮子和机器人中心的位移分别为；

假设机器人三个轮子和中心的速度为机器人的角速度为。

则有几何关系可得

；

机器人三个轮子的驱动速度

所以轮子的驱动速度为

由上式可得（计算程序见附录一）

解线性方程可得（见附录二）

对于地面坐标，由坐标变换得

为了验证上面的结论正确性，对其进行仿真，给定一组一定的速度如果机器人能按照期望运动则说明推导是正确的。（见附录三，四）

附录一

clc

clear

symsvxvyaw;

v0=[vxvy];

e=zeros(3,2);

vi=zeros(3,2);

e(1,:)=[01];

e(2,:)=[05*sqrt(3)05];

e(3,:)=[05*sqrt(3)05];

fori=1:3

forj=1:2

(v0(j)+a*w*e(i,j))*e(i,j)

end

end

附录二

symsv1v2v3aw;

A=[01a;

05*3^0505a;

05*3^0505a];

b=[v1v2v3];

c=[Ab];

rref(c)

附录三

clear

clc

tic;

v1=2;

v2=15;

v3=051;

a=06;

dt=003;

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:100000

drawnow

v1=2*sin(w1*sin(05*t)*t)+05;

v2=2*sin(w2*t);

v3=251*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:4

axis([5555]);

plot(xy(j,1),xy(j,2),);

holdon

end

holdoff

end

toc

附录四仿真动画

clear

clc

tic;

v1=2;

v2=15;

v3=051;

a=06;

dt=005;

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:10000

drawnow

v1=2*sin(w1*sin(05*t)*t)+05;

v2=2*sin(w2*t);

v3=251*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:n

xy(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:4

axis([5555]);

plot(xy(j,1),xy(j,2),);

holdon

end

end

holdoff

end

toc