机械臂的逆运动学求解方法与流程

摘要: 本发明涉及机器人,特别是涉及一种机械臂的逆运动学求解方法。背景技术:1、工业机械臂中,多自由度串联机械臂是最常见的构型。其中,针对部分特殊应用场景,如伸缩臂码垛...
机械臂的逆运动学求解方法与流程

本发明涉及机器人,特别是涉及一种机械臂的逆运动学求解方法。

背景技术:

1、工业机械臂中,多自由度串联机械臂是最常见的构型。其中,针对部分特殊应用场景,如伸缩臂码垛、喷涂、焊接等应用场景,要求机械臂在旋转关节之外增加平动关节,形成旋转和平动混合的异构构型。这类机械臂的逆运动学求解比纯旋转关节机械臂更为复杂,因为平动关节引入了一个额外的自由度,且其运动范围受机械结构限制。现有的逆运动学求解方法均存在问题,比如,数值迭代法通过迭代逼近目标位姿,但存在收敛慢、易陷入局部极小、无法保证实时性等问题;几何解析法仅针对特定构型建立几何关系,直接求解关节角度,该方法虽然计算速度快、精度高,但对构型假设敏感,异构机械臂的解析推导困难;基于臂型角参数化法适用于具有球腕的机械臂,但平动关节破坏了球腕结构,无法直接应用。

技术实现思路

1、鉴于上述问题,提出了本发明以便提供一种克服上述问题或者至少部分地解决上述问题的机械臂的逆运动学求解方法,能够解决现有的逆运动学求解方法计算速度慢或者不适合异构机械臂解析的问题,提高了用户满意度。

2、具体地,本发明提供了一种机械臂的逆运动学求解方法,所述机械臂包括 m 个关节,m 为大于 5的自然数,从所述机械臂的基座起算的第一关节至第m-2关节,以及第m关节均为旋转关节,第 m-1关节为平动关节;所述机械臂的末端设置为水平;所述方法包括:

3、获取目标点位;

4、获取所述目标点位的目标位姿矩阵,通过工具长度偏移补偿反推得到第 m关节坐标系下的目标位姿矩阵,并从中提取所述第 m关节对应的空间位置与旋转矩阵;

5、基于所述机械臂的末端水平约束,将所述空间位置投影至水平面并转换至第一关节坐标系,获取所述第一关节、第二关节、第三关节及所述平动关节之间的平面几何关系,后以所述第一关节的关节变量为已知定值,求解第二关节、第三关节及所述平动关节的关节变量;其中,所述第二关节的关节变量求解得到两组解;

6、根据已得到的所述关节变量以及所述旋转矩阵,求解第四关节至所述第m关节中除所述平动关节外的全部所述旋转关节的关节变量;其中,所述第四关节的关节变量求解得到两组解,所述第m关节的关节变量求解得到两组解;

7、将各所述关节的关节变量进行组合,得到多组关节变量解,对各组所述关节变量解进行物理运动范围约束筛选,并对各所述旋转关节的关节变量做周期归一化处理;

8、若筛选后仍存在多组所述关节变量解,则根据与所述目标点位临近的上一所述目标点位求解输出的最优关节变量解,得到所述目标点位对应的所述最优关节变量解;

9、将所述最优关节变量解对应的全部所述关节变量代入正运动学模型进行校验,若校验结果满足预设精度要求,则判定所述目标点位逆运动学求解成功,并保存和/或输出所述目标点位的所述最优关节变量解。

10、可选地,m等于6,即所述机械臂包含五个所述旋转关节和一个所述平动关节,从所述机械臂的基座起算的第五关节为所述平动关节。

11、可选地,所述获取所述目标点位的目标位姿矩阵,通过工具长度偏移补偿反推得到第 m关节坐标系下的目标位姿矩阵,并从中提取所述第 m关节对应的空间位置与旋转矩阵,包括:

12、获取带工具坐标系的所述目标点位的目标位姿矩阵t0-7;

13、根据工具长度,利用工具齐次变换矩阵t6-7及其逆矩阵inv(t6-7),反推得到第六关节坐标系下的目标位姿矩阵t0-6= t0-7·inv(t6-7);

14、从t0-6中提取空间位置(px,py,pz)和旋转矩阵r0-6。

15、可选地,所述基于所述机械臂的末端水平约束,将所述空间位置投影至水平面并转换至第一关节坐标系,获取所述第一关节、第二关节、第三关节及所述平动关节之间的平面几何关系,后以所述第一关节的关节变量为已知定值,求解第二关节、第三关节及所述平动关节的关节变量,包括:

16、以所述第一关节的关节变量为已知定值,把所述空间位置投影至水平面并旋转变换至第一关节坐标系,得到所述空间位置在第一关节坐标系下的横向坐标与纵向坐标;

17、输入预设的所述机械臂的末端的水平度约束角,基于平面几何关系构建三角方程,所述三角方程包含所述空间位置在第一关节坐标系下的横向坐标、纵向坐标、第二关节的关节变量、第三关节的关节变量以及所述平动关节的关节变量;其中,所述水平度约束角为所述机械臂的末端的工具法向与竖直方向的夹角;

18、对所述三角方程进行求解,得到两组所述第二关节的关节变量,分别对应所述机械臂的所述第三关节向上转动构型和所述机械臂的所述第三关节向下转动构型;

19、针对每一组所述第二关节的关节变量,求解得到对应的第三关节的关节变量和所述平动关节的关节变量。

20、可选地,所述根据已得到的所述关节变量以及所述旋转矩阵,求解第四关节至第m关节中除所述平动关节外的全部所述旋转关节的关节变量,包括:

21、根据所述第一关节的关节变量、所述第二关节的关节变量和所述第三关节的关节变量,计算得到第三关节坐标系到所述机械臂的基座坐标系的齐次变换矩阵t0-3;

22、提取所述t0-3中的旋转矩阵并求取逆矩阵,结合从所述目标位姿矩阵中提取的旋转矩阵r0-6,计算得到第六关节坐标系到第三关节坐标系的旋转矩阵;

23、基于r3-6中元素与所述关节变量的三角函数对应关系,利用反正切函数解析提取所述第四关节的关节变量和所述第六关节的关节变量的具体表达式,得到两组所述第四关节的关节变量和两组所述第六关节的关节变量。

24、可选地,所述对各组所述关节变量解进行物理运动范围约束筛选,并对各所述旋转关节的关节变量做周期归一化处理,包括:

25、针对各所述旋转关节的关节变量,通过加减360°整数倍的方式完成角度周期归一化,将各所述旋转关节的关节变量换算至预设转角区间;

26、针对所述平动关节的关节变量,结合结构偏移量,检查求解得到的所述平动关节的关节变量是否位于所述平动关节实际运动范围内;

27、将超出对应物理运动极限的各所述关节的关节变量剔除。

28、可选地,所述若筛选后仍存在多组所述关节变量解,则根据与所述目标点位临近的上一所述目标点位求解输出的最优关节变量解,得到所述目标点位对应的所述最优关节变量解,包括:

29、调取与所述目标点位临近的上一所述目标点位求解输出的最优关节变量解;

30、分别计算所述目标点位的各组所述关节变量解与上一所述目标点位求解输出的最优关节变量解之间的距离;其中,所述旋转关节取最短角距离并考虑周期性,所述平动关节取绝对值距离;

31、选取距离最小的一组作为所述目标点位对应的最优关节变量解。

32、可选地,所述方法包括:

33、采用denavit-hartenberg参数法建立从所述机械臂的基座到第六关节的各连杆坐标系;各所述关节之间通过所述连杆相连;

34、分别构建各所述关节的齐次变换矩阵;其中,所述旋转关节的所述齐次变换矩阵包含绕相应坐标轴的旋转角度,所述平动关节的所述齐次变换矩阵包含沿其运动方向的平移量;所述旋转角度和所述平移量均为所述关节变量;

35、将各所述关节的所述齐次变换矩阵按顺序相乘,得到从所述机械臂的基座到所述第六关节的齐次变换矩阵,以得到所述正运动学模型。

36、可选地,所述将所述最优关节变量解对应的全部所述关节变量代入正运动学模型进行校验,若校验结果满足预设精度要求,则判定所述目标点位逆运动学求解成功,包括:

37、将所述最优关节变量解对应的全部所述关节变量输入所述正运动学模型,求解得到校验位姿矩阵;

38、分别计算所述校验位姿矩阵与所述目标位姿矩阵之间的位置误差和姿态角误差;其中,当所述姿态角误差的绝对值大于180°时,将360°与该姿态角误差绝对值的差值作为有效姿态角误差;

39、判断所述位置误差是否小于或等于位置误差阈值,且所述有效姿态角误差是否小于或等于姿态角误差阈值;

40、若是,则判定所述目标点位逆运动学求解成功;

41、若否,则判定所述目标点位逆运动学求解失败。

42、可选地,所述获取目标点位包括:

43、确定多个目标轨迹点;

44、对相邻目标轨迹点做插值运算,得到多个插值点;

45、将每个所述目标轨迹点和每个所述插值点均作为所述目标点位。

46、本发明的机械臂的逆运动学求解方法中,机械臂包括基座,第一关节设置在基座上,从基座起算依次为第一关节、第二关节、第三关节直到第m关节,其中,第m-1个关节为平动关节,其余均为旋转关节。目标点位为机械臂的末端所在位置。关节变量包括平动关节的平移量与旋转关节的旋转角度。获取目标点位的目标位姿矩阵,通过工具长度偏移补偿消除工具长度偏移,反推得到第m关节坐标系下的目标位姿矩阵,并从中提取第 m关节对应的空间位置与旋转矩阵。对于伸缩臂码垛、喷涂、焊接等应用场景,机械臂的末端是水平的,将空间位置投影并转换至第一关节坐标系,可以基于末端水平约束将三维问题变换为二维平面几何问题,由于第一关节的旋转角度为由机械臂安装方位决定的已知的固定值,只需要在平面关系内求解第二关节、第三关节及平动关节的关节变量。再根据已得到的关节变量和旋转矩阵求解第四关节至第m关节中除平动关节外的全部旋转关节的关节变量。之后将各关节的关节变量进行组合得到多组关节变量解,进行物理范围约束筛选,并对各旋转关节的关节变量做周期归一化处理。若筛选后仍存在多组关节变量解,则根据上一点位求解输出的关节变量解得到当前目标点位对应的最优关节变量解。最后将最优关节变量解对应的全部关节变量代入正运动学模型进行位姿校验,若校验结果满足预设精度要求,则判定当前目标点位逆解成功。由于将机械臂在具体作业场景的作业姿态要求,比如吸盘水平,直接融入解析方程,将复杂的三维计算变换为二维计算,减少未知数,提高求解稳定性,而且可以基于几何约束的闭式求解,无需迭代,单次逆解计算时间在微秒级,满足高速轨迹插补需求。在求解过程中将平动关节的关节平移量作为逆运动学求解的未知量参与解析推导,使得求解过程针对旋转关节和平动关节混合的异构构型,更有针对性,避免现有的逆运动学求解方法不适合异构机械臂解析的问题,提高了用户满意度。

47、根据下文结合附图对本发明具体实施例的详细描述,本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。