一种机械臂配准与导航方法与流程

本发明涉及机械臂定位，尤其涉及一种机械臂配准与导航方法。

背景技术：

1、在导航式手术中，定位相机需要能够准确定位执行工具的空间位姿，即确立定位相机坐标系至执行工具坐标系的转换矩阵。在实际手术中，机械臂可能需要结合多种导航式工具使用。每次切换导航工具，导航工具参考阵列坐标系ftoolrf至机械臂末端法兰盘坐标系fflange的转换矩阵ttoolrftoflange就会改变，为了获得准确的ttoolrftoflange用于导航，需要重新进行机械臂配准。而现有机械臂配准技术都需要机械臂带着导航工具进行若干次移动，采集相应位姿数据以求解配准矩阵。导致每次切换工具后的操作十分繁琐。为此，我们提出一种机械臂配准与导航方法。

技术实现思路

1、基于背景技术存在的技术问题，本发明提出了一种机械臂配准与导航方法，只需要在最初进行一次机械臂配准，每次更换末端工具后都可以直接进行机械臂导航定位无需进行额外的机械臂配准步骤，简化了操作步骤，解决了现有技术在每次更换执行工具后，都需要重新进行机械臂配准，导致每次切换工具后的操作十分繁琐的问题。

2、本发明提供如下技术方案：一种机械臂配准与导航方法，包括如下步骤：

3、s1、机械臂配准，在机械臂末端法兰盘处固连一光学参考阵列，移动机械臂并采集数据，计算机械臂配准矩阵；

4、获得定位相机坐标系fcamera至机械臂基座坐标系fbase的旋转矩阵rcb；

5、s2、机械臂末端换上经过标定的导航工具，进行切换工具后的机械臂导航，标定的导航工具中工具参考阵列坐标系ftoolrf至执行工具坐标系ftool的转换矩阵已知；

6、计算机械臂末端法兰盘坐标系fflange至当前工具参考阵列坐标系ftoolrf的旋转矩阵，计算工具参考阵列位于目标位置时，基座坐标系fbase至末端法兰盘坐标系fflange的旋转矩阵，并与当前机械臂基座坐标系fbase至机械臂末端法兰盘坐标系fflange的平移矩阵tbf_a结合，指挥机械臂运动至相应位置；

7、计算当前工具参考阵列坐标系ftoolrf至工具参考阵列坐标系目标位置f’toolrf的转换矩阵，得到当前工具参考阵列坐标系ftoolrf至工具参考阵列坐标系目标位置f’toolrf的平移矩阵；

8、基于基座坐标系fbase至末端法兰盘坐标系fflange的旋转矩阵、机械臂末端法兰盘坐标系fflange至当前工具参考阵列坐标系ftoolrf的旋转矩阵和当前工具参考阵列坐标系ftoolrf至工具参考阵列坐标系目标位置f’toolrf的平移矩阵得到机械臂末端法兰盘在fbase下需要进行的额外平移tn；

9、基于机械臂基座坐标系fbase至机械臂末端法兰盘坐标系fflange的转换矩阵与平移矩阵tn，控制机械臂运动至相应位置。

10、优选的，步骤s1中移动机械臂并采集数据的具体过程如下：

11、在机械臂末端法兰盘处固连一光学参考阵列，其坐标系为frobotendrf；

12、初始状态下，记录机械臂基座坐标系fbase至机械臂末端法兰盘坐标系fflange的转换矩阵为{rbf_1,tbf_1}，记录定位相机坐标系fcamera至参考阵列坐标系frobotendrf的转换矩阵为{rcr_1,tcr_1}；

13、保持机械臂末端法兰盘在机械臂基座下的姿态不变，控制机械臂末端平移到n处位置，其中n≥2，在每一个位置，记录机械臂基座坐标系fbase至机械臂末端法兰盘坐标系fflange的转换矩阵为{rbf_i,tbf_i}，记录定位相机坐标系fcamera至参考阵列坐标系的转换矩阵为{rcr_i,tcr_i}，其中i＝2,3…n。

14、优选的，步骤s2中计算机械臂旋转矩阵的具体过程如下：

15、记f’toolrf为工具参考阵列坐标系的目标位置，fcamera至f’toolrf的转换矩阵为给定的{rcr_target,tcr_target}，ftoolrf为工具参考阵列坐标系当前所处位置；

16、计算fflange至ftoolrf的旋转矩阵rfr：在当前时刻，从机械臂获得fbase至fflange的转换矩阵为{rbf_a，tbf_a}，从定位相机获得fcamera至ftoolrf的转换矩阵为{rcr_a，tcr_a}，则fflange至ftoolrf的旋转矩阵rfr计算为：

17、rfr＝(rbf_a)-1·(rcb)-1·rcr_a

18、计算当前ftoolrf至f’toolrf的旋转矩阵rs：

19、rs＝(rcr_a)-1·rcr_target

20、计算工具参考阵列位于目标位置f’toolrf时，fbase至fflange的旋转矩阵rbf_target：

21、rbf_target＝rbf_a·rfr·rs·(rfr)-1。

22、优选的，所述步骤s2中应用机械臂旋转矩阵的具体过程如下：

23、使用当前fbase至fflange的平移矩阵tbf_a与上一步计算所得的旋转矩阵rbf_target构成一新的转换矩阵{rbf_target，tbf_a}，将该转换矩阵作为新的fbase至fflange的转换矩阵tbasetoflange发送给机械臂，指挥机械臂运动至相应位置；

24、至此fcamera至ftoolrf的旋转矩阵已经被调整为rcr_target。

25、优选的，所述步骤s2中计算机械臂平移矩阵的具体过程如下：

26、机械臂完成上一步的运动后，fbase至fflange的转换矩阵为{rbf_target，tbf_a}，fcamera至ftoolrf的旋转矩阵已经被调整为rcr_target

27、从定位相机获得fcamera至ftoolrf的转换矩阵为{rcr_b，tcr_b}，其中rcr_b＝rcr_target

28、计算当前ftoolrf至f’toolrf的转换矩阵{rm，tm}：

29、{rm，tm}＝{rcr_b，tcr_b]-1·{rcr_target，ter_target}

30、＝{(rcr_b)-1，-(rcr-b)-1tcr_b}·{rcr_target，tcr_target}

31、＝{(rcr_b)-1·rcr_target，(rcr_b)-1·tcr_target-(rcr_b)-1tcr_b}

32、＝{e，(rcr_target)-1·tcr_target-(rcr_target)-1tcr_b}

33、其中e为3x3单位矩阵，故当前ftoolrf至f’toolrf的平移矩阵tm：

34、tm＝(rcr_target)-1·tcr_target-(rcr_target)-1tcr_b

35、即为了到达目标位置，工具参考阵列需要在当前ftoolrf下，进行平移tm；

36、计算机械臂末端法兰盘在fbase下需要进行的额外平移tn：

37、tn＝rbf_target·rfr·tm

38、其中rbf_target为此时fbase至fflange的旋转矩阵，rfr为fflange至ftoolrf的旋转矩阵。

39、优选的，所述步骤s2中应用机械臂平移矩阵的具体过程如下：

40、利用当前fbase至fflange的转换矩阵{rbf_target，tbf_a}与上一步计算得到的平移矩阵tn构成一新的转换矩阵{rbf_target，tbf_a+tn}，将该转换矩阵作为新的fbase至fflange的转换矩阵tbasetoflange发送给机械臂，指挥机械臂运动至相应位置；

41、机械臂达到指定位置后，工具参考阵列即达到目标位置，此时fcamera至ftoolrf的转换矩阵满足给定的{rcr_target，tcr_target}。

42、本发明提供了一种机械臂配准与导航方法，只需要在最初进行一次机械臂配准，每次更换末端工具后都可以直接进行机械臂导航定位，即切换工具后的机械臂导航，无需进行额外的机械臂配准步骤，简化了操作步骤。

43、不同于传统的导航方案需要机械臂配准同时求解获得ftoolrf至fflange的转换矩阵ttoolrftoflange，以及fcamera至fbase的转换矩阵tcameratobase，本方案的导航方式只需要机械臂配准求解获得fcamera至fbase的旋转矩阵，从而简化了机械臂配准步骤：机械臂末端只需进行至少2次平移，就可以求得fcamera至fbase的旋转矩阵，完成机械臂配准。