[发明专利]基于旋量理论的手术机器人约束运动控制方法

权利要求书

1.一种机器人手术系统，该系统包括：人机交互设备、手术机器人和手术工具，所述手术机器人采用一种基于旋量理论的手术机器人约束运动控制方法进行控制，其特征在于，所述基于旋量理论的手术机器人约束运动控制方法包括：S1，RCM点标定；S2，虚拟RCM点与切口孔对准；S3，生成RCM约束；S4，控制末端执行器运动；

其中，步骤S1包括：S11：双目相机和机器人基坐标位姿的确定；S12：工具轴线在末端执行器坐标中的确定；S13：定义RCM点在轴线上的位置确定，采用双目相机、视觉辅助标记物和视觉探针对虚拟RCM点在工具轴上的位置进行标定；

S11包括：

首先，固定双目相机(7)和手术机器人(3)的位置，将手术机器人末端执行器上的标记物置于双目相机(7)的视野下，保证标记物同时出现在两个镜头视野内；采集辅助标记物坐标系相对于双目相机坐标系的位姿，记为与此同时，通过手术机器人运动学程序采集手术机器人基坐标系相对于末端执行器坐标系的位姿，记为改变末端执行器的位置和姿态，重复上述步骤，采集多组数据，利用机器人手眼标定公式：AX＝XB，求解双目相机坐标系相对于手术机器人基坐标系的位姿关系，记为位姿关系可描述为：其中，和分别是第i组和第j组中的和同理；最后通过极大似然估计方法得到

S12包括：

将手术工具安装在末端执行器上，虚拟RCM点始终在手术工具的轴线上，由于安装、匹配误差导致手术工具在实际中的位置和模型中不一致，因此采用视觉探针在手术工具长轴的表面取点，获取工具轴上向量；

S12进一步包括：

通过预先注册，双目相机在识别到视觉探针的角点后计算出探针尖端的三维坐标信息，在双目相机视野下，使用探针尖端在手术工具外鞘近端上任一点A点的圆周一圈取四个点，通过计算平均值获得A点圆心在双目相机坐标系下的三维坐标，同理可获得外鞘远端的任一点B点的三维坐标；这样得到了双目相机坐标系下手术工具轴线上的向量^CP_AB；由S11计算得到的双目相机相对于手术机器人基坐标系的位姿以及很容易获取的基坐标系相对于末端执行器的位姿可得到向量在末端执行器坐标系下的位姿：是的逆矩阵，进而得到手术工具轴线的单位方向向量n_d，同理，可获得手术工具尖端中心点在{E}下的三维坐标^Ep_tip；

S13包括：

结合患者的医学影像设定从手术工具尖端点到轴线上虚拟RCM点的偏移量，得到轴线上虚拟RCM点的坐标^Ep_rcm＝^Ep_tip-l_os*n_d，l_os为医生结合患者的医学影像设定从手术工具尖端点到轴线上虚拟RCM点的偏移量；

S3包括：

将RCM运动表示为六维特殊欧式群SE(3)中的绕旋量轴的刚体运动；SE(3)是三维实向量空间和特殊正交群SO(3)的积空间；特殊正交群SO(3)也称为旋转矩阵群，是一个描述绕坐标系原点旋转的空间；根据旋量理论，刚体的运动都可以分解为绕旋转轴的旋转运动和沿着坐标轴的平移运动；运动旋量ξ可以表示为一个六维向量形式：其中，ν表示刚体运动的线速度，表示旋转运动的一个单位向量ω＝[ω₁,ω₂,ω₃]^T，ω对应的反对称矩阵为是从坐标系原点指向旋转轴上任一点的向量，h为螺旋的节距，因此，运动旋量的矩阵形式表示为运动旋量采用螺旋轴的表达形式，即其中，为旋转轴的单位向量，θ是旋转角度或距离；变化量记为Δ＝(α,β,γ,x,y,z)，其中，α,β,γ分别表示人机交互设备绕自身坐标系的z轴,y轴,x轴的旋转增量，x,y,z表示沿x轴，y轴，z轴移动增量；在末端执行器的坐标系{E}中，以虚拟RCM点^Ep_tip作为运动向量中的q，以S12中得到的手术工具轴线的单位方向向量n_d作为绕轴的旋转角度作为θ，建立运动旋量；同时，以点^Ep_tip为原点，以n_d作为z轴，任意给定{E}中的一个向量作为x轴方向，通过向量叉积得到y轴，进而得到{E}的附属坐标系{R}；将人机交互设备输出的α,β,γ,x,y,z经过适当的比例控制策略，得到α′,β′,γ′,x′,y′,z′，通过矩阵指数表达方式得到机器人末端执行器在附属坐标系{R}下变化矩阵T，即通过得到T∈SE(3)；其求解公式为：

对于旋量若||ω||＝1，则对于任意沿螺旋轴距离都有

若||ω||＝0，||v||＝1，则I表示单位矩阵，ω表示刚体螺旋运动的旋转角速度。