具体实施方式

本发明应用的主操作臂是授权公告号为CN210872029U，名称为医生操作台的实用新型专利中的医生机械臂。应用的从操作臂是专利号为201711314221.6，名称为微创手术主操作臂的机械臂。

如图1所示的主操作臂，第一臂杆2的后端与基座1通过第一关节连接，第一臂杆2能够在水平面上转动，转动连接座5与第一臂杆2的前端通过第二关节连接，转动连接座5能够在水平面上转动，第二臂杆3与转动连接座5通过第三关节连接，第二臂杆3能够在垂直面上转动。

手术过程中，医生用手握住手腕4进行操作，主操作臂各关节分别绕第一关节的关节轴线一14、第二关节的关节轴线二15、第三关节的关节轴线三16转动，其中，关节轴线三16与重力方向垂直，关节轴线一14、关节轴线二15与地面垂直。

如图2和3所示，手腕4包括固定连杆4-1、第一L型连杆4-2、第二L型连杆4-3、手柄4-4、开合座4-5、第一编码器4-6、第二编码器4-7、第三编码器4-8。手腕4设有关节M4、关节M5、关节M6。

如图4所示的主操作臂位置调整构型图，其中M1、M2、M3三个关节是图1所示结构图中的第一关节、第二关节、第三关节。如图3和11，M4、M5、M6三个关节为姿态调整关节，这6个关节的自由度分别决定主操作臂的末端在笛卡尔坐标系中的位置和姿态。根据上述主操作臂构型，本发明可以采用D-H法进行主操作臂的运动学分析，建立其D-H参数表，D-H参数表包括M1、M2、M3三个关节的连杆长度a_i，连杆偏移量d_i，连杆转角α_Mi和关节角θ_Mi。比如图1中，a₁表示关节M1的轴线和关节M2的轴线之间的距离，a₂表示关节M2的轴线和关节M3的轴线之间的距离，a₃表示关节M3的轴线与手腕4之间的距离。连杆偏移量是指两相邻公垂线沿轴方向的距离，连杆偏移量d_i均是0。已知各关节的参数，根据空间变换的原理得到两个相邻关节之间的齐次变换矩阵如下所示。

根据上式建立各个相邻关节之间的变换，然后控制器根据下述公式可求主操作臂末端坐标系(即手腕4坐标系)相对于基坐标系的齐次变换矩阵i取值1、2、3，从齐次变换矩阵即可得到主操作臂末端相对于基坐标系的位置。

还可以写成下面这种形式：

然后令：

P_M即为主操作臂的手腕4的位置坐标。

在主操作臂末端位置点建立相应的动态坐标系，根据以下公式计算出手腕4三个关节M4、M5、M6的转动角度值θ_Mi，表示手腕4的姿态。

θ_Mi＝A_Mi/μ_Mi

其中，A_Mi表示该关节编码器数值的变化量，μ_Mi为该关节编码器精度、减速器减速比值与机械结构传动比值的积。参考图3，θ_M4为关节M4的转动角度值，对应的编码器是第一编码器4-6，θ_M5为关节M5的转动角度值，对应的编码器是第二编码器4-7，θ_M6为关节M6的转动角度值，对应的编码器是第三编码器4-8。

如图5-7所示，从操作臂包括从端基座201、连杆座202、从端连杆I203、从端连杆II204、器械升降座206、器械安装装置205。连杆座202旋转地安装在从端基座201的一端，其旋转轴线与重力方向一致。手术器械207可安装在器械安装装置205上。

从操作臂的构型如附图8所示，关节S1为使连杆座202、从端连杆I203、从端连杆II204和器械升降座206整体横向移动的旋转关节，关节S2是平行四边形绕轴线俯仰旋转关节(使从端连杆I203、从端连杆II204、器械升降座206伸展或折叠)，关节L3是器械安装装置205沿着器械升降座206移动的关节，关节S1、关节S2确定了远端不动点(图7中的P点)的位置，在远端不动点轴线上下移动的关节L3决定了P点的位置。如图9和10所示，手术器械采用“自传-偏转-末端自传”式腕部自由度布局，手术器械末端腕部的关节S4、关节S5、关节S6决定了手术器械末端的姿态。同理，从操作臂端也采用D-H参数法建立从手的运动学模型，依据上述齐次变换矩阵计算公式求得从操作臂的末端在基坐标系中的齐次变换矩阵从齐次变换矩阵中可以得到从操作臂手术器械末端在基坐标系中的位置。同样可以根据以下公式计算出手术器械腕部三个关节的转动角度值θ_Si，分别是θ_S4、θ_S5、θ_S6，表示从操作手器械末端腕部的姿态。

θ_Si＝A_Si/μ_Si

其中A_Si表示该关节编码器数值的变化量，μ_Si为该关节编码器精度、减速器减速比值与机械结构传动比值的积。

根据设计的主操作臂与从操作臂的构型，手术器械及其末端同构式主手的腕部三个关节轴线交于一点，可实现运动学分析中的位置与姿态解耦。结合结构特点，本发明建立位姿分离、末端同构的主从映射。在位置运动映射中，需将主操作臂的位置基坐标系映射到从操作臂描述的基坐标系中，映射后得到从操作臂位置的齐次变换矩阵如下：

其中，为主操作臂基坐标系到从操作臂基坐标系的转换矩阵，为主从映射的比例因子，将主操作手的运动按照一定的比例缩小后映射为操作手的运动。还可以写成下面这种形式：

从操作臂位置坐标为根据从机械臂构型按照D-H参数法推出从操作臂关节S1、关节S2、关节L3的齐次变换矩阵：

其中，矩阵中的各元素均是关节1至关节3参数(θ_s1、θ_s2、l_s3)的表达式。根据以上描述建立如下关系式：

p_sx＝x_s

p_sy＝y_s

p_sz＝z_s

根据这三个方程可以求出关节S1、关节S2、关节L3的未知参数θ_s1、θ_s2、l_s3。

而器械与主手末端的姿态映射采用“一对一”映射方式实现。因此从操作臂器械末端腕部三关节的期望角度值计算公式如下：

θ_s4＝θ_M4

θ_s5＝θ_M5

θ_s6＝θ_M6

其中，θ_M4、θ_M5、θ_M6分别表示主操作手腕部三个关节的实际转动角度值。然后利用运动控制器给从操作臂输出相应的数值，达到主从跟随的目的。本发明根据主从操作臂构型，即位姿分离、末端同构的思路提供了一种微创手术机器人简化运动学模型的建模方法，简化了整个运动学计算过程的复杂程度，提高了运动控制系统的实时性。

以上所述仅对本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。