[发明专利]机器人关节位姿优化方法、机器人控制方法和机器人

权利要求书

1.一种机器人关节位姿优化方法，其特征在于，包括：

构建以关节角速度为优化变量、机器人末端速度为控制目标的第一优化函数，所述第一优化函数包括所述关节角速度满足的不等式约束和含松弛变量的等式约束；

构建以关节角速度为优化变量、所述机器人的关节角度远离关节限位为控制目标的第二优化函数，所述第二优化函数包括所述关节角速度满足的含相同松弛变量的约束条件；

根据所述第一优化函数求解得到所述松弛变量的解，并根据所述松弛变量的解及所述第二优化函数得到最优的关节角速度；

根据所述最优的关节角速度得出所述机器人的最优关节角度，并根据所述机器人的最优关节角度优化所述机器人关节的位姿。

2.根据权利要求1所述的机器人关节位姿优化方法，其特征在于，所述关节角速度满足的所述不等式约束通过如下步骤获取，包括：

构建所述机器人各个关节在对应控制周期满足的关节角度约束，并将所述关节角度约束转换为以关节角速度为变量的第一约束条件；

构建各个关节在所述对应控制周期满足的关节角加速度约束，并将所述关节角加速度约束转换为以关节角速度为变量的第二约束条件；

基于所述第一约束条件、所述第二约束条件和所述关节角速度的自身限位，构建得到所述关节角速度的合成约束以作为所述不等式约束。

3.根据权利要求2所述的机器人关节位姿优化方法，其特征在于，所述第一约束条件为：

其中，和分别为第i个关节角度的上限和下限；为t时刻的第i个关节角度；为第i个关节的关节角速度；T为所述机器人的控制指令周期。

4.根据权利要求3所述的机器人关节位姿优化方法，其特征在于，所述第二约束条件为：

其中，和分别为第i个关节角加速度的上限和下限；和分别为t-1时刻和t时刻的第i个关节角速度。

5.根据权利要求4所述的机器人关节位姿优化方法，其特征在于，所述第一优化函数以松弛变量的平方为优化指标；

所述第一优化函数的表达式为：

其中，w为松弛变量，J为所述机器人的速度雅可比矩阵，为所述机器人所有关节的关节角速度向量；为所述机器人的末端速度向量；和分别为第i个关节角速度的上限和下限。

6.根据权利要求1所述的机器人关节位姿优化方法，其特征在于，所述第二优化函数以关节角度与关节限位的欧式距离为优化指标；

所述欧式距离的计算公式为：

其中，q为所述欧式距离；和分别为第i个关节角度的上限和下限；为t时刻的第i个关节角度。

7.根据权利要求6所述的机器人关节位姿优化方法，其特征在于，所述第二优化函数的所述含相同松弛变量的约束条件与所述第一优化函数的所述等式约束相同，所述第二优化函数还包括所述关节角速度满足的所述不等式约束；

所述第二优化函数的表达式为：

其中，为t-1时刻的第i个关节角度；T为所述机器人的控制指令周期；w为所述松弛变量，J为所述机器人的速度雅可比矩阵，为所述机器人所有关节的关节角速度向量；为所述机器人的末端速度向量；和分别为第i个关节角度的上限和下限；和分别为第i个关节角速度的上限和下限；和分别为第i个关节角加速度的上限和下限；和分别为t-1时刻和t时刻的第i个关节角速度。

8.一种机器人控制方法，其特征在于，包括：

获取机器人在当前控制指令周期的与关节角速度相关的状态参数；

根据所述状态参数利用如权利要求1至7中任一项所述的机器人关节位姿优化方法来优化下一控制指令周期的关节位姿；

根据所述优化的关节位姿控制所述机器人执行相应操作。

9.一种机器人，其特征在于，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-7中任一项所述的机器人关节位姿优化方法或权利要求8所述的机器人控制方法。

10.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序被处理器执行时，实施根据权利要求1-7中任一项所述的机器人关节位姿优化方法或权利要求8所述的机器人控制方法。