六轴机械臂正解（FK）和逆解（IK）算法
整理出了如下几个计算六轴机械臂正解和逆解的关键点：01_机器人坐标系和关节的说明02_算法坐标系的建立03_D-H参数表的建立04_FK（正解）算法05_Matlab辅助计算FK（正解）06_IK（逆解）算法07_Matlab辅助计算IK（逆解）文档中针对FK以及IK算法的推导均有详细的推导过程。希望各位能根据推导过程写出自己的代码。图1针对机器人的关节坐标系,我们还需要规定各个关节的转动方向,如下图所示,我们依次描叙了各个关节的正负方向。在关节坐标系下,机器人各个关节的转动方向,必须和下图相符图22.机器人直角坐标系目前所说的直角坐标系是机器人的工具坐标系,如下图所示,因为该机器人没有安装执行末端,所以,工具坐标系原点在第六轴末端法兰中心处,红色箭头是Ⅹ轴,蓝色箭头是乙轴,根据右手定则确定Y轴方向@图3基坐标系所在的位置是定义机器人基座的位置,如上图所示,坐标系原点在第一关节的中心处,红色箭头所示为Ⅹ轴,蓝色箭头所示为乙轴,根据右手定则,即可确定Y轴的方向。具体如上图旁边那个坐标系所示。当我们说六轴机械臂位姿时,说的是六轴机械臂末端的位姿,该位姿包括六个参数,分别为Ⅹ YZABC。六轴机械臂未端的位姿是以基坐标系作为参考坐标系的:笛卡尔坐标系的Ⅹ轴,Ⅹ轴距离Y:笛卡尔坐标系的Y轴,Y轴距离Z:笛卡尔坐标系的Z轴,z轴距离A:XY-Z固定角坐标系下的偏转角B:X-Y-Z固定角坐标系下的俯仰角C:XY-Z固定角坐标系下的回转角ZA2呈Ax图如上图所示,A对应第一个坐标系中的γ,B对应第二个坐标系中的β,C对应第三个坐标系中的Q。为什么我们需要花费专门的一节来讲解六轴机器人的坐标系和关节?因为我们后续要学习的FK和就是以这个为基础的。FK是已知六轴机械臂在关节坐标系下各个关节转动的角度,然后求解直角坐标系下的Ⅹ YZABOK是已知六轴杋械臂在直角坐标系下的 XYZABO,然后求解关节坐标系下各个关节转动的角度二.算法坐标系的建立为了得出D-H参数表,我们首先需要针对六轴机器人的各个关节建立坐标系,在固定的坐标系下,才能最终得出连杆之间的变换关系,从而建立D-H参数表。82个2算法坐标系确定的通用方法如下1)坐标系的Z轴,与各个关节的旋转中心轴线重合2)坐标系的X轴,与沿着相邻两个z轴的公垂线重合3〕坐标系的Y轴,可以通过右手定则来确定当相邻两个z轴相交时,确定坐标系的方法如下1)坐标系的Y轴,沿着第一个Z轴与下一个z轴相交的延长线为Y轴2)坐标系的Ⅹ轴,通过右手定则确定当相邻两个Z轴平行时,确定坐标系的方法如下1)坐标系Ⅹ轴,相邻两个乙轴平行,做两个乙轴的公垂线,相交于下一个z轴为Ⅹ轴,方向为第一个Z轴到下一个Z轴的方向2)坐标系Y轴,通过右手定则确定三.D-H参数表的建立D-H参数表,实际上是相邻各个关节坐标的变换关系表,根据之前针对各个关节所建立的丛标系,按照如下四条变换规则,即可得到D-H参数表。变换规则1)绕Z轴,旋转9n+1n与Xn+1平行(方向一致))沿Zn轴,平移与Xn+1共线3)沿Xn轴,平移aAn与Xn+1原点重4)将Zn绕Xn+1轴,旋转αn+1Zn与Zn+1共线建立D-H参数表的详细步骤第一步,从第一关节到第二关节之间的变换绕z轴,旋转0度,让X0与X1平行且方向一致沿z轴,平移242,让Ⅺ0与X1共线沿Ⅺ轴,平移0,Ⅺ与X1原点重合将石绕X1轴,旋转90度,Z0与z1共线第二步,从第二关节到第三关节之前的交化绕z1轴,旋转90度,让X1与X2平行且方向一致沿Z1轴,平移0,让X1与X2共线沿X1轴,平移225,X1与X2原点重合将Z1绕X2轴,旋转0度,Z1与Z2共线第三步:从第三关节到第四关节之间的交换绕z2轴,旋转0度,让X2与X3平行且方向一致沿Z2轴,平移0,让Ⅹ2与X3共线沿X2轴,平移0,X2与X3原点重合将Z2绕X3轴,旋转90度,Z2与z3共线第四步:从第四关节到第五关节之间的变换绕z3轴,旋转0度让X3与X4平行且方向一致沿3轴,平移22886,让X3与X4共线沿3轴,平移0,X3与X4原点重合将Z3绕X1轴,旋转90度,Z3与Z1共线第五步:从第五关节到第六关节之间的变换绕Z4轴,旋转90度,让X4与X5平行且方向一致沿Z4轴,平移0,让X4与X共线沿X轴,平移0,X1与K5原点重合将Z4绕X5轴,旋转90度,Z4与z共线第六步:从第六关节到未端之间的变换绕z轴,旋转0度,让X5与X6平行且方向一致沿z轴,平移-50,让X5与X6共线沿X轴,平移0,X5与K6原点重合将z绕X轴,旋转0度,Z5与z6共线根据上图所示机械臂的尺寸参数以及以上六个步骤的变换,D-H参数表如下所示420901-202253-4228.864-5900900-500四.正解(FK)算法根据连杆变换规贝T=R(Z, B1*T(Z, di)*T(X, ai)*R(X, ai)6;00因为R(Z06:00100000aT(Z,a1)=00001000011000T(xa10100001d0001000 caROX. a)= osa oa;0001根据以上矩阵变换,可以得到如下连杆变换的通用矩阵如下Bi -se, ca s0,sa, a,c61s0 ce:cad sa asa0 saca d00根据以上连杆变换的通用矩阵和之前的D-H参数表,可以得到如下δ个连杆变换矩阵e10s61^0S0 0T6100102420001s20-22S620225c62000001620s60b203001000040s日4000c0 0010228.860001s50c650C650s650000006-s6006000C001-40FK算法需要解决的问题是:已知各个关节的转动角度,需要求出末端的位姿。已知:(61,2,θ3,θ4,θs,θ6求解:(X,Y,乙,A,B,C)计算公式如下所示13机器末端的位姿矩阵为72272313273300011722723py07*1T2T*37*4T5Tp000在以上的推导过程中,、T、2、37、4、7分别为相对于01、02、63、64、6s、的已知量。由此可以求解出位置变量(pPP)以及姿态变量a1T2723五. Matlab辅助计算正解(FK)根据上一章,我们有如下结论10s10100102420001s2-c620-225s6CS20225C60010
- 2020-12-05下载
- 积分：1
1553B中文手册
恩菲特公司的1553B中文手册，手册丰富，详细，是1553B开发者的优选方案成都恩菲特科長有限公司EPH31580日录恩菲特产品保修条款…安全标志符号,D香看看看。。。垂垂看导D看警告第1章芯片概述…11.简介12.EPH31580特性111.3.EPH31580描述……14.电气特性+++··+++++++.1415.功能描述.…1.6.曼彻斯特解码…171.7.吋间戳178.中断19.存储器访问和寄存器访问1.10. BUS CONTROL框架.171.11. REMOTE TERMINAL框架……11.12. BUS MONITOR框榘18第2章软件接口自●●看命·●●●看鲁●息1921.上电状态22.存储器管理23.寄存器定义…21.中断屏敞寄存器(读/写22.配置寄冇器#1(读/23.配置寄存器#2(读/写)24.开始/复位寄存器(写)2.5.BC/RT命令堆栈指针寄存器(读)….262.6.BC控制字尕RT子地址控制宇寄存器(读)…2627.时标寄存器(读/写)28.中断状态寄存器(读29.配置寄存器#3(读/写)……2.10.配置寄冇器#4(读/写)21.配置寄存器#5(读/写)2.12.数据堆栈指针寄存器(读)…322.13.BC下一条消息开始时间寄存器(读/)214.BC帧时间/RT最后一个命令字/BM触发设置寄存器(读/写)2.15.RT状态字寄存器(读)216.RIB字寄存器(读).342.17. BLOCK STATUS WORD第3章 BUS CONTROL OPERATION……38HTTP:www.enpht.comTel:028-851482738528FAX:028-85148287107第3页成都恩菲特科長有限公司EPH31580BC存储器管理…38ACTIVE AREAS DOUBLE BUFFERING38PROGRAMMING OF BC MESSAGE FRAMES39BC Memory managcmcntMessage Block Formats∴39BC控制字∴…42DESCRIPTOR STACK44C MESSAGE GAP TIMEBC FRAME AUTO REPEATMINOR AND MAJOR FRAMES甲甲···:·…46BCⅠ NTERRUPT……46OTUER FUNCTIONBC SOFTWARE INITIALIZATION SEQUENCE··47BC PSEUDO CODE EXAMPLE.49第4章 REMOTE TERMINAL OPERATION,51RT存储器结构51RT存储器管理SUBADDRESS CONTROL WORDRT STACK AND INTERRUPTSTIME TAG WORDDATA BLOCK POINTER OR MODE DATA WORD57COMMAND WORD RECEIVEDRT COMMAND ILLEGALIZATION57SELECTED MODE CODE INTERRUPT60BROADCAST OPT⊥0NBUSY BITRT ADDRESS INPUTS62RT STATUS WORDRT-TO-RT RESPONSE TIMEOUT64SUMMARY OF RESPONSES TO MODE CODE MESSAGESRT SOFTWARE INITIALIZATION PROCEDURERT PSEUDO CODE EXAMPLE.68第5章 BUS MONITOR OPERATION.71MONITUR SELECTION FUNCTIONMESSAGE MONITOR FORMATSBM存储器管理MESSAGE MONITOR BLOCK STATUS WORDBM SOFTWARE INITIALIZATION PROCEDURE,76第6章 EXTERNAL INTERFACES通垂函a看自PIN DESCRIPTIONS BY FUNCTIONAL GROUPS隔离变压器到外部系统的连接BUFFERED u TRANSPARENT MODEHTTP:w.enphtcomTEL:028-851482738528FAX:028-85148287107第4页成都恩菲特科長有限公司EPH31580外部时序接口BUFFERED模式接口时序85与几和典型器件的连接与ADSP2101的连接与68040的连接与80286的连接.环境温度封装形式…订货号附录一:变压器选型手册97HTTP:/ww.enphtcomTEL:028-85148273/8528FAX:028-85148287107第5页成都恩菲特科長有限公司EPH31580恩菲特产品保修条款产品名称:1553A/B协议芯片产品型号:EP-H31580保修期限:一年1.恩菲特公司对由恩菲特公司出售的硬件产品和附件提供质量保修,保修期限如上所示。在保修期内如果出现因质量原因而产生故障,恩菲特公司在收到关于产品故障的通知并经查验核实后,有权选择维修或整套更换产品。整套更换的产品可以是新的或接近新的2.恩菲特公司保证软件产品经过充分测试。如果恩菲特公司在保修期内收到关于软件故障的通知,将在查验核实后免费更换软件3.恩菲特公司不保证在产品修理过程中产品可不中断地使用。但恩菲特公司保证在合理的期限内修理好发生故障的产品4.产品保修期从产品发运之日或由恩菲特公司开始安装之日起开始计算。如果用户的进度安排延后使恩菲特公司在产品发运之日起30天内仍未开始安装,产品保修期从交付之日后的笫31天开始计算5.恩菲特公司对任何下列情况而导致的产品故障和损坏不提供免费保修:(a)错误的使用或不适当的维护和校正,(b)非恩菲特公司提供的软件、接口、部件或其它物品,(c)未经许可的拆卸、修改和错误使用,(a)超过产品技术规格指明的范围使用,(e)不适当的运输、搬运和存贮,(f)其它不可抗力原因造成的故障或损坏(如HTTP:ww.enphtcomTEL:028-851482738528FAX:028-85148287107第6页成都恩菲特科長有限公司EPH31580地震、战争、交通事故等)6.在法律允许的范围内,上述保修条款是唯一明确的,同时没有任何其它的保修条款,不论是书面的或口头的。恩菲特公司明确表示拒绝承认任何暗示的保修条款和商业条款7.如果用户因使用恩菲特公司产品造成对其它物品损坏或身体伤害,经法院裁定其直接原因是恩菲特公司产品缺陷,恩菲特公司对此负责。版权声明所有恩菲特公司出售的软件产品或随同硬件产品出售的软件和文件,其版杈属恩菲特公司所有,恩菲特公司保留软件产品和文件方面的所有版权。用户对产品的购买并不表示用户在版权方面的任何许未经恩菲特公司书面许可的任何复制和出售均是被禁止的成都恩菲特科拈冇限公司HTTP:www.enpht.comTel:028-851482738528FAX:028-85148287107第7页成都恩菲特科長有限公司EPH31580安全标志符号符号说明符号说明贴在产品上的标志符号表示仪器在操作前必须表示使用者必须遵循产品手保证相应的接线端接地册中相应的警告或注意内容良好,以免电击而引起以免造成人身伤害或设备损设备损坏或人身伤害。坏交流直流表示相应的操作危险。△操作员应严格按规定操高压危险。警告作,否则可能导致人身危险表示相应的操作危险操作员应严格按规定操凸⊥机外壳楼地通常与注意,否则可能寻受慢备设备的金属外壳相连接。损坏或永久性数据丢失HTTP:ww.enphtcomTEL:028-851482738528FAX:028-85148287107第8页成都恩菲特科長有限公司EPH31580敬在操作,维护及修理设备的整个过程中要严格遵守以下安全事项违反这些安全规程或任何本手册中警告和注意事项规定的操作导致的设备损坏或人身伤害,恩菲特公司不对此类事故承担责任●对第一类安全设备(具有俣护地接线端子的设各),必须在产品的主电源输入端或供电电源电缆提供一个可靠的安全地连接对于模块式设备,为确保模块安全接地,应将模块前面板上的紧固螺钉旋紧,以保证模块的紧固面板与机箱保护地可靠接通●仪器不应接触易燃易爆气体或在有易燃易爆气体的环境下操作●为了避免火灾,应使用具有相同电压和电流的保险溶丝。不能使用修理过的保险熔丝或短接保险盒●操作人员不应打开机盖。这只能由经过培训的专业技术人员进行。打开机盖是危险的,因为设备中可能存在危险电压。甚至在设备断电以后,高压也可能存在。为了避免人身伤害,应由经过培训的专业技术人员来操作。●不要操作危险设备或在危险的条件下操作设备。如果任何削弱安全性或可能导致安全保护设施失效的情况存在(包括物理损坏,潮湿或别的原因),应立即拔除电源线,直到由专业技术人员确认后方可操作●不要单独维修或调整仪器,以免发生危险时可得到帮助和救治HTTP:www.enpht.comTel:028-851482738528FAX:028-85148287107第9页成都恩菲特科長有限公司EPH31580不能更换或更改产品中的元器件,除非有明确的认可和授权。因为这将带来其它危险。如仪器出了故障,应将其送到恩菲特公司指定的维修点进行维修,从而保证仪器的各项功能文献版本吏新历史所有版本和手册更新及发行时间都列举在下面。手册的初始版本是Ver1.00。不论何时更新手册,版本号都在尾数加1。当更新涉及到较为重要的内容时,版本号中间的数加1,当更新涉及到核心内容时,版本号第一位数加1。更新的内容通过手册发行,包括修改的内容及对手册新增加的内容。新版本均包括了对旧版本更改的内容。每个新版本或更新后版夲都有一页标注该文献的更改情况Ⅴrl.0l..,,,∴,,.,2005.05HTTP:/ww.enphtcomTEL:028-85148273/8528FAX:028-85148287107第10页
- 2021-05-06下载
- 积分：1
