CN112975954B - 机械臂的控制方法、计算机设备和存储介质 - Google Patents

Abstract

本申请涉及一种机械臂的控制方法、计算机设备和存储介质。所述方法通过获取工具器械的位姿和转动角度，并根据工具器械的位姿和转动角度确定机械臂上各关节的关节值，然后采用机械臂正运动学求解方法，根据机械臂上各关节的关节值确定机械臂的目标关节值。上述方法通过在机械臂末端设置工具器械绕轴线的转动角度，将原来5自由度的机械臂转换成6自由度的机械臂，避免了因缺少一个自由度而导致无法准确获取机械臂的末端位姿的问题。同时，运用工具器械绕轴线的转动角度可得到各关节的关节值的解析解，进而提高了机械臂的控制准确性与高效性。

Description

机械臂的控制方法、计算机设备和存储介质

技术领域

本申请涉及医疗机器人技术领域，特别是涉及一种机械臂的控制方法、计算机设备和存储介质。

背景技术

随着机器人技术的飞速发展，机器人在医疗领域得到了广泛的应用，主要应用于外科手术、辅助医疗诊断等，因此医疗机器人的控制显得格外的重要。

目前，对于医疗机器人的控制主要是对5个自由度医疗机械臂进行控制，且在控制过程中，其构型不存在解析解，采用数值迭代算法，根据医疗机械臂末端的指令位姿求解5元方程组，计算医疗机械臂各关节角度值，以控制医疗机械臂按照指令位姿进行相应的手术操作或定位操作。

但是，上述控制方法存在控制效率低，不利于实际手术操作的应用的问题。

发明内容

基于此，有必要针对上述技术问题，提供一种能够有效提高控制效率的机械臂的控制方法、计算机设备和存储介质。

第一方面，一种机械臂的控制方法，所述方法包括：

获取工具器械的位姿和转动角度；所述工具器械的位姿包括所述工具器械在轴线的方向向量和所述工具器械的位置坐标；

根据所述工具器械的位姿和转动角度确定机械臂上各关节的关节值；

采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值确定所述机械臂的目标关节值。

在其中一个实施例中，所述根据所述工具器械的位姿和转动角度确定机械臂上各关节的关节值，包括：

根据所述工具器械的位姿和转动角度确定所述机械臂上的第一关节至第三关节的关节值；

根据所述第一关节至第三关节的关节值确定所述机械臂上的第四关节和第五关节的关节值。

在其中一个实施例中，所述根据所述工具器械的位姿和转动角度确定所述机械臂上的第一关节至第三关节的关节值，包括：

根据所述工具器械的位姿和转动角度确定所述机械臂上的汇交点的位置坐标；

根据所述汇交点的位置坐标确定所述第一关节至第三关节的关节值。

在其中一个实施例中，所述采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值确定所述机械臂的目标关节值，包括：

采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值计算所述机械臂的当前末端位姿；

判断所述当前末端位姿是否满足目标位姿精度，若满足，则将计算得到的机械臂上各关节的关节值作为控制所述机械臂的目标关节值；若不满足，则确定新的工具器械的转动角度，并返回执行所述获取工具器械的位姿和工具器械的转动角度的步骤，重新确定所述机械臂的当前末端位姿，直至所述当前末端位姿满足所述目标位姿精度为止。

在其中一个实施例中，所述确定新的工具器械的转动角度，包括：

在所述转动角度上增加或减少预设角度，得到所述新的工具器械的转动角度。

在其中一个实施例中，若确定的所述机械臂的目标关节值包括多组各关节的目标关节值，则所述方法还包括：

根据预设的筛选条件，从所述多组各关节的目标关节值中筛选出一组目标关节值。

在其中一个实施例中，所述根据预设的筛选条件，从所述多组各关节的目标关节值中筛选出一组目标关节值，包括：

获取所述机械臂上的各关节在上一时刻的关节值；

将所述多组各关节的目标关节值与所述各关节在上一时刻的关节值进行比较，并从中筛选出变化值最小的一组目标关节值。

在其中一个实施例中，所述方法还包括：

将所述机械臂的目标关节值输入到机械臂控制器中，控制所述机械臂到达目标位姿。

第二方面，一种机械臂的控制装置，所述装置包括：

获取模块，用于获取工具器械的位姿和转动角度；所述工具器械的位姿包括所述工具器械在轴线的方向向量和所述工具器械的位置坐标；

确定模块，用于根据所述工具器械的位姿和转动角度确定机械臂上各关节的关节值；

解算模块，用于采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值确定所述机械臂的目标关节值。

第三方面，一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述第一方面所述的方法。

第四方面，一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述第一方面所述的方法。

上述机械臂的控制方法、装置、计算机设备和存储介质，通过获取工具器械的位姿和转动角度，并根据工具器械的位姿和转动角度确定机械臂上各关节的关节值，然后采用机械臂正运动学求解方法，根据机械臂上各关节的关节值确定机械臂的目标关节值。上述方法通过在机械臂末端设置工具器械绕轴线的转动角度(虚拟关节的转动角度)，将原来5自由度的机械臂转换成6自由度的机械臂，避免了因缺少一个自由度而导致无法准确获取机械臂的末端位姿的问题。同时，运用工具器械绕轴线的转动角度可得到各关节的关节值的解析解，进而提高了机械臂的控制准确性与高效性。

附图说明

图1为一个实施例中机械臂的控制方法的应用系统图；

图2为一个实施例中机械臂的控制方法的流程示意图；

图3为一个实施例中机械臂的结构示意图；

图4为图2实施例中S102的一种实现方式的流程示意图；

图5为图4实施例中S201的一种实现方式的流程示意图；

图6为图2实施例中S103的一种实现方式的流程示意图；

图7为一个实施例中机械臂的控制方法的流程示意图；

图8为图2实施例中S104的一种实现方式的流程示意图；

图9为一个实施例中机械臂的控制方法的流程示意图；

图10为一个实施例中机械臂的控制方法的流程示意图；

图11为一个实施例中机械臂的控制装置的结构框图；

图12为一个实施例中机械臂的控制装置的结构框图；

图13为一个实施例中机械臂的控制装置的结构框图；

图14为一个实施例中机械臂的控制装置的结构框图；

图15为一个实施例中机械臂的控制装置的结构框图；

图16为一个实施例中机械臂的控制装置的结构框图；

图17为一个实施例中机械臂的控制装置的结构框图；

图18为一个实施例中计算机设备的内部结构图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

本申请提供的机械臂的控制方法，可以应用于如图1所示的应用系统中。其中，机械臂102可以通过无线或有线的方式与控制终端104进行通信。机械臂102可以为一种任意的5个自由度串联的医疗机械臂，其中包括：基座O、关节G1、连杆L1、关节G2、连杆L2、关节G3、连杆L3、关节G4、连杆L4、关节G5、连杆L5。该机械臂结构描述：连杆L1通过关节G1装配在基座O上；连杆L2通过关节G2装配在连杆L1上；连杆L3通过关节G3装配在连杆L2 上；连杆L4通过关节G4装配在连杆L3上；连杆L5通过关节G5装配在连杆 L4上；连杆L5一侧的末端可以安装手术需要的工具器械等。在医疗领域，可以使用上述机械臂辅助医生进行手术操作。该系统中的控制终端104可以但不限于是各种个人计算机、笔记本电脑、智能手机、平板电脑和便携式可穿戴设备。

本领域技术人员可以理解，图1中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的应用系统的限定，具体的应用系统可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，如图2所示，提供了一种机械臂的控制方法，以该方法应用于图1中的控制终端为例进行说明，包括以下步骤：

S101，获取工具器械的位姿和转动角度；工具器械的位姿包括工具器械在轴线的方向向量和工具器械的位置坐标。

其中，工具器械在机械臂的末端位置上，工具器械的位姿即指机械臂末端工作指令位姿；工具器械在轴线的方向向量表示工具器械的轴线在基坐标系中的方向向量；工具器械的位置坐标表示工作器械在基坐标系中的三维坐标；工具器械的转动角度为工具器械绕轴线的转动角度。本实施例中，将工具器械视作机械臂中的第六个虚拟关节，即在工具器械所在位置上设置虚拟的一个关节 (以下文中提到的工具器械即为虚拟关节，虚拟关节即为工具器械)。如图3所示，考虑工具器械(图中的G6)的机械臂构型不存在解析解。针对此问题，在工具器械中可以增加一个虚拟轴(图3中的Z1虚线)，将原有的5自由度的机械臂(如图1所示的机械臂)转换为6自由度的机械臂(如图3所示的机械臂)，从而满足医疗控制应用需求。图3中机械臂中各关节之间的相对运动可描述为：连杆L1相对于基座O作垂直方向的移动；连杆L2相对于连杆L1作航向运动；连杆L3相对于连杆L2作航向运动；连杆L4相对于连杆L3作横滚运动；连杆 L5相对于连杆L4作俯仰运动；工具器械G6(第六虚拟关节G6)可绕虚拟轴 Z1进行旋转运动。

本实施例中，控制终端可以在机械臂末端的工具器械上设置一个虚拟轴Z1，该虚拟轴的轴线与工具器械的轴线重合，即垂直于关节G5连杆L5，虚拟轴Z1 上设置虚拟关节G6，虚拟关节G6与工具器械重合，可绕虚拟轴Z1进行旋转运动。控制终端设置一个初始的虚拟关节G6绕轴线Z1的转动角度(即工具器械的转动角度)，最后，控制终端可根据机械臂中各关节的当前位置与初始的虚拟关节G6绕轴线Z1的转动角度，通过机械臂运动学计算得到虚拟关节在轴线 Z1的方向向量和位置坐标，即工具器械的位姿。可选的，控制终端也可以根据机械臂的几何结构参数获取到该机械臂末端上的工具器械的位姿。

S102，根据工具器械的位姿和转动角度确定机械臂上各关节的关节值。

其中，各关节的关节值也即各关节的运动角度，例如，图3中的θ₁、θ₂、θ₃、θ₄、θ₅。本实施例中，当控制终端获取到工具器械的位姿后，可基于数值迭代的方式实现各关节值的求解。例如，传统5自由度关节控制时，由在末端的工具器械的位置坐标与方向向量可得如下5元方程组(1)：

上式中，x_p、y_p与z_p分别为工具器械的位置坐标，n_z1与n_z2为其方向向量对应的X值与Y值，θ₁,θ₂,θ₃,θ₄,θ₅为5个关节的关节值。基于以上5元方程组，对 5个关节的关节值设置迭代初值，运用牛顿迭代等数值方法对5元参数进行数值求解，进而得到各关节的关节值，其计算效率与稳定性依赖设置初始值与迭代算法的收敛性。

本实施例中，控制终端可以不用对多元方程组进行数值迭代求解，先根据工具器械的位姿和转动角度解算汇交点W(图3中的关节G4轴线L4与关节 G5轴线Z2的交点)的坐标，再根据汇交点W的坐标可分别求取机械臂上前3 个关节与后3个关节的关节值(后3个关节包括在工具器械位置上设置的虚拟关节)。例如，控制终端可以采用传统的五元方程组，运用数值迭代方式求解机械臂中的θ₁、θ₂、θ₃、θ₄、θ₅；控制终端还可以先采用所提出的虚拟关节方法，由虚拟关节绕虚拟轴的转动角度工具器械的位姿先求出汇交点的坐标，再由汇交点的坐标分解得到前3个关节的三元方程组。然后，由三元方程组可直接得到θ₁、θ₂、θ₃的解析解，再代入运动学方程可得后两关节的二元方程组。最后，由二元方程组可直接求解机械臂关节值θ₄、θ₅，避免采取数值迭代的方式实现关节值求解直接求解，其本质原因为：3元以上方程组一般无解析解，而三元及其以下方程组存在解析解，解析解可直接得到关节值，避免数值迭代产生的稳定性与效率问题。

S103，采用机械臂正运动学求解方法，根据机械臂上各关节的关节值确定机械臂的目标关节值。

本实施例中，当控制终端基于前述步骤计算得到机械臂上各关节的关节值时，可以进一步的采用机械臂正运动学求解方式，根据机械臂上各关节的关节值计算工具器械的末端位姿，若计算得到的工具器械的末端位姿符合预设精度要求时，则将逆解得到的关节值θ₁、θ₂、θ₃、θ₄、θ₅作为该指令位姿下关节值输入；若该计算得到的工具器械的末端位姿不符合预设精度要求，则重新设置虚拟关节绕虚拟轴线的转动角度(即工具器械绕轴线的转动角度)，由上述步骤重新计算机械臂各关节的关节值θ₁、θ₂、θ₃、θ₄、θ₅，并判断这一组关节值对应的机械臂的末端位姿是否满足预设精度需求。如此反复，直到计算得到的工具器械的末端位姿符合预设精度要求为止。

上述实施例提供的机械臂的控制方法通过获取工具器械的位姿和转动角度，并根据工具器械的位姿和转动角度确定机械臂上各关节的关节值，然后采用机械臂正运动学求解方法，根据机械臂上各关节的关节值确定机械臂的目标关节值。上述方法通过在机械臂末端设置工具器械绕轴线的转动角度(虚拟关节的转动角度)，将原来5自由度的机械臂转换成6自由度的机械臂，避免了因缺少一个自由度而导致无法准确获取机械臂的末端位姿的问题。同时，运用工具器械绕轴线的转动角度可得到各关节的关节值的解析解，进而提高了机械臂的控制准确性与高效性。

在一个实施例中，提供了上述S102的一种实现方式，如图4所示，上述S102 “根据工具器械的位姿确定机械臂上各关节的关节值”，具体包括：

S201，根据工具器械的位姿和转动角度确定机械臂上的第一关节至第三关节的关节值。

其中，第一关节至第三关节为机械臂中的第一关节、第二关节、第三关节，例如，图3中的关节G1、关节G2、关节G3。本实施例中，当控制终端设置了工具器械绕轴线的转动角度时，并获取到工具器械的一个初始的位姿时，即可根据该工具器械的初始的位姿和各关节连杆几何参数进行逆运算，解算得到机械臂上前三个关节的关节值θ₁、θ₂、θ₃。例如，在图3中的机械臂结构中，当控制终端设置了工具器械G6(即虚拟关节G6)和工具器械G6绕轴线的转动角度θ₆，并获取到此时工具器械G6的一个初始位姿(工具器械的坐标和轴线的方向向量)，则可根据工具器械G6的转动角度θ₆、初始位姿和各关节连杆几何参数解算得到关节G1、关节G2、关节G3的运动角度θ₁、θ₂、θ₃。

可选的，如图5所示，上述S201“根据工具器械的位姿和转动角度确定机械臂上的第一关节至第三关节的关节值”可以具体包括步骤：

S301，根据工具器械的位姿和转动角度确定机械臂上的汇交点的位置坐标。

其中，汇交点为机械臂中末端关节的轴线与相邻关节的轴线之间的交点，例如图3中的关节G5的轴线Z2与关节G4的轴线Z3之间的交点W即为汇交点。本实施例中，当控制终端设置了一个初始的工具器械的转动角度，以及获取到工具器械的一个初始的位姿时，可由如下方程组(2)计算得到汇交点W的位置坐标：

上式中，x_p,y_p,z_p表示汇交点的位置坐标，X_p，Y_p，Z_p表示工具器械的位置坐标，n表示工具器械在轴线的方向向量，L表示机械臂关节的连杆参数，可以由机械臂的结构设计参数确定。g_i，g₂，g₃表示映射关系。θ₆表示工具器械绕轴线的转动角度。本实施例中，当控制终端获取到工具器械的位姿和转动角度，以及相关的机械臂关节的连杆参数和映射关系时，即可代入到上述方程组(2) 解算得到汇交点的位置坐标。

S302，根据汇交点的位置坐标确定第一关节至第三关节的关节值。

当控制终端得到汇交点的位置坐标后，由于汇交点位置坐标由前三个关节值θ₁、θ₂、θ₃和各关节的连杆参数决定，即可构建如下方程组(3)：

上式中，l₁为机械臂第i个关节的关节连杆参数，可以由机械臂的结构设计参数确定。对上述方程组(2)进行三角函数运算，即可解算出机械臂中三个关节的关节值的封闭解，例如，解算出图3中的关节G1的关节值θ₁、关节G2的关节值θ₂、关节G3的关节值θ₃。本实施例中，当控制终端获取到汇交点的位置坐标，以及机械臂中第一关节、第二关节、第三关节的连杆参数时，即代入如上方程组(3)中解算得到第一关节至第三关节的关节值。

S202，根据第一关节至第三关节的关节值确定机械臂上的第四关节和第五关节的关节值。

本实施中，当基于前述步骤逆解得到第一关节至第三关节的关节值θ₁、θ₂、θ₃，可将其代入机械臂运动学，可得第四关节的关节值与第五关节的关节值的解析表达式(4)：

式中，Atan₂为反正切函数，R_ab(θ₁,θ₂,θ₃)表示转换矩阵R的第a行第b列； R_cd(θ₁,θ₂,θ₃)表示转换矩阵R的第d行第d列；R_ef(θ₁,θ₂,θ₃)表示转换矩阵R的第e 行第f列；R_gh(θ₁,θ₂,θ₃)表示转换矩阵R的第g行第h列。本实施例中，当控制终端获取到第一关节、第二关节、第三关节的关节值，以及相关转换矩阵的参数时，即可代入如上方程组(4)中解算得到第四关节和第五关节的关节值。

例如图3中，根据关节G1的关节值θ₁、关节G2的关节值θ₂、关节G3的关节值θ₃，推导出关节G4的关节值θ₄和关节G5的关节值θ₅。

上述实施例所述的方法实现了机械臂上各关节的关节值的解算，且该解算方法先通过汇交点坐标的求取，分离出含θ₁、θ₂、θ₃的三元方程组，通过三角变换，可得到机械臂前三个关节的关节值的解析表达式，再进一步的基于前三个关节的关节值得到后两个关节的关节值的解析表达式，进而实现了控制过程中机械臂各关节的关节值的解算，相比于传统运用数值迭代求解五元方程组的方法，极大的简化了计算过程，降低了计算难度，从而提高了机械臂逆解的速度，进而提高了机械臂的控制效率与准确性。

在一个实施例中，提供了上述S103的一种实现方式，如图6所示，上述S103 “采用机械臂正运动学求解方法，根据机械臂上各关节的关节值确定机械臂的目标关节值”，具体包括：

S401，采用机械臂正运动学求解方法，根据机械臂上各关节的关节值计算机械臂的当前末端位姿。

本实施例中，当控制终端基于前述实施例所述的方法计算得到机械臂上各关节的关节值时，可以进一步的采用机械臂正运动学求解方法，根据机械臂上各关节的关节值计算工具器械的当前末端位姿。

S402，判断当前末端位姿是否满足满足目标位姿精度，若满足，则执行步骤S403，若不满足，则执行步骤S404

其中，目标位姿精度为用户期望通过控制机械臂末端达到的位姿精度，可以由控制终端预先确定。本实施例中，当控制终端由上述解算得到的关节值θ₁、θ₂、θ₃、θ₄、θ₅计算得到机械臂的当前末端位姿时，可以进一步的判断当前末端位姿是否满足目标位姿精度，若满足，则说明上述所解算的机械臂各关节的关节值为所需要控制的关节值；若不满足，则说明上述计算的机械臂各关节的关节值不是最终机械臂的目标位姿对应的关节值，还需要重新设置工具器械的转动角度，并根据新的转动角度重新计算得到各关节的关节值，以及工具器械的当前末端位姿。

S403，将计算得到的机械臂上各关节的关节值作为控制机械臂的目标关节值。

本实施例涉及机械臂的当前末端位姿满足预设的目标位姿精度的情况，在此情况下，控制终端直接将当前计算得到的机械臂各关节的关节值作为指令目标位姿所控制的目标关节值，进而实现机械臂的控制。

S404，确定新的工具器械的转动角度，并返回执行获取工具器械的位姿和工具器械的转动角度的步骤，重新确定机械臂的当前末端位姿，直至当前末端位姿满足目标位姿精度为止。

本实施例涉及确定机械臂的当前末端位姿不满足目标位姿精度的情况，在此情况下，控制终端重新解算机械臂的各关节的关节值，具体确定的过程包括：控制设备重新设置工具器械(即虚拟关节，例如图3中的G6)的转动角度θ₆，然后返回前述实施例中S101的步骤，以根据新的工具器械的转动角度和工具器械的位姿解算机械臂上各关节的关节角值，然后根据解算的机械臂上各关节的关节值计算新的机械臂的当前末端位姿，再返回上述S402的步骤，重新判断新的机械臂的当前末端位姿是否满足目标位姿精度，如此重复上述过程，直到当前末端位姿满足目标位姿为止。需要说明的是，当获取到新的工具器械的转动角度时，可以根据新的转动角度和工具器械原来的位姿重新解算机械臂上各关节的关节值；可选的，当获取到新的工具器械的转动角度时，控制终端也可以先根据新的工具器械的转动角度和各关节的当前位置计算得到新的工具器械的位姿，然后再基于新的工具器械的转动角度和新的工具器械的位姿重新解算机械臂上各关节的关节值。

上述实施例通过对比计算得到的各关节的关节值确定的机械臂的当前末端位姿与目标位姿精度之间误差，从而得到控制机械臂时需要的各关节的目标关节值，提高了机械臂的控制准确性与高效性。

可选的，本申请还提供了新的工具器械的转动角度(即虚拟关节的旋转角度)的获取方法，该方法具体包括：在转动角度上增加或减少预设角度，得到新的工具器械的转动角度。其中，预设角度可以是微小的角度，可以预先根据实际计算精度确定。

在控制装置确定新的工具器械的转动角度时，具体可以设置邻域范围和步长，并在邻域范围内实现对工具器械的转动角度更新，即，θ₆'＝θ₆+δ，其中，δ为预设步长，只至根据每次遍历的转动角度按照前述实施例所述的求解方法，求解出的机械臂的当前末端位姿满足目标位姿精度为止。

在实际应用中，当控制终端计算出机械臂的各关节的目标关节值时，目标关节值可能存在多种解，在此种情况下，还需要控制终端从多组各关节的目标关节值中筛选出最终用于控制机械臂的各关节的目标关节值。因此，在一个实施例中，在控制终端执行步骤S103之后，如图7所示，图2实施例所述的方法还包括步骤：

S104，根据预设的筛选条件，从多组目标关节值中筛选出一组目标关节值。

其中，筛选条件可以由控制终端预先根据实际控制需求确定。具体的，当控制终端得到多组目标关节值时，可以进一步的从多组目标关节值中筛选出一组满足筛选条件的各关节的目标关节值；可选的，在筛选时，控制终端也可以从多组目标关节值中随机筛选一组目标关节值控制机械臂。

可选的，提供了上述S104的一种具体实现方式，如图8所示，该方式包括：

S501，获取机械臂上的各关节在上一时刻的关节值。

其中，机械臂上的各关节在上一时刻的关节值是控制终端在上一时刻对机械臂进行控制时存储的各关节的关节值。控制终端在筛选当前时刻的各关节的关节值时，可以先从存储的数据中获取机械臂上的各关节在上一时刻的关节值，作为参考数据。

S502，将多组各关节的目标关节值与各关节在上一时刻的关节值进行比较，并从中筛选出变化值最小的一组目标关节值。

控制终端得到各关节在上一时刻的关节值时，可以进一步的将各关节在上一时刻的关节值与多组各关节的目标关节值依次进行比较，并根据比较结果，从多组各关节的目标关节值中筛选出变化值最小的一组目标关节值。这样的筛选方法参考上一时刻的各关节的关节值，可以在一定程度上提高获取目标关节值的效率与准确性，进而提高机械臂控制的准确性与效率。需要说明的是，最终筛选出来的目标关节值即为最终控制机械臂时的关节值。

在实际应用中，当控制终端执行了上述S104的步骤之后，就可以输出目标关节值控制机械臂，基于此，如图9所示，控制终端还执行步骤：

S105，将机械臂的目标关节值输入到机械臂控制器中，控制机械臂到达目标位姿。

其中，机械臂控制器与机械臂的各关节电机驱动器连接，用于控制机械臂的各关节按照目标关节值(关节转动角度)进行运动。本实施例涉及控制终端控制机械臂的过程，具体的，当控制终端按照上述实施例解算得到机械臂的各关节的目标关节值后，即可将目标关节值导入到与机械臂控制器中，驱动机械臂控制器控制机械臂上各关节按照导入的目标关节值进行运动，最终到达目标位姿。需要说明的是，控制终端可以与机械臂控制器进行有线或无线连接，以传输数据至机械臂控制器。机械臂控制器也可以安装在机械臂上，也可以安装在控制终端，还可以独立安装，此处不限定。

综合上述所有实施例，还提供了一种机械臂的控制方法,如图10所示，该方法包括：

S601，获取工具器械的位姿和转动角度。

S602，根据工具器械的位姿和转动角度确定机械臂上的汇交点的位置。

S603，根据汇交点的位置确定机械臂上第一关节、第二关节和第三关节的关节值。

S604，根据第一关节、第二关节和第三关节的关节值，确定机械臂上第四关节和第五关节的关节值。

S605，采用机械臂正运动学求解方法，根据解算的机械臂上第一关节、第二关节、第三关节、第四关节和第五关节的关节值(机械臂上各关节的关节值) 计算得到机械臂的当前末端位姿。

S606，判断当前末端位姿是否满足目标位姿精度，若满足，则执行步骤S607，若不满足，则执行步骤S608。

S607，将计算得到的各关节的关节值作为控制机械臂的目标关节值。

S608，确定新的工具器械的转动角度，并返回执行S601的步骤，根据新的工具器械的转动角度和工具器械的位姿重新解算机械臂上各关节的关节值，直到解算出的机械臂的当前末端位姿满足目标位姿精度为止。

S609，判断确定的目标关节值是否包括多组各关节的目标关节值，若是，则执行步骤S610-S612，若否，则执行步骤S612。

S610，获取机械臂上的各关节在上一时刻的关节值。

S611，将多组各关节的目标关节值与各关节在上一时刻的关节值进行比较，并从中筛选出变化值最小的一组目标关节值。

S612，将机械臂的目标关节值输入到机械臂控制器中，控制机械臂到达目标位姿。

上述实施例中各步骤的详细说明在前述实施例中都有阐述，详细内容请参见前述说明，此处不赘述。

上述实施例提供的方法，通过在5自由度的机械臂上设置虚拟关节G6(工具器械)，将5自由度的机械臂转换为6自由度的机械臂进行控制解算，通过计算汇交点坐标得到机械臂关节值的解析表达式，只有在最后判断机械臂的当前末端位姿是否满足目标位姿精度时进行迭代运算，具体在上一时刻关节值领域范围内实现对虚拟关节的转动角度(工具器械绕轴线的转动角度)进行遍历求解，得到符合目标位姿精度要求的机械臂的目标关节角度。可见，整个过程只需要一次迭代运算，且该迭代范围有限，不依赖传统数值迭代法中初值与迭代算法的选取，不仅降低了计算的复杂度，而且提高了解算的准确性。

应该理解的是，虽然图2-10的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2-10中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。

在一个实施例中，如图11所示，提供了一种机械臂的控制装置，包括：

获取模块11，用于获取工具器械的位姿和转动角度；所述工具器械的位姿包括所述工具器械在轴线的方向向量和所述工具器械的位置坐标；

确定模块12，用于根据所述工具器械的位姿和转动角度确定机械臂上各关节的关节值；

解算模块13，用于采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值确定所述机械臂的目标关节值。

在一个实施例中，如图12所示，上述确定模块12，包括：

第一确定单元121，用于根据所述工具器械的位姿和转动角度确定所述机械臂上的第一关节至第三关节的关节值；

第二确定单元122，用于根据所述第一关节至第三关节的关节值确定所述机械臂上的第四关节和第五关节的关节值。

在一个实施例中，如图13所示，上述第一确定单元121，包括：

第一确定子单元1211，用于根据所述工具器械的位姿和转动角度确定所述机械臂上的汇交点的位置坐标；

第二确定子单元1212，用于根据所述汇交点的位置坐标确定所述第一关节至第三关节的关节值。

在一个实施例中，如图14所示，上述解算模块13，包括：

第三确定单元131，用于采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值计算所述机械臂的当前末端位姿；

判断单元132，用于判断所述当前末端位姿是否满足目标位姿精度，若满足，则将计算得到的机械臂上各关节的关节值作为控制所述机械臂的目标关节值；不满足，则确定新的工具器械的转动角度，并返回执行所述获取工具器械的位姿和工具器械的转动角度的步骤，重新确定所述机械臂的当前末端位姿，直至所述当前末端位姿满足所述目标位姿精度为止。

在一个实施例中，如图15所示，上述机械臂的控制装置，还包括：

筛选模块14，用于根据预设的筛选条件，从所述多组各关节的目标关节值中筛选出一组目标关节值；

在一个实施例中，如图16所示，上述筛选模块14，包括：

获取单元141，用于获取所述机械臂上的各关节在上一时刻的关节角度值；

筛选单元142，用于将所述多组各关节的目标关节值与所述各关节在上一时刻的关节值进行比较，并从中筛选出变化值最小的一组目标关节值。

在一个实施例中，如图17所示，上述机械臂的控制装置，还包括：

控制模块15，用于将所述机械臂的目标关节值输入到机械臂控制器中，控制所述机械臂到达目标位姿。

关于机械臂的控制装置的具体限定可以参见上文中对于机械臂的控制方法的限定，在此不再赘述。上述机械臂的控制装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图18所示。该计算机设备包括通过系统总线连接的处理器、存储器和网络接口。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储各机械臂的各关节的关节数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种机械臂的控制方法。

本领域技术人员可以理解，图18中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现以下步骤：

获取工具器械的位姿和转动角度；所述工具器械的位姿包括所述工具器械在轴线的方向向量和所述工具器械的位置坐标；

根据所述工具器械的位姿和转动角度确定机械臂上各关节的关节值；

采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值确定所述机械臂的目标关节值。

上述实施例提供的一种计算机设备，其实现原理和技术效果与上述方法实施例类似，在此不再赘述。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：

获取工具器械的位姿和转动角度；所述工具器械的位姿包括所述工具器械在轴线的方向向量和所述工具器械的位置坐标；

根据所述工具器械的位姿和转动角度确定机械臂上各关节的关节值；

采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值确定所述机械臂的目标关节值。

上述实施例提供的一种计算机可读存储介质，其实现原理和技术效果与上述方法实施例类似，在此不再赘述。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-Only Memory，ROM)、磁带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器(Random Access Memory，RAM)或外部高速缓冲存储器。作为说明而非局限，RAM可以是多种形式，比如静态随机存取存储器(Static Random Access Memory， SRAM)或动态随机存取存储器(Dynamic Random Access Memory，DRAM)等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种机械臂的控制方法，其特征在于，所述方法包括：

获取工具器械的位姿和转动角度；所述工具器械的位姿包括所述工具器械在轴线的方向向量和所述工具器械的位置坐标，所述转动角度为所述工具器械绕所述轴线的转动角度；

根据所述工具器械的位姿和转动角度确定机械臂上各关节的关节值；

采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值确定所述机械臂的目标关节值。

2.根据权利要求1所述的方法，其特征在于，所述根据所述工具器械的位姿和转动角度确定机械臂上各关节的关节值，包括：

根据所述工具器械的位姿和转动角度确定所述机械臂上的第一关节至第三关节的关节值；

根据所述第一关节至第三关节的关节值确定所述机械臂上的第四关节和第五关节的关节值。

3.根据权利要求2所述的方法，其特征在于，所述根据所述工具器械的位姿和转动角度确定所述机械臂上的第一关节至第三关节的关节值，包括：

根据所述工具器械的位姿和转动角度确定所述机械臂上的汇交点的位置坐标；

根据所述汇交点的位置坐标确定所述第一关节至第三关节的关节值。

4.根据权利要求1-3任一项所述的方法，其特征在于，所述采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值确定所述机械臂的目标关节值，包括：

采用机械臂正运动学求解方法，根据所述机械臂上各关节的关节值计算所述机械臂的当前末端位姿；

判断所述当前末端位姿是否满足目标位姿精度，若满足，则将计算得到的机械臂上各关节的关节值作为控制所述机械臂的目标关节值；若不满足，则确定新的工具器械的转动角度，并返回执行所述获取工具器械的位姿和工具器械的转动角度的步骤，重新确定所述机械臂的当前末端位姿，直至所述当前末端位姿满足所述目标位姿精度为止。

5.根据权利要求4所述的方法，其特征在于，所述确定新的工具器械的转动角度，包括：

在所述转动角度上增加或减少预设角度，得到所述新的工具器械的转动角度。

6.根据权利要求1-3任一项所述的方法，其特征在于，若确定的所述机械臂的目标关节值包括多组各关节的目标关节值，则所述方法还包括：

根据预设的筛选条件，从所述多组各关节的目标关节值中筛选出一组目标关节值。

7.根据权利要求6所述的方法，其特征在于，所述根据预设的筛选条件，从所述多组各关节的目标关节值中筛选出一组目标关节值，包括：

获取所述机械臂上的各关节在上一时刻的关节值；

将所述多组各关节的目标关节值与所述各关节在上一时刻的关节值进行比较，并从中筛选出变化值最小的一组目标关节值。

8.根据权利要求1所述的方法，其特征在于，所述方法还包括：

将所述机械臂的目标关节值输入到机械臂控制器中，控制所述机械臂到达目标位姿。

9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至8中任一项所述方法的步骤。

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

Images