CN106647248B - 一种确定串联机器人反解结果的方法及装置 - Google Patents

Abstract

本发明实施例公开了一种确定串联机器人反解结果的方法及装置。该方法包括：获取末端关节的目标坐标；其中，在末端关节之上，以串联的方式连接有至少一个前端关节；随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各前端关节分别对应的关节变量；根据各随机关节变量组，计算末端关节的随机坐标；根据目标坐标，在各随机坐标中选取末端关节的近似坐标；根据与近似坐标对应的随机关节变量组，采用优化算法确定目标坐标的反解结果。本实施例中的方法适用于任意自由度的串联机器人，求反解结果速度快，反解结果的精度高，计算和存储上开销小，没有累积误差。

Description

一种确定串联机器人反解结果的方法及装置

技术领域

本发明实施例涉及机器人控制技术，尤其涉及一种确定串联机器人反解结果的方法及装置。

背景技术

仿生机器人已应用到很多领域，其可以代替或部分代替人去完成危险环境中的作业，如地震灾害或者医疗行业，医疗行业中可以将机器人尺寸做到足够小进入人手无法达到的空间，如人体自然腔道(如食道)完成手术或定点给药。但是，有些环境不能让机器人完全自主运行，如医疗手术环境或者非常复杂的地震灾区环境等，需要有经验的人来操控这些机器人完成相应的任务，这就要求使用主从控制系统来操控机器人。

在主从控制系统中，一般需要有主操作手、从操作手和控制系统。主操作手一般由人来操作，从操作手一般为串联机器人，在主从机器人的控制系统中，对主、从机器人之间的实时性要求高。对于高实时性，主从机器人的控制系统中电机、控制器的响应时间、主从机器人间的通信时间很短，满足实时性要求比较容易，难点在于从串联机器人的反解时间。

在现有技术中，对于串联机器人，一般采用主从异构的方式来进行主从操作，主从异构机器人大多采用采用雅克比算法进行控制，拾取主手机器人在三维空间内速度变化，再对从机器人进行运动学建模，计算其雅克比矩阵，对矩阵求逆。雅克比矩阵控制算法适用于从机器人自由度(串联机器人的关节个数)可以简化为3或者6的情况，但位移累积误差大，在奇异位置时，雅克比矩阵会失效，因而对于高度冗余的串联机器人(12自由度或者更多)，这种方法不适用。此外，数值法中的循环坐标下降法、牛顿法、拟牛顿法、或者这三者中的两两结合、神经网络等虽然可以被使用，但这些算法求反解的速度慢，有局限性、计算和存储上开销大，因而对于串联机器人不实用。

发明内容

有鉴于此，本发明实施例提供一种确定串联机器人反解结果的方法及装置，可以快速而准确地求出串联机器人的反解结果。

第一方面，本发明实施例提供了一种确定串联机器人反解结果的方法，包括：

获取末端关节的目标坐标；其中，在所述末端关节之上，以串联的方式连接有至少一个前端关节；

随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各所述前端关节分别对应的关节变量；

根据各所述随机关节变量组，计算所述末端关节的随机坐标；

根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标；

根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果。

第二方面，本发明实施例还提供了一种确定串联机器人反解结果的装置，包括：

目标坐标获取模块，用于获取末端关节的目标坐标；其中，在所述末端关节之上，以串联的方式连接有至少一个前端关节；

随机关节变量组产生模块，与所述目标坐标获取模块相连，用于随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各所述前端关节分别对应的关节变量；

随机坐标计算模块，与所述随机关节变量组产生模块相连，用于根据各所述随机关节变量组，计算所述末端关节的随机坐标；

近似坐标选取模块，与所述随机坐标计算模块相连，用于根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标；

反解结果确定模块，与所述近似坐标选取模块相连，用于根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果。

本发明实施例通过随机产生多个随机关节变量组，计算末端关节的随机坐标，在各随机坐标中选取末端关节的近似坐标，来确定目标坐标的反解结果。本发明实施例中的方法适用于任意自由度的串联机器人，求反解结果速度快，反解结果的精度高，计算和存储上开销小，没有累积误差。

附图说明

图1是本发明实施例一提供的一种确定串联机器人反解结果的方法流程图；

图2是本发明实施例二提供的一种确定串联机器人反解结果的方法流程图；

图3是本发明实施例三提供的一种确定串联机器人反解结果的方法流程图；

图4是本发明实施例四提供的一种确定串联机器人反解结果的方法流程图；

图5是本发明实施例五提供的一种确定串联机器人反解结果的装置结构示意图；

图6是本发明实施例六提供的用于实施确定串联机器人反解结果的方法的主从机器人的闭环控制系统示意图；

图7是本发明实施例六提供的一种确定串联机器人反解结果的方法中的各关节坐标系示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

实施例一

图1是本发明实施例一提供的一种确定串联机器人反解结果的方法流程图，本实施例可适用于已知串联机器人末端坐标求解各关节变量的情况，该方法可以由确定串联机器人反解结果的装置来执行，该装置可以由软件和/或硬件方式实现，该装置可以集成在任何提供存储和运算功能的设备中，例如典型的是工业PC机等。该方法包括：

S101、获取末端关节的目标坐标；其中，在所述末端关节之上，以串联的方式连接有至少一个前端关节。串联机器人以串联的方式连接有多个关节，最低端的关节为末端关节，位于该末端关节之上的关节相对于该关节都可以被称为前端关节。

可以理解的是，在对机器人进行控制时，控制结果是通过末端关节的位置以及姿态来体现的，但是为了使末端关节能够达到一个预设的位置以及姿态，需要各前端关节的共同配合运动才能实现。

在本实施例中，所述末端关节的目标坐标，具体是指与期望末端关节能够达到的位置以及姿态的坐标表示。在工业PC机获取末端关节的目标坐标后，通过下面的步骤计算各个前端关节的关节变量(典型的位置以及姿态)，实现最终能够将所述末端关节调整至该目标坐标之上。

S102、随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各所述前端关节分别对应的关节变量。

每个关节都有对应的关节变量，它表示该关节的运动情况，整个串联机器人的各个关节的运动情况可以用关节变量组表示，关节变量组中包括各个前端关节的关节变量。当随机产生多个随机关节变量组时，与之对应的末端的位置和姿态也是随机的，得到的末端的位置和姿态有可能是理想的，也有可能是不理想的，经过筛选后才能得到需要的。随机关节变量组可由随机函数产生，随机关节变量可根据关节的实际运动范围限制在一定范围。例如当关节变量为关节角时，可通过公式θ_n＝θ_Min-n+θ_Max-n×Random()产生各个随机关节角，Random()为一种随机函数，用于产生一个随机数。θ_n为第n个关节的关节角，θ_Min-n为第n个关节的最小关节角，θ_Max-n为第n个关节的最大关节角，并组成随机关节角组(θ₁，θ₂，…，θ_N),n∈[1,N],N为随机关节变量中包括的关节角总数。其与前端关节的总数量相一致

产生的随机关节变量组的组数可根据求解的精确度设置，当增大随机关节变量组的组数时，能提高结果的精确度。

当串联机器人的关节数增加一个时，只是在产生的随机关节变量组里增加一个关节变量，计算的复杂度只是线性增加，所以本实施例适用于任意关节数(自由度)的串联机器人。

S103、根据各所述随机关节变量组，计算所述末端关节的随机坐标。

当各个前端关节的关节变量确定了，末端关节的位置和姿态就确定了，也即确定了末端关节的坐标。例如可建立基坐标系和关节坐标系，由关节之间的连接关系确定各个坐标系之间的变换矩阵及末端关节到基坐标系的坐标变换矩阵，通过把每个随机关节变量组代入到坐标变换矩阵，得到对应的多个末端关节的随机坐标。

S104、根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标。

因为随机关节变量组有多组，所以对应的末端关节的随机坐标有多个。随机坐标和目标坐标的差异有大有小，随机关节变量组是随机产生的，随机坐标与目标坐标相同的概率很小。可设置误差阈值，将误差阈值范围内的随机坐标作为末端关节的近似坐标，便于做进一步筛选。

因为误差阈值可人为设置，所以可以控制结果的误差。本实施例中的结果是通过随机的实验试出来的，而不是由计算得出，所以没有累积误差。

其中，选取的近似坐标的数量可以为1个也可以为多个。其中，可以将与目标坐标之间的误差值最小的一个随机坐标作为所述近似坐标；也可以将与目标坐标之间的误差值满足预设误差条件的多个随机坐标作为所述近似坐标，本实施例对此并不进行限制。

S105、根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果。

末端关节的近似坐标有对应的随机关节变量组，当各个关节按该随机关节变量组中的关节变量进行运动时，可使末端关节处于与目标坐标相接近的近似坐标对应的位置上。所以可将近似坐标对应的随机关节变量组作为目标坐标的反解结果。

本实施例通过随机产生多个随机关节变量组，计算末端关节的随机坐标，在各随机坐标中选取末端关节的近似坐标，来确定目标坐标的反解结果。本实施例中的方法适用于任意自由度的串联机器人，求反解结果速度快，反解结果的精度高，计算和存储上开销小，没有累积误差。

实施例二

图2是本发明实施例二提供的一种确定串联机器人反解结果的方法流程图，实施例二以上述实施例为基础，将根据各所述随机关节变量组，计算所述末端关节的随机坐标，优化为，建立串联机器人的基坐标系、每一个关节的关节坐标系，其中，i∈[1,n]；n为所述串联机器人包括的关节总数，根据所述基坐标系，以及所述关节坐标系，确定各个坐标系之间的变换矩阵，根据公式计算所述末端关节在所述基坐标系中的变换矩阵，作为所述预设的坐标变换矩阵，把每个随机关节变量组代入到预设的坐标变换矩阵中，得到与所述末端关节对应的多个随机坐标。

本实施例提供的确定串联机器人反解结果的方法包括以下步骤：S201、S202、S203、S204、S205、S206、S207和S208。其中，S201、S202、S207和S208分别与实施例一中的S101、S102、S104和S105相同，相同的步骤不再赘述。

S201、获取末端关节的目标坐标；其中，在所述末端关节之上，以串联的方式连接有至少一个前端关节。

S202、随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各所述前端关节分别对应的关节变量。

S203、建立串联机器人的基坐标系、每一个关节的关节坐标系，其中，i∈[1,n]；n为所述串联机器人包括的关节总数。

例如可根据空间机器人的连杆参数，应用D-H建模方法，建立串联机器人的基坐标系O₀(x₀,y₀,z₀)、每一个关节的关节坐标系O_i(x_i,y_i,z_i)。每个关节坐标系中的关节变量表示了该关节的运动情况。关节坐标系中的关节变量可统一为用基坐标表示。

S204、根据所述基坐标系，以及所述关节坐标系，确定各个坐标系之间的变换矩阵。

关节之间的连接关系确定各个坐标系之间的变换矩阵

某个关节坐标系的变量可用相邻关节坐标系表示，各个关节坐标系经几次变换后都可转换成用基坐标系表示。

S205、根据公式计算所述末端关节在所述基坐标系中的变换矩阵，作为所述预设的坐标变换矩阵。

每组随机关节变量组对应着末端关节的随机坐标，这个对应关系可以是末端关节在所述基坐标系中的变换矩阵，串联机器人确定后，各个关节之间的连接关系也就确定了，预设的坐标变换矩阵就确定了。末端到基坐标系O₀(x₀,y₀,z₀)的坐标变换矩阵

可由各个坐标系之间的变换矩阵

逐项右乘得到，

其中，d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z用于表示末端关节的姿态，p_x、p_y及p_z用于表示末端关节的位置；

S206、把每个随机关节变量组代入到预设的坐标变换矩阵中，得到与所述末端关节对应的多个随机坐标。

因为预设的坐标变换矩阵

可由各个坐标系之间的变换矩阵

逐项右乘得到，所以预设的坐标变换矩阵中的d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z、p_x、p_y及p_z的取值是各个关节变量的函数，把每个随机关节变量组代入到预设的坐标变换矩阵中，得到与所述末端关节对应的多个随机坐标。随机关节变量组表示各个关节的运动情况，预设的坐标变换矩阵表示关节之间的连接关系，二者结合可得到对应的随机坐标。接下来就可判断得到的末端的随机坐标与目标坐标是否接近。

S207、根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标。

S208、根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果。

本实施例通过把每个随机关节变量组代入到预设的坐标变换矩阵中，得到与所述末端关节对应的多个随机坐标，通过检验随机坐标与目标坐标的差异即可确定对应的随机关节变量组是否可作为反解，简化了串联机器人求反解的过程。

实施例三

图3是本发明实施例三提供的一种确定串联机器人反解结果的方法流程图，实施例三以上述实施例为基础，将根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标，优化为，计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值，获取所述误差值小于误差阈值的末端关节的随机坐标，作为所述末端关节的近似坐标。

本实施例提供的确定串联机器人反解结果的方法包括以下步骤：S301、S302、S303、S304、S305和S306。其中，S301、S302、S303和S306分别与实施例一中的S101、S102、S103和S105相同，相同的步骤不再赘述。

S301、获取末端关节的目标坐标；其中，在所述末端关节之上，以串联的方式连接有至少一个前端关节。

S302、随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各所述前端关节分别对应的关节变量。

S303、根据各所述随机关节变量组，计算所述末端关节的随机坐标。

S304、计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值。

末端关节的随机坐标有多个，它们与末端关节的目标坐标之间的差异可用误差值表示。末端的坐标包含末端的位置和姿态，所以误差值包括位置误差和姿态误差。因为末端的坐标是关节变量的函数，且末端关节的目标坐标是已知量，所以误差值是随机关节变量的函数。

进一步地，计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值，包括：

根据公式：ΔE＝ΔP+ΔR；以及

计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值ΔE。

其中，ΔP表示位置差异，ΔR表示姿态差异，ω_ρ和ω_γ为所述位置差异ΔP和所述姿态差异ΔR的权重，P_s为末端关节的目标位置，p_sj为末端关节的目标位置分量，d_sj为末端关节的目标姿态，P_s、p_sj及d_sj可从所述末端关节的目标坐标获得，P_r为末端关节的随机位置，p_rj为末端关节的随机位置分量，d_rj为末端关节的随机姿态，P_r、p_rj及d_rj可从所述末端关节的随机坐标中获得。

其中，ω_γ权重大小的选择应足以使姿态误差与位置误差同步收敛，一般应该大于10⁴；如果机器人的运动状态对末端位置的精度要求更高一些，则ω_γ的取值一般为0；若是对位置和姿态都需要满足一定的精度，则ω_γ的取值一般为10⁶。

S305、获取所述误差值小于误差阈值的末端关节的随机坐标，作为所述末端关节的近似坐标。

当末端关节的随机坐标与目标坐标的误差值小于误差阈值，说明末端关节的随机坐标与目标坐标很近似，所以可将该末端关节的随机坐标作为末端关节的近似坐标。

S306、根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果。

本实施例通过计算末端关节的目标坐标与末端关节的随机坐标之间的误差值，可知与末端关节的随机坐标对应的随机关节变量组能否作为反解，能方便且有效地筛选出反解结果。

实施例四

图4是本发明实施例四提供的一种确定串联机器人反解结果的方法流程图，实施例四以上述实施例为基础，将根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果，优化为，获取与至少两个所述近似坐标分别对应随机关节变量组，作为待选随机关节变量组；选取距离所述末端关节最远的一个前端关节，作为当前比对关节；分别在各所述待选随机关节变量组中，获取与所述当前比对关节对应的关节变量；根据所述关节变量，确定与各所述待选随机关节变量组分别对应的当前比对关节移动量；保留与所述关节移动量的最小值对应的待选随机关节变量组，并过滤掉其余待选随机关节变量组；如果保留的所述待选随机关节变量组的数量值大于1，则朝着靠近所述末端关节的方向，选取新的前端关节作为所述当前比对关节，并返回执行分别在各所述待选随机关节变量组中，获取与所述当前比对关节对应的关节变量的操作，直至最终保留的所述待选随机关节变量组的数量值为1；将最终保留的所述待选随机关节变量组，作为所述目标坐标的反解结果。

本实施例提供的确定串联机器人反解结果的方法包括以下步骤：S401、S402、S403、S404、S405、S406、S407、S408、S409、S410和S411。其中，S401、S402、S403和S404分别与实施例一中的S101、S102、S103及S104相同，相同的步骤不再赘述。

S401、获取末端关节的目标坐标；其中，在末端关节之上，以串联的方式连接有至少一个前端关节。

S402、随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各所述前端关节分别对应的关节变量。

S403、根据各所述随机关节变量组，计算所述末端关节的随机坐标。

S404、根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标。

S405、获取与至少两个所述近似坐标分别对应随机关节变量组，作为待选随机关节变量组。

满足误差阈值的末端关节的近似坐标有多个，与之对应的随机关节变量组也有多个，这些随机关节变量组可以作为目标坐标的反解结果，但它们未必是最优的反解结果。可以从待选随机关节变量组中进一步选出最优反解结果。

S406-S411为从待选随机关节变量组中选出最优反解结果的过程。关节移动量越小时所消耗的能量越少，所以要以关节移动量小的待选随机关节变量组作为最优反解结果。串联机器人有很多个关节，在关节移动量相同的情况下，远离末端关节的关节消耗的能量比靠近末端关节的关节消耗的能量多，所以在寻找最优反解结果时要优先保证远离末端关节的关节移动量小。综上，可从离末端关节最远的关节开始，朝着靠近末端关节的方向，逐个关节地找最小的关节移动量。

S406、选取距离所述末端关节最远的一个前端关节，作为当前比对关节。

一种从待选随机关节变量组中进一步选出最优反解结果的方法是，选关节移动量最小的待选随机关节变量组作为最优反解结果。因为串联机器人有多个关节，可以逐个关节地找关节移动量的最小值。当关节移动量相同时，远离末端的关节消耗的能量比靠近末端的关节消耗得多，所以优先保证离末端远的关节的关节移动量小。所以开始时选取距离末端关节最远的前端关节作为当前比对关节。

S407、分别在各所述待选随机关节变量组中，获取与所述当前比对关节对应的关节变量。

待选随机关节变量组中包括各个关节的关节变量，分别对应各个关节。每个待选随机关节变量组对应着一组关节关节变量。因为是逐个关节进行比较的，所以每次比较的时候需获取当前比对关节的关节变量。

S408、根据所述关节变量，确定与各所述待选随机关节变量组分别对应的当前比对关节移动量。

关节移动量越小消耗的能量越少，根据关节变量确定当前比对关节的关节移动量。

S409、保留与关节移动量的最小值对应的待选随机关节变量组，并过滤掉其余待选随机关节变量组。

待选随机关节变量组根据当前比对关节的关节移动量进行一次优选，找出当前比对关节的关节移动量的最小值，保留该最小值对应的待选随机关节变量组。当保留的待选随机关节变量组为一组，该待选随机关节变量组即为最优的反解结果。当保留的待选随机关节变量组不止一组时，还需进行进一步优选。

S410、判断保留的待选随机关节变量组的数量值是否大于1，如果大于1则返回执行步骤S406，如果不大于1则继续执行步骤S411。

如果保留的待选随机关节变量组的数量值大于1，说明没有找到最优反解结果，则返回执行步骤S406，朝着靠近末端关节的方向，选取新的前端关节作为当前比对关节，选取该当前比对关节的关节移动量最小值对应的保留的待选随机关节变量组。如果不大于1则说明最终保留的那个待选随机关节变量组是最优的，继续执行步骤S411。

S411、将最终保留的所述待选随机关节变量组，作为所述目标坐标的反解结果。

经过在各个关节选取最小关节移动量后，最终保留的待选随机关节变量组在各个关节的关节移动量都是最小的。最终保留的待选随机关节变量组对应的末端关节的随机坐标与目标坐标之间的误差在误差阈值范围内，最终保留的待选随机关节变量组的关节移动的能量消耗是最小的，可作为目标坐标的反解结果，而且是最优的。

本实施例从距离末端关节最远的前端关节开始，朝着靠近末端关节的方向，保留与关节移动量的最小值对应的待选随机关节变量组，可找到使串联机器人移动关节时消耗最少能量的最优反解结果。

实施例五

图5是本发明实施例五提供的一种确定串联机器人反解结果的装置结构示意图，该装置用于执行上述实施例中的确定串联机器人反解结果的方法。该装置包括：目标坐标获取模块501、随机关节变量组产生模块502、随机坐标计算模块503、近似坐标选取模块504和反解结果确定模块505。

目标坐标获取模块501，用于获取末端关节的目标坐标；其中，在所述末端关节之上，以串联的方式连接有至少一个前端关节。

随机关节变量组产生模块502，与所述目标坐标获取模块501相连，用于随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各所述前端关节分别对应的关节变量。

随机坐标计算模块503，与所述随机关节变量组产生模块502相连，用于根据各所述随机关节变量组，计算所述末端关节的随机坐标。

近似坐标选取模块504，与所述随机坐标计算模块503相连，用于根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标。

反解结果确定模块505，与所述近似坐标选取模块504相连，用于根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果。

本实施例通过随机产生多个随机关节变量组，计算末端关节的随机坐标，在各随机坐标中选取末端关节的近似坐标，来确定目标坐标的反解结果。本实施例中的方法适用于任意自由度的串联机器人，求反解结果速度快，反解结果的精度高，计算和存储上开销小，没有累积误差。

进一步地，所述随机坐标计算模块包括代入坐标变换矩阵单元，用于把每个随机关节变量组代入到预设的坐标变换矩阵中，得到与所述末端关节对应的多个随机坐标。

进一步地，所述随机坐标计算模块还包括坐标系建立单元、变换矩阵确定单元及坐标变换矩阵计算单元。

坐标系建立单元，用于建立所述串联机器人的基坐标系O₀(x₀,y₀,z₀)、每一个关节的关节坐标系O_i(x_i,y_i,z_i)，其中，i∈[1,n]；n为所述串联机器人包括的关节总数。

变换矩阵确定单元，与所述坐标系建立单元相连，用于根据所述基坐标系，以及所述关节坐标系，确定各个坐标系之间的变换矩阵

坐标变换矩阵计算单元，分别与所述变换矩阵确定单元及所述代入坐标变换矩阵单元相连，用于根据公式

计算所述末端关节在所述基坐标系O₀(x₀,y₀,z₀)中的变换矩阵

将所述

作为所述预设的坐标变换矩阵。

其中，d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z用于表示末端关节的姿态，p_x、p_y及p_z用于表示末端关节的位置；所述d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z、p_x、p_y及p_z的取值，由与各所述前端关节分别对应的关节变量确定。

本实施例通过把每个随机关节变量组代入到预设的坐标变换矩阵中，得到与所述末端关节对应的多个随机坐标，通过检验随机坐标与目标坐标的差异即可确定对应的随机关节变量组是否可作为反解，简化了串联机器人求反解的过程。

进一步地，所述近似坐标选取模块包括误差值计算单元及误差值判断单元。

误差值计算单元，用于计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值。

误差值判断单元，与所述误差值计算单元相连，用于获取所述误差值小于误差阈值的末端关节的随机坐标，作为所述末端关节的近似坐标。

本实施例通过计算末端关节的目标坐标与末端关节的随机坐标之间的误差值，可知与末端关节的随机坐标对应的随机关节变量组能否作为反解，能方便且有效地筛选出反解结果。

进一步地，反解结果确定模块包括待选随机关节变量组获取单元、当前比对关节选取单元、关节变量获取单元、移动量确定单元、筛选随机关节变量组单元、缩减随机关节变量组数量值单元及保留随机关节变量组单元。

待选随机关节变量组获取单元，用于获取与至少两个所述近似坐标分别对应随机关节变量组，作为待选随机关节变量组。

当前比对关节选取单元，与所述待选随机关节变量组获取单元相连，用于选取距离所述末端关节最远的一个前端关节，作为当前比对关节。

关节变量获取单元，与所述当前比对关节选取单元相连，用于分别在各所述待选随机关节变量组中，获取与所述当前比对关节对应的关节变量。

移动量确定单元，与所述关节变量获取单元相连，用于根据所述关节变量，确定与各所述待选随机关节变量组分别对应的当前比对关节移动量。

筛选随机关节变量组单元，与所述移动量确定单元相连，用于保留与所述关节移动量的最小值对应的待选随机关节变量组，并过滤掉其余待选随机关节变量组。

缩减随机关节变量组数量值单元，与所述筛选随机关节变量组单元相连，用于如果保留的所述待选随机关节变量组的数量值大于1，则朝着靠近所述末端关节的方向，选取新的前端关节作为所述当前比对关节，并返回执行分别在各所述待选随机关节变量组中，获取与所述当前比对关节对应的关节变量的操作，直至最终保留的所述待选随机关节变量组的数量值为1。

保留随机关节变量组单元，与所述缩减随机关节变量组数量值单元相连，用于将最终保留的所述待选随机关节变量组，作为所述目标坐标的反解结果。

本实施例从距离末端关节最远的前端关节开始，朝着靠近末端关节的方向，保留与关节移动量的最小值对应的待选随机关节变量组，可找到使串联机器人移动关节时消耗最少能量的最优反解结果。

进一步地，误差值计算单元具体用于根据公式ΔE＝ΔP+ΔR以及计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值ΔE。

其中，ΔP表示位置差异，ΔR表示姿态差异，ω_ρ和ω_γ为所述位置差异ΔP和所述姿态差异ΔR的权重，P_s为末端关节的目标位置，p_sj为末端关节的目标位置分量，d_sj为末端关节的目标姿态，P_s、p_sj及d_sj可从所述末端关节的目标坐标获得，P_r为末端关节的随机位置，p_rj为末端关节的随机位置分量，d_rj为末端关节的随机姿态，P_r、p_rj及d_rj可从所述末端关节的随机坐标中获得。

本发明实施例所提供的确定串联机器人反解结果的装置可用于执行本发明任意实施例所提供的确定串联机器人反解结果的方法，具备执行该方法相应的功能和有益效果。

实施例六

图6是本发明实施例六提供的用于实施确定串联机器人反解结果的方法的主从机器人的闭环控制系统示意图，图7是本发明实施例六提供的一种确定串联机器人反解结果的方法中的各关节坐标系示意图。本实施例是上述实施例的技术方案的应用，是一个优选的具体实施方式。

一种确定串联机器人反解结果的方法所在的系统包括：操作者601、主机器人602、PC机603、控制器604、从机器人605及光学跟踪仪606，如图6所示。其中，主机器人602与PC机603之间通过TCP/IP协议来通信，控制器604与PC机603之间通过以太网来通信。

图7示出了从机器人的三个关节，包括第一关节701、第二关节702和末端关节703，并在第一关节上建立了基坐标系，在第二关节702上建立了中间坐标系，在末端关节703上建立了末端坐标系。本领域技术人员可以理解的是，从机器人的关节的个数可以是其他的任意值。

操作者601操作主机器人602，把主机器人601的运动指令通过PC机603和控制器604传递给从机器人605，光学跟踪仪606给操作者601反馈从机器人605的当前运动状态和位置状态。

PC机603从主机器人602接收到的运动指令是从机器人605的末端关节703的目标坐标，然后PC机603应用本发明上述实施例提供的确定串联机器人反解结果的方法，来获得从机器人605的第一关节701和第二关节702的关节变量，控制器604根据该关节变量控制从机器人605各关节的运动，以最优的方式使末端关节处于目标坐标。

本实施例给出了一种应用确定串联机器人反解结果的方法的主从机器人系统和从机器人的关节结构，使得PC机能够根据主机器人给出的从机器人的末端关节的目标坐标，快速而准确地求出从机器人各关节的关节变量，控制器根据该关节变量控制从机器人实时跟随主机器人的运动。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (10)

1.一种确定串联机器人反解结果的方法，其特征在于，包括：

获取末端关节的目标坐标；其中，在所述末端关节之上，以串联的方式连接有至少一个前端关节；

随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各所述前端关节分别对应的关节变量；

把每个随机关节变量组代入到预设的坐标变换矩阵中，得到与所述末端关节对应的多个随机坐标；

根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标；

根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果，所述优化算法用于：从离所述末端关节最远的关节开始，朝着靠近所述末端关节的方向，逐个关节地找最小的关节移动量，确定关节移动量小的待选随机关节变量组作为最优反解结果。

2.根据权利要求1所述的方法，其特征在于，在把每个随机关节变量组代入到预设的坐标变换矩阵中，得到与所述末端关节对应的多个随机坐标之前，还包括：

建立所述串联机器人的基坐标系O₀(x₀,y₀,z₀)、每一个关节的关节坐标系O_i(x_i,y_i,z_i)，其中，i∈[1,n]；n为所述串联机器人包括的关节总数；

根据所述基坐标系，以及所述关节坐标系，确定各个坐标系之间的变换矩阵

根据公式

计算所述末端关节在所述基坐标系O₀(x₀,y₀,z₀)中的变换矩阵

将所述

作为所述预设的坐标变换矩阵；

其中，d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z用于表示末端关节的姿态，p_x、p_y及p_z用于表示末端关节的位置；

所述d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z、p_x、p_y及p_z的取值，由与各所述前端关节分别对应的关节变量确定。

3.根据权利要求2所述的方法，其特征在于，根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标，包括：

计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值；

获取所述误差值小于误差阈值的末端关节的随机坐标，作为所述末端关节的近似坐标。

4.根据权利要求3所述的方法，其特征在于，根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果，包括：

获取与至少两个所述近似坐标分别对应随机关节变量组，作为待选随机关节变量组；

选取距离所述末端关节最远的一个前端关节，作为当前比对关节；

分别在各所述待选随机关节变量组中，获取与所述当前比对关节对应的关节变量；

根据所述关节变量，确定与各所述待选随机关节变量组分别对应的当前比对关节移动量；

保留与所述关节移动量的最小值对应的待选随机关节变量组，并过滤掉其余待选随机关节变量组；

如果保留的所述待选随机关节变量组的数量值大于1，则朝着靠近所述末端关节的方向，选取新的前端关节作为所述当前比对关节，并返回执行分别在各所述待选随机关节变量组中，获取与所述当前比对关节对应的关节变量的操作，直至最终保留的所述待选随机关节变量组的数量值为1；

将最终保留的所述待选随机关节变量组，作为所述目标坐标的反解结果。

5.根据权利要求3所述的方法，其特征在于，计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值，包括：

根据公式：ΔE＝ΔP+ΔR；以及

计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值ΔE；

其中，ΔP表示位置差异，ΔR表示姿态差异，ω_ρ和ω_γ为所述位置差异ΔP和所述姿态差异ΔR的权重，P_s为末端关节的目标位置，p_sj为末端关节的目标位置分量，d_sj为末端关节的目标姿态，P_s、p_sj及d_sj可从所述末端关节的目标坐标获得，P_r为末端关节的随机位置，p_rj为末端关节的随机位置分量，d_rj为末端关节的随机姿态，P_r、p_rj及d_rj可从所述末端关节的随机坐标中获得。

6.一种确定串联机器人反解结果的装置，其特征在于，包括：

目标坐标获取模块，用于获取末端关节的目标坐标；其中，在所述末端关节之上，以串联的方式连接有至少一个前端关节；

随机关节变量组产生模块，与所述目标坐标获取模块相连，用于随机产生多个随机关节变量组；其中，每个随机关节变量组中包括与各所述前端关节分别对应的关节变量；

随机坐标计算模块，与所述随机关节变量组产生模块相连，用于根据各所述随机关节变量组，计算所述末端关节的随机坐标，包括代入坐标变换矩阵单元，用于把每个随机关节变量组代入到预设的坐标变换矩阵中，得到与所述末端关节对应的多个随机坐标；

近似坐标选取模块，与所述随机坐标计算模块相连，用于根据所述目标坐标，在各所述随机坐标中选取所述末端关节的近似坐标；

反解结果确定模块，与所述近似坐标选取模块相连，用于根据与所述近似坐标对应的随机关节变量组，采用优化算法确定所述目标坐标的反解结果，所述优化算法用于：从离所述末端关节最远的关节开始，朝着靠近所述末端关节的方向，逐个关节地找最小的关节移动量，确定关节移动量小的待选随机关节变量组作为最优反解结果。

7.根据权利要求6所述的装置，其特征在于，所述随机坐标计算模块还包括：

坐标系建立单元，用于建立所述串联机器人的基坐标系O₀(x₀,y₀,z₀)、每一个关节的关节坐标系O_i(x_i,y_i,z_i)，其中，i∈[1,n]；n为所述串联机器人包括的关节总数；

变换矩阵确定单元，与所述坐标系建立单元相连，用于根据所述基坐标系，以及所述关节坐标系，确定各个坐标系之间的变换矩阵

坐标变换矩阵计算单元，分别与所述变换矩阵确定单元及所述代入坐标变换矩阵单元相连，用于根据公式

计算所述末端关节在所述基坐标系O₀(x₀,y₀,z₀)中的变换矩阵

将所述

作为所述预设的坐标变换矩阵；

其中，d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z用于表示末端关节的姿态，p_x、p_y及p_z用于表示末端关节的位置；

所述d_1x、d_1y、d_1z、d_2x、d_2y、d_2z、d_3x、d_3y及d_3z、p_x、p_y及p_z的取值，由与各所述前端关节分别对应的关节变量确定。

8.根据权利要求7所述的装置，其特征在于，所述近似坐标选取模块包括：

误差值计算单元，用于计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值；

误差值判断单元，与所述误差值计算单元相连，用于获取所述误差值小于误差阈值的末端关节的随机坐标，作为所述末端关节的近似坐标。

9.根据权利要求8所述的装置，其特征在于，反解结果确定模块包括：

待选随机关节变量组获取单元，用于获取与至少两个所述近似坐标分别对应随机关节变量组，作为待选随机关节变量组；

当前比对关节选取单元，与所述待选随机关节变量组获取单元相连，用于选取距离所述末端关节最远的一个前端关节，作为当前比对关节；

关节变量获取单元，与所述当前比对关节选取单元相连，用于分别在各所述待选随机关节变量组中，获取与所述当前比对关节对应的关节变量；

移动量确定单元，与所述关节变量获取单元相连，用于根据所述关节变量，确定与各所述待选随机关节变量组分别对应的当前比对关节移动量；

筛选随机关节变量组单元，与所述移动量确定单元相连，用于保留与所述关节移动量的最小值对应的待选随机关节变量组，并过滤掉其余待选随机关节变量组；

缩减随机关节变量组数量值单元，与所述筛选随机关节变量组单元相连，用于如果保留的所述待选随机关节变量组的数量值大于1，则朝着靠近所述末端关节的方向，选取新的前端关节作为所述当前比对关节，并返回执行分别在各所述待选随机关节变量组中，获取与所述当前比对关节对应的关节变量的操作，直至最终保留的所述待选随机关节变量组的数量值为1；

保留随机关节变量组单元，与所述缩减随机关节变量组数量值单元相连，用于将最终保留的所述待选随机关节变量组，作为所述目标坐标的反解结果。

10.根据权利要求8所述的装置，其特征在于，误差值计算单元具体用于：

根据公式：ΔE＝ΔP+ΔR；以及

计算所述末端关节的目标坐标与所述末端关节的随机坐标之间的误差值ΔE；

其中，ΔP表示位置差异，ΔR表示姿态差异，ω_ρ和ω_γ为所述位置差异ΔP和所述姿态差异ΔR的权重，P_s为末端关节的目标位置，p_sj为末端关节的目标位置分量，d_sj为末端关节的目标姿态，P_s、p_sj及d_sj可从所述末端关节的目标坐标获得，P_r为末端关节的随机位置，p_rj为末端关节的随机位置分量，d_rj为末端关节的随机姿态，P_r、p_rj及d_rj可从所述末端关节的随机坐标中获得。

Images