轨迹规划程序

1、正解：iai-1i-1di i关节变量初值1000 1 102-17000 2 212030901300无初始位姿：t1=0T10=cos(t1) -sin(t1) 0 0;sin(t1)*cos(0) cos(t1)*cos(0) -sin(0) 0;sin(t1)*sin(0) cos(t1)*sin(0) cos(0) 0;0001t2=(120)/180*piT21=cos(t2) -sin(t2) 0 -170;sin(t2)*cos(0) cos(t2)*cos(0) -sin(0) 0;sin(t2)*sin(0) cos(t2)*sin(0) cos(0) 0;0001t3=0

2、T32=cos(t3) -sin(t3) 0 0;sin(t3)*cos(pi/2) cos(t3)*cos(pi/2) -sin(pi/2) -130;sin(t3)*sin(pi/2) cos(t3)*sin(pi/2) cos(pi/2) 130*cos(pi/2);0001T30=T10*T21*T32T30 =-0.5000-0.00000.8660-57.41670.8660-0.00000.500065.000001.00000.00000.00000001.0000中间位姿：t1=-pi/2T10=cos(t1) -sin(t1) 0 0;sin(t1)*cos(0) cos(

3、t1)*cos(0) -sin(0) 0;sin(t1)*sin(0) cos(t1)*sin(0) cos(0) 0;0001t2=(180)/180*piT21=cos(t2) -sin(t2) 0 -170;sin(t2)*cos(0) cos(t2)*cos(0) -sin(0) 0;sin(t2)*sin(0) cos(t2)*sin(0) cos(0) 0;0001t3=0T32=cos(t3) -sin(t3) 0 0;sin(t3)*cos(pi/2) cos(t3)*cos(pi/2) -sin(pi/2) -130;sin(t3)*sin(pi/2) cos(t3)*sin

4、(pi/2) cos(pi/2) 130*cos(pi/2);0001T30=T10*T21*T32T30 =0.0000-0.00001.0000130.00001.00000.0000-0.0000170.000001.00000.00000.00000001.0000抓球位姿：t1=-(151.9275)/180*piT10=cos(t1) -sin(t1) 0 0;sin(t1)*cos(0) cos(t1)*cos(0) -sin(0) 0;sin(t1)*sin(0) cos(t1)*sin(0) cos(0) 0;0001t2=-(118.0725)/180*piT21=cos(

5、t2) -sin(t2) 0 -170;sin(t2)*cos(0) cos(t2)*cos(0) -sin(0) 0;sin(t2)*sin(0) cos(t2)*sin(0) cos(0) 0;0001t3=0T32=cos(t3) -sin(t3) 0 0;sin(t3)*cos(pi/2) cos(t3)*cos(pi/2) -sin(pi/2) -130;sin(t3)*sin(pi/2) cos(t3)*sin(pi/2) cos(pi/2) 130*cos(pi/2);0001T30=T10*T21*T32T30 =-0.0000-0.00001.0000280.00001.00

6、00-0.00000.000080.000001.00000.00000.00000001.0000所得结果与图所示完全一致反解：中间位姿到初始位姿symst1t2x=130;y=170;T10=cos(t1) -sin(t1) 0 0;sin(t1)*cos(0) cos(t1)*cos(0) -sin(0) 0;sin(t1)*sin(0) cos(t1)*sin(0) cos(0) 0;0001;T21=cos(t2) -sin(t2) 0 -170;sin(t2)*cos(0) cos(t2)*cos(0) -sin(0) 0;sin(t2)*sin(0) cos(t2)*sin(0)

7、 cos(0) 0;0001;t3=0T32=cos(t3) -sin(t3) 0 0;sin(t3)*cos(pi/2) cos(t3)*cos(pi/2) -sin(pi/2) -130;sin(t3)*sin(pi/2) cos(t3)*sin(pi/2) cos(pi/2) 130*cos(pi/2);0001;T30 =T10*T21*T32;T31=T21*T32;f1=x-T30(1,4);f2=y-T30(2,4);t1,t2=solve(f1,f2,t1,t2)t1=vpa(t1/pi*180,6)t2=vpa(t2/pi*180,6)结果：t1 =-90.0181-164.

8、853t2 =180.0360.所得结果与图所示一致抓取位姿到初始位姿的关节角度：t1 =-151.961-176.185t2 =-118.093-61.9431计算雅克比矩阵：（1）中间位姿到初始位姿symst1t2q1q2q3v1v2w1w2t1=-pi/2T10=cos(t1) -sin(t1) 0 0;sin(t1)*cos(0) cos(t1)*cos(0) -sin(0) 0;sin(t1)*sin(0) cos(t1)*sin(0) cos(0) 0;0001t2=(180)/180*piT21=cos(t2) -sin(t2) 0 -170;sin(t2)*cos(0) cos

9、(t2)*cos(0) -sin(0) 0;sin(t2)*sin(0) cos(t2)*sin(0) cos(0) 0;0001t3=0T32=cos(t3) -sin(t3) 0 0;sin(t3)*cos(pi/2) cos(t3)*cos(pi/2) -sin(pi/2) -130;sin(t3)*sin(pi/2) cos(t3)*sin(pi/2) cos(pi/2) 130*cos(pi/2);0001T31=T21*T32;T30=T10*T21*T32;T20=T10*T21;z1 =T10(1:3,3);z2 =T20(1:3,3);z3=T30(1:3,3)p1=T31(

10、1:3,4);p2=T32(1:3,4);r1=T10(1,1:3);T10(2,1:3);T10(3,1:3);r2=T20(1,1:3);T20(2,1:3);T20(3,1:3);p11=cross(z1,r1*p1);p22=cross(z2,r2*p2);j=p11(1,1) p22(1,1) ;p11(2,1) p22(2,1) ;p11(3,1) p22(3,1) ;z1(1,1) z2(1,1) ;z1(2,1) z2(2,1) ;z1(3,1) z2(3,1)所得结果如下：=-170.00000.0000 130.0000 130.00000000001.00001.0000

11、计算各关节变量值：v=v1;v2; w1;w2;q=q1;q2;aa=j*q;f1=v(1,1)-aa(1,1);f2=v(2,1)-aa(2,1);q1,q2=solve(f1,f2,q1,q2);q1=vpa(q1,4)q2=vpa(q2,4)求得的结果：q1 =.3602e-18*v2-.5882e-2*v1q2 =.5882e-2*v1+.7692e-2*v2（2）抓球位姿到中间位姿：将各关节 t1=-(151.9275)/180*pit2=-(118.0725)/180*pi带入上述程序得雅克比矩阵j =-80.0000-0.0000280.0000130.00000000001.0

12、0001.0000计算各关节变量值：q1 =-.1250e-1*v1-.1837e-17*v2q2 =0.7692e-2*v2+0.2692e-1*v1出师表两汉：诸葛亮先帝创业未半而中道崩殂， 今天下三分， 益州疲弊， 此诚危急存亡之秋也。然侍卫之臣不懈于内，忠志之士忘身于外者，盖追先帝之殊遇，欲报之于陛下也。诚宜开张圣听，以光先帝遗德，恢弘志士之气，不宜妄自菲薄，引喻失义，以塞忠谏之路也。宫中府中，俱为一体；陟罚臧否，不宜异同。若有作奸犯科及为忠善者，宜付有司论其刑赏，以昭陛下平明之理；不宜偏私，使内外异法也。侍中、侍郎郭攸之、费祎、董允等，此皆良实，志虑忠纯，是以先帝简拔以遗陛下：愚以为

13、宫中之事，事无大小，悉以咨之，然后施行，必能裨补阙漏，有所广益。将军向宠，性行淑均，晓畅军事，试用于昔日，先帝称之曰愚以为营中之事，悉以咨之，必能使行阵和睦，优劣得所。“能 ”，是以众议举宠为督：亲贤臣， 远小人， 此先汉所以兴隆也； 亲小人， 远贤臣， 此后汉所以倾颓也。 先帝在时，每与臣论此事， 未尝不叹息痛恨于桓、 灵也。 侍中、尚书、 长史、 参军，此悉贞良死节之臣，愿陛下亲之、信之，则汉室之隆，可计日而待也。臣本布衣，躬耕于南阳，苟全性命于乱世，不求闻达于诸侯。先帝不以臣卑鄙，猥自枉屈，三顾臣于草庐之中，咨臣以当世之事，由是感激，遂许先帝以驱驰。后值倾覆，受任于败军之际，奉命于危难之间，尔来二十有一年矣。先帝知臣谨慎，故临崩寄臣以大事也。受命以来，夙夜忧叹