[发明专利]一种关节型机械臂逆运动学数值唯一解求取方法

摘要

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

主权项

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)、和(B、C、D、E、F)5个连杆以及(1、2、3、4、5、6)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₆：其中，cθ,cφ,sθ,sφ,分别是cosθ,cosφ,sinθ,sinφ,的简写，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即为关节坐标系逆运动学数值解。