[发明专利]一种主从式手术机器人控制方法

权利要求书

1.一种主从式手术机器人控制方法，其特征在于：包括以下步骤：

S1、获取主手机器人各关节的位置信息和速度信息，利用构造法计算主手机器人的雅可比矩阵，经过主手机器人正运动学计算得到各关节在笛卡尔空间的位置信息和速度信息，并经过主手机器人正动力学计算得到各连杆与关节之间的力和力矩迭代方程；

S2、根据主手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得的各关节在笛卡尔空间的位置信息及力和力矩的关系信息计算各连杆的重力补偿系数，并将重力对各关节的力和力矩作用映射至各关节；

S3、根据末端受力情况，以及重力对各关节的作用，经过逆动力学计算得到各关节的期望力矩，并根据实时反馈的当前实际力反馈信息，引入内环/外环控制架构进行控制；

S4、根据主从手机器人比例及坐标映射关系得到从手机器人末端在笛卡尔空间的位置信息及速度信息，再经过从手机器人运动学位置逆解及基于逆雅可比矩阵变换得到从手机器人各关节的位置信息和速度信息，并计算各连杆与关节之间的力和力矩迭代方程；

S5、根据从手机器人各连杆质量及惯性张量计算惯性矩阵，根据已求得位置及力和力矩的信息对从手机器人进行重力补偿，实时获取从手执行端的力反馈信息，引入内环/外环控制架构进行控制。

2.根据权利要求1所述的一种主从式手术机器人控制方法，其特征在于：在所述步骤S1与S4中，主手机器人与从手机器人的雅克比矩阵计算的构造法，

上半部分下半部分其中，z_i-1为第i个关节在基坐标系中表示的三维向量，o_n为末端执行器坐标原点在基坐标系中表示的三维向量，o_i-1为第i个关节坐标原点在基坐标系中表示的三维向量，为雅可比矩阵的上半部分，为雅可比矩阵的下半部分。

3.根据权利要求1所述的一种主从式手术机器人控制方法，其特征在于：在所述步骤S1与S4中，主手机器人与从手机器人各连杆与关节之间的力迭代方程力矩迭代方程其中I为连体坐标系中的惯性张量，f_i是由连杆i-1施加在连杆i上的力，为坐标系i+1到坐标系i的旋转矩阵，f_i+1是由连杆i+1施加在连杆i上的力，m_i为连杆i的质量，a_c,i连杆i质心的加速度，g_i重力引起的加速度在坐标系i中的表示，τ_i连杆i-1施加到连杆i的力矩，从坐标系i的原点到连杆i质心的向量，坐标系i相对于基坐标系的角加速度，ω_i坐标系i相对于基坐标系的角速度，τ_i+1连杆i+1施加到连杆i的力矩。

4.根据权利要求1所述的一种主从式手术机器人控制方法，其特征在于：在所述步骤S3中，各连杆的重力向量为g(q)＝[g₁(q),...,g_n(q)]^T,重力力矩对各关节的作用为力与力矩方程及各重力向量分量求得，g(q)重力引起的加速度在各坐标系中的向量表示，g_n(q)为g(q)的分量，[]^T为矩阵的转置，g(k)为总势能P对关节k的位移q_k的偏导数。

5.根据权利要求1所述的一种新型主从式手术机器人控制方法，其特征在于：在所述步骤S3与S5中，内环控制方程为M(q)为惯量矩阵，a_q表示待输入的加速度项，为离心项与科里奥项的表示，q为关节位置，为关节速度。

6.根据权利要求1所述的一种主从式手术机器人控制方法，其特征在于：在所述步骤S3与S5中，外环控制方程为其中，K₀和K₁是对角矩阵，其对角元素分别由位置增益和速度增益组成；为参考加速度，K₀与K₁为增益矩阵，为位置增益，为加速度增益。

7.根据权利要求1所述的一种主从式手术机器人控制方法，其特征在于：在所述步骤S4中，通过以下方法获得主从手机器人比例及坐标映射关系，

(1)首先通过进行主手机器人正运动学求解，得到主手机器人相对于主手机器人的基坐标系的位置变化为Δd_mb＝T_main(Δθ₁,...,Δθ₆)；θ₁～θ₆为关节位置，Δθ₁～Δθ₆为关节位置变化，为坐标系1到坐标系0的齐次变化矩阵，为坐标系2到坐标系1的齐次变化矩阵，为坐标系3到坐标系2的齐次变化矩阵，为坐标系4到坐标系3的齐次变化矩阵，为坐标系5到坐标系4的齐次变化矩阵，为坐标系6到坐标系5的齐次变化矩阵；

(2)然后通过Δd_mu＝T_bu*Δd_mb关系式，将(1)中所述的Δd_mb从主手机器人的基坐标系转换到大地坐标系中，m表示主手，b表示大地坐标系，Δd_mu为主手机器人在大地坐标系中的位置变化，T_bu为主手机器人的基坐标系相对于大地坐标系的变换矩阵；

(3)再通过主手机器人与从手器械臂间的比例变换关系Δd_su＝k*Δd_mu，根据(2)中所述的Δd_mu得到从手器械臂相对于大地坐标系的位置变化Δd_su，其中k为比例因子；

(4)再通过Δd_sd＝T_u-d*Δd_su关系式，根据T_u-d和(3)中所述的Δd_su，将从手器械臂相对于大地坐标系的位置变化转换为从手器械臂相对于显示器坐标系的位置变化Δd_sd，其中T_u-d为从大地坐标系到显示器坐标系的变换矩阵；

(5)通过Δd_sc＝Δd_sd关系式，根据(4)中所述的Δd_sd，得到从手器械臂相对于从手持镜臂的位置变化Δd_sc；

(6)通过从手器械臂正运动学关系求得从手器械臂末端相对于从手持镜臂基坐标系的变换矩阵；最后通过Δd_sb＝T_slaveΔd_sc关系式，根据(5)中所述的Δd_sc得到从手器械臂相对于从手执行端基坐标系的运动；其中，θ₁～θ₆为关节位置，为坐标系1到坐标系0的齐次变化矩阵，为坐标系2到坐标系1的齐次变化矩阵，为坐标系3到坐标系2的齐次变化矩阵，为坐标系4到坐标系3的齐次变化矩阵，为坐标系5到坐标系4的齐次变化矩阵，为坐标系6到坐标系5的齐次变化矩阵。