CN109895101B - 一种关节型机械臂逆运动学数值唯一解求取方法 - Google Patents

Abstract

本发明一种关节型机械臂逆运动学数值唯一解求取方法属于现代智能制造技术领域，涉及工业机器人领域一种肩关节朝前偏置的关节型六自由度机械臂的逆运动学数值唯一解求解方法。该方法按照改进的DH参数法建立机械臂关节坐标系,确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵。对于给定的末端坐标系O₆的位姿矩阵，采用一种改进的牛顿迭代法——Levenberg‑Marquardt迭代算法。利用雅可比矩阵J计算关节坐标系逆运动学解，求得一组对应于位姿矩阵的满足精度要求的六个关节旋转角度值θ_i。该方法克服了传统牛顿迭代法对雅可比矩阵J必须满秩的要求，建模方法更加简单明了，行之有效。具有求解精度高、求解速度快、求解过程更加简单易行的特点。

Description

一种关节型机械臂逆运动学数值唯一解求取方法

技术领域

本发明属于现代智能制造技术领域，涉及工业机器人领域一种肩关节朝前偏置的关节型六自由度机械臂的逆运动学数值唯一解求解方法。

背景技术

关节型机械臂的逆向运动学问题是在给定机械臂末端执行器坐标系相对于基坐标系的位置和姿态，以及所有机械臂关节几何参数的情况下，求取所有机械臂关节转动的角度值，是正向运动学的逆过程。正向运动学根据机械臂的结构几何参数可以得到前后相邻关节坐标系之间的齐次坐标变换矩阵T，也即两关节之间的位姿矩阵；若已知每个关节转动角度θ,通过将各关节齐次变换矩阵T依次连续右乘即可得到机械臂末端执行器坐标系的位姿矩阵，所得结果唯一。而逆向运动学的求解则相对复杂，且可能具有无解或多解情况，比如末端执行器坐标系在奇异点处无解，而反三角函数的周期性导致机械臂关节旋转角度解析解理论上的多解问题。

大多数关节型六自由度机械臂的腕部都是满足Pieper准则的，即腕部相邻的三个关节旋转轴相交于一点，则腕部相邻的三个关节是解耦的。目前，机器人领域通用的坐标系建模方法多是Denavit-Hartenberg参数法,简称DH参数法，即一种为机械臂关节链中的每一个杆件建立坐标系的矩阵方法，该方法描述了相邻关节之间的坐标方向和几何参数，直观明了。例如，陶茂生等人公开的“用于六自由度的工业机器人的逆运动学求解方法”，专利号CN105573143A，需要根据求出的六个关节旋转轴转动角度对应的八组结果，解离上一关节空间所处位置对应的各个关节旋转轴角度差值范数总和最小的解。但是所求范数未能再次带入计算，只是作为在众多逆解中选择较优解的判断依据，其计算精度与计算速度不能满足现代生产的要求。朱齐丹等人公开的“一类六自由度机械臂运动学逆解的快速简便求法”，专利号CN103942427A，采用欧拉角变换矩阵进行求解，没有避免该方法的万向节锁问题。此外，田国会等人公开的“智能空间下的服务机器人逆运动学求解方法和装置”，以及吕亚辉等公开的“一种基于粒子群优化算法的机器人逆运动学求解方法”，采用了遗传算法和粒子群算法等智能算法进行逆运动学求解，程序复杂需要高性能的计算配置，且存在稳定性不足的问题。

发明内容

本发明目的是针对现有机械臂的逆运动学求解存在的缺陷及不足：通用求解方法坐标系建模方法单一，求解过程复杂难懂，存在多解，且计算时间长求解速度慢。发明一种关节型机械臂逆运动学数值唯一解的求取方法，用于快速获取肩关节朝前偏置的关节型六自由度机械臂的唯一逆运动学解。方法按照改进的DH参数法建立机械臂关节坐标系,确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵，并将齐次坐标变换矩阵依次右乘，得到末端坐标系O₆对于基坐标系的位姿矩阵；整理位姿矩阵得到各关节转角θ_i、RPY转角r_x,r_y,r_z和平移距离p_x,p_y,p_z之间的关系式。再将关系式分别对关节转角θ_i求偏导数，得到6×6的雅可比矩阵J。最后，对于给定的末端坐标系O₆的位姿矩阵，本方法采用一种改进的牛顿迭代法迭代法——Levenberg-Marquardt迭代算法，利用雅可比矩阵J计算关节坐标系逆运动学解，求得一组对应于位姿矩阵的满足精度要求的六个关节旋转角度值θ_i，克服了传统牛顿迭代法对雅可比矩阵J必须满秩的要求。本方法行之有效，过程简单明了。

本发明采用的技术方案是一种关节型机械臂逆运动学数值唯一解的求取方法，该方法按照改进的DH参数法建立机械臂关节坐标系,确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵，并将齐次坐标变换矩阵依次右乘，得到末端坐标系O₆对于基坐标系的位姿矩阵；整理位姿矩阵得到各关节转角θ_i,i＝1,2,3,4,5,6与RPY转角r_x,r_y,r_z和平移距离p_x,p_y,p_z之间的关系式；再将关系式分别对关节转角θ_i求偏导数，得到6×6的雅可比矩阵J；最后，对于给定的末端坐标系O₆的位姿矩阵，采用一种改进的牛顿迭代法—Levenberg-Marquardt迭代算法，利用雅可比矩阵J计算关节坐标系逆运动学解，求得一组对应于位姿矩阵的满足精度要求的六个关节旋转角度值θ_i；方法的具体步骤如下：

步骤一，构建关节型机械臂及坐标系。

关节型机械臂由基座A、末端执行器G、5个连杆B、C、D、E、F和6个旋转关节1、2、3、4、5、6组成；基于改进的DH参数法建立机械臂各关节坐标系，坐标系包括：基坐标系O₀，机械臂六个旋转关节对应坐标系O₁～O₆以及机械臂末端执行器的坐标系O₆；各关节坐标系具体为：关节i的z_i轴与关节i的轴线在同一直线上，而x_i轴则在关节i和i+1轴线的公共法线上，其方向从i指向i+1；当两关节轴线相交时，x_i-1的方向与两矢量的叉积z_i-1×z_i同轴、同向或反向，x_i-1的方向总是沿着公共法线从轴i-1指向轴i；当两轴x_i-1和x_i平行且同向时，第i个转动关节的θ_i为零；y_i轴由右手直角坐标系规则确定，其中，i＝1,2,3,4,5,6；将第一个关节坐标系的初始位置设置在机械臂的基座上与基坐标系{O₀:x₀,y₀,z₀}重合，基坐标系始终保持不变。

步骤二，建立机械臂关节相邻坐标系的齐次坐标变换矩阵。

根据机械臂相邻关节之间的4个结构几何参数：连杆夹角θ_i、连杆扭角α_i、连杆长度a_i、连杆距离d_i计算相邻坐标系的齐次坐标变换矩阵^i-1T_i,i＝1,2,...6；各几何参数的定义：相邻关节i和i+1之间的连杆转角θ_i为x_i轴与x_i-1轴之间的夹角，绕z_i轴从x_i-1轴到x_i轴，符合右手规则时为正，对于转动关节，θ_i为变量；连杆扭角α_i为z_i轴与z_i+1轴之间的夹角，绕x_i轴从z_i轴到z_i+1轴，符合右手规则时为正，当两关节轴线平行时，α_i＝0，当两关节轴线垂直时，α_i＝-90°或90°；连杆长度a_i为z_i轴与z_i+1轴的公垂线长度，沿x_i轴方向测量，当两关节轴线平行时，a_i＝l_i，l_i为连杆的长度，当两关节轴线垂直时，a_i＝0；相邻关节之间的连杆距离d_i为x_i轴与x_i-1轴之间的距离，在z_i轴上测量，对于转动关节，d_i为常数。

按照关节坐标系之间的齐次变换规则计算出相邻坐标系的各齐次变换矩阵^i-1T_i；机械臂的相邻关节坐标系间的齐次坐标变换矩阵^i-1T_i为：

其中，θ_i为连杆夹角、α_i为连杆扭角、a_i为连杆长度、d_i为连杆距离。

步骤三，建立机械臂逆运动学求解的齐次坐标变换矩阵。

将机械臂关节相邻坐标系的各齐次变换矩阵^i-1T_i依次右乘得到：

其中，等式右边矩阵：⁰T₁、¹T₂、²T₃、³T₄、⁴T₅、⁵T₆分别为第一、第二、第三、第四、第五、末端坐标系相对于前一坐标系的齐次坐标变换矩阵。

等式左边矩阵⁰T₆为末端坐标系O₆相对于基坐标系O₀的齐次坐标变换矩阵；其中，n_x,n_y,n_z分别为末端坐标系{O₆:x₆,y₆,z₆}的x₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；o_x,o_y,o_z分别为末端坐标系的y₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；a_x,a_y,a_z分别为末端坐标系的z₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；p_x,p_y,p_z为末端坐标系原点O₆在基坐标系中的笛卡尔坐标。

⁰T₆还可以用RPY组合变换表示：

其中，

分别是

的简写，Trans(p_x,p_y,p_z)代表末端坐标系沿着基坐标系的x,y,z轴平移p_x,p_y,p_z距离；

表示末端坐标系先绕着基坐标系z轴旋转

角，再绕新的y轴旋转θ角，再绕新的z轴旋转φ角，即RPY转角r_x,r_y,r_z。

联立公式(2)和(3)，得到各关节转角θ_i(i＝1,2,3,4,5,6)、RPY转角r_x,r_y,r_z和平移距离p_x,p_y,p_z之间的关系式：

步骤四，建立机械臂逆运动学求解的雅可比矩阵。

根据公式(4)中的r_x,r_y,r_z,p_x,p_y,p_z与θ_i,i＝1,2,3,4,5,6之间的关系式，将f_i,i＝1,2,3,4,5,6分别对θ_i,i＝1,2,3,4,5,6求偏导数得到关节坐标系的雅可比矩阵：

步骤五，关节型机械臂逆运动学数学唯一解求取。

对于给定的机械臂末端坐标系O₆的位姿矩阵，使用经典牛顿迭代算法，计算关节坐标系逆运动学数值解时，由于雅可比矩阵J是与关节转角θ_i有关的矩阵，在某些特殊的情况下，J^TJ是奇异的，此时牛顿迭代法无解。所以，求解关节坐标系逆运动学数值解时，采用一种改进的牛顿迭代法迭代法——Levenberg-Marquardt迭代算法，LM迭代法克服了牛顿迭代法对雅可比矩阵J必须满秩的要求。

当使用LM迭代法计算给定的机械臂末端坐标系O₆的位姿矩阵T_end对应的逆运动学数值解θ_end时，记录下当前末端坐标系O₆的位姿矩阵T_cur以及对应的各关节转角θ_cur。由T_end和T_cur计算微分算子Δ：

由微分算子Δ可以得到微分运动向量D＝[dx dy dz δx δy δz]^T。

对于当前迭代点θ_cur，LM迭代法的搜索方向为：

dθ＝(J^TJ+μI)^-1J^TD (7)

其中，μ为正参数，是为了防止当J^TJ接近奇异时dθ过大。将θ_cur与dθ相加得到新的迭代点θ_new，再利用正运动学模型计算新的迭代点对应的机械臂末端坐标系位姿矩阵T_new，将其带入公式(5)和公式(6)，更新雅可比矩阵J_new和微分运动向量D_new。

为保证LM迭代法全局收敛，在每一次迭代后比较D_new和D的二范数。若||D_new||₂<||D||₂，则令

将μ_new,D_new,J_new带入下一次迭代。若||D_new||₂≥||D||₂，则令μ_new＝2μ，将μ_new,D,J带入下一次迭代。直到||D_new||₂小于迭代精度时，结束迭代过程；此时，θ_new即为关节坐标系逆运动学数值解。

本发明的有益效果是该方法采用Levenberg-Marquardt迭代算法快速求取肩关节朝前偏置的关节型六自由度机械臂的唯一逆运动学解，克服了传统牛顿迭代法对雅可比矩阵J必须满秩的要求，对该类构型的机械臂，坐标系建模方法简单，并且数值解精度高。本发明中提出的逆运动学求解过程更加行之有效，具有求解精度高、求解速度快、求解过程更加简单易行的特点。

附图说明

附图1—关节型机械臂逆运动学数值唯一解的求取方法流程图。

附图2—关节型六自由度机械臂的结构图。其中，A-基座，B-连杆1，C-连杆2，D-连杆3，E-连杆4，F-连杆5，G-末端执行器，a₂-连杆2的长度，a₃-连杆3的长度，a₄-连杆4的长度，d₄-连杆3与连杆4的距离，d₆-连杆5与末端执行器之间的距离。

附图3—关节型六自由度机械臂的结构简图及各关节坐标系。其中，1-关节1，2-关节2，3-关节3，4-关节4，5-关节5，6-关节6；各坐标系的确定方法采取改进的DH参数法，各关节均绕着该关节坐标系的z轴旋转，具体的，z_i轴沿第i+1关节的轴线，x_i沿z_i轴和z_i-1轴的公垂线，指向背离z_i-1轴的方向，y_i轴由右手直角坐标系规则确定，O_i为第i个坐标系的原点；实施例将第一个关节坐标系的初始位置设置在机械臂的基座上与基坐标系{O₀:x₀,y₀,z₀}重合，基坐标系始终保持不变。

具体实施方式

下面结合附图和技术方案详细叙述本发明的具体实施方式。

附图1为本发明关节型机械臂逆运动学数值唯一解的求取方法流程图；附图2为关节型六自由度机械臂结构图，机械臂由基座A、末端执行器G、5个连杆B、C、D、E、F和6个旋转关节1、2、3、4、5、6组成。机械臂各关节坐标系如附图3所示，方法流程的具体步骤如下：

步骤一,基于改进的DH参数法，建立机械臂各关节坐标系,如附图3所示。

步骤二，给定初始机械臂相邻关节之间的4个结构几何参数：连杆转角θ_i、连杆扭角α_i、连杆长度a_i、连杆距离d_i。本实施例中，机械臂各连杆的DH参数为：i＝1时，连杆转角为θ₁，连杆距离为0，连杆长度为0，连杆扭角为0；i＝2时，连杆转角为θ₂+90°，连杆距离为0，连杆长度为a₂，连杆扭角为90°；i＝3时，连杆转角为θ₃，连杆距离为0，连杆长度为a₃，连杆扭角为0；i＝4时，连杆转角为θ₄，连杆距离为d₄，连杆长度为a₄，连杆扭角为90°；i＝5时，连杆转角为θ₅，连杆距离为0，连杆长度为0，连杆扭角为-90°；i＝6时，连杆转角为θ₆+90°，连杆距离为d₆，连杆长度为0，连杆扭角为90°。在本实施例中，d₆包含工具长度。

基于改进的DH参数法，求出实施例中各结构的参数值如下：

a₂＝160,a₃＝790,a₄＝155,d₄＝795,d₆＝145

计算机械臂关节相邻坐标系的齐次坐标变换矩阵^i-1T_i,i＝1,2,...6；利用公式(1)机械臂的相邻关节坐标系间的齐次坐标变换矩阵^i-1T_i，本实施例中各变换矩阵如下：

步骤三，将公式(2)与(3)联立得到各关节转角θ_i,i＝1,2,3,4,5,6与RPY转角r_x,r_y,r_z和平移距离p_x,p_y,p_z之间的关系式(4)。

步骤四，根据机械臂的实际工作范围，本实施例给定一组关节转角θ₁＝30°,θ₂＝-20°,θ₃＝40°,θ₄＝-40°,θ₅＝20°,θ₆＝50°通过正运动学计算得到末端坐标系相对于基坐标系的位姿矩阵T_end，

令当前末端坐标系O₆的位姿矩阵T_cur，

以及对应的各关节转角θ_cur＝[0 0 0 0 0 0]^T。通过公式(5)计算此时迭代点θ_cur对应的雅可比矩阵J，

步骤五，设正参数μ＝0.1，将μ,J,T_cur,θ_cur,T_end带入LM迭代算法，得到机械臂关节坐标系逆运动学数值解θ_end，

θ₁＝30.000000000007°,θ₂＝-19.999999999997°,θ₃＝39.999999999986°,θ₄＝-39.999999999747°,θ₅＝20.000000000004°,θ₆＝49.999999999744°

由此得到唯一的一组机械臂关节旋转角度表达式，计算结果误差小于1×10^-7°，利用已确定的表达式再进行几组正逆运算，结果同样满足误差小于1×10^-7°的结论。

本发明对于关节型六自由度机械臂逆运动学数值唯一解求取方法，对此类构型的机械臂关节坐标系建模方法简单易行，能够迅速获得唯一解，并且结果误差小于1×10^-7°。

Claims (1)

1.一种关节型机械臂逆运动学数值唯一解的求取方法，其特征是，该方法按照改进的DH参数法建立机械臂关节坐标系,确定机械臂相邻关节之间的4个结构几何参数，计算相邻两坐标系的齐次坐标变换矩阵，并将齐次坐标变换矩阵依次右乘，得到末端坐标系O₆对于基坐标系的位姿矩阵；整理位姿矩阵得到各关节转角θ_i,i＝1,2,3,4,5,6、RPY转角r_x,r_y,r_z和平移距离p_x,p_y,p_z之间的关系式；再将关系式分别对关节转角θ_i求偏导数，得到6×6的雅可比矩阵J；最后，对于给定的末端坐标系O₆的位姿矩阵，采用一种改进的牛顿迭代法—Levenberg-Marquardt迭代算法，利用雅可比矩阵J计算关节坐标系逆运动学解，求得一组对应于位姿矩阵的满足精度要求的六个关节旋转角度值θ_i；方法的具体步骤如下：

步骤一，构建关节型机械臂及坐标系：

关节型机械臂由基座(A)、末端执行器(G)、5个连杆(B、C、D、E、F)以及6个旋转关节(1、2、3、4、5、6)组成；基于改进的DH参数法建立机械臂各关节坐标系，坐标系包括：基坐标系O₀，机械臂六个旋转关节对应坐标系O₁～O₆以及机械臂末端执行器的坐标系O₆；各关节坐标系具体为：关节i的z_i轴与关节i的轴线在同一直线上，而x_i轴则在关节i和i+1轴线的公共法线上，其方向从i指向i+1；当两关节轴线相交时，x_i-1的方向与两矢量的叉积z_i-1×z_i同向或反向，x_i-1的方向总是沿着公共法线从轴i-1指向轴i；当两轴x_i-1和x_i平行且同向时，第i个转动关节的θ_i为零；y_i轴由右手直角坐标系规则确定，其中，i＝1,2,3,4,5,6；将第一个关节坐标系的初始位置设置在机械臂的基座上与基坐标系{O₀:x₀,y₀,z₀}重合，基坐标系始终保持不变；

步骤二，建立机械臂关节相邻坐标系的齐次坐标变换矩阵：

根据机械臂相邻关节之间的4个结构几何参数：连杆夹角θ_i、连杆扭角α_i、连杆长度a_i、连杆距离d_i计算相邻坐标系的齐次坐标变换矩阵^i-1T_i,i＝1,2,...6；各几何参数的定义：相邻关节i和i+1之间的连杆转角θ_i为x_i轴与x_i-1轴之间的夹角，绕z_i轴从x_i-1轴到x_i轴，符合右手规则时为正，对于转动关节，θ_i为变量；连杆扭角α_i为z_i轴与z_i+1轴之间的夹角，绕x_i轴从z_i轴到z_i+1轴，符合右手规则时为正，当两关节轴线平行时，α_i＝0，当两关节轴线垂直时，α_i＝-90°或90°；连杆长度a_i为z_i轴与z_i+1轴的公垂线长度，沿x_i轴方向测量，当两关节轴线平行时，a_i＝l_i，l_i为连杆的长度，当两关节轴线垂直时，a_i＝0；相邻关节之间的连杆距离d_i为x_i轴与x_i-1轴之间的距离，在z_i轴上测量，对于转动关节，d_i为常数；

按照关节坐标系之间的齐次变换规则计算出相邻坐标系的各齐次变换矩阵^i-1T_i：

其中，θ_i为连杆夹角、α_i为连杆扭角、a_i为连杆长度、d_i为连杆距离；

步骤三，建立机械臂逆运动学求解的齐次坐标变换矩阵：

将机械臂关节相邻坐标系的各齐次变换矩阵^i-1T_i依次右乘得到：

其中，等式右边矩阵：⁰T₁、¹T₂、²T₃、³T₄、⁴T₅、⁵T₆分别为第一、第二、第三、第四、第五、末端坐标系相对于前一坐标系的齐次坐标变换矩阵；

等式左边矩阵⁰T₆为末端坐标系O₆相对于基坐标系O₀的齐次坐标变换矩阵；其中，n_x,n_y,n_z分别为末端坐标系{O₆:x₆,y₆,z₆}的x₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；o_x,o_y,o_z分别为末端坐标系的y₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；a_x,a_y,a_z分别为末端坐标系的z₆轴与基坐标系的x₀,y₀,z₀轴的夹角余弦值；p_x,p_y,p_z为末端坐标系原点O₆在基坐标系中的笛卡尔坐标；

使用RPY组合变换表示⁰T₆：

其中，

分别是

的简写，Trans(p_x,p_y,p_z)代表末端坐标系沿着基坐标系的x,y,z轴平移p_x,p_y,p_z距离；

表示末端坐标系先绕着基坐标系z轴旋转

角，再绕新的y轴旋转θ角，再绕新的z轴旋转φ角，即RPY转角r_x,r_y,r_z；

计算各关节转角θ_i,i＝1,2,3,4,5,6、RPY转角r_x,r_y,r_z和平移距离p_x,p_y,p_z之间的关系：

步骤四，建立机械臂逆运动学求解的雅可比矩阵：

根据r_x,r_y,r_z,p_x,p_y,p_z与θ_i,i＝1,2,3,4,5,6之间的关系式，将f_i,i＝1,2,3,4,5,6分别对θ_i,i＝1,2,3,4,5,6求偏导数得到关节坐标系的雅可比矩阵J：

步骤五，关节型机械臂逆运动学数学唯一解求取：

对于给定的机械臂末端坐标系O₆的位姿矩阵，使用经典牛顿迭代算法，计算关节坐标系逆运动学数值解时，由于雅可比矩阵J是与关节转角θ_i有关的矩阵，在某些特殊的情况下，J^TJ是奇异的，此时牛顿迭代法无解，所以，求解关节坐标系逆运动学数值解时，采用一种改进的牛顿迭代法——Levenberg-Marquardt迭代算法，LM迭代法克服了牛顿迭代法对雅可比矩阵J必须满秩的要求；

当使用LM迭代法计算给定的机械臂末端坐标系O₆的位姿矩阵T_end对应的逆运动学数值解θ_end时，记录下当前末端坐标系O₆的位姿矩阵T_cur以及对应的各关节转角θ_cur，由T_end和T_cur计算微分算子Δ：

由微分算子Δ得到微分运动向量D：

D＝[dx dy dz δx δy δz]^T；

对于当前迭代点θ_cur，LM迭代法的搜索方向dθ：

dθ＝(J^TJ+μI)^-1J^TD (7)

其中，μ为正参数，是为了防止当J^TJ接近奇异时dθ过大，将θ_cur与dθ相加得到新的迭代点θ_new，再利用正运动学模型计算新的迭代点对应的机械臂末端坐标系位姿矩阵T_new，并更新雅可比矩阵J_new和微分运动向量D_new；

为保证LM迭代法全局收敛，在每一次迭代后比较D_new和D的二范数，若||D_new||₂＜||D||₂，则令

将μ_new,D_new,J_new带入下一次迭代，若||D_new||₂≥||D||₂，则令μ_new＝2μ，将μ_new,D,J带入下一次迭代，直到||D_new||₂小于迭代精度时，迭代过程结束；此时，θ_new即为关节坐标系逆运动学数值解。

Images