一种机械臂碰撞判断方法及装置与流程

本发明涉及医疗设备，具体而言，涉及一种机械臂碰撞判断方法及装置。

背景技术：

1、随着科技的不断发展，手术机器人的应用前景变得越来越广阔，其中，机械臂具有精确的运动控制和高度灵活的操作能力，通过机械臂上安装不同的器械可以协助医生完成高精度的手术操作。

2、腹腔镜手术机器人以其在临床上的稳定性能，提升手术效率等诸多优点，逐渐让医患所熟知和信赖。但是，由于大多数腹腔镜手术机器人控制台的主屏幕内视野为病灶位置，从而导致机械臂在移动过程中滑台杆之间发生碰撞时不易被察觉，从而可能影响手术进行。

技术实现思路

1、本发明解决的问题是如何准确判断机械臂是否发生碰撞。

2、为解决上述问题，本发明提供一种机械臂碰撞判断方法及装置。

3、第一方面，本发明提供了一种机械臂碰撞判断方法，包括：

4、获取左机械臂的左器械杆状态信息信息、右机械臂的右器械杆状态信息和持镜臂的持镜臂状态信息；

5、根据所述左器械杆状态信息得到左器械杆外壳直线关系，根据所述右器械杆状态信息得到右器械杆外壳直线关系，根据所述持镜臂的持镜臂状态信息得到持镜臂左外壳直线关系和持镜臂右外壳直线关系；

6、当所述左器械杆外壳直线关系和所述持镜臂左外壳直线关系满足第一预设判别条件时，判断所述左机械臂和所述持镜臂碰撞，当所述右器械杆外壳直线关系和所述持镜臂右外壳直线关系满足第二预设判别条件时，判断所述右机械臂和所述持镜臂碰撞。

7、可选地，所述左器械杆状态信息包括左器械杆末端坐标、左器械杆远端坐标、所述左器械杆关节夹角、所述左器械杆四边形余角、所述左机械臂外壳距离和所述左机械臂中心轴距离；所述根据所述左器械杆状态信息得到左器械杆外壳直线关系，包括：

8、根据左器械杆关节夹角分别得到x轴左器械杆关节夹角、y轴左器械杆关节夹角和z轴左器械杆关节夹角；

9、根据所述左器械杆末端坐标、所述左器械杆远端坐标、所述左器械杆关节夹角、所述左器械杆四边形余角、所述左机械臂中心轴距离和所述左机械臂外壳距离,生成所述左器械杆外壳直线关系。

10、可选地，所述所述左器械杆外壳直线关系满足：

11、xl＝xl3+(xl1cosθplx-xl3)*tl+dlsinθkl；

12、yl＝yl3+(yl1cosθply-yl3)*tl-dlr；

13、zl＝zl3+(zl1cosθplz-zl3)*tl+dlcosθkl；

14、其中，(xl，yl，zl)为所述左器械杆外壳直线关系的坐标，(xl1，yl1，zl1)为所述左器械杆末端坐标，(xl3，yl3，zl3)为所述左器械杆远端坐标，cosθplx为所述左器械杆关节夹角的x轴分量，cosθply为所述左器械杆关节夹角的y轴分量，cosθplz为所述左器械杆关节夹角的z轴，θkl为所述左器械杆四边形余角，tl为所述左器械杆外壳直线关系的调节参数，dl为所述左机械臂中心轴距离，dlr为所述左机械臂外壳距离。

15、可选地，所述右器械杆状态信息包括右器械杆末端坐标、右器械杆远端坐标、所述右器械杆关节夹角、所述右器械杆四边形余角、所述右机械臂外壳距离和所述右机械臂中心轴距离；所述根据所述右器械杆状态信息得到右器械杆外壳直线关系，包括：

16、根据所述右器械杆末端坐标、所述右器械杆远端坐标、所述右器械杆关节夹角、所述右器械杆四边形余角、所述右机械臂中心轴距离和所述右机械臂外壳距离,生成所述右器械杆外壳直线关系。

17、可选地，所述所述右器械杆外壳直线关系满足：

18、xr＝xr3+(xr1cosθprx-xr3)*tr+drsinθkr；

19、yr＝yr3+(yr1cosθpry-yr3)*tr+drl；

20、zr＝zr3+(zr1cosθprz-zr3)*tr+drcosθkr；

21、其中，(xr，yr，zr)为所述右器械杆外壳直线关系的坐标，(xr1，yr1，zr1)为所述右器械杆末端坐标，(xr3，yr3，zr3)为所述右器械杆远端坐标，cosθprx为所述右器械杆关节夹角的x轴分量，cosθpry为所述右器械杆关节夹角的y轴分量，cosθprz为所述右器械杆关节夹角的z轴分量，θkr为所述右器械杆四边形余角，tr为所述右器械杆外壳直线关系的调节参数，dr为所述右机械臂中心轴距离，drl为所述右机械臂外壳距离。

22、可选地，所述持镜臂状态信息包括镜头末端坐标、镜头远端坐标、持镜臂四边形余角、持镜臂左外壳距离、持镜臂右外壳距离和镜头中心轴距离；所述根据所述持镜臂的持镜臂状态信息得到持镜臂左外壳直线关系和持镜臂右外壳直线关系，包括：

23、根据所述镜头末端坐标、所述镜头远端坐标、所述持镜臂四边形余角和所述持镜臂左外壳距离，生成所述持镜臂左外壳直线关系；

24、根据所述镜头末端坐标、所述镜头远端坐标、所述持镜臂四边形余角和所述持镜臂右外壳距离，生成所述持镜臂右外壳直线关系。

25、可选地，所述持镜臂左外壳直线关系满足：

26、xcl＝xc2+(xc1-xc2)*tc+dcsinθkc；

27、ycl＝yc2+(yc1-yc2)*tc+dcl；

28、zcl＝zc2+(zc1-zc2)*tc+dccosθkc；

29、所述持镜臂右外壳直线关系满足：

30、xcr＝xc2+(xc1-xc2)*tc+dcsinθkc；

31、ycr＝yc2+(yc1-yc2)*tc-dcr；

32、zcr＝zc2+(zc1-zc2)*tc+dccosθkc；

33、其中，(xcl，ycl，zcl)为所述持镜臂左外壳直线关系的坐标，(xcr，ycr，zcr)为所述持镜臂右外壳直线关系的坐标，(xc1，yc1，zc1)为所述镜头末端坐标，(xc2，yc2，zc2)为所述镜头远端坐标，tc为所述持镜臂左外壳直线关系的调整参数，θkc为所述持镜臂四边形余角，dcl为所述持镜臂左外壳距离，dcr为所述持镜臂右外壳距离，dc为所述镜头杆距离。

34、可选地，所述第一预设判别条件包括第一相交条件和第一范围条件；所述第二预设判别条件包括第二相交条件和第二范围条件；所述当所述左器械杆外壳直线关系和所述持镜臂左外壳直线关系满足第一预设判别条件时，判断所述左机械臂和所述持镜臂碰撞，当所述右器械杆外壳直线关系和所述持镜臂右外壳直线关系满足第二预设判别条件时，判断所述右机械臂和所述持镜臂碰撞，包括：

35、获取左滑台长度、左关节角度、左四边形角度、右滑台长度、右关节角度和右四边形角度；

36、根据所述左滑台长度、所述左关节角度、所述左四边形角度和左器械杆远端坐标得到所述第一范围条件，根据所述右滑台长度、所述右关节角度、所述右四边形角度和右器械杆远端坐标得到所述第二范围条件；

37、当所述左器械杆外壳直线关系、所述持镜臂左外壳直线关系满足所述第一相交条件时，得到左交点坐标；

38、当所述左交点坐标满足第一范围条件时，判断所述左机械臂和所述持镜臂碰撞；

39、当所述右器械杆外壳直线关系、所述持镜臂右外壳直线关系满足所述第二相交条件是，得到右交点坐标；

40、当所述右交点坐标满足第二范围条件时，判断所述右机械臂和所述持镜臂碰撞。

41、可选地，所述第一相交条件满足：

42、xol＝xl3+(xl1cosθplx-xl3)*tl+dlsinθkl＝xc2+(xc1-xc2)*tc+dcsinθkc；

43、yol＝yl3+(yl1cosθply-yl3)*tl-dlr＝yc2+(yc1-yc2)*tc+dcl；

44、zol＝zl3+(zl1cosθplz-zl3)*tl+dlcosθkl＝zc2+(zc1-zc2)*tc+dccosθkc；

45、所述第一范围条件满足：

46、xl3<xol<xl3+hl*cosθrlx+hl*cosθwlx；

47、yl3<yol<yl3+hl*cosθrly+hl*cosθwly；

48、zl3<zol<zl3+hl*cosθrlz+hl*cosθwlz；

49、所述第二相交条件满足：

50、xor＝xr3+(xr1cosθprx-xr3)*tr+drsinθkr＝xc2+(xc1-xc2)*tc+dcsinθkc；

51、yor＝yr3+(yr1cosθpry-yr3)*tr+drl＝yc2+(yc1-yc2)*tc-dcr；

52、zor＝zr3+(zr1cosθprz-zr3)*tr+drcosθkr＝zc2+(zc1-zc2)*tc+dccosθkc；

53、所述第二范围条件满足：

54、xr3<xor<xr3+hr*cosθrrx+hr*cosθwrx；

55、yr3<yor<yr3+hr*cosθrry+hr*cosθwry；

56、zr3<zor<zr3+hr*cosθrrz+hr*cosθwrz；

57、其中，(xol，yol，zol)为所述左交点，(xor，yor，zor)为所述右交点坐标，θrlx为所述左关节角度的x轴分量，θrly为所述左关节角度的y轴分量，θrlz为所述左关节角度的z轴分量，θwlx为所述左四边形角度的x轴分量，θwly为所述左四边形角度的y轴分量，θwlz为所述左四边形角度的z轴分量，θrlx为所述左关节角度的x轴分量，θrry为所述左关节角度的y轴分量，θrrz为所述左关节角度的z轴分量，θwrx为所述左四边形角度的x轴分量，θwry为所述左四边形角度的y轴分量，θwrz为所述左四边形角度的z轴分量，hl为所述左滑台长度，hr为所述右滑台长度。

58、第二方面，一种机械臂碰撞判断装置，包括：

59、获取模块，用于获取左机械臂的左器械杆状态信息信息、右机械臂的右器械杆状态信息和持镜臂的持镜臂状态信息；

60、处理模块，用于根据所述左器械杆状态信息得到左器械杆外壳直线关系，根据所述右器械杆状态信息得到右器械杆外壳直线关系，根据所述持镜臂的持镜臂状态信息得到持镜臂左外壳直线关系和持镜臂右外壳直线关系；

61、判断模块，用于当所述左器械杆外壳直线关系和所述持镜臂左外壳直线关系满足第一预设判别条件时，判断所述左机械臂和所述持镜臂碰撞，当所述右器械杆外壳直线关系和所述持镜臂右外壳直线关系满足第二预设判别条件时，判断所述右机械臂和所述持镜臂碰撞。

62、本发明的机械臂碰撞判断方法、装置及存储介质的有益效果是：在三维空间中，根据获取到的左器械杆状态信息、右器械杆状态信息和持镜臂状态信息，可以准确地判断机械臂的运动状态和运动轨迹，从而根据状态信息可以准确地判断机械臂之间是否发生碰撞；根据左器械杆状态信息构建左器械杆外壳直线关系，可以得到左机械臂靠近持镜臂一侧的外壳在三维空间中的直线轨迹，根据持镜臂状态信息构建持镜臂左外壳直线关系，同样可以得到持镜臂靠近左机械臂一侧的外壳直线轨迹，通过两个外壳直线轨迹可以准确地判断左机械臂的右侧外壳和持镜臂的左侧外壳是否发生碰撞，同理，根据构建的右机械臂外壳直线关系和持镜臂右外壳直线关系也可以判断右机械臂和持镜臂是否碰撞；进一步判断左器械杆外壳直线关系和持镜臂左外壳直线关系是否满足第一预设判别条件，若是，则判断左机械臂和持镜臂发生碰撞，若否，则说明左机械臂和持镜臂没有发生碰撞，同样地通过判断右器械外壳直线关系和持镜臂右外壳直线关系是否满足第二预设条件，进而判断右机械臂和持镜臂是否发生碰撞，通过上述方法可以对医生不易察觉的碰撞准确地进行判断，在机械臂接触的的外壳接触的时候第一时间准确地判断碰撞的发生，可以有效地避免事故的发生，降低了手术过程中的风险，提高了医生的工作效率。