[发明专利]一种串联六自由度机械臂逆运动学解算方法及介质

说明书

一种串联六自由度机械臂逆运动学解算方法及介质，属于空间机器人技术领域。本发明包括：对于6自由度机械臂，通过位姿转换模块将输入的6x1末端期望位姿转换为4x4齐次变换矩阵Tdes，通过前向动力学模块将6x1的关节初值转换为4x4齐次变换矩阵T_temp，基于牛顿迭代法将T_temp与Tdes构建为非线性方程组，通过偏导数求取模块求取该非线性方程组对应的雅克比矩阵，并对此非线性方程组与其雅克比矩阵进行排列组合，使用伪逆法求降维后的方程组的解，并对初值进行迭代更新直至所求关节角对应的非线性方程组的误差小于设定值，使用关节角筛选模块剔除排列组合导致的重复解。

技术领域

本发明涉及一种串联六自由度机械臂逆运动学解算方法及介质，属于空间机器人领域。

背景技术

机械臂逆运动学解算问题为已知目标位姿求解对应机械臂关节角，使得在该组关节角下机械臂末端位姿与目标位姿重合。机械臂逆运动学解算方法主要有：数值解法与解析解法。解析解法一般将机械臂的空间几何参数分解为平面几何参数，结合机械臂的数值参数求取对应关节角；数值解法使用机械臂数值参数与目标姿态信息求解对应关节角，所得关节角与初值选取关系较大，当初值选取不当时收敛较慢，或存在无解情况。6(7)自由度串联机械臂由于构型多样，现有解析解法在逆运动学求解过程中需要针对对应构型进行公式推导，所得方法不具有普适性；现有数值解法对应一组初值只能求解一组关节角。申请号201510848596，用于六自由度的工业机器人的逆运动学求解方法从解析解入手，但与机械臂构型息息相关，求解顺序为1-5-6-3-2-4，当机械臂构型改变时，该方法失效。申请号201610538295.7，一种偏置机械臂逆运动学求解方法使用固定关节角+虚拟机械臂法解决了7自由度偏置构型的机械臂逆运动问题，属于解析法+数值法的变种，在求解过程中先使用几何参数构造对应的虚拟机械臂，之后用数值法求解关节角，当机械臂构型改变时，该方法失效，需要对新的构型进行分析推导，不具有普适性。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提供了一种串联六(7)自由度机械臂逆运动学解算方法及介质，解决了现有机械臂逆运动学方法通用性问题，所示方法求解过程简单易懂，且与机械臂构型关系不大，可以普遍适用。

本发明的技术解决方案是：一种串联六自由度机械臂逆运动学解算方法，包括如下步骤：

S1，获取输入参数，判断输入参数为六自由度参数或七自由度参数；若为七自由度参数，则将输入参数作降阶处理，进入S2；若为六自由度参数，则进入S2；

S2，将输入参数中的机械臂末端期望位姿和机械臂关节初值转换为齐次变换矩阵，并初始化外层排列参数、中层排列组合参数和内层迭代次数，进入S3；

S3，判断外层排列参数是否在第一设定值范围内；若在，则计算当前外层排列参数对应的排列数组和排列数组大小，进入S4；否则，进入S6；

S4，判断中层排列组合参数是否在第二设定值范围内；若在，则进入S5；否则，则将中层排列组合参数置为初值、外层排列参数自加一后，返回S3；

S5，判断内层迭代次数是否在第三设定值范围内；若在第三设定值范围内，则通过构建降维非线性方程组与降维雅克比矩阵计算迭代后的关节角，并判断此关节角是否满足误差条件，若此关节角满足误差条件，则记录该满足误差条件的关节角，并对中层排列组合参数加一后，返回S4，若此关节角不满足误差条件，则对内层迭代次数加一后，重复S5；若不在第三设定值范围内，则对中层排列组合参数加一后，返回S4；

S6，剔除重复的关节角，将得到的关节角输出用于后续处理。

进一步地，所述将输入参数作降阶处理的方法为：剔除七自由度参数中首关节或末关节中的一个，将七自由度参数降阶为六自由度参数。