[发明专利]一种双臂机器人穿刺系统标定方法及系统

权利要求书

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}中平移的单位方向向量，即第二平移参数。