CN100579479C - 内镜操作手术机器人穿刺孔定位装置 - Google Patents

Abstract

本发明提供的是一种内镜操作手术机器人穿刺孔定位装置。它包括安装在机器人末端机械臂上的可伸缩式连杆，可伸缩式连杆的端部安装微型CCD摄像头，机器人末端机械臂的下方安装有两个红光点状光斑激光器，两个红光点状光斑激光器发出的光线在摄像头正下方光轴上相交，红光点状光斑激光器和微型CCD摄像头与控制工业PC机相连。本发明提高了穿刺孔标定精度；节省CCD摄像机等标定器件，避免仅利用摄像机进行双目及多目标定时在三维重建过程中占用大量计算机系统资源，导致系统工作缓慢，使穿刺孔标定过程迅速、准确，有利于降低成本，便于产品开发和生产。

Description

内镜操作手术机器人穿刺孔定位装置

(一)技术领域

本发明涉及的是一种机器人，具体地说是一种内镜操作手术机器人穿刺孔定位装置。本发明还涉及内镜操作手术机器人穿刺孔定位的定位方法。

(二)背景技术

随着科学技术的进步，人们逐步将传统医疗器械与信息、微电子、新材料、自动化、精密制造、机器人等技术有机地结合起来，以提高医疗质量。

与传统的开放性手术相比，内镜微创手术以其创面小、患者痛楚小、恢复期短、手术风险和费用低等优点，受到医生和患者的普遍欢迎，具有广阔的应用前景。在进行此类手术的过程中，需根据主刀医生的要求不断调整内镜的方位，以便从显示器上及时准确地观察到手术部位的图像。为此，人们研制了各种控制内镜运动的设备。《高技术通讯》2004(11)中刊登的题为“面向机器人辅助神经外科手术的视觉标定方法”的科技论文中，介绍了一种利用双目立体视觉技术，通过双摄像头和图像卡采集双目图像，实现临床手术中不用接触病人头部标记点(Mark点)的精确定位。该方法与传统的接触式标定相比，具有安全性高、易于操作、可实时更新数据等优点。其不足之处在于：采用双目标定数据处理量大，导致计算机系统运行缓慢。

《武汉理工大学学报(交通科学与工程版)》2007，31(5)中刊登的题为“微创手术机器人视觉定位系统的研究”的科技论文中，介绍了一种采用摄像机模型为带有一阶径向畸变的针孔模型，在完成摄像机内外参数标定(两步法)和完成机器人“手一眼”关系标定(AX＝XB法)的前提下，利用立体视觉(单目改变位置看两次)来求取任意标记点在机器人坐标系中的位置坐标值。该立体视觉定位系统，采用一个CCD摄像机(单目)安装在机器人末端执行器法兰盘上，通过控制机器人机械臂的移动在不同的位置摄取两幅图像，然后通过三维重建来完成。优点在于节省了视频采集元件，降低了成本。其不足之处在于：摄取两幅图像有时间间隔，处理数据量大，系统运行缓慢。

(三)发明内容

本发明的目的在于提供一种可以提高穿刺孔标定精度，节省CCD摄像机等标定器件，有利于提高系统运行速度，便于产品开发和生产的内镜操作手术机器人穿刺孔定位装置。本发明的目的还在于提供一种基于图像和几何模型的内镜操作手术机器人穿刺孔定位方法。

本发明的目的是这样实现的：

本发明的内镜操作手术机器人穿刺孔定位装置的组成包括：安装在机器人末端机械臂上的可伸缩式连杆，可伸缩式连杆的端部安装微型CCD摄像头，机器人末端机械臂的下方安装有两个红光点状光斑激光器，两个红光点状光斑激光器发出的光线在摄像头正下方光轴上相交，红光点状光斑激光器和微型CCD摄像头与控制工业PC机相连。

两个红光点状光斑激光器的倾角不同，一个红光点状光斑激光器的倾角为23.385°，另一个红光点状光斑激光器的倾角为37.3°。

本发明的内镜操作手术机器人穿刺孔定位装置的定位方法为：用工业PC机首先控制机器人移动关节在其行程内上下移动，通过微型CCD摄像头判断是否捕获三个连通域，捕获三个连通域后分别计算其各自面积并进行逐一比较，通过控制移动关节的移动判断面积较小的两个连通域的中心距的变化，并最终控制移动关节使两个面积较小的连通域的中心距为0，即两个红光点状光斑激光器发出的激光在腹部皮肤处重合，求出两束激光照射的腹部位置相对机器人基础坐标系的位置；此时，工业PC机再通过微型CCD摄像头判断图像中面积较大的连通域与面积较小连通域间的中心距是否为0，若为0则表示面积较大连通域与面积较小连通域重合，求出腹腔镜穿刺孔位置相对辅助腹腔镜手术机器人基础坐标系的位置；若两个连通域的中心距不为0，则计算两连通域的中心距和所在平面矢量

，工业PC机控制机器人机械臂运动，使机械臂的末端沿

运动一定距离，再重新进行运行程序，直至三个连通域重合，求出腹腔镜手术穿刺孔相对机器人基础坐标系的位置。

所述的求出两束激光照射的腹部位置相对机器人基础坐标系的位置的方法是：

$\left\{\begin{array}{c}{x}_g=a_2{c}_2+(a_3+l_4)c_{23}\\ y_g=a_2{s}_2+(a_3+l_4)s_{23}\\ z_g=d_1-160\end{array}\right.$

其中，s₂＝sinθ₂；c₂＝cosθ₂；s₂₃＝sin(θ₂+θ₃)；c₂₃＝cos(θ₂+θ₃)；a₂、a₃分别为大小转动臂的长，θ₂为大转动臂的转角，θ₃为小转动臂的转角。

所述的使机械臂的末端沿

运动一定距离的方法是： $L=\frac{\mathrm{df}}{10(v-f)}$ ，其中：f为焦距，u为物距，v为像距，d为两个连通域在图像中的中心距。

本发明是一种基于图像和几何模型的内镜操作手术机器人穿刺孔定位装置及方法，用于内镜(内窥镜)操作手术机器人标定内镜手术穿刺孔的位置，为机器人提供必要的内镜姿态信息，便于对内镜的操作。在机器人末端机械臂下方安装两个不同倾角的红光点状光斑激光器，同时，在末端机械臂末梢的可伸缩式连杆安装一个微型CCD摄像头，使两条光线的交点落在微型CCD摄像头镜头的下方即图像中心，通过两条光线的交点间接测量穿刺孔相对于机器人基础坐标系的位置坐标。本发明提高了穿刺孔标定精度；节省CCD摄像机等标定器件，避免仅利用摄像机进行双目及多目标定时在三维重建过程中占用大量计算机系统资源，导致系统工作缓慢，使穿刺孔标定过程迅速、准确，有利于降低成本，便于产品开发和生产。

标定系统包括按照不同倾角倾斜安装在机器入末端机械臂下方的两个红光点状光斑激光器，安装在末端机械臂末梢的微型CCD摄像头，视频采集卡以及机器人控制主机工业PC机组成(参见图1)。其中，微型CCD摄像头通过视频采集卡与机器人控制主机工业PC机连接。

红光点状光斑激光器1、2及摄像头安装尺寸已知(参见图2)，由内镜操作手术机器人连杆坐坐标系及结构简图，内镜操作机器人为PRRRRR结构，共有6个自由度，其中3个被动关节为集中于一点的球铰链结构，无驱动电机(参见图3)运动学方程及正逆运动学模型如下：

各杆件间的变换矩阵： $T_1^0=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_1\\ 0& 0& 0& 1\end{array}\right]$ $T_2^1=\left[\begin{array}{cccc}{c}_2& -s_2& 0& 0\\ s_2& c_2& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$T_3^2=\left[\begin{array}{cccc}{c}_3& -s_3& 0& a_2\\ s_3& c_3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_4^3=\left[\begin{array}{cccc}{c}_4& -s_4& 0& a_3\\ s_4& c_4& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

其中，s₂＝sinθ₂；c₂＝cosθ₂；s₂₃＝sin(θ₂+θ₃)；c₂₃＝cos(θ₂+θ₃)，其中，a₂、a₃分别为大小转动臂的长，θ₂为大转动臂的转角，θ₃为小转动臂的转角。

由 $T_4^0=T_1^0\left(d_1\right)T_2^1\left({\mathrm{\θ}}_2\right)T_3^2\left({\mathrm{\θ}}_3\right)T_4^3\left({\mathrm{\θ}}_4\right),$ $T_4^0=\left[\begin{array}{cccc}{c}_{23}& -s_{23}& 0& a_2{c}_2+a_3{c}_{23}\\ s_{23}& c_{23}& 0& a_2{s}_2+a_3{s}_{23}\\ 0& 0& 1& d_1\\ 0& 0& 0& 1\end{array}\right]$

可知本发明被动关节中心的位置为(a₂c₂+a₃c₂₃，a₂s₂+a₃s₂₃，d₁)，即

$\left\{\begin{array}{c}{x}_p=a_2{c}_2+a_3{c}_{23}\\ y_p=a_2{s}_2+a_3{s}_{23}\\ z_p=d_1\end{array}\right.;$ 逆向运动学模型为 $\left\{\begin{array}{c}{\mathrm{\θ}}_3=\mathrm{\π}-\mathrm{\α}\\ {\mathrm{\θ}}_2=\arctan \left(\frac{y_p}{x_p}\right)-\arctan \left(\frac{a_3{s}_2}{a_2+a_3{c}_3}\right)\\ \mathrm{\α}=\arccos \left(\frac{-(x_p^2+y_p^2)+a_2^2+a_3^2}{2a_2{a}_3}\right)\\ d_1=z_p\end{array}\right..$

则两红光点状光斑激光器交点处的位置为(a₂c₂+(a₃+160)c₂₃，a₂s₂+(a₃+160)s₂₃，d₁-160)，即：

$\left\{\begin{array}{c}{x}_g=a_2{c}_2+(a_3+l_4)c_{23}\\ y_g=a_2{s}_2+(a_3+l_4)s_{23}\\ z_g=d_1-160\end{array}\right..---\left(1\right)$

视觉测量中常用到如下一些坐标系，其定义分别如下(参见图4)：

(1)世界坐标系(X_W，Y_W，Z_W)：也称为绝对坐标系，用来描述环境中任何物体的位置，世界坐标系中空间的一点M表示为(X_W，Y_W，Z_W)。

(2)摄像机坐标系(X_c，Y_c，Z_c)：该坐标系的原点定义在摄像机镜头的光心。Z_c轴为光轴，Z_c轴和Y_c轴分别平行于图像平面的水平轴和垂直轴。

(3)图像像素坐标系(u，v)：也称为帧存坐标，它以像素为单位，一般以图像左上角为原点，uv分别代表数字图像中的列数与行数。

(4)图像物理坐标系(X，Y)：其中心是光轴与图像平面的交点为原点，X轴与Y轴分别与图像像素坐标系的uv轴平行。

定义了上述各种空间直角坐标系后，就可以得到相机在线性模型下的成像描述，即建立空间物点和对应像点之间的数学关系。

1、世界坐标系与摄像机坐标系的变换关系

世界坐标系中的点到相机坐标系的变换过程可由一个旋转变换矩阵R和一个平移变换向量t来描述。设空间中某M点在世界坐标系和相机坐标系下的齐次坐标分别是[X_w，Y_w，Z_w]^T和[X_c，Y_c，Z_c]^T，于是存在如下关系：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^r& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]---\left(2\right)$

其中，R为3×3正交单位矩阵，t为三维平移向量，t＝[t_x，t_y，t_z]^T，

$R=\left[\begin{array}{ccc}{r}_1& r_2& r_3\\ r_4& r_5& r_6\\ r_7& r_8& r_9\end{array}\right].$

2、图像坐标系与摄像机坐标系的变换关系

摄像机坐标系中的点M在图像物理坐标系中的像点m坐标为：

$\left\{\begin{array}{c}x=f{x}_c/Z_c\\ y=f{y}_c/Z_c\end{array}\right.---\left(3\right)$

将上式的图像物理坐标系进一步转化成图像像素坐标系为：

$\left\{\begin{array}{c}u=x/d_x+u_0\\ v=y/d_y+v_0\end{array}\right.---\left(4\right)$

其中，u₀，v₀是图像中心(光轴与图像平面的交点)的坐标，d_x和d_y分别为一个像素在X和Y方向的物理尺寸。由式(3)和式(4)可以得到空间点M与像点m的像素坐标之间的变换关系，以齐次坐标表示：

$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}1/d_x& 0& u_0\\ 0& 1/d_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cccc}f/d_x& 0& u_0& 0\\ 0& f/d_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]---\left(5\right)$

令α＝f/d_y，β＝f/d_y则式(5)改写为：

$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}\mathrm{\α}& 0& u_0& 0\\ 0& \mathrm{\β}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]---\left(6\right)$

3.世界坐标系与图像坐标系的变换关系

将式(2)带入式(6)就可以得到就空间点M在世界坐标系中的坐标与其像点m在图像像素坐标系中的坐标之间的变换关系：

$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}\mathrm{\α}& \mathrm{\γ}& u_0& 0\\ 0& \mathrm{\β}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ 0^r& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\begin{array}{}\left[\begin{array}{ccc}\mathrm{\α}& \mathrm{\γ}& u_0\\ 0& \mathrm{\β}& v_0\\ 0& 0& 1\end{array}\right]\end{array}\left[\begin{array}{cc}R& t\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]---\left(7\right)$

上式可简写成：z_c·U_uv＝A·X_w    (8)

其中，U_uv＝[u，v，1]^T和X_w＝[x_w，y_w，z_w，1]^T分别是空间点M＝[x_w，y_w，z_w]^r和其像点m＝[u，v]^T的齐次坐标，矩阵A为3×4不可逆矩阵，也称为投影矩阵。

$A=\left[\begin{array}{cccc}\mathrm{\α}{r}_1+u_0{r}_7& \mathrm{\α}{r}_2+u_0{r}_8& \mathrm{\α}{r}_3+u_0{r}_9& \mathrm{\α}{t}_x+u_0{t}_z\\ \mathrm{\β}{r}_4+v_0{r}_7& \mathrm{\β}{r}_5+v_0{r}_8& \mathrm{\β}{r}_6+v_0{r}_9& \mathrm{\β}{t}_y+v_0{t}_z\\ r_7& r_8& r_9& t_z\end{array}\right]---\left(9\right)$

由于实际成像系统中存在着各种误差因素，实际成像模型并不满足线性关系，而是一种非线性关系。对于中焦距普通质量的镜头，在图像的边界大概有1-3个像素大小的畸变误差。为了获得较高的测量精度，必须采用非线性模型来对摄像系统进行标定。描述图像点的非线性畸变可用下面的公式：

$\left\{\begin{array}{c}x=x_d+{\mathrm{\δ}}_x\\ y=y_d+{\mathrm{\δ}}_y\end{array}\right.---\left(10\right)$

其中，(x，y)为针孔模型成像条件下的图像点的理想坐标，(x_d，y_d)为图像点实际的坐标，δ_x与δ_y分别为X利Y方向上的畸变值，它和图像点的位置有关。

摄像机标定采用非线性成像模型的畸变误差模型——Weng’s标定模型，总的畸变表示为：

$\left\{\begin{array}{c}{\mathrm{\δ}}_x=k_1{x}_d(x_d^2+y_d^2)+{\mathrm{\ρ}}_1(3x_d^3+y_d^2)+2{\mathrm{\ρ}}_2{x}_d{y}_d+s_1(x_d^2+y_d^2)\\ {\mathrm{\δ}}_y=k_1{x}_d(x_d^2+y_d^2)+{\mathrm{\ρ}}_2(x_d^3+3y_d^2)+2{\mathrm{\ρ}}_1{x}_d{y}_d+s_2(x_d^2+y_d^2)\end{array}\right.---\left(11\right)$

令g₁＝s₁+ρ₁，g₂＝s₂+ρ₂，g₃＝2ρ₁，g₄＝2ρ₂，g₅＝k₁则有：

$\left\{\begin{array}{c}{\mathrm{\δ}}_x=(g_1+g_2)x_d^2+g_4{x}_d{y}_d+g_1{y}_d^2+g_5{x}_d(x_d^2+y_d^2)\\ {\mathrm{\δ}}_y=g_2{x}_d^2+g_3{x}_d{y}_d+(g_2+g_4)y_d^2+g_5{y}_d(x_d^2+y_d^2)\end{array}\right.---\left(12\right)$

将(12)带入(10)得：

$\left\{\begin{array}{c}x=x_d+(g_1+g_3)x_d^2+g_4{x}_d{y}_d+g_1{y}_d^2+g_5{x}_d(x_d^2+y_d^2)\\ y=y_d+g_2{x}_d^2+g_3{x}_d{y}_d+(g_2+g_4)y_d^2+g_5{y}_d(x_d^2+y_d^2)\end{array}\right.---\left(13\right)$

上式即为非线性成像模型的畸变误差模型，摄像机标定利用上述模型进行畸变校正。

摄像机过程是：拍摄已知世界坐标的空间点阵图像，获取对应像点的图像坐标，然后计算内外参数。运用分步法进行求解。首先用提取的网格结点拟合一个近似无误差的网格像场；而后根据拟合出的网格像场与空间实际网格的关系，用(8)式进行求解，由此可解得摄像机的投影矩阵。最后根据拟合出来的网格场与提取出来的结点值对像差修正系数进行求解。

在视觉测量系统中，采用平面靶标作为标定物(参见图5)，该靶标有一系列的均匀分布，垂直交叉的直线，线之间的交点的相对位置是已知的。并且世界坐标系的X轴为该靶标的水平方向，Y轴为该靶标的垂直方向，其原点位于平面靶标。显然对于平面靶标上的任一点都有Z_w＝0。

在标定前对系统要做如下调整：

a.调整平面靶标，使靶标上的互相处置的线分别平行于X_w轴和Y_w轴。

b.调整摄像头，使其主光轴方向尽量垂直于标定平面。

c.调整微型CCD摄像头成水平状态。

拟合标准网格(参见图6)对图6所示的网格像场中水平方向与垂直方向的直线可分别表示如下：

y＝ia+b₀                              (14)

x＝ia+b₁                              (15)

其中a为线与线之间的间隔，b₀，b₁为偏移量，i为网格线的标号。

采用最小二乘法计算a，b₀，b₁等参数，具体做法如下：

取图像中部区域的k水平线和l条垂直线，记：

$f(a,b_0,b_1)=\underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}{(\mathrm{ia}+b_0-y_{\mathrm{ij}})}^2+\underset{i=1}{\overset{l}{\mathrm{\Σ}}}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{(\mathrm{ia}+b_1-x_{\mathrm{ij}})}^2=\underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}[{(\mathrm{ia}+b_0-y_{\mathrm{ij}})}^2+{(\mathrm{ja}+b_1-x_{\mathrm{ij}})}^2]$

(16)

分别求偏导数，得3个方程：

$\frac{\mathrm{df}}{\mathrm{da}}=2\underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}[(\mathrm{ia}+b_0-y_{\mathrm{ij}})+(\mathrm{ja}+b_1-x_{\mathrm{ij}})j]$

$=\frac{1}{3}\mathrm{kl}[(k+1)(2k+1)+(l+1)(2l+1)]a+\mathrm{kl}(k+1)b_0+\mathrm{kl}(l+1)b_1-2\underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}({\mathrm{iy}}_{\mathrm{ij}}+{\mathrm{jx}}_{\mathrm{ij}})=0$

$\frac{\mathrm{df}}{d{b}_0}=2\underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}(\mathrm{ia}+b_0-y_{\mathrm{ij}})=\mathrm{kl}(k+l)-2\underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}=0$

$\frac{\mathrm{df}}{d{b}_1}=2\underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}(\mathrm{ja}+b_1-y_{\mathrm{ij}})=\mathrm{kl}(k+l)-2\underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}=0$

解方程

WF＝B                                  (17)

其中：

$W=\left[\begin{array}{ccc}\frac{1}{3}[(k+1)(2k+1)+(l+1)(2l+1)& k+1& l+1\\ k+1& 2& 0\\ l+1& 0& 2\end{array}\right],$ $F=\left[\begin{array}{c}a\\ b_0\\ b_1\end{array}\right],$

$B=\frac{2}{\mathrm{kl}}\left[\begin{array}{c}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}({\mathrm{iy}}_{\mathrm{ij}}+j{x}_{\mathrm{ij}})\\ \underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}{y}_{\mathrm{ij}}\\ \underset{i=1}{\overset{k}{\mathrm{\Σ}}}\underset{j=1}{\overset{l}{\mathrm{\Σ}}}{x}_{\mathrm{ij}}\end{array}\right]$

由此可得到整个网格的交点。

求解摄像机位置参数。

将(8)式展开，消去Z_c有：

$\left\{\begin{array}{c}(u-u_0)x_w{r}_7+(u-u_0)y_w{r}_8+(u-u_0)z_w{r}_9+(u-u_0)t_z-x_w\mathrm{\α}{r}_1-y_w\mathrm{\α}{r}_2-z_w\mathrm{\α}{r}_3-\mathrm{\α}{t}_x=0\\ (v-v_0)x_w{r}_7+(v-v_0)y_w{r}_8+(v-v_0)z_w{r}_9+(v-v_0)t_z-x_w\mathrm{\β}{r}_4-y_w\mathrm{\β}{r}_5-z_w\mathrm{\β}{r}_6-\mathrm{\β}{t}_y=0\end{array}\right.$

(18)

假设如下一组中间变量：

$\left\{\begin{array}{c}{q}_1=\mathrm{\α}{r}_1+u_0{r}_7\\ q_2=\mathrm{\α}{r}_2+u_0{r}_8\\ q_3=\mathrm{\α}{r}_3+u_0{r}_9\\ q_4=\mathrm{\β}{r}_4+v_0{r}_7\\ q_5=\mathrm{\β}{r}_5+v_0{r}_8\\ q_6=\mathrm{\β}{r}_6+v_0{r}_9\\ q_7=r_7\\ q_8=r_8\\ q_9=r_9\\ q_{10}=\mathrm{\α}{t}_x+u_0{t}_z\\ q_{11}=\mathrm{\β}{t}_y+v_0{t}_z\\ q_{12}=t_z\end{array}\right.---\left(19\right)$

(19)式可以表示为：HQ＝0                     (20)

其中：

$H=\left[\begin{array}{cccccccccccc}-x_w& -y_w& -z_w& 0& 0& 0& u{x}_w& {\mathrm{uy}}_w& u{z}_w& -1& 0& u\\ 0& 0& 0& -x_w& -y_w& {-z}_w& {\mathrm{vx}}_w& {\mathrm{vy}}_w& {\mathrm{vz}}_w& 0& -1& v\end{array}\right],$

Q＝[q₁ q₂ q₃ q₄ q₅ q₆ q₇ q₈ q₉ q₁₀ q₁₁ q₁₂]^r

由于(20)式是一齐次方程，它有无穷多组解，而所需的摄像机参数满足如下两个条件：

(1)因为r₇，r₈，r₉是正交矩阵的最后一行的行向量，它应是一单位向量，所以 $q_7^2+q_8^2+q_9^2=1.$

(2)q₁₂的符号应与摄像机在世界坐标中的位置相适应，若它在Z_w轴的正向则为正，反之为负，在此系统中取为正。应此，当Q确定后，实际的摄像机参数应是：

$N=\frac{Q}{\sqrt{q_7^2+q_8^2+q_9^2}}$

对于(20)式的求解只需一特解就可知全部的解，可先假设已知一未知数的值，求其余未知数，即得一特解，因世界坐标系原点一般定义在测量对象上，t_x的值是不为零的，于是可假设q₁₂＝1，对Q进行求解，再按(20)式求实际的值，在该假设下，(20)式可化为：

H′Q′+q＝0                                   (21)

其中H′为H的前11列，q为H的最后1列，Q′为Q的前面11个变量。根据最小二乘法可解得Q′，而后可求得Q值，再根据(19)式求得摄像机参数。

由图像物理坐标与图像坐标的关系有：

x_d＝(u_d-u₀)dx＝(u-u₀)f/α

y_d＝(v_d-v₀)dy＝(v-v₀)f/β

x＝(u-u₀)dx＝(u-u₀)f/α

y＝(v-v₀)dy＝(v-v₀)f/β

其中x_d，y_d和x，y分别表示有畸变误差和无畸变误差的图像像素坐标，代入式(13)，得到误差系数g_i的线性方程，即可求解得到，根据以上参数可对摄像头进行标定。

摄像头标定完成后，系统即可进行对腹壁穿刺孔位置坐标的标定。首先，对图像进行二值化并标记(参见图7)，然后计算不同连通域的面积及中心距。

将连通域中的像素表示为R，则定义如下：

面积： $A=\underset{(r,c)\∈R}{\mathrm{\Σ}}1---\left(22\right)$

面积为连通域R中的像素的个数。

中心： $\stackrel{\‾}{r}=\frac{1}{A}\underset{(r,c)\∈R}{\mathrm{\Σ}}r---\left(23\right)$

$\stackrel{\‾}{c}=\frac{1}{A}\underset{(r,c)\∈R}{\mathrm{\Σ}}c---\left(24\right)$

中心(r，c)即区域R中像素的平均位置。根据连通域中心坐标计算不同连通域中心距及其所在图像坐标系矢量。

假设摄像头为理想单透镜，f_L为焦距，u为物距，v为像距，根据高斯成像公式 $\frac{1}{f_L}=\frac{1}{u}+\frac{1}{v},$ 可得： $u=\frac{\mathrm{fv}}{v-f}.$

若图像中两个连通域中心距为d个像素，机械臂在其基础坐标系沿

运动的距离为：

$L=\frac{\mathrm{df}}{10(v-f)}---\left(25\right)$

腹腔镜手术穿刺孔定位过程开始后，工业PC机首先控制机器人移动关节在其行程内上下移动，通过微型CCD摄像头判断是否捕获三个连通域，捕获三个连通域后分别计算其各自面积并进行逐一比较，通过控制移动关节的移动判断面积较小的两个连通域的中心距的变化，并最终控制移动关节使两个面积较小的连通域的中心距为0，即两个红光点状光斑激光器发出的激光在腹部皮肤处重合，此时即可得出两束激光照射的腹部位置相对机器人基础坐标系的位置(见(1)式)。此时，工业PC机再通过微型CCD摄像头判断图像中面积较大的连通域与面积较小连通域间的中心距是否为0，若为0则表示面积较大连通域与面积较小连通域重合，腹腔镜穿刺孔位置相对辅助腹腔镜手术机器人基础坐标系的位置即可求出(见(1)式)。若两个连通域的中心距不为0，则计算两连通域的中心距和所在平面矢量

，工业PC机控制机器人机械臂运动，使机械臂4的末端沿

运动一定距离(见(25)式)，再重新进行运行程序，直至三个连通域重合，此时腹腔镜手术穿刺孔相对机器人基础坐标系的位置即可求出(见(1)式)(标定流程图参见图8)。

定位系统中，红光点状光斑激光器发出的激光交于微型CCD摄像头的光轴下方180mm，其交点的位置可在工业PC机中读取，即提激光交点相对机器人基础坐标系的位置(见(1)式)；工业PC机通过微型CCD摄像头判断两束激光是否相交以及两束激光的交点和穿刺孔在图像中的相对位置关系。

使用前首先在患者腹部用红笔标记一个直径约为10mm的圆，机器人到达工作位置后，拉出安装在可伸缩式连杆末端的摄像头，程序运行，机器人能够根据本方法自动标定出穿刺孔的位置。穿刺孔位置标定完成后标定程序自动退出，降摄像头退回初始位置，避免影响机器人工作。机器人进行下一步的工作。

(四)附图说明

图1标定系统构成示意图；

图2标定系统结构示意图；

图3内镜操作手术机器人结构简图及各连杆坐标系；

图4线性模型与坐标系统图；

图5平面靶标定图像；

图6标准网格像场图；

图7图像标记流程图；

图8穿刺孔标定程序流程图；

图9控制系统结构简图。

(五)具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1，本发明的内镜操作手术机器人穿刺孔定位装置的组成包括：安装在机器人末端机械臂4上的可伸缩式连杆5，可伸缩式连杆的端部安装微型CCD摄像头3，机器人末端机械臂的下方安装有两个红光点状光斑激光器1和2，两个红光点状光斑激光器发出的光线在摄像头正下方光轴上相交，红光点状光斑激光器和微型CCD摄像头与控制工业PC机0相连。两个红光点状光斑激光器的倾角不同，一个红光点状光斑激光器的倾角为23.385°，另一个红光点状光斑激光器的倾角为37.3°。

腹腔镜手术穿刺孔定位过程开始后，工业PC机首先控制机器人移动关节在其行程内上下移动，通过微型CCD摄像头判断是否捕获三个连通域，捕获三个连通域后分别计算其各自面积并进行逐一比较，通过控制移动关节的移动判断面积较小的两个连通域的中心距的变化，并最终控制移动关节使两个面积较小的连通域的中心距为0，即两个红光点状光斑激光器发出的激光在腹部皮肤处重合，此时即可得出两束激光照射的腹部位置相对机器人基础坐标系的位置(见(1)式)。此时，工业PC机再通过微型CCD摄像头判断图像中面积较大的连通域与面积较小连通域间的中心距是否为0，若为0则表示面积较大连通域与面积较小连通域重合，腹腔镜穿刺孔位置相对辅助腹腔镜手术机器人基础坐标系的位置即可求出(见(1)式)。若两个连通域的中心距不为0，则计算两连通域的中心距和所在平面矢量，工业PC机控制机器人机械臂运动，使机械臂4的末端沿

运动一定距离(见(25)式)，再重新进行运行程序，直至三个连通域重合，此时腹腔镜手术穿刺孔相对机器人基础坐标系的位置即可求出(见(1)式)(标定流程图参见图8)。

定位系统中，红光点状光斑激光器发出的激光交于微型CCD摄像头的光轴下方180mm，其交点的位置可在工业PC机中读取，即提激光交点相对机器人基础坐标系的位置(见(1)式)；工业PC机通过微型CCD摄像头判断两束激光是否相交以及两束激光的交点和穿刺孔在图像中的相对位置关系。

Claims (1)

1、一种内镜操作手术机器人穿刺孔定位装置，其特征是：它包括：安装在机器人末端机械臂上的可伸缩式连杆，可伸缩式连杆的端部安装微型CCD摄像头，机器人末端机械臂的下方安装有两个红光点状光斑激光器，两个红光点状光斑激光器发出的光线在摄像头正下方光轴上相交，红光点状光斑激光器和微型CCD摄像头与控制工业PC机相连；两个红光点状光斑激光器的倾角不同，一个红光点状光斑激光器的倾角为23.385°，另一个红光点状光斑激光器的倾角为37.3°；用工业PC机首先控制机器人移动关节在其行程内上下移动，通过微型CCD摄像头判断是否捕获三个连通域，捕获三个连通域后分别计算其各自面积并进行逐一比较，通过控制移动关节的移动判断面积较小的两个连通域的中心距的变化，并最终控制移动关节使两个面积较小的连通域的中心距为0，即两个红光点状光斑激光器发出的激光在腹部皮肤处重合，求出两束激光照射的腹部位置相对机器人基础坐标系的位置；此时，工业PC机再通过微型CCD摄像头判断图像中面积较大的连通域与面积较小连通域间的中心距是否为0，若为0则表示面积较大连通域与面积较小连通域重合，求出腹腔镜穿刺孔位置相对辅助腹腔镜手术机器人基础坐标系的位置；若两个连通域的中心距不为0，则计算两连通域的中心距和所在平面矢量工业PC机控制机器人机械臂运动，使机械臂的末端沿

运动一定距离，再重新进行运行程序，直至三个连通域重合，求出腹腔镜手术穿刺孔相对机器人基础坐标系的位置。

Images