机器人关节位姿优化方法、机器人控制方法和机器人与流程

1.本技术涉及机器人控制技术领域，尤其涉及一种机器人关节位姿优化方法、机器人控制方法和机器人。


背景技术：

2.机器人一般具备多个自由度，通过多个自由度的协调来完成各种任务，为了增大机器人的运动范围和灵活度，很多机器人添加了冗余关节，但对于冗余关节的处理一般都需要非常复杂的逻辑处理和分类计算，这给机器人的控制带来了很多挑战。例如，在具有冗余自由度的机械臂或多足机器人的运动规划中，有的采用如牛顿法等方法来计算运动规划中的运动学逆解，不仅计算时间较长，且计算难度较高，这就导致运动规划的总时间较长、规划效率较低等。


技术实现要素：

3.本技术实施例提供一种机器人关节位姿优化方法、机器人控制方法和机器人，该寻优方法采用双层优化的思想来处理关节冗余，可以求解得到较优的关节角度，以保证优先完成机器人关节在笛卡尔空间的速度任务，还使得各关节角度具备较大的活动裕度。
4.本技术的实施例提供一种机器人关节位姿优化方法，包括：
5.构建以关节角速度为优化变量、机器人末端速度为控制目标的第一优化函数，所述第一优化函数包括所述关节角速度满足的不等式约束和含松弛变量的等式约束；
6.构建以关节角速度为优化变量、所述机器人的关节角度远离关节限位为控制目标的第二优化函数，所述第二优化函数包括所述关节角速度满足的含相同松弛变量的约束条件；
7.根据所述第一优化函数求解得到所述松弛变量的解，并根据所述松弛变量的解及所述第二优化函数得到最优的关节角速度；
8.根据所述最优的关节角速度得出所述机器人的最优关节角度，并根据所述机器人的最优关节角度优化所述机器人关节的位姿。
9.在一些实施例中，所述关节角速度满足的所述不等式约束通过如下步骤获取，包括：
10.构建所述机器人各个关节在对应控制周期满足的关节角度约束，并将所述关节角度约束转换为以关节角速度为变量的第一约束条件；
11.构建各个关节在所述对应控制周期满足的关节角加速度约束，并将所述关节角加速度约束转换为以关节角速度为变量的第二约束条件；
12.基于所述第一约束条件、所述第二约束条件和所述关节角速度的自身限位，构建得到所述关节角速度的合成约束以作为所述不等式约束。
13.在一些实施例中，所述第一约束条件为：
[0014][0015]
其中，和分别为第i个关节角度的上限和下限；为t时刻的第i个关节角度；为第i个关节的关节角速度；t为所述机器人的控制指令周期。
[0016]
在一些实施例中，所述第二约束条件为：
[0017][0018]
其中，和分别为第i个关节角加速度的上限和下限；和分别为t
‑
1时刻和t时刻的第i个关节角速度。
[0019]
在一些实施例中，所述第一优化函数以松弛变量的平方为优化指标；
[0020]
所述第一优化函数的表达式为：
[0021][0022]
其中，w为松弛变量，j为所述机器人的速度雅可比矩阵，为所述机器人所有关节的关节角速度向量；为所述机器人的末端速度向量；和分别为第i个关节角速度的上限和下限。
[0023]
在一些实施例中，所述第二优化函数以关节角度与关节限位的欧式距离为优化指标；
[0024]
所述欧式距离的计算公式为：
[0025][0026]
其中，
[0027]
其中，q为所述欧式距离；和分别为第i个关节角度的上限和下限；为t时刻的第i个关节角度。
[0028]
在一些实施例中，所述第二优化函数的所述含相同松弛变量的约束条件与所述第一优化函数的所述等式约束相同，所述第二优化函数还包括所述关节角速度满足的所述不等式约束；
[0029]
所述第二优化函数的表达式为：
[0030]
[0031]
其中，其中，为t
‑
1时刻的第i个关节角度；t为所述机器人的控制指令周期；w为所述松弛变量，j为所述机器人的速度雅可比矩阵，为所述机器人所有关节的关节角速度向量；为所述机器人的末端速度向量；和分别为第i个关节角度的上限和下限；和分别为第i个关节角速度的上限和下限；和分别为第i个关节角加速度的上限和下限；和分别为t
‑
1时刻和t时刻的第i个关节角速度。
[0032]
本技术的实施例还提供一种机器人控制方法，包括：
[0033]
获取机器人在当前控制指令周期的与关节角速度相关的状态参数；
[0034]
根据所述状态参数利用上述的机器人关节位姿优化方法来优化下一控制指令周期的关节位姿；
[0035]
根据所述优化的关节位姿控制所述机器人执行相应操作。
[0036]
本技术的实施例还提供一种机器人，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的机器人关节位姿优化方法或机器人控制方法。
[0037]
本技术的实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序被处理器执行时，实施上述的机器人关节位姿优化方法或机器人控制方法。
[0038]
本技术的实施例具有如下有益效果：
[0039]
本技术实施例的机器人关节位姿优化方法采用双层优化的思想来处理关节冗余，通过构建以关节角速度为优化变量、机器人末端速度为控制目标的第一优化函数，以及以关节角度远离关节限位为控制目标的第二优化函数，对这两个优化函数引入相同的松弛变量并添加相应的约束条件，并联合这两个优化函数进行关节角度最优求解，可以保证求解到的解能够优先完成笛卡尔空间任务，并在此基础上给各个关节角度留有足够的余量，从而远离关节限位。此外，该关节位姿优化方法除了适用于转动关节机器人的关节角度优化外，同样适用于平动关节及平动
‑
转动混合的机器人的关节位置优化，具备较强的普适性。
附图说明
[0040]
为了更清楚地说明本技术实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本技术的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
[0041]
图1所示为本技术实施例机器人关节位姿优化方法的第一流程示意图；
[0042]
图2所示为本技术实施例机器人关节位姿优化方法的第二流程示意图；
[0043]
图3所示为本技术实施例机器人控制方法的流程示意图；
[0044]
图4所示为本技术实施例机器人关节位姿优化装置的结构示意图。
具体实施方式
[0045]
下面将结合本技术实施例中附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。
[0046]
通常在此处附图中描述和示出的本技术实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本技术的实施例的详细描述并非旨在限制要求保护的本技术的范围，而是仅仅表示本技术的选定实施例。基于本技术的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本技术保护的范围。
[0047]
二次规划问题主要是在满足等式及不等式约束条件下，从多个解中选取最优解的过程。示范性地，二次规划问题的主要形式如下：
[0048][0049]
其中，为优化指标，h为海森矩阵；x为n维优化变量；f为行向量；a
eq
·
x＝b
eq
为等式约束，a
eq
为mxn(m≤m)维矩阵，b
eq
为m行列向量；ax≤b为不等式约束，a为n列矩阵，b为列向量。
[0050]
对于机器人来说，常常通过添加冗余关节来增大机器人的运动范围和灵活度等。现有的对冗余关节的处理一般非常复杂且计算时间长，在实际使用中存在一定的限制。对此，考虑到机器人自身结构的特性，其往往在不同运用场合下会存在关节角度、关节角速度、关节力矩等多种限制，为求解得到能够用于执行相应任务且满足对应多种限制的最优运动学逆解，本技术实施例将利用二次规划问题来进行优化求解，具体而言，通过构建双层优化指标(即双层二次优化问题)来进行关节角度的优化求解。
[0051]
本技术实施例的机器人关节位姿优化方法通过构建以关节角速度为优化变量、机器人末端速度为控制目标的第一优化函数，以及以关节角速度为优化变量、关节角度远离关节限位为控制目标的第二优化函数，并对这两个优化函数引入相同的松弛变量并添加相应的约束条件，通过联合这两个优化函数进行关节角度最优求解，可以保证求解到的解能够优先完成笛卡尔空间的任务，并在此基础上使得各个关节尽量远离关节限位，达到关节位姿优化目的。下面结合实施例对该机器人关节位姿优化方法进行说明。
[0052]
实施例1
[0053]
图1所示为本技术实施例机器人关节位姿优化方法的第一流程示意图。
[0054]
示范性地，该机器人关节位姿优化方法包括：
[0055]
步骤s110，构建以关节角速度为优化变量、机器人末端速度为控制目标的第一优化函数，该第一优化函数包括关节角速度满足的不等式约束和含松弛变量的等式约束。
[0056]
为了保证机器人的末端能够优先执行笛卡尔空间中的任务，这里以关节角速度为控制变量，以末端速度为控制目标来构建出一个二次规划问题，即上述的第一优化函数。可以理解，该第一优化函数中的“第一”主要用于与后文提及的其他优化函数进行区分。
[0057]
示范性地，该第一优化函数将引入一松弛变量，并基于该松弛变量得到待优化的
指标。例如，在一种实施方式中，该优化指标可为松弛变量的平方等，当然也可以取与松弛变量有关的其他形式，这里不作限定。
[0058]
同时，该第一优化函数还将添加有相应的约束条件，其主要包括关节角速度满足的含松弛变量的等式约束、以及与关节角速度直接或间接相关的不等式约束，以保证求解出的关节角速度最优解能够使得在各个关节约束条件下达到末端的速度指标。
[0059]
由于机器人的速度雅克比矩阵可以反映各个关节角速度与末端速度之间的关系，在引入松弛变量的情况下，这里将构建关节角速度、松弛变量与末端速度三者之间满足的等式约束方程。
[0060]
对于第一优化函数的不等式约束，例如，可包括但不限于包括关节角度约束、关节角速度自身物理约束及关节角加速度约束等。
[0061]
示范性地，如图2所示，该关节角速度满足的不等式约束可通过如下步骤得到，包括：
[0062]
步骤s210，构建机器人各个关节在对应控制周期满足的关节角度约束，并将该关节角度约束转换为以关节角速度为变量的第一约束条件。
[0063]
由于机器人关节结构的限制而使得一些关节位置或角度不可达，即存在关节限位，本实施例将先构建机器人各个关节角度满足的关节限位约束，进而转换得到对应的关节角速度约束。
[0064]
应当明白的是，对于平动关节类型的机器人，由于不存在转动，故上述的关节限位主要是指对关节位置的限位；而对于转动关节类型的机器人，上述的关节限位主要是指对关节角度的限位。
[0065]
以转动关节类型的机器人为例，假设关节角度的上限为下限为机器人的控制指令周期为t，为t时刻的第i个关节角度，为第i个关节的关节角速度，则t+1时刻的关节角度为：
[0066][0067]
对应地，根据关节角度约束，则有：
[0068][0069]
于是，可得到转换后的以关节角速度为变量的第一约束方程为：
[0070][0071]
可以理解，对于平动关节机器人，可将上述对应关节的关节角度及关节角速度适应性地替换为平移位置和平移速度，原理类似，故不作展开描述。对于平动
‑
转动混合运动的机器人，同样也适用。
[0072]
步骤s220，构建各个关节在对应控制周期满足的关节角加速度约束，并将该关节角加速度约束转换为以关节角速度为变量的第二约束条件。
[0073]
由于机器人的单个关节的输出力矩是受限的，相应地将对关节的角加速度进行了自身限制，同样，本实施例将先构建机器人各个关节角加速度满足的约束，进而转换得到对
应的关节角速度约束。
[0074]
仍以转动关节类型的机器人为例，假设关节角加速度的上限为下限为t时刻的第i个关节角速度为则t+1时刻的关节角速度为：
[0075][0076]
对应地，根据关节角加速度约束，则有：
[0077][0078]
于是，可得到转换后的以关节角速度为变量的第二约束方程为：
[0079][0080]
其中，为t
‑
1时刻的第i个关节角速度。
[0081]
步骤s230，基于该第一约束条件、该第二约束条件和该关节角速度的自身约束，构建得到该关节角速度的合成约束以作为上述的不等式约束。
[0082]
机器人同样存在关节角速度的约束，假设第i个关节角速度的上限为下限为于是，关节角速度的自身约束可表示为：
[0083][0084]
由于同一t时刻的各个关节角速度需要同时满足多种限制，这里为了方便求解，将联合上述添加的约束方程及自身限制来得到该关节角速度的约束合成。示范性地，以添加关节角度、关节角速度及关节角加速度这三种约束为例，则有：
[0085][0086]
于是，该合成约束通过一个不等式约束来描述，则有：
[0087][0088]
可以理解，利用上述的关节角度(或关节位置)约束、关节角速度约束及关节角加速度约束来得到关节角速度的合成不等式约束，仅为一种添加不等式的示例，在实际运用中，还可以添加更多的约束条件，这里并不作限定。
[0089]
对于该第一优化函数，若以将松弛变量的平方作为优化指标，并构建得到相应的等式约束方程和不等式约束，此时第一优化函数可写成如下形式：
[0090]
通过对该优化函数进行最优求解，即可以得到在关节限位、关节角速度的自身约
束及关节角加速度的约束下，使得机器人末端速度为的关于关节角速度及松弛变量w的解。
[0091]
可以理解，以松弛变量为优化指标，可以保证即便该第一优化函数在约束条件存在冲突的情况下，依然会求解出该约束方程的接近解，进而可用于下一步处理；否则，如果上述约束条件本身存在冲突，如等式约束无解等，则将导致整个二次规划的寻优方案求解出错。
[0092]
步骤s120，构建以关节角速度为优化变量、机器人的关节角度远离关节限位为控制目标的第二优化函数，该第二优化函数包括关节角速度满足的含相同松弛变量的约束条件。
[0093]
为了解决关节冗余的问题，本实施例将以各关节角度远离关节限位为目标，来构建另一个二次规划问题，即上述的第二优化函数。例如，在一种实施方式中，该第二优化函数以关节角度与关节限位的欧式距离为优化指标，当然，除了欧式距离，也可以采用其他的能够反映关节角度距离关节限位的计算公式，在此并不作限定。
[0094]
示范性地，该欧式距离的计算公式为：
[0095][0096]
其中，
[0097]
其中，q为欧式距离；和分别为第i个关节角度的上限和下限；为t时刻的第i个关节角度。
[0098]
本实施例利用双层二次规划问题来综合求解出一个最优关节角速度，本实施例中，为使得最终求解出的关节角速度能够在保证笛卡尔空间控制末端速度的前提下，进一步使得各个关节角度逐步趋近于最优解，则该第二优化函数的关节角速度同样需要添加相应的约束条件。
[0099]
例如，在一种实施方式中，基于上述第一优化函数的约束条件，该第二优化函数的关节角速度可添加含同一松弛变量的等式约束，以及与关节角速度相关的不等式约束。
[0100]
对于该第二优化函数，若以各个关节角度距离关节限位最远为优化指标为例，并添加与第一优化函数具有相同的等式约束方程和不等式约束，此时第二优化函数可写成如下形式：
[0101][0102]
其中，其中，为t
‑
1时刻的第i个关节角度。
[0103]
可以理解，第二优化函数的约束条件设置与第一优化函数相同时，可以保证在满足关节限位、关节角速度的自身约束以及关节角加速度的约束下，第一优化函数求解得到
的解能够满足末端速度的需求，即实现优先执行速度任务，与此同时，还能保证使得各个关节角度具有最大的活动裕度，从而达到关节位姿优化目的。
[0104]
步骤s130，根据第一优化函数求解得到松弛变量的解，并根据该松弛变量的解及该第二优化函数得到最优的关节角速度。
[0105]
示范性地，基于第一优化函数和第二优化函数，可利用开源的求解器先对第一优化函数进行最优求解，以得到关节角速度及松弛变量的解；进而，利用求解出的松弛变量的解对第二优化函数进行最优求解，此时可得到最优的关节角速度，即为所需的关节角速度。
[0106]
步骤s140，根据该最优的关节角速度得出机器人的最优关节角度，并根据该机器人的最优关节角度优化机器人关节的位姿。
[0107]
示范性地，由于机器人的关节角速度与关节角度存在积分关系，在求解出最优的关节角速度后，即可通过积分运算来得到最优的关节角度。进而，可根据该最优的关节角度调整机器人关节的位姿(如角度或位置)，以保证机器人能够执行相应的任务操作。
[0108]
本实施例的机器人关节位姿优化方法通过双层优化的思想来处理关节冗余的问题，通过构建以关节角速度为优化变量、机器人末端速度为控制目标且引入松弛变量的第一优化函数，以及以关节角度远离关节限位为控制目标且引入相同的松弛变量的第二优化函数，并对这两个优化函数添加各个关节的角度、角速度、角加速度等合成的约束条件，其中，第二优化函数是在第一优化函数满足的关节角度约束范围内作进一步关节角速度求解的，因此可以实现在各种关节约束条件下，优先完成笛卡尔空间的速度任务，并在此基础上给各个关节角度留有足够的余量，即具备较大的活动裕度。此外，该方法具有通用性，可用于实现转动关节、平动关节及平动
‑
转动混合运动等各类机器人的关节位置或关节角度的自动寻优。
[0109]
实施例2
[0110]
请参照图3，基于上述实施例1的方法，本实施例提出一种机器人控制方法，例如，该机器人可以是转动关节类型的机器人，也可以是平动关节类型的机器人，还可以是平动
‑
转动混合运动的机器人等，这里不作限定。本技术中，所述转动关节类型的机器人是指机器人通过输出旋转角度的舵机，驱动与舵机相连的机器人关节实现对应转动角度的运动。所述平动关节类型的机器人是指机器人通过输出直线位移的舵机，驱动与舵机相连的机器人关节实现对应平移角度的运动。
[0111]
本实施例中，该机器人具有冗余关节，通过协调这些冗余关节可以完成各种任务。示范性地，如图3所示，该机器人控制方法包括：
[0112]
步骤s310，获取机器人在当前控制指令周期的与关节角速度相关的状态参数。
[0113]
步骤s320，根据所述状态参数利用机器人关节位姿优化方法来优化下一控制指令周期的关节位姿。
[0114]
示范性地，可根据构建的第一优化函数和第二优化函数来确定具体需要获取哪些状态参数，这些状态参数可以是通过传感器等直接采集得到的，也可以是通过采集得到的数据经过运算处理后得到的，这里并不作限定。
[0115]
例如，以上述的第一优化函数为例，可获取到对应控制指令周期的关节角度、期望的末端速度以及雅克比矩阵等，然后将已知的量代入优化函数以用于对关节角速度这一优化变量及松弛变量进行求解，第二优化函数同理。
[0116]
可以理解，关于本实施例所采用的机器人关节位姿优化方法，具体参见上述实施例1的描述，这里不再重复描述。
[0117]
步骤s330，根据所述优化的关节位姿控制机器人执行相应操作。
[0118]
示范性地，在求解出机器人下一控制指令周期所需的关节角度或关节位置后，则将这些关节角度或关节位置作为指令发送到对应的关节电机，以使得机器人执行相应操作，例如，可以是机器人避障任务、人机交互任务等。
[0119]
本实施例的机器人控制方法通过采用的机器人关节位姿优化方法来进行关节位姿的自动寻优，可以使机器人关节位置在保证笛卡尔空间位姿的前提下，始终具备最大的活动裕度等。
[0120]
实施例3
[0121]
请参照图4，基于上述实施例1的方法，本实施例提出一种机器人关节位姿优化装置100，示范性地，该机器人关节位姿优化装置100包括：
[0122]
构建模块110，用于构建以关节角速度为优化变量、机器人末端速度为控制目标的第一优化函数，所述第一优化函数包括所述关节角速度满足的不等式约束和含松弛变量的等式约束。
[0123]
构建模块110，还用于构建以关节角速度为优化变量、所述机器人的关节角度远离关节限位为控制目标的第二优化函数，所述第二优化函数包括所述关节角速度满足的含相同松弛变量的约束条件。
[0124]
求解模块120，用于根据所述第一优化函数求解得到所述松弛变量的解，并根据所述松弛变量的解及所述第二优化函数得到最优的关节角速度。
[0125]
求解模块120还用于根据所述最优的关节角速度得出所述机器人的最优关节角度。
[0126]
优化模块130，用于根据所述机器人的最优关节角度优化所述机器人关节的位姿。
[0127]
可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1中的可选项同样适用于本实施例，故在此不再重复描述。
[0128]
本技术还提供了一种机器人，示范性地，该机器人包括处理器和存储器，其中，存储器存储有计算机程序，处理器通过运行所述计算机程序，从而使机器人执行上述的机器人关节位姿优化方法或者上述机器人关节位姿优化装置中的各个模块的功能。
[0129]
本技术还提供了一种可读存储介质，用于储存上述机器人中使用的所述计算机程序。
[0130]
在本技术所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本技术的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用
的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0131]
另外，在本技术各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。
[0132]
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本技术的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本技术各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read
‑
only memory)、随机存取存储器(ram，random access memory)、磁碟或者光盘等各种可以存储程序代码的介质。
[0133]
以上所述，仅为本技术的具体实施方式，但本技术的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本技术揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本技术的保护范围之内。