(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211057223.2 (22)申请日 2022.08.30 (71)申请人 亿嘉和科技股份有限公司 地址 210012 江苏省南京市雨 花台区安德 门大街57号5幢1楼至 3楼、 8楼至12 楼 (72)发明人 王昊　陈张　 (74)专利代理 机构 南京瑞弘专利商标事务所 (普通合伙) 32249 专利代理师 梁天彦 (51)Int.Cl. B25J 3/00(2006.01) B25J 9/16(2006.01) B25J 13/00(2006.01) (54)发明名称 一种基于异构主手的带电作业机器人主从 遥操方法及系统 (57)摘要 一种基于异构主手的带电作业机器人主从 遥操方法及系统， 带电作业机器人为主从异构型 机器人， 包括以下步骤： 首先分别获取异构主从 手运动信息获； 然后进行异构主从手运动信息映 射， 以基座坐标系为基础， 将主手的运动映射转 换为从手的运动， 映射时将遥操解耦， 分解为主 从手末端的位置遥操和主从手姿态遥操； 最后进 行运动状态 监测， 检测从手运动是否靠近奇异位 或极限位置， 若靠近奇异位则告警并停止运动。 本发明方法不受主手构型限制， 适用于异构主手 完成遥操任务， 适应度高； 通过解耦方式优化异 构主从手之间的运动映射， 保证异构主从手的遥 操简单、 易懂、 高效； 本发明在遥操过程中加入了 运动状态监测， 提高了异构主从手遥操的安全 性。 权利要求书3页 说明书9页 附图1页 CN 115284247 A 2022.11.04 CN 115284247 A 1.一种基于异构主手的带电作业机器人主从遥操方法， 其特征是带电作业机器人为主 从异构型机器人， 包括以下步骤： 1)异构主从手运动信息获取： 通过主手遥操 从手， 获取主从手运动信息， 包括主手的末 端位姿和末端运动速度， 以及从手的末端位姿和各关节角角度信息， 主手及从手的末端位 姿包括末端位置和旋转信息； 2)异构主从手运动信息映射： 基于主从手运动信息， 以基座坐标系为基础， 将主手的运 动映射转换为从手的运动， 映射时将遥操解耦， 分解为主从手末端的位置遥操和主从手姿 态遥操； 3)运动状态监测： 当主手的运动通过映射转换为从手的运动后， 将运动信息下发给从 手， 并实时检测从手运动是否 靠近奇异位或极限位置， 若靠 近奇异位则停止运动。 2.根据权利要求1所述的一种基于异构主手的带电作业机器人主从遥操方法， 其特征 是异构主从手运动信息的获取 具体如下： 1.1)获取主手运动信息， 主手运动信息包括主手末端在主手基座坐标系Baset下的位 置信息P(P x,Py,Pz),主手末端姿态在其基座坐标系Baset下的姿态信息R(Rx,Ry,Rz),和主 手末端在基座坐标系Baset下的运动速度信息V(Vx,Vy,Vz)， 首先对P(Px,Py,Pz)和R(Rx, Ry,Rz)进行处理， 通过RPY角转四元数公式， 将获取的姿态信息R(Rx,Ry,Rz)转换为四元素Q (Qx,Qy,Qz,Qw),然后将四元素Q(Qx,Qy,Qz,Qw)和位置信息P(P x,Py,Pz)组合生成主手位姿 T； 1.2)使用二阶差分， 处理主手末端速度信息v(vx,vy,vz)， 以获得主手运动速度信息V (Vtx,Vty,Vtz)， 具体公式为： 其中vel_buff＝(position*3 ‑4*pos_hist1+pos_hist2)/0.002， pos_hist2为一个周 期前机器人末端位置， pos_hist1为当前周期机器人末端位置， inp_v el3为两个周期前机器 人末端移动速度的二阶差 分系数， inp_v el2为一个周期前机器人末端移动速度的二阶差 分 系数， inp_vel1为当前周期机器人末端移动速度的二阶差分系数， out_vel3为两个周期前 机器人末端移动速度， out_vel2为一个周期前机器人末端移动速度， out_vel1为当前周期 前机器人末端移动速度， 采样频率为20Hz， 通过二阶差 分处理速度信息去除速度突变， 获取 稳定的速度信息； 1.3)获取从手的运动信息， 从手运动信息包括从手末端在其基座坐标系Baser下的位 置信息P’(Px,Py,Pz),从手末端姿态在其基座坐标系Baser下的姿态信息R ’(Rx,Ry,Rz),从 手当前关节角信息Theta(q1,q2,q3,q4,q5,q6)。 3.根据权利要求1所述的一种基于异构主手的带电作业机器人主从遥操方法， 其特征 是主从手运动映射中， 主从手位置 遥操步骤如下： 2 .1 .1)记录主手末端遥操起点在其基座坐标系Baset下的位置信息Pti (initialTouc hX， initialTouc hY， initialTouc hZ)； 2 .1 .2)记录从手末端遥操起点在其基座坐标系Baser下的位置信息Prn (initialRobotX,i nitialRobotY,i nitialRobotZ)； 2.1.3)获取主手末端在其基座 坐标系Baset下的当前位置信息Ptn(Ptnx,Ptny,Ptnz)；权　利　要　求　书 1/3 页 2 CN 115284247 A 22.1.4)将Ptn与Pti作差， 获取主手末端位置移动信息Tchange(TchangeX,TchangeY, TchangeY)， 即Tc hangeX＝Ptnx‑Ptix,TchangeY＝Ptny ‑Ptiy,TchangeZ＝Ptnz ‑Ptiz； 2.1.5)获取从手当前位置信息Prn(Prnx,Prny,Prnz)； 2.1.6)比较主手各方向速度值V(Vtx,Vty,Vtz)， 取速度值最大的方向为当前运动方向 D， 则在方向D上的从手目标位置Prt(Prtx,Prty,Prtz)为主手位置移动信息TchangeD加上 从手D方向的末端遥操起点位置initialRobotD， 其他方向的从手目标位置为当前从手位 置； 2.1.7)取当前从手末端姿态Rrn(Rrnx,Rrny.Rrnz)为从手目标末端姿态Rrt (Rrtx.Rrty,Rrt z)， 即Rrtx ＝Rrnx,Rrty＝Rrny,Rrt z＝Rrnz， Rrt+Prt即为从手当前周期目 标位姿Tt； 2.1.8)使用解析法求解从手目标关节角Thetat,求得八组机器人关节角Thetat1 … Theta8,每组关节角包 含六个关节角值(q1,q2,q3,q4,q5,q6)； 2.1.9)选解， 使用最小距离法选出目标解， 读取当前从手关节角Thetac(qc1,qc2,qc3, qc4,qc5,qc6),将八组解一一与当前关节角Thetac求差值， 取与当前关节角Thetac差值最 小的解为目标解； 主从手姿态遥操与主从手位置 遥操类似， 具体步骤如下: 2.2.1)首先将主从手末端对齐， 即将主手末端Zt坐标系朝向和从手末端Zr坐标系朝向 重合； 2.2.2)记录主手末端姿态变化Rchange(RchangeX,RchangeY,RchangeZ),记Rti(Rtix, Rtiy,Rtiz)为主手初始姿态， Rtn(Rtnx,Rtny,Rtnz)为主手当前姿态， 则RchangeX＝Rtnx ‑ Rtix,RchangeY＝Rtny ‑Rtiy,RchangeZ＝Rtnz ‑Rtiz； 2.2.3)主手末端姿态变化Rchange(RchangeX,RchangeY,RchangeZ)加从手初始姿态 Rri(Rrix,Rriy,Rriz)即为从手目标姿态Rr t(Rrtx,Rrty,Rrtz)； 2.2.4)求 解， 具体步骤同位置 遥操步骤2.1.8)； 2.2.5)选解， 具体步骤同位置 遥操步骤2.1.9)。 4.根据权利要求1所述的一种基于异构主手的带电作业机器人主从遥操方法， 其特征 是主从手运动状态监测具体步骤如下： 3.1)读取当前周期从手的关节角值Thetac(qc1,qc2,qc 3,qc4,qc 5,qc6)； 3.2)求解当前从手运动灵活性指标k： 式中， JN为机器人雅克比矩阵， tr为求矩阵的迹， 指标k用于评价从手当前位姿是否靠近 奇异位资， 当k(JN)→0， 当前机器人靠近奇异点， 当k(JN)→1时， 当前机器人末端远离奇异 点； 3.3)当k值小于0.1时运动性能急剧下降， 靠 近奇异位姿， 发送告警， 停止 遥操运动。 5.一种基于异构主手的带电作业机器人主从遥操系统， 其特征是包括交互子系统、 感 知子系统、 控制子系统、 计算子系统和通信子系统， 所述交互子系统包括人机交 互设备， 用于 遥操的人机交 互； 所述感知子系包括机器人传感器， 用于采集机器人作业现场信息；权　利　要　求　书 2/3 页 3 CN 115284247 A 3