程序太慢,需要优化逆微积分

时间:2017-07-26 09:56:28

标签: matlab

请考虑以下代码:

T01 = [cos(t1) -sin(t1)*cos(alpha1) sin(alpha1)*sin(t1) a1*cos(t1);sin(t1) cos(t1)*cos(alpha1) -cos(alpha1)*sin(t1) a1*sin(t1);0 sin(alpha1) cos(alpha1) d1 ;0 0 0 1];
T12 = [cos(t2) -sin(t2)*cos(alpha2) sin(alpha2)*sin(t2) a2*cos(t2);sin(t2) cos(t2)*cos(alpha2) -cos(alpha2)*sin(t2) a2*sin(t2);0 sin(alpha2) cos(alpha2) d2 ;0 0 0 1];
T23 = [cos(t3) -sin(t3)*cos(alpha3) sin(alpha3)*sin(t3) a3*cos(t3);sin(t3) cos(t3)*cos(alpha3) -cos(alpha3)*sin(t3) a3*sin(t3);0 sin(alpha3) cos(alpha3) d3 ;0 0 0 1];
T34 = [cos(t4) -sin(t4)*cos(alpha4) sin(alpha4)*sin(t4) a4*cos(t4);sin(t4) cos(t4)*cos(alpha4) -cos(alpha4)*sin(t4) a4*sin(t4);0 sin(alpha4) cos(alpha4) d4 ;0 0 0 1];
T45 = [cos(t5) -sin(t5)*cos(alpha5) sin(alpha5)*sin(t5) a5*cos(t5);sin(t5) cos(t5)*cos(alpha5) -cos(alpha5)*sin(t5) a5*sin(t5);0 sin(alpha5) cos(alpha5) d5 ;0 0 0 1];
T56 = [cos(t6) -sin(t6)*cos(alpha6) sin(alpha6)*sin(t6) a6*cos(t6);sin(t6) cos(t6)*cos(alpha6) -cos(alpha6)*sin(t6) a6*sin(t6);0 sin(alpha6) cos(alpha6) d6 ;0 0 0 1];

T= T01*T12*T23*T34*T45*T56 ;

vect = [T(1,4);T(2,4);T(3,4)];
tet= [t1;t2;t3;t4;t5;t6];

J=jacobian(vect,tet);

XYZinit=[x;y;z];
H=pinv(J);

qCible=(tet+(H*(vect-XYZinit)))*180.0/pi;

在这种情况下,程序计算qCible的时间太长。 我看了雅各比亚,这是非常复杂的" 4967757600021511/405648192073033408478945025720320"还有更多!

有没有办法让Matlab将这些数字简化为小数点后的3位数。

P.S这是例如雅可比矩阵的第二行:

[ (667495948725283505644223413159337121101697927362615132085977267803570842210039392837475731450497*cos(t1))/66749594872528440074844428317798503581334516323645399060845050244444366430645017188217565216768000 + (180182749026672061615969125490030497260998891573948681728240766153*cos(t1)*sin(t2))/822752278660603021077484591278675252491367932816789931674304512000 + (23859816081157487611247053147082199287638984338758594710122908873*sin(t3)*(cos(t1) + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064))/822752278660603021077484591278675252491367932816789931674304512000 + (895104220870018647914711647235873838858522473234640872822096410779044699920717183*sin(t1)*sin(t2))/66749594872528440074844428317798503581334516323645399060845050244444366430645017188217565216768000 - (4967757600021511*sin(t5)*(cos(t1)*sin(t2) - (4967757600021511*cos(t1))/81129638414606681695789005144064 + sin(t3)*(cos(t1) + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064 - (4967757600021511*cos(t4)*(cos(t3)*(cos(t1) + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/81129638414606681695789005144064 + (4967757600021511*sin(t4)*(sin(t3)*(cos(t1) + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/81129638414606681695789005144064))/405648192073033408478945025720320 - (21*cos(t4)*(sin(t3)*(cos(t1) + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/1000 - (17277797851872663442627176416851563336929412302080901873592316727*sin(t4)*(cos(t3)*(cos(t1) + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/822752278660603021077484591278675252491367932816789931674304512000 - (4967757600021511*sin(t4)*(sin(t3)*(cos(t1) + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/405648192073033408478945025720320 + (sin(t5)*(cos(t4)*(sin(t3)*(cos(t1) + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))) + sin(t4)*(cos(t3)*(cos(t1) + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)))))/5 + (19*cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)))/100, (sin(t5)*(cos(t4)*(sin(t3)*((4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))) + sin(t4)*(cos(t3)*((4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)))))/5 - (895104220870018647914711647235873838858522473234640872822096410779044699920717183*cos(t1)*cos(t2))/66749594872528440074844428317798503581334516323645399060845050244444366430645017188217565216768000 + (180182749026672061615969125490030497260998891573948681728240766153*cos(t2)*sin(t1))/822752278660603021077484591278675252491367932816789931674304512000 - (4967757600021511*sin(t5)*(cos(t2)*sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + sin(t3)*((4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) - (4967757600021511*cos(t4)*(cos(t3)*((4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/81129638414606681695789005144064 + (4967757600021511*sin(t4)*(sin(t3)*((4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/81129638414606681695789005144064))/405648192073033408478945025720320 + (23859816081157487611247053147082199287638984338758594710122908873*sin(t3)*((4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064))/822752278660603021077484591278675252491367932816789931674304512000 - (21*cos(t4)*(sin(t3)*((4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/1000 - (17277797851872663442627176416851563336929412302080901873592316727*sin(t4)*(cos(t3)*((4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/822752278660603021077484591278675252491367932816789931674304512000 - (4967757600021511*sin(t4)*(sin(t3)*((4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 + (4967757600021511*cos(t2)*sin(t1))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2))))/405648192073033408478945025720320 + (19*cos(t3)*(cos(t1)*cos(t2) - sin(t1)*sin(t2)))/100, (23859816081157487611247053147082199287638984338758594710122908873*cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064))/822752278660603021077484591278675252491367932816789931674304512000 - (4967757600021511*sin(t5)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + (4967757600021511*cos(t4)*(sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/81129638414606681695789005144064 + (4967757600021511*sin(t4)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/81129638414606681695789005144064))/405648192073033408478945025720320 - (21*cos(t4)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/1000 - (4967757600021511*sin(t4)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/405648192073033408478945025720320 + (17277797851872663442627176416851563336929412302080901873592316727*sin(t4)*(sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/822752278660603021077484591278675252491367932816789931674304512000 - (19*sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)))/100 + (sin(t5)*(cos(t4)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))) - sin(t4)*(sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)))))/5, (21*sin(t4)*(sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/1000 - (17277797851872663442627176416851563336929412302080901873592316727*cos(t4)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/822752278660603021077484591278675252491367932816789931674304512000 - (4967757600021511*cos(t4)*(sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/405648192073033408478945025720320 - (4967757600021511*sin(t5)*((4967757600021511*cos(t4)*(sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/81129638414606681695789005144064 + (4967757600021511*sin(t4)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/81129638414606681695789005144064))/405648192073033408478945025720320 + (sin(t5)*(cos(t4)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))) - sin(t4)*(sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)))))/5, (4967757600021511*cos(t5)*((4967757600021511*sin(t1))/81129638414606681695789005144064 + (4967757600021511*cos(t1)*sin(t2))/81129638414606681695789005144064 - sin(t1)*sin(t2) - sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + (4967757600021511*cos(t4)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/81129638414606681695789005144064 - (4967757600021511*sin(t4)*(sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))))/81129638414606681695789005144064))/405648192073033408478945025720320 + (cos(t5)*(cos(t4)*(sin(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) - cos(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1))) + sin(t4)*(cos(t3)*(sin(t1) - (4967757600021511*cos(t1)*cos(t2))/81129638414606681695789005144064 + (4967757600021511*sin(t1)*sin(t2))/81129638414606681695789005144064) + sin(t3)*(cos(t1)*sin(t2) + cos(t2)*sin(t1)))))/5, 0]

1 个答案:

答案 0 :(得分:2)

对于变量表示问题,使用double来获取浮点视图而不是理性。例如,如果a = sym('4967757600021511/405648192073033408478945025720320');,您可以像double(a)那样加倍1.2246e-17

更新

通过添加示例来更新问题,您可以使用与前一部分vpa类似的double在符号表达式中执行此操作。例如,如果符号表达式的名称为a,则可以使用vpa(a, 3)