机器人逆解

%TA1400型机器人逆运动学求解 %算法思路来源于有卜王辉的论文——基于切断点自由度解耦的手腕偏置型6R机器人位置反解 %该方法是为了验证其算法的正确与否，所以编程实现 %该方法大致的思路是：用其中的一个关键表示其他关节，最后建立起一个只含有个一个未知... %数的关节的非线性方程 %用关节6的转动角表示其他的关节角，采用迭代的方式求得 %长度单位为mm %2011.7.3 clc %清理matlab输出窗口 clear all %清理空间所有变量 format long %定义机器人定的机械常数 syms JointOne JointTwo JointThree JointFour JointFive JointSix %单位 度 syms Joint%6个关节的转角值 syms s1 s2 s3 s4 s5 s6 c1 c2 c3 c4 c5 c6 %s1对应sin(JointOne*pi/180),c1对应cos(JointOne*pi/180) %机器人D-H参数 syms a1 a2 a3 a5%关节连杆长度 syms d4 %连杆距离 %设定机器人D-H参数 a1=150;%160; a2=570;%560; a3=200;%130; a5=30;%40; d4=640;%600;%单位mm MechanicalConstant=[a1 a2 a3 a5 d4]; %机器人末端工具坐标系 syms nx ny nz syms ox oy oz syms ax ay az %机器人末端工具坐标系的姿态 syms px py pz %机器人末端工具坐标系的位置 %机器人末端工具坐标系位姿（数值型） 指定机器人末端工具坐标系位姿 %{ Te=[ -0.82629575, -0.47274236, 0.30618621, 658.53407; 0.43118622, -0.18118622, 0.88388350, 0.0 ; -0.36237244, 0.86237244, 0.35355340, 483.02788; 0.0, 0.0, 0.0, 1.0 ]; %} Te=[ 0.008660254037844 -0.005000000000000 0.000000000000000 8.199999999999999; 0.004330127018922 0.007500000000000 -0.005000000000000 0.000000000000000; 0.002500000000000 0.004330127018922 0.008660254037844 7.700000000000000; 0 0 0 0.010000000000000;]*100; %{ Te=[1 0 0 950; 0 -1 0 0 ; 0 0 -1 -640; 0 0 0 1]; %} %机器人末端工具坐标系字母与数值对应 nx=Te(1,1);ox=Te(1,2);ax=Te(1,3);px=Te(1,4); ny=Te(2,1);oy=Te(2,2);ay=Te(2,3);py=Te(2,4); nz=Te(3,1);oz=Te(3,2);az=Te(3,3);pz=Te(3,4); %由于建立得到的关节6的等式是非线性一元多次等式 %第一步设法寻找到关节6的有根区间 %关节6的取值范围【-180~180】 syms JointSixLimUp %关节6转动范围的上限 syms JointSixLimDown %关节6转动范围的下限 syms StepSizeAngle %步长，用于搜索有根区间 syms NumberStep %步数 %以机器人TA1400为例 JointSixLimUp=180; JointSixLimDown=-180; StepSizeAngle=1;%步长设置为2 NumberStep=fix((JointSixLimUp-JointSixLimDown)/StepSizeAngle);%向零方向取整 %%{ %对余数进行处理 if(JointSixLimDown+StepSizeAngle*NumberStep<JointSixLimUp) NumberStep=NumberStep+1; end %%} syms A B%Asin(JointFive)+Bcos(JointFive)=0 syms FunctionJointSix%关节6转角的方程式 syms JointSixRootXn %寻找的有根区间左端 syms JointSixRootXnn%寻找的有根区间右端 syms FunctionJointSixRootXn %区间左端时，判断方程式的正负号 syms FunctionJointSixRootXnn%区间右端时，判断方程式的正负号，用于寻找有根区间 JointSixRootXn=JointSixLimDown; %#ok<NASGU> JointSixRootXnn=JointSixLimDown;%赋初值 for i=0:1:NumberStep JointSix=JointSixLimDown+i*StepSizeAngle;%计算当前的关节6的转角 JointSixRootXnn=JointSix;%更新有根区间上限 %计算关节6的正余弦值 s6=sin(JointSix*pi/180); c6=cos(JointSix*pi/180); %求解关节1的转动角 JointOne=atan2(py-(ny*c6-oy*s6)*a5,px-(nx*c6-ox*s6)*a5)*180/pi; A=((py*ox-px*oy)*s6+(px*ny-py*nx)*c6)*d4; B=(px*ay-py*ax+a5*((ox*ay-oy*ax)*s6+(ny*ax-nx*ay)*c6))*d4; %求解关节5的转动角 %计算关节5的第一个解 %JointFive=atan2(B,-A)*180/pi-atan2(0,+sqrt(A^2+B^2))*180/pi; JointFive=atan2(B,-A)*180/pi-atan2(-0,-sqrt(A^2+B^2))*180/pi; %{ if (JointFive>180) JointFive=JointFive-360; %#ok<NASGU> elseif (JointFive<-180) JointFive=JointFive+360; %#ok<NASGU> end %} %计算关节1、5的正余弦值 s1=sin(JointOne*pi/180); c1=cos(JointOne*pi/180); s5=sin(JointFive*pi/180); c5=sin(JointFive*pi/180); FunctionJointSix=((pz-(nz*c6-oz*s6)*a5-((nz*c6-oz*s6)*s5+az*c5)*d4-((nx*c6-ox*s6)*s5+ax*c5)*a3/c1)/a2)^2+(((px-(nx*c6-ox*s6)*a5)/c1-a1+((nz*c6-oz*s6)*s5+az*c5)*a3-((nx*c6-ox*s6)*s5+ax*c5)*d4/c1)/a2)^2-1; %关节6转角的方程式中的系数 %{ t1=nx*c6-ox*s6; t2=nz*c6-oz*s6; n1=d4*s5+a5; FunctionJointSix=((px-t1*n1-ax*d4*c5)/c1+a3*(t2*s5+az*c5)-a1)^2+(t2*n1+... az*d4*c5-pz+a3*(t1*s5+ax*c5)/c1)^2-a2^2 %} %测试用： plot(JointSix,FunctionJointSix); grid on hold on %判断此时的JointSix是否为有效解 if (FunctionJointSix==0) disp('寻找到一个有效的解'); %调用求解其它根的函数，并储存求得的解 end %跟新正负号判断系数的下限FunctionJointSixRootXnn if (i==0) FunctionJointSixRootXn=FunctionJointSix;%%#ok<NASGU> %第一次计算时赋初值 FunctionJointSixRootXnn=FunctionJointSix; else FunctionJointSixRootXnn=FunctionJointSix; end %判断是否为有根区间 if ((FunctionJointSixRootXn>0&&FunctionJointSixRootXnn<0)||... (FunctionJointSixRootXn<0&&FunctionJointSixRootXnn>0)) disp('寻找到有根区间'); %***************************************************************8 %调用二分法精确求解，获得JointSix的精确解 syms JointSixRootMid%定义一个有根区间的中间值 while(JointSixRootXnn-JointSixRootXn>1*10^(-8)) JointSixRootMid=(JointSixRootXn+JointSixRootXnn)/2; %计算有根区间的中间值JointSixRootMid的正余弦值 s6=sin(JointSixRootMid*pi/180); c6=cos(JointSixRootMid*pi/180); %求解关节1的转动角 JointOne=atan2(py-(ny*c6-oy*s6)*a5,px-(nx*c6-ox*s6)*a5)*180/pi; A=((py*ox-px*oy)*s6+(px*ny-py*nx)*c6)*d4; B=(px*ay-py*ax+a5*((ox*ay-oy*ax)*s6+(ny*ax-nx*ay)*c6))*d4; %求解关节5的转动角 %计算关节5的第一个解 %%%%%%%%%%%%%%%忽略第二个解%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %JointFive=atan2(B,-A)*180/pi-atan2(0,+sqrt(A^2+B^2))*180/pi; JointFive=atan2(B,-A)*180/pi-atan2(-0,-sqrt(A^2+B^2))*180/pi; %{ if (JointFive>180) JointFive=JointFive-360; %#ok<NASGU> elseif (JointFive<-180) JointFive=JointFive+360; %#ok<NASGU> end %} %计算关节1、5的正余弦值 s1=sin(JointOne*pi/180); c1=cos(JointOne*pi/180); s5=sin(JointFive*pi/180); c5=sin(JointFive*pi/180); FunctionJointSixMid=((pz-(nz*c6-oz*s6)*a5-((nz*c6-oz*s6)*s5+az*c5)*d4-((nx*c6-ox*s6)*s5+ax*c5)*a3/c1)/a2)^2+(((px-(nx*c6-ox*s6)*a5)/c1-a1+((nz*c6-oz*s6)*s5+az*c5)*a3-((nx*c6-ox*s6)*s5+ax*c5)*d4/c1)/a2)^2-1; %关节6转角的方程式中的系数 %{ t1=nx*c6-ox*s6; t2=nz*c6-oz*s6; n1=d4*s5+a5; FunctionJointSixMid=((px-t1*n1-ax*d4*c5)/c1+a3*(t2*s5+az*c5)-a1)^2+... (t2*n1+az*d4*c5-pz+a3*(t1*s5+ax*c5)/c1)^2-a2^2; %} %判断此时的JointSix是否为有效解 if (FunctionJointSixMid==0) disp('寻找到一个有效的解'); %调用求解其它根的函数，并储存求得的解 end if ((FunctionJointSixRootXn>0&&FunctionJointSixMid>0)||... (FunctionJointSixRootXn<0&&FunctionJointSixMid<0)) JointSixRootXn=JointSixRootMid; else JointSixRootXnn=JointSixRootMid; end end disp('子程序执行');%测试用 JointSixAccurateValue=JointSixRootXn;%获得精确解 %调用求解其它关节转角的函数，并存储求得的解 %****************************************** JointSix=JointSixAccurateValue; %计算关节6的正余弦值 s6=sin(JointSix*pi/180); c6=cos(JointSix*pi/180); %求解关节1的转动角 JointOne=atan2(py-(ny*c6-oy*s6)*a5,px-(nx*c6-ox*s6)*a5)*180/pi; A=((py*ox-px*oy)*s6+(px*ny-py*nx)*c6)*d4; B=(px*ay-py*ax+a5*((ox*ay-oy*ax)*s6+(ny*ax-nx*ay)*c6)