CN113843792A - 一种手术机器人的手眼标定方法 - Google Patents

一种手术机器人的手眼标定方法 Download PDF

Info

Publication number
CN113843792A
CN113843792A CN202111114945.2A CN202111114945A CN113843792A CN 113843792 A CN113843792 A CN 113843792A CN 202111114945 A CN202111114945 A CN 202111114945A CN 113843792 A CN113843792 A CN 113843792A
Authority
CN
China
Prior art keywords
coordinate system
tcp
axis
optical position
tool
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202111114945.2A
Other languages
English (en)
Other versions
CN113843792B (zh
Inventor
汪阳
房鹤
万安
崔小飞
祝胜
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sichuan Fengzhun Robot Technology Co ltd
Original Assignee
Sichuan Fengzhun Robot Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Sichuan Fengzhun Robot Technology Co ltd filed Critical Sichuan Fengzhun Robot Technology Co ltd
Priority to CN202111114945.2A priority Critical patent/CN113843792B/zh
Publication of CN113843792A publication Critical patent/CN113843792A/zh
Application granted granted Critical
Publication of CN113843792B publication Critical patent/CN113843792B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Abstract

本发明提供一种手术机器人的手眼标定方法，包括：控制机械臂末端移动，形成Tcp1；保持机械臂末端姿态不变，控制机械臂末端沿Tcp1坐标系的x1轴正方向移动到达O2，形成Tcp2；再沿Tcp2坐标系的y2轴正方向移动到达O3，形成Tcp3；计算Tcp3坐标系到光学定位仪坐标系转换矩阵；计算Tool3坐标系到光学定位仪坐标系转换矩阵；获得Tool3坐标系到Tcp3坐标系转换矩阵。本发明通过直接测量坐标轴方向的方式求解旋转矩阵，通过拟合球的方式求解平移向量，操作简便，不需要机械臂大范围运动，求解过程不需要设定初值，求解精度受机械臂的运动精度影响较小，是一种操作简便精度高的手术机器人手眼标定方法。

Description

一种手术机器人的手眼标定方法
技术领域
本发明属于手术机器人导航定位技术领域，具体涉及一种手术机器人的手眼标定方法。
背景技术
手术机器人具有导航定位精度高、重复性好的优势，已被广泛应用于骨科、神经外科、口腔科等临床手术中。手术机器人导航定位原理为：光学定位仪通过对安装在机械臂末端的工具示踪器进行跟踪，通过工具示踪器与机械臂末端TCP坐标系的变换关系，得到机械臂末端的位姿，从而引导机械臂运动至规划的目标位姿。完成这一手术机器人导航定位过程，需要事先获得工具示踪器与机械臂末端TCP坐标系的变换关系，这一过程称为手眼标定。
传统的手眼标定方法为：在多个位置采集机械臂末端TCP和工具示踪器的位姿，然后采用迭代方法求解，得到工具示踪器与机械臂末端TCP坐标系的变换关系。此种方法具有以下三个明显缺陷：(1)迭代方法对初值敏感，如果初值偏差较大，会导致迭代结果明显错误；(2)为达到较高的标定精度，通常需要机械臂在空间大范围运动多个位置，标定过程算法复杂，需消耗较多时间；(3)标定计算过程使用到了机械臂自身的位姿读数，使得机械臂的定位误差对标定结果有较大影响，而手术机器人通常采用协作机械臂，定位精度较低。
发明内容
针对现有技术存在的缺陷，本发明提供一种手术机器人的手眼标定方法，可有效解决上述问题。
本发明采用的技术方案如下：
本发明提供一种手术机器人的手眼标定方法，包括以下步骤：
步骤S1，在机械臂末端固定安装工具示踪器；其中，所述工具示踪器包括刚性支架以及固定安装于所述刚性支架的4个不对称布置的定位球；4个定位球分别称为：1号定位球、2号定位球、3号定位球和4号定位球；
步骤S2，控制机械臂末端移动，进而带动所述工具示踪器同步移动，使所述工具示踪器位于光学定位仪的测量视野中央时停止移动，设此时机械臂末端移动至点O1，此时机械臂末端TCP坐标系记为Tcp1＝(x1,y1,z1,o1)，其中，x1,y1,z1,o1分别表示Tcp1坐标系的x轴、y轴、z轴和坐标原点；此时的工具示踪器坐标系记为Tool1；此时，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，记为
Figure BDA0003274941770000021
步骤S3，保持机械臂末端姿态不变，控制机械臂末端沿Tcp1坐标系的x1轴正方向移动设定长度到达点O2，此时机械臂末端TCP坐标系记为Tcp2＝(x2,y2,z2,o2)，其中，x2,y2,z2,o2分别表示Tcp2坐标系的x轴、y轴、z轴和坐标原点；此时的工具示踪器坐标系记为Tool2；此时，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，记为
Figure BDA0003274941770000022
步骤S4，保持机械臂末端姿态不变，控制机械臂末端沿Tcp2坐标系的y2轴正方向移动设定长度到达点O3，此时机械臂末端TCP坐标系记为Tcp3＝(x3,y3,z3,o3)，其中，x3,y3,z3,o3分别表示Tcp3坐标系的x轴、y轴、z轴和坐标原点；此时的工具示踪器坐标系记为Tool3；此时，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，记为
Figure BDA0003274941770000023
采用光学定位仪测量工具示踪器的2号定位球、3号定位球和4号定位球在光学定位仪坐标系下的坐标，分别记为
Figure BDA0003274941770000024
和
Figure BDA0003274941770000025
步骤S5，保持TCP3坐标系的原点固定不动，控制机械臂末端以原点O3为旋转中心点，分别旋转到6个不同的姿态，在每个姿态，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，分别记为：
Figure BDA0003274941770000031
Figure BDA0003274941770000032
和
Figure BDA0003274941770000033
其中：
Figure BDA0003274941770000034
代表旋转到第1个姿态时的坐标；
Figure BDA0003274941770000035
代表旋转到第2个姿态时的坐标；
Figure BDA0003274941770000036
代表旋转到第3个姿态时的坐标；
Figure BDA0003274941770000037
代表旋转到第4个姿态时的坐标；
Figure BDA0003274941770000038
代表旋转到第5个姿态时的坐标；
Figure BDA0003274941770000039
代表旋转到第6个姿态时的坐标；
步骤S6，计算Tcp3坐标系到光学定位仪坐标系的转换矩阵Tcp3TL；
步骤S6.1，计算得到Tcp3坐标系到光学定位仪坐标系的旋转矩阵Tcp3RL；
步骤S6.1.1，采用下式，获得Tcp3坐标系的x3轴在光学定位仪坐标系下的方向向量
Figure BDA00032749417700000310
Figure BDA00032749417700000311
其中：
Figure BDA00032749417700000312
和
Figure BDA00032749417700000313
分别为方向向量
Figure BDA00032749417700000314
在x轴、y轴和z轴方向的向量分量；
具体的，由于机械臂末端从点O1移动到点O2，再从点O2移动到点O3时，机械臂末端姿态始终不变，所以，坐标系Tcp1，Tcp2和Tcp3的x轴、y轴和z轴方向分别平行，因此，坐标系Tcp1的x1轴在光学定位仪坐标系下的方向向量
Figure BDA00032749417700000315
等于坐标系Tcp3的x3轴在光学定位仪坐标系下的方向向量
Figure BDA00032749417700000316
步骤S6.1.2，采用下式，获得Tcp3坐标系的y3轴在光学定位仪坐标系下的初始方向向量
Figure BDA0003274941770000041
Figure BDA0003274941770000042
具体的，由于机械臂末端从点O1移动到点O2，再从点O2移动到点O3时，机械臂末端姿态始终不变，所以，坐标系Tcp1，Tcp2和Tcp3的x轴、y轴和z轴方向分别平行，因此，坐标系Tcp2的y2轴在光学定位仪坐标系下的方向向量
Figure BDA0003274941770000043
等于坐标系Tcp3的y3轴在光学定位仪坐标系下的初始方向向量
Figure BDA0003274941770000044
步骤S6.1.3，采用下式，获得Tcp3坐标系的z3轴在光学定位仪坐标系下的方向向量
Figure BDA0003274941770000045
Figure BDA0003274941770000046
其中：
Figure BDA0003274941770000047
和
Figure BDA0003274941770000048
分别为方向向量
Figure BDA0003274941770000049
在x轴、y轴和z轴方向的向量分量；
步骤S6.1.4，采用下式，获得Tcp3坐标系的y3轴在光学定位仪坐标系下的修正后的方向向量
Figure BDA00032749417700000410
Figure BDA00032749417700000411
其中：
Figure BDA00032749417700000412
和
Figure BDA00032749417700000413
分别为方向向量
Figure BDA00032749417700000414
在x轴、y轴和z轴方向的向量分量；
步骤S6.1.5，建立矩阵
Figure BDA00032749417700000415
采用下式对其进行归一化处理，得到Tcp3坐标系到光学定位仪坐标系的旋转矩阵Tcp3RL：
Figure BDA0003274941770000051
其中：
Figure BDA0003274941770000052
和
Figure BDA0003274941770000053
分别为方向向量
Figure BDA0003274941770000054
在x轴、y轴和z轴方向的归一化后的向量分量；
Figure BDA0003274941770000055
和
Figure BDA0003274941770000056
分别为方向向量
Figure BDA0003274941770000057
在x轴、y轴和z轴方向的归一化后的向量分量；
Figure BDA0003274941770000058
和
Figure BDA0003274941770000059
分别为方向向量
Figure BDA00032749417700000510
在x轴、y轴和z轴方向的归一化后的向量分量；
步骤S6.2，计算
Figure BDA00032749417700000511
和
Figure BDA00032749417700000512
组成的球的球心，即是Tcp3坐标系的原点O3在光学定位仪坐标系下的坐标
Figure BDA00032749417700000513
步骤S6.3，由此得到Tcp3坐标系到光学定位仪坐标系的转换矩阵Tcp3TL：
Figure BDA00032749417700000514
步骤S7，根据步骤S4得到的1号定位球、2号定位球、3号定位球和4号定位球在光学定位仪坐标系下的坐标，计算得到Tool3坐标系到光学定位仪坐标系的转换矩阵Tool3TL；
步骤S8，采用下式，得到Tool3坐标系到Tcp3坐标系的转换矩阵Tool3TTcp3：
Tool3TTcp＝Tool3TL·(Tcp3TL)-1
由此实现手眼标定。
优选的，光学定位仪采用双目测量原理，测量得到工具示踪器上各个定位球在光学定位仪坐标系下的坐标。
优选的，步骤S5中，机械臂末端以原点O3为旋转中心点，分别旋转到6个不同的姿态，在每个姿态，工具示踪器的1号定位球均在光学定位仪的测量视野范围内。
优选的，步骤S6.2中，采用最小二乘法拟合
Figure BDA0003274941770000061
Figure BDA0003274941770000062
和
Figure BDA0003274941770000063
组成的球的球心，得到Tcp3坐标系的原点O3在光学定位仪坐标系下的坐标。
优选的，步骤S7具体为：
分别获得1号定位球、2号定位球、3号定位球和4号定位球在Tool3坐标系下的坐标；
结合1号定位球、2号定位球、3号定位球和4号定位球在Tool3坐标系下的坐标，以及在光学定位仪坐标系下的坐标，通过刚体配准理论，得到Tool3坐标系到光学定位仪坐标系的转换矩阵Tool3TL。
本发明提供的一种手术机器人的手眼标定方法具有以下优点：
本发明通过直接测量坐标轴方向的方式求解旋转矩阵，通过拟合球的方式求解平移向量，操作简便，不需要机械臂大范围运动，求解过程不需要设定初值，求解精度受机械臂的运动精度影响较小，是一种操作简便精度高的手术机器人手眼标定方法。
附图说明
图1为本发明提供的一种手术机器人的手眼标定方法的示意图；
图2为本发明提供的一种手术机器人的手眼标定方法的坐标系关系原理图；
图3为本发明提供的工具示踪器的示意图。
具体实施方式
为了使本发明所解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
本发明提供一种手术机器人的手眼标定方法，参考图1和图2，包括以下步骤：
步骤S1，在机械臂末端固定安装工具示踪器；其中，所述工具示踪器包括刚性支架以及固定安装于所述刚性支架的4个不对称布置的定位球；4个定位球分别称为：1号定位球、2号定位球、3号定位球和4号定位球；参考图3，1代表1号定位球，2代表2号定位球，3代表3号定位球，4代表4号定位球。
每个定位球在工具示踪器坐标系下的坐标已知。
步骤S2，控制机械臂末端移动，进而带动所述工具示踪器同步移动，使所述工具示踪器位于光学定位仪的测量视野中央时停止移动，设此时机械臂末端移动至点O1，此时机械臂末端TCP坐标系记为Tcp1＝(x1,y1,z1,o1)，其中，x1,y1,z1,o1分别表示Tcp1坐标系的x轴、y轴、z轴和坐标原点；此时的工具示踪器坐标系记为Tool1；此时，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，记为
Figure BDA0003274941770000071
作为一种具体实施例，光学定位仪采用双目测量原理，测量得到工具示踪器上定位球在光学定位仪坐标系下的坐标。
步骤S3，保持机械臂末端姿态不变，控制机械臂末端沿Tcp1坐标系的x1轴正方向移动设定长度到达点O2，例如，移动300mm，此时机械臂末端TCP坐标系记为Tcp2＝(x2,y2,z2,o2)，其中，x2,y2,z2,o2分别表示Tcp2坐标系的x轴、y轴、z轴和坐标原点；此时的工具示踪器坐标系记为Tool2；此时，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，记为
Figure BDA0003274941770000081
步骤S4，保持机械臂末端姿态不变，控制机械臂末端沿Tcp2坐标系的y2轴正方向移动设定长度到达点O3，此时机械臂末端TCP坐标系记为Tcp3＝(x3,y3,z3,o3)，其中，x3,y3,z3,o3分别表示Tcp3坐标系的x轴、y轴、z轴和坐标原点；此时的工具示踪器坐标系记为Tool3；此时，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，记为
Figure BDA0003274941770000082
采用光学定位仪测量工具示踪器的2号定位球、3号定位球和4号定位球在光学定位仪坐标系下的坐标，分别记为
Figure BDA0003274941770000083
和
Figure BDA0003274941770000084
步骤S5，保持TCP3坐标系的原点固定不动，控制机械臂末端以原点O3为旋转中心点，分别旋转到6个不同的姿态，在每个姿态，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，分别记为：
Figure BDA0003274941770000085
Figure BDA0003274941770000086
和
Figure BDA0003274941770000087
其中：
Figure BDA0003274941770000088
代表旋转到第1个姿态时的坐标；
Figure BDA0003274941770000089
代表旋转到第2个姿态时的坐标；
Figure BDA00032749417700000810
代表旋转到第3个姿态时的坐标；
Figure BDA00032749417700000811
代表旋转到第4个姿态时的坐标；
Figure BDA00032749417700000812
代表旋转到第5个姿态时的坐标；
Figure BDA00032749417700000813
代表旋转到第6个姿态时的坐标；
本步骤中，机械臂末端以原点O3为旋转中心点，分别旋转到6个不同的姿态，在每个姿态，工具示踪器的1号定位球均在光学定位仪的测量视野范围内。
步骤S6，计算Tcp3坐标系到光学定位仪坐标系的转换矩阵Tcp3TL；
步骤S6.1，计算得到Tcp3坐标系到光学定位仪坐标系的旋转矩阵Tcp3RL；
步骤S6.1.1，采用下式，获得Tcp3坐标系的x3轴在光学定位仪坐标系下的方向向量
Figure BDA0003274941770000091
Figure BDA0003274941770000092
其中：
Figure BDA0003274941770000093
和
Figure BDA0003274941770000094
分别为方向向量
Figure BDA0003274941770000095
在x轴、y轴和z轴方向的向量分量；
具体的，由于机械臂末端从点O1移动到点O2，再从点O2移动到点O3时，机械臂末端姿态始终不变，所以，坐标系Tcp1，Tcp2和Tcp3的x轴、y轴和z轴方向分别平行，因此，坐标系Tcp1的x1轴在光学定位仪坐标系下的方向向量
Figure BDA0003274941770000096
等于坐标系Tcp3的x3轴在光学定位仪坐标系下的方向向量
Figure BDA0003274941770000097
步骤S6.1.2，采用下式，获得Tcp3坐标系的y3轴在光学定位仪坐标系下的初始方向向量
Figure BDA0003274941770000098
Figure BDA0003274941770000099
具体的，由于机械臂末端从点O1移动到点O2，再从点O2移动到点O3时，机械臂末端姿态始终不变，所以，坐标系Tcp1，Tcp2和Tcp3的x轴、y轴和z轴方向分别平行，因此，坐标系Tcp2的y2轴在光学定位仪坐标系下的方向向量
Figure BDA00032749417700000910
等于坐标系Tcp3的y3轴在光学定位仪坐标系下的初始方向向量
Figure BDA00032749417700000911
步骤S6.1.3，采用下式，获得Tcp3坐标系的z3轴在光学定位仪坐标系下的方向向量
Figure BDA00032749417700000912
Figure BDA00032749417700000913
其中：
Figure BDA0003274941770000101
和
Figure BDA0003274941770000102
分别为方向向量
Figure BDA0003274941770000103
在x轴、y轴和z轴方向的向量分量；
本步骤中，通过对方向向量
Figure BDA0003274941770000104
和方向向量
Figure BDA0003274941770000105
进行叉乘运算，得到与方向向量
Figure BDA0003274941770000106
和方向向量
Figure BDA0003274941770000107
所在平面垂直的方向向量，即方向向量
Figure BDA0003274941770000108
步骤S6.1.4，采用下式，获得Tcp3坐标系的y3轴在光学定位仪坐标系下的修正后的方向向量
Figure BDA0003274941770000109
Figure BDA00032749417700001010
其中：
Figure BDA00032749417700001011
和
Figure BDA00032749417700001012
分别为方向向量
Figure BDA00032749417700001013
在x轴、y轴和z轴方向的向量分量；
步骤S6.1.3-步骤S6.1.4的原理为：由于计算中存在误差，因此，方向向量
Figure BDA00032749417700001014
和初始方向向量
Figure BDA00032749417700001015
并不一定相互垂直，因此，首先通过方向向量
Figure BDA00032749417700001016
和方向向量
Figure BDA00032749417700001017
进行叉乘运算，得到方向向量
Figure BDA00032749417700001018
本步骤可保证方向向量
Figure BDA00032749417700001019
和方向向量
Figure BDA00032749417700001020
相互垂直。然后，使方向向量
Figure BDA00032749417700001021
和方向向量
Figure BDA00032749417700001022
进行叉乘运算，得到方向向量
Figure BDA00032749417700001023
通过此操作，可保证方向向量
Figure BDA00032749417700001024
和
Figure BDA00032749417700001025
两两互相垂直。
步骤S6.1.5，建立矩阵
Figure BDA00032749417700001026
采用下式对其进行归一化处理，得到Tcp3坐标系到光学定位仪坐标系的旋转矩阵Tcp3RL：
Figure BDA00032749417700001027
其中：
Figure BDA00032749417700001028
和
Figure BDA00032749417700001029
分别为方向向量
Figure BDA00032749417700001030
在x轴、y轴和z轴方向的归一化后的向量分量；
Figure BDA0003274941770000111
和
Figure BDA0003274941770000112
分别为方向向量
Figure BDA0003274941770000113
在x轴、y轴和z轴方向的归一化后的向量分量；
Figure BDA0003274941770000114
和
Figure BDA0003274941770000115
分别为方向向量
Figure BDA0003274941770000116
在x轴、y轴和z轴方向的归一化后的向量分量；
Tcp3RL为三行三列矩阵，每一列分别是Tcp3坐标系的x轴、y轴和z轴在光学定位仪坐标系下的方向向量，则Tcp3RL就是Tcp3坐标系到光学定位仪坐标系的旋转矩阵。
步骤S6.2，计算
Figure BDA0003274941770000117
和
Figure BDA0003274941770000118
组成的球的球心，即是Tcp3坐标系的原点O3在光学定位仪坐标系下的坐标
Figure BDA0003274941770000119
作为一种具体实施例，采用最小二乘法拟合
Figure BDA00032749417700001110
Figure BDA00032749417700001111
和
Figure BDA00032749417700001112
组成的球的球心，得到Tcp3坐标系的原点O3在光学定位仪坐标系下的坐标。
步骤S6.3，由此得到Tcp3坐标系到光学定位仪坐标系的转换矩阵Tcp3TL：
Figure BDA00032749417700001113
步骤S7，根据步骤S4得到的1号定位球、2号定位球、3号定位球和4号定位球在光学定位仪坐标系下的坐标，计算得到Tool3坐标系到光学定位仪坐标系的转换矩阵Tool3TL；
步骤S7具体为：
分别获得1号定位球、2号定位球、3号定位球和4号定位球在Tool3坐标系下的坐标；
结合1号定位球、2号定位球、3号定位球和4号定位球在Tool3坐标系下的坐标，以及在光学定位仪坐标系下的坐标，通过刚体配准理论，得到Tool3坐标系到光学定位仪坐标系的转换矩阵Tool3TL。
其中，刚体配准理论是求解坐标系转换关系的一种方法，以若干个点在两个坐标系中对应的坐标为已知条件，通过奇异值分解法或者迭代解法求解两个坐标系之间的转换矩阵。
步骤S8，采用下式，得到Tool3坐标系到Tcp3坐标系的转换矩阵Tool3TTcp3：
Tool3TTcp＝Tool3TL·(Tcp3TL)-1
由此实现手眼标定。
具体的，由于工具示踪器是固定安装在机械臂末端的，工具示踪器坐标系与TCP坐标系的相对位姿关系始终不变，所以Tool3TTcp3即是Tool3坐标系到Tcp3坐标系的转换矩阵。
本发明提供的一种手术机器人的手眼标定方法，通过直接测量坐标轴方向的方式求解旋转矩阵，通过拟合球的方式求解平移向量，操作简便，不需要机械臂大范围运动，求解过程不需要设定初值，求解精度受机械臂的运动精度影响较小，是一种操作简便精度高的手术机器人手眼标定方法。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视本发明的保护范围。

Claims (5)

1.一种手术机器人的手眼标定方法，其特征在于，包括以下步骤：
步骤S1，在机械臂末端固定安装工具示踪器；其中，所述工具示踪器包括刚性支架以及固定安装于所述刚性支架的4个不对称布置的定位球；4个定位球分别称为：1号定位球、2号定位球、3号定位球和4号定位球；
步骤S2，控制机械臂末端移动，进而带动所述工具示踪器同步移动，使所述工具示踪器位于光学定位仪的测量视野中央时停止移动，设此时机械臂末端移动至点O1，此时机械臂末端TCP坐标系记为Tcp1＝(x1,y1,z1,o1)，其中，x1,y1,z1,o1分别表示Tcp1坐标系的x轴、y轴、z轴和坐标原点；此时的工具示踪器坐标系记为Tool1；此时，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，记为
Figure FDA0003274941760000011
步骤S3，保持机械臂末端姿态不变，控制机械臂末端沿Tcp1坐标系的x1轴正方向移动设定长度到达点O2，此时机械臂末端TCP坐标系记为Tcp2＝(x2,y2,z2,o2)，其中，x2,y2,z2,o2分别表示Tcp2坐标系的x轴、y轴、z轴和坐标原点；此时的工具示踪器坐标系记为Tool2；此时，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，记为
Figure FDA0003274941760000012
步骤S4，保持机械臂末端姿态不变，控制机械臂末端沿Tcp2坐标系的y2轴正方向移动设定长度到达点O3，此时机械臂末端TCP坐标系记为Tcp3＝(x3,y3,z3,o3)，其中，x3,y3,z3,o3分别表示Tcp3坐标系的x轴、y轴、z轴和坐标原点；此时的工具示踪器坐标系记为Tool3；此时，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，记为
Figure FDA0003274941760000013
采用光学定位仪测量工具示踪器的2号定位球、3号定位球和4号定位球在光学定位仪坐标系下的坐标，分别记为
Figure FDA0003274941760000014
和
Figure FDA0003274941760000015
步骤S5，保持TCP3坐标系的原点固定不动，控制机械臂末端以原点O3为旋转中心点，分别旋转到6个不同的姿态，在每个姿态，采用光学定位仪测量工具示踪器的1号定位球在光学定位仪坐标系下的坐标，分别记为：
Figure FDA0003274941760000021
Figure FDA0003274941760000022
和
Figure FDA0003274941760000023
其中：
Figure FDA0003274941760000024
代表旋转到第1个姿态时的坐标；
Figure FDA0003274941760000025
代表旋转到第2个姿态时的坐标；
Figure FDA0003274941760000026
代表旋转到第3个姿态时的坐标；
Figure FDA0003274941760000027
代表旋转到第4个姿态时的坐标；
Figure FDA0003274941760000028
代表旋转到第5个姿态时的坐标；
Figure FDA0003274941760000029
代表旋转到第6个姿态时的坐标；
步骤S6，计算Tcp3坐标系到光学定位仪坐标系的转换矩阵Tcp3TL；
步骤S6.1，计算得到Tcp3坐标系到光学定位仪坐标系的旋转矩阵Tcp3RL；
步骤S6.1.1，采用下式，获得Tcp3坐标系的x3轴在光学定位仪坐标系下的方向向量
Figure FDA00032749417600000210
Figure FDA00032749417600000211
其中：
Figure FDA00032749417600000212
和
Figure FDA00032749417600000213
分别为方向向量
Figure FDA00032749417600000214
在x轴、y轴和z轴方向的向量分量；
具体的，由于机械臂末端从点O1移动到点O2，再从点O2移动到点O3时，机械臂末端姿态始终不变，所以，坐标系Tcp1，Tcp2和Tcp3的x轴、y轴和z轴方向分别平行，因此，坐标系Tcp1的x1轴在光学定位仪坐标系下的方向向量
Figure FDA00032749417600000215
等于坐标系Tcp3的x3轴在光学定位仪坐标系下的方向向量
Figure FDA00032749417600000216
步骤S6.1.2，采用下式，获得Tcp3坐标系的y3轴在光学定位仪坐标系下的初始方向向量
Figure FDA0003274941760000031
Figure FDA0003274941760000032
具体的，由于机械臂末端从点O1移动到点O2，再从点O2移动到点O3时，机械臂末端姿态始终不变，所以，坐标系Tcp1，Tcp2和Tcp3的x轴、y轴和z轴方向分别平行，因此，坐标系Tcp2的y2轴在光学定位仪坐标系下的方向向量
Figure FDA0003274941760000033
等于坐标系Tcp3的y3轴在光学定位仪坐标系下的初始方向向量
Figure FDA0003274941760000034
步骤S6.1.3，采用下式，获得Tcp3坐标系的z3轴在光学定位仪坐标系下的方向向量
Figure FDA0003274941760000035
Figure FDA0003274941760000036
其中：
Figure FDA0003274941760000037
和
Figure FDA0003274941760000038
分别为方向向量
Figure FDA0003274941760000039
在x轴、y轴和z轴方向的向量分量；
步骤S6.1.4，采用下式，获得Tcp3坐标系的y3轴在光学定位仪坐标系下的修正后的方向向量
Figure FDA00032749417600000310
Figure FDA00032749417600000311
其中：
Figure FDA00032749417600000312
和
Figure FDA00032749417600000313
分别为方向向量
Figure FDA00032749417600000314
在x轴、y轴和z轴方向的向量分量；
步骤S6.1.5，建立矩阵
Figure FDA00032749417600000315
采用下式对其进行归一化处理，得到Tcp3坐标系到光学定位仪坐标系的旋转矩阵Tcp3RL：
Figure FDA0003274941760000041
其中：
Figure FDA0003274941760000042
和
Figure FDA0003274941760000043
分别为方向向量
Figure FDA0003274941760000044
在x轴、y轴和z轴方向的归一化后的向量分量；
Figure FDA0003274941760000045
和
Figure FDA0003274941760000046
分别为方向向量
Figure FDA0003274941760000047
在x轴、y轴和z轴方向的归一化后的向量分量；
Figure FDA0003274941760000048
和
Figure FDA0003274941760000049
分别为方向向量
Figure FDA00032749417600000410
在x轴、y轴和z轴方向的归一化后的向量分量；
步骤S6.2，计算
Figure FDA00032749417600000411
和
Figure FDA00032749417600000412
组成的球的球心，即是Tcp3坐标系的原点O3在光学定位仪坐标系下的坐标
Figure FDA00032749417600000413
步骤S6.3，由此得到Tcp3坐标系到光学定位仪坐标系的转换矩阵Tcp3TL：
Figure FDA00032749417600000414
步骤S7，根据步骤S4得到的1号定位球、2号定位球、3号定位球和4号定位球在光学定位仪坐标系下的坐标，计算得到Tool3坐标系到光学定位仪坐标系的转换矩阵Tool3TL；
步骤S8，采用下式，得到Tool3坐标系到Tcp3坐标系的转换矩阵Tool3TTcp3：
Figure FDA00032749417600000415
由此实现手眼标定。
2.根据权利要求1所述的一种手术机器人的手眼标定方法，其特征在于，光学定位仪采用双目测量原理，测量得到工具示踪器上各个定位球在光学定位仪坐标系下的坐标。
3.根据权利要求1所述的一种手术机器人的手眼标定方法，其特征在于，步骤S5中，机械臂末端以原点O3为旋转中心点，分别旋转到6个不同的姿态，在每个姿态，工具示踪器的1号定位球均在光学定位仪的测量视野范围内。
4.根据权利要求1所述的一种手术机器人的手眼标定方法，其特征在于，步骤S6.2中，采用最小二乘法拟合
Figure FDA0003274941760000051
和
Figure FDA0003274941760000052
组成的球的球心，得到Tcp3坐标系的原点O3在光学定位仪坐标系下的坐标。
5.根据权利要求1所述的一种手术机器人的手眼标定方法，其特征在于，步骤S7具体为：
分别获得1号定位球、2号定位球、3号定位球和4号定位球在Tool3坐标系下的坐标；
结合1号定位球、2号定位球、3号定位球和4号定位球在Tool3坐标系下的坐标，以及在光学定位仪坐标系下的坐标，通过刚体配准理论，得到Tool3坐标系到光学定位仪坐标系的转换矩阵Tool3TL。
CN202111114945.2A 2021-09-23 2021-09-23 一种手术机器人的手眼标定方法 Active CN113843792B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111114945.2A CN113843792B (zh) 2021-09-23 2021-09-23 一种手术机器人的手眼标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111114945.2A CN113843792B (zh) 2021-09-23 2021-09-23 一种手术机器人的手眼标定方法

Publications (2)

Publication Number Publication Date
CN113843792A true CN113843792A (zh) 2021-12-28
CN113843792B CN113843792B (zh) 2024-02-06

Family

ID=78978830

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111114945.2A Active CN113843792B (zh) 2021-09-23 2021-09-23 一种手术机器人的手眼标定方法

Country Status (1)

Country Link
CN (1) CN113843792B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114454167A (zh) * 2022-02-11 2022-05-10 四川锋准机器人科技有限公司 一种种植牙机器人末端夹持器几何尺寸的标定方法
CN115431278A (zh) * 2022-11-03 2022-12-06 杭州柳叶刀机器人有限公司 基于vtk特征点变换的机器人标定方法、系统及存储介质
WO2023134237A1 (zh) * 2022-01-14 2023-07-20 北京京东乾石科技有限公司 用于机器人的坐标系标定方法、装置、系统以及介质

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103209809A (zh) * 2010-05-14 2013-07-17 康耐视公司 用于机器视觉系统和机器人之间的稳健校准的系统和方法
CN109129465A (zh) * 2018-07-25 2019-01-04 广东奥普特科技股份有限公司 一种机器人手眼标定系统及其工作流程
CN109658460A (zh) * 2018-12-11 2019-04-19 北京无线电测量研究所 一种机械臂末端相机手眼标定方法和系统
CN109974584A (zh) * 2019-04-12 2019-07-05 山东大学 一种辅助激光截骨手术机器人的标定系统及标定方法
CN110193849A (zh) * 2018-02-27 2019-09-03 北京猎户星空科技有限公司 一种机器人手眼标定的方法及装置
WO2020024178A1 (zh) * 2018-08-01 2020-02-06 深圳配天智能技术研究院有限公司 一种手眼标定方法、系统及计算机存储介质
CN110977980A (zh) * 2019-12-17 2020-04-10 上海嘉奥信息科技发展有限公司 基于光学定位仪的机械臂实时手眼标定方法及系统
CN111633643A (zh) * 2020-05-07 2020-09-08 武汉理工大学 一种基于tcp坐标系下平移运动的旋转矩阵标定方法
CN111823233A (zh) * 2020-06-30 2020-10-27 浙江德尚韵兴医疗科技有限公司 基于高精度立体光学定位的机械臂手眼标定系统及方法
WO2021012124A1 (zh) * 2019-07-19 2021-01-28 西门子（中国）有限公司 机器人手眼标定方法、装置、计算设备、介质以及产品
CN112318506A (zh) * 2020-10-28 2021-02-05 上海交通大学医学院附属第九人民医院 机械臂自动标定方法、装置、设备、机械臂和介质
CN113386136A (zh) * 2021-06-30 2021-09-14 华中科技大学 一种基于标准球阵目标估计的机器人位姿矫正方法及系统

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103209809A (zh) * 2010-05-14 2013-07-17 康耐视公司 用于机器视觉系统和机器人之间的稳健校准的系统和方法
CN110193849A (zh) * 2018-02-27 2019-09-03 北京猎户星空科技有限公司 一种机器人手眼标定的方法及装置
CN109129465A (zh) * 2018-07-25 2019-01-04 广东奥普特科技股份有限公司 一种机器人手眼标定系统及其工作流程
WO2020024178A1 (zh) * 2018-08-01 2020-02-06 深圳配天智能技术研究院有限公司 一种手眼标定方法、系统及计算机存储介质
CN109658460A (zh) * 2018-12-11 2019-04-19 北京无线电测量研究所 一种机械臂末端相机手眼标定方法和系统
CN109974584A (zh) * 2019-04-12 2019-07-05 山东大学 一种辅助激光截骨手术机器人的标定系统及标定方法
WO2021012124A1 (zh) * 2019-07-19 2021-01-28 西门子（中国）有限公司 机器人手眼标定方法、装置、计算设备、介质以及产品
CN110977980A (zh) * 2019-12-17 2020-04-10 上海嘉奥信息科技发展有限公司 基于光学定位仪的机械臂实时手眼标定方法及系统
CN111633643A (zh) * 2020-05-07 2020-09-08 武汉理工大学 一种基于tcp坐标系下平移运动的旋转矩阵标定方法
CN111823233A (zh) * 2020-06-30 2020-10-27 浙江德尚韵兴医疗科技有限公司 基于高精度立体光学定位的机械臂手眼标定系统及方法
CN112318506A (zh) * 2020-10-28 2021-02-05 上海交通大学医学院附属第九人民医院 机械臂自动标定方法、装置、设备、机械臂和介质
CN113386136A (zh) * 2021-06-30 2021-09-14 华中科技大学 一种基于标准球阵目标估计的机器人位姿矫正方法及系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
吕睿: "基于重定位的叶片机器人磨抛系统手眼标定算法", 《中国机械工程》 *
桂海军;张诗雷;沈国芳;栾楠;林艳萍;徐兵;: "光学导航定位多自由度机械臂辅助颅颌面骨畸形整复技术方法的建立及初步应用", 中国口腔颌面外科杂志, no. 03 *
段星光;高亮;李建玺;王永贵;李浩源;郭艳君;: "颅颌面外科手术机器人空间配准及实验", 机器人, no. 01 *
钟宇: "基于目标检测的机器人手眼标定方法", 《计算机工程》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023134237A1 (zh) * 2022-01-14 2023-07-20 北京京东乾石科技有限公司 用于机器人的坐标系标定方法、装置、系统以及介质
CN114454167A (zh) * 2022-02-11 2022-05-10 四川锋准机器人科技有限公司 一种种植牙机器人末端夹持器几何尺寸的标定方法
CN115431278A (zh) * 2022-11-03 2022-12-06 杭州柳叶刀机器人有限公司 基于vtk特征点变换的机器人标定方法、系统及存储介质
CN115431278B (zh) * 2022-11-03 2023-02-14 杭州柳叶刀机器人有限公司 基于vtk特征点变换的机器人标定方法、系统及存储介质

Also Published As

Publication number Publication date
CN113843792B (zh) 2024-02-06

Similar Documents

Publication Publication Date Title
CN107042528B (zh) 一种工业机器人的运动学标定系统及方法
CN113843792A (zh) 一种手术机器人的手眼标定方法
CN108324373B (zh) 一种基于电磁定位系统的穿刺手术机器人精确定位实现方法
RU2466858C1 (ru) Способ контроля точности контурных перемещений промышленных роботов
CN109238199B (zh) 一种机器人旋转轴运动学参数标定方法
CN109227551B (zh) 一种视觉定位机器人手眼坐标转换方法
CN111409067B (zh) 一种机器人用户坐标系自动标定系统及其标定方法
CN110614635B (zh) 一种scara机器人运动学参数的辨识方法
CN114216456B (zh) 一种基于imu与机器人本体参数融合的姿态测量方法
Liu et al. Binocular-vision-based error detection system and identification method for PIGEs of rotary axis in five-axis machine tool
CN111360812B (zh) 一种基于相机视觉的工业机器人dh参数标定方法及标定装置
CN113211445B (zh) 一种机器人参数标定方法、装置、设备及存储介质
CN112304218B (zh) 工业机器人的工具中心点位置标定方法及系统
Xiao et al. A new fixed axis-invariant based calibration approach to improve absolute positioning accuracy of manipulators
Santolaria et al. Self-alignment of on-board measurement sensors for robot kinematic calibration
CN112603542A (zh) 手眼标定方法、装置、电子设备和存储介质
JP2011224672A (ja) ロボットのツールベクトルの導出方法及び較正方法
CN110900608B (zh) 基于最优测量构型选择的机器人运动学标定方法
Traslosheros et al. A method for kinematic calibration of a parallel robot by using one camera in hand and a spherical object
CN112549018A (zh) 一种机器人线激光快速手眼标定方法
CN115139338B (zh) 一种机器人tcp快速高精度标定方法
CN113878586B (zh) 机器人运动学标定装置、方法及系统
CN112762822B (zh) 一种基于激光跟踪仪的机械臂校准方法及系统
CN110125982B (zh) 微操作机器人三自由度运动控制系统运动轨迹正交性测量方法
CN113720261B (zh) 一种基于平面的标靶球头标定方法、系统及应用

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant