六自由度机器人matlab源程序

clear %下平台中各支点的位置，世界坐标系 %基本的设计参数 a1=[-52.0945,295.4423,0]'; a2=[52.0945,295.4423,0]'; a3=[281.9078,-102.6060,0]'; a4=[229.8133,-192.8363,0]'; a5=[-229.8133,-192.8363,0]'; a6=[-281.9078,-102.6060,0]'; %上平台中各支点的位置，世界坐标系 %基本的设计参数 b1=[-114.9067,96.4181,600]'; b2=[114.9067,96.4181,600]'; b3=[140.9539,51.3030,600]'; b4=[26.0472,-147.7212,600]'; b5=[-26.0472,-147.7212,600]'; b6=[-140.9539,51.3030,600]'; %上平台在下平台的投影 %请注意是投影不是b点在下平台坐标系中的坐标 %这样做是为了计算y在世界坐标系中的单位向量 bm1=[-114.9067,96.4181,0]'; bm2=[114.9067,96.4181,0]'; bm3=[140.9539,51.3030,0]'; bm4=[26.0472,-147.7212,0]'; bm5=[-26.0472,-147.7212,0]'; bm6=[-140.9539,51.3030,0]'; %计算各条退的长度，一个很重要的初始条件 leglen1=norm(b1-a1); leglen2=norm(b2-a2); leglen3=norm(b3-a3); leglen4=norm(b4-a4); leglen5=norm(b5-a5); leglen6=norm(b6-a6); %计算各条腿的单位向量 %计算刚体x轴在世界坐标系中的单位向量 x1=(b1-a1);x1=x1/norm(x1); x2=(b2-a2);x2=x2/norm(x2); x3=(b3-a3);x3=x3/norm(x3); x4=(b4-a4);x4=x4/norm(x4); x5=(b5-a5);x5=x5/norm(x5); x6=(b6-a6);x6=x6/norm(x6); %计算各条腿在下平面投影的向量 %为了计算刚体y轴在世界坐标系中的单位向量 xm1=(bm1-a1); xm2=(bm2-a2); xm3=(bm3-a3); xm4=(bm4-a4); xm5=(bm5-a5); xm6=(bm6-a6); %计算刚体y轴在世界坐标系中的单位向量 y1=cross(xm1,x1);y1=y1/norm(y1); y2=cross(xm2,x2);y2=y2/norm(y2); y3=cross(xm3,x3);y3=y3/norm(y3); y4=cross(xm4,x4);y4=y4/norm(y4); y5=cross(xm5,x5);y5=y5/norm(y5); y6=cross(xm6,x6);y6=y6/norm(y6); %计算刚体z轴在世界坐标系中的单位向量 z1=cross(x1,y1);z1=z1/norm(z1); z2=cross(x2,y2);z2=z2/norm(z2); z3=cross(x3,y3);z3=z3/norm(z3); z4=cross(x4,y4);z4=z4/norm(z4); z5=cross(x5,y5);z5=z5/norm(z5); z6=cross(x6,y6);z6=z6/norm(z6); %合成旋转矩阵 %最重要的结果 trans1=[x1,y1,z1]; trans2=[x2,y2,z2]; trans3=[x3,y3,z3]; trans4=[x4,y4,z4]; trans5=[x5,y5,z5]; trans6=[x6,y6,z6];