CN111823233B - 基于高精度立体光学定位的机械臂手眼标定系统及方法 - Google Patents

Abstract

本发明属于医疗器械技术领域，具体涉及一种基于高精度立体光学定位系统的机械臂手眼标定方法及系统。包括刚体基座，刚体基座固定在机械臂末端；带有反光球的刚体通过螺钉固定在刚体基座上；圆盘中心有一个锥形孔洞，且和刚体基座末端匹配；NDI光学定位仪设于机械臂旁侧。方法步骤包括：一、组装系统；二、匀速使刚体基座绕凹槽作圆周运动；得到刚体固定装置到红外传感器的转移矩阵；三、以四种不同的姿态将刚体基座末端插入到圆盘中央的凹槽中，将刚体基座末端设置为新的TCP；步骤四、在示教模式下，记录下此时刚体基座末端在NDI光学定位仪系统下的坐标Pi和机械臂系统下坐标Qi；五、转化为D‑D姿态估计问题。本发明流程简单，精度高。

Description

基于高精度立体光学定位的机械臂手眼标定系统及方法

技术领域

本发明属于医疗器械技术领域，具体涉及一种基于高精度立体光学定位系统的机械臂手眼标定方法及系统。

背景技术

近年来，随着信息与工业技术的发展，医疗手术领域正不断地实现精细化、信息化。在外科手术领域中，机械臂越来越多地辅助甚至代替人手进行操作。相比人手，机械臂具有精度高、稳定性好等优点。为了实现机械臂的精准定位，需要配备一台高精度的光学定位系统。为了获取光学定位系统坐标系与机械臂坐标系之间的转换关系，我们需要对机械臂进行手眼标定(手指机械臂，眼指光学定位系统)。手眼标定的精度在很大程度上也影响了整个系统的准确性。经典的标定方案通过计算机械臂的正逆解和相机的内外参获得一组方程AX＝XB(其中A,B,X均为矩阵)。求解该方程可得手眼转换关系X。

目前方程AX＝XB的求解技术已经相当成熟，有众多的文献提供了各种不同的方法。这些方法大多要求较复杂的数学运算，给编程实现带来一定麻烦，且无法进一步提升精度。由于相机的内外参标定过程复杂，精度相对较低，在数据源上增加了误差。CN20180950623与US2010168915A1均提供了相应的利用NDI光学定位仪进行机械臂手眼标定的方法，但存在操作繁琐、算法实现复杂的缺陷。此外，在标定过程中，需要调整机械臂的姿态确保标定物位于相机视野当中，程序繁琐。基于以上几点，设计一种操作简单、编程便捷、精度较高的标定方法是必要的。

发明专利内容

本发明专利要解决的技术问题是，克服现有技术中的不足，提供基于高精度立体光学定位的机械臂手眼标定系统及方法。

为解决技术问题，本发明专利所采用的技术方案是：

提供一种基于高精度立体光学定位的机械臂手眼标定系统，包括刚体基座，NDI光学定位仪，带有反光球的刚体和圆盘；

刚体基座固定在机械臂末端，刚体基座的末端为圆锥形；带有反光球的刚体通过螺钉固定在刚体基座上；圆盘中心有一个锥形孔洞，且和刚体基座末端匹配；NDI光学定位仪设于机械臂旁侧。

作为一种改进，刚体基座通过带定位销的法兰盘固定在机械臂末端。

一种利用上述的一种基于高精度立体光学定位的机械臂手眼标定系统的方法，步骤为：

步骤一、将NDI光学定位仪和机械臂系统分别通过支架固定在地面上，两者保持适当距离；将NDI光学定位仪预热半小时，将带锥形凹槽的圆盘固定在桌上，位于NDI光学定位仪和机械臂系统之间，将带有反光球的刚体固定在刚体基座上；

步骤二、定制手持刚体基座的底端，将末端插入桌上圆盘的锥形凹槽并使其固定，匀速使刚体基座绕凹槽作圆周运动，期间保持反光球正对NDI光学定位仪；通过NDI光学定位仪中的NDI Track软件得到刚体固定装置到红外传感器的转移矩阵，从而可以在光学定位系统中读取到刚体基座末端在NDI光学定位仪坐标系下的坐标；

步骤三、将刚体基座装载在机械臂末端，以四种不同的姿态将刚体基座末端插入到圆盘中央的凹槽中，通过机械臂TCP配置程序将刚体基座末端设置为新的TCP(toolcenter position，即工具中心位置)；

步骤四、在示教模式下手动将机械臂移动到合适位置，使刚体上的反光球正对NDI光学定位仪。保持刚体基座的姿态不变，向机械臂发送驱动指令使刚体基座在小范围内上下、左右、前后随机移动，移动到第i个位置时，停留一秒并记录下此时刚体基座末端在NDI光学定位仪系统下的坐标Pi和机械臂系统下坐标Qi，采样完成后我们得到两组点列{P₁,P₂,...P_n}和{Q₁,Q₂,...Q_n},分别表示一系列样本点在光学定位系统和机械臂系统下的坐标；

步骤五、得到同一组点在两个坐标系下的坐标之后，机械臂的手眼标定问题转化为D-D姿态估计问题，最终完成机械臂手眼标定。

作为一种改进，步骤五包括以下几个子步骤：

(1)首先计算两组坐标的质心

(2)计算两组坐标去质心后的坐标点

P_i＝P_i-P，Q_i＝Q_i-Q；

(3)计算优化问题，得到旋转矩阵

(4)根据得到的R^*计算t^*

t^*＝P-R^*Q；

作为一种改进，子步骤(3)根据以下的方法求解优化问题：

首先定义矩阵

然后对W进行SVD分解，得到

W＝UΣV^T；

最终求得

R^*＝UV^T。

作为一种改进，步骤二中，NDI光学定位仪系统自带NDI Track软件，发行厂家NDI公司。

作为一种改进，步骤三中，四种不同的姿态为具体指保持机械臂末端的空间位置不变(即将刚体基座末端插在凹槽中)，改变机械臂各个关节角的大小使机械臂呈现不同的姿态。

与现有技术相比，本发明专利的有益效果在于：

1、流程简单。本发明主要流程包括三部分：刚体基座的标定(步骤二)、机械臂TCP的标定(步骤三)、数据采样及计算(步骤四)。其中步骤二、三需手动完成，但操作简单、省时。步骤四由程序自动实现。除去步骤一中的准备及预热过程，剩余流程耗时十五分钟。

2、精度高。本发明借助高精度立体光学定位系统和机械臂自带的TCP标定功能，直接获取同一组点在光学系统坐标系和机械臂坐标系下的坐标，避免了求解相机内外参和计算机械臂正逆解过程中引入的误差，从而提高了标定精度。

附图说明

图1为一种基于高精度立体红外光学定位仪的机械臂手眼标定系统的结构示意图。

图中，1-机械臂；2-带有反光球的刚体；3-刚体基座；4-圆盘；5-NDI光学定位仪。

具体实施方式

下文给出了一种具体实施方式。首先需要说明的是，本发明专利涉及医疗器械技术、视觉技术和定位技术。在发明专利的实现过程中，可能会涉及到多个硬件功能模块的应用。申请人认为，如在仔细阅读申请文件、准确理解本发明专利的实现原理和发明专利目的以后，在结合现有公知技术的情况下，本领域技术人员完全可以运用其掌握的技术实现本发明专利。凡本发明专利申请文件提及的均属范畴，申请人不一一列举。另外，本发明专利的实现依赖于多种软件的应用，而这些软件均为现有技术，且有成熟产品可市场购置获得。

请参考图1为一种基于高精度立体红外光学定位仪的机械臂1手眼标定系统的结构示意图，主要由机械臂1，带有反光球的刚体2，刚体基座3，圆盘4，NDI光学定位仪5组成。刚体基座3通过带定位销的法兰盘固定在机械臂1末端，刚体基座3基座的末端为圆锥形；带有反光球的刚体2通过螺钉固定在刚体基座3上；圆盘4中心有一个锥形孔洞，且和刚体基座3末端匹配；NDI光学定位仪5设于机械臂1旁侧。

一种利用一种基于高精度立体光学定位的机械臂手眼标定系统的标定方法，步骤为：

步骤一、将NDI光学定位仪5和机械臂1分别通过支架固定在地面上，两者保持适当距离。打NDI光学定位仪5预热半小时。将带锥形凹槽的圆盘4固定在桌上，位于NDI光学定位仪5和机械臂1之间。将带有反光球的刚体2固定在刚体基座3上。

步骤二、手持刚体基座3的底端，将末端插入桌上圆盘4的锥形凹槽并使其固定，匀速使刚体基座3绕凹槽作圆周运动，期间保持反光球正对NDI光学定位仪5。通过NDI光学定位仪5系统自带软件可以得到刚体固定装置到红外传感器的转移矩阵，从而可以在光学定位系统中读取到刚体基座3末端在NDI光学定位仪5坐标系下的坐标。

步骤三、将刚体基座3装载在机械臂1末端法兰盘上，以四种不同的姿态将刚体基座3末端插入到圆盘4中央的凹槽中。通过机械臂1自带程序可以将刚体基座3末端设置为新的TCP。

步骤四、在示教模式下手动将机械臂1移动到合适位置，使刚体2上的反光球正对NDI光学定位仪5。保持刚体基座3的姿态不变，使用程序驱动机械臂1在小范围内上下、左右、前后随机移动。移动到第i个位置时，停留一秒并记录下此时刚体基座3末端在NDI光学定位仪5系统下的坐标P_i和机械臂1下坐标Q_i。采样完成后我们得到两组点列{P₁,P₂,...P_n}和{Q₁,Q₂,...Q_n},分别表示一系列样本点在光学定位系统和机械臂1下的坐标。

步骤五、得到同一组点在两个坐标系下的坐标之后，机械臂1的手眼标定问题转化为3D-3D姿态估计问题。该问题可以由以下算法求解：

(1)首先计算两组坐标的质心

(2)计算两组坐标去质心后的坐标点P_i＝P_i-P，Q_i＝Q_i-Q；

(3)计算优化问题，得到旋转矩阵

(4)根据得到的R^*计算t^*

t^*＝P-R^*Q；

子步骤(3)根据以下的方法求解优化问题：首先定义矩阵

然后对进行SVD分解，得到

W＝UΣV^T；

最终求得

R^*＝UV^T。

至此，我们就完成了整个手眼标定过程。

Claims (3)

1.一种机械臂手眼标定的方法，其特征在于，该方法是利用下述基于高精度立体光学定位的机械臂手眼标定系统实现的：

该系统包括刚体基座、NDI光学定位仪、带有反光球的刚体和圆盘；刚体基座固定在机械臂末端，刚体基座的末端为圆锥形；带有反光球的刚体通过螺钉固定在刚体基座上；圆盘中心有一个锥形孔洞，且和刚体基座末端匹配；NDI光学定位仪设于机械臂旁侧；所述刚体基座通过带定位销的法兰盘固定在机械臂末端；

所述机械臂手眼标定的方法具体包括以下步骤：

步骤一、将NDI光学定位仪和机械臂系统分别通过支架固定在地面上，两者保持适当距离；将NDI光学定位仪预热半小时，将带锥形孔洞的圆盘固定在桌上，位于NDI光学定位仪和机械臂系统之间，将带有反光球的刚体固定在刚体基座上；

步骤二、定制手持刚体基座的底端，将末端插入桌上圆盘的锥形孔洞并使其固定，匀速使刚体基座绕凹槽作圆周运动，期间保持反光球正对NDI光学定位仪；通过NDI光学定位仪中的NDI Track软件得到刚体基座到NDI光学定位仪的转移矩阵，从而在光学定位系统中读取到刚体基座末端在NDI光学定位仪坐标系下的坐标；

步骤三、将刚体基座装载在机械臂末端，以四种不同的姿态将刚体基座末端插入到圆盘中央的凹槽中，通过机械臂TCP配置程序将刚体基座末端设置为新的TCP，即工具中心位置；

步骤四、在示教模式下手动将机械臂移动到合适位置，使刚体上的反光球正对NDI光学定位仪；保持刚体基座的姿态不变，向机械臂发送驱动指令使刚体基座在小范围内上下、左右、前后随机移动，移动到第i个位置时，停留一秒并记录下此时刚体基座末端在NDI光学定位系统下的坐标Pi和机械臂系统下坐标Qi，采样完成后得到两组点列{P1,P2,...Pn}和{Q1,Q2,...Qn},分别表示一系列样本点在光学定位系统和机械臂系统下的坐标；

步骤五、得到同一组点在两个坐标系下的坐标之后，机械臂的手眼标定问题转化为D-D姿态估计问题，最终完成机械臂手眼标定；

所述步骤五包括以下几个子步骤：

(1)首先计算两组坐标的质心

(2)计算两组坐标去质心后的坐标点

(3)计算优化问题，得到旋转矩阵

R^*为最小二乘问题的解；

(4)根据得到的R^*计算t^*

t^*＝P-R^*Q。

2.根据权利要求1所述的方法，其特征在于，所述步骤五的子步骤(3)中，根据以下的方法求解优化问题：

首先定义矩阵

然后对矩阵W进行SVD分解，得到

W＝U∑V^T；

U的列向量为矩阵MM^T的特征向量，V的列向量为矩阵M^TM的特征向量；

最终求得

R^*＝UV^T。

3.根据权利要求1所述的方法，其特征在于，步骤三中，四种不同的姿态为具体指保持机械臂末端的空间位置不变,即将刚体基座末端插在凹槽中，改变机械臂各个关节角的大小使机械臂呈现四种不同的姿态。

Images