CN111870288A 手术机器人及其机械臂的控制方法、控制装置 （深圳市精锋医疗科技有限公司）

道荷坳社区龙岗大道8288号大运软件置本发明涉及一种手术机器人及其机械臂的内部的安装状态信息及位置状态信息获得的动模型计算负载的六维力/力矩矢量；根据总的六的六维力/力矩矢量计算施加于动力机构的外力量获得动力机构在机械臂的基坐标系的目标位2在所述操作命令关联于所述任务自由度仅与所述机械臂的位置有效自由度或姿态有换算所述动力机构在基坐标系的当前位姿信息得到所述动力机构位于所述受力连杆根据所述受力连杆的目标位姿信息控制所述受力连杆及其近端的各所述连杆运动使构及所述动力机构与所述受力连杆之间的各所述连杆运动使所述动力机构保持当前位置解析第二受力连杆的外力获得所述第二受力连杆在相邻所述受力连杆的坐标系的目获取所述动力机构在基坐标系的当前位姿信息根据第一受力连杆的目标位姿信息控制第一受力连杆及其近端的各所述连杆运动使受力连杆及与其相邻所述受力连杆之间的各所述连杆运动使所述第二受力连杆到达对应所述受力连杆之间的各所述连杆运动使所述动力机构所述机械臂包括三个位置有效自由度及三个所述操作命令仅关联于所述机械臂的三个姿态有效所述机械臂包括三个位置有效自由度及两个3所述操作命令仅关联于所述机械臂的两个姿态有效自所述机械臂具有沿近端向远端依次设置的一个沿竖直方向转动的自由度以及一个绕垂直于竖在所述操作命令关联于所述任务自由度仅与所述机械臂的位置有效自由度或姿态有换算所述动力机构在基坐标系的当前位姿信息得到所述动力机构位于所述受力连杆根据所述受力连杆的目标位姿信息控制所述受力连杆及其近端的各所述连杆运动使构及所述动力机构与所述受力连杆之间的各所述连杆运动使所述动力机构保持当前位置解析第二受力连杆的外力获得所述第二受力连杆在相邻所述受力连杆的坐标系的目获取所述动力机构在基坐标系的当前位姿信息根据第一受力连杆的目标位姿信息控制第一受力连杆及其近端的各所述连杆运动使受力连杆及与其相邻所述受力连杆之间的各所述连杆运动使所述第二受力连杆到达对应所述受力连杆之间的各所述连杆运动使所述动力机构4其中，所述计算机程序由所述处理器加载并5[0002]本发明涉及微创手术医疗器械领域，特别是涉及一种手术机器人及其机械臂的[0003]微创手术是指利用腹腔镜、胸腔镜等现代医疗器械及相关设备在人体腔体内部生对患者实施手术操作之前，需要拖动机械臂以使其远端能够运动至医生规划的患者手内部的安装状态信息及位置状态信息获得的动力机构在相应状态下的负载参数，负载参机构的外力的六维力/力矩矢量；解析外力的六维力/力矩矢量获得动力机构在机械臂的基坐标系的目标位姿信息，并根据目标位姿信息控制机械臂中各关节运动使动力机构到置状态信息与各所述动力部相对于相应所述导轨的位置状态相关；所述安装状态信息包6[0009]其中，所述根据所述动力机构内部的安装状态信息及位置状态信置状态时对应的负载参数，分别建立对应于所述动力机构的每一安装状态的一个参数计[0010]其中，所述根据所述动力机构内部的安装状态信息及位置状态信所述参数计算模型及所述动力机构的位置状态信息计算出所述动力机构在相应状态下的力/力矩矢量获得所述动力机构在所述机械臂的基坐标系的目标位姿信息以使所述动力述任务自由度与所述机械臂的有效自由度完全匹配的情况相关联，根据所述第一操作命令解析获得的所述目标位姿信息可对所述动力机构进行自由拖动控制；所述第二操作命令同所述任务自由度与所述机械臂的有效自由度不完全匹配、但包含于所述机械臂的有效自由度的情况相关联，根据所述第二操作命令解析获得的所述目标位姿信息只能在设[0013]其中，所述第二操作命令同所述动力机构的任务自由度选择自所述机械臂的有将所述外力的六维力/力矩矢量转换为所述动力机构在所述机械臂的基坐标系的目标位维力传感器下的一组负载参数，一组负载参数包括位于相应六维力传感器远端的各连杆确定相应六维力传感器坐标系下、由六维力传感器远端的各连杆导致的负载对应的负载别计算每一六维力传感器下的负载的六维力/力矩矢量；获取零偏的六维力/力矩矢量及7的外力的六维力/力矩矢量及作用于其远端相邻一个所述六维力传感器的外力的六维力/加于受力连杆上的外力的六维力/力矩矢量获得受力连杆在相应坐标系的目标位姿信息，置状态信息与各所述动力部相对于相应所述导轨的位置状态相关；所述安装状态信息包[0017]其中，所述根据所述动力机构内部的安装状态信息及位置状态信置状态时对应的负载参数，分别建立对应于所述动力机构的每一安装状态的一个参数计[0018]其中，所述根据所述动力机构内部的安装状态信息及位置状态信所述参数计算模型及所述动力机构的位置状态信息计算出所述动力机构在相应状态下的务自由度解析施加于所述受力连杆上的外力的六维力/力矩矢量获得所述受力连杆在相述任务自由度与所述机械臂的有效自由度完全匹配的情况相关联，根据所述第一操作命令解析获得的所述目标位姿信息可对所述动力机构进行自由拖动控制；所述第二操作命令同所述任务自由度与所述机械臂的有效自由度不完全匹配、但包含于所述机械臂的有效自由度的情况相关联，根据所述第二操作命令解析获得的所述目标位姿信息只能在设[0021]其中，所述第二操作命令同所述动力机构的任务自由度选择自所述机械臂的有析施加于所述受力连杆上的外力的六维力/力矩矢量获得所述受力连杆在相应坐标系的结合所述动力机构的任务自由度解析所述受力连杆上的外力的六维力/力矩矢量获得所述动力机构在所述机械臂的基坐标系的目标位姿信息；根据所述目标位姿信息控制所述机械臂中各所述连杆运动使所述动力机构8量获得所述受力连杆在相应坐标系的目标位姿信息，并根据所述目标位姿信息控制所述得所述受力连杆在所述机械臂的基坐标系的目标位姿信息；根据所述目标位姿信息控制所述机械臂中所述受力连杆及其近端的各所述连杆运动使所述受力连杆到达对应的目标量获得所述受力连杆在相应坐标系的目标位姿信息，并根据所述目标位姿信息控制所述得所述受力连杆在所述机械臂的基坐标系的目标位姿信息，并获取所述动力机构在所述机械臂的基坐标系的当前位姿信息；换算所述动力机构在所述机械臂的基坐标系的当前位姿信息得到所述动力机构位于所述受力连杆的坐标系、在所述受力连杆到达所述受力连杆在所述机械臂的基坐标系的目标位姿信息对应的目标位姿时的目标位姿信息；根据所述受力连杆的目标位姿信息控制所述受力连杆及其近端的各所述连杆运动使所述受力连杆到达对应的目标位姿、并根据所述动力机构的目标位姿信息控制所述动力机构及所述动力机构与所述受力连杆之间的各所述连杆运动使所述动力机构保持当前位置或位[0025]其中，所述受力连杆为两个以上的情况下，若获取到输入的是所述第一操作命坐标系的目标位姿信息，并根据所述目标位姿信息控制所述机械臂做相应运动的步骤之得所述受力连杆在所述机械臂的基坐标系的目标位姿信息；解析各相邻两个所述受力连杆中相对远离所述机械臂近端的所述受力连杆的外力的六维力/力矩矢量获得所述受力连杆在相邻所述受力连杆的坐标系的目标位姿信息；根据绝对邻近所述机械臂近端的所述受力连杆的目标位姿信息控制绝对邻近所述机械臂近端相邻两个所述连杆中相对远离所述机械臂近端的所述受力连杆的目标位姿信息控制相对远离所述机械臂近端的所述受力连杆及与其相邻所述受力连杆之间的各所述连杆运动使相对远离所述机械臂近端的所述受力连杆到达且所述受力连杆中不包括所述动力机构，所述解析施加于所述受力连杆上的外力的六维连杆上的外力的六维力/力矩矢量获得所述受力连杆在所述机械臂的基坐标系的目标位姿信息；解析各相邻两个所述受力连杆中相对远离所述机械臂近端的所述受力连杆的外力的六维力/力矩矢量获得所述受力连杆在相邻所述受力连杆的坐标系的目标位姿信息；9机械臂的基坐标系的当前位姿信息得到所述动力机构位于相邻所述受力连杆的坐标系、在各所述受力连杆到达相应坐标系的目标位姿信息对应的目标位姿时的目标位姿信息；根据绝对邻近所述机械臂近端的所述受力连杆的目标位姿信息控制绝对邻近所述机械臂近端的所述受力连杆及其近端的各所述连杆运动使绝对邻近所述机械臂近端的所述受力连杆到达对应的目标位姿、根据相对远离所述机械臂近端的所述受力连杆的目标位姿信息控制相对远离所述机械臂近端的所述受力连杆及与其相邻所述受力连杆之间的各所述连杆运动使相对远离所述机械臂近端的所述受力连杆到达对应的目标位姿、根据所述动力机构的目标位姿信息控制所述动力机构及所述动力机构与相邻所述受力连杆之间的各力矩矢量获得所述受力连杆在相应坐标系的目标位姿信息，并根据所述目标位姿信息控杆上的外力的六维力/力矩矢量获得所述受力连杆在所述机械臂的基坐标系的目标位姿坐标系的目标位姿信息；解析除所述动力机构以外的各相邻两个所述受力连杆中相对远离所述机械臂近端的所述受力连杆的外力的六维力/力矩矢量获得所述受力连杆在相邻所述受力连杆的坐标系的目标位姿信息；换算所述动力机构在所述机械臂的基坐标系的目标位姿信息得到所述动力机构位于相邻所述受力连杆的坐标系、在与所述动力机构相邻的所述受力连杆到达相应坐标系的目标位姿信息对应的目标位姿时的目标位姿信息；绝对邻近所述机械臂近端的所述受力连杆的目标位姿信息控制绝对邻近所述机械臂近端的所述受力连杆及其近端的各所述连杆运动使绝对邻近所述机械臂近端的所述受力连杆到达对应的目标位姿、根据相对远离所述机械臂近端的所述受力连杆的目标位姿信息控制相对远离所述机械臂近端的所述受力连杆及与其相邻所述受力连杆之间的各所述连杆运动使相对远离所述机械臂近端的所述受力连杆到达对应的目标位姿、根据所述动力机构的目标位姿信息控制所述动力机构及所述动力机构与相邻所述受力连杆之间的各所述由度解析所述动力机构的外力的六维力/力矩矢量获得所述动力机构在所述机械臂的基坐标系的目标位姿信息，并根据所述动力机构的目标位姿信息控制所述机械臂中各所述联于所述动力机构的任务自由度的操作命令；在所述操作命令关联于所述任务自由度仅与所述机械臂的位置有效自由度或姿态有效自由度匹配的情况、且所述受力连杆不包含所述动力机构位于所述受力连杆的坐标系、在所述受力连杆到达所述受力连杆在基坐标系的目标位姿信息对应的目标位姿时的目标位姿信息；根据所述受力连杆的目标位姿信息控制所述受力连杆及其近端的各所述连杆运动使所述受力连杆到达对应的目标位姿、并根据所述动力机构的目标位姿信息控制所述动力机构及所述动力机构与所述受力连杆之间的各所述连杆运动使所述动力机构保[0031]另一方面，本发明提供一种手术机器人的控制方法，所述手术机器人包括机械获取输入的关联于所述动力机构的任务自由度的操作命令；在所述操作命令关联于所述任务自由度仅与所述机械臂的位置有效自由度或姿态有效自由外力获得所述受力连杆在基坐标系的目标位姿信息，并获取所述动力机构在基坐标系的当前位姿信息，基坐标系指所述机械臂的基坐标系；换算所述动力机构在基坐标系的当前位姿信息得到所述动力机构位于所述受力连杆的坐标系、在所述受力连杆到达所述受力连杆在基坐标系的目标位姿信息对应的目标位姿时的目标位姿信息；根据所述受力连杆的目标位姿信息控制所述受力连杆及其近端的各所述连杆运动使所述受力连杆到达对应的目标位姿、并根据所述动力机构的目标位姿信息控制所述动力机构及所述动力机构与所述受力连杆之间的各所述连杆运动使所述动力机构[0033]通过准确确定受力部件远端结构导致的负载的负载参数，有利于准确确定负载端。[0046]除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描[0048]手术机器人包括主操作台1及从操作设备2。主操作台1具有手柄11医生通过操作手柄11向从操作设备2发送控制命令，以令从操作设备2根据医生操作手柄[0049]一实施例中，可通过手柄11遥操作机械臂21各关节联动以使动力机构22运动至[0050]另一实施例中，可通过拖动动力机构22使机械臂21各关节联动进而使动力机构[0051]如图4所示，动力机构22包括连接于机械臂21远端关节的外壳223，外壳223内部[0052]该动力机构22内部的安装状态及位置状态的变化都容易使负载发生变化进而影一动力部222相对于相应导轨221的位置状态不同于其余动力部222相对于相应导轨的位置状态。图4～图7假设了动力部222上所安装的操作臂31的类型不影响负载变化的情况，这基本能够反映动力机构22内部的不同状态变化，这些状态变化都会导致对于六维力传置于机械臂21远端关节上并与动力机构222的外壳223刚性连接。对于该六维力传感器而[0056]步骤S11,根据动力机构内部的安装状态信息及位置状态信息获得的动力机构在[0058]步骤S12,根据负载参数确定在六维力传感器坐标系下、由动力机构构成的负载[0059]步骤S13,获取机械臂中各关节的位置[0061]具体可通过对六维力传感器采集的数据进行解耦和滤波而获取到相应的六维[0063]步骤S16,解析外力的六维力/力矩矢量获得动力机构在机械臂的基坐标系的目标位姿信息，并根据目标位姿信息控制机械臂中各关节运动使动力机构到达对应的目标轨221位置的改变可以通过控制装置控制驱动部驱动动力部222在导轨221上滑动来进行，比如，控制装置可以生成一些随机位置参数给各驱动部让它们驱动动力部222在导轨221[0071]步骤S115，结合调用的参数计算模型及动力机构的位置状态信息计算出动力机构22中设置有与手术机器人中控制装置连接的读写器，操作臂31安装于动力部222上时，[0074]在该步骤S113之中，为了获取该位置状态信息中各动力部222相对于相应导轨[0075]具体而言，步骤S112所提及的参数计算模型的数量与动力机构22内部的[0080]如果允许只有一个动力部222安装具有图像末端器械34A的操作臂31，总共可以许一特定位置的动力部222安装具有图像末端器械34A的操作臂31且必然已经安装，总共[0081]通过限制不同的条件可以相应减少可识别的安装状态的数量，进而减少参数计[0085]及根据采样并计算参数计算模型输入输出数据对模型参数进行估计进而确定该[0086]其中，该参数计算模型可以是MISO(多输入单输出)形式或MIMO(多输入多输出)相关联的输入输出数据进行训练而获得该参数计算模型。该参数计算模型可以是线性的[0087]上述步骤S112中，可以将参数计算模型及其相对应的安装状态信息关联地存储[0088]上述步骤S12所提及的用于确定负载的六维力/力矩矢量的负载力学模型的负载n+1是参数计算模型的零位参数，且k1～kn+1均通过测定(如标定及/[0091]机械臂21中各关节的位置状态信息可以通过配置于各关节的位置传感器来获取，该传感器简单而言同样可以是驱动各关节运动的驱动部(即具有编码器的电机)中的[0095]步骤S161,解析外力的六维力/力矩矢量为动力机构在机械臂的基坐标系的增量)。[0098]步骤S163，根据各关节组件的位置信息计算出动力机构在机械臂的基坐标系的[0099]其中，动力机构在机械臂的基坐标系的当前位姿信息实际指机械臂的工具坐标型，输出C点与基座的模型转换矩阵t.计算方法为[0100]步骤S164，根据动力机构在机械臂的基坐标系的当前位姿信息及增量位姿信息[0104]步骤S166，根据各关节组件的目标位置信息控制机械臂中各关节组件联动以使[0105]该步骤举例可以使用CSP(周期同步位置)模式并结合PID调控以控制机械臂21中[0106]该实施例中，具体在步骤S16中解析外力的六维力/力矩矢量获得动力机构在机械臂的基坐标系的目标位姿信息之中，可以获取输入的关联于动力机构的任务自由度的与机械臂21的有效自由度完全匹配的情况相关联，根据第一操作命令解析获得的目标位姿信息可对动力机构22进行自由拖动控制；第二操作命令同任务自由度与机械臂21的有解析获得的目标位姿信息只能在设定的任务自由度内对动力机构22进行拖动控制。更进一步地，第二操作命令同动力机构22的任务自由度选择自机械臂21的有效自由度中与姿[0108]具体而言，动力机构22的任务自由度可被理解为动力机构22在笛卡尔空间允许效自由度与机械臂21的构型(即结构特征)相关，动力机构22的有效自由度可被理解为动[0109]可以在步骤S16中根据该任务自由度(的配置信息)对外力的六维力/力矩矢量进行解析，然后再将经过解析后的外力的六维力/力矩矢量映射为动力机构的增量位姿信务自由度的配置信息与机械臂21的有效自由度的信息完全匹配，此时对动力机构22进行力机构22的任务自由度的配置信息包含于机械臂21的有效自由度的信息内、且不完全匹[0116]该步骤S16中，具体可以利用刚度矩阵解析外力的六维力/力矩矢量获得动力机[0118]该刚度矩阵的控制参数可以被设置为是可调节的，以根据需要实现外力信息到钮或触摸条等类型的输入装置一般输入产生[0122]一示例中，对应于离散型的物理参数，该参数调整模型可以是一个控制参数字字典中物理参数与控制参数之间的映射关系，索引并调用相应的控制参数以对刚度矩阵该参数计算模型是确定的一种数学公式，由输入装置经操作产生的连续型的物理参数作为该控制参数计算模型的自变量，控制参数作为该参数计算模型的因变量跟随输入至该于物理参数与控制参数在控制参数计算模型中存在因变量和自变量的关系，根据物理参[0125]一实施例中，若动力机构22的任务自由度的配置信息只部分包含于机械臂21的[0126]该手术机器人可以具有另外一种硬件配置，主要体现在六维力传感器装设数量机构以外配置有六维力传感器的连杆来实现相应的控制目的，尤其适用于在机械臂的冗数通过测定即可获得，而该动力机构的负载参数根据前述实施例记载的步骤S111至步骤的六维力/力矩矢量，结合每一六维力传感器下的负载的六维力/力矩矢量计算出作用于[0133]步骤S25，根据计算出的作用于每一六维力传感器的外力的六维力/力矩矢量及的六维力/力矩矢量、零偏的六维力/力矩矢量与作用于其远端相邻一个六维力传感器的相应六维力传感器坐标系下的总的六维力/力矩矢量大于其远端负载的六维力/力矩矢量、零偏的六维力/力矩矢量与作用于其远端相邻一个六维力传感器的外力的六维力/力下的总的六维力/力矩矢量与其远端负载的六维力/力矩矢量、零偏的六维力/力矩矢量、及作用于其远端相邻一个六维力传感器的外力的六维力/力矩矢量之间的差值则为施加[0135]步骤S26，解析施加于受力连杆上的外力的六维力/力矩矢量获得受力连杆在相机构的任务自由度解析施加于受力连杆上的外力的六维力/力矩矢量获得受力连杆在相[0137]在该示例的硬件配置前提下，机械臂21上装配有六维力传感器的连杆可能存在[0139]结合动力机构的任务自由度解析受力连杆上的外力的六维力/力矩矢量获得动[0141]该情况跟前述实施例如图8所示的情况是一致的，比如也可实现图8示意的配置[0143]解析受力连杆上的外力的六维力/力矩矢量获得受力连杆在机械臂的基坐标系[0144]根据目标位姿信息控制机械臂中受力连杆及其近端的各连杆运动使受力连杆到[0145]该情况将机械臂21相当于分成了两段，并控制受力连杆近端的各连杆相应运动[0147]步骤S2611，解析受力连杆上的外力的六维力/力矩矢量获得受力连杆在机械臂的目标位姿得条件下，换算动力机构在机械臂的基坐标系的当前位姿信息获得动力机构:r:r是受力连杆在机械臂的基坐标系的目标位姿信息，是受力连杆在机械臂的基坐标使受力连杆到达对应的目标位姿、并根据动力机构的目标位姿信息控制动力机构及动力机构与受力连杆之间的各连杆运动使动力机构保[0151]该情况将机械臂21相当于分成了两段，并控制受力连杆近端的各连杆相应运动使受力连杆到达对应的目标位姿，并控制受力连杆远端的各连杆相应运动使动力机构22[0154]步骤S2621，解析绝对邻近机械臂近端的受力连杆上的外力的六维力/力矩矢量机械臂近端的受力连杆及其近端的各连杆运动使绝对邻近机械臂近端的受力连杆到达对应的目标位姿、根据各相邻两个连杆中相对远离机械臂近端的受力连杆的目标位姿信息控制相对远离机械臂近端的受力连杆及与其相邻受力连杆之间的各连杆运动使相对远离邻近机械臂21近端的受力连杆根据相应目标位姿信息运动使其到达在机械臂的基坐标系的目标位姿以外，其余受力连杆分别根据其各自相应的目标位姿信息运动使相应受力连杆相对于其近端邻近的受力连杆的坐标系分别运动各自到达对应的位姿。如果机械臂21远端的受力连杆远端还具有连杆，这些连杆则跟随机械臂21远端的受力连杆进行运动即[0160]步骤S2631，解析绝对邻近机械臂近端的受力连杆上的外力的六维力/力矩矢量杆到达相应坐标系的目标位姿信息对应的目标位姿的条件下，换算动力机构在机械臂的机械臂近端的受力连杆及其近端的各连杆运动使绝对邻近机械臂近端的受力连杆到达对应的目标位姿、根据相对远离机械臂近端的受力连杆的目标位姿信息控制相对远离机械臂近端的受力连杆及与其相邻受力连杆之间的各连杆运动使相对远离机械臂近端的受力连杆到达对应的目标位姿、根据动力机构的目标位姿信息控制动力机构及动力机构与相邻受力连杆之间的各连杆运动使动力机构[0165]该情况下机械臂21也相当于被划分成了多段，各受力连杆相对于各自相应的坐[0167]步骤S2641，解析绝对邻近机械臂近端的受力连杆上的外力的六维力/力矩矢量[0168]步骤S2642，解析动力机构的外力的六维力/力矩矢量获得动力机构在机械臂的的受力连杆的外力的六维力/力矩矢量获得受力连杆在相邻受力连杆的坐标系的目标位的目标位姿的条件下，换算动力机构在机械臂的基坐标系的目标位姿信息获得动力机构机械臂近端的受力连杆及其近端的各连杆运动