(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210792476.8 (22)申请日 2022.07.05 (71)申请人 哈尔滨工业大 学 地址 150001 黑龙江省哈尔滨市南岗区西 大直街92号 (72)发明人 刘伊威　崔士鹏　 (74)专利代理 机构 哈尔滨华夏松花江知识产权 代理有限公司 23213 专利代理师 岳昕 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于平滑切换的机械臂碰撞响应控制 方法 (57)摘要 一种基于平滑切换的机械臂碰撞响应控制 方法， 涉及碰撞响应领域。 本发明是为了解决目 前的机械臂碰撞响应方法还无法实现控制状态 的平滑切换， 进而导致机械臂作业安全性低的问 题。 本发明包括： 建立机械臂动力学回归模型； 设 计机械臂的阈函数β(τext)， 并根据阈函数β (τext)获取机械臂平滑切换函数ω； 根据平滑切 换函数获取机械臂关节控制力矩； 将机械臂关节 控制力矩代入机械臂动力学回归模 型中， 获取机 械臂碰撞的平滑切换方式： 若机械臂没有受到外 力则阈函数β(τext)＜0， 平滑切换函数ω＝1， 机械臂则在位置控制模式下工作， 若机械臂受到 外力作用则阈函数β(τext)＞0， 平滑切换函数 ω＝0， 机械臂则在柔顺控制模式下工作。 本发明 用于实现机 械臂碰撞响应 。 权利要求书4页 说明书6页 附图1页 CN 115194764 A 2022.10.18 CN 115194764 A 1.一种基于平滑切换的机械臂碰撞响应控制方法， 其特征在于： 所述方法具体包括以 下步骤： 步骤一、 建立机 械臂动力学回归 模型； 步骤二、 设计机械臂的阈函数β( τext)， 并根据阈函数β( τext)获取机械臂平滑切换函数 ω； 步骤三、 根据步骤二设计的平 滑切换函数获取机 械臂关节控制力矩； 步骤四、 将步骤三获得的机械臂关节控制力矩代入步骤一建立的机械臂动力学回归模 型中， 获取机 械臂碰撞的平 滑切换方式： 若机械臂没有受到外力则阈函数β( τext)＜0， 平滑切换函数ω＝1， 机械臂则在位置控 制模式下工作， 若机械臂受到外力作用则阈函数β( τext)＞0， 平滑切换函数ω＝0， 机械臂则 在柔顺控制模式下工作； 所述位置控制模式为根据机械臂运动位置进行控制， 机械臂运动位置收敛于期望位 置； 所述柔顺控制模块 为根据机械臂外力矩对机 械臂进行控制。 2.根据权利要求1所述的一种基于平滑切换的机械臂碰撞响应控制方法， 其特征在于： 所述步骤一中的机 械臂动力学回归 模型， 如下式： 其中， q， 分别为机械臂关节位置、 速度、 加速度； M(q)， D， g(q)分别为机械臂的惯 量矩阵、 粘性摩擦矩阵、 重力向量； 为机械臂的科氏力和向心力矩阵； 为机 械臂关于 的回归矩阵； p为机械臂动力学参数向量； τm， τext分别为机械臂关节控制力矩和 所受外力矩。 3.根据权利要求2所述的一种基于平滑切换的机械臂碰撞响应控制方法， 其特征在于： 所述步骤二中的机 械臂的阈函数β( τext)， 如下： 其中， Ri为机械臂第i个关节的阈值， 是人为设定的， 取值范 围通常为0.3～0.5， i∈[1, n]是机械臂关节的标号， n 是机械臂关节的总数量， τext,i为τext的第i个分量， 即机械臂第i个 关节所受力矩。 4.根据权利要求3所述的一种基于平滑切换的机械臂碰撞响应控制方法， 其特征在于： 所述步骤二中的机 械臂平滑切换函数， 如下式： 其中， κ 为切换函数； 其中， 当τext的任一分量大于对应预设关节阈值， 阈函数β( τext)的值大于零， 切换函数 ω＝0； 当τext的所有分量都小于对应的预设关节阈值， 阈函数β( τext)的值小于零， 切换函数 ω＝1， 因此ω在0和1之间的机 械臂切换 是平滑的。权　利　要　求　书 1/4 页 2 CN 115194764 A 25.根据权利要求4所述的一种基于平滑切换的机械臂碰撞响应控制方法， 其特征在于： 所述步骤三中的根据步骤二设计的平滑切换函数获取机械臂关节控制力矩， 包括以下步 骤： 步骤三一、 根据步骤二获得的平 滑切换函数获取机 械臂参考速度向量； 步骤三二、 利用步骤三一获得的机械臂参考速度向量和机械臂关节速度获取机械臂滑 动向量； 步骤三三、 利用步骤三一获得的机械臂参考速度向量和步骤三二获得的机械臂滑动向 量获取机 械臂关节控制力矩。 6.根据权利要求5所述的一种基于平滑切换的机械臂碰撞响应控制方法， 其特征在于： 所述步骤三一中的根据步骤二获得的平 滑切换函数获取机 械臂参考速度向量， 如下： 其中， qd为机械臂期望跟踪位置， eq＝q‑qd为机械臂位置误差， λq为位置误差系数， 为大 于0的常数， 是为机械臂期望跟踪速度。 7.根据权利要求6所述的一种基于平滑切换的机械臂碰撞响应控制方法， 其特征在于： 所述步骤三二中的利用步骤三一获得的机械臂参考速度向量获取机械臂滑动向量， 如下 式： 8.根据权利要求7所述的一种基于平滑切换的机械臂碰撞响应控制方法， 其特征在于： 所述步骤三三中的利用步骤三一获得 的机械臂参考速度向量和 步骤三二中的机械臂滑动 向量获取机 械臂关节控制力矩： 其中， K为正定对角矩阵； 为机械臂关于 的回归矩阵； 为机械臂动力学 参数向量p的估计向量， 为机械臂参考加速度向量； 所述 通过如下自适应律进行 更新： 其中， Г为 正定矩阵， ()T是转置。 9.根据权利要求8所述的一种基于平滑切换的机械臂碰撞响应控制方法， 其特征在于： 所述步骤四中的将步骤三获得 的机械臂关节控制力矩代入步骤一建立的机械臂动力学回 归模型中， 获取机 械臂碰撞的平 滑切换方式， 具体为： 步骤四一、 将步骤三获得的械臂关节控制力矩代入步骤一建立的机械臂动力学回归模 型， 获得改写后的机 械臂动力学模型： 首先， 将公式(6)带入到公式(1)中获得如下公式： 然后， 将公式(8)改写， 获得改写后的机 械臂动力学模型： 权　利　要　求　书 2/4 页 3 CN 115194764 A 3