CN115781690A - 多关节机械臂的控制方法、装置、电子设备及存储介质 - Google Patents

Abstract

本申请提供了一种多关节机械臂的控制方法、装置、电子设备及存储介质，该方法包括：确定第一约束点在预设的第一坐标系下的第一坐标，其中，第一约束点用于表征多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点；根据第一坐标，确定第一坐标系与基于执行机构的末端的所在位置建立的第二坐标系之间的偏差信息；基于偏差信息，建立运动学模型，其中，运动学模型用于指示执行机构在执行操作任务时执行机构的末端的第一运动信息与多关节机械臂的各个关节的第二运动信息之间的关系；根据运动学模型，确定多关节机械臂的各个关节的关节转动角度；基于各个关节的关节转动角度，分别控制各个关节的转动。本方案能够控制多关节机械臂。

Description

多关节机械臂的控制方法、装置、电子设备及存储介质

技术领域

本申请实施例涉及机械臂控制技术领域，尤其涉及一种多关节机械臂的控制方法、装置、电子设备及存储介质。

背景技术

随着技术的不断发展，机械臂被应用的越来越多，其中在医疗领域应用的愈加广泛。例如以医疗领域的颅内肿瘤开颅手术为例，传统的颅内肿瘤开颅手术切口都比较大，对于垂体瘤等脑肿瘤患者而言，开颅通常都会留下十几公分的切口，切口大就会出血多，同时增加了术中的风险。因此现在一种手术方式是使用手术机器人通过其多关节机械臂把持内镜进入患者颅脑内部辅助医生完成手术操作，极大程度避免对颅脑内结构的骚扰，使手术的创伤降低到极低程度，从而实现微创。

对于手术机器人的多关节机械臂而言，由于要满足创口小的需要，机械臂的执行机构(如手术器械)在术中需要尽量围绕创口运动以降低对创口处的牵引作用，从而避免损伤创口，实现这样功能的创口约束称为远心不动点约束(又称RCM约束)。相关技术中，往往是通过在多关节机械臂中设计机械的运动机构，来实现多关节机械臂的执行机构围绕创口运动的功能(例如一些方式中，采用平行四变形的机械结构，或者在机械臂末端增加一个2自由度的工具来实现)。

但是，采用机械的运动机构来实现多关节机械臂的执行机构围绕创口运动，存在机械机构约束较大、使用场景受限、容易影响机械臂任务空间等劣势，且容易额外地增加多关节机械臂的结构的成本。因此，需要有一种新的技术方案来至少部分地改善这些问题。

发明内容

为了解决上述问题，本申请实施例提供了一种多关节机械臂的控制方法、装置、电子设备及存储介质，以至少部分地解决上述问题。

根据本申请的一方面，提供了一种多关节机械臂的控制方法，所述多关节机械臂包括执行机构和多个关节，所述多个关节依次连接，所述执行机构连接在依次连接的多个关节的末端，所述多关节机械臂的控制方法包括：

确定第一约束点在预设的第一坐标系下的第一坐标，其中，所述第一约束点用于表征所述多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点；

根据所述第一坐标，确定所述第一坐标系与基于所述执行机构的末端的所在位置建立的第二坐标系之间的偏差信息；

基于所述偏差信息，建立运动学模型，其中，所述运动学模型用于指示所述执行机构在执行操作任务时所述执行机构的末端的第一运动信息与所述多关节机械臂的各个关节的第二运动信息之间的关系；

根据所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度；

基于所述各个关节的关节转动角度，分别控制所述各个关节的转动。

在一些可选的实施例中，所述根据所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度，包括：确定在所述第一坐标系下的所述执行机构的末端的位姿信息；基于所述位姿信息和所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度。

在一些可选的实施例中，所述第二坐标系以所述执行机构的末端的所在位置点为原点、以过所述第一约束点和所述执行机构的末端的所在位置点的直线为z轴建立。

在一些可选的实施例中，所述确定在所述第一坐标系下的所述执行机构的末端的位姿信息，包括：确定所述执行机构的末端的所在位置点在所述第一坐标系下的第二坐标；根据所述第一坐标和所述第二坐标，确定用于指示所述第二坐标系的z轴方向的第二z轴向量；根据所述第二z轴向量以及用于指示所述第一坐标系的z轴方向的第一z轴向量，确定所述执行机构的末端从所述第二坐标系旋转变换到所述第一坐标系的旋转轴向量以及旋转角度，其中，所述旋转轴向量用于指示该旋转变换的旋转轴的方向；根据所述旋转轴向量和所述旋转角度，确定用于指示所述第二坐标系的x轴方向的第二x轴向量以及用于指示所述第二坐标系的y轴方向的第二y轴向量；根据所述第二x轴向量、第二y轴向量、第二z轴向量和所述第二坐标，确定在所述第一坐标系下的所述执行机构的末端的位姿信息。

在一些可选的实施例中，所述根据所述第一向量以及用于指示所述第一坐标系的z轴的方向的第二z轴向量，确定所述执行机构的末端从所述第二坐标系旋转变换到所述第一坐标系的旋转轴向量以及旋转角度，包括：

以公式V＝z₁×z₂计算所述旋转轴向量，其中，V用于表征所述旋转轴向量，z₁用于表征所述第一z轴向量，z₂用于表征所述第二z轴向量；

以公式θ_V＝arccos(z₁·z₂)计算所述旋转角度，其中，θ_V用于表征所述旋转角度。

在一些可选的实施例中，所述根据所述旋转轴向量和所述旋转角度，确定用于指示所述第二坐标系的x轴方向的第二x轴向量以及用于指示所述第二坐标系的y轴方向的第二y轴向量，包括：

以公式x₂＝Rx₁计算所述第二x轴向量，其中，x₂用于表征所述第二x轴向量，x₁用于表征用于指示所述第一坐标系的x轴方向的第一x轴向量；

以公式y₂＝Ry₁计算所述第二y轴向量，其中，y₂用于表征所述第二y轴向量，y₁用于表征用于指示所述第一坐标系的y轴方向的第一y轴向量；

其中，R＝cos(θ_V)+(1-cos(θ_V))VV^T+sinθ_VSkew(V)

其中，R用于表征旋转变换矩阵，θ_V用于表征所述旋转角度，V用于表征所述旋转轴向量，Skew(V)用于表征V的反对称矩阵。

在一些可选的实施例中，所述根据所述第一坐标，确定所述第一坐标系与基于所述执行机构的末端的所在位置建立的第二坐标系之间的偏差信息，包括：根据所述第一坐标，确定所述第一坐标与所述第一坐标系的原点坐标在沿所述第一坐标系的x轴方向的第一偏差、以及所述第一坐标与所述第一坐标系的原点坐标在沿所述第一坐标系的y轴方向的第二偏差，将所述第一偏差和所述第二偏差作为所述偏差信息。

在一些可选的实施例中，所述第一运动信息包括：所述执行机构在执行操作任务时，所述执行机构的末端分别沿所述第一坐标系的x轴方向、y轴方向、z轴方向平移的三个线速度分量信息，所述执行机构的末端分别以所述第一坐标系的x轴方向、y轴方向、z轴方向为轴旋转的三个角速度分量信息；所述第二运动信息包括所述多关节机械臂的各个关节的角速度信息；所述基于所述偏差信息，建立运动学模型，包括：根据所述第一偏差、所述第二偏差以及所述执行机构的末端的所在位置点在所述第一坐标系下的第二坐标，确定所述多关节机械臂的雅可比矩阵，并基于所述雅可比矩阵，建立所述运动学模型，其中，所述雅可比矩阵用于将所述第一运动信息变换到所述第二运动信息。

根据本申请实施例中的另一方面，提供了一种多关节机械臂的控制装置，所述多关节机械臂包括执行机构和多个关节，所述多个关节依次连接，所述执行机构连接在依次连接的多个关节的末端，所述多关节机械臂的控制装置包括：

第一确定模块，用于确定第一约束点在预设的第一坐标系下的第一坐标，其中，所述第一约束点用于表征所述多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点；

第二确定模块，用于根据所述第一坐标，确定所述第一坐标系与基于所述执行机构的末端的所在位置建立的第二坐标系之间的偏差信息；

模型建立模块，用于基于所述偏差信息，建立运动学模型，其中，所述运动学模型用于指示所述执行机构在执行操作任务时所述执行机构的末端的第一运动信息与所述多关节机械臂的各个关节的第二运动信息之间的关系；

第三确定模块，用于根据所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度；

关节控制模块，用于基于所述各个关节的关节转动角度，分别控制所述各个关节的转动。

根据本申请实施例中的再一方面，提供了一种电子设备，包括：所述电子设备包括存储器以及处理器，所述存储器上用于存储计算机可执行程序，所述处理器用于运行所述计算机可执行程序以实施前面任一项所述的多关节机械臂的控制方法。

根据本申请实施例中的再一方面，提供了一种计算机存储介质，其中，所述计算机存储介质存储有计算机指令，所述计算机指令用于使计算机执行根据前面任一项所述的多关节机械臂的控制方法。

根据本申请实施例中的再一方面，提供了一种计算机程序产品，其中，包括计算机程序，其中，所述计算机程序在被处理器执行时实现前面任一项所述的多关节机械臂的控制方法。

综合以上内容，本申请实施例中的多关节机械臂的控制方案，由于能够确定第一约束点在预设的第一坐标系下的第一坐标，其中，第一约束点用于表征多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点；再根据第一坐标，确定第一坐标系与基于执行机构的末端的所在位置建立的第二坐标系之间的偏差信息；之后基于偏差信息，建立运动学模型，其中，运动学模型用于指示执行机构在执行操作任务时执行机构的末端的第一运动信息与多关节机械臂的各个关节的第二运动信息之间的关系；再根据运动学模型，确定多关节机械臂的各个关节的关节转动角度；最后基于各个关节的关节转动角度，分别控制各个关节的转动。基于此，本方案能够在引入第一约束点的情况下通过软件实现对多关节机械臂的各个关节的转动进行精确控制，使得多关节机械臂的执行机构能够在执行操作任务的任一时刻都经过该第一约束点，从而在该第一约束点为微创手术创口的一个点时，能够实现多关节机械臂的执行机构围绕创口运动的功能，保障微创手术过程中医生利用多关节机械臂的操作执行机构时不会拉扯创口，避免对患者造成二次伤害，提高手术安全性，因而本申请的这种技术方案，无需采用额外的机械的运动机构来实现多关节机械臂的执行机构围绕创口运动，从而避免了机械机构约束较大、使用场景受限、影响机械臂任务空间等问题的出现，也不会额外地增加多关节机械臂的结构的成本。

附图说明

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

图1示出了根据本申请的一种示例性的多关节机械臂的示意图。

图2示出了根据本申请的一种示例性的多关节机械臂的控制方法的流程图。

图3示出了根据本申请的第一坐标系和第一约束点的示意图。

图4示出了根据本申请的步骤S104的一个可选的子流程图。

图5示出了根据本申请的步骤S1041的一个可选的子流程图。

图6示出了根据本申请的一个示例性的旋转轴向量的示意图。

图7示出了根据本申请的一种示例性的多关节机械臂的控制装置的框图。

图8示出了根据本申请的一种示例性的电子设备的框图。

具体实施方式

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

本申请实施例中提供一种多关节机械臂的控制方法，该方法可以由多关节机械臂的控制装置执行，该多关节机械臂的控制装置可以包括至少一个能够用于进行数据处理的处理器，例如CPU、MCU、DSP、FPGA等，或者也可以是能够进行数据处理的计算机设备、服务器等，在此不进行限制。

图1示出了根据本申请的一种示例性的多关节机械臂的示意图。参照图1所示，该多关节机械臂10包括执行机构11和多个关节12，所述多个关节12依次连接，执行机构11连接在依次连接的多个关节12的末端。具体地，该各个关节12都可以绕其自身的转动轴转动，当各个关节12以特定的转动角度转动时，可以带动执行机构11的末端调整到不同的位置和姿态(即位姿)。可选地，参照图1所示，各个关节12通过多个连接臂形成依次连接，依次连接的多个关节12的首端的关节12连接在基座13上，该基座13可以是多关节机械臂10的底座或者是该多关节机械臂10所处的机器人的底座，在此不进行限制。

本申请中不限制多关节机械臂10的关节12的数量，例如可以是6关节机械臂或者7关节机械臂。其中6关节机械臂共有6轴，6自由度；7关节机械臂共有7轴，7自由度。参照图1所示的示例，该多关节机械臂10为6关节机械臂。为便于说明本申请的技术方案，均以图1中的多关节机械臂10为6关节机械臂进行说明。

参照图1所示，本申请中的执行机构11至少部分可以呈竖长针状，当然这仅作为示例，并不作为对本申请的限制。为便于说明本申请的技术方案，均以图1中的执行机构11至少部分呈竖长针状为例进行说明。可选地，以多关节机械臂10用于神经内镜微创手术为例，该执行机构11可以是执行神经内镜微创手术操作的探针，其末端可以固定设置包括但不限于内镜等结构。

图2示出了根据本申请的一种示例性的多关节机械臂的控制方法的流程图。参照图2示出的流程图，该多关节机械臂的控制方法包括如下步骤S101、S102、S103、S104、S105，具体地：

步骤S101：确定第一约束点在预设的第一坐标系下的第一坐标，其中，该第一约束点用于表征多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点。

示例地，以多关节机械臂10用于神经内镜微创手术为例，执行机构11执行操作任务，可以是指执行机构11伸入创口内执行手术操作。

可选地，第一坐标系P1为三维x-y-z坐标系，被预先建立并作为参考坐标系。

可选地，第一约束点A可以依据需要被预先设定。示例地，以多关节机械臂10用于神经内镜微创手术为例，该第一约束点A在手术前进行设定，例如可以设定为患者手术所开的创口上的其中一个点。参照图1所示，在设定了第一约束点A后，该多关节机械臂10的各个关节12转动以带动执行机构11动作以执行操作任务的任一时刻，执行机构11始终经过该第一约束点A，因此第一约束点A对执行机构11的操作任务形成约束作用。本实施例中，在多关节机械臂10的执行机构11的末端(例如可以是神经内镜的末端)的运动被第一约束点A约束的同时，执行机构11的末端也能够具备良好的运动各向同性，提高执行机构11的末端的运动灵活性。

参照图1进一步理解，其中实线部分的执行机构11示出了在执行操作任务的一个时刻的执行机构11的位置的示意，各虚线部分示出了执行操作任务的其他一些时刻的执行机构11的位置的示意，在执行操作任务的任一时刻执行机构11均经过该第一约束点A。应理解，图1并不作为对本申请实施例中的任何限制。

在预设了第一坐标系P1，并指定了第一约束点A后，则第一约束点A在第一坐标系P1下的坐标即可被唯一地确定。

图3示出了根据本申请的第一坐标系和第一约束点的示意图。可选地，本申请中可以按需要设定第一坐标系P1的原点的位置，在一些实施例中也可以将第一坐标系P1的原点设定到与第一约束点A相重合。为了便于说明本实施例，参照图3所示，本申请的下文中均以第一坐标系P1的原点与第一约束点A不重合为例进行说明。

步骤S102：根据第一坐标，确定第一坐标系与基于执行机构的末端的所在位置建立的第二坐标系之间的偏差信息。

可选地，执行机构11的末端可以是指执行机构11上的与关节12的末端的连接位置距离最远的点。

可选地，第二坐标系P2为三维x-y-z坐标系。可选地，参照图3所示，本申请实施例中，第二坐标系P2以执行机构11的末端的所在位置点B为原点、以过第一约束点A和执行机构11的末端的所在位置点的直线为z轴建立。这样的第二坐标系P2的z轴方向可以代表执行机构11整体方向。本申请通过这样的方式建立第二坐标系P2，便于后续确定执行机构11的末端的位姿信息。

示例地，参照图3所示，若记第一约束点A在第一坐标系P1下的第一坐标为(x_rcm，y_rcm，z_rcm)，执行机构11的末端的所在位置点B的在第一坐标系P1下的第二坐标为(x_ee，y_ee，z_ee)，则以(x_ee，y_ee，z_ee)为第二坐标系P2的原点，而以过(x_rcm，y_rcm，z_rcm)和(x_ee，y_ee，z_ee)的直线作为第二坐标系P2的z轴，以建立第二坐标系P2。需要说明的是，这并不作为对本申请的任何限制。

本申请不限制步骤S102的具体实现，在一些可选的实施例中，步骤S102具体包括：根据第一坐标，确定第一坐标与第一坐标系P1的原点坐标在沿第一坐标系P1的x轴方向的第一偏差e_x、以及第一坐标与第一坐标系P1的原点坐标在沿第一坐标系P1的y轴方向的第二偏差e_y，将第一偏差和第二偏差作为偏差信息。当第一坐标系P1和第一约束点A被确定后，第一偏差e_x和第二偏差e_y也即被确定。本申请通过这样确定偏差信息，便于后续步骤中建立多关节机械臂10的运动学方程，以便于后续实现对多关节机械臂10的各个关节12的转动进行控制。

步骤S103：基于偏差信息，建立运动学模型，其中，运动学模型用于指示执行机构在执行操作任务时执行机构的末端的第一运动信息与多关节机械臂的各个关节的第二运动信息之间的关系。

在一些可选的实施例中，第一运动信息包括：执行机构11在执行操作任务时，执行机构11的末端分别沿第一坐标系P1的x轴方向、y轴方向、z轴方向平移的三个线速度分量信息，执行机构11的末端分别以第一坐标系P1的x轴方向、y轴方向、z轴方向为轴旋转的三个角速度分量信息；第二运动信息包括多关节机械臂10的各个关节12的角速度信息；步骤S103具体包括：根据第一偏差、第二偏差以及执行机构11的末端的所在位置点B在第一坐标系P1下的第二坐标，确定多关节机械臂的雅可比矩阵，并基于雅可比矩阵，建立运动学模型，其中，雅可比矩阵用于将第一运动信息变换到第二运动信息。

可选地，本申请中多关节机械臂10的运动学模型，可以表示如下运动学方程：

其中，

用于表征第一运动信息，包括执行机构11的末端分别沿第一坐标系P1的x轴方向、y轴方向、z轴方向平移的三个线速度分量信息v_x、v_y、v_z，执行机构11的末端分别以第一坐标系P1的x轴方向、y轴方向、z轴方向为轴旋转的三个角速度分量信息ω_x、ω_y、ω_z。(具体地，

表示对X求偏导，X用于表征执行机构11的末端的位姿信息，如下文所述，可包括三个位移信息和三个角度信息。)

用于表征该多关节机械臂10的运动学方程中引入了第一约束点A作为约束条件。

用于表征第二运动信息，包括多关节机械臂10的各个关节12的角速度信息

n为多关节机械臂10的关节12的个数。(具体地，

表示对θ求偏导，θ用于表征多关节机械臂10的各个关节的关节转动角度，即θ₁、θ₂、...、θ_n，n为多关节机械臂10的关节12的个数。)

J_a用于表征该多关节机械臂10的雅可比矩阵，对于多关节机械臂10为6关节机械臂的情形为例，雅可比矩阵J_a可以表示为：

其中，x_ee，y_ee，z_ee基于第二坐标确定，e_x用于表征第一偏差，e_y用于表征第二偏差。应理解，若多关节机械臂10不是6关节机械臂，则上式中的(θ₁，θ₂，θ₃，θ₄，θ₅，θ₆)，更改为(θ₁，θ₂，...，θ_n)，n为多关节机械臂10的关节12的个数，其余情况同理。

由此，以本申请中的多关节机械臂10为6关节机械臂为例，则多关节机械臂10的运动学模型可以表示为如下运动学方程：

因此本申请中，在后续步骤中可以通过该运动学模型来确定多关节机械臂10的各个关节12的关节转动角度，从而便于基于各个关节12的关节转动角度，分别控制各个关节12的转动，以实现对多关节机械臂10的控制。

步骤S104：根据运动学模型，确定多关节机械臂的各个关节的关节转动角度。

本申请中通过步骤S104，根据运动学模型，对多关节机械臂10求运动学逆解，得到多关节机械臂10的各个关节12的关节转动角度的结果。

在一些可选的实施例中，参照图4所示的流程图，步骤S104具体包括步骤S1041和S1042。

步骤S1041：确定在第一坐标系下的执行机构的末端的位姿信息。

具体地，该执行机构11的末端的位姿信息可以包括执行机构11的末端在第一坐标系P1下的位置信息和姿态信息，由于本申请的第二坐标系P2以执行机构11的末端的所在位置点B为原点、以过第一约束点A和执行机构11的末端的所在位置点B的直线为z轴建立，则位置信息可以表征为第一坐标系P1的原点沿x轴、y轴、z轴经平移变换到第二坐标系P2的原点的三个位移信息，姿态信息可以表征为第一坐标系P1的x轴、y轴、z轴经旋转变换到第二坐标系P2的x轴、y轴、z轴的三个角度信息。

在一些可选的实施例中，参照图5所示的流程图，该步骤S1041具体包括步骤S501～S505：

步骤S501：确定执行机构的末端的所在位置点在第一坐标系下的第二坐标。

具体地，执行机构11的末端的所在位置点B的第二坐标在第一坐标P1下唯一确定，可以直接获取，例如第二坐标记为(x_ee，y_ee，z_ee)。

步骤S502：根据第一坐标和第二坐标，确定用于指示第二坐标系的z轴方向的第二z轴向量。

在一些可选的实施例中，以如下公式确定用于指示第二坐标系的z轴方向的第二z轴向量：

z₂＝norm((x_ee-x_rcm)，(y_ee-y_rcm)，(z_ee-z_rcm))

其中，z₂用于表征所述第二z轴向量；x_rcm、y_rcm、z_rcm基于第一坐标(x_rcm，y_rcm，z_rcm)确定；x_ee、y_ee、z_ee基于第二坐标(x_ee，y_ee，z_ee)确定。

步骤S503：根据第二z轴向量以及用于指示第一坐标系的z轴方向的第一z轴向量，确定执行机构的末端从第二坐标系旋转变换到第一坐标系的旋转轴向量以及旋转角度，其中，旋转轴向量用于指示该旋转变换的旋转轴的方向。

具体地，执行机构11的末端从第二坐标系P2旋转变换到第一坐标系P1，可以对第二坐标系P2的各坐标轴做旋转变换到第一坐标系P1的各坐标轴。可选地，第一z轴向量可以取第一坐标系P1的z轴单位方向向量进行计算，例如，第一z轴向量z₁＝(0，0，1)。

在一些可选的实施例中，步骤S503具体包括：

以公式V＝z₁×z₂计算旋转轴向量，其中，V用于表征旋转轴向量，z₁用于表征第一z轴向量，z₂用于表征第二z轴向量；

以公式θ_V＝arccos(z₁·z₂)计算旋转角度，其中，θ_V用于表征旋转角度。

本申请中通过这种方式可以准确地计算用于指示第二坐标系的z轴方向的第二z轴向量z₂、旋转轴向量V和旋转角度θ_V，以便于进行后续的数据处理。

便于理解地，参照图6所示，其中示出了第一z轴向量z₁、第二z轴向量z₂、旋转轴向量V的示意，第二z轴向量z₂在以旋转轴向量V为旋转轴进行旋转，旋转角度达到θ_V时，第二z轴向量z₂旋转变换到第一z轴向量z₁，并同步实现执行机构11的末端从第二坐标系P2旋转变换到第一坐标系P1的过程。应理解，图6的示例并不作为对本申请实施例的任何限制。

步骤S504：根据旋转轴向量和旋转角度，确定用于指示第二坐标系的x轴方向的第二x轴向量以及用于指示第二坐标系的y轴方向的第二y轴向量。

在一些可选的实施例中，步骤S504具体包括：

以公式x₂＝Rx₁计算第二x轴向量，其中，x₂用于表征第二x轴向量，x₁用于表征用于指示第一坐标系的x轴方向的第一x轴向量；

以公式少₂＝Ry₁计算第二y轴向量，其中，y₂用于表征第二y轴向量，y₁用于表征用于指示第一坐标系的y轴方向的第一y轴向量；

其中，R＝cos(θ_V)+(1-cos(θ_V))VV^T+sinθ_V Skew(V)

其中，R用于表征旋转变换矩阵，θ_V用于表征旋转角度，V用于表征旋转轴向量，Skew(V)用于表征V的反对称矩阵。

可选地，第一x轴向量x₁可以取第一坐标系P1的x轴单位方向向量进行计算，例如，第一x轴向量x₁＝(1，0，0)。可选地，第一y轴向量y₁可以取第一坐标系P1的y轴单位方向向量进行计算，例如，第一y轴向量y₁＝(0，1，0)。

本申请通过上述方式可以准确计算出用于指示第二坐标系的x轴方向的第二x轴向量x₂以及用于指示第二坐标系的y轴方向的第二y轴向量y₂，此外结合前述已经计算得到的用于指示第二坐标系的z轴方向的第二z轴向量z₂，至此，在存在第一约束点A约束状态下的执行机构11的末端姿态被完全确定。

步骤S505：根据第二x轴向量、第二y轴向量、第二z轴向量和第二坐标，确定在第一坐标系下的执行机构的末端的位姿信息。

可以通过第二x轴向量x₂以及第一x轴向量x₁计算向量夹角以得到夹角θ_x、通过第二y轴向量y₂以及第一y轴向量y₁计算向量夹角以得到夹角θ_y、通过第二z轴向量z₂以及第一z轴向量z₁计算向量夹角以得到夹角θ_z。将三个角度信息θ_x、θ_y、θ_z作为在第一坐标系P1下的执行机构11的末端的位姿信息中的姿态信息。

第二坐标为执行机构11的末端在第一坐标系P1下的坐标，可以通过第二坐标(x_ee，y_ee，z_ee)和第一坐标系P1的原点坐标，分别确定第一坐标系P1的原点沿x轴、y轴、z轴经平移变换到第二坐标系P2的原点(第二坐标)的三个位移l_x、l_y、l_z。将三个位移信息l_x、l_y、l_z作为在第一坐标系P1下的执行机构11的末端的位姿信息中的位置信息。

基于此，得到了位置信息和姿态信息，故而在第一坐标系P1下的执行机构11的末端的位姿信息X被确定。本申请中通过上述步骤S501～S505的方式计算执行机构11的末端的位姿信息X，可以使得计算更准确。

步骤S1042：基于位姿信息和运动学模型，确定多关节机械臂的各个关节的关节转动角度。

具体地，可以将执行机构11的末端的位姿信息X利用到运动学模型的运动学方程中计算，对多关节机械臂10求运动学逆解，从而得到多关节机械臂10的各个关节12的关节转动角度，例如，多关节机械臂10是6关节机械臂，则求出前文中的各个关节转动角度θ₁，θ₂，θ₃，θ₄，θ₅，θ₆。

本申请通过上述步骤S1041～S1042的方式，可以对多关节机械臂10求运动学逆解，更准确地得到多关节机械臂的各个关节的关节转动角度的结果。

步骤S105：基于各个关节的关节转动角度，分别控制各个关节的转动。

在得到了各个关节12的关节转动角度后，也就完成了对多关节机械臂10的进行运动学逆解的过程，因此可以通过该各个关节12的关节转动角度分别控制各个关节12的转动，以实现对多关节机械臂10的精准控制。通过本实施例的方案的实时地计算，可以实现对多关节机械臂10的各个关节12的转动进行实时控制，可以使得执行机构10在执行操作任务的任意时刻都经过第一约束点。

基于此，本申请实施例中的多关节机械臂的控制方法，由于能够确定第一约束点在预设的第一坐标系下的第一坐标，其中，第一约束点用于表征多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点；再根据第一坐标，确定第一坐标系与基于执行机构的末端的所在位置建立的第二坐标系之间的偏差信息；之后基于偏差信息，建立运动学模型，其中，运动学模型用于指示执行机构在执行操作任务时执行机构的末端的第一运动信息与多关节机械臂的各个关节的第二运动信息之间的关系；再根据运动学模型，确定多关节机械臂的各个关节的关节转动角度；最后基于各个关节的关节转动角度，分别控制各个关节的转动。基于此，本方法能够在引入第一约束点的情况下通过软件实现对多关节机械臂的各个关节的转动进行精确控制，使得多关节机械臂的执行机构能够在执行操作任务的任一时刻都经过该第一约束点，从而在该第一约束点为微创手术创口的一个点时，能够实现多关节机械臂的执行机构围绕创口运动的功能，保障微创手术过程中医生利用多关节机械臂的操作执行机构时不会拉扯创口，避免对患者造成二次伤害，提高手术安全性，因而本申请的这种技术方案，无需采用额外的机械的运动机构来实现多关节机械臂的执行机构围绕创口运动，从而避免了机械机构约束较大、使用场景受限、影响机械臂任务空间等问题的出现，也不会额外地增加多关节机械臂的结构的成本。

可以理解的是，上述实验例仅作为本申请实施例中的一些示例的实施例，而非对本申请实施例中的多关节机械臂的控制方案的任何限制。

基于与前述的多关节机械臂的控制方法相同的发明构思，根据本申请实施中的另一方面，参照图7的框图，提供了一种多关节机械臂的控制装置100，所述多关节机械臂10包括执行机构11和多个关节12，所述多个关节12依次连接，所述执行机构11连接在依次连接的多个关节12的末端，其包括：

第一确定模块101，用于确定第一约束点在预设的第一坐标系下的第一坐标，其中，所述第一约束点用于表征所述多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点；

第二确定模块102，用于根据所述第一坐标，确定所述第一坐标系与基于所述执行机构的末端的所在位置建立的第二坐标系之间的偏差信息；

模型建立模块103，用于基于所述偏差信息，建立运动学模型，其中，所述运动学模型用于指示所述执行机构在执行操作任务时所述执行机构的末端的第一运动信息与所述多关节机械臂的各个关节的第二运动信息之间的关系；

第三确定模块104，用于根据所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度；

关节控制模块105，用于基于所述各个关节的关节转动角度，分别控制所述各个关节的转动。

在一些可选的实施例中，所述第三确定模块104具体用于：确定在所述第一坐标系下的所述执行机构的末端的位姿信息；基于所述位姿信息和所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度。

在一些可选的实施例中，所述第二坐标系以所述执行机构的末端的所在位置点为原点、以过所述第一约束点和所述执行机构的末端的所在位置点的直线为z轴建立。

在一些可选的实施例中，所述第三确定模块104具体用于：确定所述执行机构的末端的所在位置点在所述第一坐标系下的第二坐标；根据所述第一坐标和所述第二坐标，确定用于指示所述第二坐标系的z轴方向的第二z轴向量；根据所述第二z轴向量以及用于指示所述第一坐标系的z轴方向的第一z轴向量，确定所述执行机构的末端从所述第二坐标系旋转变换到所述第一坐标系的旋转轴向量以及旋转角度，其中，所述旋转轴向量用于指示该旋转变换的旋转轴的方向；根据所述旋转轴向量和所述旋转角度，确定用于指示所述第二坐标系的x轴方向的第二x轴向量以及用于指示所述第二坐标系的y轴方向的第二y轴向量；根据所述第二x轴向量、第二y轴向量、第二z轴向量和所述第二坐标，确定在所述第一坐标系下的所述执行机构的末端的位姿信息。

在一些可选的实施例中，所述第三确定模块104具体用于：

以公式V＝z₁×z₂计算所述旋转轴向量，其中，V用于表征所述旋转轴向量，z₁用于表征所述第一z轴向量，z₂用于表征所述第二z轴向量；

以公式θ_V＝arccos(z₁·z₂)计算所述旋转角度，其中，θ_V用于表征所述旋转角度。

在一些可选的实施例中，所述第三确定模块104具体用于：

以公式x₂＝Rx₁计算所述第二x轴向量，其中，x₂用于表征所述第二x轴向量，x₁用于表征用于指示所述第一坐标系的x轴方向的第一x轴向量；

以公式y₂＝Ry₁计算所述第二y轴向量，其中，y₂用于表征所述第二y轴向量，y₁用于表征用于指示所述第一坐标系的y轴方向的第一y轴向量；

其中，R＝cos(θ_V)+(1-cos(θ_V))VV^T+sinθ_VSkew(V)

其中，R用于表征旋转变换矩阵，θ_V用于表征所述旋转角度，V用于表征所述旋转轴向量，Skew(V)用于表征V的反对称矩阵。

在一些可选的实施例中，所述第二确定模块102具体用于：根据所述第一坐标，确定所述第一坐标与所述第一坐标系的原点坐标在沿所述第一坐标系的x轴方向的第一偏差、以及所述第一坐标与所述第一坐标系的原点坐标在沿所述第一坐标系的y轴方向的第二偏差，将所述第一偏差和所述第二偏差作为所述偏差信息。

在一些可选的实施例中，所述第一运动信息包括：所述执行机构在执行操作任务时，所述执行机构的末端分别沿所述第一坐标系的x轴方向、y轴方向、z轴方向平移的三个线速度分量信息，所述执行机构的末端分别以所述第一坐标系的x轴方向、y轴方向、z轴方向为轴旋转的三个角速度分量信息；所述第二运动信息包括所述多关节机械臂的各个关节的角速度信息；所述模型建立模块104具体用于：根据所述第一偏差、所述第二偏差以及所述执行机构的末端的所在位置点在所述第一坐标系下的第二坐标，确定所述多关节机械臂的雅可比矩阵，并基于所述雅可比矩阵，建立所述运动学模型，其中，所述雅可比矩阵用于将所述第一运动信息变换到所述第二运动信息。

本申请实施例中的多关节机械臂的控制装置100，与前述各实施例中的多关节机械臂的控制方法相对应，该多关节机械臂的控制装置100的相关内容可参照上述的多关节机械臂的控制方法进行理解，在此不再进行赘述。

本申请实施例中的多关节机械臂的控制装置100，由于其第一确定模块101能够确定第一约束点在预设的第一坐标系下的第一坐标，其中，第一约束点用于表征多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点；其第二确定模块102能够根据第一坐标，确定第一坐标系与基于执行机构的末端的所在位置建立的第二坐标系之间的偏差信息；之后模型建立模块103能够基于偏差信息，建立运动学模型，其中，运动学模型用于指示执行机构在执行操作任务时执行机构的末端的第一运动信息与多关节机械臂的各个关节的第二运动信息之间的关系；第三确定模块104能够再根据运动学模型，确定多关节机械臂的各个关节的关节转动角度；最后关节控制模块105能够基于各个关节的关节转动角度，分别控制各个关节的转动。基于此，本装置能够在引入第一约束点的情况下通过软件实现对多关节机械臂的各个关节的转动进行精确控制，使得多关节机械臂的执行机构能够在执行操作任务的任一时刻都经过该第一约束点，从而在该第一约束点为微创手术创口的一个点时，能够实现多关节机械臂的执行机构围绕创口运动的功能，保障微创手术过程中医生利用多关节机械臂的操作执行机构时不会拉扯创口，避免对患者造成二次伤害，提高手术安全性，因而本申请的这种技术方案，无需采用额外的机械的运动机构来实现多关节机械臂的执行机构围绕创口运动，从而避免了机械机构约束较大、使用场景受限、影响机械臂任务空间等问题的出现，也不会额外地增加多关节机械臂的结构的成本。

根据本申请实施例中的再一方面，参照图8中的框图，本申请实施例提供了一种电子设备800。如图8所示，所述电子设备800包括存储器801以及处理器802，所述存储器801上用于存储计算机可执行程序，所述处理器802用于运行所述计算机可执行程序以实施上述实施例的任一项所述的多关节机械臂的控制方法。

根据本申请实施例中的再一方面，提供了一种计算机存储介质，其中，所述计算机存储介质存储有计算机指令，所述计算机指令用于使计算机执行根据前面任一项所述的多关节机械臂的控制方法。

根据本申请实施例中的再一方面，提供了一种计算机程序产品，其中，包括计算机程序，其中，所述计算机程序在被处理器执行时实现前面任一项所述的多关节机械臂的控制方法。

对于多关节机械臂的控制装置、电子设备、计算机存储介质、计算机程序产品实施例而言，由于其基本相似于上述多关节机械臂的控制方法实施例，所以描述的比较简单，相关之处参见上述多关节机械臂的控制方法实施例的部分说明即可，在此不再进行赘述。

可以理解的是，上述实施例仅作为本申请实施例中的一些示例的实验例，而非对本申请实施例中的多关节机械臂的控制方法、装置、电子设备、计算机存储介质、计算机程序产品的任何限制。

需要说明的是，本文使用的术语“包括”及其变形是开放性包括，即“包括但不限于”。术语“基于”是“至少部分地基于”。术语“一个实施例”表示“至少一个实施例”；术语“另一实施例”表示“至少一个另外的实施例”；术语“一些实施例”表示“至少一些实施例”。其他术语的相关定义将在下文描述中给出。需要注意，本申请中提及的“第一”、“第二”等概念仅用于对不同的装置、模块或单元进行区分，并非用于限定这些装置、模块或单元所执行的功能的顺序或者相互依存关系。

需要注意，本申请中提及的“一个”、“多个”的修饰是示意性而非限制性的，本领域技术人员应当理解，除非在上下文另有明确指出，否则应该理解为“一个或多个”。

最后应说明的是：以上实施例仅用以说明本申请实施例的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。

Claims (12)

1.一种多关节机械臂的控制方法，所述多关节机械臂包括执行机构和多个关节，所述多个关节依次连接，所述执行机构连接在依次连接的多个关节的末端，其特征在于，所述方法包括：

确定第一约束点在预设的第一坐标系下的第一坐标，其中，所述第一约束点用于表征所述多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点；

根据所述第一坐标，确定所述第一坐标系与基于所述执行机构的末端的所在位置建立的第二坐标系之间的偏差信息；

基于所述偏差信息，建立运动学模型，其中，所述运动学模型用于指示所述执行机构在执行操作任务时所述执行机构的末端的第一运动信息与所述多关节机械臂的各个关节的第二运动信息之间的关系；

根据所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度；

基于所述各个关节的关节转动角度，分别控制所述各个关节的转动。

2.根据权利要求1所述的方法，其特征在于，所述根据所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度，包括：

确定在所述第一坐标系下的所述执行机构的末端的位姿信息；

基于所述位姿信息和所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度。

3.根据权利要求2所述的方法，其特征在于，所述第二坐标系以所述执行机构的末端的所在位置点为原点、以过所述第一约束点和所述执行机构的末端的所在位置点的直线为z轴建立。

4.根据权利要求3所述的方法，其特征在于，所述确定在所述第一坐标系下的所述执行机构的末端的位姿信息，包括：

确定所述执行机构的末端的所在位置点在所述第一坐标系下的第二坐标；

根据所述第一坐标和所述第二坐标，确定用于指示所述第二坐标系的z轴方向的第二z轴向量；

根据所述第二z轴向量以及用于指示所述第一坐标系的z轴方向的第一z轴向量，确定所述执行机构的末端从所述第二坐标系旋转变换到所述第一坐标系的旋转轴向量以及旋转角度，其中，所述旋转轴向量用于指示该旋转变换的旋转轴的方向；

根据所述旋转轴向量和所述旋转角度，确定用于指示所述第二坐标系的x轴方向的第二x轴向量以及用于指示所述第二坐标系的y轴方向的第二y轴向量；

根据所述第二x轴向量、第二y轴向量、第二z轴向量和所述第二坐标，确定在所述第一坐标系下的所述执行机构的末端的位姿信息。

5.根据权利要求4所述的方法，其特征在于，所述根据所述第一向量以及用于指示所述第一坐标系的z轴的方向的第二z轴向量，确定所述执行机构的末端从所述第二坐标系旋转变换到所述第一坐标系的旋转轴向量以及旋转角度，包括：

以公式V＝z₁×z₂计算所述旋转轴向量，其中，V用于表征所述旋转轴向量，z₁用于表征所述第一z轴向量，z₂用于表征所述第二z轴向量；

以公式θ_V＝arccos(z₁·z₂)计算所述旋转角度，其中，θ_V用于表征所述旋转角度。

6.根据权利要求5所述的方法，其特征在于，所述根据所述旋转轴向量和所述旋转角度，确定用于指示所述第二坐标系的x轴方向的第二x轴向量以及用于指示所述第二坐标系的y轴方向的第二y轴向量，包括：

以公式x₂＝Rx₁计算所述第二x轴向量，其中，x₂用于表征所述第二x轴向量，x₁用于表征用于指示所述第一坐标系的x轴方向的第一x轴向量；

以公式y₂＝Ry₁计算所述第二y轴向量，其中，y₂用于表征所述第二y轴向量，y₁用于表征用于指示所述第一坐标系的y轴方向的第一y轴向量；

其中，R＝cos(_V)+(1-cos(_V))VT+sinθ_VSkew(V)

其中，R用于表征旋转变换矩阵，θ_V用于表征所述旋转角度，V用于表征所述旋转轴向量，Skew(V)用于表征V的反对称矩阵。

7.根据权利要求1-6中任一项所述的方法，其特征在于，所述根据所述第一坐标，确定所述第一坐标系与基于所述执行机构的末端的所在位置建立的第二坐标系之间的偏差信息，包括：

根据所述第一坐标，确定所述第一坐标与所述第一坐标系的原点坐标在沿所述第一坐标系的x轴方向的第一偏差、以及所述第一坐标与所述第一坐标系的原点坐标在沿所述第一坐标系的y轴方向的第二偏差，将所述第一偏差和所述第二偏差作为所述偏差信息。

8.根据权利要求7所述的方法，其特征在于，所述第一运动信息包括：所述执行机构在执行操作任务时，所述执行机构的末端分别沿所述第一坐标系的x轴方向、y轴方向、z轴方向平移的三个线速度分量信息，所述执行机构的末端分别以所述第一坐标系的x轴方向、y轴方向、z轴方向为轴旋转的三个角速度分量信息；所述第二运动信息包括所述多关节机械臂的各个关节的角速度信息；

所述基于所述偏差信息，建立运动学模型，包括：

根据所述第一偏差、所述第二偏差以及所述执行机构的末端的所在位置点在所述第一坐标系下的第二坐标，确定所述多关节机械臂的雅可比矩阵，并基于所述雅可比矩阵，建立所述运动学模型，其中，所述雅可比矩阵用于将所述第一运动信息变换到所述第二运动信息。

9.一种多关节机械臂的控制装置，所述多关节机械臂包括执行机构和多个关节，所述多个关节依次连接，所述执行机构连接在依次连接的多个关节的末端，其特征在于，所述装置包括：

第一确定模块，用于确定第一约束点在预设的第一坐标系下的第一坐标，其中，所述第一约束点用于表征所述多关节机械臂的执行机构在执行操作任务的任一时刻需经过的空间点；

第二确定模块，用于根据所述第一坐标，确定所述第一坐标系与基于所述执行机构的末端的所在位置建立的第二坐标系之间的偏差信息；

模型建立模块，用于基于所述偏差信息，建立运动学模型，其中，所述运动学模型用于指示所述执行机构在执行操作任务时所述执行机构的末端的第一运动信息与所述多关节机械臂的各个关节的第二运动信息之间的关系；

第三确定模块，用于根据所述运动学模型，确定所述多关节机械臂的各个关节的关节转动角度；

关节控制模块，用于基于所述各个关节的关节转动角度，分别控制所述各个关节的转动。

10.一种电子设备，其特征在于，所述电子设备包括存储器以及处理器，所述存储器上用于存储计算机可执行程序，所述处理器用于运行所述计算机可执行程序以实施权利要求1-8中任一项所述的方法。

11.一种计算机存储介质，其特征在于，所述计算机存储介质存储有计算机指令，所述计算机指令用于使计算机执行根据权利要求1-8中任一项所述的方法。

12.一种计算机程序产品，其特征在于，包括计算机程序，其中，所述计算机程序在被处理器执行时实现权利要求1-8中任一项所述的方法。

Images