(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210478391.2 (22)申请日 2022.05.05 (65)同一申请的已公布的文献号 申请公布号 CN 114571469 A (43)申请公布日 2022.06.03 (73)专利权人 北京科技大 学 地址 100083 北京市海淀区学院路3 0号 (72)发明人 于欣波　贺威　王靖元　张爽　 杨宽　吴逸帆　闵高晨　孔令欢　 (74)专利代理 机构 北京市广友专利事务所有限 责任公司 1 1237 专利代理师 张仲波　邓琳 (51)Int.Cl. B25J 9/16(2006.01) (56)对比文件 CN 108829137 A,2018.1 1.16 CN 102902269 A,2013.01.3 0 US 201612 9588 A1,2016.0 5.12 US 20142 29006 A1,2014.08.14 US 20182 25113 A1,2018.08.09 CN 113146610 A,2021.07.23CN 106078742 A,2016.1 1.09 CN 10409720 5 A,2014.10.15 CN 107139171 A,2017.09.08 Xinbo Yu.Human-Robot Variable Impedance Sk ills Transfer Learn ing Based on Dynamic Movement Primitives. 《Human- Robot Variable Impedance Sk ills Transfer Learning Based o n Dynamic Movement Primitives》 .IE EE Xplore,2022, Milton Cesar Paes Santos.A N ovel Null-Space-Based UA V Trajectory Track ing Controller With Collision Avoidance. 《A Novel Nul l-Space-Based UA V Trajectory Tracking Controller With Collision Avoidance》 .IE EE,2017, 黄海丰.协作机 器人智能控制与人机交 互研 究综述. 《协作机 器人智能控制与人机交 互研究 综述》 .工程科 学学报,202 2,第44卷(第4期), 许志远.七轴冗余机 器人避障运动规划研 究. 《七轴冗余机 器人避障运动规划研究》 .中国 优秀硕士学位 论文全文数据库 信息科技 辑, 2020,I140 -63. 审查员 苏展 (54)发明名称 一种机械臂零空间实时避障控制方法及系 统 (57)摘要 本发明公开了一种机械臂零空间实时避障 控制方法及系统， 所述方法由感知任务、 规划任 务与控制任务组成， 包括： 采集障碍物信息， 获取 障碍物的三维坐标， 完成所述感知任务； 基于人 工势场法， 使用虚拟排斥力计算函数生成机械臂 与所处工作空间内障碍物之间的排斥力， 进而根 据虚拟阻抗控制算法生成机械臂对当前工作空 间内障碍物进行躲避的避障轨迹， 完成所述规划 任务； 基于预设性能函数根据需求对机械臂的瞬 态性能进行约束， 设计机械臂轨迹跟踪控制程序， 得到机械臂跟踪避障轨迹的关节力矩， 并实 时控制机械臂按照预设性能跟踪避障轨迹， 完成 所述控制任务。 本发明能够实现机械臂在协作工 作的同时躲避障碍物， 提高协作机器人系统的安 全性与高效性。 权利要求书7页 说明书13页 附图3页 CN 114571469 B 2022.07.26 CN 114571469 B 1.一种机械臂零空间实时避障控制方法， 其特征在于， 所述方法基于包括机械臂、 双目 立体摄像头、 服务器的机械臂 零空间实时避障控制系统， 所述方法由感知 任务、 规划任务与 控制任务组成， 包括以下步骤： 采集障碍物信息， 获取障碍物的三维坐标， 完成所述感知任务； 所述感知任务包括： 得到障碍物表面与潜在碰撞点最近点的三维坐标， 其表现形式如 下： 其中， 分别表示所述方法探测到上述 障碍物表面与潜在碰撞点最近点相对于 机械臂基座坐标系的 轴坐标， 轴坐标和 轴坐标； 且机械臂上的潜在碰撞点的三维坐 标的表现形式如下： 潜在碰撞点的三维坐标通过机械臂前向运动学计算得到， 机械臂上的潜在碰撞点和障 碍物之间的距离表现形式如下： 至此， 完成所述感知任务； 基于人工势场法， 使用虚拟排斥力计算函数生成机械臂与所处工作空间内障碍物之间 的排斥力， 进而根据虚拟阻抗控制算法生成机械臂对当前工作空间内障碍物进行躲避的避 障轨迹， 完成所述 规划任务； 所述规划任务包括： 根据障碍物与机械臂之间的相对距离， 实时生成避障轨迹； 所述规 划任务由虚拟排斥力函数与虚拟阻抗控制算法组成； 基于人工势场法， 规划任务中的虚拟排斥力函数 表现形式如下： 其中， 是包围机械臂的一层虚拟保护外壳， 为虚拟排斥力函数的输出， 为 虚拟排斥力函数的输入， 是设置的最大虚拟排斥力， 为虚拟排斥力函数的 激活距离， 也是进入 的边界距离， 为设置的最小安全距离， 为虚拟排斥力系 数， 此系数的表现形式如下： 所述虚拟排斥力函数的作用 在于， 在三维空间中， 在物理意义上直观地作用于机械臂 上； 所述虚拟阻抗控制算法的输入是虚拟力矩， 输出是机械臂各个关节对应的运动轨迹，权　利　要　求　书 1/7 页 2 CN 114571469 B 2所述虚拟阻抗控制算法的表现形式如下： 其中 是设置的虚拟阻抗控制参数， 其作用为， 根据障碍物的特点， 生成匹 配障碍物特点的避障轨迹； 分别为机械臂执行避障任务时的关节期望轨迹的 角加速度， 角速度， 角度； 分别为机械臂的关节角真实的加速度、 速度、 角度； 为关节虚拟排斥力矩， 用于作为虚拟阻抗控制算法的输入； 虚拟排斥力为三维任务空间中的力， 虚拟排斥力函数 ， 而虚拟排斥力矩为 机械臂关节空间中的力， 三 维任务空间的虚拟排斥力与机械臂关节空间中的虚拟排斥力矩 之间的表现形式如下： 其中 代表机械臂潜在碰撞点对应的雅可比矩阵； 虚拟排斥力矩结合虚拟阻抗控制算法， 得到下述实时避障参 考轨迹的加速度： 通过积分 得到参考轨迹的速度与角度， 从而得到避障的参 考轨迹； 基于预设性能函数根据需求对机械臂的瞬态性能进行约束， 设计机械臂轨迹跟踪控制 程序， 得到机械臂跟踪避障轨迹的关节力矩， 并实时控制 机械臂按照预设性能跟踪避障轨 迹， 完成所述控制任务； 所述控制任务包括： 基于反馈控制、 前馈补偿、 误差约束， 计算出机械臂各个关节的控 制力矩， 以控制 机械臂跟踪上述得到的期望位置和期望姿态， 使机械臂在执行任务的同时 实现实时避障； 其控制力矩的控制器表现形式如下： 其中， 为对机械臂输入的控制力矩； 为控制增益矩阵； 为机械臂的当前速度， 为虚拟控制量； 径向基函数神经网络； 为神经网络的开关函数； 为鲁棒项； 为机械臂的动力学 参数的上界； 为鲁棒项的系数； 鲁棒项可将逼近域为半全局的神经网络扩展为作用域为全局的带有鲁棒项的神经网 络， 当神经网络输入越过神经网络逼近域 时， 鲁棒项起作用； 其中 为预设性能函数误差约 束系数， 其设计方法如下： 为了提高机械臂躲避障碍物的瞬态性能， 首先设计如下的期 望预 设性能函数： 权　利　要　求　书 2/7 页 3 CN 114571469 B 3