穿刺手术机器人的运动学建模方法及设备与流程

本发明涉及医疗器械领域，具体涉及一种穿刺手术机器人的运动学建模方法及设备。

背景技术：

1、中国专利文件cn114469286b公开了一种小型化穿刺机器人，该机器人用于安装穿刺针，在光学定位系统的辅助下自动执行穿刺手术。在投入到实际应用场景时，需要在穿刺机器人的某些部位安装定位标志物，以使定位系统能够捕捉到定位标志物的空间位置，结合定位标志物与穿刺针末端针尖位置的关系，确定末端针尖位置，此过程涉及坐标系转换操作，需要获得相应的坐标系转换矩阵。

2、通过坐标系转换矩阵得到末端针尖位置的方式不止一种，如图1所示，比如可以通过计算从光学定位坐标系o0到末端针尖位置坐标系o4的转换矩阵t04，进而确定针尖位置，这里的末端针尖位置坐标系o4是由末端视觉定位坐标系推导出的末端针尖位置坐标系，所以必须在靠近穿刺针的位置设置定位标志物，具体可以在末端处(比如cn114469286b记载的直线导轨或者连接关节)设置定位标志物，图1中的两个竖直排列的球状物。

3、然而对于小型化穿刺机器人而言，末端处可设置定位标志物的空间十分有限，并且在手术过程中机器人末端需要转动、摆动等，在末端处设置的定位标记物可能会影响到手术过程，所以就需要在远离末端处设置定位标记物，进而获得相应的坐标系转换矩阵。

4、对于cn114469286b公开的小型化穿刺机器人，将定位标志物设置在顶板上是一种可行的方式，图1中三个布置在同一平面上的球状物，相应的坐标系是图1所示的基座视觉定位坐标系o1。从光学定位坐标系o0到末端针尖位置坐标系o5的转换矩阵t05是无法测量得到的，需要设计合适的正向运动学算法计算得到。

技术实现思路

1、有鉴于此，本发明提供一种穿刺手术机器人的运动学建模方法，包括：

2、获取机器人结构参数，包括关键部件的尺寸信息和关键点的位置信息；

3、根据所述结构参数确定机器人的上层末端关键点在基坐标系中的投影位置信息；

4、根据所述投影位置信息、上层末端关键点的三维位置信息、下层末端关键点的三维位置信息计算上层末端关键点与下层末端关键点之间连线的关键角度信息；

5、根据所述关键角度信息、所述尺寸信息和所述三维位置信息计算从基座坐标系到由基座视觉定位坐标系推导出的末端针尖位置坐标系的转换矩阵t25。

6、可选地，所述结构参数具体包括上下层分布的两个驱动组件的关键尺寸、末端穿刺针固定部的关键尺寸，以及基于所述关键尺寸和相应平面坐标系所确定的关键点的位置信息和关键点之间连线的夹角；所述两个末端关键点为上层驱动组件与末端的连接关节和下层驱动组件与末端的连接关节；

7、根据所述结构参数确定机器人的上层末端关键点在基坐标系中的投影位置信息，具体包括：

8、以下层驱动部件上的点为原点建立坐标系ol0，坐标系ol0与基座坐标系重合，获得下层连接关节fl在坐标系ol0中的二维位置信息(py_l,px_l)；

9、基于二维位置信息(py_l,px_l)获得下层连接关节fl在基座坐标系中的三维位置信息(py_l,px_l，0)；

10、以上层驱动部件上的点为原点建立坐标系oh0，获得上层连接关节fh在坐标系oh0中的二维位置信息(py_h,px_h)；

11、基于二维位置信息(py_h,px_h)获得上层连接关节fh在基座坐标系中的三维位置信息(py_l,px_l，len1)，其中len1为上层与下层之间的高度；

12、根据下层连接关节fl和上层连接关节fh的三维位置信息确定上层连接关节fh在基座坐标系的xoy平面的投影位置信息(qx，qy)。

13、可选地，坐标系ol0的原点为下层驱动机构的两个直线驱动机构的固定端al与bl的中点；坐标系oh0的原点为上层驱动机构的两个直线驱动机构的固定端ah与bh的中点。

14、可选地，计算上层末端关键点与下层末端关键点之间连线的关键角度信息具体包括：

15、根据下层连接关节fl和上层连接关节fh的三维位置信息计算下层连接关节fl与上层连接关节fh之间连线的长度len_1；

16、根据投影位置信息(qx，qy)和长度len_1计算关键角度信息。

17、可选地，采用如下方式计算关键角度信息θz和θx：

18、

19、

20、可选地，所述末端穿刺针固定部的关键尺寸包括关键尺寸包括指穿刺针与连接关节在水平方向的坐标轴上的距离len4和指固定穿刺针用的固定部件底端与下层连接关节在竖直方向的坐标轴上的距离len2。

21、可选地，采用如下方式计算转换矩阵t25：

22、t25＝trans(px_l,py_l,0)·rotez(θz)·rotex(θx)·trans(len4,0,-len2)，其中rotez和rotex表示欧拉角旋转转换，trans表示平移转换。

23、可选地，在得到转换矩阵t25之后还包括：

24、利用转换矩阵t25计算从光学定位坐标系o0到由基座视觉定位坐标系推导出的末端针尖位置坐标系o5的转换矩阵t05。

25、可选地，采用如下方式计算转换矩阵t05：

26、t05＝t01t12t25

27、其中t01是从光学定位系统获得从光学定位坐标系o0到基座视觉定位坐标系o1的转换矩阵；t12是通过三坐标仪确定的从基座视觉定位坐标系o1到基座坐标系o2的转换矩阵。

28、相应地，本发明还提供一种穿刺手术机器人的运动学建模设备，包括：处理器以及与所述处理器连接的存储器；其中，所述存储器存储有可被所述处理器执行的指令，所述指令被所述处理器执行，以使所述处理器执行上述穿刺手术机器人的运动学建模方法。

29、根据本发明提供的运动学建模方法及设备，基于穿刺手术机器人的已知结构参数，可以计算出上层末端关键点在基座坐标系中的二维投影位置，并结合上、下层末端关键点的三维位置，可以计算出上、下层末端关键点的连线在基座坐标系中的关键角度信息，利用角度信息和关键部件的尺寸信息以及关键点的三维位置信息即可计算出穿刺针针尖对于基座坐标系的坐标系关系即转换矩阵，此转换矩阵可以用于手术导航系统中的视觉定位系统根据基座上的定位标记物的空间位置直接计算出穿刺针针尖位置，不再需要在靠近穿刺针的末端处设置定位标记物。

技术特征：

1.一种穿刺手术机器人的运动学建模方法，其特征在于，包括：

2.根据权利要求1所述的运动学建模方法，其特征在于，所述结构参数具体包括上下层分布的两个驱动组件的关键尺寸、末端穿刺针固定部的关键尺寸，以及基于所述关键尺寸和相应平面坐标系所确定的关键点的位置信息和关键点之间连线的夹角；所述两个末端关键点为上层驱动组件与末端的连接关节和下层驱动组件与末端的连接关节；

3.根据权利要求2所述的方法，其特征在于，坐标系ol0的原点为下层驱动机构的两个直线驱动机构的固定端al与bl的中点；坐标系oh0的原点为上层驱动机构的两个直线驱动机构的固定端ah与bh的中点。

4.根据权利要求2所述的方法，其特征在于，计算上层末端关键点与下层末端关键点之间连线的关键角度信息具体包括：

5.根据权利要求4所述的方法，其特征在于，采用如下方式计算关键角度信息θz和θx：

6.根据权利要求5所述的方法，其特征在于，所述末端穿刺针固定部的关键尺寸包括关键尺寸包括指穿刺针与连接关节在水平方向的坐标轴上的距离len4和指固定穿刺针用的固定部件底端与下层连接关节在竖直方向的坐标轴上的距离len2。

7.根据权利要求6所述的方法，其特征在于，采用如下方式计算转换矩阵t25：

8.根据权利要求1-7中任一项所述的方法，其特征在于，在得到转换矩阵t25之后还包括：

9.根据权利要求8所述的方法，其特征在于，采用如下方式计算转换矩阵t05：

10.一种穿刺手术机器人的运动学建模设备，其特征在于，包括：处理器以及与所述处理器连接的存储器；其中，所述存储器存储有可被所述处理器执行的指令，所述指令被所述处理器执行，以使所述处理器执行如权利要求1-9中任意一项所述的穿刺手术机器人的运动学建模方法。

技术总结
本发明提供一种穿刺手术机器人的运动学建模方法及设备，所述方法包括获取机器人结构参数，包括关键部件的尺寸信息和关键点的位置信息；根据所述结构参数确定机器人的上层末端关键点在基坐标系中的投影位置信息；根据所述投影位置信息、上层末端关键点的三维位置信息、下层末端关键点的三维位置信息计算上层末端关键点与下层末端关键点之间连线的关键角度信息；根据所述关键角度信息、所述尺寸信息和所述三维位置信息计算从基座坐标系到由基座视觉定位坐标系推导出的末端针尖位置坐标系的转换矩阵。

技术研发人员：张昊任,史纪鹏,陈向前
受保护的技术使用者：真健康（珠海）医疗科技有限公司
技术研发日：
技术公布日：2024/1/15