一种七轴工业机械臂的逆动学求解方法
【技术领域】
[0001] 本发明设及设及一种走轴工业机械臂的逆动学求解方法，尤其设及走轴工业机械 臂的建模方法。
【背景技术】
[0002] 在现有技术中走轴工业机械臂的逆动学求解方法有如下两种：
[0003] 现有技术的方法如二次规划法，几何分析法W及位姿分解方法等，
[0004] 现有技术的方法（一）：二次规划法在祖迪，吴镇巧的《一种冗余机器人逆运动学 求解得有效方法》中描述的较为详细。该方法重点在首先利用雅可比矩阵W及梯度投影法 求出一组关节角，在二次计算时固定其中一个角度来求解剩余关节角；
[0005] 对于方法（二）所述；几何分析法在崔泽，韩增军的《基于自运动的仿人走自由度 机械臂逆解算法》中描述的较为详细。该方法重点在自运动构型的基础上，W0坐标系为其 他坐标系的基系，根据同一向量在不同坐标系之间的投影特性与关节角之间存在着的对应 关系，实现对走自由度机械臂逆运动学的求解；
[0006] 对于方法（=)所述；位姿分解方法在潘博，付宜利的《面向冗余机器人实时控制 的逆运动学求解有效方法》所述比较详细。该方法针对具有球形手腕的7自由度机械臂结 构特点，采用位姿分离的方法，将7自由度问题降为4自由度位置冗余问题W及3自由度姿 态问题，在此基础之上再利用梯度投影法进行逆运动学的求解。
[0007] 在现有的=种方案中，上述现有技术中的的缺点为：
[0008] 方法（一）所述方法中梯度投影法中，自运动放大系数k选取为常数，第一需要经 过反复的计算机仿真通常才能确定一个k值，同时固定k值，缺少机器人运动优化能力，常 导致机器人关节角震荡。同时二次计算虽然精度高但是运算量大，实时性差；
[0009] 方法（二）虽然利用自运动轨迹和几何方法可W求解逆运动学，但是该方法只适 用于两杆之间存在轴线平行于连杆的转动副的情况，即存在横滚转动副的走自由度机器人 才能够如此简化，不具有很强的通用性；
[0010] 方法（=)利用位姿分解能简化逆运动学的求解，同方法二一样也限制于机械臂 的构型，不具有很好的通用性。
【发明内容】
[0011] 本发明的实施例提供了一种走轴工业机械臂的逆动学求解方法，本发明提供了如 下方案，包括：
[0012] 步骤1、将虚拟机械臂末端执行器任务设定的起始点和终止点中间对应的位置离 散化，获取离散点；
[0013] 步骤2、建立第一关节角和第走姿态的坐标原点的平面方程，根据每两个离散点得 到第一关节角的变化趋势，根据所述变化趋势得到离散点集合中每一点对应的第一关节角 的值；
[0014] 其中，第一平面坐标系为基座底部坐标点、第零姿态的坐标原点W及第一或第二 姿态的坐标原点构成的平面；
[0015] 步骤3、采用每个离散点的末端位姿矩阵作为初始末端位姿矩阵T，根据变换矩阵 ;:'厂=;i7;7';节7巧;7' = 获取第五或第六姿态的坐标原点，再确定Sr位姿矩阵，根据 ^位姿矩阵求出第=姿态的坐标原点；根据第五或第六姿态的坐标原点和第=姿态的坐 标原点的距离方程式，求出第二关节角；
[0016] 步骤4、建立第四关节角、第四关节角的第一特征角W及第四关节角的第二特征角 之间的条件关系，根据所述条件关系求解第四关节角；其中，
[0017] 所述第四关节角的第一特征角为W第S姿态的坐标原点为顶点，与第四姿态的坐 标原点及第五或第六姿态坐标原点形成的夹角；
[0018] 所述第四关节角的第二特征角W第=姿态的坐标原点为顶点，与第一或第二姿态 的坐标原点及第五或第六姿态坐标原点形成的夹角；
[001引步骤5、由正运动学方程片7；巧;C7；，rr=(片)-吁的左右两边的第四列对应相 等得到的方程得到关于第一关节角、第二关节角、第四关节角W及第=关节角的两个等式， 从而求出第=关节角；
[0020] 步骤6、确定巧位姿矩阵，根据辟位姿矩阵得到第四姿态的坐标原点坐标，再求出 第四姿态的坐标原点和第走姿态的坐标原点之间的空间距离，再由第四姿态的坐标原点、 第五或第六姿态的坐标原点和第走姿态的坐标原点构成的=角形中，求出第六关节角的特 征角，根据第六关节角的特征角求解第六关节角；其中，第六关节角的特征角为W第五或第 六姿态的坐标原点为顶点，与第四姿态的坐标原点、第走姿态的坐标原点形成的夹角；
[0021] 步骤7、根据第六关节角的求解结果，结合末端坐标的关系，分情况求解第五关节 角和第走关节角。
[0022] 根据本发明的上述方法，在规划第一关节角之前，包括：
[0023]W虚拟机械臂末端执行器的W基座的底部点0为基坐标点，再另外依次确定走个 位姿状态对应的空间坐标系的坐标原点，其中第一姿态和第二姿态公用同一个坐标原点为 第一或第二姿态的坐标原点，第五姿态和第六姿态公用同一个坐标原点为第五或第六姿态 的坐标原点；
[0024] 根据本发明的上述方法，所述建立第四关节角、第四关节角的第一特征角W及第 四关节角的第二特征角之间的条件关系，包括；第四关节角的第一特征角与第四关节轴的 变换关系；当第四关节角的第一特征角变小时，该变化量是由于第四关节轴向上俯仰造成， 同理当第四关节角的第一特征角变大时，该变化量是由于第四关节轴向下俯仰造成，根据 所述变换关系建立第四关节角、第四关节角的第一特征角W及第四关节角的第二特征角之 间的条件关系。
[00巧]根据本发明的上述方法，包括；在建立第一关节角和第走姿态的坐标原点的平面 方程之前，
[0026] 旋转第一关节角，将第走姿态的坐标原点旋转至第一平面坐标系内，使第一关节 角和第走姿态的坐标原点处于同一平面。
[0027] 根据本发明的上述方法，包括：
[0028] 所述第四关节角的第一特征角通过W第S姿态的坐标原点为顶点，与第四姿态的 坐标原点及第五或第六姿态坐标原点形成的直角=角形中的=角函数求解；
[0029] 所述第四关节角的第二特征角通过第=姿态的坐标原点为顶点，与第一或第 二姿态的坐标原点及第五或第六姿态坐标原点形成的=角形的=角函数求解。
[0030] 根据本发明的上述方法，根据第六关节角的特征角求解第六关节角，包括；第六关 节角对应为两个解，具体为第六关节角的特征角的补角的正负值。
[0031] 根据本发明的上述方法，根据第六关节角的求解结果，结合末端坐标的关系，分情 况求解第五关节角和第走关节角，包括：
[0032] 当第六关节角不为为零时，由末端坐标的关系求出第五姿态角，再由末端姿态的 关系求出第走姿态角；
[0033] 当第六关节角为零时，机械臂处于奇异位置，不能唯一确定第五关节角和第走关 节角，令第五关节角等于零，则可求出第走关节角，得到第五关节角和第走关节角的一组特 解。
[0034] 由上述本发明的实施例提供的技术方案可W看出，本发明实施例根据每两个离散 点得到第一关节角的变化趋势得到离散点集合中每一点对应的第一关节角的值；采用每个 离散点的末端位姿矩阵，获取第五或第六姿态的坐标原点，再求出第=姿态的坐标原点，求 出第二关节角；根据第四关节角、第四关节角的第一特征角W及第四关节角的第二特征角 之间的条件关系求解第四关节角；由关于第一关节角、第二关节角、第四关节角的方程W及 第=关节角的两个等式，求出第=关节角；获取第四姿态的坐标原点坐标，求出第六关节角 的特征角，根据第六关节角的特征角求解