[发明专利]机械臂关节角度的确定方法、装置、存储介质及电子设备

权利要求书

1.一种机械臂关节角度的确定方法，其特征在于，包括：

获取目标机械臂各关节的长度信息和各关节的当前角度信息；

根据所述各关节的长度信息和所述各关节的当前角度信息，确定所述目标机械臂末端关节的当前位置信息，以及所述目标机械臂的雅可比矩阵；

根据预设的关节角度优化条件，构建关节角度代价函数；

利用所述末端关节的当前位置信息、所述雅可比矩阵和所述关节角度代价函数，确定关节角度增量信息；

利用所述关节角度增量信息更新所述各关节的当前角度信息，并重新执行所述确定所述目标机械臂末端关节的当前位置信息的步骤，直至所述末端关节的位置信息与目标位置一致。

2.根据权利要求1所述的确定方法，其特征在于，所述利用所述末端关节的当前位置信息、所述雅可比矩阵和所述关节角度代价函数，确定关节角度增量信息，包括：

根据所述末端关节的当前位置信息和上一次确定的所述末端关节的位置信息，确定所述末端关节的位置增量信息；

根据所述关节角度代价函数，确定关节角度的优化梯度方向；

根据所述位置增量信息、所述优化梯度方向和所述雅可比矩阵，确定关节角度增量信息。

3.根据权利要求2所述的确定方法，其特征在于，所述根据所述位置增量信息、所述优化梯度方向和所述雅可比矩阵，确定关节角度增量信息，包括：

通过以下公式，确定关节角度增量信息：

δq＝J⁺δp+(I-J⁺J)η

其中，δq表征所述关节角度增量信息，J表征所述雅可比矩阵、J⁺表征所述雅可比矩阵的伪逆矩阵，I表征所述雅可比矩阵的单位矩阵，η表征所述优化梯度方向，δp表征所述末端关节的位置增量信息。

4.根据权利要求1-3中任一项所述的确定方法，其特征在于，所述关节角度优化条件为目标关节的角度靠近所述目标关节的限位中心；

所述根据预设的关节角度优化条件，构建关节角度代价函数，包括：

根据所述目标关节的限位中心和所述目标关节的当前角度信息，构建关节角度代价函数。

5.根据权利要求4所述的确定方法，其特征在于，所述目标关节为所述目标机械臂关节中的至少一个关节。

6.根据权利要求1-3中任一项所述的确定方法，其特征在于，所述目标机械臂末端关节的当前位置信息通过以下方式确定：

针对目标机械臂的每一关节，根据该关节的长度信息和该关节的当前角度信息，构建该关节在其父级坐标系下的坐标系；

根据每一关节在其父级坐标系下的坐标系，确定所述目标机械臂末端关节在世界坐标系下的坐标系；

根据所述末端关节在世界坐标系下的坐标系和所述末端关节的长度信息，确定所述末端关节的当前位置信息。

7.根据权利要求1-3中任一项所述的确定方法，其特征在于，所述目标机械臂的雅可比矩阵通过以下方式确定：

根据各关节的当前角度信息，构建当前关节角度矩阵；

将所述末端关节的当前位置信息对所述当前关节角度矩阵的偏微分，确定为所述目标机械臂的雅可比矩阵。

8.一种机械臂关节角度的确定装置，其特征在于，包括：

获取模块，用于获取目标机械臂各关节的长度信息和各关节的当前角度信息；

第一确定模块，用于根据所述各关节的长度信息和所述各关节的当前角度信息，确定所述目标机械臂末端关节的当前位置信息，以及所述目标机械臂的雅可比矩阵；

构建模块，用于根据预设的关节角度优化条件，构建关节角度代价函数；

第二确定模块，用于利用所述末端关节的当前位置信息、所述雅可比矩阵和所述关节角度代价函数，确定关节角度增量信息；

执行模块，用于利用所述关节角度增量信息更新所述各关节的当前角度信息，并重新执行所述确定所述目标机械臂末端关节的当前位置信息的步骤，直至所述末端关节的位置信息与目标位置一致。