(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211288913.9 (22)申请日 2022.10.20 (71)申请人 哈工大机 器人 （合肥） 国际创新研究 院 地址 230031 安徽省合肥市经济技 术开发 区宿松路396 3号智能装备 科技园C区 (72)发明人 仲军　龚锦　易永彬　韩震峰　 (74)专利代理 机构 合肥市浩智运专利代理事务 所(普通合伙) 34124 专利代理师 朱文振 (51)Int.Cl. G06F 17/11(2006.01) G06F 17/16(2006.01) (54)发明名称 一种七自由度遥操作机械臂系统运动学求 解方法及系统 (57)摘要 本发明提供一种七自由度遥操作机械臂系 统运动学求解方法及系统， 方法包括： 建立七自 由度遥操作机械臂D ‑H坐标系； 获取七自由度遥 操作机械臂的不少于2组的连杆位姿变换数据， 利用D‑H矩阵法， 以预置关系构建逻辑处理七自 由度遥操作机械臂D ‑H坐标系及连杆位姿变换数 据， 据以建立七自由度遥操作机械臂的正运动学 方程， 以表征机械臂连杆坐标关系； 实时采集七 自由度遥操作机械臂的预置机械臂角度， 将预置 机械臂角度设置为已知值； 利用矩阵反变换方法 处理机械臂连杆坐标关系以及预置机械臂角度， 据以求解得到七自由度机械臂待求角度， 以作为 七自由度遥操作机械臂运动学求解结果。 解决了 现有技术中的求解逆运动学过程中计算量大以 及求解难度大的技 术问题。 权利要求书4页 说明书10页 附图4页 CN 115525863 A 2022.12.27 CN 115525863 A 1.一种七自由度遥操作机 械臂系统运动学求 解方法， 其特 征在于， 所述方法包括： S1、 采集获取被测机械臂系统的连杆坐标数据， 据以建立七自由度遥操作机械臂D ‑H坐 标系； S2、 获取七自由度遥操作机械臂的不少于2组的连杆位姿变换数据， 利用D ‑H矩阵法， 以 预置关系构建逻辑处理所述七自由度遥操作机械臂D ‑H坐标系及所述连杆位姿变换数据， 据以建立所述七自由度遥操作机 械臂的正 运动学方程， 以表征机 械臂连杆坐标关系； S3、 实时采集所述七自由度遥操作机械臂 的预置机械臂角度， 将所述预置机械臂角度 设置为已知值； S4、 利用矩阵反变换方法， 以预置反变换逻辑处理所述机械臂连杆坐标关系以及预置 机械臂角度， 据以求解得到七自由度机械臂待求角度， 以作为七自由度遥操作机械臂运动 学求解结果。 2.根据权利要求1所述的七自由度机械臂逆运动学求解方法， 其特征在于， 所述步骤S1 包括： S11、 获取七自由度机 械臂关节角度及移动距离及方向数据； S12、 以机械臂基座为起始端建立基坐标0坐标系， 顺次编号至所述七自由度机械臂的 末端处， 据以建立7号 坐标系， 以作为所述七自由度遥操作机 械臂D‑H坐标系； S13、 根据所述七自由度遥操作机械臂D ‑H坐标系， 以预置逻辑获取机械臂坐标系变换 矩阵： 3.根据权利要求1所述的七自由度机械臂逆运动学求解方法， 其特征在于， 所述步骤S2 包括： S21、 处理所述七自由度遥操作机械臂D ‑H坐标系及所述连杆位姿变换数据， 求解得出 机械臂各个相邻两个连 杆之间的坐标系矩阵 权　利　要　求　书 1/4 页 2 CN 115525863 A 2S22、 连乘 处理所述坐标系矩阵， 据以得到机械臂末端坐标系相对于机械臂基坐标系的 变换矩阵 4.根据权利要求1所述的七自由度机械臂逆运动学求解方法， 其特征在于， 所述步骤S3 中， 在关节角度解算过程中， 使第二角度尽量少运动， 以将所述第二角度作为已知值， 并通 过所述七自由度机械臂上的传感器组采集获取所述第二角度的数值， 供代入方程以求解所 述七自由度机 械臂的第一角度、 第三角度、 第四角度、 第五角度、 第六角度及第七角度。 5.根据权利要求4所述的七自由度机械臂逆运动学求解方法， 其特征在于， 所述步骤S4 包括： S41、 获取七自由度机 械臂位姿数据， 据以计算第六关节旋转角度； S42、 根据位姿矩阵计算七自由度机 械臂的第七关节旋转角度； S43、 根据位姿矩阵计算七自由度机 械臂的第五关节旋转角度； S44、 根据位姿矩阵中的元 素等式关系求 解得到七自由度机 械臂的第三关节旋转角度。 S45、 根据位姿矩阵中的元素等式关系求解得到七自由度机械臂的第二、 第三及第四关 节的旋转角度。 6.根据权利要求5所述的七自由度机械臂逆运动学求解方法， 其特征在于， 所述步骤 S41包括： S411、 根据所述机 械臂的正 运动学得到位姿矩阵 S412、 在所述 位姿矩阵的等式两边左乘 以得到下述的第一矩阵等式： S413、 令所述第一矩阵等式两边第2行第4列元 素相等， 以得到下述第一角度求取逻辑： c1py‑s1px＝0 S414、 根据所述角度求取逻辑 求解得到第一角度 θ1的值为： Atan2(py,px)以及 θ1＝Atan2(‑py,‑px) 其中s1＝sin( θ1)， c1＝cos( θ1)。权　利　要　求　书 2/4 页 3 CN 115525863 A 3