CN113133832B - 一种双臂机器人穿刺系统标定方法及系统 - Google Patents

Abstract

本申请涉及一种双臂机器人穿刺系统标定方法及系统。包括：对双臂机器人穿刺系统进行运动学分析，得到所述双臂机器人穿刺系统的待标定参数；使用多点法对穿刺针的第一初始参数进行标定，并对所述穿刺针的针尖进行双平面超声成像，根据双平面超声成像对所述超声探头的第二初始参数进行标定；利用所述超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用所述S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，并通过轨迹拟合对所述第一平移参数、第二平移参数和旋转参数进行标定。本申请实施例能够高效地对双臂机器人穿刺系统的穿刺针和经直肠超声探头同时进行标定，且无需采用第三方跟踪设备。

Description

一种双臂机器人穿刺系统标定方法及系统

技术领域

本申请属于医学图像处理技术领域，特别涉及一种双臂机器人穿刺系统标定方法及系统。

背景技术

近年来，随着机器人技术和医学图像处理技术的高速发展，使得影像引导的机器人在医学领域取得了优秀的成果。特别是具有多自由度末端的双臂机器人穿刺系统，该系统不仅能分开控制超声探头与穿刺针，实现在狭小的手术空间内灵活地控制入针角度与姿态；而且能提供三维影像，实现高精度的影像定位与良好的视觉效果。

目前，多自由度末端的双臂机器人穿刺系统标定方案主要包括：

一、采用定位传感器和标定模板进行标定。该方法在机器人的左右末端法兰分别安装定位传感器，通过机器人手眼标定，完成定位传感器的标定；然后控制机器人以不同的姿态让超声探头扫描标定模板，通过定位传感器建立超声图像和标定模板之间的坐标变换等式，通过解最小二乘问题，得到超声图像坐标系至定位传感器坐标系间的坐标变换矩阵。

二、使用机器人操作和平面校准装置的超声探头标定方法。该方法通过使用机器人操作超声探头扫描平面校准装置，进而建立超声图像和对应平面校准装置实际物理位置之间标定的方程式组合，然后采用方程式求解出超声图像坐标系到机器人操作器坐标系之间的坐标变换矩阵。

然而，综上所述的标定方法通常面向零自由度末端的双臂机器人穿刺系统进行设计，难以高效地完成对多自由度末端的双臂机器人穿刺系统进行标定。同时，现有标定方法常需要借助定位传感器、相机、模型等第三方跟踪设备，这些设备本身的定位和尺寸具有一定误差，且成本较高。

发明内容

本申请提供了一种双臂机器人穿刺系统标定方法及系统，旨在至少在一定程度上解决现有技术中的上述技术问题之一。

为了解决上述问题，本申请提供了如下技术方案：

一种双臂机器人穿刺系统标定方法，包括：

对双臂机器人穿刺系统进行运动学分析，得到所述双臂机器人穿刺系统的待标定参数；所述待标定参数包括穿刺针的第一初始参数、第一平移参数，以及超声探头的第二初始参数、第二平移参数和旋转参数；

使用多点法对所述穿刺针的第一初始参数进行标定，并对所述穿刺针的针尖进行双平面超声成像，根据所述双平面超声成像对所述超声探头的第二初始参数进行标定；

利用所述超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用所述S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，并通过轨迹拟合对所述第一平移参数、第二平移参数和旋转参数进行标定。

本申请实施例采取的技术方案还包括：所述对双臂机器人穿刺系统进行运动学分析包括：

对所述穿刺针进行运动学分析；所述穿刺针绕自身旋转不改变自身的位置，所述穿刺针的位置^BP(d_n)由其初始位置^RP(0)和平移距离d_n决定，所述穿刺针的运动学等式为：

^BP(d_n)＝^BT_R·^RP(d_n)

^RP(d_n)＝^RP(0)+v_n·d_n

上式中，^BT_R为{R}到{B}的齐次坐标变换矩阵，{R}表示双臂机器人右臂的末端法兰坐标系，{B}为双臂机器人的基坐标系；^RP(d_n)为穿刺针平移d_n时，其针尖在{R}中的坐标；v_n为穿刺针平移时在{R}中的单位方向向量，即第一平移参数。

本申请实施例采取的技术方案还包括：所述对双臂机器人穿刺系统进行运动学分析包括：

对所述超声探头进行运动学分析；所述超声探头绕自身旋转时，其位置由其旋转角度θ决定；所述超声探头平移时，其位置由平移距离d_p决定；所述超声探头的运动学等式为：

^BP(d_p/θ)＝^BT_L·^LT_ST(d_p/θ)·^STP

n＝n(0)+d_p·v_p

c＝c(0)+d_p·v_p

上式中，^BT_L为{L}到{B}的齐次坐标变换矩阵，{L}表示双臂机器人左臂的末端法兰坐标系；^BP(d_p/θ)为超声探头平移d_p或旋转θ时，超声探头在{B}中的位置向量。^LT_ST(d_p/θ)为^LT_s(d_p/θ)和^LT_T(d_p/θ)组成的集合，为超声探头平移d_p或旋转θ时，{S}或{T}到{L}的齐次坐标变换矩阵；{S}和{T}分别对应超声探头的S平面超声图像坐标系和T平面超声图像坐标系；^STP为^SP和^TP组成的集合，表示S平面超声图像或T平面超声图像中的像素坐标；R(n，θ)为坐标系绕轴n旋转θ角度时，坐标系的旋转变换矩阵；n和c分别为超声探头旋转平面的法向量和旋转中心坐标；n(0)和C(0)分别为超声探头处于初始位置时，在{L}中旋转平面的单位法向量和旋转中心坐标，即旋转参数；v_p为超声探头在{L}中平移的单位方向向量，即第二平移参数。

本申请实施例采取的技术方案还包括：所述使用多点法对所述穿刺针的第一初始参数进行标定具体为：

将所述穿刺针复位至初始位置；

控制所述双臂机器人以不同的姿态让针尖碰一个固定参考点，记录所述双臂机器人不同姿态下的参数^BT_R(i)；

基于所述参数^BT_R(i)，对下述等式解最小二乘，得到所述穿刺针的第一初始参数^RP(0)：

^BT_R(i-1)·^RP(0)＝^BT_R(i)·^RP(0)，i＝1...n

本申请实施例采取的技术方案还包括：所述根据所述双平面超声成像对所述超声探头的第二初始参数进行标定具体为：

将所述穿刺针和超声探头复位至初始位置；

控制所述双臂机器人以不同的姿态让所述超声探头扫描标定好的针尖，记录所述双臂机器人不同姿态下的参数(^BT_L(i)，^BT_R(i))；

基于所述参数(^BT_L(i)，^BT_R(i))，对下述等式解最小二乘，得到所述超声探头的第二初始参数^LT_ST(0，0)：

本申请实施例采取的技术方案还包括：所述对所述第一平移参数进行标定包括：

控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让所述S平面能够扫描到针尖；

保持所述双臂机器人姿态不变，并控制电机，使所述穿刺针开始平移，并保存不同平移距离下的S平面超声图像；

基于所述S平面超声图像，根据下述等式，求得所述针尖在{ER}下平移的轨迹点坐标^RP(i)，对所述轨迹点做三维空间中的直线拟合，得到所述穿刺针的第一平移参数v_n：

^RP(i)＝^BT^-1 _R·^BT_L·^LT_S(0，0)·^SP(i)

本申请实施例采取的技术方案还包括：所述对所述第二平移参数进行标定包括：

控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让S平面能扫描到针尖；

保持所述双臂机器人姿态不变，控制电机，使所述超声探头开始平移，并保存不同平移距离下的S平面超声图像；

基于所述S平面超声图像，根据下述等式，求得所述超声探头在{EL}下平移轨迹点的坐标向量^LP(i)，对所述轨迹点做三维空间中的直线拟合，求得所述超声探头的第二平移参数v_p：

^LP(i)＝^LT_S(0,0)·^SP(i)

本申请实施例采取的技术方案还包括：所述对所述旋转参数进行标定包括：

控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让T平面与穿刺针相交；

保持所述双臂机器人姿态不变，控制电机，使所述超声探头开始旋转，并保存不同旋转角度下的T平面超声图像；

基于所述T平面超声图像，根据下述等式，求得所述超声探头在{L}下旋转轨迹点的坐标向量^LP(i)，对所述轨迹点做三维空间中的圆拟合，得到所述超声探头的旋转参数(n(0),c(0))：

^LP(i)＝^LT_T(0,0)·^TP(i)

本申请实施例采取的另一技术方案为：一种双臂机器人穿刺系统标定系统，包括：

运动学分析模块：用于对双臂机器人穿刺系统进行运动学分析，得到所述双臂机器人穿刺系统的待标定参数，所述待标定参数包括穿刺针的第一初始参数、第一平移参数、以及超声探头的第二初始参数、第二平移参数和旋转参数；

第一参数标定模块：用于使用多点法对所述穿刺针的第一初始参数进行标定，并对所述穿刺针的针尖进行双平面超声成像，根据所述双平面超声成像对所述超声探头的第二初始参数进行标定；

第二参数标定模块：用于利用所述超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用所述S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，并通过轨迹拟合对所述第一平移参数、第二平移参数和旋转参数进行标定。

相对于现有技术，本申请实施例产生的有益效果在于：本申请实施例的双臂机器人穿刺系统标定方法及系统利用双臂机器人高精度定位性能和经直肠超声探头能够提供两个垂直超声平面的特点，对多自由度的超声探头和多自由度针管进行运动学分析，得到穿刺针和超声探头的待标定参数，并基于经直肠超声探头提供的两个垂直超声平面同时实现对穿刺针和经直肠超声探头的参数标定，使得穿刺针管标定和超声探头标定成为一个闭环，能够高效地对双臂机器人穿刺系统的穿刺针和经直肠超声探头同时进行标定，且无需采用第三方跟踪设备，有利于节约成本。

附图说明

图1是本申请实施例的双臂机器人穿刺系统标定方法的流程图；

图2为具有多自由度末端的双臂机器人穿刺系统的结构示意图；

图3为经直肠超声探头的几何模型示意图；

图4为本申请实施例的双臂机器人穿刺系统的坐标系设置示意图；

图5为本申请实施例的双臂机器人穿刺系统的待标定参数标定框架图；

图6为本申请实施例的双臂机器人穿刺系统标定系统结构示意图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请。

针对现有技术的不足，本申请实施例的双臂机器人穿刺系统标定方法在不利用第三方跟踪设备的情况下，利用双臂机器人高精度定位性能和经直肠超声探头能够提供两个垂直超声平面的特点，建立了一种基于双平面超声图像的标定方法，对多自由度的超声探头和多自由度针管进行运动学分析，根据超声探头和穿刺针的不同运动状态，将待标定参数分为初始参数和运动参数，然后基于双平面超声图像完成初始参数和运动参数的标定。

具体地，请参阅图1，是本申请实施例的双臂机器人穿刺系统标定方法的流程图。本申请实施例的双臂机器人穿刺系统标定方法包括以下步骤：

S10：对双臂机器人穿刺系统进行运动学分析，得到双臂机器人穿刺系统的待标定参数，将待标定参数划分为初始参数和运动参数；

本步骤中，请一并参阅图2，为双臂机器人穿刺系统的结构示意图。其包括一个双臂机器人、一台带有经直肠超声探头的超声仪以及两个末端执行器；其中，两个末端执行器分别安装在双臂机器人的左右臂的末端法兰上，右臂末端执行器装配有穿刺针，穿刺针可绕自身平移和旋转。左臂末端执行器装配有经直肠超声探头，经直肠超声探头可绕自身平移和旋转。每个末端执行器都设有初始位置，每次运动由初始位置开始，操作完成后再复位至初始位置。经直肠超声探头的几何模型如图3所示,它提供了两个相互垂直的S平面和T平面。

进一步地，请参阅图4，为双臂机器人穿刺系统的坐标系设置示意图，其中，{L}和{R}分别表示左、右臂的末端法兰坐标系；{S}和{T}分别对应经直肠超声探头提供的S平面超声图像坐标系和T平面超声图像坐标系；{B}为双臂机器人的基坐标系(即整个系统的世界坐标系)。本申请实施例中，对双臂机器人穿刺系统进行的运动学分析包括穿刺针的运动学分析和超声探头的运动学分析两个部分，具体包括：

S11：对穿刺针进行运动学分析，得到穿刺针的待标定参数；穿刺针的待标定参数包括第一初始参数^RP(0)和第一平移参数v_n；

穿刺针绕自身旋转不改变自身的位置，穿刺针的位置(^BP(d_n))由其初始位置(^RP(0))和平移距离(d_n)决定。其运动学等式如下：

^BP(d_n)＝^BT_R·^RP(d_n) (1)

^RP(d_n)＝^RP(0)+v_n·d_n (2)

式(1)、(2)中，^BT_R为{R}到{B}的齐次坐标变换矩阵，由机器人操作系统(ROS)提供；^RP(d_n)为穿刺针平移d_n时，其针尖在{R}中的坐标；v_n为穿刺针在{R}中平移时的单位方向向量，为待标定参数。

S12：对超声探头进行运动学分析，得到超声探头的待标定参数，超声探头的待标定参数包括第二初始参数^LT_ST(0，0)、第二平移参数v_p和旋转参数(n(0)，c(0))；

当超声探头绕自身旋转时，超声探头的位置(^BP(d_p/θ))由初始位置和旋转角度(θ)决定；当超声探头平移时，超声探头的位置由初始位置和平移距离(d_p)决定。其运动学等式如下：

^BP(d_p/θ)＝^BT_L·T_ST(d_p/θ)·^STP

n＝n(0)+d_p·v_p

c＝c(0)+d_p·v_p (3)

式(3)中，^BT_L为{L}到{B}的齐次坐标变换矩阵，由ROS提供；^LT_ST(d_p/θ)为为^LT_s(d_p/θ)和^LT_T(d_p/θ)组成的集合，表示超声探头平移d_p或旋转θ时，图像坐标系{S}或{T}到{L}的齐次坐标变换矩阵；^STP为^sP和^TP组成的集合，表示S平面超声图像或T平面超声图像中的像素坐标；R(n，θ)为坐标系绕轴n旋转θ时，坐标系的旋转变换矩阵；n和c分别为超声探头旋转平面的法向量和旋转中心坐标；n(0)和C(0)分别为超声探头处于初始位置时，在{L}中旋转平面的单位法向量和旋转中心坐标，为待标定参数；v_p为超声探头在{L}中平移的单位方向向量，为待标定参数。

根据上述运动学分析结果，得到的待标定参数包括穿第一初始参数、第一平移参数、第二初始参数、第二平移参数和旋转参数，其中，第一平移参数、第二平移参数以及旋转参数为运动参数，具体如下表1所示：

表1待标定参数分类

当超声探头和穿刺针都处于初始位置时，需求得第一初始参数^RP(0)和第二初始参数^LT_ST(0，0)才可进行运动学建模；当超声探头和穿刺针平移时，还需求得第一平移参数v_n和第二平移参数v_p才可进行平移时的运动学建模；当超声探头旋转时，还需求得旋转参数(n(0),c(0))才可进行旋转时的运动学建模。

S20：基于运动学分析结果，使用多点法对穿刺针的第一初始参数进行标定，并对穿刺针的针尖进行双平面超声成像，根据双平面超声成像对超声探头的第二初始参数进行标定；

本步骤中，第一初始参数和第二初始参数的标定过程具体如下：

穿刺针的第一初始参数^RP(0)为穿刺针在初始位置时，针尖在{R}下的坐标。本申请实施例采用多点法对初始参数^RP(0)进行标定，具体标定过程为：首先，让穿刺针复位至初始位置；然后控制机器人以不同的姿态让针尖去碰一个固定的参考点，记录下机器人不同姿态下的参数^BT_R(i)；最后对下述等式解最小二乘，得到穿刺针的第一初始参数^RP(0)：

^BT_R(i-1)·^RP(0)＝^BT_R(i)·^RP(0),i＝1...n (4)

超声探头的第二初始参数^LT_ST(0,0)是参数^LT_S(0,0)和^LT_T(0,0)的集合，为超声探头处于初始位置时，图像坐标系({S}和{T})到{L}的齐次坐标变换矩阵。本申请实施例采用针尖的双平面超声成像对第二初始参数^LT_ST(0,0)进行标定。具体标定过程为：首先，让穿刺针和超声探头复位至初始位置；然后控制机器人以不同的姿态让超声探头扫描标定好的针尖，记录机器人不同姿态下的参数(^BT_L(i),^BT_R(i))；最后，通过对下述等式解最小二乘，得到超声探头的第二初始参数^LT_ST(0,0)：

S30：利用双臂机器人穿刺系统的经直肠超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，通过轨迹拟合对双臂机器人穿刺系统的运动参数进行标定；

本步骤中，双臂机器人穿刺系统的待标定参数标定过程如图5所示，运动参数的标定过程具体包括：

第一平移参数v_n为穿刺针在{R}下平移的单位方向向量。本申请实施例采用超声S平面跟踪穿刺针的平移轨迹，并通过三维空间的直接拟合对v_n进行标定。具体标定过程为：首先，控制机器人，使穿刺针和超声探头接近平行，让S平面能扫描到针尖；然后，保持机器人姿态不变，并控制电机，使穿刺针开始平移，并保存不同平移距离下的S平面超声图像；最后，根据下述等式(6)，求得穿刺针尖在{R}下平移的轨迹点，对轨迹点做三维空间中的直线拟合，得到穿刺针的第一平移参数v_n：

^RP(i)＝^BT^-1 _R·^BT_L·^LT_S(0,0)·^SP(i) (6)

第二平移参数v_p为超声探头在{L}下平移的单位方向向量。本申请实施例采用超声S平面跟踪超声探头的平移轨迹，并通过三维空间的直接拟合对v_p进行标定。具体标定过程为：首先，控制机器人，使穿刺针和超声探头接近平行，让S平面能扫描到针尖；然后，保持机器人姿态不变，控制电机，使超声探头开始平移，保存不同平移距离下的S平面超声图像；最后，根据下述等式(7)，求得超声探头在{L}下平移轨迹点的坐标向量^LP(i)，对轨迹点做三维空间中的直线拟合，求得超声探头的第二平移参数v_p：

^LP(i)＝^LT_S(0,0)·^SP(i) (7)

旋转参数(n(0),c(0))为超声探头旋转时，旋转平面的法向量和旋转中心坐标。本申请实施例采用T平面跟踪超声探头的旋转轨迹，然后通过三维空间的圆拟合，完成对旋转参数(n(0),c(0))的标定。具体标定过程为：首先，控制机器人，使穿刺针和超声探头接近平行，让T平面与针相交；然后，保持机器人姿态不变，控制电机，使超声探头开始旋转，并保存不同旋转角度下的T平面超声图像；最后，根据下述等式(8)，求得超声探头在{L}下旋转轨迹点的坐标向量^LP(i)，对轨迹点做三维空间中的圆拟合，得到超声探头的旋转参数(n(0),c(0))：

^LP(i)＝^LT_T(0,0)·^TP(i) (8)

本申请实施例的双臂机器人穿刺系统标定方法利用双臂机器人高精度定位性能和经直肠超声探头能够提供两个垂直超声平面的特点，对多自由度的超声探头和多自由度针管进行运动学分析，得到穿刺针和超声探头的待标定参数，并基于经直肠超声探头提供的两个垂直超声平面同时实现对穿刺针和经直肠超声探头的参数标定。本申请实施例融合了双臂机器人的机构和超声图像，使得穿刺针管标定和超声探头标定成为一个闭环，能够高效地对双臂机器人穿刺系统的穿刺针和经直肠超声探头同时进行标定，且无需采用第三方跟踪设备，有利于节约成本。

为了验证本申请实施例的可行性和有效性，以下实施例通过在已有的经直肠超声探头引导的双臂机器人穿刺系统上进行了实验测试。在整个系统完成标定后，通过超声探头对双臂机器人穿刺系统在不同的运动状态(初始状态、平移和旋转)下的针尖进行扫描。分别计算针管标定下的针尖坐标以及探头标定下的针尖坐标，以两者的距离为定位误差,实验得到的不同运动状态下的针尖定位误差均可达到1mm以内。实验结果表明，本申请实施例能够高效的对双臂机器人穿刺系统进行标定，且标定精度较高。

请参阅图6，为本申请实施例的双臂机器人穿刺系统标定系统结构示意图。本申请实施例的双臂机器人穿刺系统标定系统40包括：

运动学分析模块41：用于对双臂机器人穿刺系统进行运动学分析，得到双臂机器人穿刺系统的待标定参数，将待标定参数划分为初始参数和运动参数；

第一参数标定模块42：用于基于运动学分析结果，使用多点法对穿刺针的第一初始参数进行标定，并对穿刺针的针尖进行双平面超声成像，根据双平面超声成像对超声探头的第二初始参数进行标定；

第二参数标定模块43：用于利用双臂机器人穿刺系统的经直肠超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，通过轨迹拟合对双臂机器人穿刺系统的运动参数进行标定。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本申请。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本申请中所定义的一般原理可以在不脱离本申请的精神或范围的情况下，在其它实施例中实现。因此，本申请将不会被限制于本申请所示的这些实施例，而是要符合与本申请所公开的原理和新颖特点相一致的最宽的范围。

Claims (9)

1.一种双臂机器人穿刺系统标定方法，所述双臂机器人穿刺系统包括双臂机器人，其特征在于，所述双臂机器人穿刺系统标定方法包括：

对双臂机器人穿刺系统进行运动学分析，得到所述双臂机器人穿刺系统的待标定参数；所述待标定参数包括穿刺针的第一初始参数、第一平移参数，以及超声探头的第二初始参数、第二平移参数和旋转参数；

使用多点法对所述穿刺针的第一初始参数进行标定，并对所述穿刺针的针尖进行双平面超声成像，根据所述双平面超声成像对所述超声探头的第二初始参数进行标定；

利用所述超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用所述S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，并通过轨迹拟合对所述第一平移参数、第二平移参数和旋转参数进行标定。

2.根据权利要求1所述的双臂机器人穿刺系统标定方法，其特征在于，所述对双臂机器人穿刺系统进行运动学分析包括：

对所述穿刺针进行运动学分析；所述穿刺针绕自身旋转不改变自身的位置，所述穿刺针的位置^BP(d_n)由其初始位置^RP(0)和平移距离d_n决定，所述穿刺针的运动学等式为：

^BP(d_n)＝^BT_R·^RP(d_n)

^RP(d_n)＝^RP(0)+v_n·d_n

上式中,^BT_R为{R}到{B}的齐次坐标变换矩阵，{R}表示双臂机器人右臂的末端法兰坐标系，{B}为双臂机器人的基坐标系；^RP(d_n)为穿刺针平移d_n时，其针尖在{R}中的坐标；v_n为穿刺针平移时在{R}中的单位方向向量，即第一平移参数。

3.根据权利要求2所述的双臂机器人穿刺系统标定方法，其特征在于，所述对双臂机器人穿刺系统进行运动学分析包括：

对所述超声探头进行运动学分析；所述超声探头绕自身旋转时，其位置由其旋转角度q决定；所述超声探头平移时，其位置由平移距离d_p决定；所述超声探头的运动学等式为：

^BP(d_p/q)＝^BT_L·^LT_ST(d_p/q)·^STP

n＝n(0)+d_p·v_p

c＝c(0)+d_p·v_p

上式中，^BT_L为{L}到{B}的齐次坐标变换矩阵，{L}表示双臂机器人左臂的末端法兰坐标系；^BP(d_p/q)为超声探头平移d_p或旋转q时，超声探头在{B}中的位置向量，^LT_ST(d_p/q)为^LT_S(d_p/q)和^LT_T(d_p/q)组成的集合，为超声探头平移d_p或旋转q时，{S}或{T}到{L}的齐次坐标变换矩阵；{S}和{T}分别对应超声探头的S平面超声图像坐标系和T平面超声图像坐标系；^STP为^SP和^TP组成的集合，表示S平面超声图像或T平面超声图像中的像素坐标；R(n,θ)为坐标系绕轴n旋转θ角度时，坐标系的旋转变换矩阵；n和c分别为超声探头旋转平面的法向量和旋转中心坐标；n(0)和C(0)分别为超声探头处于初始位置时，在{L}中旋转平面的单位法向量和旋转中心坐标，即旋转参数；v_p为超声探头在{L}中平移的单位方向向量，即第二平移参数。

4.根据权利要求2所述的双臂机器人穿刺系统标定方法，其特征在于，所述使用多点法对所述穿刺针的第一初始参数进行标定具体为：

将所述穿刺针复位至初始位置；

控制所述双臂机器人以不同的姿态让针尖碰一个固定参考点，记录所述双臂机器人不同姿态下的参数^BT_R(i)；

基于所述参数^BT_R(i)，对下述等式解最小二乘，得到所述穿刺针的第一初始参数^RP(0)：

^BT_R(i-1)·^RP(0)＝^BT_R(i)·^RP(0),i＝1...n。

5.根据权利要求3所述的双臂机器人穿刺系统标定方法，其特征在于，所述根据所述双平面超声成像对所述超声探头的第二初始参数进行标定具体为：

将所述穿刺针和超声探头复位至初始位置；

控制所述双臂机器人以不同的姿态让所述超声探头扫描标定好的针尖，记录所述双臂机器人不同姿态下的参数(^BT_L(i),^BT_R(i))；

基于所述参数(^BT_L(i),^BT_R(i))，对下述等式解最小二乘，得到所述超声探头的第二初始参数^LT_ST(0,0)：

6.根据权利要求5所述的双臂机器人穿刺系统标定方法，其特征在于，所述对所述第一平移参数进行标定包括：

控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让所述S平面能够扫描到针尖；

保持所述双臂机器人姿态不变，并控制电机，使所述穿刺针开始平移，并保存不同平移距离下的S平面超声图像；

基于所述S平面超声图像，根据下述等式，求得所述针尖在{R}下平移的轨迹点坐标^RP(i)，对所述针尖在{R}下平移的轨迹点做三维空间中的直线拟合，得到所述穿刺针的第一平移参数v_n：

^RP(i)＝^BT^-1 _R·^BT_L·^LT_S(0,0)·^SP(i)。

7.根据权利要求6所述的双臂机器人穿刺系统标定方法，其特征在于，所述对所述第二平移参数进行标定包括：

控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让S平面能扫描到针尖；

保持所述双臂机器人姿态不变，控制电机，使所述超声探头开始平移，并保存不同平移距离下的S平面超声图像；

基于所述S平面超声图像，根据下述等式，求得所述超声探头在{L}下平移轨迹点的坐标向量^LP(i)，对所述超声探头在{L}下平移的轨迹点做三维空间中的直线拟合，求得所述超声探头的第二平移参数v_p：

^LP(i)＝^LT_S(0,0)·^SP(i)。

8.根据权利要求7所述的双臂机器人穿刺系统标定方法，其特征在于，所述对所述旋转参数进行标定包括：

控制所述双臂机器人，使所述穿刺针和超声探头接近平行，让T平面与穿刺针相交；

保持所述双臂机器人姿态不变，控制电机，使所述超声探头开始旋转，并保存不同旋转角度下的T平面超声图像；

基于所述T平面超声图像，根据下述等式，求得所述超声探头在{L}下旋转轨迹点的坐标向量^LP(i)，对所述超声探头在{L}下旋转的轨迹点做三维空间中的圆拟合，得到所述超声探头的旋转参数(n(0),c(0))：

^LP(i)＝^LT_T(0,0)·^TP(i)。

9.一种双臂机器人穿刺系统标定系统，其特征在于，包括：

运动学分析模块：用于对双臂机器人穿刺系统进行运动学分析，得到所述双臂机器人穿刺系统的待标定参数，所述待标定参数包括穿刺针的第一初始参数、第一平移参数、以及超声探头的第二初始参数、第二平移参数和旋转参数；

第一参数标定模块：用于使用多点法对所述穿刺针的第一初始参数进行标定，并对所述穿刺针的针尖进行双平面超声成像，根据所述双平面超声成像对所述超声探头的第二初始参数进行标定；

第二参数标定模块：用于利用所述超声探头获取两个相互垂直的S平面超声图像和T平面超声图像，使用所述S平面超声图像和T平面超声图像分别跟踪超声探头和穿刺针的运动轨迹，并通过轨迹拟合对所述第一平移参数、第二平移参数和旋转参数进行标定。

Images