六轴机械臂正解（FK）和逆解（IK）算法

【实例简介】
整理出了如下几个计算六轴机械臂正解和逆解的关键点： 01_机器人坐标系和关节的说明 02_算法坐标系的建立 03_D-H参数表的建立 04_FK（正解）算法 05_Matlab辅助计算FK（正解） 06_IK（逆解）算法 07_Matlab辅助计算IK（逆解） 文档中针对FK以及IK算法的推导均有详细的推导过程。希望各位能根据推导过程写出自己的代码。
图1 针对机器人的关节坐标系,我们还需要规定各个关节的转动方向,如下图所示,我们依次描 叙了各个关节的正负方向。在关节坐标系下,机器人各个关节的转动方向,必须和下图相符 图2 2.机器人直角坐标系 目前所说的直角坐标系是机器人的工具坐标系,如下图所示,因为该机器人没有安装执行末 端,所以,工具坐标系原点在第六轴末端法兰中心处,红色箭头是Ⅹ轴,蓝色箭头是乙轴, 根据右手定则确定Y轴方向 @ 图3 基坐标系所在的位置是定义机器人基座的位置,如上图所示,坐标系原点在第一关节的中心 处,红色箭头所示为Ⅹ轴,蓝色箭头所示为乙轴,根据右手定则,即可确定Y轴的方向。 具体如上图旁边那个坐标系所示。 当我们说六轴机械臂位姿时,说的是六轴机械臂末端的位姿,该位姿包括六个参数,分别为 Ⅹ YZABC。六轴机械臂未端的位姿是以基坐标系作为参考坐标系的 :笛卡尔坐标系的Ⅹ轴,Ⅹ轴距离 Y:笛卡尔坐标系的Y轴,Y轴距离 Z:笛卡尔坐标系的Z轴,z轴距离 A:XY-Z固定角坐标系下的偏转角 B:X-Y-Z固定角坐标系下的俯仰角 C:XY-Z固定角坐标系下的回转角 ZA 2 呈A x 图 如上图所示,A对应第一个坐标系中的γ,B对应第二个坐标系中的β,C对应第三个坐标系 中的Q。 为什么我们需要花费专门的一节来讲解六轴机器人的坐标系和关节?因为我们后续要学习 的FK和就是以这个为基础的。 FK是已知六轴机械臂在关节坐标系下各个关节转动的角度,然后求解直角坐标系下的 Ⅹ YZABO K是已知六轴杋械臂在直角坐标系下的 XYZABO,然后求解关节坐标系下各个关节转动的角 度 二.算法坐标系的建立 为了得出D-H参数表,我们首先需要针对六轴机器人的各个关节建立坐标系,在固定的坐 标系下,才能最终得出连杆之间的变换关系,从而建立D-H参数表。 8 2 个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+1 n与Xn+1平行(方向一致) )沿Zn轴,平移 与Xn+1共线 3)沿Xn轴,平移a An与Xn+1原点重 4)将Zn绕Xn+1轴,旋转αn+1 Zn与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参数表如下所示 42 0 90 1-2 0 225 3-4 228.86 4-5 90 0 90 0 -50 0 四.正解(FK)算法 根据连杆变换规贝 T=R(Z, B1*T(Z, di)*T(X, ai)*R(X, ai) 6;00 因为R(Z0 6:00 10 00 00a T(Z,a1)= 00 0010 0001 1000 T(xa10100 001d 0001 00 0 ca ROX. a)= o sa o a;0 0 01 根据以上矩阵变换,可以得到如下连杆变换的通用矩阵如下 Bi -se, ca s0,sa, a,c61 s0 ce:ca d sa asa 0 sa ca d 00 根据以上连杆变换的通用矩阵和之前的D-H参数表,可以得到如下δ个连杆变换矩阵 e10s61^0 S0 0 T 610 010242 0001 s 20-22 S620225c62 00 0 0 01 620s6 0 b20 30 01 00 00 40s日40 00 c0 0 010228.86 0001 s50c650 C650s650 00 00 0 6-s600 6 00 0 C00 1-40 FK算法需要解决的问题是:已知各个关节的转动角度,需要求出末端的位姿。 已知:(61,2,θ3,θ4,θs,θ6 求解:(X,Y,乙,A,B,C) 计算公式如下所示 13 机器末端的位姿矩阵为 7227231 32733 0001 1722723 py 07*1T2T*37*4T5T p 000 在以上的推导过程中,、T、2、37、4、7分别为相对于01、02、63、64、6s、的 已知量。由此可以求解出位置变量(pPP)以及姿态变量a1T2723 五. Matlab辅助计算正解(FK) 根据上一章,我们有如下结论 10s10 10 010242 0001 s2-c620-225s6 C S20225C6 00 10 【实例截图】
【核心代码】