本发明涉及机械控制，尤其涉及基于双边控制的机械臂示教方法、系统、设备及存储介质。
背景技术：
1、随着机器人技术的飞速发展，其应用领域日益广泛。机器人可以代替人类完成大量重复性的任务，或者在一些危险或恶劣的工作环境中操作。传统的机器人技能获取主要是通过机器人编程或动觉教学来实现的，机器人编程是指通过编程设置机器人的运动/力量轨迹，但机器人的行为通常不容易进行脚本化。例如，在复杂接触环境中的任务操作中，很难设计具有高适应性和稳定性的运动规划算法。动觉示教克服了机器人编程的一些缺点，并有效地获取技能，但操作员仍然需要处在危险的工作环境中。此外，除了施加力来完成任务外，操作员还需要抵抗工具的动态力。缺乏可靠的远程操作机制来独立记录工具的接触力来完成任务，导致收集的接触力偏大，这是次优的。
2、远程操作示教意味着操作员可以通过控制主机器人的运动来远程控制从机器人执行任务，双向远程操作可以提供力反馈，以完成接触任务的示教，遥操作示教克服了机器人编程或动觉示教的缺点。双边控制方法主要分为四种类型：位置控制、力控制、位置-力量控制和力-位置混合控制。位置-力量双向方法是远程操作系统中常用的方法，从机器人追踪主机器人的位置，主机器人追踪与环境交互的从机器人的反馈力。
3、基于导纳控制实现机器人拖动，在外界刚度变化大时，机器人会发生反弹或者震荡现象。调整导纳参数，可以减小反弹或震荡，但是参数变化会向系统注入能量，破坏系统的无源性。导纳遥操作发生反弹或者震荡是由于遥操作系统并不能完美地执行导纳模型，由于机器人末端执行器响应时间与响应误差，机器人行为与名义导纳模型是有一定差异的，该差异破坏了系统的无源性。
4、为了克服这些缺陷，本技术提出了基于双边控制的机械臂示教方法、系统、设备及存储介质，基于导纳控制的遥操作拖动示教能够使操作人员以低阻抗拖动主机械臂末端进行示教，并收集到机器人独立于环境交互的数据。
技术实现思路
1、本技术的目的是提供基于双边控制的机械臂示教方法、系统、设备及存储介质，旨在解决的问题。
2、为实现上述目的，本技术提供如下技术方案：
3、本技术提供基于双边控制的机械臂示教方法，包括：
4、获取主机器人和从机器人末端的力传感器读数；
5、对力传感器读数进行重力补偿，得到所述主机器人和从机器人的接触力；
6、根据所述主机器人和从机器人的接触力判断机械臂的运动状态是否稳定，若不稳定，则调整导纳模型的参数；
7、将所述主机器人的接触力和所述从机器人的接触力进行求差后输入导纳模型，并利用导纳模型将所述主机器人和从机器人的接触力转化为笛卡尔空间位置信息；
8、根据逆运动学将所述笛卡尔空间位置信息转化为关节信息，发送至所述主机器人和所述从机器人的机械臂，使所述主机器人和所述从机器人的机械臂运动。
9、进一步的，在获取主机器人和从机器人末端的力传感器读数的步骤中，具体包括下述步骤：
10、由所述主机器人和所述从机器人末端安装的六维力传感器采集所述力传感器读数，所述力传感器读数为笛卡尔空间六个自由度的力和力矩，主从机器人的力传感器读数分别为fh和fe。
11、进一步的，在对力传感器读数进行重力补偿，得到所述主机器人和从机器人的接触力的步骤中，具体包括下述步骤：
12、计算所述主机器人和从机器人末端工具重力fht和fet，力传感器读数减去末端工具重力，得到重力补偿后的所述主机器人和从机器人接触力fs及fm；计算公式为：
13、fs＝fe-fet
14、fm＝fh-fht。
15、进一步的，在根据所述主机器人和从机器人的接触力判断机械臂的运动状态是否稳定，若不稳定，则调整导纳模型的参数的步骤中，具体包括下述步骤：
16、系统的初始导纳模型为：
17、
18、其中m0为初始虚拟质量，b0为初始虚拟阻尼，两者都为六维对角矩阵；ti为当前时间；i为程序离散运行的时间步；为导纳模型计算的指令加速度；经过积分得到指令位置x(ti)，主机器人和从机器人执行得到实际位置xm(ti)，xs(ti)；
19、当fs(ti+1)(x(ti)-xs(ti))＞ε时，则不调整导纳参数；
20、若导纳震荡在遥操作中所述接触力表现为：
21、fs(ti+1)(x(ti)-xs(ti))≤ε
22、则调整导纳参数，每检测到一次不稳定现象时计数器k加一。
23、进一步的，所述调整导纳参数具体包括下述步骤：
24、设定检测阈值ε检测导纳控制异常的反弹震荡行为，当需要调整导纳模型中的虚拟质量时，则通过增大惯性项减小反弹震荡行为，m(ti)的调整计算公式为：
25、
26、b(ti+1)＝rm(ti+1)
27、其中sp(p＝0,1,...,k)为虚拟质量的调整量，β为遗忘因子，当震荡消失后质量矩阵恢复到初始值m0，r为阻尼和质量比值；
28、虚拟质量的调整量sp的计算公式为：
29、sp＝diag{s1,s2,s3,s4,s5,s6}
30、
31、其中为在一个时间步中允许导纳参数增大的上界，下标j为自由度，t(ti)为在(0,ti)时间内系统耗散的能量；
32、根据系统无源的条件，系统(0,ti)耗散的能量大于(ti,ti+1)时间内注入的能量，公式为：
33、
34、由于项建立不等式：
35、
36、根据速度项存在的上界：
37、
38、导出新的不等式：
39、
40、积分导出m的调整方法：
41、
42、sp(ti)＝diag{s1,s2,s3,s4,s5,s6}。
43、进一步的，在将所述主机器人的接触力和所述从机器人的接触力进行求差后输入导纳模型，并利用导纳模型将所述主机器人和从机器人的接触力转化为笛卡尔空间位置信息的步骤中，具体包括下述步骤：
44、将所述接触力求差输入导纳模型后，输出笛卡尔空间加速度，表达式为：
45、
46、其中m(ti+1)、b(ti+1)分别为惯性矩阵和阻尼矩阵，为机器人末端的在笛卡尔空间的加速度，fm(ti+1)、fs(ti+1)为当前时刻的接触力。
47、进一步的，在根据逆运动学将所述笛卡尔空间位置信息转化为关节信息，发送至所述主机器人和所述从机器人的机械臂，使所述主机器人和所述从机器人的机械臂运动的步骤中，具体包括下述步骤：
48、笛卡尔空间速度计算公式为：
49、
50、其中δt由雅各比的逆矩阵计算得到关节速度，再与机器人实际关节位置积分得到机器人的关节位置，将关节位置发送给机械臂伺服系统；
51、其表达式为：
52、
53、
54、其中δt为运行周期时间间隔；δt＝ti+1-ti，j-1为雅各比矩阵的逆；为关节指令速度，q(ti+1)为关节指令位置。
55、本技术提供基于双边控制的机械臂示教方法的系统，所述系统包括：
56、获取模块：获取主机器人和从机器人末端的力传感器读数；
57、处理模块：对力传感器读数进行重力补偿，得到所述主机器人和从机器人的接触力；
58、判断模块：根据所述主机器人和从机器人的接触力判断机械臂的运动状态是否稳定，若不稳定，则调整导纳模型的参数；
59、输入模块：将所述主机器人的接触力和所述从机器人的接触力进行求差后输入导纳模型，并利用导纳模型将所述主机器人和从机器人的接触力转化为笛卡尔空间位置信息；
60、发送模块：根据逆运动学将所述笛卡尔空间位置信息转化为关节信息，发送至所述主机器人和所述从机器人的机械臂，使所述主机器人和所述从机器人的机械臂运动。
61、本技术提供一种设备，所述设备包括处理器、与所述处理器耦接的存储器，其中，所述存储器存储有用于实现基于双边控制的机械臂示教方法的程序指令；所述处理器用于执行所述存储器存储的所述程序指令以实现基于基于双边控制的机械臂示教。
62、本技术提供一种存储介质，存储有处理器可运行的程序指令，所述程序指令用于执行基于双边控制的机械臂示教方法。
63、本技术提供了基于双边控制的机械臂示教方法、系统、设备及存储介质，具有以下有益效果：
64、(1)由主从机器人末端安装的六维力传感器采集力传感器读数，收集到从端独立完成任务的信息，相比于直接拖动机械臂与外界环境接触，收集的力传感器读数更加准确，并且遥操作进行远程示教，当任务恶劣时能保证示教人员的安全；
65、(2)基于可测的力传感器读数和机器人当前位置信息，判断导纳遥操作系统是否稳定，若导纳震荡在遥操作中得到的接触力表现为fs(ti+1)(x(ti)-xs(ti))＞ε，则调整导纳参数；若导纳震荡在遥操作中得到的接触力表现为fs(ti+1)(x(ti)-xs(ti))≤ε，则不调整参数；采用导纳模型将接触力转换为位置信息，实低阻抗的拖动示教，更有利于操作人员对点位或轨迹示教；
66、(3)本发明采用的示教方法基于机械臂伺服系统的位置环，适用于市场上大部分机械臂系统，适用范围广。
1.基于双边控制的机械臂示教方法，其特征在于，包括：
2.根据权利要求1所述的基于双边控制的机械臂示教方法，其特征在于，在获取主机器人和从机器人末端的力传感器读数的步骤中，具体包括下述步骤：
3.根据权利要求1所述的基于双边控制的机械臂示教方法，其特征在于，在对力传感器读数进行重力补偿，得到所述主机器人和从机器人的接触力的步骤中，具体包括下述步骤：
4.根据权利要求1所述的基于双边控制的机械臂示教方法，其特征在于，在根据所述主机器人和从机器人的接触力判断机械臂的运动状态是否稳定，若不稳定，则调整导纳模型的参数的步骤中，具体包括下述步骤：
5.根据权利要求4所述的基于双边控制的机械臂示教方法，其特征在于，所述调整导纳参数具体包括下述步骤：
6.根据权利要求1所述的基于双边控制的机械臂示教方法，其特征在于，在将所述主机器人的接触力和所述从机器人的接触力进行求差后输入导纳模型，并利用导纳模型将所述主机器人和从机器人的接触力转化为笛卡尔空间位置信息的步骤中，具体包括下述步骤：
7.根据权利要求1所述的基于双边控制的机械臂示教方法，其特征在于，在根据逆运动学将所述笛卡尔空间位置信息转化为关节信息，发送至所述主机器人和所述从机器人的机械臂，使所述主机器人和所述从机器人的机械臂运动的步骤中，具体包括下述步骤：
8.一种根据权利要求1所述的基于双边控制的机械臂示教方法的系统，其特征在于，所述系统包括：
9.一种设备，其特征在于，所述设备包括处理器、与所述处理器耦接的存储器，其中，所述存储器存储有用于实现权利要求1-7任一项所述的基于双边控制的机械臂示教方法的程序指令；所述处理器用于执行所述存储器存储的所述程序指令以实现基于双边控制的机械臂示教。
10.一种存储介质，其特征在于，存储有处理器可运行的程序指令，所述程序指令用于执行权利要求1-7任一项所述的基于双边控制的机械臂示教方法。
