基于双臂协作势场的力觉引导遥操作系统及控制方法

本发明属于空间遥操作控制的，主要涉及了一种基于双臂协作势场的力觉引导遥操作系统及控制方法。

背景技术：

1、近年来，遥操作技术广泛应用于深空深海探测、电力巡检带电作业、核生化设施运维等环境复杂多变，任务需求多种多样的场合，由于现阶段机器人控制技术与传感器技术水平有限，难以研发出完全自主的机器人，因而开发遥操作机器人代替人工进入上述危险环境中完成作业是大势所趋。力反馈手控器能够让操作者感受到机器人作业时与环境的力交互情况，力觉反馈是增强遥操作系统的临场感、透明度，拓宽人机交互维度的重要手段。

2、然而，由于自由度超过6的机械臂的操作空间与关节空间之间的映射关系并非显式且较为复杂，并且由于人的手腕与手臂构成了耦合系统，二者之间的运动通常会相互影响，因此人往往难以直接遥操作控制机械臂末端达到理想的位置与姿态，操作过程中不可避免会产生位置误差。近年来，基于传统虚拟夹具和人工势场思想的虚拟力引导方法成为解决上述问题的有效方法，但是这些方法往往只考虑了单台机械臂作业的情况，没有针对双臂协作遥操作系统的虚拟力觉引导方法，而双臂机器人具有更高的自由度，其作业更加灵活，在执行诸如搬运、装配等复杂作业任务时，相较于传统的单臂机器人，拥有协调操作能力的双臂机器人更加优越，具有更广阔的应用前景。相应的，双臂机器人的遥操作控制难度更大，往往需要操作者一心多用，同时控制两台机械臂完成包括避障、趋近目标、调整作业姿态、双臂闭链协同运动在内的协调作业任务，这对操作者的身心有着巨大负担。

3、此外，当前大部分力觉引导方法基于预定义的结构化环境构建虚拟夹具，不适用于复杂多变的非结构化环境；或是基于抛物线函数构建引力场与斥力场，但是此法容易发生局部最优问题，此时机械臂末端所受吸引力与排斥力合力为零因而无法到达目标点。同时，当前的力觉引导方法大多没有考虑机械臂的工作空间对于完成作业任务的影响。

技术实现思路

1、本发明正是针对现有技术的不足，公开了一种基于双臂协作势场的力觉引导遥操作系统及控制方法，通过机械臂运动学模型得到两台机械臂末端工具中心点的实时位姿，循环判断目标物体与双臂的工作空间边界的最短距离dp-s,i是否低于阈值ds，若均高于阈值则采取双臂对称协作策略；若其与双臂的工作空间边界距离低于阈值，则采取单臂优先协作策略，将该臂作为优先臂；根据协作策略分别确定双臂的协作因子δi；本发明方法基于环境点云图像、双臂末端工具中心点的位姿信息以及双臂工作空间确定协作策略，进而动态更新协作因子；根据协作因子、目标物体距离双臂末端工具中心点的距离以及障碍物的位置构建双臂协作势场；基于双臂协作势场生成对应的动态虚拟约束力，通过两台力反馈手控器引导、协助操作者操控两台机械臂来完成双臂协作任务，能够提高操作者的控制精度、作业效率以及人机协作时的安全系数，更能适应复杂的非结构化环境。

2、为了实现上述目的，本发明采取的技术方案是：基于双臂协作势场的力觉引导遥操作系统，包括两台机械臂、力反馈手控器、路由器、视觉单元、从端电脑、主端电脑、显示器和移动平台；

3、所述两台机械臂分别安装在移动平台上，与从端电脑连接；

4、所述力反馈手控器位于操作台上，与从端电脑连接，用于采集笛卡尔坐标系下的六自由度位姿和机械臂的控制指令并将其发送给主端电脑，接收主端电脑的双臂协作势场力觉引导控制模块算法生成的动态虚拟约束力数据并反馈给操作者；

5、所述路由器负责建立整个控制系统的局域网，实现机械臂、从端电脑、视觉单元、路由器、主端电脑和力反馈手控器之间的实时数据交换；

6、所述视觉单元采用结构光相机，位于移动平台上，安装在两台机械臂之间，与从端电脑相连，用于实时采集机器人周围环境的点云信息，并传递至主端电脑；

7、所述从端电脑位于移动平台内部，包括机械臂驱动模块、移动平台控制模块和网络通信模块，还包括各控制模块之间的集成与通讯功能；

8、所述主端电脑位于操作台旁，包括力反馈手控器驱动模块、双臂协作势场力觉引导控制模块、机械臂运动学模型、机械臂动力学模型、点云信息处理模块以及网络通信模块，还包括各控制模块之间的集成与通讯功能；

9、所述显示屏位于操作台正前方，用于显示点云图像以及虚拟约束力的数据；

10、所述移动平台，用于安装机械臂、视觉单元以及从端电脑。

11、为了实现上述目的，本发明还采取的技术方案是：基于双臂协作势场的力觉引导遥操作控制方法，包括如下步骤：

12、s1：作业开始前，基于双臂的运动学模型，使用蒙特卡洛法分别确定双臂的工作空间，求解并记录双臂工作空间的边界坐标；

13、s2：获取环境点云信息，并基于图像特征分割点云；根据点云图像获取障碍物的位置、目标物体的位置，使用包围盒算法，以目标物的点云包围盒形状确定双臂各自目标点的位置与姿态角；

14、s3：通过机械臂运动学模型得到两台机械臂末端工具中心点的实时位姿，循环判断目标物体与双臂工作空间边界的最短距离dp-s,i是否低于阈值ds：

15、若目标物体与双臂工作空间边界的最短距离dp-s,i均高于阈值则采取双臂对称协作策略，此时左右臂的协作因子δi为1；

16、若目标物体与双臂工作空间边界的最短距离dp-s,i小于阈值ds，则采取单臂优先协作策略，将该臂作为优先臂，优先臂的协作因子δ1＝0.5，另一臂的协作因子δ1＝2；

17、s4：根据双臂末端工具中心点与各自目标点的相对位姿构建双臂协作势场，通过力反馈手控器生成虚拟位置吸引力fa,i(pt,i)，引导操作者操控双臂末端工具趋近目标点；

18、s5：循环判断机械臂末端工具中心点是否接近障碍物，若是则进入避障阶段，在障碍物点云区域外一定范围内构建斥力势场并以此通过力反馈手控器生成虚拟排斥力fr，i，j(dp，i-ob，j)，结合趋近目标点的虚拟位置吸引力fa，i(pt，i)协助操作者控制机械臂进行避障；否则返回执行步骤s4；

19、s6：循环判断机械臂末端工具中心点是否达到目标点附近，若是则进入调整作业姿态阶段，根据机械臂末端与对应目标点的相对姿态角通过力反馈手控器生成虚拟姿态吸引力fz，i(rt，i)，与步骤s4的虚拟位置吸引力fa，i(pt，i)结合为虚拟位姿吸引力fvr，i，引导操作者精确调整机械臂末端位姿，执行作业任务；否则返回执行步骤s4；

20、s7：循环判断双臂之间是否形成一定的闭链约束关系，若是则双臂处于双臂闭链协同运动阶段，记录并持续更新双臂末端工具中心点的相对位姿，根据双臂协作势场生成双臂相对阻抗模型，通过所述力反馈手控器生成虚拟相对阻抗约束力frc(t)，协助操作者操控双臂协同运动，使双臂末端的相对位置不发生大的突变，保持同步运动。

21、作为本发明的一种改进，述步骤s5中，判断机械臂末端工具中心点是否接近障碍物的方法为：计算机械臂末端工具中心点pt的坐标与障碍物之间的距离dp-ob，判断该值是否低于避障阈值dob，若是则构建斥力势场；

22、所述步骤s6中，判断机械臂末端工具中心点是否到达目标点附近的方法是：机械臂末端工具中心点p的坐标是否在目标点的点云区域内，在该区域范围内则同时生成虚拟位置吸引力和虚拟姿态吸引力；

23、所述步骤s7中，判断双臂之间是否形成一定的闭链约束关系的方法是：根据双臂夹爪的开闭状态、夹爪的点云轮廓与目标物体的点云轮廓相交的区域面积，循环检测双臂是否与目标物体形成了一个整体并开始协同运动。

24、作为本发明的另一种改进，所述步骤s4中的虚拟位置吸引力fa，i(pt，i)和步骤s5中的虚拟排斥力fr，i，j(dp，i-ob，j)均为3维力，不包含力矩；步骤s6中的虚拟位姿吸引力fvr，i包含力与力矩，为6维力；步骤s7的虚拟相对阻抗约束力frc(t)包含力与力矩，为6维力。

25、作为本发明的另一种改进，所述步骤s5中，机器人末端工具中心点与障碍物之间的最小距离dp-ob的计算方法如下：

26、s51：假设机械臂的末端工具中心坐标pt＝(xt，yt，zt)，障碍物点的包围盒模型的顶点qi＝(xi，yi，zi)，计算pt分别到包围盒的各顶点qi的距离λ1；

27、

28、s52：获取包围盒棱边的向量ni，求出pt到每个顶点的向量mi，再判断ni和mi之间的夹角，若为钝角则舍去；若为锐角则计算得最短距离λ2为：

29、

30、s53：在各面上任取三个顶点c1、c2、c3，设pt到各面上的垂足为pc＝(xc，yc，zc)，根据ptpc⊥c1c2、ptpc⊥c2c3、ptpc⊥c1c3求出pc的坐标值，再根据面上的顶点坐标确定垂足的取舍，最后求出最短距离λ3为：

31、

32、s54：取λ1、λ2、λ3中的最小值作为机械臂末端工具中心点到障碍物的距离dp-ob：

33、dp-ob＝min(λ1，λ2，λ3)。

34、作为本发明的又一种改进，所述步骤s4中，双臂协作势场的计算公式如下：

35、

36、式中，ua，i(pt，i)表示双臂协作势场，δi表示第i台机械臂在双臂协作势场中的协作因子，表示第i台机械臂末端工具中心点与其对应的目标点的距离，rt，i表示第i个目标点的点云区域球体的半径；

37、所述虚拟位置吸引力fa，i(pt，i)的计算公式如下：

38、

39、上式表示第i台机械臂所受到的吸引力作用，经所述力反馈手控器生成虚拟位置吸引力反馈给操作者。

40、作为本发明的又一种改进，所述步骤s5中，斥力势场的计算公式如下：

41、

42、式中，ur，j，i(dp，i-ob，j)表示第j个障碍物的斥力势场，ε表示斥力势场的调节因子，dp，i-ob，j表示第j个障碍物与第i台机械臂末端工具中心点的距离，dob，j表示第j个障碍物的斥力势场作用范围；

43、所述虚拟排斥力的计算公式如下：

44、

45、上式表示第i台机械臂受到第j个障碍物的斥力作用；表示第i台机械臂收到总计n个障碍物的斥力作用，经所述力反馈手控器生成虚拟排斥力反馈给操作者；

46、所述第i台机械臂的避障任务的虚拟约束力公式如下：

47、

48、作为本发明的更进一步改进，所述步骤s6中，虚拟姿态吸引力的计算公式如下：

49、

50、式中，rei＝rt，i-ro，i，表示第i台机械臂t时刻的姿态角rt，i与其对应的目标点的姿态角ro，i的相对姿态，表示的两者相对角速度，k是kelvin-voigt线性模型的刚度系数，b是kelvin-voigt线性模型的阻尼系数；

51、当操作者在精确调整机械臂末端位姿时所受的6维虚拟位姿吸引力计算公式如下：

52、

53、式中，fvr，i是一个6×1的矩阵，fa，i和fz，i均为3×1的矩阵。

54、作为本发明的更进一步改进，所述步骤s7中，虚拟相对阻抗约束力的计算原理为：首先记录双臂刚形成闭链约束时的末端工具中心点的相对位姿ce(0)作为双臂相对阻抗模型的平衡位置；再循环更新末端工具中心点的相对位姿并计算其与平衡位置之差，最后将该差值代入双臂相对阻抗模型，虚拟相对阻抗约束力计算公式如下：

55、

56、式中，e(t)＝ce(t)-ce(0)是t时刻两个机械臂末端工具中心点的相对位置与双臂刚形成闭链约束时的相对位置ce(0)之差，其中ce(t)是一个6×1的矩阵，x(t)和r(t)均为3×1的矩阵，md、bd、kd分别是双臂相对阻抗模型的惯性、阻尼与刚度系数。

57、与现有技术相比，本发明具有的有益效果：

58、(1)与传统的人工势场方法函数相比，本发明的双臂协作势场函数解决了局部最优问题，具有更好的引导效果。

59、(2)本发明的控制方法将双臂协作任务分解为避障、趋近目标、调整作业姿态、双臂闭链协同运动四个任务阶段，分别针对每个任务阶段基于双臂协作势场生成对应的动态虚拟约束力，能够提高操作者的控制精度、作业效率以及人机协作时的安全系数，更能适应复杂的非结构化环境。

60、(3)本发明考虑了机械臂的工作空间对于双臂协作策略影响，进而动态更新协作因子以调整双臂协作势场产生的吸引力大小，符合双臂协作的控制特点，实用性强。

61、(4)本发明设计的遥操作系统具有低时延、高精度、高稳定性、高安全性、六维力感知与引导等优点，且采用双臂协作，自由度高，能够完成更复杂的作业任务，更符合实际应用的需求。