CN111538949B

CN111538949B - 冗余机器人逆运动学求解方法、装置和冗余机器人

Info

Publication number: CN111538949B
Application number: CN202010659812.2A
Authority: CN
Inventors: 王岳嵩; 黄荔群; 任晓雨; 赵明国; 熊友军
Original assignee: Shenzhen Ubtech Technology Co ltd
Current assignee: Shenzhen Ubtech Technology Co ltd
Priority date: 2020-07-10
Filing date: 2020-07-10
Publication date: 2020-10-16
Anticipated expiration: 2040-07-10
Also published as: CN111538949A; US11845186B2; US20220009096A1

Abstract

本发明实施例公开了一种冗余机器人逆运动学求解方法、装置和冗余机器人，该方法包括：根据预先规划的末端位姿获取该冗余机器人在当前构型下满足该末端位姿的雅克比矩阵零空间的表达式，以得到在当前构型的零空间中各关节角速度之间的关系式；基于正运动学算法构建该冗余机器人的能量函数，并根据各关节角速度之间的关系式对该能量函数进行能量最优求解以得到目标关节角，将该目标关节角作为满足该末端位姿的运动学逆解并发送至对应的关节电机。本发明的技术方案提出基于能量最优求解冗余机器人逆运动学，通过该方法计算得到的关节角运动学逆解符合生物体特性，理解直观，且末端轨迹经过不同象限时，各关节角能够保持连续和光滑等。

Description

冗余机器人逆运动学求解方法、装置和冗余机器人

技术领域

本发明涉及冗余机器人技术领域，尤其涉及一种冗余机器人逆运动学求解方法、装置和冗余机器人。

背景技术

冗余机器人逆运动学是指给定一个规划的机器人末端位姿，利用逆运动学算法求解一组关节角并发送给各关节电机进行运动控制的过程。由于冗余机器人逆运动学有无穷解的特点，如何高效求解它成为了一个广泛关注的问题。

目前，求解逆运动学的解法一般可分成两大类，即解析法和数值迭代法。对于解析法逆运动学，利用机器人的几何特征求解各关节角，通常需要添加一些额外的几何约束进行求解，比如规定臂型角的角度或者肘关节在某一平面内等。其优点是计算量小且是精确解，其缺点在于给出的解在无穷逆解中不是最优的，其次机器人在奇异点处无法直接求解，并且机械臂末端轨迹通过某些象限时，机器人关节角可能会出现跳变、翻转等情况。

对于数值迭代法逆运动学，其中最典型的是基于雅克比伪逆矩阵的速度级求解，通过求解关节角速度，然后通过积分得到关节角。使用雅克比矩阵的伪逆矩阵实际是在求解过程添加了一个约束，即使迭代过程中各关节角变化之和最小。对于该基于雅克比伪逆的数值迭代法的本质是根据末端点工作空间的误差，把它投影到每个关节上做一个简单的比例控制。这种迭代方法可以通过若干次迭代计算，收敛到无穷逆解中的一个。但这种计算方法的缺点是计算量较大，增益矩阵的设定需考虑系统算力，并且迭代出的结果不可控，有时机器人会出现抖动的情况，并且机器人关节角容易超出工作范围。

发明内容

有鉴于此，本发明目的是为了克服现有技术中的不足，提供一种冗余机器人逆运动学求解方法、装置和冗余机器人。

本发明实施例提供一种冗余机器人逆运动学求解方法，包括：

根据预先规划的末端位姿获取该冗余机器人在在满足所述末端位姿下的当前构型的雅克比矩阵零空间的表达式，以得到在所述当前构型的零空间中各关节角速度之间的关系式；

基于正运动学算法构建该冗余机器人的能量函数，并根据所述各关节角速度之间的关系式对所述能量函数进行能量最优求解，以得到目标关节角；

将所述目标关节角作为该冗余机器人满足所述末端位姿的运动学逆解并发送至对应的关节电机以进行关节控制。

在一种实施例中，在上述的冗余机器人逆运动学求解方法中，还包括：

判断所述运动学逆解是否满足预设条件；

若不满足，则对该冗余机器人进行所述关节控制后，计算下一构型的雅克比矩阵零空间的表达式，以得到在所述下一构型的零空间中各关节角速度之间的关系式；

根据所述下一构型对应的各关节角速度之间的关系式对所述能量函数进行能量最优求解，以得到满足所述末端位姿的下一运动学逆解并发送至所述关节电机以进行关节控制，返回所述判断步骤；

若满足，则停止执行所述下一构型的雅克比矩阵零空间的表达式的计算步骤。

在一种实施例中，在上述的冗余机器人逆运动学求解方法中，若该冗余机器人的自由度为n，当前构型为

，则该冗余机器人在满足相同末端位姿的构型

的集合为：

，于是，所述雅克比矩阵零空间的表达式满足：

，

；

其中，I _n是

的单位矩阵；p是

的任意向量；J为该冗余机器人在当前构型的雅克比矩阵；

为所述雅克比矩阵的伪逆矩阵；

表示各关节的角速度。

在一种实施例中，在上述的冗余机器人逆运动学求解方法中，所述“根据所述各关节角速度之间的关系式对所述能量函数进行能量最优求解”，包括：

根据各关节角速度之间的关系式将所述能量函数中各关节角变量基于同一个关节角变量进行描述；

然后对所述能量函数进行极值求解。

在一种实施例中，在上述的冗余机器人逆运动学求解方法中，所述能量函数包括重力势能函数、重力补偿函数或斥力势能函数。

在一种实施例中，在上述的冗余机器人逆运动学求解方法中，所述能量函数为所述重力势能函数，所述基于正运动学算法构建该冗余机器人的能量函数包括：

利用齐次变换矩阵计算该冗余机器人各关节的坐标系位姿，根据各关节的坐标系位姿计算对应关节的质心位置；

以世界坐标系作为势能零点，基于各关节质量和对应关节的所述质心位置构建该冗余机器人的重力势能函数。

在一种实施例中，在上述的冗余机器人逆运动学求解方法中，所述能量函数为所述重力补偿函数，所述基于正运动学算法构建该冗余机器人的能量函数包括：

利用齐次变换矩阵及链式法则计算该冗余机器人各关节的坐标系位姿，根据所述坐标系位姿计算对应关节的质心位置和旋转轴矢量，其中，所述坐标系位姿包括坐标系位置；

若该冗余机器人的关节数目为n，对于第i个关节，根据所述第i个关节的所述旋转轴矢量和第i+1个到第n个关节所产生的重力矩之和计算所述第i个关节所需的重力补偿力矩；其中，每一关节的所述重力矩根据对应关节的关节质量、所述坐标系位置和所述质心位置计算得到；

基于所有关节所需的所述重力补偿力矩之和构建该冗余机器人的重力补偿函数。

本发明另一实施例提出一种冗余机器人逆运动学求解装置，包括：

零空间求解模块，用于根据预先规划的末端位姿获取该冗余机器人在满足所述末端位姿下的当前构型的雅克比矩阵零空间的表达式，以得到在所述当前构型的零空间中各关节角速度之间的关系式；

能量函数构建模块，用于基于正运动学算法构建该冗余机器人的能量函数，并根据所述各关节角速度之间的关系式对所述能量函数进行能量最优求解，以得到目标关节角；

运动学逆解获取模块，用于将所述目标关节角作为该冗余机器人满足所述末端位姿的运动学逆解并发送至对应的关节电机以进行关节控制。

本发明又一实施例提出一种冗余机器人，基于上述的冗余机器人逆运动学求解方法进行关节控制。

本发明再一实施例提出一种计算机可读存储介质，其存储有计算机程序，在所述计算机程序被执行时，实施上述的冗余机器人逆运动学求解方法。

本发明的实施例具有如下优点：

本发明实施例的技术方案在冗余机器人的逆运动学求解过程中，以能量为评判标准并结合雅克比矩阵的零空间，通过构建能量函数并以能量最优作为目标，从无穷的运动学逆解中求解出一组目标关节角以用于关节运动控制。通过该方法计算得到的关节角运动学逆解符合生物体特性，理解直观，且末端轨迹经过不同象限时，各关节角能够保持连续和光滑等。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。

图1示出了本发明实施例的冗余机器人的模型示意图；

图2示出了本发明实施例的冗余机器人的相同末端位姿的构型变化示意图；

图3示出了本发明实施例的冗余机器人逆运动学求解方法的第一流程示意图；

图4示出了本发明实施例的冗余机器人逆运动学求解方法的第二流程示意图；

图5示出了本发明实施例的冗余机器人逆运动学求解方法的第三流程示意图；

图6示出了本发明实施例的冗余机器人逆运动学求解装置的结构示意图。

主要元件符号说明：

10-冗余机器人逆运动学求解装置；100-零空间求解模块；200-能量函数构建模块；300-运动学逆解获取模块。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

已知规划的机械臂末端位姿（包括位置和姿态）后，利用逆运动学可以求解得到一组关节角进行位姿控制。对于存在冗余度的机械臂，其零空间表现为无穷组关节角的机械臂构型均满足相同的末端位姿，进一步可以理解为，机械臂的具有关节角速度而末端速度始终为零。例如，图1示出了一个双-四自由度全旋转关节的冗余机器人的结构示意图。其中一个四自由度的机械臂的零空间示意图如图2所示。当该机械臂具有不同的关节角构型时，各关节的位姿不同，但末端始终具有相同的位姿。

为从无穷组解中求解得到一组较优的运动学逆解，本发明实施例的方法结合生物体如人在运动时通常都是以耗能最小、最舒服的方式运动的这一特征，即从能量的角度考虑，在满足条件的零空间中，选取能量最优的构型作为运动学逆解结果。该方法构思简单，不仅符合生物体的一般运动规律，且机械臂末端在工作空间跟随轨迹运动中，各关节角还具有连续光滑的特点等等。

下面结合具体的实施例进行该冗余机器人逆运动学求解方法的说明。

实施例1

请参照图3，本实施例提出一种冗余机器人逆运动学求解方法，结合冗余机器人的零空间并通过将能量作为评判标准，可以很好地改善现有技术中逆运动学求解方法的不足。

如图3所示，该冗余机器人逆运动学求解方法主要包括以下步骤：

步骤S10，根据预先规划的末端位姿获取该冗余机器人在满足该末端位姿下的当前构型的雅克比矩阵零空间的表达式，以得到在该当前构型的零空间中各关节角速度之间的关系式。

给定冗余机器人一个期望的末端位姿后，利用逆运动学求解得到一组关节角并发送给关节电机进行关节控制，使得该冗余机器人执行相应的运动。本实施例中，在给定一个已规划的末端位姿后，将求解构型的雅克比矩阵并结合雅克比矩阵的零空间（Nullspace）概念和能量函数，从而求解得到能量最优的一关节角构型以作为所需的运动学逆解。

对于上述步骤S10，示范性地，若该冗余机器人的自由度（等于关节数目）为n，当前构型为

，则该冗余机器人在满足相同的上述末端位姿的构型

的集合可记为：

。

其中，I _n是

的单位矩阵；p是

的任意向量；J为该冗余机器人在当前构型下的雅克比矩阵；

为该雅克比矩阵的伪逆矩阵。可以理解，算子

用于将任意向量p投影到雅克比矩阵的零空间，从而产生关节角速度

（即

）。通过对关节角速度进行积分，即可计算得到所需的关节角。

对于一个冗余机器人的机械臂，其末端速度

和关节角速度

，存在如下映射关系：

。于是，根据雅克比矩阵的零空间的定义可知，该雅克比矩阵零空间的表达式满足：

，即对于任意的关节角速度

，其末端速度始终为零。

雅克比矩阵是关于关节角的函数，同时雅克比矩阵也决定着其零空间的维数。通常在非奇异情形下，雅克比矩阵的零空间为一维，即自由变量的个数为一个，也就是说，使得末端速度为0的关节角速度

，其实是关于一个关节角速度变量的函数，即各关节角速度变量之间存在一种关系，使得其他的关节角速度均可通过同一个关节角速度来描述。由于关节角速度与关节角满足积分关系，于是，对于各关节角，可选择其中任意一个关节角来描述其他的关节角。

以图1所示的四自由度机械臂为例，若给定了一个末端的位置，为求解得到该机械臂的四个关节角（

），于是，根据上述公式可知，关节角速度

满足：

，其中，该雅克比矩阵J为3x4的矩阵，是关于上述四个关节角变量的函数。

由于该雅克比矩阵的零空间满足

，且该零空间为一维零空间（由于雅克比矩阵J为3x4的矩阵），此时的关节角速度

是关于一个关节角速度变量的函数。于是，可以从上述的四个关节角中任意选择一个关节角，如θ ₁来表示其他的关节角，根据获取的雅克比矩阵的零空间表达式，将各关节角速度之间的关系通过同一关节角变量描述如下：

；

；

，其中，k为实数；f ₁、f ₂和f ₃分别表示关于变量关节角θ ₁的三个函数。可以理解，上述的k每取一个值，将对应得到一组关节角，进而机器人存在一种构型与之对应，但末端的位置始终保持不变。

步骤S20，基于正运动学算法构建该冗余机器人的能量函数，并根据得到的各关节角速度之间的关系式对该能量函数进行能量最优求解以得到目标关节角。

利用各关节角来计算机器人各关节的位姿，属于正向运动学范畴。本实施例中，将能量作为评价标准，通过根据未知的各关节角基于正运动学算法来构建该冗余机器人的能量函数。可以理解，该能量函数中的能量可以是真实的物理势能，也可以是根据实际需要构建的虚拟能量。示范性地，该物理势能可包括但不限于为重力势能、重力补偿势能等；该构建的虚拟能量可包括但不限于为在相应关节处添加势场而产生的虚拟斥力势能等。

示范性地，在构建好能量函数后，由于该能量函数是关于各关节角的函数，可利用各关节角速度之间的关系式对该能量函数中的各关节角变量基于同一个关节角变量θ _i进行描述，其中i从1到n中进行任意取值。于是，将得到仅为关于该关节角变量θ _i的能量函数表达式。然后，对该能量函数进行能量最优化求解。示范性地，通过对该能量函数进行极值求解，即有：

。最后，将极值得到的结果带入上述的雅克比矩阵的零空间的表达式中，即可获得一组目标关节角。

可以理解，此时的目标关节角即为该能量最优时的一组关节角。所述的能量最优，可以是能量取最小值或最大值，具体根据实际的能量函数及运动目标来确定。例如，以该冗余机器人受到的重力势能为例，同生物体（比如人）的运动类似，往往总是期望以能量损耗最小且最舒服的方式进行运动，那么对于冗余机器人而言也是同理，此时的重力势能函数应当从最小重力势能为目标进行逆运动学求解。

步骤S30，将该目标关节角作为该冗余机器人满足相同该末端位姿的运动学逆解并发送至对应的关节电机以进行关节控制。

于是，将该组关节角作为使该冗余机器人保持相同的末端位姿的最优运动学逆解，并将其发送至对应的关节电机进行关节位置控制，从而实现所需的机器人运动。可以理解，当该冗余机器人根据关节角进行相应的关节控制后，其关节位姿将更新，即由当前构型切换为下一构型。

考虑到一次构型的切换可能还不能满足需求，例如，为进一步避障等，那么还需要求解下一组或多组的运动学逆解。进一步可选地，如图4所示，该冗余机器人逆运动学求解方法还包括：

步骤S40，判断所述运动学逆解是否满足预设条件。

示范性地，若上述步骤S30求解得到的运动学逆解未满足预设条件，则还需要求解下一组运动学逆解，即执行步骤S50-S60。反之，若判断出满足该预设条件，则执行步骤S70，即不再针对下一构型来计算下一组运动学逆解的操作。可以理解，上述预设条件可以根据实际需求来设定，如可设为在末端相同的前提下避开某一位置的障碍物，或者经过特定位置等。

步骤S50，计算下一构型的雅克比矩阵零空间的表达式，以得到在该下一构型的零空间中各关节角速度之间的关系式。

步骤S60，根据下一构型对应的各关节角速度之间的关系式对该能量函数进行能量最优求解，以得到满足上述末端位姿的下一运动学逆解并发送至各关节电机以进行关节控制。然后，返回所述判断步骤，即步骤S40。

步骤S70，不执行下一构型的雅克比矩阵零空间的表达式的计算操作。

由于在运动过程中不同时刻下的各关节角不同，而雅克比矩阵是一个关于关节角的线性变换，当下一时刻的关节角发生变化时，雅克比矩阵也将不同，故其对应的零空间也将改变。因此，若需要求解下一运动学逆解，在每进行一次关节控制后，则需要重新求解该冗余机器人在新的构型下的雅克比矩阵的零空间。

对于步骤S50-S60，如图2所示，假设当前该冗余机器人处于构型1，参见上述步骤S10-S30求解得到一组关节角并进行关节控制后，则该冗余机器人的各关节将进行关节位姿更新，使之处于构型2的状态，此时的雅克比矩阵也将发生改变，故需要计算该构型2对应的雅克比矩阵零空间的表达式，然后得到新的各关节角速度之间的关系。之后，仍以能量最优为目标，对于该能量函数进行下一组关节角求解，具体步骤陈参见上述步骤S10-S30，在此不再重复描述。随后，再次判断该下一运动学逆解是否满足要求，即返回步骤S40。若满足，则执行步骤S70；反之，重复上述步骤S50-S60，直到求解的运动学逆解能够满足该预设条件为止。可以理解，若期间末端的位置发生变化，则应停止上述的重复计算各关节角的过程。

在无穷的运动学逆解中，本实施例的冗余机器人逆运动学求解方法利用雅克比矩阵零空间，并以能量为评判标准，通过选取能量最优下的构型作为运动学逆解的结果，构思简单，直观且能够可在线计算，为冗余逆运动学的求解方式提供了新的思路。不仅如此，该方法符合生物体的一般运动规律，使得在靠近基座的关节运动较少，远端关节运动较多，即考虑以耗能最小的方式进行运动控制；并且，机械臂末端在工作空间实时跟随轨迹运动中，各关节角还具有连续光滑的特点，故可以很好地解决现有解决方案中存在跳变、翻转等不连续的情况。

实施例2

请参照图3，基于上述实施例1的方法，本实施例提出一种冗余机器人逆运动学求解方法，对于上述步骤S20中的基于正运动学算法构建冗余机器人的能量函数，示范性地，该能量函数可包括但不限于为重力势能函数、重力补偿函数、斥力势能函数等，具体可根据实际需求进行能量函数构建。

在一种实施例中，由于各关节在空间运动过程中会受到重力作用，故将产生重力势能。示范性地，该能量函数为重力势能函数。对于该重力势能函数的构建，如图5所示，主要包括以下子步骤：

子步骤S210，利用齐次变换矩阵及链式法则计算该冗余机器人各关节的坐标系位姿，根据各关节的坐标系位姿计算对应关节的质心位置。

两个坐标系之间的平移和旋转的关系可由齐次变换矩阵表述，对于多个坐标系之间的关系则可利用齐次变换矩阵的链式法则来确定。根据各关节角，通过齐次变换矩阵可以依次求解得到每个关节坐标系的位置和姿态（即坐标系位姿），这属于机器人的正向运动学。可以理解，上述子步骤S210中计算得到的各关节的坐标系位姿的表达式与各关节角相关，构建得到的能量函数也是关于各关节角变量的函数。

然后根据各关节的坐标系位姿，以及关节质心在自身关节坐标系的矢量，计算各关节质心在世界坐标系中的位置，即上述的关节质心位置。示范性地，若该冗余机器人的关节数目为n，对于第i个关节，i=1,…,n，通过正向运动学可得到：

，其中，

、

依次表示第i个关节坐标系的位置、第i个关节的旋转轴矢量和第i个关节的质心位置；

表示关于各个关节角变量的正向运动学函数，具体的正运动学计算过程可参见已有的相关文献，在此不再详述。

子步骤S220，以世界坐标系作为势能零点，基于各关节质量和对应关节的质心位置构建该冗余机器人的重力势能函数。

机器人的重力势能等于该机器人的总质量乘以重力加速度再乘以该机器人的质心位置，故该机器人的质心位置可通过对所有关节质心的加权平均计算得到。例如，每个关节质心的权重可等于对应关节的关节质量除以机器人的总质量。

示范性地，若第i个关节的质心位置为

，权重为

；其中，m _i为第i个关节的质量，该冗余机器人的质心位置

满足：

。于是，该重力势能函数G的表达式为：

；其中，M表示该冗余机器人的总质量，通过所有关节的质量相加得到；g为重力加速度。

然后，对该重力势能函数进行微分极值求解，即有：

。于是，将极值得到的结果带入上述的雅克比矩阵的零空间的表达式中，即可获得该冗余机器人在取得最小的重力势能的情况下的一组关节角。

在另一实施例中，该能量函数可构建为重力补偿函数，并通过选取最小的重力补偿对应的构型作为运动学逆解结果。

由于机械臂受到重力作用，在关节处将产生重力矩，且该重力矩随关节角的变化而变化。在运动控制中，通常把重力矩当成一种非线性扰动，通过前馈控制将机器人的重力矩在各关节进行直接补偿掉，这样在推导控制算法中就不需要考虑重力项，便于将误差解耦，从而简单化对机器人的设计控制。

若该能量函数为重力补偿函数，对于上述步骤S20中的基于正向运动学算法构建该冗余机器人的能量函数，示范性地，可利用齐次变换矩阵及链式法则计算得到各关节的坐标系位姿，其中，该关节的坐标系位姿是关于各关节角的表达式。然后利用该坐标系位姿计算出对应关节的质心位置和旋转轴矢量，具体可参见上述子步骤S210，在此不再重复描述。

示范性地，若该冗余机器人的关节数目为n，对于第i个关节电机，根据第i个关节的旋转轴矢量和第i+1个到第n个关节所产生的重力矩之和计算第i个关节所需的重力补偿力矩，即第i个关节所需的重力补偿力矩

的表达式为：

；

其中，

为第i个关节的旋转轴矢量；

表示第j个关节受到的重力，m _j表示第j个关节的质量，

表示重力加速度矢量；

表示第j个关节的质心位置，即第j个关节质心在世界坐标系的位置；

表示第i个关节的坐标系位置，即第i个关节坐标系在世界坐标系的位置，这些位置均可以通过正向运动学计算得到。可以理解，某一个关节所产生的重力矩根据该关节受到的重力以及该关节的质心位置到坐标系位置之间的距离计算得到。

其中，对于每个关节，其所需补偿的力矩等于该关节之后所有关节产生的重力矩在它旋转轴方向的投影。例如，若冗余机器人的关节数目为3，则对于第2个关节而言，所需的重力补偿力矩应为第1个关节所产生的重力矩在该第2个关节的旋转轴方向上的投影。又例如，对于第3个关节而言，所需的重力补偿力矩应为第1个关节和第2个关节所产生的重力矩之和在该第3个关节的旋转轴方向上的投影。当然，若冗余机器人包含更多的关节，则以此类推，可计算每个关节所需的重力补偿力矩。

本实施例中，该冗余机器人所需的重力补偿为各关节的重力补偿力矩之和。示范性地，对于该冗余机器人的重力补偿函数，即为：

，

。

与上述的重力势能函数类似，由于该重力补偿函数同样仅存在一个关节角θ _i变量。然后，对该重力势能函数进行微分及极值求解，即有：

。于是，将极值得到的结果带入上述的雅克比矩阵的零空间的表达式中，即可获得该冗余机器人在取得最小重力补偿下的一组关节角。将该组关节角作为该冗余机器人在相同末端位姿下的运动学逆解，将其发送至对应的关节电机以控制该冗余机器人的运动。

在另一种实施例中，该能量函数还可以是虚拟的斥力势能等。示范性地，可通过在各关节添加人工势场，并计算出该冗余机器人在当前位置下周围障碍物等对其产生的斥力势能的表达式等，具体可根据实际需求进行势场添加。同理，根据求解得到的各关节角之间的关系式并通过对该斥力势能函数进行最优化求解，从而获得该冗余机器人在取得斥力势能最优时的关节角。

实施例3

请参照图6，基于上述实施例1的方法，本实施例提出一种冗余机器人逆运动学求解装置10，包括：

零空间求解模块100，用于根据预先规划的末端位姿获取该冗余机器人在满足所述末端位姿下的当前构型的雅克比矩阵零空间的表达式，以得到在所述当前构型的零空间中各关节角速度之间的关系式。

能量函数构建模块200，用于基于正运动学算法构建该冗余机器人的能量函数，并根据所述各关节角速度之间的关系式对所述能量函数进行能量最优求解，以得到目标关节角。

运动学逆解获取模块300，用于将所述目标关节角作为该冗余机器人满足所述末端位姿的运动学逆解并发送至对应的关节电机以进行关节控制。

可以理解，本实施例的装置对应于上述实施例1或2的方法，上述实施例1或2的可选项同样适用于本实施例，故在此不再详述。

本发明的另一实施例还提出一种冗余机器人，其采用上述的冗余机器人逆运动学求解方法进行逆运动学分析求解以进行关节运动控制。

本发明的又一实施例还提供了一种计算机可读存储介质，其存储有计算机程序，在所述计算机程序被实施时，执行上述的冗余机器人逆运动学求解方法。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是智能手机、个人计算机、服务器、或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。