一种基于5G网络的体感遥操作机器人系统控制方法

一种基于5g网络的体感遥操作机器人系统控制方法
技术领域
1.本发明属于机器人控制领域，更具体地，涉及一种基于5g网络的体感遥操作机器人系统控制方法。


背景技术：

2.5g是目前广泛使用的移动通讯技术，是4g移动通讯技术的升级版本。5g有着高速、低延时、节能、高带宽以及接入设备数量多等特点。可以有效解决4g时代因网络限制原因造成的各种问题，促进工业、医疗等行业进入万物互联的新时代。
3.传统的遥操作往往基于有线连接，有线连接虽然稳定但却受限于网线，导致了不够灵活以及前期准备繁琐等问题。在将5g技术运用到5g后可以大大提升设备灵活性以及降低前期准备需要的工作量，但因为5g是高频短波信号，其特点是传播范围小、穿透力弱等。这就造成了使用5g网络进行遥操作时会容易出现丢包等现象，难以满足体感遥操作的通信需求。


技术实现要素：

4.针对现有技术的以上缺陷或改进需求，本发明提供了一种基于5g网络的体感遥操作机器人系统控制方法，其目的在于，利用5g和体感遥操作技术，发生丢包时可自动从体感遥操作控制模式切换至机载控制模式，有效避免机器人因为网络波动原因造成的急停和伤害。
5.为实现上述目的，本发明提出了一种基于5g网络的体感遥操作机器人系统控制方法，包括如下步骤：
6.s1、在操作者活动过程中，获取操作者的实时关节角度和对应时间戳，并将其传输给机器人；
7.s2、以实时关节角度作为目标关节角度，根据目标关节角度和机器人当前关节角度确定机器人的下一个关节角度；将得到的每个关节角度都以栈的形式储存在机器人中，机器人按关节角度运动；
8.s3、比较当前机载时间戳和目标关节角度的对应时间戳，当时间戳差值大于丢包判定阈值时判定为丢包，进入步骤s4；否则重复步骤s1和s2；
9.s4、发生丢包时切换为机载控制模式：在每两个栈内数据间取均值自动增补栈，将增补后新栈中的数据依次出栈作为机器人新的关节角度；
10.当栈内数据没有完全出栈前，机器人接收到传输的新的目标关节角度时，切换回体感遥操作控制模式，即重复步骤s1～s3；
11.当栈内数据全部出栈后，机器人仍然没有接收到新的目标关节角度时，机器人停止在栈内最后一个关节角度的位置。
12.作为进一步优选的，步骤s2中，确定机器人的下一个关节角度，具体为：将目标关节角度与机器人当前关节角度作差，当该差值的绝对值小于角度阈值时，将目标关节角度
设置为机器人的下一个关节角度；否则，将当前关节角度加上或减去角度阈值设置为机器人的下一个关节角度。
13.作为进一步优选的，得到的每个关节角度以栈的形式储存在机器人中，当栈内数据溢出时优先舍弃栈底的数据。
14.作为进一步优选的，通过体感遥操作工作站将实时关节角度和对应时间戳经5g cpe传输给机器人。
15.作为进一步优选的，步骤s1中，先测得操作者胸部的四元数和上臂的四元数，然后根据对应穿戴部位四元数，求得对应运动中各关节角的旋转角度作为实时关节角度。
16.作为进一步优选的，步骤s4中，当栈内数据全部出栈后，机器人仍然没有接收到新的目标关节角度时，机器人停止在栈内最后一个关节角度的位置，并在恢复通信后弹出错误信息提示操作者排除故障。
17.作为进一步优选的，所述机器人的机械臂末端安装有柔性夹持装置，用于核酸检测咽拭子采样。
18.总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：
19.1、本发明首先判断通信过程是否发生丢包，进而设计了丢包时采用的机载控制模式，使得在出现网络波动的情况下机载控制模式下的关节角度也是由操作者控制曾经过的关节角度，可以有效避免误操作，避免机器人因为网络丢包原因的急停以及对患者造成伤害。
20.2、本发明使用事件触发判断通信过程是否发生丢包，相对现有的通信方法增加了判断机制，可以增加通信可靠性，同时可以使机器人系统在发生短暂网络波动的情况下自动重新由操作者接入控制，可以增加控制系统的实用性。
21.3、本发明使用控制模式切换的方式降低在网络波动发生的情况下对系统造成的影响以及避免对患者造成伤害，还可以有效提醒操作者排除网络故障，提升遥操作控制效果，同时避免操作者重复动作造成所述机器人系统误操作。
22.4、本发明设计了机器人关节角度的确定方式，通过角度阈值判定，使得机器人运动轨迹更加平滑。
附图说明
23.图1为本发明实施例基于5g网络的体感遥操作机器人系统结构示意图；
24.图2为本发明实施例基于5g网络的体感遥操作机器人系统控制方法的流程图。
具体实施方式
25.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
26.本发明实施例提供的一种基于5g网络的体感遥操作机器人系统，如图1所示，该机器人系统包括机器人、体感遥操作工作站、图像工作站、影像采集设备、5g客户前置设备
(customer premise equipment,cpe)；所述体感遥操作工作站通过5g cpe与机器人通信连接，所述影像采集设备用于实时收集操作端图像信息，其与所述图像工作站连接；所述图像工作站用于实时显示操作端图像以供操作者判断下一步操作；所述机器人机械臂末端安装有柔性夹持装置，该机器人系统可用于方舱医院内的核酸检测咽拭子采样。
27.上述体感遥操作机器人系统的控制方法，如图2所示，包括如下步骤：
28.s1、获取操作者位置与运动姿态并转换为关节角度。
29.通过惯性传感器获取操作者实时的位置与运动姿态，具体的，测得胸部的四元数和上臂的四元数后，则肩部的四元数可以表示为根据对应穿戴部位四元数可以求得对应运动所需控制输出各机械关节角的旋转角度，将该实时关节角度与对应的时间戳经5g cpe传输至移动机器人关节电机。
30.s2、平滑运动轨迹。
31.将接收到的实时关节角设置为目标关节角度，然后将目标关节角度与机器人的当前关节角度作差。当该差值的绝对值小于预设的角度阈值q
thres
时，将目标关节角度设置为机器人的下一个关节角度；当该差值的绝对值大于或等于角度阈值q
thres
时，将当前关节角度加上或减去角度阈值设置为机器人的下一个关节角度。同时，每一个关节角度会以栈的形式储存在所述机器人中，当栈内数据溢出时会优先舍弃栈底的数据。
32.s3、判断是否发生丢包。
33.不断比较当前机载时间戳与目标关节角度时间戳，当当前机载时间戳与目标关节角度时间戳差值的绝对值大于丢包判定阈值t
thres
时，判定为丢包，进入步骤s4；否则继续体感遥操作控制模式，即如前所述根据操作者动作确定机器人运动。
34.s4、发生丢包时切换为机载控制模式。
35.机载控制模式时：栈内数据会在每两个数据间取均值自动增补栈，将增补后的新栈中的数据依次出栈作为机械臂新的关节角度；使得机载控制模式下的关节角度也是由操作者控制经过的关节角度，可有效避免误操作。
36.当栈内数据全部出栈后机器人仍然没有接收到体感遥操作工作站传输的新的目标关节角度时，机械臂会停止在栈内最后一个关节角度的位置，并在恢复通信后在所述图像工作站弹出错误信息提示操作者排除故障，避免操作者重复动作造成机器人系统误操作。
37.当栈内数据没有完全出栈前机器人接收到体感遥操作工作站传输的新的目标关节角度时，自动切换回体感遥操作模式继续由操作者控制。
38.综上，本发明方法通过将已经过的轨迹点存入堆栈，当因网络故障出现丢包现象时，可以自动切换至机载控制模式，并在网络恢复时自动切换回遥操作控制模式，在网络无法及时恢复通信时主动提醒操作者排除网络故障。
39.本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。