一种基于折弯机机械手的运动关节角求解方法
【专利摘要】本发明公开了一种基于折弯机机械手的运动关节角求解方法，包括如下步骤：S1、获得基座至执行末端的全部位姿变换矩阵Mi；S2、根据所述位姿变换矩阵Mi建立各个关节的位姿方程Wi；S3、根据所述Wi得到各个关节的位姿矩阵的微变换dWi；S4、根据所述dWi得到各个关节位姿矩阵的微分平移变量di与微分旋转变量δi；其中：di＝dxi+dyj+dzk；δi＝δxi+δyj+δzk；(i，j，k)为沿坐标系x，y，z轴方向的单位方向向量；S5、根据所述δx、δy和δz判断各个关节相对于x，y，z轴的旋转方向。上述方法，可以方便地选取符合要求的关节角度，一定程度上节省了时间，简化了计算过程。
【专利说明】
一种基于折弯机机械手的运动关节角求解方法
技术领域
[0001] 本发明涉及机器人技术领域，特别涉及一种基于折弯机机械手的运动关节角求解 方法。
【背景技术】
[0002] 对应于机械手末端在空间的某一位置，其各关节在空间中的位置变换可以有多组 不同的状态，实际上机械手在运动过程中只能选取一个位置，其受到关节角转角有范围的 限制，同时在机械手跟踪折弯机板材运动轨迹时，解的选择要符合关节角变化平缓以及减 少机器人在工作过程中角度突变的情况，更要减少关节运动的时间，满足目标关节角与当 前关节角尽可能接近的原则。
[0003] 然而在实际情况中，由于解的复杂性，并不一定能够很容易的选取合适的解，满足 上述要求的限制。比如，有些情况是可能存在有两个相同大小，但是正负（方向）不同的逆 解，虽然从数值角度符合要求，但是不能随机确定一组合适的关节角的解，所以此时需要一 种能够易于直接判断选取关节角的方法。
[0004] 机器人的控制需要唯一的确切解，若不考虑避障要求，可以按照以下步骤获取唯 一的解，首先:若关节角的求解表达式的值域不能完全包含该关节运动范围，则在必要的条 件下可能需要将求得的关节角加减360度，补出关节运动范围以外的其他可能解;其次，剔 除关节运动范围以外的可能解;最后，有运动的连续性，选取于该关节角上一时刻的值距离 最近的可能解为唯一的最优解。因此，各关节角的求解顺序为:先确定第一角度的唯一最优 解，再利用第一角度的逆解求取和多解处理步骤求取第二角度的最优解，依次类推求解其 余关节角，最后取得的一组解是目标关节角。
【发明内容】
[0005] 本发明的目的是提供一种基于折弯机机械手的运动关节角求解方法，该方法可以 解决求解运动角度较为复杂的问题。
[0006] 为实现上述目的，本发明提供一种基于折弯机机械手的运动关节角求解方法，包 括如下步骤：
[0007] S1、获得基座至执行末端的全部位姿变换矩阵Mi;
[0008] S2、根据所述位姿变换矩阵Mi建立各个关节的位姿方程Wi;
[0009] S3、根据所述Wi得到各个关节的位姿矩阵的微变换dWi;
[0010] S4、根据所述dWi得到各个关节位姿矩阵的微分平移变量di与微分旋转变量Si;
[0011] 其中：di = dxi+dyj+dzk; 5i = 5xi+5yj+5zk;
[0012] (1，」，1〇为沿坐标系1，7,2轴方向的单位方向向量；
[0013 ] S5、根据所述5X、和5Z判断各个关节相对于x，y，z轴的旋转方向。
[0014]相对于上述【背景技术】，本发明提供的运动关节角求解方法，针对折弯机在进行板 材折弯的过程中，机械手末端是缓慢变化这一特征，引用微分平移变量cU与微分旋转变量心 来描述各关节的位姿变换矩阵Mi，计算得到的位姿方程Wi以及微变换dWi能更加直观的表达 各个关节直接所对应的运动关系，并通过各关节的位姿变换矩阵1判断所应选取的折弯机 机械手运动关节角的值。采用本发明涉及的求解方法，能够直观的体现出折弯机机械手折 弯板材运动过程中各运动关节角运动变化的趋势和转动的方向，从而方便地选取符合要求 的关节角度，一定程度上节省了时间，简化了计算过程。
[0015] 优选地，所述步骤S1中，根据D-H法获得所述位姿变换矩阵I。
[0016] 优选地，所述步骤S2中，建立所述各个关节的位姿方程1的方式具体包括：
[0017] ffi=ff=MiXM2X---Mi；
[0018] Wi-^ffiXMr1；
[0019] 2=1-jMi-i-S
[0020] Wi-3=ffi-2XMiV1；
[0021] 1
[0022] Wi-n=Wi-n+1 X Mi-柯―1;其中，n = l、2、……i_l;
[0023] 主
[0024] ffi=ff2XM2_1〇
[0025]优选地，所述步骤S3中，所述各个关节的位姿矩阵的微变换dWi具体为：dWi = WiX [_I+T(dx，dy，dz) XR(f，d9)]，
[0026]其中 7V/x，c(v'，c/z) 为三阶单位矩阵，達，办
[0028] 优选地，所述步骤S4中，根据所述dWi得到各个关节位姿矩阵的微分平移变量cU与 微分旋转变量~的具体步骤包括：
[0029] 设定f xd9 = 5x，f yd9 = 5y，f zd9 = 5z; rr: 勒 _〇]得到-i+7Vfc办，必）x=|言g；
[0031] 其中：
[0032] 优选地，所述步骤S5中，根据所述Sx、Sy和Sz判断各个关节分别朝向 x，y，z轴的哪一 方向旋转的具体步骤包括：
[0033]当Sx>〇时，则该关节绕X轴方向旋转；
[0034]当Sx<〇时，则该关节绕X轴相反方向旋转；
[0035]当Sy > 0时，则该关节绕Y轴方向旋转；
[0036] 当Sy<〇时，则该关节绕Y轴相反方向旋转；
[0037] 当^>〇时，则该关节绕Z轴方向旋转；
[0038]当Sz<〇时，则该关节绕Z轴相反方向旋转。
【附图说明】
[0039] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现 有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本 发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据 提供的附图获得其他的附图。
[0040] 图1为本发明实施例所提供的机器人D-H表示法参考坐标系的示意图；
[0041 ]图2为本发明实施例所提供的运动关节角求解方法的流程图。
【具体实施方式】
[0042]下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完 整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他 实施例，都属于本发明保护的范围。
[0043]为了使本技术领域的技术人员更好地理解本发明方案，下面结合附图和具体实施 方式对本发明作进一步的详细说明。
[0044] 请参考图1和图2,图1为本发明实施例所提供的机器人D-H表示法参考坐标系的示 意图；图2为本发明实施例所提供的运动关节角求解方法的流程图。
[0045] 本发明提高的一种基于折弯机机械手的运动关节角求解方法，主要包括如下步 骤：
[0046] S1、获得基座至执行末端的全部位姿变换矩阵M1;
[0047] S2、根据所述位姿变换矩阵Mi建立各个关节的位姿方程Wi;
[0048] S3、根据所述Wi得到各个关节的位姿矩阵的微变换dWi;
[0049] S4、根据所述dWi得到各个关节位姿矩阵的微分平移变量di与微分旋转变量心；
[0050] 其中：di = dxi+dyj+dzk; 5i = 5xi+5yj+5zk;
[0051 ] (i，j，k)为沿坐标系x，y，z轴方向的单位方向向量；
[0052 ] S5、根据所述5X、和心判断各个关节分别朝向x，y，z轴的哪一方向旋转。
[0053]本发明可以先设定折弯机机械手末端在折弯开始点时处于折弯坐标系下的位姿 值Wq(xq，yo，zo，rao，rbQ，rcQ);再设定折弯机机械手末端在折弯跟随过程当中处于折弯坐标 系下的实时位置姿态值W(x，y，z，ra，rb，rc)，其中：x，y，z为设定坐标系下的沿各方向的直 线距离，ra，rb，rc为绕坐标系旋转的各个角度值。
[0054]根据齐次变换矩阵D-H坐标法，对机器人建立如说明书附图1所示的坐标系，它是 以6个关节轴为原点建立的坐标系。依照D-H表示法原理，可以得到相邻坐标系间的变换矩 阵，分别记为:11屬，1? 4^6。当然，根据实际情况，关节轴的个数可以根据实际需要而 定，并不限于本文所述。
[0055]基于说明书附图1所示，步骤Sl;Mi为第一连杆对基座标的位姿变换矩阵;M2为第二 连杆对第一连杆的位姿变换矩阵，M3为第三连杆对第二连杆的位姿变换矩阵，M4为第四连杆 对第三连杆的位姿变换矩阵，M 5为第五连杆对第四连杆的位姿变换矩阵，M6为第六连杆对第 五连杆的位姿变换矩阵。
[0056] 然后进行步骤S2;根据所述位姿变换矩阵姐建立各个关节的位姿方程I，并根据所 述1得到各个关节的位姿矩阵的微变换dWi(步骤S3)，而后在步骤S4中，求解各个关节位姿 矩阵的微分平移变量cU与微分旋转变量5 1;将微分平移变量cU与微分旋转变量~分别表示 为如下形式；
[0057] di = dxi+dyj+dzk；8i = 8xi+8yj+8zk；
[0058] (i，j，k)为沿坐标系x，y，z轴方向的单位方向向量；
[0059] 通过cU能够得知各个关节角度的平移量，而通过\获得各个关节角度的旋转量。
[0060] 通过步骤S5、根据5x、5y和^判断各个关节分别朝向x，y，z轴的哪一方向旋转。这样 便能够在获知所需旋转角度的前提下，还能够明确得知关节角度的旋转方向，从而方便地 选取符合要求的关节角度，一定程度上节省了时间，简化了计算过程。
[0061] 针对各个关节的位姿方程I以及各个关节的位姿矩阵的微变换dWi的计算过程，本 发明给出如下【具体实施方式】。
[0062] 为了得到折弯机机械手各个关节位姿方程％~％，可以利用矩阵W6左乘MrT1，如下 所示：
[0063] ffi=ff=MiXM2X---Mi；
[0064] Wi-^ffiXMr1；
[0065] I-2=1-jMi-1-1;
[0066] Wi-3=ffi-2XMiV1；
[0067] i
[0068] Wi-n=Wi-n+1 X Mi-柯―1;其中，n = 1、2、……i -1;
[0069] \
[0070] ffi=ff2XM2_1〇
[0071] 本发明中，i = 6,则有：
[0072] ff6=ff=MiXM2X---M6；
[0073] W5=W6 X M6-1=Mi X M2 X M3 X M4 X Ms;
[0074] ff4=ff5XM5_1=MiXM2XM3XM4；
[0075] ff3=ff4XM4_1=MiXM2XM3；
[0076] ff2=ff3XM3_1=MiXM2
[0077] Wi=W2 XM2-1=Mi。
[0078]通过以上6个公式可以建立各关节的坐标微分平移和微分旋转矩阵关系表达式；
[0079] dffi=ffiX [-I+T(dx,dy,dz) XR(f ,d9)],
[0080] 其中，可设A = -I+T(dx,dy ,dz) XR(f,d9)，则dWi = A XWi，
[0081] A = -I+T(dX，dy，dz)XR(f，d0)为折弯机机械手各个关节位姿矩阵的微变换。
[0082] 1为与矩阵1'((11，(17，(12)\1?(:^(10)同阶的单位矩阵，1?(;^(10)表示绕某一关节轴线 f旋转变化d0的矩阵;T(dx，dy，dz)表示沿x，y，z平移的距离的变换矩阵。
[0083] 针对 A = -I+T(dx，dy，dz)XR(f，d9);
[0084] 微分平移矩阵为;叙办y;
[0085] 其中，E3为三阶单位矩阵，淺=(办,办,￡/2) 7；： I I 祕恭邏1
[0086] % i 二g&\ i j
[0087] 设定 fxd0 = 5x，fydQ = 5y，fzdQ = 5z; (产. Hj; I § -：& % I
[0089]其中：Cs=i丨龜 0 ~~：&!!, ^^dxydy,dz)T〇 1% & § I
[0090]采用上述方式得到 di = dxi+dy j+dzk; 5i = 5xi+5y j+5zk;
[0091 ]且di为微分平移变量与\为微分旋转变量；
[0092] (i，j，k)为沿坐标系x，y，z轴方向的单位方向向量；
[0093 ]可以根据所述5X、和^判断各个关节分别朝向x，y，z轴的哪一方向旋转的具体步 骤包括：
[0094]当Sx>〇时，则该关节绕X轴方向旋转；
[0095]当Sx<〇时，则该关节绕X轴相反方向旋转；
[0096]当Sy>〇时，则该关节绕Y轴方向旋转；
[0097]当Sy<〇时，则该关节绕Y轴相反方向旋转；
[0098]当^>〇时，则该关节绕Z轴方向旋转；
[0099]当心<〇时，则该关节绕Z轴相反方向旋转。
[0100]通过上述可知，根据折弯机机械手板材折弯的变化过程，各个关节位姿矩阵Wi，w2， ^4，15和16变化前后的关系可以表示为12=1"1 +八\1"，其中12为关节微分变化后的矩 阵，Wml为关节微分变化前的矩阵；
[0101] A XWm = Wm2_Wmi=Wmi X [_I+T(dx，dy，dz) XR(f，d9)]，并上述根据 7X叙办心）求得各个关节所对应的A值。
[0102]根据上文所述，可以得到各关节对应的坐标变换和角度微分变换：相应的值的正 负情况，从而判断每一关节在微分运动变化的时候的旋转方向，继而求得各关节角度变化 程度(在微分旋转之前的角度的基础上减还是加一定的角度），这样子在解各运动关节多重 解的过程中可以有助于判断和选取理想的最优解，减少了排除所有逆解都要比较选取的情 况，提尚效率。
[0103] 利用di = dxi+dyj+dzk和5i = 5xi+5yj+5z相应值的正负情况，从而判断每一关节在微 分运动变化的时候的旋转方向，从而继而求得各关节角度变化程度(在微分旋转之前的角 度的基础上减还是加一定的角度），这样便能够在解各运动关节多重解的过程中可以有助 于判断和选取理想的最优解。
[0104]需要说明的是，在本说明书中，诸如第一和第二之类的关系术语仅仅用来将一个 实体或步骤与另外几个实体或步骤区分开来，而不一定要求或者暗示这些实体或步骤之间 存在任何这种实际的关系或者顺序。
[0105]以上对本发明所提供的基于折弯机机械手的运动关节角求解方法进行了详细介 绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只 是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员 来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修 饰也落入本发明权利要求的保护范围内。
【主权项】
1. 一种基于折弯机机械手的运动关节角求解方法，其特征在于，包括如下步骤： 51、 获得基座至执行末端的全部位姿变换矩阵M1; 52、 根据所述位姿变换矩阵姐建立各个关节的位姿方程W1; 53、 根据所述^得到各个关节的位姿矩阵的微变换dW1; 54、 根据所述dWi得到各个关节位姿矩阵的微分平移变量cU与微分旋转变量δ?; 其中：di = dxi+dyj+dzk;5i = 5xi+5yj+5zk; (1」_，1〇为沿坐标系1，7,2轴方向的单位方向向量； 55、 根据所述53^、5\和52判断各个关节相对于x，y，z轴的旋转方向。2. 根据权利要求1所述的运动关节角求解方法，其特征在于，所述步骤Sl中，根据D-H法 获得所述位姿变换矩阵M1。3. 根据权利要求1所述的运动关节角求解方法，其特征在于，所述步骤S2中，建立所述 各个关节的位姿方程^的方式具体包括： Wi=W=Mi X M2 X - --Mi; Wi-I=WiXMr1; Wi-2=Wi-i X Mi-1-S ffi-3 = ffi-2XMi-2_1； .V Λ>· Wi-η=Wi-η+ι X Mi-η+Γ1;其中，η=1、2、……i-1； :#.· ：Λ· Α· Wi=W2XM2'4. 根据权利要求1至3任意一项所述的运动关节角求解方法，其特征在于，所述步骤S3 中，所述各个关节的位姿矩阵的微变换dWi具体为:dWi=W i X [-I+T(dx，dy，dz) X R(f，d0)]，5. 根据权利要求4所述的运动关节角求解方法，其特征在于，所述步骤S4中，根据所述 dWi得到各个关节位姿矩阵的微分平移变量di与微分旋转变量31的具体步骤包括：6. 根据权利要求5所述的运动关节角求解方法，其特征在于，所述步骤S5中，根据所述 δχ、Sy和52判断各个关节分别朝向X，y，Z轴的哪一方向旋转的具体步骤包括： 当δχ > O时，则该关节绕X轴方向旋转； i3x<0时，则该关节绕X轴相反方向旋转； 当δγ > O时，则该关节绕Y轴方向旋转； 当3y<0时，则该关节绕Y轴相反方向旋转； 当32> O时，则该关节绕Z轴方向旋转； 当32<〇时，则该关节绕Z轴相反方向旋转。
【文档编号】B25J9/16GK106003055SQ201610653080
【公开日】2016年10月12日
【申请日】2016年8月10日
【发明人】陆啸天, 王钦若
【申请人】广东工业大学