CN117681228A

CN117681228A - 一种机器人主手端的控制装置及方法

Info

Publication number: CN117681228A
Application number: CN202311713916.7A
Authority: CN
Inventors: 李为民; 刘丹; 请求不公布姓名
Original assignee: West China Hospital of Sichuan University; Shanghai Microport Medbot Group Co Ltd
Current assignee: West China Hospital of Sichuan University; Shanghai Microport Medbot Group Co Ltd
Priority date: 2023-12-13
Filing date: 2023-12-13
Publication date: 2024-03-12

Abstract

本申请提供了一种机器人主手端的控制装置及方法，其中，目标机器人的主手端的工作区间被划分为多个测量子区域，该装置包括：确定模块，用于在目标机器人控制的过程中，确定主手末端所在的测量子区域；匹配模块，用于匹配主手末端所在的测量子区域的MDH参数，其中，所述多个测量子区域中各测量子区域的MDH参数是预先进行分区的MDH参数标定得到的；控制模块，用于根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制。通过上述方案解决了现有的通过求解整个工作区域内的平均值的方式进行MDH参数确定所存在的准确度和精度较低的问题，达到了有效提升控制精度和准确度的技术效果。

Description

一种机器人主手端的控制装置及方法

技术领域

本申请属于设备控制技术领域，尤其涉及一种机器人主手端的控制装置及方法。

背景技术

现有的机器人参数的手眼标定，一般是基于单目相机进行的机器人工作的整个区域的参数标定，然而，单目相机标定的精度较低，且求解整个工作区域内的平均值的方式进行参数标定，数据不够精细，导致参数标定的精度较低。

针对现有的机器人参数标定的精度较低的问题，目前尚未提出有效的解决方案。

发明内容

本申请目的在于提供一种机器人主手端的控制装置及方法，可以提升对机器人参数标定的精度，从而提升对机器人控制的精度和准确度。

本申请提供一种机器人主手端的控制装置及方法是这样实现的：

一种机器人主手端的控制装置，目标机器人的主手端的工作区间被划分为多个测量子区域，包括：

确定模块，用于在目标机器人控制的过程中，确定主手末端所在的测量子区域；

匹配模块，用于匹配主手末端所在的测量子区域的MDH参数，其中，所述多个测量子区域中各测量子区域的MDH参数是预先进行分区的MDH参数标定得到的；

控制模块，用于根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制。

在一个实施方式中，上述装置还包括：

标定模块，用于对所述多个测量子区域中的各测量子区域预先进行分区的MDH参数标定；

所述标定模块包括：

划分单元，用于以所述目标机器人的主手端的工作区间所在的空间坐标系，将所述目标机器人的主手端的工作区间划分为多个测量子区域；

建立单元，用于建立目标机器人的主手端的理论运动学模型；

设置单元，用于为所述主手端设置在当前测量子区域中多组构型；

控制单元，用于控制所述主手端依次运行至所述多组构型中的各组构型；

采集单元，用于采集得到所述主手端运动至多组构型中的各组构型时主手末端的测量位姿；

计算单元，用于通过所述理论运动学模型，计算得到所述主手端运动至多组构型中的各组构型时主手末端的理论位姿；

辨识单元，用于根据运动至各组构型时主手末端的测量位姿与理论位姿的差值，对所述理论运动学模型中的MDH参数进行参数辨识，以得到当前测量子区域标定后的MDH参数。

在一个实施方式中，上述计算单元可以包括：

获取子单元，用于获取主手端运动至当前构型时机械臂的关节码盘数值；

转换子单元，用于将所述关节码盘数值，转换为关节角度；

第一计算子单元，用于根据所述理论运动学模型，计算得到所述关节角度对应的主手末端的理论位姿；

其中，所述主手末端的理论位姿包括：位置信息和姿态信息，其中，位置信息通过主手末端相对于主手基坐标系的平移向量表征，姿态信息通过主手末端相对于主手基坐标系的旋转矩阵表征。

在一个实施方式中，所述辨识单元包括：

第二计算子单元，用于根据运动至各组构型时主手末端的测量位姿与理论位姿，计算得到多组构型中各组构型对应的位置偏差和姿态偏差；

生成子单元，用于根据各组构型对应的位置偏差和姿态偏差求取均值，并通过均值生成误差函数；

建立子单元，用于建立针对所述误差函数的损失函数；

确定子单元，用于通过优化损失函数，得到当前测量子区域标定后的MDH参数。

在一个实施方式中，所述划分单元具体用于以所述目标机器人的主手端的工作区间所在的空间坐标系，将主手工作区间按照上、下、左、右、前、后划分为八个象限区域；将划分得到的八个象限区域，作为主手端的工作区间的八个测量子区域。

在一个实施方式中，所述建立单元具体用于：

获取所述目标机器人的初始MDH参数表；

根据所述初始MDH参数表，转换得到所述主手端相邻两个关节的变换矩阵；

根据相邻两个关节的变换矩阵，得到主手末端坐标系相对于主手端基坐标系的变换矩阵；

将主手末端坐标系相对于主手端基坐标系的变换矩阵，作为所述目标机器人的主手端的理论运动学模型。

在一个实施方式中，所述标定模块还包括：

生成单元，用于根据运动至各组构型时主手末端的测量位姿与理论位姿的差值，对所述理论运动学模型中的MDH参数进行参数辨识，以得到当前测量子区域标定后的MDH参数之后，通过参数辨识，在每个测量子区域中得到一个可靠目标点和该目标点对应的MDH参数；

连接单元，用于将多个测量子区域的各测量子区域中的可靠目标点作为顶点相连，得到一个空间区域；

插值单元，用于对多个测量子区域的各测量子区域中的可靠目标点对应的MDH参数进行插值，将MDH参数转换为所述空间区域内任意点的连续函数。

在一个实施方式中，所述控制模块还用于在根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制的过程中，确定主手末端的位置是否位于所述空间区域中；在确定所述主手末端的位置位于所述空间区域中的情况下，通过所述连续函数，确定所述主手末端的位置对应的MDH参数，以对所述主手端进行运动控制；在确定所述主手末端的位置不位于所述空间区域中的情况下，在所述空间区域中确定与主手末端的位置空间距离最近的点作为临近点，通过所述连续函数，确定所述临近点对应的MDH参数，以对所述主手端进行运动控制。

一种机器人主手端的控制方法，所述方法包括：目标机器人的主手端的工作区间被划分为多个测量子区域；

在目标机器人控制的过程中，确定主手末端所在的测量子区域；

匹配主手末端所在的测量子区域的MDH参数，其中，所述多个测量子区域中各测量子区域的MDH参数是预先进行分区的MDH参数标定得到的；

根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制。

一种电子设备，包括处理器以及用于存储处理器可执行指令的存储器，所述处理器执行所述指令时实现上述方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序/指令，该计算机程序/指令被处理器执行时实现上述方法的步骤。

本申请提供的机器人主手端的控制方法，将主手端的工作区间划分为多个测量子区域，辨识得到多组MDH参数，每一组MDH构成的运动学模型在所在的测量子区域内是最贴近真实运动情况的，基于此，在进行主手机械臂控制的时候，可以采用分区控制的方式，即，在不同的测量子区域内，使用该区域测量数据辨识得到的MDH参数进行运动控制，从而解决了现有的通过求解整个工作区域内的平均值的方式进行MDH参数确定所存在的准确度和精度较低的问题，达到了有效提升控制精度和准确度的技术效果。

附图说明

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

图1是本申请提供的机器人主手端的控制方法一种实施例的方法流程图；

图2是本申请提供的单臂微创机器人系统的架构示意图；

图3是本申请提供的DH基坐标系的示意图；

图4是本申请提供的主手测量区域划分示意图；

图5是本申请提供的双目摄像机拍照示意图；

图6是本申请提供的测量场景示意图；

图7是本申请提供的主手末端的测量位姿和理论位姿的获取流程图；

图8是本申请提供的权重矩阵的建立流程图；

图9是本申请提供的Loss函数的优化流程图；

图10是本申请提供的长方体构造示意图；

图11是本申请提供的长方体外点的示意图；

图12是本申请提供的一种机器人主手端的控制方法的电子设备的硬件结构框图；

图13是本申请提供的机器人主手端的控制装置的模块结构示意图。

具体实施方式

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

针对现有的机器人参数标定精度不高的问题，在本例中，将主手的工作区间划分为多个测量区域，最终辨识得的多组MDH参数。每一组MDH构成的运动学模型在所在的测量区域内是最贴近真实运动情况的。为此，在进行主手机械臂控制的时候，可以采用分区控制的方式，即，在不同的测量区域内，使用该区域测量数据辨识得到的MDH参数进行运动控制。

图1是本申请提供的机器人主手端的控制方法一种实施例的方法流程图。虽然本申请提供了如下述实施例或附图所示的方法操作步骤或装置结构，但基于常规或者无需创造性的劳动在所述方法或装置中可以包括更多或者更少的操作步骤或模块单元。在逻辑性上不存在必要因果关系的步骤或结构中，这些步骤的执行顺序或装置的模块结构不限于本申请实施例描述及附图所示的执行顺序或模块结构。所述的方法或模块结构的在实际中的装置或终端产品应用时，可以按照实施例或者附图所示的方法或模块结构连接进行顺序执行或者并行执行(例如并行处理器或者多线程处理的环境，甚至分布式处理环境)。

具体的，如图1所示，上述的机器人主手端的控制方法，可以如图1所示，包括如下步骤：

步骤101：在目标机器人控制的过程中，确定主手末端所在的测量子区域，其中，目标机器人的主手端的工作区间被划分为多个测量子区域；

步骤102：匹配主手末端所在的测量子区域的MDH参数，其中，所述多个测量子区域中各测量子区域的MDH参数是预先进行分区的MDH参数标定得到的；

步骤103：根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制。

其中，上述对多个测量子区域中的各测量子区域预先进行分区的MDH参数标定可以包括：

S1：以所述目标机器人的主手端的工作区间所在的空间坐标系，将所述目标机器人的主手端的工作区间划分为多个测量子区域；

例如：可以以目标机器人的主手端的工作区间所在的空间坐标系，将主手工作区间按照上、下、左、右、前、后划分为八个象限区域；将划分得到的八个象限区域，作为主手端的工作区间的八个测量子区域。

S2：建立目标机器人的主手端的理论运动学模型；

其中，主手端的理论运动学模型可以是基于MDH参数法建立主手的运动学模型，其中，基座为杆0，从基座往后依次定义为杆1、杆2、…杆i，其中，杆i离基座近的一端的关节为关节i，远的一端为关节i+1。根据关节i的轴向确定Z_i轴，相邻的两个Z轴Z_i与Z_i+1确定X_i轴，Y_i由右手坐标系Z_i×X_i确定。上述MDH参数法中包含四个重要参数：杆件长度a_i为Z_i-1到Z_i的距离、杆件扭角α_i为从Z_i-1到Z_i的转角、关节距离d_i为X_i-1到X_i的距离、关节转角θ_i为X_i-1到X_i的转角。

例如，如果主手的机械臂为7关节的，那么获取目标机器人的初始MDH参数表；根据所述初始MDH参数表，转换得到所述主手端相邻两个关节的变换矩阵；根据相邻两个关节的变换矩阵，得到主手末端坐标系相对于主手端基坐标系的变换矩阵；将主手末端坐标系相对于主手端基坐标系的变换矩阵，作为所述目标机器人的主手端的理论运动学模型。即，得到主手末端坐标系{7}相对于基坐标系{0}的变换矩阵，将将该变换矩阵作为主手末端位姿的理论运动学模型。

S3：为所述主手端设置在当前测量子区域中多组构型；

S4：控制所述主手端依次运行至所述多组构型中的各组构型；

S5：通过双目摄像机，采集得到所述主手端运动至多组构型中的各组构型时主手末端的测量位姿；

具体的，可以搭建靶标场并设置双目摄像机，首先，可以将靶标球固定在主手末端的基坐标系原点，进行拍照并以此建立工作坐标系。在每个测量子区域内，设计多组空间主手机械臂构型作为测量样本，在测量过程中保证靶标球始终处于相机的视场内，控制主手机械臂摆出不同构型，确定主手运动到指定位姿后，对同一位姿进行连续拍照测量(得到多个测量值)，并记录该位姿下主手机械臂各个关节码盘的数值，通过在各个测量子区域内重复上述测量步骤，以完成数据采集。

S6：通过所述理论运动学模型，计算得到所述主手端运动至多组构型中的各组构型时主手末端的理论位姿；

具体的，通过所述理论运动学模型，计算得到所述主手端运动至多组构型中的各组构型时主手末端的理论位姿，可以包括：获取主手端运动至当前构型时机械臂的关节码盘数值；将所述关节码盘数值，转换为关节角度；

根据所述理论运动学模型，计算得到所述关节角度对应的主手末端的理论位姿；其中，所述主手末端的理论位姿包括：位置信息和姿态信息，其中，位置信息通过主手末端相对于主手基坐标系的平移向量表征，姿态信息通过主手末端相对于主手基坐标系的旋转矩阵表征。

例如：将记录的主手机械臂的关节码盘数值转换得到关节角度[q₁,q₂,q₃,q₄,q₅,q₆,q₇]，根据之前建立的主手运动学模型，计算得到该关节角度下对应的主手末端位姿其中，P_mdh为主手末端相对于主手基坐标系的平移向量(位置信息)，R_mdh为主手末端相对于主手基坐标系的旋转矩阵(姿态信息)，通过双目摄像机实际测量的主手基坐标系下的末端位置可以表示为/>

S7：根据运动至各组构型时主手末端的测量位姿与理论位姿的差值，对所述理论运动学模型中的MDH参数进行参数辨识，以得到当前测量子区域标定后的MDH参数。

在实现的时候，可以根据运动至各组构型时主手末端的测量位姿与理论位姿，计算得到多组构型中各组构型对应的位置偏差和姿态偏差；根据各组构型对应的位置偏差和姿态偏差求取均值，并通过均值生成误差函数；建立针对所述误差函数的损失函数；通过优化损失函数，得到当前测量子区域标定后的MDH参数。

其中，上述误差函数可以表示为：

其中，e(mdh)表示误差，λ表示权重，用于表征位置偏差和姿态偏差在整体误差中的相对重要程度，n表示构型的组数；

其中，误差的累加从i＝1至i＝m

其中，e_pos表示位置偏差，e_pos＝P_cam-P_mdh，P_cam表示主手末端相对于主手基坐标系的平移向量的测量值，P_mdh表示主手末端相对于主手基坐标系的平移向量的理论值，e_rot表示姿态偏差，e_rot＝2×acos(|q_cam·q_mdh|)，q_cam表示主手末端相对于主手基坐标系的旋转矩阵的测量值的四元数，q_mdh表示主手末端相对于主手基坐标系的旋转矩阵的理论值的四元数，m表示当前构型实测拍摄的数据组数，i表示当前构型实测拍摄的第i个数据组。

上述损失函数可以表示为：

其中，φ(mdh)表示求MDH参数的损失值，W为权重矩阵；

其中，w_i表示当前构型的第i个数据组方差的倒数，m表示每组构型实测拍摄的数据组数。

在进行参数辨识的时候，最后得到的，因为MDH参数中包括四个参数，主手机械臂为七自由度结构，每个测量子区域所辨识得到的参数就是4乘以7个，即，28个。相应的，如果是划分为4个测量子区域，那么就一个有28乘以4个参数，如果划分为8个测量子区域，那么就有28乘以8个测量子区域。

考虑到在实际的控制过程中，相邻两个测量子区域的控制参数会发生跳变，为了减少这种不确定性的发生，在本例中，通过将主手的工作区间划分为n个测量区域，最终辨识得的mdh参数为n组。每一组mdh参数构成的运动学模型在所在的测量区域内是最贴近真实运动情况的。为此，在进行主手机械臂控制的时候，可以采用分区控制的方式，即，在不同的测量区域内，使用该区域测量数据辨识得到的mdh参数进行运动控制。

为了避免相邻两个分区控制参数跳变带来的不稳定性，可以将对相邻两区的mdh参数进行插值。假设将主手的工作区间划分为八个区域，在每个区域内都经过辨识得到一个可靠的点和它的mdh参数，那么将这8个点作为顶点相连，得到一个空间区域，对多个测量子区域的各测量子区域中的可靠目标点对应的MDH参数进行插值，将MDH参数转换为所述空间区域内任意点的连续函数；在根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制的过程中，确定主手末端的位置是否位于所述空间区域中；在确定所述主手末端的位置位于所述空间区域中的情况下，通过所述连续函数，确定所述主手末端的位置对应的MDH参数，以对所述主手端进行运动控制；在确定所述主手末端的位置不位于所述空间区域中的情况下，在所述空间区域中确定与主手末端的位置空间距离最近的点作为临近点，通过所述连续函数，确定所述临近点对应的MDH参数，以对所述主手端进行运动控制。

下面结合一个具体实施例对上述方法进行说明，然而值得注意的是，该具体实施例仅是为了更好地说明本申请，并不构成对本申请的不当限定。

单臂微创机器人系统如图2所示，可以由医生控制台、图像台车以及患者台车组成。操作者可以在控制台通过操作主手和脚踏来控制从端的器械及内窥镜，图像台车主要用于显示内窥镜画面，使得助手医生在手术过程中能够更好地协助主刀医生。患者台车由驱动臂和器械工具臂组成，驱动臂用于术前摆位或术中大范围调整手术视野，手术器械及内窥镜分别安装在四条器械工具臂上。

在手术过程中，主刀医生主要通过医生控制台的主手进行手术操作，其中，主手为七自由度结构，其末端控制点最终在工作空间内拥有位置参数和姿态参数，并与从端器械形成主从映射关系。因此，主手参数标定的精确度将极大影响主刀医生的操作手感以及手术的精度。目前的控制算法一般都是基于主手的运动学模型建立的，在主手的制造、装配以及安装过程中会有各种因素导致主手的实际运动学模型与理论运动学模型有差异。

针对现有的单目相机所存在的手眼标定方法精度较低，以及在整个机器人工作区域内做参数标定所存在的精度较低的问题，在本例中提供了一种分区域的动态MDH参数补偿方法，以进行分区域的精细化标定，提高标定精度。通过新的参数标定方法，提高参数标定的精度，获得更准确的主手模型以提升控制精度和操作手感。

具体的，在本例中提供了一种基于视觉的腔镜手术机器人参数标定方法，建立DH基坐标系，然后，细化主手运动工作区间，在各个细化的工作区间进行参数标定，获得各分区的DH参数并在过渡区间进行参数融合，可以包括如下步骤：

步骤1：对机器人的主手端建立如图3所示的DH基坐标系；

步骤2：基于不同工作区域，涉及的DH误差也不一样，因此，可以划分工作区间并基于双目相机进行参数标定。

可以将主手工作的整个工作区间看作是空间坐标系，可以如图4所示，按照立体坐标系的八个象限划分了八个测量子区域，即，基于x、y和z轴，得到xoy面、yoz面和zox面将其划分为8个测量子区域，依据空间坐标系将主手工作区间划分为上、下、左、右、前、后8个区域，得到主手空间坐标系下的8个象限区域。在划分工作区间后，可以如图5所示在各个区间内设定几组不被遮挡的主手末端位置，并设置双目相机导轨，依据不同区域预设的位置点移动双目相机进行点位采集。

步骤3：参数标定：使用加权非线性最小二乘法进行参数辨识，最终的参数可以包括：关节轴夹角、关节转角、连杆长度、连杆偏距。

步骤4：在参数标定完后，在控制过程中，将使用各分区对应的DH参数进行补偿，以提高控制精度。具体的，可以通过空间插值的方法，实现相邻分区DH参数的平滑过渡，避免参数跳变带来的不稳定性。

基于此，在本例中先建立了主手的运动学模型，以获得主手参数的理论值；然后，再在主手的工作空间内划分出多个测量子区域，并使用双目摄像机进行测量。将获得的数据进行处理后，使用加权非线性最小二乘法进行参数辨识，通过辨识得到的参数计算出主手的实际运动学模型并输入控制系统以提升控制精度。

其中，主手理论运动学模型的建立可以包括：

以七自由度结构的主手为例，基于MDH参数法建立主手的运动学模型，其中，基座为杆0，从基座往后依次定义为杆1、杆2、…杆i，其中，杆i离基座近的一端的关节为关节i，远的一端为关节i+1。

根据关节i的轴向确定Z_i轴，相邻的两个Z轴Z_i与Z_i+1确定X_i轴，Y_i由右手坐标系Z_i×X_i确定。

上述MDH参数法中包含四个重要参数：杆件长度a_i为Z_i-1到Z_i的距离、杆件扭角α_i为从Z_i-1到Z_i的转角、关节距离d_i为X_i-1到X_i的距离、关节转角θ_i为X_i-1到X_i的转角。

根据上述定义，可以得到主手的MDH参数表，如下表1所示：

表1

根据MDH参数表可以得到相邻两个关节的变换矩阵：

最终，可以得到主手末端坐标系{7}相对于基坐标系{0}的变换矩阵：

将该变换矩阵作为主手末端位姿的理论运动学模型。

其中，数据采集可以包括：

S1：计算主手的实际工作区间，并在该工作区间内，以主手末端(即，七关节)的基坐标系原点作为测量区域的原点，根据实际测量情况在主手的工作空间内划分出多个测量区域。例如，按照立体坐标系的八个象限划分了八个测量子区域。

S2：搭建靶标场并设置双目摄像机，首先，使用靶标球标定测量区原点，再将靶标球固定在主手末端，在同一测量区域内设计多组不同构型，得到不同的主手末端位姿，并进行多次拍照测量。

具体的，可以将靶标球固定在主手末端的基坐标系原点，进行拍照并以此建立工作坐标系。在每个测量区域内，设计多组空间主手机械臂构型作为测量样本，在测量过程中保证靶标球始终处于相机的视场内。控制主手机械臂摆出不同构型，确定主手运动到指定位姿后，如图6所示，对同一位姿进行连续拍照测量，在图6中，黑色圆点为主手末端靶标球的真实位置，黑色十字为真实的多次测量值，并记录该位姿下主手机械臂各个关节码盘的数值。通过在各个测量区域内重复上述测量步骤，完成数据采集。

其中，数据处理可以包括：

S1：如图7所示，将记录的主手机械臂的关节码盘数值转换得到关节角度[q₁,q₂,q₃,q₄,q₅,q₆,q₇]，根据预先建立的主手运动学模型，计算得到该关节角度下对应的主手末端位姿其中，P_mdh为主手末端相对于主手基坐标系的平移向量(位置信息)，R_mdh为主手末端相对于主手基坐标系的旋转矩阵(姿态信息)；实际测量的主手基坐标系下的末端位置信息为/>

S2：计算主手末端的位置偏差值为：e_pos＝P_cam-P_mdh，在计算主手末端的姿态偏差时，首先将旋转矩阵转换成四元数q_cam和q_mdh，并计算两个四元数之间的角度差异e_rot＝2×acos(|q_cam·q_mdh|)。

用mdh代表四个参数，即：

mdh_i＝[α_i,a_i,θ_i,d_i]

假设在一个测量子区域内，设置n组构型进行辨识，每组构型拍摄测量得到m组数据：

误差函数表示为：

其中，λ为权重，用于反映位置和姿态误差在整体误差中的相对重要性，其中，λ的取值可以根据具体需求进行调整。

具体的，可以建立Loss函数：

其中，W为权重矩阵：

对同一测量区域内不同构型的测量数据进行方差计算，方差越小，测量数据越集中，数据的可靠性就越高，因此可以方差的倒数作为权重。具体的，可以如图8所示，同一测量分区内m组测量点的测量数据{T_cam}_m，计算每个测量点{T_cam}的方差(σ²)_m，对每个测量点的方差取倒数w_m＝(σ²)_m ^-1，从而得到权重矩阵。

因为Loss函数是非线性的，在本例中通过加权非线性最小二乘法来进行函数拟合，当Loss函数取到最小值时，模型参数最接近真实参数：

Loss函数的最优条件为：

由此可见，Loss函数的最优问题为非线性零点问题。

假设当mdh＝mdh^*时，Loss函数满足最优条件：

从当前参数mdh₀出发，假设mdh^*＝mdh₀+Δmdh，则：

F(mdh₀+Δmdh)在mdh₀处泰勒展开有：

简化可得：

/>

其中：

随着迭代次数增加，误差会逐渐减小，H_e可忽略，即：

Δmdh≈(J_e(mdh₀)^TJ_e(mdh₀))^-1·W·J_e(mdh₀)^T·e(mdh₀)

如图9所示，根据计算Δmdh，得到新的参数mdh₁＝mdh₀+Δmdh，并代入运动学模型，重新优化Loss函数，得到新的Δmdh，多次迭代直至误差达到允许误差范围后停止迭代。

通过将主手的工作区间划分为n个测量子区域，最终辨识得的mdh参数为n组。每一组mdh参数构成的运动学模型在所在的测量区域内是最贴近真实运动情况的。为此，在进行主手机械臂控制的时候，可以采用分区控制的方式，即，在不同的测量区域内，使用该区域测量数据辨识得到的mdh参数进行运动控制。

为了避免相邻两个分区控制参数跳变带来的不稳定性，可以对相邻两区的mdh参数进行插值。具体的，可以在主手的工作区间内得到一个连续函数，函数的输入是空间坐标(x,y,z)，函数输出是该点对应的mdh参数。假设将主手的工作区间划分为八个区域，在每个区域内都经过辨识得到一个可靠的点和它的mdh参数。例如：如图10所示，将这八个点作为长方体的顶点，在长方体内构造连续函数：

mdh(x,y,z)＝a₁x+a₂y+a₃z+a₄xy+a₅xz+a₆yz+a₇xyz+a₈

求解方程可以得到常数[a₁,…,a₈]，每一个mdh参数对应一组常数[a₁,…,a₈]，根据点的空间坐标和常数[a₁,…,a₈]可以求得长方体内任意一点的mdh参数。

如图11所示，对于长方体之外的点，将选取的长方体上与该点距离最近的点的mdh参数作为该点的mdh参数。例如，点q为长方体之外的点，点p为长方体上距离点q最近的点，因此，将点p的mdh参数，作为点p的mdh参数，其中，点p的mdh参数可以通过mdh_p＝f(x_p,y_p,z_p)求得。

即，通过将工作空间分为长方体之内和长方体之外，然后使用插值法，可以得到连续的mdh参数，避免了控制过程中mdh参数的跳变。

在上例中，基于视觉的腔镜手术机器人参数标定方法，建立DH坐标系，依据加权优化方法，进而进行坐标系的旋转平移变换。划分工作区间，根据主手实际位姿不同，参数标定的结果不同，将机器人主手正常的工作区域精细划分为8个小的区域，采用动态DH参数补偿方法进行标定，并使用双目相机进行点位信息的采集，在采集过程中实现双目相机沿导轨自动运行到不同区域预设的点位位置；各区间进行DH参数标定，根据主手实际位置不同，标定参数不同，采用加权非线性最小二乘法进行参数辨识；经控制器进行补偿，获取8个区域中心的MDH参数，并通过空间插值法在边界过渡区域进行参数融合，获取当前主手位姿的MDH参数，经控制器进行补偿。在建立DH坐标系时，误差大的参数权重分配小，依据此法实现坐标系的旋转平移变换。进一步的，划分工作区间，包含双目相机、相机导轨、点位采集软件等，依据不同位姿点位，分区域进行采集；并设计自动移动的双目相机导轨，可以在有遮挡的情况下自动变换位置完成拍摄，经控制器进行补偿，在8个工作区域内继续细分区域，实现分区域的动态DH参数偏差，细化精度补偿。

通过分区域进行主手标定，实现了动态DH参数标定，相较于利用主手整个工作区域取均值的标定方法，可以有效提升标定结果的精度，采用自动移动双目相机的方式，有效减少了人力成本，通过加权非线性最小二乘法，提出了依据误差的大小重新分配权重的方式，从而可以有效提高参数辨识的精度；进一步的，通过空间插值的方法，基于分区的MDH参数构造连续函数，实现边界区域MDH参数的平滑过渡，避免控制时MDH参数跳变带来的不稳定性。

本申请上述实施例所提供的方法实施例可以在移动终端、计算机终端或者类似的运算装置中执行。以运行在电子设备上为例，图12是本申请提供的一种机器人主手端的控制方法的电子设备的硬件结构框图。如图1所示，电子设备10可以包括一个或多个(图中仅示出一个)处理器02(处理器02可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置)、用于存储数据的存储器04、以及用于通信功能的传输模块06。本领域普通技术人员可以理解，图12所示的结构仅为示意，其并不对上述电子装置的结构造成限定。例如，电子设备10还可包括比图12中所示更多或者更少的组件，或者具有与图12所示不同的配置。

存储器04可用于存储应用软件的软件程序以及模块，如本申请实施例中的机器人主手端的控制方法对应的程序指令/模块，处理器02通过运行存储在存储器04内的软件程序以及模块，从而执行各种功能应用以及数据处理，即实现上述的应用程序的机器人主手端的控制方法。存储器04可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器04可进一步包括相对于处理器02远程设置的存储器，这些远程存储器可以通过网络连接至电子设备10。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

传输模块06用于经由一个网络接收或者发送数据。上述的网络具体实例可包括电子设备10的通信供应商提供的无线网络。在一个实例中，传输模块06包括一个网络适配器(Network Interface Controller，NIC)，其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输模块06可以为射频(Radio Frequency，RF)模块，其用于通过无线方式与互联网进行通讯。

在软件层面，上述机器人主手端的控制装置可以如图13所示，包括：

确定模块1301，用于在目标机器人控制的过程中，确定主手末端所在的测量子区域，其中，目标机器人的主手端的工作区间被划分为多个测量子区域；

匹配模块1302，用于匹配主手末端所在的测量子区域的MDH参数，其中，所述多个测量子区域中各测量子区域的MDH参数是预先进行分区的MDH参数标定得到的；

控制模块1303，用于根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制。

在一个实施方式中，上述装置还可以包括：标定模块，用于对所述多个测量子区域中的各测量子区域预先进行分区的MDH参数标定；该标定模块可以包括：

在一个实施方式中，上述计算单元可以包括：获取子单元，用于获取主手端运动至当前构型时机械臂的关节码盘数值；转换子单元，用于将所述关节码盘数值，转换为关节角度；第一计算子单元，用于根据所述理论运动学模型，计算得到所述关节角度对应的主手末端的理论位姿；其中，所述主手末端的理论位姿包括：位置信息和姿态信息，其中，位置信息通过主手末端相对于主手基坐标系的平移向量表征，姿态信息通过主手末端相对于主手基坐标系的旋转矩阵表征。

在一个实施方式中，所述辨识单元可以包括：第二计算子单元，用于根据运动至各组构型时主手末端的测量位姿与理论位姿，计算得到多组构型中各组构型对应的位置偏差和姿态偏差；生成子单元，用于根据各组构型对应的位置偏差和姿态偏差求取均值，并通过均值生成误差函数；建立子单元，用于建立针对所述误差函数的损失函数；确定子单元，用于通过优化损失函数，得到当前测量子区域标定后的MDH参数。

在一个实施方式中，所述划分单元具体可以用于以所述目标机器人的主手端的工作区间所在的空间坐标系，将主手工作区间按照上、下、左、右、前、后划分为八个象限区域；将划分得到的八个象限区域，作为主手端的工作区间的八个测量子区域。

在一个实施方式中，所述建立单元具体可以用于获取所述目标机器人的初始MDH参数表；根据所述初始MDH参数表，转换得到所述主手端相邻两个关节的变换矩阵；根据相邻两个关节的变换矩阵，得到主手末端坐标系相对于主手端基坐标系的变换矩阵；将主手末端坐标系相对于主手端基坐标系的变换矩阵，作为所述目标机器人的主手端的理论运动学模型。

在一个实施方式中，所述标定模块还可以包括：生成单元，用于根据运动至各组构型时主手末端的测量位姿与理论位姿的差值，对所述理论运动学模型中的MDH参数进行参数辨识，以得到当前测量子区域标定后的MDH参数之后，通过参数辨识，在每个测量子区域中得到一个可靠目标点和该目标点对应的MDH参数；连接单元，用于将多个测量子区域的各测量子区域中的可靠目标点作为顶点相连，得到一个空间区域；插值单元，用于对多个测量子区域的各测量子区域中的可靠目标点对应的MDH参数进行插值，将MDH参数转换为所述空间区域内任意点的连续函数。

在一个实施方式中，控制模块还可以用于在根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制的过程中，确定主手末端的位置是否位于所述空间区域中；在确定所述主手末端的位置位于所述空间区域中的情况下，通过所述连续函数，确定所述主手末端的位置对应的MDH参数，以对所述主手端进行运动控制；在确定所述主手末端的位置不位于所述空间区域中的情况下，在所述空间区域中确定与主手末端的位置空间距离最近的点作为临近点，通过所述连续函数，确定所述临近点对应的MDH参数，以对所述主手端进行运动控制。

在一个实施方式中，所述误差函数可以表示为：

其中：

在一个实施方式中，所述损失函数可以表示为：

其中，φ(mdh)表示求MDH参数的损失值，W为权重矩阵；

本申请的实施例还提供能够实现上述实施例中的机器人主手端的控制方法中全部步骤的一种电子设备的具体实施方式，所述电子设备具体包括如下内容：处理器(processor)、存储器(memory)、通信接口(Communications Interface)和总线；其中，所述处理器、存储器、通信接口通过所述总线完成相互间的通信；所述处理器用于调用所述存储器中的计算机程序，所述处理器执行所述计算机程序时实现上述实施例中的机器人主手端的控制方法中的全部步骤，例如，所述处理器执行所述计算机程序时实现下述步骤：

步骤1：在目标机器人控制的过程中，确定主手末端所在的测量子区域，其中，目标机器人的主手端的工作区间被划分为多个测量子区域；

步骤2：匹配主手末端所在的测量子区域的MDH参数，其中，所述多个测量子区域中各测量子区域的MDH参数是预先进行分区的MDH参数标定得到的；

步骤3：根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制。

本申请的实施例还提供能够实现上述实施例中的机器人主手端的控制方法中全部步骤的一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，该计算机程序被处理器执行时实现上述实施例中的机器人主手端的控制方法的全部步骤，例如，所述处理器执行所述计算机程序时实现下述步骤：

从上述描述可知，本申请实施例将主手端的工作区间划分为多个测量子区域，辨识得到多组MDH参数，每一组MDH构成的运动学模型在所在的测量子区域内是最贴近真实运动情况的，基于此，在进行主手机械臂控制的时候，可以采用分区控制的方式，即，在不同的测量子区域内，使用该区域测量数据辨识得到的MDH参数进行运动控制，从而解决了现有的通过求解整个工作区域内的平均值的方式进行MDH参数确定所存在的准确度和精度较低的问题，达到了有效提升控制精度和准确度的技术效果。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于硬件+程序类实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

上述对本说明书特定实施例进行了描述。其它实施例在所附权利要求书的范围内。在一些情况下，在权利要求书中记载的动作或步骤可以按照不同于实施例中的顺序来执行并且仍然可以实现期望的结果。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。

虽然本申请提供了如实施例或流程图所述的方法操作步骤，但基于常规或者无创造性的劳动可以包括更多或者更少的操作步骤。实施例中列举的步骤顺序仅仅为众多步骤执行顺序中的一种方式，不代表唯一的执行顺序。在实际中的装置或客户端产品执行时，可以按照实施例或者附图所示的方法顺序执行或者并行执行(例如并行处理器或者多线程处理的环境)。

上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、车载人机交互设备、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。

虽然本说明书实施例提供了如实施例或流程图所述的方法操作步骤，但基于常规或者无创造性的手段可以包括更多或者更少的操作步骤。实施例中列举的步骤顺序仅仅为众多步骤执行顺序中的一种方式，不代表唯一的执行顺序。在实际中的装置或终端产品执行时，可以按照实施例或者附图所示的方法顺序执行或者并行执行(例如并行处理器或者多线程处理的环境，甚至为分布式数据处理环境)。术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、产品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、产品或者设备所固有的要素。在没有更多限制的情况下，并不排除在包括所述要素的过程、方法、产品或者设备中还存在另外的相同或等同要素。

为了描述的方便，描述以上装置时以功能分为各种模块分别描述。当然，在实施本说明书实施例时可以把各模块的功能在同一个或多个软件和/或硬件中实现，也可以将实现同一功能的模块由多个子模块或子单元的组合实现等。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

本领域技术人员也知道，除了以纯计算机可读程序代码方式实现控制器以外，完全可以通过将方法步骤进行逻辑编程来使得控制器以逻辑门、开关、专用集成电路、可编程逻辑控制器和嵌入微控制器等的形式来实现相同功能。因此这种控制器可以被认为是一种硬件部件，而对其内部包括的用于实现各种功能的装置也可以视为硬件部件内的结构。或者甚至，可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

本领域技术人员应明白，本说明书的实施例可提供为方法、系统或计算机程序产品。因此，本说明书实施例可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本说明书实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本说明书实施例可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本说明书实施例，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本说明书实施例的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

以上所述仅为本说明书实施例的实施例而已，并不用于限制本说明书实施例。对于本领域技术人员来说，本说明书实施例可以有各种更改和变化。凡在本说明书实施例的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本说明书实施例的权利要求范围之内。

Claims

1.一种机器人主手端的控制装置，其特征在于，目标机器人的主手端的工作区间被划分为多个测量子区域，包括：

2.根据权利要求1所述的装置，其特征在于，还包括：

所述标定模块包括：

3.根据权利要求2所述的装置，其特征在于，所述计算单元包括：

转换子单元，用于将所述关节码盘数值，转换为关节角度；

4.根据权利要求2所述的装置，其特征在于，所述辨识单元包括：

建立子单元，用于建立针对所述误差函数的损失函数；

5.根据权利要求2所述的装置，其特征在于，所述划分单元具体用于以所述目标机器人的主手端的工作区间所在的空间坐标系，将主手工作区间按照上、下、左、右、前、后划分为八个象限区域；将划分得到的八个象限区域，作为主手端的工作区间的八个测量子区域。

6.根据权利要求2所述的装置，其特征在于，所述建立单元具体用于：

获取所述目标机器人的初始MDH参数表；

7.根据权利要求2所述的装置，其特征在于，所述标定模块还包括：

8.根据权利要求7所述的装置，其特征在于，所述控制模块还用于在根据主手末端所在的测量子区域的MDH参数，对所述主手端进行运动控制的过程中，确定主手末端的位置是否位于所述空间区域中；在确定所述主手末端的位置位于所述空间区域中的情况下，通过所述连续函数，确定所述主手末端的位置对应的MDH参数，以对所述主手端进行运动控制；在确定所述主手末端的位置不位于所述空间区域中的情况下，在所述空间区域中确定与主手末端的位置空间距离最近的点作为临近点，通过所述连续函数，确定所述临近点对应的MDH参数，以对所述主手端进行运动控制。

9.一种机器人主手端的控制方法，其特征在于，所述方法包括：目标机器人的主手端的工作区间被划分为多个测量子区域；

10.一种计算机可读存储介质，其上存储有计算机程序/指令，其特征在于，该计算机程序/指令被处理器执行时实现权利要求9所述方法的步骤。