机器人技术大作业
机器人测控技术大作业
机器人测控技术大作业题目: 电气工程学院学院名称：电气工程学院专业班级：自动学生姓名：学号： 2015指导教师：***考虑如图1所示的双关节刚性机械臂，试分析以下问题：图1 双关节机械臂示意图（1） 用D-H 建模法建立上述机械臂的运动学方程；（2） 忽略重力、摩擦力和干扰项的情况下，建立该机械臂的动力学方程；（3） 如果取11l =，20.8l =，120.5m m ==，初始状态：11220.100.10q q q q ⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦ 试设计一个PD 控制器，让其跟踪一条如下指定的曲线：12sin 2sin 2d d q tq t ππ=⎧⎨=⎩，并利用Matlab 中给出仿真结果。
解：Y 0X 0X 1Y 1X 2Y2①建立坐标系a 、机座坐标系｛0｝b 、杆件坐标系｛i ｝ ②确定参数d i ——相邻坐标系x 轴之间的距离； θi ——相邻坐标系x 轴之间的夹角； l i ——相邻坐标系z 轴之间的距离； αi ——相邻坐标系z 轴之间的夹角。
③相邻杆件位姿矩阵M 01=Rot(z,θ1)·Trans(l 1,0,0)=1000010000110011θθθθc s s c -1000010000101l 001 = 100001001101111011θθθθθθs L c s c L s c -同理可得：M 12=Rot(z,θ2)·Trans(l 2,0,0)=100001002202222022θθθθθθs L c s c L s c -M 23（h ）=Rot(z,θ3)·Trans(l 3,0,0)=100001003303333033θθθθθθs L c s c L s c -④建立方程。
将相邻杆件位置矩阵依次相乘，得：M 0h =M 01·M 12·M 23(h)=1000010012331221101231231233122110123123θθθθθθθθθθc l s l s l c s c l c l c l s c ++++-结果中一些字母的含义如下：C θ123=cos(θ1+θ2+θ3) s θ123=sin(θ1+θ2+θ3) c θ12=cos(θ1+θ2) s θ12=sin(θ1+θ2)（2）二连杆机械臂的动能和势能:21111111111111,,,cos 2K m v v d P m gh h d θθ====-22111111111,cos 2K m d P m gd θθ==-2222221,2K m v P mgy ==()()222222211212211212sin sin cos cos v x y x d d y d d θθθθθθ=+=++=--+()()()222222211221221221122211221211cos 22cos cos K m d m d m d d P m gd m gd θθθθθθθθθθ⎧=++++⎪=>⎨⎪=--+⎩21K K K +=2222121122122212211211()()22cos ()m m d m d m d d θθθθθθθ=+++++其中：连杆1的动能K 1 和势能 P 1:连杆2的动能K 2 和势能 P 2 : 总动能和总势能:21P P P +=)cos(cos )(21221121θθθ+-+-=gd m gd m m二连杆机械臂的拉格朗日函数L:P K L -=)2(21)(21222121222212121θθθθθ ++++=d m d m m)cos(cos )()(cos 2122112121212212θθθθθθθ++++++gd m gd m m d d m n i q Lq L dt d ii i ,2,1,=∂∂-∂∂=F动力学方程:111d L L T dt θθ∂∂=-∂∂ 222d L L T dt θθ∂∂=-∂∂矩阵形式orm:⎥⎦⎤⎢⎣⎡+⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡+⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡+⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡2112212212121211122221222211122111212221121121D D D D D D D D D D D D D D T T θθθθθθθθ (3)自适应PD 跟踪轨迹的simulink 模型① S-Function 的s 函数如下：② S-Function1的t函数如下：图1 PD自适应关节1的跟踪输出和给定信号图2 PD自适应关节2的跟踪输出和给定信号上面的是关节1和关节2的跟踪输出和相对应的给定信号。
机器人技术及应用-大作业1
机器⼈技术及应⽤-⼤作业1⼀、D-H参数⼆、各连杆变换矩阵将D-H参数代⼊到连杆变换矩阵，得：三、各连杆⾄末端连杆的变换矩阵T43=T43T42=T32*T43;T41=T21*T42;Matlab 编程：clcclear allsyms af a d th realsyms d2 th1 th3 th4 realDH=[ 0 0 0 th1 ;-90 0 d2 0 ;90 0 0 th3 ;90 0 60 th4 ]; %DH参数for i=1:4af=DH(i,1) ; %取出DH参数a=DH(i,2) ;d=DH(i,3) ;th=DH(i,4);T{i}=[cos(th) -sin(th) 0 a;sin(th)*cos(af) cos(th)*cos(af) -sin(af) -d*sin(af) ; sin(th)*sin(af) cos(th)*sin(af) cos(af) d*cos(af) ;0 0 0 1 ] ; %连杆变换⽅程endT10=T{1}T21=T{2}T32=T{3}T43=T{4}T42=T32*T43T41=T21*T42四、雅可⽐矩阵雅可⽐矩阵结果：Matlab编程：clcclear allsyms d2 realsyms th1 th3 th4 realT21 =[ 1, 0, 0, 0;0, cos(90), sin(90), d2*sin(90);0, -sin(90), cos(90), d2*cos(90);0, 0, 0, 1];T32 =[ cos(th3), -sin(th3), 0, 0;cos(90)*sin(th3), cos(90)*cos(th3), -sin(90), 0;sin(90)*sin(th3), sin(90)*cos(th3), cos(90), 0;0, 0, 0, 1];T44=[ 1, 0, 0, 0;0, 1, 0, 0;0, 0, 1, 0;0, 0, 0, 1];T43 =[ cos(th4), -sin(th4), 0, 0;cos(90)*sin(th4), cos(90)*cos(th4), -sin(90), -60*sin(90); sin(90)*sin(th4), sin(90)*cos(th4), cos(90), 60*cos(90); 0, 0, 0, 1];T42=T32*T43n1=T41(1:3,1);o1=T41(1:3,2);a1=T41(1:3,3);p1=T41(1:3,4);n2=T42(1:3,1);o2=T42(1:3,2);a2=T42(1:3,3);p2=T42(1:3,4);n3=T43(1:3,1);o3=T43(1:3,2);a3=T43(1:3,3);p3=T43(1:3,4);n4=T44(1:3,1);o4=T44(1:3,2);a4=T44(1:3,3);p4=T44(1:3,4);Sp1=[0 -p1(3) p1(2);p1(3) 0 -p1(1);-p1(2) p1(1) 0] ; %位置⽮量的反对称矩阵Sp2=[0 -p2(3) p2(2);p2(3) 0 -p2(1);p2(2) p2(1) 0];Sp3=[0 -p3(3) p3(2);p3(3) 0 -p3(1);p3(2) p3(1) 0];Sp4=[0 -p4(3) p4(2);p4(3) 0 -p4(1);p4(2) p4(1) 0];pxn1=Sp1*n1;pxo1=Sp1*o1;pxa1=Sp1*a1;pxn2=Sp2*n2;pxo2=Sp2*o2;pxa2=Sp2*a2;pxo3=Sp3*o3;pxa3=Sp3*a3;pxn4=Sp4*n4;pxo4=Sp4*o4;pxa4=Sp4*a4;JL1=[pxn1(3) pxo1(3) pxa1(3)].' ;JL2=[pxn2(3) pxo2(3) pxa2(3)].' ;JL3=[pxn3(3) pxo3(3) pxa3(3)].' ;JL4=[pxn4(3) pxo4(3) pxa4(3)].' ;Ja1=[n1(3) o1(3) a1(3)].' ;Ja2=[n2(3) o2(3) a2(3)].' ;Ja3=[n3(3) o3(3) a3(3)].' ;Ja4=[n4(3) o4(3) a4(3)].' ;J1=[JL1 ; Ja1] ;J2=[JL2 ; Ja2] ;J3=[JL3 ; Ja3] ;J4=[JL4 ; Ja4] ;Jn=[J1 J2 J3 J4];JJn=vpa(Jn,2)五、关节驱动⼒如下图所⽰，杆各长设为li，受外⼒4F=（fx，fy）T静⼒传递：关节驱动⼒：设⼿抓坐标系原点受⼒：4F=[fx,fy,0]T 杆3受⼒：=fz fx f 033-=???=3030010*333fxl fzl fz fx l n杆2受⼒：-+=--===fz c fz s c fx s fz s s fx c fz fx c s s c c c s s s s c c f x R z R f R f 22112110*2202121121121)*,(*),(3321332322θθ-++-=?+=32232131321313332332322fxl c fxl fxl s c fzl s fxl s s fzl c f P n R n杆1受⼒：+---+=??-+-===fz c c fzs c fxs s fzs c fzs c fxs c s fzs fx c fz c fz s c fx s fz s s fx c c s s c f x R f R f 21)211(112)211(12112211211*110110001)*1,(22221211θ-+++++-=?+=321)231132(1132)231132(1213312221221211fxl c c s fxl c s fzl fxl s s fxl c s fxl c s fzl fxl c s s fxl fzl c f P n R n关节驱动⼒：关节1为转动关节：321)231132(11fxl c c s fxl c s fzl fxl s -++=τ关节2为移动关节：fz c 22=τ关节3为转动关节：33fxl -=τ六、速度与加速度分析操作臂的速度和加速度分析该机械⼿由四个关节组成，其中第⼆个关节为移动关节其余三个为转动关节，俩个连杆为l1和l2，应⽤递推法对末端构件的速度、⾓速度和雅克⽐矩阵计算如下所⽰，平⾯简图如下所⽰。
机器人大型作业内容与要求.
机器人大型作业内容与要求
1、内容（范围）
围绕下列内容之一，撰写研究报告/论文。
1）机器人重解选择方法
2）机器人奇异问题解决方法
3）机器人轨迹生成方法
4）机器人位置控制方法
5）机器人控制系统
6）机器人路径规划方法
7）机器人任务规划方法
8）机器人视觉
9）机器人视觉伺服方法
10）其它内容：可结合本人所学专业与研究方向，选择自己感兴趣且与本课程相关的内容。
报告的形式：（1）自己设计方法或系统；（2）国内/国外研究综述。
2、要求（评分标准）
1）在广泛调研分析的基础上独立完成，内容充实，严禁抄袭和下载；（50%）2）要有自己的分析和思路；（20%）
3）内容新颖，层次分明，条理清晰，系统性、逻辑性强；（10%）
4）报告书写规范，文笔流畅；（10%）
5）设计或提出的方法或系统具有一定的新意或原创性。
（10%）
3、报告/论文的格式
参照国家核心期刊科技论文的格式与规范撰写。
作业提交时间：2010年2月5日前
作业提交方式：必须同时以如下2种提交：
1）打印报告/论文全文（含附件）放入本人院办信箱；
2）将报告/论文全文（含附件）通过电子邮件发至：zdmeng@。
机器人技术基础大作业格式
《机器人技术基础》大作业题目：班级：姓名：成绩：一、机器人功能描述（200字）具有供人观赏，娱乐为目的，具有机器人的外部特征，也可以像人，像某种动物等。
同时具有机器人的功能，可以行走或完成动作，有语言能力，会唱歌，有一定的感知能力，可以自主的连续表演事先编好的多套动作。
二、机器人系统的功能构成（框图+文字说明）驱动：电动传动机械结构系统：感受系统：智能传感器基本思路：通过对人类动作的深入了解，分析人类的动作特性，并且与控制对象跳舞机器人的工作原理、动作过程进行比较，从而确定机器人的基本构成并选择合适的机械构件，组装完成机器人的造型。
分析机器人动作的局限性与优势，设定机器人的舞蹈动作，按动作编写程序，完成作品设计。
跳舞机器人的结构完全模仿真人，并实现了双腿分立走路，双臂有很强的自由度，可以完成多种高难度动作。
机器人的双脚为轮式结构，这样不仅可以实现转身和滑步，更突出的优点是在走路时减少了重心的调整，从而减少了机器人的倾斜度，实现了类似真人的走路及跳舞模式。
舞蹈机器人的控制方式是将uC/OS-Ⅱ操作系统嵌入Atmega128处理器中,采用PID算法,对电机、舵机进行实时可靠的控制，进而对机器人主动轮的速度、方向进行有效的控制，使机器人的动作定位更加准确，动作过程更加优美协调。
机器人的双脚为轮式结构，此结构可以很完美地实现转身和滑步。
更突出的优点是在走路时减少了重心的调整，同时也克服了塑料构件机械强度不够高的局限性。
跳舞机器人完全实现了智能化运行，可以用相应软件通过编程实现对舵机的控制，做出各种不同的动作，带给人们另类娱乐。
它可以走进各种不同的场合，如：在学校用于科技教育学习；在家庭用于提供丰富的生活享受；用于社会可以增加更多的新型娱乐项目等。
随着社会对服务业的需求不断扩大，可以代替人的机器人将会有更广阔的前景从近几年世界范围内推出的机器人产品来看，机器人技术正在向智能化、模块化和系统化的方向发展。
其发展趋势主要为：结构的模块化和可重构化；控制技术的的开放化；PC化和网络化；伺服驱动技术的数字化和分散化。
机器人技术大作业
《机器人技术》大作业(201５年秋季学期)题目工业机器人概述姓名学号班级专机械设计制造及其自动化业报告提交日期2015年12月5日哈尔滨工业大学内容及要求1.以某种机器人(如搬运、焊接、喷漆、装配等工业机器人;服务机器人;仿生鱼、蛇等仿生机器人;军用及其它机器人等)为例,撰写一篇大作业,题目自拟，以下内容仅作参考:1)机器人得机械结构设计(包括各部分名称、功能、传动等)；2) 机器人得运动学及动力学分析;3) 机器人得控制及轨迹规划;４)驱动及伺服系统设计；5)电气控制电路图及部分控制子程序。
2.题目自拟,拒绝雷同与抄袭;3.参考文献不少于７篇,其中至少有2篇外文文献；4.报告统一用该模板撰写,字数不少于５000字，上限不限;5.正文为小四号宋体，1、25倍行距;图表规范,标注为五号宋体;6.用A4纸单面打印；左侧装订,１枚钉；7.提交打印稿及03版wｏｒd电子文档,由班长收齐。
8.此页不得删除。
评语：成绩(２0分）: 教师签名:年月日工业机器人概述机器人学就是当今世界极为活跃得研究领域之一,它涉及计算机科学、机械学、电子学、自动控制、人工智能等多种学科。
随着计算机、人工智能与光机电一体化技术得迅速发展,机器人已经成为人类得好帮手。
在航空航天,深海探测中,往往使用机器人代替人类去完成复杂得极限工作任务。
工业机器人就是一个多功能、多自由度得机械与电气一体化得自动机械设备与系统,它可以在制造过程中完成各种任务。
它结合制造主机或生产线，可以形成一个单一得或多台机器自动化系统,在无人参与下,实现搬运、焊接、装配与喷涂等多种生产作业。
目前,工业机器人技术飞速发展，在生产中得应用日益广泛,已成为现代制造业重要得生产高度自动化设备。
一、工业机器人特性自20世纪60年代美国第一代机器人得开始,工业机器人得发展与应用迅速发展起来,工业机器人得最重要得特性概括如下。
1、可编程。
生产自动化得进一步发展就是柔性自动化。
机器人大作业综述
IRB1600型机器人的运动学分析及仿真目录1.引言................................................................................................................ - 2 -1.1 ABB公司简介.................................................................................... - 3 -1.2ABB发展历史 .................................................................................... - 4 -2. IRB1600 ........................................................................................................ - 5 -2.1 IRB1600的资料................................................................................. - 6 -2.2建立基于D-H方法的连杆坐标系 ................................................... - 8 -2.3建立六自由度点焊机器人的运动学方程....................................... - 10 -3. 虚拟样机的建立........................................................................................ - 12 -3.1 导入.................................................................................................. - 12 -3.2 添加约束副...................................................................................... - 13 -3.3 基于ADAMS的机器人运动学仿真 ............................................. - 14 -4. 结语............................................................................................................ - 18 -5. 参考资料.................................................................................................... - 19 -1.引言通过这学期对《机器人技术基础》这门课的学习，我对工业机器人有了一定的了解。
大作业(搬运机器人)
无锡科技职业学院中德机电学院
工业机器人技术大作业
设计题目搬运机器人设计与检修
学生姓名
系别控制系
专业
班级
授课教师龚运新
大作业的内容和要求(含技术要求、图表要求等)：
1、论述搬运机器人的机械设计部分
⑴叙述搬运机器人机械各部分名称及功能
⑵各部件图纸设计及要求（最低画出一个部件图纸）
2、叙述搬运机器人电气设计各部分名称及功能
3、画出搬运机器人电气控制电路图
4、编写部分搬运机器人控制子程序
5、叙述搬运机器人故障检修方法
1。
机器人技术基础大作业整理版
KUKA KR40PA码垛机器人运动学仿真目录摘要 (3)1 引言 (4)1.1机器人发展 (4)1.1.1 发展 (4)1.1.2 现状 (5)2 KUKA机器人综述 (6)2.1 公司简介 (6)2.2 产品 (6)3 机器人理论基础 (7)3.1.求正运动学公式 (7)3.2运动学逆解 (8)3.3 静力分析 (9)4机器人建模 (10)4.1问题描述 (10)4.2模型描述 (10)5 仿真 (11)5.1 轨迹规划 (11)5.2特征曲线 (13)5.2.1 位移曲线 (13)5.2.2 速度曲线 (14)5.2.3 加速度曲线 (16)总结 (17)参考文献 (18)摘要本次作业主要针对KUKA KR40PA码垛机器人进行运动学仿真，根据已知的条件和需要以KUKA机器人为研究对象，对KUKA机器人进行运动学仿真，通过讨论该机器人的运动学问题，然后在matlab环境下，用simmechanics工具箱对该机器人的正运动学、逆运动学、轨迹规划进行了仿真, 通过仿真，观察到了机器人各个关节的运动，并得到了所需的数据，从而能够达到预定的目标.关键字：KUKA KR40PA码垛机器人运动学仿真 matlab1 引言1.1机器人发展1.1.1 发展虽然机器人已经历了30 多个春秋, 繁衍了三代, 是一个拥有几十万台机器人的大家族, 可是至今还没有统一的定义。
什么叫做机器人? 在世界范围内, 对于什么是机器人, 以及什么不属护机器人, 在看法上存在着很大的差差别。
为此在第18 届ISIR (国际机器人学术讨论会)期间, 专门成立了一个工作小组, 它的任务是制定机器人分类的标准, 并确定机器人数量的原则。
总之,不管机器人的定义如何, 现在的工业机器人已从原来概念的“纲领工人”或“通用自动机”逐步演进为从事专门任务的柔性机械。
进入80年代以后, 机器人的发展十分迅速。
198 0年, 全世界工业机器人仅有1万多台,可是到1984 年,除了中国、前苏联和东欧国家之外,全世界已有工业机器人102444 台, 其中以日本为最多, 高达4.4万台, 其次是美国,共有1.3万台, 以下依次是: 德国6600台、法国3380台、瑞典2400台。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
大作业：PUMA 机器人1. 坐标系建立利用D-H 参数法建立坐标系：2．D-H 参数表关节i θ(︒) i α(︒) i l i d运动范围1 90 -90 0 0 -160°~160°2 0 0 a 2 d 2-225°~45° 3 90 90 -a 3 0 -45°~225° 4 0 -90 0 4d-110°~170° 5 0 90 0 0-100°~100° 66d-266°~266°3. 正运动学推导3.1变换矩阵求取1i-1i 11Rot(,) Trans(0,0,d ) Trans(l ,0,0) Rot(,) = 001i i i i i i i i i i i i i i i i i i i i i i c s c s s l c c c c c s l s A z x s c d θθαθαθθθαθαθθααα----⎡⎤⎢⎥-⎢⎥=⎢⎥⎢⎥⎣⎦列各D-H 变换矩阵如下：11010101001000001-⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥⎣⎦c s s c A 22122220222020010001c s a c s c a s A d -⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦ 33233033303301000001c s a c s c a s A -⎡⎤⎢⎥--⎢⎥=⎢⎥⎢⎥⎣⎦344404040400100001-⎡⎤⎢⎥⎢⎥=⎢⎥-⎢⎥⎣⎦c s s c A d 455050505001000001c s s c A ⎡⎤⎢⎥-⎢⎥=⎢⎥⎢⎥⎣⎦ 5666-6066000010001c s s c A d ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦ 注：为书写方面，本文中cos ,sin i i ci si θθ==又由00123456123456T A A A A A A =⋅⋅⋅⋅⋅，利用Matlab 进行符号运算，运行程序PUMAzhengyundongxue （程序详见附录）得：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=100060pz az sz nz py ay sy ny px axsx nx T其中- c6*(s5*(c1*c2*s3 + c1*c3*s2) + c5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3))) - s6*(c4*s1 + s4*(c1*c2*c3 - c1*s2*s3))s6*(c1*c4 - s4*(c2*c3*s1 - s1*s2*s3)) - c6*(s5*(c2*s1*s3 + c3*s1*s2) nx ny == - c5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3)))s4*s6*(c2*s3 + c3*s2) - c6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2))nz ⎧⎪⎪⎪⎨⎪⎪=⎪⎩s6*(s5*(c1*c2*s3 + c1*c3*s2) + c5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3))) - c6*(c4*s1 + s4*(c1*c2*c3 - c1*s2*s3))s6*(s5*(c2*s1*s3 + c3*s1*s2) - c5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3))) sx sy ==+ c6*(c1*c4 - s4*(c2*c3*s1 - s1*s2*s3))s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) + c6*s4*(c2*s3 + c3*s2)sz ⎧⎪⎪⎪⎨⎪⎪=⎪⎩c5*(c1*c2*s3 + c1*c3*s2) - s5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3)) c5*(c2*s1*s3 + c3*s1*s2) + s5*(c1*s4 + c4*(c2*c3*s1 - s1*s2*s3))c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2)ax ay az =⎧⎪=⎨⎪=⎩24623346- d *s1+d *(c1*c2*s3 + c1*c3*s2) - d *s5*(s1*s4 - c4*(c1*c2*c3 - c1*s2*s3)) + a *c1*c2 + c5*d6*(c1*c2*s3 + c1*c3*s2) +a *c1*s2*s3 -a *c1*c2*c3c1*d2 + d *(c2*s1*s3 + c3*s1*s2) + d *s5*(c px py ==263342631*s4 + c4*(c2*c3*s1 - s1*s2*s3)) + a *c2*s1 + c5*d *(c2*s1*s3 + c3*s1*s2) +a *s1*s2*s3 - a *c2*c3*s1d *(c2*c3 - s2*s3) - a *s2 + d *(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2)) + a pz =3*c2*s3 +a *c3*s2⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩ 3.2变换矩阵验证 3.2.1初步验证根据DH 参数表中θi 的初始值，将其带入6T 后得到末端执行器的坐标为px= -d 2py=a 2+d 4+d 6 pz=a 3与初始设定一致，因此初步判断变换矩阵正确。
3.2.2使用matlab 机器人工具箱详细验证4 0 -90 0 d4=0.43307 -110°~170°5 0 90 0 0 -100°~100°6 0 0 0 d6=0.05625 -266°~266°利用matlab机器人工具箱按照以上DH参数，编写并运行程序PUMAyanzheng（程序详见附录）得到如下图，与PUMA560机器人一致，表明DH参数法建模正确。
4. 逆运动学推导反变换法求逆00123456123456T A A A A A A=⋅⋅⋅⋅⋅，且有⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=16pzazsznzpyaysynypxaxsxnxT（1）求1θ由010123451623456161-=⋅=⋅⋅⋅⋅=B A T A A A A A T得11111111111111110001''''''''''''0001nx*c ny*s sx*c sy*s ax*c ay*s px*c py*s -nz -sz -az -pz ny*c - nx*s sy*c - sx*s ay*c - ax*s py*c - px*s nx sx ax px ny sy ay py nz sz az pz ++++⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦4*6*(2*32*3)6*(5*(2*33*2)4*5*(2*32*3))6*(5*(2*32*3)4*5*('''c4*s6 2*33*2))4*6*(2*33*+ c5*c6*s42)--+---++=⎧⎪=⎨++⎪=-⎩s s c c s s c s c s c s c c c c s s c s c c s s c c c s c s s s c s c s nx ny nz's6*(s5*(c2*s3 + c3*s2) - c4*c5*(c2*c3 - s2*s3)) - c6*s4*(c2*c3 - s2*s3)'s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) - c6*s4*(c2*s3 + c3*s2)'c4*c6 + c5*s4*s6=-⎧⎪=⎨⎪=⎩sx sy sz 'c5*(c2*s3 + c3*s2) + c4*s5*(c2*c3 - s2*s3)'c4*s5*(c2*s3 + c3*s2) - c5*(c2*c3 - s2*s3)'s4*s5ax ay az =⎧⎪=⎨⎪=⎩24633246px'=a *c2 + d *(c2*s3 + c3*s2) - d *(c5*(c2*s3 + c3*s2) + c4*s5*(c2*c3 - s2*s3)) - a *c2*c3 + a *s2*s3'a *s2 - d *(c2*c3 - s2*s3) + d *(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2))py =3326- a *c2*s3 - a *c3*s2'd - d *s4*s5pz ⎧⎪⎪⎪⎪⎨⎪⎪=⎪⎪⎩由矩阵对应元素均相等，1(3,3)T61(3,3)B =，1(3,4)T61(3,4)B =得2611s4*s511d - d *s4*s5ay*c - ax*s py*c - px*s =⎧⎨=⎩得到：()261111-=-py*c px*s d d *ay*c - ax*s整理有66211+-+=c *(py d *ay)(px d *ax)*s d三角变换66sin cos μμ+=⎧⎨+=⎩px d *ax r py d *ay r其中66*arctan *μ⎧=⎪⎨+=⎪+⎩r px d ax ()py d ax 得到21arccos μ=±-d θ()r（2）求3θ由21110103453216456B3=63A A A T A A A T ---==得664611121314 c4*c5*c6 + s4*s6- c6*s4 + c4*c5*s6c4*s5-d *c4*s521222324-c4*s6 + c5*c6*s4c4*c6 + c5*s4*s6s4*s5-d *s4*s531323334-c6*s5-s5*s6c5d d *c5414243440001b b b b b b b b b b b b b b b b ⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥-⎢⎥⎢⎥⎣⎦⎣⎦其中B3个元素如下：32111c2323123121232312313123231231412323123*3b ny*s * - nz*s +c *nx*c b sx*c *c - sz*s + sy*s *c b ax*c *c - az*s + ay*s *c b py*s *c - pz*s + c *px*c a a c =⎧⎪=⎪⎨=⎪⎪=+-⎩ 2b21= ny*c1 - nx*s1b22= sy*c1 -sx *s1b23= ay*c1 - ax*s1b24= py*c1 - px*s1-d ⎧⎪⎪⎨⎪⎪⎩23123123123322312312333231231233423123123*3b nz*c + ny*s *s + nx*c *s b sz*c + sx*c *s + sy*s *s b az*c + ax*c *s + ay*s *s b pz*c + py*s *s + px*c *s a s =⎧⎪=⎪⎨=⎪⎪=-⎩ 410420430441b b b b =⎧⎪=⎪⎨=⎪⎪=⎩注：为书写方便，本文中232323cos(),23sin()c s θθθθ=+=+。