CN113262052A - 面向外科手术的主从式柔性连续体机器人及其控制方法 - Google Patents

Abstract

面向外科手术的主从式柔性连续体机器人及其控制方法，属于涉及仿生机器人领域。本发明包括机器人从手和机器人主手，所述机器人从手包括首尾相连的第一从手构件、第二从手构件和第三从手构件，第一从手构件、第二从手构件和第三从手构件采用线驱动方式，由四根绳索连接在相应构件的末端，通过改变绳索的长度来驱动第一从手构件、第二从手构件和第三从手构件的自由度变化；所述机器人主手包括三个结构相同且长度可变的直杆依次相连，每个直杆内均安装有姿态传感器。本发明的机器人及控制方法具有精准高、操作灵活可靠的、响应速度快、实时性高的优点。

Description

面向外科手术的主从式柔性连续体机器人及其控制方法

技术领域

本发明涉及主从式柔性连续体机器人，尤其涉及面向外科手术的主从式柔性连续体机器人及其控制方法，属于涉及仿生机器人领域。

背景技术

机器人因为其高精度被逐渐应用到医疗手术中，现阶段已有许多刚性手臂用于医疗手术中，这种医疗手术用手臂结构简单易于操作控制。传统刚性结构的机器人，能够针对特定的任务需要实现目标要求，但刚性机器人多由杆和各种运动副实现运动，其灵活性较差。由于人体内环境复杂，刚性手臂受制于结构特点，不够灵活，在人体内运动的空间有限。而自然界中诸如章鱼手臂和蛇等，有很好的避障能力和很大的移动空间，仿照这些生物的特点，能够设想出一种灵活弯曲的连续体柔性操作臂，在末端夹持操作器械，这种机器人能够灵活弯曲适应复杂的体内环境。

现已有的柔性机械有仿蛇形机器人和仿象鼻机器人等。

现有仿蛇形机器人由柔性主体和主控器两部分组成，其中柔性主体由弹簧，支架，驱动装置和顶盖组成，多段柔性主体相连，能够向各方向弯曲。置于机器人尾端的主控制器控制每个柔性主体末端的控制板，以实现每个柔性主体的运动。具有很好的承载能力。但这种采用主控制器控制柔性主体的机器人，由于主控制器与柔性主体连为一体，受结构限制，在医用手术时，必须近程控制，这样就使得其操作灵活性差，且操作过程易与患者发生碰撞，其危险性高；

现有仿象鼻机器人用磁流变液技术，外层由多段弹簧串联，中心有一根脊柱，脊柱中充有形态随磁场变化的特殊液体，当机器人通电时，外圈使内部磁场发生变化，机器人导管内的特殊液体实现液固转换，从而带动机器人弯曲，实现了传统刚性结构机器人抓取不规则形状物体困难的问题，但是这种采用固液转换的方式实现机器人的控制精度低，若脊柱中中特殊液体液固转换出现误差，则直接导致机器人操作不到位或操作过当，产生的后果不堪设想。

基于上述陈述，亟需提出一种精准的、操作灵活可靠的、响应速度快、实时性高的用于外科手术的机器人及机器人控制方法，以适应外科手术的狭窄工作环境，并实现精确控制，确保医生操作该机器人时，不会导致机器人产生意外运动，避免手术过程中机器人意外与人体接触，造成损伤患者的影响。

发明内容

本发明提出了一种控制精度高、操作灵活。响应速度快的面向外科手术的主从式柔性连续体机器人及其控制方法；本发明的技术方案如下：

方案一：

面向外科手术的主从式柔性连续体机器人，包括机器人从手和机器人主手，所述机器人主手通过驱动器与线驱动部建立连接关系，线驱动部用于控制机器人从手动作；

所述机器人从手包括首尾相连的第一从手构件、第二从手构件和第三从手构件，第一从手构件、第二从手构件和第三从手构件采用线驱动方式，由四根绳索连接在相应构件的末端，通过改变绳索的长度来驱动第一从手构件、第二从手构件和第三从手构件的自由度变化；

所述机器人主手包括三个结构相同且长度可变的直杆依次相连，每个直杆内均安装有姿态传感器。

优选的：所述第一从手构件包括顶端单元节和四个第一中间单元节，顶端单元节和四个第一中间单元节通过弹簧4建立配合安装；

第二从手构件包括五个第二中间单元节，五个第二中间单元节依次通过弹簧建立配合，位于第一从手构件和第二从手构件相配合处的第一中间单元节和第二中间单元节通过弹簧建立配合；

第三从手构件包括四个第三中间单元节和底端单元节，底端单元节和四个第三中间单元节通过弹簧建立配合安装，位于第二从手构件和第三从手构件相配合处的第二中间单元节和第三中间单元节通过弹簧建立配合。

优选的：所述顶端单元节和底端单元节的结构相同，均包括圆盘和设在圆盘其中一端面中心位置的锥体安装部；

所述第一中间单元节、第二中间单元节和第三中间单元节的结构相同，均包括第一圆盘和设在第一圆盘两侧端面中心位置的第一锥体安装部。

优选的：所述直杆包括外壳体和伸缩杆，外壳体的一端具有敞口，外壳体的另一端具有套接部，外壳体的侧壁上加工有长孔，伸缩杆为圆形杆，伸缩杆的外侧壁上具有凸起部，伸缩杆安装在外壳体内，伸缩杆上的凸起部与外壳体上的长孔配合安装。

方案二：

面向外科手术的主从式柔性连续体机器人控制方法，其目的是通过控制绳索的变化量，实现控制机器人从手，本主从式机器人的从手包括第一从手构件、第二从手构件和第三从手构件，三个构件首尾相连组成。每个构件均由多个单元节组成。构件间有弹簧作为具有一定柔性的支撑。每个构件分别采用线驱动的方式，由4个绳索连接在构件的末端，通过改变绳索的长度来驱动杆件的偏转和弯曲两个自由度的变化，这两个自由度可以用偏转角和弯曲角两个数据加以描述，因此从机器人的顶端到末端的三个构件分别有4、8、12根穿过，共操作6个自由度。每个构件所弯曲形成的姿态皆可近似为一个圆弧，可弯曲成任意形状的圆弧。

控制模块从主手端得到传感器的数据，通过计算得到从手端每个构件所需要的偏转角和弯曲角的变化量，从而得到驱动每个从手构件所需的4根自身驱动线长度变化量。

由于上端的驱动线会穿过下端的构件，所以下端构件的运动会影响上端的线长变化。通过对多端运动的解耦和计算，可得12根独立的线长变化量。并驱动12个与绳索一一对应的电机来实现长度变化。

具体绳索长度变化控制实现机器人从手控制的方法包括以下步骤：

步骤一、三个机器人主手的每个末端姿态传感器分别传出相对于世界坐标系的X-Y-Z固定角，以表示每个主手构件末端姿态的坐标系，设姿态传感器角度分别为ɑ₁、β₁、γ₁和ɑ₂、β₂、γ₂以及ɑ₃、β₃、γ₃，设每个主手构件末端坐标系相对于自身底端坐标系的X-Y-Z固定角的3*3旋转矩阵为R_m，而末端坐标系相对于固定世界坐标系的3*3旋转矩阵为R，由偏转角和弯曲角表达的3*3旋转矩阵为R_n；

步骤二、根据旋转矩阵利用每组欧拉角求得代表偏转和弯曲的偏转角和弯曲角θ′₁、

和θ′₂、

以及θ′₃、

使末端坐标系相对于底端坐标系的X-Y-Z固定角的3*3旋转矩阵R₁(R_m1)与偏转角弯曲角的3*3旋转矩阵R_n1相等，求得主手构件末端的弯曲角和偏转角θ′₁、

式中，c表示Cos余弦函数，s表示Sin正弦函数，a₁₁、a₁₂、a₁₃、a₂₁、a₂₂、a₂₃、a₃₁、a₃₂、a₃₃分别对应＝旋转矩阵R_n1中各元素，例如α₁₁＝cα₁cβ₁；

由于姿态传感器的参数是相对于固定世界坐标系得到的，而第二个构件末端坐标系是相对于自身底端坐标系而言的，所以需要坐标变换来求得R_m2；

R_m2R₁＝R₂

解得R_m2后，设

则同理第一个构件，与偏转角和弯曲角表达的3*3旋转矩阵R_n2相等，求得弯曲角θ′₂和偏转角

同理可求得θ′₃和

由于主手构件相当于从手对应构件所呈圆弧的弦，因此从手构件与主手构件的偏转角相等，即

而弯曲角为为主手构件弯曲角的二倍，即θ_i＝2θ′_i；

步骤三、设每个从手构件圆盘的厚度为h，直径为d，竖直状态下的原始长度为1，而每个构件有n个圆盘构成，根据已经求出的各个构件的偏转角和弯曲角，求得每个构件所需的驱动绳长的变化量，设dij为第i个构件(i＝1、2、3)的第j根驱动绳(j＝1、2、3、4)的所需变化量，则：

再将下部绳长变化对上部绳长变化的影响耦合起来，获得12根绳长各自的实际变化量：

本发明有益效果体现在：

一种主从同构式的连续体机器人，主要有从手、主手和控制器组成，医生操控主手，位移传感器收集参数信息，控制器操控从手复现主手动作。其中，主手是被动式，由医生操作弯曲成需要的形状，而从手是主动式，由电机控制。由于连续体机器人自由度高，对于同一目标点的路径可能有多种解，同时直接由计算机控制机器人过于抽象，自由度越多运动学逆解越多解算越复杂，实时性不高，主从式操作能够避免这一问题，从手不再需要计算机得出路径解，只需要精准的复现主手的操作即可，响应速度快实时性高。

从手采用了更加精小的设计方案，以适应外科手术狭窄的操作环境；提出了主从异构的设计方法，以解决主手难以固定和医生操作时从手其他构件易产生意外运动的问题，避免手术过程中从手的非手术操作部分与人体内部的意外接触；主手端采用姿态传感器而不是位置传感器来记录主手的形变，使从手可以更加准确地跟踪主手的运动方向；主手端的每段直杆长度随着医生操作直杆的方向变化而变化，在从手复现主手偏转和弯曲方向的基础上，主手直杆长度实时变化，以实现主手每个构件的末端位置与从手每个直杆末端的末端位置完全相同，实现只操作主手末端点，从手末端点即可到达同一位置，使医生操作更加直观和方便。

本发明的机器人及控制方法具有精准高、操作灵活可靠的、响应速度快、实时性高的优点。

附图说明

图1为面向外科手术的主从式柔性连续体机器人的从手；

图2为机器人从手的弯曲状态示意图；

图3为顶端单元节的立体图；

图4为第一中间单元节的立体图；

图5为第二中间单元节的立体图；

图6为第三中间单元节的立体图；

图7为底端单元节的立体图；

图8为机器人主手的立体图；

图9为外壳体的立体图；

图10为伸缩杆的立体图；

图11为偏转、弯曲角与XYZ固定角的关系图；

图12为从手关节物理量标识图；

图13为主手关节物理量标识图；

图14为从手控制流程图；

图15为主手控制流程图；

图中，1-机器人从手，2-机器人主手，3-绳索，4-弹簧，5-直杆，11-第一从手构件，12-第二从手构件，13-第三从手构件，111-顶端单元节，112-第一中间单元节，113-第二中间单元节，114-第三中间单元节，115-底端单元节，1111-圆盘，1112-锥体安装部，1121-第一圆盘，1122-第一锥体安装部，51-外壳体，52-伸缩杆，53-套接部，54-长孔，55-凸起部。

具体实施方式

为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

具体实施方式一：

本主从式机器人的从手如图1所示，三个构件首尾相连组成，具体为第一从手构件11、第二从手构件12和第三从手构件13，三个构件首尾相连组成。每个构件由图1-图7所示的顶端单元节111、四个第一中间单元节112、五个第二中间单元节113、四个第三中间单元节114和底端单元节115组成，共有15个关节。构件间有弹簧4作为具有一定柔性的支撑。每个构件分别采用线驱动的方式，由4个绳索连接在构件的末端，通过改变绳索的长度来驱动杆件的偏转和弯曲两个自由度的变化，这两个自由度可以用偏转角和弯曲角两个数据加以描述，因此从机器人的顶端到末端的三个构件分别有4、8、12根穿过，共操作6个自由度。由于机器人的顶端到末端的三个构件分别有4、8、12根穿过，因此在第一中间单元节112、第二中间单元节113、第三中间单元节114和底端单元节115上加工有穿线孔(如图4-图7所示)，该穿线孔用于穿过绳索3，随着绳索3的数量增多，在相应的单元节上的孔数量也随之增加，例如在第一中间单元节112上加工有四个穿线孔，在第二中间单元节113上加工有八个穿线孔，第三中间单元节114和底端单元节115上加工有十二个穿线孔；

每个构件所弯曲形成的姿态皆可近似为一个圆弧，可弯曲成任意形状的圆弧。

本实施方式中的面向外科手术的主从式柔性连续体机器人，还包括控制模块，控制模块从主手端得到传感器的数据，通过计算得到从手端每个构件所需要的偏转角和弯曲角的变化量，从而得到驱动每个构件所需的4根自身驱动线长度变化量。由于上端的驱动线会穿过下端的构件，所以下端构件的运动会影响上端的线长变化。通过对多端运动的解耦和计算，可得12根独立的线长变化量。并驱动12个与绳索一一对应的电机来实现长度变化。

本实施方式中的面向外科手术的主从式柔性连续体机器人，还包括线驱动部，线驱动部接收控制模块发出的指令，并用于控制机器人从手1动作，线驱动部拉动或松懈绳索3进而生成从手动作；在本实施例中线驱动部可以是DC马达、伺服电机、步进电机等，但本发明不限于此。

面向外科手术的主从式柔性连续体机器人，包括机器人从手1和机器人主手2，所述机器人主手2通过驱动器与线驱动部建立连接关系，线驱动部用于控制机器人从手1动作；

所述机器人从手1包括首尾相连的第一从手构件11、第二从手构件12和第三从手构件13，第一从手构件11、第二从手构12和第三从手构13件采用线驱动方式，由四根绳索3连接在相应构件的末端，通过改变绳索3的长度来驱动第一从手构件11、第二从手构/12和第三从手构件13的自由度变化；

所述机器人主手2包括三个结构相同且长度可变的直杆5依次相连，每个直杆5内均安装有姿态传感器。

优选的：所述第一从手构件11包括顶端单元节111和四个第一中间单元节112，顶端单元节111和四个第一中间单元节112通过弹簧4建立配合安装；

第二从手构件12包括五个第二中间单元节113，五个第二中间单元节113依次通过弹簧4建立配合，位于第一从手构件11和第二从手构件12相配合处的第一中间单元节112和第二中间单元节113通过弹簧4建立配合；

第三从手构件13包括四个第三中间单元节114和底端单元节115，底端单元节115和四个第三中间单元节114通过弹簧4建立配合安装，位于第二从手构件12和第三从手构件13相配合处的第二中间单元节113和第三中间单元节114通过弹簧4建立配合。

优选的：所述顶端单元节111和底端单元节115的结构相同，均包括圆盘1111和设在圆盘1111其中一端面中心位置的锥体安装部1112；

所述第一中间单元节112、第二中间单元节113和第三中间单元节114的结构相同，均包括第一圆盘1121和设在第一圆盘1121两侧端面中心位置的第一锥体安装部1122。

优选的：所述直杆5包括外壳体51和伸缩杆52，外壳体51的一端具有敞口56，外壳体51的另一端具有套接部53，外壳体51的侧壁上加工有长孔54，伸缩杆52为圆形杆，伸缩杆52的外侧壁上具有凸起部55，伸缩杆52滑动安装在外壳体51内，伸缩杆52上的凸起部55与外壳体51上的长孔54配合安装，如图10所示，姿态传感器安装在伸缩杆52的左端面上。

从手的结构与控制方法

本主从式机器人包括主手、控制模块、驱动部和从手，控制模块从主手端得到传感器的数据，通过计算得到从手端每个构件所需要的偏转角和弯曲角的变化量，从而得到驱动每个构件所需的4根自身驱动线长度变化量，并通过驱动部调节驱动线长度完成控制从手做出相应动作，在本实施例中，从手的控制算法是：

三段主手构件的每个末端姿态传感器分别传出相对于世界坐标系的X-Y-Z固定角，以表示每个构件末端姿态的坐标系，设姿态传感器角度分别为ɑ₁、β₁、γ₁和ɑ₂、β₂、γ₂以及ɑ₃、β₃、γ₃。设每个构件末端坐标系相对于自身底端坐标系的X-Y-Z固定角的3*3旋转矩阵为R_m，而末端坐标系相对于固定世界坐标系的3*3旋转矩阵为R，由偏转角和弯曲角表达的3*3旋转矩阵为R_n。

由图11所示，根据旋转矩阵可用每组欧拉角求得代表偏转和弯曲的偏转角和弯曲角θ′₁、

和θ′₂、

以及θ′₃、

使末端坐标系相对于底端坐标系的X-Y-Z固定角的3*3旋转矩阵R₁(R_m1)与偏转角弯曲角的3*3旋转矩阵R_n1相等，可求得主手构件末端的弯曲角和偏转角θ′₁、

由于姿态传感器的参数是相对于固定世界坐标系得到的，而第二个构件末端坐标系是相对于自身底端坐标系而言的，所以需要坐标变换来求得R_m2。

R_m2R₁＝R₂

解得R_m2后，设

则同理第一个构件，与偏转角和弯曲角表达的3*3旋转矩阵R_n2相等，求得弯曲角θ′₂和偏转角

同理可求得θ′₃和

由于主手构件相当于从手对应构件所呈圆弧的弦，因此从手构件与主手构件的偏转角相等，即

而弯曲角为为主手构件弯曲角的二倍，即θ_i＝2θ′_i。

如图12所示，设每个从手构件圆盘的厚度为h，直径为d，竖直状态下的原始长度为l，而每个构件有n个圆盘构成。根据已经求出的各个构件的偏转角和弯曲角，可以求得每个构件所需的驱动绳长的变化量。设dij为第i个构件(i＝1、2、3)的第j根驱动绳(j＝1、2、3、4)的所需变化量(非实际变化量，耦合后才是实际变化量)。

再将下部绳长变化对上部绳长变化的影响耦合起来，可得12根绳长各自的实际变化量。

如图14所示，从手控制流程具体为：首先医生操作使主手形状发生改变，通过控制模块获得安装在主手端姿态传感器的数据，具体是分别获得三个姿态传感器1的参数(主手姿态传感器1的参数、主手姿态传感器2的参数和主手姿态传感器3的参数)，然后计算从手构件1所需弯曲角和偏转角、从手构件2所需弯曲角和偏转角和从手构件3所需弯曲角和偏转角，随后分别计算从手构件1达到对应姿态所需的驱动线(绳索3)长度变化量、计算从手构件2达到对应姿态所需的驱动线(绳索3)长度变化量、计算从手构件3达到对应姿态所需的驱动线(绳索3)长度变化量，随后分别通过解耦计算得到从手构件1的驱动线长独立变化量、通过解耦计算得到从手构件2的驱动线长独立变化量和通过解耦计算得到从手构件3的驱动线长独立变化量，最终将三个驱动线长独立变化量发送至用驱动部，通过驱动电机实现相应线长的变化，完成从手控制。

主手的结构与控制方式

本机器人的主手如图8-10所示，采用三个同构且长度可变的直杆依次相连，直杆5包括外壳体51和伸缩杆52，外壳体51的一端具有敞口56，外壳体51的另一端具有套接部53，外壳体51的侧壁上加工有长孔54，伸缩杆52为圆形杆，伸缩杆52的外侧壁上具有凸起部55，伸缩杆52滑动安装在外壳体51内，伸缩杆52上的凸起部55与外壳体51上的长孔54配合安装；

相邻的直杆5的数量为三个，相邻的直杆5中的伸缩杆52安装在外壳体51的套接部53内进而实现依次相连；

主手的每个直杆与从手的每个构件一一对应。在各个直杆的末端装有姿态传感器，如图10所示，姿态传感器安装在伸缩杆52的左端面上。传感器的方向沿前一直杆向上，实时读取由操作者旋转各个直杆时所造成的实时方向，并将方向数据传入控制模块。各个姿态传感器决定了从手端各个构件所形成的圆弧的偏转角和弯曲角。将主手的各个可变长度的直杆作为对应从手构件的圆弧的虚拟弦，随着圆弧的变化，不断变化直杆的长度使其能够跟随姿态传感器所对应圆弧的弦长变化，可使从手的每个构件的末端点完全复现主手每个直杆的末端点在笛卡尔空间下的位置和姿态。在控制模块中通过计算得到的直杆所需的长度变化，长度变化由各个直杆中内含电机实现。

由上可知，已求得θ_i、

如图13所示，设主手各个杆件的长度为f，由于主手需要实时调整长度使末端点与从手的末端点对齐，可根据弯曲角求得第i根杆件(i＝1、2、3)所需变化长度，具体公示如下：

如图15所示，主手控制流程具体为：首先医生操作使主手形状发生变化，其次获得三个主手姿态传感器的参数，具体是获得主手姿态传感器1的参数、主手姿态传感器2的参数和主手姿态传感器3的参数，随后计算从手构件1所需弯曲角

和偏转角θ₁、计算从手构件2所需弯曲角

和偏转角θ₂，计算从手构件3所需弯曲角

和偏转角θ₃；最后分别计算主手直杆1末端点跟踪从手构件1末端点所需的杆长变化、主手直杆2末端点跟踪从手构件2末端点所需的杆长变化和主手直杆3末端点跟踪从手构件3末端点所需的杆长变化，并驱动电机实现相应杆长的变化。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的技术方案的基础之上，所做的任何修改、等同替换、改进等，均应包括在本发明的保护范围之内。

Claims (5)

1.面向外科手术的主从式柔性连续体机器人，其特征在于：包括机器人从手(1)和机器人主手(2)；

所述机器人从手(1)包括首尾相连的第一从手构件(11)、第二从手构件(12)和第三从手构件(13)，第一从手构件(11)、第二从手构件(12)和第三从手构13件采用线驱动方式，由四根绳索(3)连接在相应构件的末端，通过改变绳索(3)的长度来驱动第一从手构件(11)、第二从手构件(12)和第三从手构件(13)的自由度变化；

所述机器人主手(2)包括三个结构相同且长度可变的直杆(5)依次相连，每个直杆(5)内均安装有姿态传感器。

2.根据权利要求1所述的面向外科手术的主从式柔性连续体机器人，其特征在于：所述第一从手构件(11)包括顶端单元节(111)和四个第一中间单元节(112)，顶端单元节(111)和四个第一中间单元节(112)通过弹簧(4)建立配合安装；

第二从手构件(12)包括五个第二中间单元节(113)，五个第二中间单元节(113)依次通过弹簧(4)建立配合；

第三从手构件(13)包括四个第三中间单元节(114)和底端单元节(115)，底端单元节(115)和四个第三中间单元节(114)通过弹簧(4)建立配合安装。

3.根据权利要求2所述的面向外科手术的主从式柔性连续体机器人，其特征在于：所述顶端单元节(111)和底端单元节(115)的结构相同，均包括圆盘(1111)和设在圆盘(1111)其中一端面中心位置的锥体安装部(1112)；

所述第一中间单元节(112)、第二中间单元节(113)和第三中间单元节(114)的结构相同，均包括第一圆盘(1121)和设在第一圆盘(1121)两侧端面中心位置的第一锥体安装部(1122)。

4.根据权利要求1所述的面向外科手术的主从式柔性连续体机器人，其特征在于：所述直杆(5)包括外壳体(51)和伸缩杆(52)，外壳体(51)的一端具有敞口56，外壳体(51)的另一端具有套接部(53)，外壳体(51)的侧壁上加工有长孔(54)，伸缩杆(52)为圆形杆，伸缩杆(52)的外侧壁上具有凸起部(55)，伸缩杆(52)安装在外壳体(51)内，伸缩杆(52)上的凸起部(55)与外壳体(51)上的长孔(54)配合安装。

5.面向外科手术的主从式柔性连续体机器人控制方法，其特征在于，包括以下步骤：

步骤一、三个机器人主手的每个末端姿态传感器分别传出相对于世界坐标系的X-Y-Z固定角，以表示每个主手构件末端姿态的坐标系，设姿态传感器角度分别为ɑ₁、β₁、γ₁和ɑ₂、β₂、γ₂以及ɑ₃、β₃、γ₃，设每个主手构件末端坐标系相对于自身底端坐标系的X-Y-Z固定角的3*3旋转矩阵为R_m，而末端坐标系相对于固定世界坐标系的3*3旋转矩阵为R，由偏转角和弯曲角表达的3*3旋转矩阵为R_n；

步骤二、根据旋转矩阵利用每组欧拉角求得代表偏转和弯曲的偏转角和弯曲角θ′₁、

和θ′₂、

以及θ′₃、

使末端坐标系相对于底端坐标系的X-Y-Z固定角的3*3旋转矩阵R₁(R_m1)与偏转角弯曲角的3*3旋转矩阵R_n1相等，求得主手构件末端的弯曲角和偏转角θ′₁、

式中，c表示Cos余弦函数，s表示Sin正弦函数，ɑ₁₁、ɑ₁₂、ɑ₁₃、ɑ₂₁、ɑ₂₂、ɑ₂₃、ɑ₃₁、ɑ₃₂、ɑ₃₃分别对应旋转矩阵R_n1中各元素；

由于姿态传感器的参数是相对于固定世界坐标系得到的，而第二个构件末端坐标系是相对于自身底端坐标系而言的，所以需要坐标变换来求得R_m2；

R_m2R₁＝R₂

解得R_m2后，设

则同理第一个构件，与偏转角和弯曲角表达的3*3旋转矩阵R_n2相等，求得弯曲角θ′₂和偏转角

同理可求得θ′₃和

由于主手构件相当于从手对应构件所呈圆弧的弦，因此从手构件与主手构件的偏转角相等，即

而弯曲角为为主手构件弯曲角的二倍，即θ_i＝2θ′_i；

步骤三、设每个从手构件圆盘的厚度为h，直径为d，竖直状态下的原始长度为l，而每个构件有n个圆盘构成，根据已经求出的各个构件的偏转角和弯曲角，求得每个构件所需的驱动绳长的变化量，设dij为第i个构件(i＝1、2、3)的第j根驱动绳(j＝1、2、3、4)的所需变化量，则：

再将下部绳长变化对上部绳长变化的影响耦合起来，获得12根绳长各自的实际变化量：

Images