(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211279780.9 (22)申请日 2022.10.19 (71)申请人 赵彬 地址 110075 辽宁省沈阳市铁西区北三西 路卫工街 万科城市阳光7号楼1-8-1 (72)发明人 赵彬　吴成东　吴兴茂　孙若怀　 (74)专利代理 机构 沈阳科苑专利商标代理有限 公司 210 02 专利代理师 许宗富 (51)Int.Cl. B25J 9/16(2006.01) B25J 19/00(2006.01) (54)发明名称 一种基于动量观测器的协作机器人碰撞检 测方法 (57)摘要 本发明属于机器人安全保护以及自动化控 制领域， 具体 说是一种基于动量观测器的协作机 器人碰撞检测方法。 包括以下步骤： 构建协作机 器人运动学模 型； 通过牛顿下山法对协作机器人 进行逆运动学行程约束优化； 基于协作机器人动 力学模型和机器人广义动量构建外力矩 阵观测 器模型； 对外力矩阵观测器模型进行优化， 并基 于优化后的外力矩 阵观测器模型判断协作机器 人是否发生碰撞。 本发明对动量观测器进行了补 偿设计， 提高了基于动量观测器碰撞检测方法的 碰撞检测速度。 权利要求书2页 说明书7页 附图1页 CN 115488895 A 2022.12.20 CN 115488895 A 1.一种基于动量观测器的协作机器人碰撞检测方法， 其特 征在于， 包括以下步骤： 构建协作机器人运动学模型； 通过广义逆和牛顿下山法对协作机器人进行逆运动问题进行迭代求解得到机器人的 目标关节值； 基于协作机器人动力学模型和目标关节值构建外力矩阵观测器模型； 对外力矩阵观测器模型进行优化， 并基于优化后的外力矩阵观测器模型判断协作机器 人是否发生碰撞。 2.根据权利要求1所述的一种基于动量观测器的协作机器人碰撞检测方法， 其特征在 于， 所述通过广义逆和牛顿下山法对协作机器人进行逆运动问题进行迭代求解得到机器人 的目标关节值， 具体为： 其中， J为雅克比矩阵， θ为协作机器人关节角度， λ为牛顿下山法迭代因子， f为表示逆 运动学的非线性方程。 3.根据权利要求1所述的一种基于动量观测器的协作机器人碰撞检测方法， 其特征在 于， 所述外力矩阵观测器模型为： 其中， 为动量p的估计值， τm为关节驱动力矩， r为观测值， 为机器人哥氏矩 阵， θ为协作机器人关节角度， 为协作机器人关节角速度， g( θ )为关节重力矩阵， K1、 K2均为 观测器增益矩阵。 4.根据权利要求1所述的一种基于动量观测器的协作机器人碰撞检测方法， 其特征在 于， 优化后的外力矩阵观测器模型r(t)为： 其中， K1、 K2、 K3、 Kt均为观测器增益矩阵， K3为PD控制系数， Kt为测速反馈控制系数， r为 观测值， τm为关节驱动力矩， 为机器人哥氏矩阵， θ为协作机器人关节角度， 为协 作机器人关节角速度， g( θ )为关节重力矩阵， p(t)为机器人广义动量方程。 5.根据权利要求1所述的一种基于动量观测器的协作机器人碰撞检测方法， 其特征在 于， 所述基于优化后的外力矩阵观测器模型判断协作机器人 是否发生碰撞， 具体为：权　利　要　求　书 1/2 页 2 CN 115488895 A 2给动量观测器的观测值设定一个阈值τth， 当检测到的外部力矩值大于设定的阈值时， 则判定为发生外 部碰撞， 否则判定为未发生碰撞。 6.一种基于动量观测器的协作机器人碰撞检测系统， 其特 征在于， 包括以下步骤： 模型构建模块， 用于构建协作机器人运动学模型； 行程约束优化模块， 用于通过广义逆和牛顿下山法对协作机器人进行逆运动问题进行 迭代求解得到机器人的目标关节值； 外力矩阵观测器模型构建模块， 用于基于协作机器人动力学模型和目标关节值构建外 力矩阵观测器模型； 碰撞检测模块， 用于对外力矩阵观测器模型进行优化， 并基于优化后的外力矩阵观测 器模型判断协作机器人 是否发生碰撞。 7.一种基于动量观测器的协作机器人碰撞检测系统， 其特征在于， 包括存储器和处理 器； 所述存储器， 用于存储计算机程序； 所述处理器， 用于 当执行所述计算机程序时， 实现如 权利要求1 ‑5任一项所述的一种基于动量观测器的协作机器人碰撞检测方法。 8.一种计算机可读存储介质， 其特征在于， 所述存储介质上存储有计算机程序， 当所述 计算机程序被处理器执行时， 实现如权利要求1 ‑5任一项所述的一种基于动量观测器的协 作机器人碰撞检测方法。权　利　要　求　书 2/2 页 3 CN 115488895 A 3