(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210699792.0 (22)申请日 2022.06.20 (71)申请人 山东大学 地址 250061 山东省济南市历下区经十路 17923号 (72)发明人 周乐来　艾嘉琦　李贻斌　田新诚　 宋锐　 (74)专利代理 机构 济南圣达知识产权代理有限 公司 372 21 专利代理师 任欢 (51)Int.Cl. B25J 9/16(2006.01) B25J 18/00(2006.01) (54)发明名称 一种近距人机协作环境下机械臂规避人体 方法及系统 (57)摘要 本发明提出了一种近距人机协作环境下机 械臂规避人体方法及系统， 属于人机协作下的机 械路径规划技术领域。 包括： 通过IMU惯性传感器 获取人体上半身关节点位置信息； 将得到的人体 关节点位置信息转换成机械臂工作空间下的等 效障碍物； 基于等效障碍物， 在为机械臂给定简 单任务的基础上， 使用改进的RRT ‑CONNECT方法 进行避碰工作路径生成； 对生 成的工作路径进行 优化， 并驱动机械臂进行移动。 本发明通过IMU获 取人体的状态， 将该状态转换到机械臂工作空间 之中， 再用改进的RRT ‑CONNECT的方法扩展得到 机械臂避碰路径， 从而避免操作员与机械臂在近 距离协同工作时的碰撞。 权利要求书2页 说明书7页 附图4页 CN 114918923 A 2022.08.19 CN 114918923 A 1.一种近距人机协作环境下机 械臂规避人体方法， 其特 征在于， 包括： 获取人体上半身关节点 位置信息； 将人体关节点 位置信息转换成机 械臂工作空间下的等效障碍物； 基于等效障碍物， 在为机 械臂给定简单任务的基础上， 进行避碰 工作路径生成； 对生成的工作路径进行优化， 并驱动机 械臂进行移动。 2.根据权利要求1所述的近距人机协作环境下机械臂规避人体方法， 其特征在于， 所述 获取人体上半身关节点 位置信息， 具体为： 获取胸部下侧、 左臂上半关节、 左臂下半关节、 右臂上半关节、 右臂下半关节在人体基 坐标系{B}下的位置信息 。 3.根据权利要求2所述的近距人机协作环境下机械臂规避人体方法， 其特征在于， 将人 体关节点 位置信息转换成机 械臂工作空间下的等效障碍物， 具体过程是： 将人体基坐标系{B}下的人体关节点位置信息转换至机械臂工作空间笛卡尔坐标系 {R}下， 然后将人体转 化为机械臂工作空间笛卡尔坐标系{R}下的障碍物。 4.根据权利要求1所述的近距人机协作环境下机械臂规避人体方法， 其特征在于， 进行 避碰工作路径生成， 具体过程是： Step1： 在机 械臂关节空间下 给定初始位置点qinit与目标位置点qgoal； Step2： 更新任务 起点为当前机 械臂的关节位置q ′init， 然后更新障碍物信息； Step3： 以初始关节位置q ′init与目标关节位置qgoal为根节点， 利用新的节点扩展方法扩 展新节点， 生成快速扩展随机树Tre eA与TreeB； Step4： 进行碰撞检测， 若判定为未发生碰撞则执 行Step5， 否则， 执 行Step3； Step5： 判断正反向树是否连接， 若判断结果 为已连接， 则执 行Step6， 否则， 执 行Step3； Step6： 对正逆随机树进行整合得到 完整的避碰路径。 5.根据权利要求4所述的近距人机协作环境下机械臂规避人体方法， 其特征在于， 所述 利用新节点扩展方法扩展新节点， 具体内容 为： 在关节空间中随机取一个关节位置向量qsample， 遍历随机树中的所有关节向量， 找到距 离qsample最近的关节向量qnear， 则 其中， ||q ‑qsample||为q与qsample之间的欧氏距离， qnear到qsample的方向为随机树新节点 qnew的扩展方向， Q_L ist为关节位置向量的集 合； 在qnew的扩展长度上， 将六维向量qnew看作两个三维向量， 设定三维空间下扩展步长为 L， 则第一个三维变量的扩展坐标为： qnew[0]＝qnear[0]+L*cos( θz)cos( θy) qnew[1]＝qnear[1]+L*cos( θz)sin( θy) qnew[2]＝qnear[2]+L*si n( θz) 其中： θy＝arctan( (qsample[1]‑qnear[1])/(qsample[0]‑qnear[0]))， θz＝arctan( (qsample[2]‑qnear[2])/[(qsample[0]‑qnear[0])2+(qsample[1]‑qnear[1])2]1/2) 第二个三维变量的扩展坐标为：权　利　要　求　书 1/2 页 2 CN 114918923 A 2qnew[3]＝qnear[3]+L*cos( θz2)cos( θy2) qnew[4]＝qnear[4]+L*cos( θz2)sin( θy2) qnew[5]＝qnear[5]+L*si n( θz2) 其中： θy2＝arctan( (qsample[4]‑qnear[4])/(qsample[3]‑qnear[3]))， θz2＝arctan( (qsample[5]‑qnear[5])/[(qsample[3]‑qnear[3])2+(qsample[4]‑qnear[4])2]1/2)。 6.根据权利要求4所述的近距人机协作环境下机械臂规避人体方法， 其特征在于， 所述 碰撞检测中， 通过正运动学计算得到六轴机械臂的各个关节点坐标paxis(0,1,2,3,4,5,6,7)， 得到 机械臂的近似碰撞模型为： Obs_Axis6＝[ Paxis6x Paxis6y Paxis6z Paxis7x Paxis7y Paxis7z r] Obs_Axis5＝[ Paxis5x Paxis5y Paxis5z Paxis6x Paxis6y Paxis6z r] Obs_Axis4 ＝[Paxis4x Paxis4y Paxis4z Paxis5x Paxis5y Paxis5z r]。 7.根据权利要求1所述的近距人机协作环境下机械臂规避人体方法， 其特征在于， 对生 成的工作路径进行优化， 并驱动机 械臂进行移动， 具体过程是： 检测当前机械臂关节 空间位置与 下一个移动的关节 空间位置是否会发生碰撞， 若发生 碰撞， 则重新 规划路径； 若不发生碰撞， 则驱动机 械臂到达该关节空间位置 。 8.一种近距人机协作环境下机 械臂规避人体系统， 其特 征在于， 包括： 惯性传感器、 处 理器和机 械臂； 所述惯性传感器用于获取人体上半身关节点位置信 息， 并将人体上半身关节点位置信 息传输给处 理器； 所述处理器用于将人体关节点位置信 息转换成机械臂工作空间下的等效 障碍物， 并基 于等效障碍物， 在为机械臂给定简单任务的基础上， 进 行避碰工作路径生成和优化， 并将优 化后的路径指令传输给机 械臂； 所述机械臂用于 接收处理器发出的路径指令， 并根据路径指令进行移动。 9.计算机可读存储介质， 其上存储有程序， 其特征在于， 该程序被处理器执行时实现如 权利要求1 ‑7任一项所述的近距人机协作环境下机 械臂规避人体方法中的步骤。 10.电子设备， 包括存储器、 处理器及存储在存储器上并可在处理器上运行的程序， 其 特征在于， 所述处理器执行所述程序时实现如权利要求1 ‑7任一项所述的近距人机协作环 境下机械臂规避人体方法中的步骤。权　利　要　求　书 2/2 页 3 CN 114918923 A 3