尺度综合 - 天津大学研究生e-learning平台.ppt

科学计算在并联机器人研究中的应用 刘祺 1013201070 董成林 1013201060 天津大学机械工程学院 School of Mechanical Engineering, TJU 目录 机构简介 1 问题提出 1 位置正解 2 程序框图 3 程序语句 4 天津大学机械工程学院 School of Mechanical Engineering, TJU 位置正解问题 2 问题提出 1 尺度综合 2 问题求解 3 尺度综合问题 3 机构简介 虎克铰 电机 静平台 支链 动平台 串联转头 问题提出 针对机器人模型的运动学正解问题，运用牛顿迭代法解决非线性方程组无显式解的问题。即通过给定3组支链长度以及机器人相关尺度参数，求解并联部分动平台的沿Z轴方向的移动距离和绕X、Y轴转动的角度。 已知： 机构并联部分 3条支链长度 应用： 牛顿迭代法 约束方程 求解： 动平台X、Y转角 Z向移动距离 位置正解 ， ， 程序框图 开始 定义初值 abs(F)=0.1|abs(G)=0.1|abs(H)=0.1 是 否 程序语句 %forward thita1=2; thita2=2; thita3=2; mm1=b1^2+b2^2-a1^2-a2^2+q1^2+q2^2; mm2=b1^2+b2^2-a1^2-a2^2+q3^2+q2^2; mm3=2*(b1^2)+q1^2+q3^2-4*(a1^2); F=mm1-2*b1*q1*cos(thita1)-2*b2*q2*cos(thita2)-2*q1*q2*sin(thita1)*sin(thita2); G=mm2-2*b1*q3*cos(thita3)-2*b2*q2*cos(thita2)-2*q3*q2*sin(thita3)*sin(thita2); H=mm3+2*(b1-q1*cos(thita1))*(b1-q3*cos(thita3))-2*q1*q3*sin(thita1)*sin(thita3)-2*b1*q1*cos(thita1)-2*b1*q3*cos(thita3); i=0; while(abs(F)=0.1|abs(G)=0.1|abs(H)=0.1) dF1=2*b1*q1*sin(thita1)-2*q1*q2*cos(thita1)*sin(thita2); dF2=2*b2*q2*sin(thita2)-2*q1*q2*sin(thita1)*cos(thita2); dF3=0; dG1=0; dG2=2*b2*q2*sin(thita2)-2*q3*q2*sin(thita3)*cos(thita2); dG3=2*b1*q3*sin(thita3)-2*q3*q2*cos(thita3)*sin(thita2); dH1=2*(b1-q3*cos(thita3))*q1*sin(thita1)+2*b1*q1*sin(thita1)-2*q1*q3*cos(thita1)*sin(thita3); dH2=0; dH3=2*(b1-q1*cos(thita1))*q3*sin(thita3)+2*b1*q3*sin(thita3)-2*q1*q3*sin(thita1)*cos(thita3); J=[dF1,dF2,dF3;dG1,dG2,dG3;dH1,dH2,dH3]; K=[-F;-G;-H]; L=inv(J)*K; dthita1=L(1,1); dthita2=L(2,1); dthita3=L(3,1); thita1=thita1+dthita1; thita2=thita2+dthita2; thita3=thita3+dthita3; i=i+1 mm1=b1^2+b2^2-a1^2-a2^2+q1^2+q2^2; mm2=b1^2+b2^2-a1^2-a2^2+q3^2+q2^2; mm3=2*(b1^2)+q1^2+q3^2-4*(a1^2); F=mm1-2*b1*q1*cos(thita1)-2*b2*q2*cos(thita2)-2*q1*q2*sin(thita1)*sin(thita2); G=mm2-2*b1*q3*cos(thita3)-2*b2*q2*cos(thita2)-2*q3*q2*sin(thita3)*sin(thita2); H=mm3+2*(b1-q1*cos(thita1))*(b1-q3*cos(thita3))-2*q1*q3*sin(thita1