(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210861497.0 (22)申请日 2022.07.20 (71)申请人 燕山大学 地址 066004 河北省秦皇岛市河北 大街西 段438号 (72)发明人 巩明德　梁凯凯　陈圣光　刘文彬　 贾东飞　 (74)专利代理 机构 北京孚睿湾知识产权代理事 务所(普通 合伙) 11474 专利代理师 刘翠芹 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 可自动切换工作模式的遥操纵机器人系统 控制方法 (57)摘要 本发明涉及一种可自动切换工作模式的遥 操纵机器人系统控制方法， 其包括以下步骤： S1、 测量从动侧机器人末端与作业目标之间的距离； S2、 通过距离阀值判断从动侧机器人所处于的运 动阶段， 并计算 当前运动阶段的加权系数； S3、 计 算当前运动阶段从动侧机器人的控制量； S4、 根 据步骤S3中的从动侧机器人的控制量， 对从动侧 机器人所采 取不同的控制模式。 本发 明采用距离 传感器检测从动侧机器人与作业目标的距离， 作 为判断从动侧机器人当前运动阶段的依据， 进而 将计算得到的模式切换函数的值作为不同控制 模式的加权系数， 使从动侧机器人既可快速接近 目标， 又易满足近距离作业的精度要求， 还可 以 实现速度控制模式与位置控制模式两种工作模 式的自动切换。 权利要求书2页 说明书6页 附图4页 CN 115157256 A 2022.10.11 CN 115157256 A 1.一种可自动切换工作模式的遥操纵机器人系统控制方法， 其特征在于， 其包括以下 步骤： S1、 通过距离传感器测量所述从动侧机器人末端与 作业目标之间的距离； S2、 通过距离 阀值判断所述从动侧机器人所处于的运动阶段， 并计算当前阶段运动对 应的加权系数， 具体如下： S21、 若所述距离值dm2≤d≤dmax时， 所述从动侧机器人处于第一运动阶段， 则所述从动 侧机器人第一 运动阶段的加权系数为S(d)＝1； S22、 若所述距 离值dm1≤d≤dm2时， 所述从动侧机器 人处于第二运动阶段， 则所述从动侧 机器人第二 运动阶段的加权系数为： 式中， S(d)为模式切换函数； d为距离传感器测量出的从动侧机器人末端与作业目标之 间的距离； dm1、 dm2为距离阀值； dmax为从动侧机器人最远操作距离； S23、 若所述距离值0≤d≤dm1时， 所述从动侧机器人处于第三运动阶段， 则所述从动侧 机器人第三 运动阶段的加权系数为S(d)＝0； S3、 根据所述从动侧机器人所处的运动阶段， 计算当前运动阶段对应的从动侧机器人 的控制量， 所述控制量表达式为： 式中， Us为从动侧机器人的控制量； S为加权系数； Fm为主动侧机器人的作用力； Fs为从 动侧机器人的作用力； 为主动侧机器人角位移的无量纲值； 为从动侧机器人角速度的 无量纲值； S4、 根据步骤S3中所述控制量的改变， 所述从动侧机器人对应不同的控制模式， 具体如 下： S41、 若所述从动侧机器人的控制量为主动侧机器人角位移与从动侧机器人角速度的 无量纲偏差信号时， 所述从动侧机器人处于 速度控制模式； S42、 若所述从动侧机器人的控制量为力信号、 角位移及角速度的偏差信号加权之和 时， 所述从动侧机器人处于由速度控制模式转换为 位置控制模式的过渡阶段； S43、 若所述从动侧机器人的控制量为主、 从动侧机器人力偏差信号时， 所述从动侧机 器人处于位置控制模式。 2.根据权利要求1所述的可自动切换工作模式的遥操纵机器人系统控制方法， 其特征 在于， 当所述从动侧机器人 处于位置控制模式时， 在所述主动侧机器人的末端施加附加力， 则位于所述主动侧机器人一侧的用户能实时从所述主动侧机器人以及从动侧机器人的运 动过程中获得反馈力， 且所述反馈力表达式为： Fr＝(1‑S)[Fd+K(qsd‑qs)]； 式中， Fr为反馈力， K为增益系数， Fd为附加力， qsd为从动侧机器人期望角位移， qs为从动 侧机器人实际角位移。 3.根据权利要求1所述的可自动切换工作模式的遥操纵机器人系统控制方法， 其特征 在于， 当所述从动侧机器人 处于步骤S4 1中的速度控制模式时， 所述速度控制方法具体步骤权　利　要　求　书 1/2 页 2 CN 115157256 A 2为： S411、 操作者扳动所述主动侧机器人上的操作手柄， 通过位移传感器测量所述主动侧 机器人的关节角度qm， 并进行无量纲化处 理得到主动侧机器人角位移的无量纲值 S412、 将所述主动侧机器人角位移的无量纲值 与所述从动侧机器人角速度的无量纲 值 进行比较， 并将产生的偏差信号作为所述从动侧机器人的驱动信号， 使得所述从动侧 机器人的角速度跟随对应主动侧的关节角度， 继而实现对所述从动侧机器人速度的控制。 4.根据权利要求1所述的可自动切换工作模式的遥操纵机器人系统控制方法， 其特征 在于， 当所述从动侧机器人 处于步骤S43中的位置控制模式时， 所述位置控制方法具体步骤 为： S431、 操作者扳动所述主动侧机器人上的操作手柄， 通过力传感器测量施加至所述操 作手柄上的操作力Fh,放大处理得到所述主动侧机器人的作用力Fm； S432、 将所述主动侧机器人的作用力Fm与所述从动侧机器人的作用力Fs进行比较， 产生 的偏差信号经 过所述从动侧控制器能调节控制所述从动侧机器人运动； S433、 根据主动侧机器人的关节角度qm计算得到从动侧机器人期望角位移qsd， 且将从 动侧机器人期 望角位移qsd与从动侧机器人实际角位移qs的进行比较， 产生的偏差信号反馈 回主动侧控制器中， 实现从动侧机器人的位置控制。 5.根据权利要求1所述的可自动切换工作模式的遥操纵机器人系统控制方法， 其特征 在于， 所述距离阀值 为： dm1＝0.2d0； dm2＝0.3d0； 式中， d0为初始时刻， 从动侧机器人末端与 作业目标之间的距离 。权　利　要　求　书 2/2 页 3 CN 115157256 A 3