六轴机械臂正解(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
剔除测量数据中异常值的若干方法
剔除测量数据中异常值的若干方法,第1期何平:剔除测量数据中异常值的若干方法21表3n,a相应的Y值3.91-00.010.010.6790.576190.4620.889).765120.6420.5460.5350.4500.7800.642130.6150.52l210.5240.44060.6980.560140.6410.5460.5140.4300.6370.507150.616230.50580.6830.554160.5950.5070.4130).406100.447180.5610.475表4Z,与n值的对应关系3458902131415161820301050zc1.381.541.651.731.801.881.921.962.002.032.072.102.132.152.202.242.392.492.58表51组测量数据(已按顺序从小到大排好)810t20.3020.3920.3920.3920.4020.4020.4]20.4120.4220.4220.4220.4320.4320.4320.43查表3得到临界值Y。(15,0.05)=0.525,根据也都有其局限性。例如:所有的准则都是以数据按正态狄克逊准则,由于Y2>%(15,0.05),故t值是异常分布为前提的,当偏离正态分布时,判断的可靠性将受值,应予舍弃。影响。还有几个准则对n值的要求也各有不同:当大样程序框图如图3所示本测定时,使用莱因达准则最适合,但当小样本测定24肖维勒准则应用软件流程图及实例时,则一般推荐使用格拉布斯准则和狄克逊准则。而肖计算算术平均值t=20.405维勒准则在某种程度上讲仅仅是莱因达准则的补充计算剩余误差v及均方差a=0.01498在实际测量中,一般取测量次数n=5~20次,特从表4中查得相应的Z值(n=15,故Z2=2.13)别精密的测量,也很少超过100~200次。因此,使用根据肖维勒准则检测l1是否为异常值以上各种准则时,必须注意测量次数的限制。对于莱因1-t|=0.105达准则、一般建议测量次数大于或等于50次,而对于而Zσ=2.13×0.01498≈0.03191格拉布斯准则和狄克逊准则,则建议小于或等于20次。但这一区别并不是十分严格的由于|1-t1>z,则t1值异常,应予舍弃。程序框图对小样本来说,由于格拉布斯准则能给出较严格如图4所示。的结果,狄克逊准则无需计算X和o,方法简便,且23几种方法的进一步讨论者的概率意义明确。因此,它们能较好地适用于采样次从以上的应用情况来看,似乎各种准则的应用实数不太多的一般测量列践都很一致,但这只是个特例,并没有普遍性。举这个设X为N(0,1),在1个大小为n的子样中混入例子,只为了更好地说明几种准则都能得到很好的应个Y:N(μ,δ)的子样。有研究结果表明:格拉布用。需要指出的是,以上各准则都是人为主观拟定的,斯方法的检出概率P略高于狄克逊方法的检出概率直到目前为止,还没有统一的规定,因此,它们的应用PD,如表6所示:(N(0,1)叫作标准正态分布)o1994-2012ChinaAcademicJournalElectronicpUblishingHouse.Allrightsreservedhttp://www.cnki.net2航空计测技术第15卷STARTSTARTSTARTSTART输入数据输入数据输入数据输入数据计算算术平均值入计x根据n值,及均方根偏差从表2中计算出相应y计算算术平均值计算剩余误差;,计算T值并选定均方根偏差σ危险率a选定危险率a计算剩余误差v,均方根偏差判别粗大误差查表得相应的(n,a)从表3中查出%(n,a)值从表4中查出相应Z值打印输出结果判别数据是否为异常?判别敦据是否异常判别粗大误差ENDExDENDEND图1莱因达准则应图2格拉布斯准则图3狄克逊准则应图4肖维勒准则应用程序框图应用程序框图用程序框图用程序框图表6P与PD的比较舍。但是,对待粗大误差,除从测量结果中及时发现和利用剔除原则鉴别外,更重要的是提高工作人员的技术a(%)水平和工作责任心,不要在情绪不宁和极度疲劳的情况5.01.0下,进行重要的测量工作。另外,要保证测量条件的稳定,防止因环境条件剧烈变化而产生的突变影响。只有δ11221122这样,才能提高测量的精度,得到满意的测量结果PG(%)10.240.429.854.22.515.712.731.3参考文献PD(%)9.335.726.850.02.212.910.526.31梁晋文等编著.误差理论与数据处理.北京:中国计由于混入的Y不一定是子样中最大的数据,所以,量出版社,1989实际检出效果还要高一些2何国伟编著,误差分析方法.北京:国防工业出版社,4结束语3王文松.测量列中离群值的判断.电测与仪表,1992,从以上论述可以看出,在进行测量数据处理时,可11)以应用各种准则进行粗大误差判别,以决定数据的取o1994-2012ChinaAcademicJournalElectronicpUblishingHouse.Allrightsreservedhttp://www.cnki.net
- 2021-05-06下载
- 积分:1