CN107688293B - 一种微创外科手术机器人的建模方法 - Google Patents

一种微创外科手术机器人的建模方法 Download PDF

Info

Publication number
CN107688293B
CN107688293B CN201710632571.0A CN201710632571A CN107688293B CN 107688293 B CN107688293 B CN 107688293B CN 201710632571 A CN201710632571 A CN 201710632571A CN 107688293 B CN107688293 B CN 107688293B
Authority
CN
China
Prior art keywords
matrix
hand
equation
coordinate system
single variable
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710632571.0A
Other languages
English (en)
Other versions
CN107688293A (zh
Inventor
王了
柏龙
张志良
肖乔木
杨建兴
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
CHONGQING JINSHAN MEDICAL ROBOT Co.,Ltd.
Original Assignee
Chongqing Jinshan Medical Robot Co ltd
Chongqing Jinshan Medical Appliance Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chongqing Jinshan Medical Robot Co ltd, Chongqing Jinshan Medical Appliance Co Ltd filed Critical Chongqing Jinshan Medical Robot Co ltd
Priority to CN201710632571.0A priority Critical patent/CN107688293B/zh
Publication of CN107688293A publication Critical patent/CN107688293A/zh
Application granted granted Critical
Publication of CN107688293B publication Critical patent/CN107688293B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Health & Medical Sciences (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Medicines Containing Antibodies Or Antigens For Use As Internal Diagnostic Agents (AREA)
  • Magnetic Resonance Imaging Apparatus (AREA)

Abstract

本发明公开了一种微创外科手术机器人的建模方法，包括：建立主手末端相对于其基坐标系的齐次变换矩阵，通过主手基坐标系与从手基座标的映射关系，得到从手末端相对于其基坐标系的齐次变换矩阵，根据齐次变换矩阵建立关于从手各关节角度的等式方程，求解方程过程中，通过酉矩阵带入复数域计算，将多个待求关节角度的等式方程变换得到关于具有一个单一变量的非线性方程；对非线性方程进行牛顿迭代，计算得到一个单一变量；根据一个单一变量得到全部变量。上述建模方法，通过牛顿迭代逼近近似解的方法，获得满足***精度要求的关节控制量，计算速度同时能满足***实时性要求。

Description

一种微创外科手术机器人的建模方法
技术领域
本发明涉及机器人建模技术领域，特别涉及一种微创外科手术机器人的建模方法。
背景技术
微创手术（Minimally Invasive Surgery）是指医生通过进入人体内的腹腔镜、胸腔镜等内窥镜观察体内病灶器官，并操作通过微小创口进入体内的特殊手术器械，在人体腔内进行手术的一种手术方式。
与传统的开放手术相比，微创手术具有体表创伤小、术中出血量少、疼痛感轻、手术时间短、术后恢复快、感染风险低的优势，因而在外科手术领域得到了广泛的认可与应用。然而，传统内窥镜微创外科手术也在临床应用中暴露出一些问题，例如：医生长时间以相同姿势站立手术很容易产生疲劳； 情绪或生理活动等不稳定因素导致的手部抖动会影响手术精度； 器械在操作过程中手眼协调性差容易引起手术误操作等。
随着现代科学技术的不断进步与发展，人们尝试将传统医疗设备（器械）与机械电子技术、计算机控制技术，计算机图形技术等现代前沿技术相结合，开创新型的医疗设备（器械）；特别是机器人技术的日臻完善及在各个领域的广泛应用，医疗机器人已在传统医疗领域逐渐渗透，正是在这样的技术背景下微创外科手术机器人应运而生。微创手术机器人的出现弥补了传统微创手术的缺陷，主要表现在：运动分辨率高手术中可对病灶区域精准定位操作、良好的稳定性消除了医生手抖动防止出现误动作、手眼协调的一致性使手术安全性上大为提高，消除了医生长时间站立的疲劳感，提高微创手术的手术效果，拓展了医生的手术能力。
发明内容
本发明的目的是提供一种微创外科手术机器人的建模方法，该建模方法通过牛顿迭代逼近近似解的方法，获得满足***精度要求的关节控制量，计算速度同时能满足***实时性要求。在求解从手逆运动学的过程中引入酉矩阵，将计算引入到复数域，避免了计算过程中出现的奇异值点。
为实现上述目的，本发明提供一种微创外科手术机器人的建模方法，包括：
建立主手末端相对于主手基坐标系的齐次变换矩阵；
通过所述主手基坐标系与从手基座标的映射关系，得到从手末端相对于从手基坐标系的齐次变换矩阵；
根据齐次变换矩阵建立关于从手各关节角度的等式方程，求解方程过程中，通过酉矩阵带入复数域计算;
将所述等式方程变换得到关于具有一个单一变量的非线性方程；
对所述非线性方程进行牛顿迭代，计算得到一个所述单一变量；
根据一个所述单一变量得到全部变量。
相对于上述背景技术，本发明提供的微创外科手术机器人的建模方法，可以利用现有技术中的姿态矩阵建立方法建立主手末端相对于其基坐标系的齐次变换矩阵；然后通过主手基坐标系与从手基座标的映射关系，得到从手末端相对于其基坐标系的齐次变换矩阵，根据齐次变换矩阵建立关于从手各关节角度的等式方程以及引入的酉矩阵得到多个待求矩阵，多个待求矩阵的个数与从手关节的数量相同；接着将多个待求矩阵变换得到关于具有一个单一变量的非线性方程；对非线性方程进行牛顿迭代，计算得到一个单一变量，最后通过计算得到的该单一变量得出全部变量，从而完成求解过程。采用如此的建模方法，通过牛顿迭代逼近近似解的方法，获得满足***精度要求的关节控制量，计算速度同时能满足***实时性要求。在求解从手逆运动学的过程中引入酉矩阵，将计算引入到复数域，避免了计算过程中出现的奇异值点。
优选地，所述通过主手基坐标系与从手基座标的映射关系，得到从手末端相对于从手基坐标系的齐次变换矩阵的步骤具体为：
将从手结构等效为具有平行四边形结构的六自由度模型；
根据所述平行四边形结构依次建立旋转矩阵，其中：
Figure 100002_DEST_PATH_IMAGE001
；
从手基坐标系的原点和所述平行四边形结构中远离所述从手基坐标系的顶点之间形成直角三角形的第一边，所述直角三角形的另外两条边形成直角，参数α为所述从手基坐标系的原点所在的两条边的夹角；
Figure DEST_PATH_IMAGE002
；
所述六自由度模型包括折弯边和所述第一边，所述第一边和所述折弯边具有夹角，参数β为180°-所述夹角；
Figure 100002_DEST_PATH_IMAGE003
；
Figure DEST_PATH_IMAGE004
；
Figure 100002_DEST_PATH_IMAGE005
；
Figure DEST_PATH_IMAGE006
；
式中：
Figure 100002_DEST_PATH_IMAGE007
；
{q}x为绕x轴转q度角；{q}y为绕y轴转q度角；{q}z为绕z轴转q度角；{l}y为沿着y轴移动距离l；{l}x为沿着x轴移动距离l；{l}z为沿着z轴移动距离l；
将全部所述旋转矩阵相乘，得到所述从手末端姿态矩阵T end ；
Figure DEST_PATH_IMAGE008
式中：
Figure 100002_DEST_PATH_IMAGE009
。
优选地，所述根据齐次变换矩阵建立关于从手各关节角度的等式方程，求解方程过程中，通过酉矩阵带入复数域计算的步骤具体为：
将
Figure 812247DEST_PATH_IMAGE008
变换得到
Figure DEST_PATH_IMAGE010
令：
Figure 100002_DEST_PATH_IMAGE011
建立一个酉矩阵P：
Figure DEST_PATH_IMAGE012
；
U'V'两个等式分别左乘P -1和右乘P后两个矩阵还是相等；
Figure 100002_DEST_PATH_IMAGE013
令：
Figure DEST_PATH_IMAGE014
;
Figure 100002_DEST_PATH_IMAGE015
;
Figure DEST_PATH_IMAGE016
；
Figure 100002_DEST_PATH_IMAGE017
；
Figure DEST_PATH_IMAGE018
；
Figure 100002_DEST_PATH_IMAGE019
；
Figure DEST_PATH_IMAGE020
；
Figure 100002_DEST_PATH_IMAGE021
；
Figure DEST_PATH_IMAGE022
；
Figure 100002_DEST_PATH_IMAGE023
；
Figure DEST_PATH_IMAGE024
；
Figure 100002_DEST_PATH_IMAGE025
；
其中：
Figure DEST_PATH_IMAGE026
其中
Figure 100002_DEST_PATH_IMAGE027
为常数矩阵或是有关机器人结构的参数矩阵，在求解的时为已知。
优选地，所述将等式方程变换得到关于具有一个单一变量的非线性方程的步骤具体为：
令
Figure DEST_PATH_IMAGE028
；
通过上述等式，观察各相等元素值获得等式方程，通过等式化简并令其中的U 24=V 24可得到关于x 1的一个非线性方程
Figure 100002_DEST_PATH_IMAGE029
。
优选地，所述对所述非线性方程进行牛顿迭代，计算得到一个所述单一变量的步骤具体为：
根据所述从手末端姿态矩阵T end 、x 1在上一个位置的第一个关节的角度x1_s、位姿矩阵的误差ε以及预设值x 0通过牛顿迭代方法计算所述
Figure 831062DEST_PATH_IMAGE029
得到数值解x 1；
判断所述数值解x 1是否收敛；若收敛，则进行所述根据一个所述单一变量得到全部变量的步骤；若不收敛，则将所述预设值x 0加1后得到更新后的预设值x 0并重复所述通过牛顿迭代方法计算所述
Figure 385671DEST_PATH_IMAGE029
得到数值解x 1的步骤。
优选地，所述预设值x 0=x1_s-操作误差度数。
优选地，所述根据一个所述单一变量得到全部变量的步骤具体包括：
根据单一变量x 1与其他单一变量x 2、x 3、……、x 6之间的数值关系依次计算x 2、x 3、……、x 6：且每当计算一个单一变量之后判断其是否为在可行域内，若是，则继续计算其他单一变量，若否，则将所述预设值x 0加1后得到更新后的预设值x 0并重复所述通过牛顿迭代方法计算所述
Figure 386821DEST_PATH_IMAGE029
得到数值解x 1的步骤。
优选地，所述根据一个所述单一变量得到全部变量的步骤之后还包括：
判断根据全部所述变量得到的正解位姿矩阵T end-S 与所述从手末端姿态矩阵T end 之差是否满足所述误差ε，若是，则保存所述正解位姿矩阵。
优选地，所述建立从手末端相对于主手末端的从手末端姿态矩阵的步骤之前还包括：建立主手末端在基坐标系下的位姿矩阵。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1为本发明实施例所提供的微创外科手术机器人的建模方法的主手构型示意图；
图2为本发明实施例所提供的微创外科手术机器人的建模方法的从手构型的平行四边形机构简图；
图3为图2的平行四边形机构的等效图；
图4为图2的平行四边形机构的坐标模型；
图5为本发明实施例所提供的微创外科手术机器人的建模方法的牛顿迭代求解从手的逆运动学流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了使本技术领域的技术人员更好地理解本发明方案，下面结合附图和具体实施方式对本发明作进一步的详细说明。
请参考图1至图5，图1为本发明实施例所提供的微创外科手术机器人的建模方法的主手构型示意图；图2为本发明实施例所提供的微创外科手术机器人的建模方法的从手构型的平行四边形机构简图；图3为图2的平行四边形机构的等效图；图4为图2的平行四边形机构的坐标模型；图5为本发明实施例所提供的微创外科手术机器人的建模方法的牛顿迭代求解从手的逆运动学流程图。
众所周知，机器人运动学中有以下两类基本问题，第一是机器人运动方程的表示问题，即正向运动学：对一给定的机器人，已知连杆几何参数和关节变量，欲求机器人末端执行器相对于参考坐标系的位置和姿态。第二个问题是机器人运动方程的求解问题，即逆向运动学：已知机器人连杆的几何参数，给定机器人末端执行器相对于参考坐标系的期望位置和姿态，求机器人能够达到预期位姿的关节变量。
主从控制机器人的工作是由控制器指挥的，对应驱动末端位姿运动的各关节参数是需要实时计算的。当机器人执行工作任务时，其控制器根据主操作手的轨迹指令运用正向运动学算法规划好位姿序列数据，实时运用逆向运动学算法计算出从手关节参数序列，并依此驱动机器人从手各关节运动角度，使末端按照预定的位姿序列运动。
目前针对主从控制机器人的运动学计算，大致可分为三种计算思路，第一是控制位置和控制姿态的关节解耦，分别独立计算其运动学问题；第二是建立7自由度的机构构型参数模型，依据传统的运动学计算方法求取正向运动学和逆向运动学结果；第三是利用雅克比矩阵求解运动学，采用瞬时速度替代微小时间量内的位移变化；本专利拟写的用于微创外科手术机器人的运动学模型基于第二种计算思路。
微创外科手术机器人是主从控制的方式，医生操作主手进行运动，从手机械臂跟随主手的运动来满足医生的操作需求和手术习惯。针对上述控制方式，手术机器人***的运动学计算包括两部分，一部分是主手的运动学计算，另一部分是从手运动学计算；
本申请可以采用D-H法建立主手参数模型，主手的构型如附图1所示，为一个7R的机构，其中
Figure DEST_PATH_IMAGE030
三个大关节我们称为位置关节，
Figure DEST_PATH_IMAGE031
四个小关节我们称为姿态关节，姿态关节带有一个冗余自由度，主手由这七个关节自由度决定末端执行器在笛卡尔空间的位置和姿态，由附图1中可看出，主手末端的四个姿态关节，其关节轴线交于一点，符合位置和姿态解耦的条件，另外在主手末端还有一个夹持工具，其夹持角
Figure DEST_PATH_IMAGE032
可由传感器实时测量。根据上述主手构型，建立其D-H参数表进行运动学分析，D-H参数包括每个关节的连杆长度
Figure DEST_PATH_IMAGE033
，连杆偏距
Figure DEST_PATH_IMAGE034
，连杆转角
Figure DEST_PATH_IMAGE035
以及关节角
Figure DEST_PATH_IMAGE036
，根据建立的已知各关节的参数，依据空间变换的原理得到相邻两个关节之间齐次变化矩阵，如下述公式。
Figure DEST_PATH_IMAGE037
依据上式建立各个相邻关节轴之间的变换，之后依据下述公式可求得末端执行器坐标系相对于基坐标系的齐次变化矩阵
Figure DEST_PATH_IMAGE038
，从齐次变化矩阵即可得到主手在某关节角度下末端执行器相对于基坐标系的位置和姿态。
Figure DEST_PATH_IMAGE039
当然，主手在某关节角度下末端执行器相对于基坐标系的位置和姿态还可以采用现有技术中的其他方法建立，本文不再赘述。
说明书附图2与附图3为从手主动臂的结构简图，构件6、7、8构成了一个平行四边形机构，从手主动臂不算末端的手术刀的运动，一共有构件5绕构件4的转动，构件6与构件5之间的摆动，构件9与构件8之间的移动三个运动。
说明书附图4为从手的平行四边形机构的坐标模型；可以看出，构件5与构件4之间的转动的坐标系1的原点我们设在构件5上的远心点1H1处，固定在构件5上，z1轴沿着l DH，y1轴垂直于l DH，平面z1Hy1在平行四面形平面内，x1符合右手定则；（θ 1=0时，初始的时候平面z1H1y1与z'0D1y'0共面）。
0'-1坐标转换，绕x'0轴旋转-（180°-α），再绕着z1轴转θ'1，沿着z轴和y轴分别移动了-l DHcosα和l DHsinα。
Figure DEST_PATH_IMAGE040
构件8与构件5之间的转动的坐标系2'的原点我们设在构件8上的远心点1H2处，固定在构件8上，y2轴沿着l HI，z2轴垂直于平面z1H1y1，向外。x2轴符合右手定则（θ 2=0时，初始的时候平面x2H2y2与z1H1y1共面）。
1-2坐标转换，绕x1轴转-（β-90°），在绕着前一步得到的坐标系的y轴转90°，在绕着z2轴转θ'2，原点不变。（其中β=β 1-β 2+β 3；β 1、β 2在机器人出厂的时候就确定了；β 3在手术前，确定远心点的时候确定平行四边形的时候确定的。）
Figure DEST_PATH_IMAGE041
构件9与构件8之间的移动的坐标系3的原点我们设在构件9上的手术刀刀具的转动关节点1I点处，固定在构件9上，y3轴沿着l HI，z3轴垂直于平面x2H2y2，向外。x2轴符合右手定则（d=0时，初始的时候平面x3I1y3与x2H2y2共面）。
2-3坐标转换，方向不变，原点沿着y3轴移动l IH
Figure DEST_PATH_IMAGE042
构件9与构件8之间的转动的坐标系4坐标的原点固定在构件9的I2点，z4轴沿着l HI，y4轴与z3相反。（θ'3=0时，初始的时候平面x4I2z4与x3I1y3共面）。
3-4坐标转换，绕x3轴转-（90°），在绕着z4轴转θ'3，原点不变
Figure DEST_PATH_IMAGE043
构件10与构件9之间的转动的坐标系5坐标的原点固定在构件10（手术刀的末端第一个关节）的I3点，y5轴沿着l IJ，z5轴沿着I3关节的旋转轴。（θ'4=0时，初始的时候平面x5I3y5与x4I2z4共面）
4-5坐标转换，绕x4轴转90°，在绕着z5轴转θ'4，原点不变
Figure DEST_PATH_IMAGE044
构件11与构件10之间的转动的坐标系6坐标的原点固定在构件11的J点（手术刀的第二个关节），z6轴沿着J关节的旋转轴，y6由关节J指向手术刀的末端，x6符合右手定则。（θ'5=0时，初始的时候平面z6Jy6与x5I3y5共面）
5-6坐标转换，绕y5轴转90°，在绕着z6轴转θ'5，沿着y5轴移动l IJ。
Figure DEST_PATH_IMAGE045
式中：
Figure DEST_PATH_IMAGE046
{q}x为绕x轴转q度角；{q}y为绕y轴转q度角；{q}z为绕z轴转q度角；{l}y为沿着y轴移动距离l；{l}x为沿着x轴移动距离l；{l}z为沿着z轴移动距离l；
综合上述分析得到手术器械末端相对于从手主动臂基坐标系的齐次变换矩阵为：
Figure DEST_PATH_IMAGE047
下文将介绍用于微创外科手术机器人的从手逆向运动学计算方法
由前面的推导得到末端的位置矩阵为
Figure DEST_PATH_IMAGE048
式中：
Figure DEST_PATH_IMAGE049
将式T end 式中前面三个变量的矩阵左乘到右边来转变成；
Figure DEST_PATH_IMAGE050
令：
Figure DEST_PATH_IMAGE051
建立一个酉矩阵P
Figure DEST_PATH_IMAGE052
U'V'两个等式分别左乘P -1和右乘P后两个矩阵还是相等。
Figure DEST_PATH_IMAGE053
令：
Figure DEST_PATH_IMAGE054
；
Figure DEST_PATH_IMAGE055
；
Figure DEST_PATH_IMAGE056
；
Figure DEST_PATH_IMAGE057
；
Figure DEST_PATH_IMAGE058
；
Figure DEST_PATH_IMAGE059
；
Figure DEST_PATH_IMAGE060
；
Figure DEST_PATH_IMAGE061
；
Figure DEST_PATH_IMAGE062
；
Figure DEST_PATH_IMAGE063
；
Figure DEST_PATH_IMAGE064
；
Figure DEST_PATH_IMAGE065
；
其中：
Figure DEST_PATH_IMAGE066
其中
Figure DEST_PATH_IMAGE067
为常数矩阵或是有关机器人结构的参数矩阵，在求解的时候是已知的，故，可作为常数矩阵来对待。
其中是末端位置的参数矩阵，在求解的时候也是是已知的，故，可作为常数矩阵来对待。
令
Figure DEST_PATH_IMAGE068
；
通过上述等式，观察各相等元素值获得等式方程，通过等式化简并令其中的U 24=V 24可得到关于x 1的一个非线性方程
Figure DEST_PATH_IMAGE069
通过牛顿迭代我们可以求出F（x1）的解，由于迭代得到的解与给定的初始值有很大的关系，在求解的时候将x1，在-180°~180°之间间隔一度就作为初始值进去迭代得到方程的数值解具体求解的流程图如说明书附图5所示。
其中，牛顿迭代计算的过程可以参考现有技术，本文仅仅给出一种具体实施方式：
步骤S101中，确定从手末端姿态矩阵T end 、x 1在上一个位置的第一个关节的角度x1_s以及位姿矩阵的误差ε；
步骤S102中，将预设值x 0设置为x1_s-操作误差度数。其中，操作误差度数与操作医生的操作习惯有关，可以取值为15°；
步骤S103中，对
Figure 380838DEST_PATH_IMAGE069
求解，在求解之前，可以将牛顿迭代的精度取值为e-15；也即步骤S1030；
步骤S104中，求出x1，然后进行步骤S105，判断其是否收敛；若不收敛，则执行步骤S110，将预设值x 0加1后得到更新后的预设值x 0并重复步骤S103；也即执行步骤S110后重复步骤S103；若收敛，则执行步骤S120，计算x2；
通过上述公式推导，x1、x 2、x 3、……、x 6之间具备确定的数值对应关系，也即求出x1之后，其余变量也可计算出来；
当求出x 2之后，判断其是否在可行域内，也即步骤S120与步骤S121，若x 2不在可行域内，则执行步骤S110；若x 2在可行域内，则执行步骤S122，计算得到x 3，判断x 3是否为正整数；然后依次执行步骤S124至步骤S129；
当x 1、x 2、x 3、……、x 6全部求出后，得到求解出来的正解的位姿矩阵T end-S ；也即步骤S130，而后判断正解位姿矩阵T end-S 与所述从手末端姿态矩阵T end 之差是否满足所述误差ε，如步骤S131；
若满足，则执行保存操作，如步骤S150；若不满足，则将求解出来的正解的位姿矩阵T end-S 设定为从手末端姿态矩阵T end ；而后执行步骤S160，判断x 0是否小于x1_s+操作误差度数。若大于或等于，则执行步骤S110，重新设定x 0，并重复步骤S103；若小于，则执行步骤S161，判断求解出来的正解的位姿矩阵T end-S 是否为空矩阵，若是，则重新设置从手末端姿态矩阵T end 、x 1在上一个位置的第一个关节的角度x1_s以及位姿矩阵的误差ε，并重复步骤S102；若不是求解出来的正解的位姿矩阵T end-S 不为空矩阵，则判断其维度是否为1，也即步骤S180，若为1，则该正解的位姿矩阵T end-S 为所求解，也即步骤S200；若正解的位姿矩阵T end-S 的维度不为1，则执行步骤S190，取各个关节运动的最短距离作为所求解。如此设置，运用牛顿迭代方法以及后续检验过程求得全部变量，也即从手每一关节的运动量均可求得；当然，在计算过程中相关参数还可以设置为其他值，且迭代过程与检验过程还可以采用现有技术中的其他步骤进行，本文步骤赘述。
本发明提供的建模方法，通过牛顿迭代逼近近似解的方法，获得满足***精度要求的关节控制量，计算速度同时能满足***实时性要求。在求解从手逆运动学的过程中引入酉矩阵，将计算引入到复数域，避免了计算过程中出现的奇异值点。
以上对本发明所提供的微创外科手术机器人的建模方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (8)

1.一种微创外科手术机器人的建模方法，其特征在于，包括：
建立主手末端相对于主手基坐标系的齐次变换矩阵；
通过所述主手基坐标系与从手基座标的映射关系，得到从手末端相对于从手基坐标系的齐次变换矩阵；
根据所述齐次变换矩阵建立关于从手各关节角度的等式方程，求解方程过程中，通过酉矩阵带入复数域计算；
将所述等式方程变换得到关于具有一个单一变量的非线性方程；
对所述非线性方程进行牛顿迭代，计算得到一个所述单一变量；
根据一个所述单一变量得到全部变量；
其中，所述通过主手基坐标系与从手基座标的映射关系，得到从手末端相对于从手基坐标系的齐次变换矩阵的步骤具体为：
将从手结构等效为具有平行四边形结构的六自由度模型；
根据所述平行四边形结构依次建立旋转矩阵，其中：
Figure DEST_PATH_IMAGE001
；
从手基坐标系的原点和所述平行四边形结构中远离所述从手基坐标系的顶点之间形成直角三角形的第一边，所述直角三角形的另外两条边形成直角，参数α为所述从手基坐标系的原点所在的两条边的夹角；
Figure 568859DEST_PATH_IMAGE002
；
所述六自由度模型包括折弯边和所述第一边，所述第一边和所述折弯边具有夹角，参数β为180°-所述夹角；
Figure DEST_PATH_IMAGE003
；
Figure 399543DEST_PATH_IMAGE004
；
Figure DEST_PATH_IMAGE005
；
Figure 240301DEST_PATH_IMAGE006
；
式中：
Figure DEST_PATH_IMAGE007
；
{q}x为绕x轴转q度角；{q}y为绕y轴转q度角；{q}z为绕z轴转q度角；{l}y为沿着y轴移动距离l；{l}x为沿着x轴移动距离l；{l}z为沿着z轴移动距离l；
将全部所述旋转矩阵相乘，得到所述从手末端姿态矩阵T end ；
Figure 117121DEST_PATH_IMAGE008
式中：
Figure DEST_PATH_IMAGE009
。
2.根据权利要求1所述的建模方法，其特征在于，所述根据齐次变换矩阵建立关于从手各关节角度的等式方程，求解方程过程中，通过酉矩阵带入复数域计算的步骤具体为：
将
Figure 949246DEST_PATH_IMAGE008
变换得到
Figure 555808DEST_PATH_IMAGE010
令：
Figure DEST_PATH_IMAGE011
建立一个酉矩阵P：
Figure 521141DEST_PATH_IMAGE012
；
U'V'两个等式分别左乘P -1和右乘P后两个矩阵还是相等；
Figure DEST_PATH_IMAGE013
令：
Figure 9891DEST_PATH_IMAGE014
；
Figure DEST_PATH_IMAGE015
；
Figure 904160DEST_PATH_IMAGE016
；
Figure DEST_PATH_IMAGE017
；
Figure 834070DEST_PATH_IMAGE018
；
Figure DEST_PATH_IMAGE019
；
Figure 873701DEST_PATH_IMAGE020
；
Figure DEST_PATH_IMAGE021
；
Figure 990693DEST_PATH_IMAGE022
；
Figure DEST_PATH_IMAGE023
；
Figure 268746DEST_PATH_IMAGE024
；
Figure DEST_PATH_IMAGE025
；
其中：
Figure 787584DEST_PATH_IMAGE026
其中
Figure DEST_PATH_IMAGE027
为常数矩阵或是有关机器人结构的参数矩阵，在求解时为已知。
3.根据权利要求2所述的建模方法，其特征在于，所述将等式方程变换得到关于具有一个单一变量的非线性方程的步骤具体为：
令
Figure 391259DEST_PATH_IMAGE028
；
通过上述等式，观察各相等元素值获得等式方程，通过等式化简并令其中的U 24=V 24可得到关于x 1的一个非线性方程
Figure DEST_PATH_IMAGE029
。
4.根据权利要求3所述的建模方法，其特征在于，所述对所述非线性方程进行牛顿迭代，计算得到一个所述单一变量的步骤具体为：
根据所述从手末端姿态矩阵T end 、x 1在上一个位置的第一个关节的角度x1_s、位姿矩阵的误差ε以及预设值x 0通过牛顿迭代方法计算所述
Figure 729967DEST_PATH_IMAGE029
得到数值解x 1；
判断所述数值解x 1是否收敛；若收敛，则进行所述根据一个所述单一变量得到全部变量的步骤；若不收敛，则将所述预设值x 0加1后得到更新后的预设值x 0并重复所述通过牛顿迭代方法计算所述
Figure 477559DEST_PATH_IMAGE030
得到数值解x 1的步骤。
5.根据权利要求4所述的建模方法，其特征在于，所述预设值x 0=x1_s-操作误差度数。
6.根据权利要求4所述的建模方法，其特征在于，所述根据一个所述单一变量得到全部变量的步骤具体包括：
根据单一变量x 1与其他单一变量x 2、x 3、……、x 6之间的数值关系依次计算x 2、x 3、……、x 6：且每当计算一个单一变量之后判断其是否为在可行域内，若是，则继续计算其他单一变量，若否，则将所述预设值x 0加1后得到更新后的预设值x 0并重复所述通过牛顿迭代方法计算所述
Figure 647641DEST_PATH_IMAGE030
得到数值解x 1的步骤。
7.根据权利要求4~6任意一项所述的建模方法，其特征在于，所述根据一个所述单一变量得到全部变量的步骤之后还包括：
判断根据全部所述变量得到的正解位姿矩阵T end-S 与所述从手末端姿态矩阵T end 之差是否满足所述误差ε，若是，则保存所述正解位姿矩阵。
8.根据权利要求1~6任意一项所述的建模方法，其特征在于，所述建立从手末端相对于主手末端的从手末端姿态矩阵的步骤之前还包括：建立主手末端在基坐标系下的位姿矩阵。
CN201710632571.0A 2017-07-28 2017-07-28 一种微创外科手术机器人的建模方法 Active CN107688293B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710632571.0A CN107688293B (zh) 2017-07-28 2017-07-28 一种微创外科手术机器人的建模方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710632571.0A CN107688293B (zh) 2017-07-28 2017-07-28 一种微创外科手术机器人的建模方法

Publications (2)

Publication Number Publication Date
CN107688293A CN107688293A (zh) 2018-02-13
CN107688293B true CN107688293B (zh) 2021-01-12

Family

ID=61153154

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710632571.0A Active CN107688293B (zh) 2017-07-28 2017-07-28 一种微创外科手术机器人的建模方法

Country Status (1)

Country Link
CN (1) CN107688293B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108983705B (zh) * 2018-08-16 2020-01-07 居鹤华 一种基于轴不变量的多轴机器人***正运动学建模与解算方法
CN112828862B (zh) * 2020-12-30 2022-09-16 诺创智能医疗科技(杭州)有限公司 并联平台的主从映射方法、机械臂***和存储介质
CN112716608B (zh) * 2021-01-20 2022-06-24 山东威高手术机器人有限公司 用于微创手术机器人的主从跟踪控制方法
CN114002990B (zh) * 2021-12-30 2022-04-08 之江实验室 并联双足机器人关节实时控制方法和装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08155647A (ja) * 1994-11-29 1996-06-18 Komatsu Ltd 溶接ロボットのウィービング制御装置
CN106003034A (zh) * 2016-06-16 2016-10-12 深圳先进技术研究院 一种主从式机器人控制***及控制方法
CN106041912A (zh) * 2016-06-16 2016-10-26 深圳先进技术研究院 主从式蛇形机器人***及其位置控制方法
CN106137400A (zh) * 2016-05-31 2016-11-23 微创（上海）医疗机器人有限公司 用于机械臂的控制***、控制方法及一种手术机器人
CN106980751A (zh) * 2017-02-27 2017-07-25 浙江大学 一种含双c轴的六轴自动化制孔锪窝机床的运动学反解方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08155647A (ja) * 1994-11-29 1996-06-18 Komatsu Ltd 溶接ロボットのウィービング制御装置
CN106137400A (zh) * 2016-05-31 2016-11-23 微创（上海）医疗机器人有限公司 用于机械臂的控制***、控制方法及一种手术机器人
CN106003034A (zh) * 2016-06-16 2016-10-12 深圳先进技术研究院 一种主从式机器人控制***及控制方法
CN106041912A (zh) * 2016-06-16 2016-10-26 深圳先进技术研究院 主从式蛇形机器人***及其位置控制方法
CN106980751A (zh) * 2017-02-27 2017-07-25 浙江大学 一种含双c轴的六轴自动化制孔锪窝机床的运动学反解方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
一般6R机器人的位置反解与运动仿真;于艳秋等;《中国机械工程》;20031231;第14卷(第24期);第2130-2133页 *
基于牛顿迭代法消除机器人奇异点的研究;周克媛;《青岛职业技术学院学报》;20090615;第22卷(第2期);第65-66页 *
基于简化形式的 Jacobian 矩阵的牛顿迭代法求解 6 自由度机器人逆解算法;何理等;《机床与液压》;20151115;第43卷(第21期);第107-112页 *
基于腹腔微创手术机器人的主从控制技术研究;谢琦等;《机器人》;20110115;第33卷(第1期);第53-58页 *

Also Published As

Publication number Publication date
CN107688293A (zh) 2018-02-13

Similar Documents

Publication Publication Date Title
CN107688293B (zh) 一种微创外科手术机器人的建模方法
Burgner-Kahrs et al. Workspace characterization for concentric tube continuum robots
CN111037561B (zh) 一种空间注册方法和装置
Richter et al. Augmented reality predictive displays to help mitigate the effects of delayed telesurgery
CN104736093A (zh) 接头估测和控制中的假想自由度
CN108406769B (zh) 串联机器人的关节旋转角度确定方法及装置
CN104718054A (zh) 操纵机械体的活动的假想自由度
CN104736094A (zh) 操纵外科***的活动的假想自由度
CN114343847A (zh) 基于光学定位***的手术机器人的手眼标定方法
Lu et al. Kinematic modeling of a class of n-tendon continuum manipulators
Eslamian et al. Towards the implementation of an autonomous camera algorithm on the da vinci platform
CN113742992B (zh) 基于深度学习的主从控制方法及应用
CN112957125B (zh) 一种腔镜手术机器人的手术臂初始位姿规划方法及装置
Singh et al. DH parameters augmented with dummy frames for serial manipulators containing spatial links
Saini et al. Intelligent control of a master-slave based robotic surgical system
Bai et al. An error compensation method for surgical robot based on RCM mechanism
Tian et al. Low delay control algorithm of robot arm for minimally invasive medical surgery
CN116141330A (zh) 机器人的运动控制方法、装置、机器人设备及存储介质
Sagitov et al. Design of simple one-arm surgical robot for minimally invasive surgery
CN115781690A (zh) 多关节机械臂的控制方法、装置、电子设备及存储介质
CN105629880B (zh) 用于具有三角形远心点机构的串联机械臂逆运动学数值解法
Almusawi et al. Artificial neural network based kinematics: Case study on robotic surgery
CN117503363A (zh) 运动臂***的自标定方法及手术机器人***
Abdelaziz et al. Multiple configurations for puncturing robot positioning
JP2022100627A (ja) ロボットの制御位置を決定する方法、及び、ロボットシステム

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
TA01 Transfer of patent application right

Effective date of registration: 20181018

Address after: 401120 1 office buildings, Jinshan International Industrial City, 18 of Nei sang Road, Hui Xing street, Yubei District, Chongqing.

Applicant after: Chongqing Jinshan Medical Instrument Co., Ltd.

Applicant after: Chongqing Jinshan medical robot Co., Ltd.

Address before: 401120 1 office buildings, Jinshan International Industrial City, 18 of Nei sang Road, Hui Xing street, Yubei District, Chongqing.

Applicant before: Chongqing Jinshan Medical Instrument Co., Ltd.

TA01 Transfer of patent application right
CB03 Change of inventor or designer information

Inventor after: Xiao Qiaomu

Inventor after: Bai Long

Inventor after: Yang Jianxing

Inventor before: Xiao Qiaomu

Inventor before: Bai Long

Inventor before: Yang Jianxin

Inventor after: Wang Liao

Inventor after: Bai Long

Inventor after: Zhang Zhiliang

Inventor after: Xiao Qiaomu

Inventor after: Yang Jianxing

Inventor before: Xiao Qiaomu

Inventor before: Bai Long

Inventor before: Yang Jianxing

CB03 Change of inventor or designer information
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right

Effective date of registration: 20210128

Address after: 401120 No.18, Cuiping Lane 2, Huixing street, Yubei District, Chongqing

Patentee after: CHONGQING JINSHAN MEDICAL ROBOT Co.,Ltd.

Address before: 401120 1 office buildings, Jinshan International Industrial City, 18 of Nei sang Road, Hui Xing street, Yubei District, Chongqing.

Patentee before: CHONGQING JINSHAN MEDICAL APPLIANCE Co.,Ltd.

Patentee before: CHONGQING JINSHAN MEDICAL ROBOT Co.,Ltd.

TR01 Transfer of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: A modeling method of minimally invasive surgical robot

Effective date of registration: 20210526

Granted publication date: 20210112

Pledgee: Changshou sub branch of Bank of Chongqing Co.,Ltd.

Pledgor: CHONGQING JINSHAN MEDICAL ROBOT Co.,Ltd.

Registration number: Y2021500000017

PE01 Entry into force of the registration of the contract for pledge of patent right
PC01 Cancellation of the registration of the contract for pledge of patent right

Date of cancellation: 20210802

Granted publication date: 20210112

Pledgee: Changshou sub branch of Bank of Chongqing Co.,Ltd.

Pledgor: CHONGQING JINSHAN MEDICAL ROBOT Co.,Ltd.

Registration number: Y2021500000017

PC01 Cancellation of the registration of the contract for pledge of patent right