(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210418696.4 (22)申请日 2022.04.20 (71)申请人 浙江大学 地址 310058 浙江省杭州市西湖区余杭塘 路866号 (72)发明人 黄方昊　董思旻　沈翀　陈正　 聂勇　唐建中　 (74)专利代理 机构 杭州求是专利事务所有限公 司 33200 专利代理师 林超 (51)Int.Cl. B25J 9/16(2006.01) B25J 17/00(2006.01) (54)发明名称 一种基于阻尼最小二乘的多关节机械臂避 障逆运动学方法 (57)摘要 本发明公开了一种基于阻尼最小二乘的多 关节机械臂避障逆运动学方法。 包括： 根据多关 节机械臂的结构建立D ‑H关节坐标系， 然后求出 多关节机械臂的正运动学的坐标转换关系和雅 可比矩阵； 再根据障碍物与多关节机械臂中各连 杆之间的相对位置关系以及多关节机械臂的正 运动学的坐标转换关系， 计算障碍物对多关节机 械臂中各连杆的总虚拟斥力； 最后基于阻尼最小 二乘法， 根据雅可比矩阵和虚拟斥力建立多关节 机械臂逆运动学优化目标函数， 采用数值迭代方 法求解多关节机械臂逆运动学优化函数， 获得多 关节机械臂的末端位姿对应的各关节角度。 本发 明将避障规划与机械臂逆运动学求解过程融合， 简化了避障方法的流程， 并保证了此类机械臂避 障的实时性。 权利要求书2页 说明书6页 附图4页 CN 114589701 A 2022.06.07 CN 114589701 A 1.一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法， 其特征在于， 包括以下 步骤： 第一步： 根据多关节机械臂的结构建立D ‑H关节坐标系， 基于D ‑H关节坐标系求出多关 节机械臂的正 运动学的坐标转换关系和雅可比矩阵； 第二步： 根据障碍物与多关节机械臂中各连杆之间的相对位置关系以及多关节机械臂 的正运动学的坐标转换关系， 计算障碍物对多关节机 械臂中各 连杆的总虚拟斥力； 第三步： 基于阻尼最小二乘法， 根据雅可比矩阵和虚拟斥力建立多关节机械臂逆运动 学优化目标函数， 采用数值迭代方法求解多关节机械臂逆运动学优化函数， 获得多关节机 械臂的末端位姿对应的各关节角度。 2.根据权利要求1所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法， 其特征在于， 所述第二 步具体为： 2.1)根据障碍物与多关节机械臂中连杆之间的相对位置关系以及多关节机械臂的正 运动学的坐标转换关系， 计算各个障碍物对多关节机械臂中每一连杆的虚拟斥力的作用方 向以及对应虚拟斥力势能； 2.2)将各个障碍物对多关节机械臂中当前连杆的虚拟斥力的作用方向和对应虚拟斥 力势能相乘后， 获得各个障碍物对多关节机械臂中当前连杆的虚拟斥力， 接着对各个虚拟 斥力进行求和， 获得障碍物对多关节机 械臂中当前 连杆的总虚拟斥力； 2.3)重复2.1) ‑2.2)， 计算并获得障碍物对多关节机 械臂中剩余连 杆的总虚拟斥力。 3.根据权利要求2所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法， 其特征在于， 所述各个障碍物对多关节机械臂中每一连杆的虚拟斥力的作用方向由障碍物 与连杆之间的相对位置确定， 具体地： 针对障碍物k对连杆i的虚拟斥力的作用方向， 障碍物k的几何中心O与连杆i的两个端 点PA、 PB之间的距离OPA、 OPB以及连杆i的两个端点PA、 PB相连后构成轴线PAPB之间的关系分为 以下三种情况： 当障碍物k的几何中心O在连杆i的轴线PAPB上的投影位于PB一侧的延长线上时， 障碍物 k到连杆i的空间距离 dik满足dik＝|OPB|‑R‑rk， R为连杆i端部所在圆的半径， rk为障碍物的 最大直径的一半， 虚拟斥力的作用方向为 当障碍物k的几何中心O在连杆i的轴线PAPB上的投影位于PA一侧的延长线上时， 障碍物 k到连杆i的空间距离dik满足dik＝|OPA|‑R‑rk， 虚拟斥力的作用方向为 当障碍物k的几何中心O在连杆i的轴线PAPB上的投影位于轴线PAPB之间时， 障碍物k到连 杆i的空间距离dik满足dik＝|OPv|‑R‑rk， 虚拟斥力的作用方向为 4.根据权利要求2所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法， 其特征在于， 所述各个障碍物对多关节机械臂中当前连杆的虚拟斥力势能的计算公式如 下： 权　利　要　求　书 1/2 页 2 CN 114589701 A 2式中， Eik表示障碍物k作用于连杆i的虚拟斥力势能， kr为障碍物斥力系数， d0为障碍物 的影响距离， dik为障碍物k到连 杆i的空间距离 。 5.根据权利要求1所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法， 其特征在于， 所述第三 步中， 多关节机 械臂逆运动学优化目标函数的公式为： 其中， J是多关节机械臂的雅可比矩阵， Fforce(Q)表示虚拟斥力对关节的累计作用力， fforce(q)是虚拟斥力对连杆的作用分量， q表示多关节机械臂 的 各关节角度， λ2表示正则项系数， x表示多关节机械臂的末端位置， ‖ ‖表示取二范数操作， min表示取最小值操作， γ是与迭代次数p有关的斥力衰退系数。 6.根据权利要求1所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学方法， 其特征在于， 所述第三步的采用数值迭代法求解逆运动学优化 目标函数 的求解过程中， 通 过调整各 关节角度q获得更新的末端位姿根据更新的末端位姿与预设末端位姿0TE计算位姿 残差， 使位姿残差最小后， 获得最终的各关节角度q， 求 解过程中的公式如下： ep＝tr(0TE‑К(qp)) Hp＝(JpTJp+λ2I)‑1 γ＝σp 其中， Hp是第p次迭代时的Hessian矩阵， Jp是第p次迭代时的雅可比矩阵， ep是第p次迭 代时的位姿残差， λ2是正则项系数， tr()表示将齐次矩阵转换为向量形式的操作， γ 是与迭 代次数p有关的斥力衰退系数； К()表示正运动学关系， σ 是单步斥力衰退系数， T表示转置 操作， qp、 qp+1分别表示第p、 p+1次迭代时的各关节角度。 7.根据权利要求1 ‑6任一所述的一种基于阻尼最小二乘的多关节机械臂避障逆运动学 方法， 其特 征在于， 所述多关节机 械臂的相邻连 杆之间采用万向节进行 连接。权　利　要　求　书 2/2 页 3 CN 114589701 A 3