一种微创外科手术机器人的建模方法与流程

本发明涉及机器人建模技术领域，特别涉及一种微创外科手术机器人的建模方法。

背景技术：

微创手术(minimallyinvasivesurgery)是指医生通过进入人体内的腹腔镜、胸腔镜等内窥镜观察体内病灶器官，并操作通过微小创口进入体内的特殊手术器械，在人体腔内进行手术的一种手术方式。

与传统的开放手术相比，微创手术具有体表创伤小、术中出血量少、疼痛感轻、手术时间短、术后恢复快、感染风险低的优势，因而在外科手术领域得到了广泛的认可与应用。然而，传统内窥镜微创外科手术也在临床应用中暴露出一些问题，例如：医生长时间以相同姿势站立手术很容易产生疲劳；情绪或生理活动等不稳定因素导致的手部抖动会影响手术精度；器械在操作过程中手眼协调性差容易引起手术误操作等。

随着现代科学技术的不断进步与发展，人们尝试将传统医疗设备(器械)与机械电子技术、计算机控制技术，计算机图形技术等现代前沿技术相结合，开创新型的医疗设备(器械)；特别是机器人技术的日臻完善及在各个领域的广泛应用，医疗机器人已在传统医疗领域逐渐渗透，正是在这样的技术背景下微创外科手术机器人应运而生。微创手术机器人的出现弥补了传统微创手术的缺陷，主要表现在：运动分辨率高手术中可对病灶区域精准定位操作、良好的稳定性消除了医生手抖动防止出现误动作、手眼协调的一致性使手术安全性上大为提高，消除了医生长时间站立的疲劳感，提高微创手术的手术效果，拓展了医生的手术能力。

技术实现要素：

本发明的目的是提供一种微创外科手术机器人的建模方法，该建模方法通过牛顿迭代逼近近似解的方法，获得满足系统精度要求的关节控制量，计算速度同时能满足系统实时性要求。在求解从手逆运动学的过程中引入酉矩阵，将计算引入到复数域，避免了计算过程中出现的奇异值点。

为实现上述目的，本发明提供一种微创外科手术机器人的建模方法，包括：

建立主手末端相对于其基坐标系的齐次变换矩阵；

通过主手基坐标系与从手基座标的映射关系，得到从手末端相对于其基坐标系的齐次变换矩阵；

根据齐次变换矩阵建立关于从手各关节角度的等式方程，求解方程过程中，通过酉矩阵带入复数域计算；

将多个所述待求关节角度的等式方程变换得到关于具有一个所述单一变量的非线性方程；

对所述非线性方程进行牛顿迭代，计算得到一个所述单一变量；

根据一个所述单一变量得到全部变量。

相对于上述背景技术，本发明提供的微创外科手术机器人的建模方法，可以利用现有技术中的姿态矩阵建立方法建立主手末端相对于其基坐标系的齐次变换矩阵；然后通过主手基坐标系与从手基座标的映射关系，得到从手末端相对于其基坐标系的齐次变换矩阵，根据齐次变换矩阵建立关于从手各关节角度的等式方程以及引入的酉矩阵得到多个待求矩阵，多个待求矩阵的个数与从手关节的数量相同；接着将多个待求矩阵变换得到关于具有一个单一变量的非线性方程；对非线性方程进行牛顿迭代，计算得到一个单一变量，最后通过计算得到的该单一变量得出全部变量，从而完成求解过程。采用如此的建模方法，通过牛顿迭代逼近近似解的方法，获得满足系统精度要求的关节控制量，计算速度同时能满足系统实时性要求。在求解从手逆运动学的过程中引入酉矩阵，将计算引入到复数域，避免了计算过程中出现的奇异值点。

优选地，所述通过主手基坐标系与从手基座标的映射关系，得到从手末端相对于其基坐标系的齐次变换矩阵的步骤具体为：：

将从手结构等效为具有六个自由度的平行四边形结构；

根据所述平行四边形结构依次建立旋转矩阵，其中：

式中：

θ1′＝q1,θ2′＝q2,θ3′＝q4,θ4′＝q5,θ5′＝q6；

{q}x为绕x轴转q度角；{q}y为绕y轴转q度角；{q}z为绕z轴转q度角；{l}y为沿着y轴移动距离l；{l}x为沿着x轴移动距离l；{l}z为沿着z轴移动距离l；

将全部所述旋转矩阵相乘，得到所述从手末端姿态矩阵tend；

式中：

优选地，所述根据齐次变换矩阵建立关于从手各关节角度的等式方程，求解方程过程中，通过酉矩阵带入复数域计算的步骤具体为：将变换得到

令：

u′＝{q3}^y-1{q2}z^-1{c1}^-1{q1}z^-1{-(π-α)}x^-1{tend}

建立一个酉矩阵p：

u'v'两个等式分别左乘p^-1和右乘p后两个矩阵还是相等；

u＝{p^-1{q3}^y-1p}{p^-1{q2}z^-1p}{p^-1{c1}^-1p}{p^-1{q1}z^-1p}{p^-1{-(π-α)}x^-1p}{p^-1{tend}p}

令：

b1＝{p^-1{c1}^-1p}；

b2＝{p^-1{-(π-α)}x^-1p}；

b3＝{p^-1{tend}p}；

b6＝{p^-1{c2}p}；

其中：

x3＝q3

其中b1b2b4b5b6为常数矩阵或是有关机器人结构的参数矩阵，在求解的时为已知。

优选地，所述将多个所述待求关节角度的等式方程变换得到关于具有一个所述单一变量的非线性方程的步骤具体为：

令

通过上述等式，观察各相等元素值获得等式方程，通过等式化简并令其中的u24＝v24可得到关于x1的一个非线性方程

f(x1)＝u24-v24＝0。

优选地，所述对所述非线性方程进行牛顿迭代，计算得到一个所述单一变量的步骤具体为：

根据所述从手末端姿态矩阵tend、x1在上一个位置的第一个关节的角度x1_s、位姿矩阵的误差ε以及预设值x0通过牛顿迭代方法计算所述f(x1)＝u24-v24＝0得到数值解x1；

判断所述数值解x1是否收敛；若收敛，则进行所述根据一个所述单一变量得到全部变量的步骤；若不收敛，则将所述预设值x0加1后得到更新后的预设值x0并重复所述通过牛顿迭代方法计算所述f(x1)＝u24-v24＝0得到数值解x1的步骤。

优选地，所述预设值x0＝x1_s-操作误差度数。

优选地，所述根据一个所述单一变量得到全部变量的步骤具体包括：

根据单一变量x1与其他单一变量x2、x3、……、x6之间的数值关系依次计算x2、x3、……、x6：且每当计算一个单一变量之后判断其是否为在可行域内，若是，则继续计算其他单一变量，若否，则将所述预设值x0加1后得到更新后的预设值x0并重复所述通过牛顿迭代方法计算所述f(x1)＝u24-v24＝0得到数值解x1的步骤。

优选地，所述根据一个所述单一变量得到全部变量的步骤之后还包括：

判断根据全部所述变量得到的正解位姿矩阵tend-s与所述从手末端姿态矩阵tend之差是否满足所述误差ε，若是，则保存所述正解位姿矩阵。

优选地，所述建立从手末端相对于主手末端的从手末端姿态矩阵的步骤之前还包括：建立主手末端在基坐标系下的位姿矩阵。

附图说明

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

图1为本发明实施例所提供的微创外科手术机器人的建模方法的主手构型示意图；

图2为本发明实施例所提供的微创外科手术机器人的建模方法的从手构型的平行四边形机构简图；

图3为图2的平行四边形机构的等效图；

图4为图2的平行四边形机构的坐标模型；

图5为本发明实施例所提供的微创外科手术机器人的建模方法的牛顿迭代求解从手的逆运动学流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为了使本技术领域的技术人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。

请参考图1至图5，图1为本发明实施例所提供的微创外科手术机器人的建模方法的主手构型示意图；图2为本发明实施例所提供的微创外科手术机器人的建模方法的从手构型的平行四边形机构简图；图3为图2的平行四边形机构的等效图；图4为图2的平行四边形机构的坐标模型；图5为本发明实施例所提供的微创外科手术机器人的建模方法的牛顿迭代求解从手的逆运动学流程图。

众所周知，机器人运动学中有以下两类基本问题，第一是机器人运动方程的表示问题，即正向运动学：对一给定的机器人，已知连杆几何参数和关节变量，欲求机器人末端执行器相对于参考坐标系的位置和姿态。第二个问题是机器人运动方程的求解问题，即逆向运动学：已知机器人连杆的几何参数，给定机器人末端执行器相对于参考坐标系的期望位置和姿态，求机器人能够达到预期位姿的关节变量。

主从控制机器人的工作是由控制器指挥的，对应驱动末端位姿运动的各关节参数是需要实时计算的。当机器人执行工作任务时，其控制器根据主操作手的轨迹指令运用正向运动学算法规划好位姿序列数据，实时运用逆向运动学算法计算出从手关节参数序列，并依此驱动机器人从手各关节运动角度，使末端按照预定的位姿序列运动。

目前针对主从控制机器人的运动学计算，大致可分为三种计算思路，第一是控制位置和控制姿态的关节解耦，分别独立计算其运动学问题；第二是建立7自由度的机构构型参数模型，依据传统的运动学计算方法求取正向运动学和逆向运动学结果；第三是利用雅克比矩阵求解运动学，采用瞬时速度替代微小时间量内的位移变化；本专利拟写的用于微创外科手术机器人的运动学模型基于第二种计算思路。

微创外科手术机器人是主从控制的方式，医生操作主手进行运动，从手机械臂跟随主手的运动来满足医生的操作需求和手术习惯。针对上述控制方式，手术机器人系统的运动学计算包括两部分，一部分是主手的运动学计算，另一部分是从手运动学计算；

本申请可以采用d-h法建立主手参数模型，主手的构型如附图1所示，为一个7r的机构，其中θ1、θ2、θ3三个大关节我们称为位置关节，θ4、θ5、θ6、θ7四个小关节我们称为姿态关节，姿态关节带有一个冗余自由度，主手由这七个关节自由度决定末端执行器在笛卡尔空间的位置和姿态，由附图1中可看出，主手末端的四个姿态关节，其关节轴线交于一点，符合位置和姿态解耦的条件，另外在主手末端还有一个夹持工具，其夹持角可由传感器实时测量。根据上述主手构型，建立其d-h参数表进行运动学分析，d-h参数包括每个关节的连杆长度ai，连杆偏距di，连杆转角αi以及关节角θi，根据建立的已知各关节的参数，依据空间变换的原理得到相邻两个关节之间齐次变化矩阵，如下述公式。

依据上式建立各个相邻关节轴之间的变换，之后依据下述公式可求得末端执行器坐标系相对于基坐标系的齐次变化矩阵从齐次变化矩阵即可得到主手在某关节角度下末端执行器相对于基坐标系的位置和姿态。

当然，主手在某关节角度下末端执行器相对于基坐标系的位置和姿态还可以采用现有技术中的其他方法建立，本文不再赘述。

说明书附图2与附图3为从手主动臂的结构简图，构件6、7、8构成了一个平行四边形机构，从手主动臂不算末端的手术刀的运动，一共有构件5绕构件4的转动，构件6与构件5之间的摆动，构件9与构件8之间的移动三个运动。

说明书附图4为从手的平行四边形机构的坐标模型；可以看出，构件5与构件4之间的转动的坐标系1的原点我们设在构件5上的远心点¹h1处，固定在构件5上，z1轴沿着ldh，y1轴垂直于ldh，平面z1hy1在平行四面形平面内，x1符合右手定则；(θ1＝0时，初始的时候平面z1h1y1与z'0d1y'0共面)。

0'-1坐标转换，绕x'0轴旋转-(180°-α)，再绕着z1轴转θ'1，沿着z轴和y轴分别移动了-ldhcosα和ldhsinα。

构件8与构件5之间的转动的坐标系2'的原点我们设在构件8上的远心点¹h2处，固定在构件8上，y2轴沿着lhi，z2轴垂直于平面z1h1y1，向外。x2轴符合右手定则(θ2＝0时，初始的时候平面x2h2y2与z1h1y1共面)。

1-2坐标转换，绕x1轴转-(β-90°)，在绕着前一步得到的坐标系的y轴转90°，在绕着z2轴转θ'2，原点不变。(其中β＝β1-β2+β3；β1、β2在机器人出厂的时候就确定了；β3在手术前，确定远心点的时候确定平行四边形的时候确定的。)

构件9与构件8之间的移动的坐标系3的原点我们设在构件9上的手术刀刀具的转动关节点¹i点处，固定在构件9上，y3轴沿着lhi，z3轴垂直于平面x2h2y2，向外。x2轴符合右手定则(d＝0时，初始的时候平面x3i1y3与x2h2y2共面)。

2-3坐标转换，方向不变，原点沿着y3轴移动lih

构件9与构件8之间的转动的坐标系4坐标的原点固定在构件9的i2点，z4轴沿着lhi，y4轴与z3相反。(θ'3＝0时，初始的时候平面x4i2z4与x3i1y3共面)。

3-4坐标转换，绕x3轴转-(90°)，在绕着z4轴转θ'3，原点不变

构件10与构件9之间的转动的坐标系5坐标的原点固定在构件10(手术刀的末端第一个关节)的i3点，y5轴沿着lij，z5轴沿着i3关节的旋转轴。(θ'4＝0时，初始的时候平面x5i3y5与x4i2z4共面)

4-5坐标转换，绕x4轴转90°，在绕着z5轴转θ'4，原点不变

构件11与构件10之间的转动的坐标系6坐标的原点固定在构件11的j点(手术刀的第二个关节)，z6轴沿着j关节的旋转轴，y6由关节j指向手术刀的末端，x6符合右手定则。(θ'5＝0时，初始的时候平面z6jy6与x5i3y5共面)

5-6坐标转换，绕y5轴转90°，在绕着z6轴转θ'5，沿着y5轴移动lij。

式中：

θ1′＝q1,θ2′＝q2,θ3′＝q4,θ4′＝q5,θ5′＝q6

{q}x为绕x轴转q度角；{q}y为绕y轴转q度角；{q}z为绕z轴转q度角；{l}y为沿着y轴移动距离l；{l}x为沿着x轴移动距离l；{l}z为沿着z轴移动距离l；

综合上述分析得到手术器械末端相对于从手主动臂基坐标系的齐次变换矩阵为：

下文将介绍用于微创外科手术机器人的从手逆向运动学计算方法

由前面的推导得到末端的位置矩阵为

式中：

将式tend式中前面三个变量的矩阵左乘到右边来转变成；

令：

u′＝{q3}^y-1{q2}z^-1{c1}^-1{q1}z^-1{-(π-α)}x^-1{tend}

建立一个酉矩阵p

u'v'两个等式分别左乘p^-1和右乘p后两个矩阵还是相等。

u＝{p^-1{q3}^y-1p}{p^-1{q2}z^-1p}{p^-1{c1}^-1p}{p^-1{q1}z^-1p}{p^-1{-(π-α)}x^-1p}{p^-1{tend}p}

令：

b1＝{p^-1{c1}^-1p}；

b2＝{p^-1{-(π-α)}x^-1p}；

b3＝{p^-1{tend}p}；

b6＝{p^-1{c2}p}；

其中：

x3＝q3

其中b1b2b4b5b6为常数矩阵或是有关机器人结构的参数矩阵，在求解的时候是已知的，故，可作为常数矩阵来对待。

其中b3是末端位置的参数矩阵，在求解的时候也是是已知的，故，可作为常数矩阵来对待。

令

通过上述等式，观察各相等元素值获得等式方程，通过等式化简并令其中的u24＝v24可得到关于x1的一个非线性方程

f(x1)＝u24-v24＝0

通过牛顿迭代我们可以求出f(x1)的解，由于迭代得到的解与给定的初始值有很大的关系，在求解的时候将x1，在-180°～180°之间间隔一度就作为初始值进去迭代得到方程的数值解具体求解的流程图如说明书附图5所示。

其中，牛顿迭代计算的过程可以参考现有技术，本文仅仅给出一种具体实施方式：

步骤s101中，确定从手末端姿态矩阵tend、x1在上一个位置的第一个关节的角度x1_s以及位姿矩阵的误差ε；

步骤s102中，将预设值x0设置为x1_s-操作误差度数。其中，操作误差度数与操作医生的操作习惯有关，可以取值为15°；

步骤s103中，对f(x1)＝u24-v24＝0求解，在求解之前，可以将牛顿迭代的精度取值为e^-15；也即步骤s1030；

步骤s104中，求出x1，然后进行步骤s105，判断其是否收敛；若不收敛，则执行步骤s110，将预设值x0加1后得到更新后的预设值x0并重复步骤s103；也即执行步骤s110后重复步骤s103；若收敛，则执行步骤s120，计算x2；

通过上述公式推导，x1、x2、x3、……、x6之间具备确定的数值对应关系，也即求出x1之后，其余变量也可计算出来；

当求出x2之后，判断其是否在可行域内，也即步骤s120与步骤s121，若x2不在可行域内，则执行步骤s110；若x2在可行域内，则执行步骤s122，计算得到x3，判断x3是否为正整数；然后依次执行步骤s124至步骤s129；

当x1、x2、x3、……、x6全部求出后，得到求解出来的正解的位姿矩阵tend-s；也即步骤s130，而后判断正解位姿矩阵tend-s与所述从手末端姿态矩阵tend之差是否满足所述误差ε，如步骤s131；

若满足，则执行保存操作，如步骤s150；若不满足，则将求解出来的正解的位姿矩阵tend-s设定为从手末端姿态矩阵tend；而后执行步骤s160，判断x0是否小于x1_s+操作误差度数。若大于或等于，则执行步骤s110，重新设定x0，并重复步骤s103；若小于，则执行步骤s161，判断求解出来的正解的位姿矩阵tend-s是否为空矩阵，若是，则重新设置从手末端姿态矩阵tend、x1在上一个位置的第一个关节的角度x1_s以及位姿矩阵的误差ε，并重复步骤s102；若不是求解出来的正解的位姿矩阵tend-s不为空矩阵，则判断其维度是否为1，也即步骤s180，若为1，则该正解的位姿矩阵tend-s为所求解，也即步骤s200；若正解的位姿矩阵tend-s的维度不为1，则执行步骤s190，取各个关节运动的最短距离作为所求解。如此设置，运用牛顿迭代方法以及后续检验过程求得全部变量，也即从手每一关节的运动量均可求得；当然，在计算过程中相关参数还可以设置为其他值，且迭代过程与检验过程还可以采用现有技术中的其他步骤进行，本文步骤赘述。

本发明提供的建模方法，通过牛顿迭代逼近近似解的方法，获得满足系统精度要求的关节控制量，计算速度同时能满足系统实时性要求。在求解从手逆运动学的过程中引入酉矩阵，将计算引入到复数域，避免了计算过程中出现的奇异值点。

以上对本发明所提供的微创外科手术机器人的建模方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。