一种6自由度机械臂逆运动学求解方法与流程

本发明涉及工业机器人领域，尤其涉及一种基于蒙特卡洛方法与遗传算法相结合的混合求解多自由度机械臂逆运动学求解方法。
背景技术：
：机械臂的逆运动学求解是工业机器人应用的基础，是机械臂控制的先决条件，直接关系到机器人离线编程、轨迹规划、实时控制等任务。运动学逆解在机器人学中占有重要地位，只有将空间位姿转换为关节变量，才能实现对机器人末端执行器按空间位姿进行编程控制。串联机械臂的运动学逆解较为困难，各国学者进行了大量的探索和研究。运动学逆解的求解方法主要分为解析法、数值法、几何法等。如解析方法简单直观，运算速度快，但是只有满足特定几何条件的机械臂结构才能使用解析法求解：即后3个关节轴相交于一点或有3个关节轴相互平行。通过学者doty的分析，24类6自由度正交机械臂结构，只有5类有解析解，其余结构是否有解析解尚不能确定。雅可比矩阵求解关节变量的迭代算法是数值法的基本思想，多种非线性优化方法被应用于逆解的数值方法，但是普遍存在运算量大，收敛速度慢的特点。近年来，有学者采用解析与数值计算相结合的方法，在一定程度上提高了计算速度。上述方法涉及矩阵的逆运算和反三角函数的运算，计算量及限制条件等方面存在不足。运动学逆解的另外一类流派是采用智能算法进行求解，如遗传算法、粒子群算法、神经网络等。智能算法求解运动学逆解，限制条件少、适用范围广，但是存在收敛慢，陷于局部极小致使结果的精度无法保证等缺陷。此类方法通过一组随机的初始关节角度作为初始种群，通过不断迭代更新，使得每次迭代的结果逐渐向真实解收敛。但是相关算法对初始种群的选择依赖较高，初始种群的好坏直接影响到计算结果。d-h矩阵(d-h方法)是denavit和hartenberg两位学者在1955年提出一种通用的位姿计算方法。这种方法在机器人的每个连杆上都固定一个坐标系，然后用4×4的齐次变换矩阵来描述相邻两连杆的空间关系。通过依次变换可最终推导出末端执行器相对于基坐标系的位姿，从而建立机器人的运动学方程。对于6自由度机械臂而言，d-h矩阵可标识为如下形式，其中ai为第i个连杆的变换矩阵，r为3*3的旋转矩阵，p为1*3的位置矩阵，i通常为1，6自由度机械臂的变换矩阵t6为：技术实现要素：本发明的目的在于提供一种6自由度机械臂逆运动学求解方法，从而解决了现有运动学逆解求解速度慢、精度不高的缺点。为实现上述目的，本发明提供了一种6自由度机械臂逆运动学求解方法，包括以下步骤：s1、根据机械臂的参数建立机械臂的d-h矩阵；s2、根据机械臂的结构和机械臂的参数，确定s3的阈值，即机械臂末端位置与目标位置pe的最大距离；s3、采用蒙特卡洛方法为机械臂全部关节赋予随机角度生成随机样本，通过所述d-h矩阵计算每个所述随机样本对应的机械臂末端位置，保留满足条件的机械臂末端位置为样本；s4、以s3保留的样本为种群组，增加新的随机种群组合成遗传算法的初始种群，利用遗传算法求得运动学逆解。进一步的，所述机械臂的参数包括：每个所述机械臂的长度、扭转角度、偏移量及关节角度。进一步的，所述s3具体包括以下步骤：s31、为机械臂全部关节赋予随机角度，生成随机样本；s32、将所述随机样本代入s1的d-h矩阵，计算对应的机械臂末端的位置s33、判断保留pm同时满足如下条件，如公式(1)所示；式(1)中，thx，thx，thx为所述阈值在x，y，z三个方向的标识；s34、若所述随机样本对应的末端位置pm满足s33的条件，则将所述对应样本保存至种群数组d1；s35、循环s32-s34的操作，直至完成全部随机样本的计算；s36、循环结束，得到种群数组d1。进一步的，所述s4具体包括以下步骤：s40、在种群数组d1的基础，增加新的关节角度的随机样本组d2，组成遗传算法的初始种群d；s41、为初始种群d编码；s42、设定遗传算法的参数；s43、计算种群中每个个体的对应的机械臂末端位姿矩阵的适应度函数值；s44、根据s43的适应度函数值，对s41种群中的个体进行遗传选择；s45、判断s44得到的遗传选择的个体是否满足交叉概率，如果所述遗传选择的个体满足交叉概率，则进行遗传交叉并保存到新的种群中；如果所述遗传选择的个体不满足交叉概率，则进入s46；s46、判断不满足交叉概率的遗传选择的个体是否满足变异概率，如果满足变异概率，则进行变异交叉并保存到新的种群中，否则去除；s47、在经过s45和s46操作得到的新种群的基础上增加新的随机个体，使得新种群保持原有种群的规模；s48、判断s47得到的种群是否达到完成条件，完成条件包括种群中最佳个体的适应度函数值fit<ε或者s43-s47循环的次数在100-300次内，达到任意一个条件均视为达到完成条件，ε的取值范围为1*10-7-2*10-7；若没有达到完成条件，则用s47所得新种群替换前一次迭代的种群，重复s43-s47的操作；若达到完成条件则退出；s49、输出结果，即运动学逆解。进一步的，所述s41的编码方式为实数编码。进一步的，所述s44中的选择采用竞赛选择法，舍弃适应度函数值过大的个体。进一步的，所述s45交叉选用多点交叉。进一步的，所述s40中，设定种群d中个体的数量为t，随机种群d2中个体的数量t2的计算公式为：式(2)中，t1为s36所得的种群数组d1中个体的数量。进一步的，所述s44中的适应度的函数的构造如公式(3)所示：fit＝|δp|+|δo|(3)式(3)中，|δp|为位置偏移量，|δo|为姿态偏移量；式(4)中，为遗传个体对应的机械臂末端位置，为遗传个体对应的机械臂末端位置的x，y，z坐标，pe为机械臂末端的目标位置；式(5)中，为遗传个体对应的机械臂末端姿态矩阵的第j列第i行，为机械臂末端目标体姿态矩阵的第j列第i行。与现有的技术相比，本发明具有如下有益效果：1、本发明所提供的6自由度机械臂逆运动学求解方法避免了计算矩阵的逆以及反三角函数，运算量小，且没有限制条件。2、本发明所提供的6自由度机械臂逆运动学求解方法，通过蒙特卡洛方法优化遗传算法的初始样本，能够有效提高遗传算法的搜索速度与搜索成功率，同时保留部分随机样本保持种群的多样性，保证算法的全局搜索能力。3、本发明所提供的6自由度机械臂逆运动学求解方法简单，误差小于1％，可操作性强。附图说明为了更清楚地说明本发明的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一个实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1是ur5机器人的结构示意图；图2是遗传算法的编码格式；图3是s3的流程图；图4是s4的流程图；图5是本发明的运动学逆解求解结果示意图；图6是本发明一种6自由度机械臂逆运动学求解方法的流程图。具体实施方式下面结合本发明实施例中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。本发明的实施例以ur5机器人(6自由度机械臂)为例对本发明的6自由度机械臂逆运动学求解方法进行详细的描述。6自由度机械臂要求以姿态oe到达位置pe。其中，为到达位置，为机械臂末端目标位置的x，y，z坐标；姿态oe为机械臂旋转量，为机械臂末端对基座坐标轴的旋转角度。ur5机械臂各关节的主要参数为：ai为第i个连杆的长度，αi为第i个连杆的扭转角度，di为连杆的偏移量，θi为第i个连杆对应的关节角度。表1ur5机械臂主要参数iai(m)αi(°)di(m)θi10900.08916(-360,360)2-0.42500(-360,360)3-0.3922500(-360,360)40-900.10915(-360,360)50-900.09456(-360,360)6000.0823(-360,360)如图1所示，本发明所提供的6自由度机械臂逆运动学求解方法包括以下步骤：s1、根据机械臂的参数建立机械臂的d-h矩阵。s2、根据机械臂的结构和机械臂的参数，确定s3的阈值，阈值即机械臂末端与目标位置pe的最大距离，机械臂的参数包括：每个机械臂的长度、扭转角度、偏移量及关节角度。在本实施例中，x，y，z三个方向的阈值thx，thy，thz均取0.1cm。s3、采用蒙特卡洛方法为机械臂全部关节赋予随机角度生成随机样本，通过d-h矩阵计算每个随机样本对应的机械臂末端位置，仅保留满足条件的机械臂末端位置为样本，即种群数组d1。本实例中，样本数为20万个。根据ur5机器人的机械臂的特性，全部6个关节的转动角度取值范围均为[0,360)。样本数据形如：[θ1,θ2,θ3,θ4,θ5,θ6]。如图3所示，s3具体包括以下步骤：s31、为机械臂全部关节赋予随机角度，生成关节角度的随机样本；s32、关节角度的随机样本代入s1的d-h矩阵，计算关节角度的随机样本对应的末端位置s33、判断保留pm同时满足如下条件，如公式(1)所示；式(1)中，thx，thy，thz为阈值在x，y，z三个方向的标识；s34、若关节角度的随机样本对应的末端位置pm满足s33的条件，则将对应样本保存至种群数组d1；s35、循环s32-s34的操作，直至完成全部随机样本的计算；s36、循环结束，获得满足s33条件的种群数组d1。s4、以s36保留的种群数组d1为基础，增加新的关节角度的种群d2，组合成遗传算法的初始种群d，利用遗传算法求得运动学逆解，如图4所示，s4具体包括以下步骤：s40、在种群数组d1的基础，增加新的关节角度的随机样本d2，组成遗传算法的初始种群d；设定种群d个体的数量为t，种群d2中个体的数量t2由公式(2)确定：式(2)中，t1为s36所得的种群数组d1中个体的数量；s41、为初始种群d编码，编码方式为实数编码，编码格式如图2所示，其中j1-j6对应机械臂的6个关节，j11、j12及j13分别关节1角度的百位、十位和个位的数值；s42、设定遗传算法的参数，本实施例中，交叉概率为0.8，变异概率为0.4；s43、计算种群中每个个体的对应的机械臂末端位姿矩阵的适应度函数值。机械臂末端位姿矩阵由d-h计算得来，适应度函数的构造如公式(3)所示：fit＝|δp|+|δo|(3)式(3)中，|δp|为位置偏移量，|δo|为姿态偏移量；式(4)中，为遗传个体对应的机械臂末端位置，为遗传个体对应的机械臂末端位置的x，y，z坐标，pe为机械臂末端的目标位置；式(5)中，为遗传个体对应的机械臂末端姿态矩阵的第j列第i行，为机械臂末端目标体姿态矩阵的第j列第i行；s44、根据s43的适应度函数值，对s41种群中的个体进行遗传选择，本实施例中选用竞赛选择法，舍弃适应度函数值过大的个体；s45、判断s44得到的遗传选择的个体是否满足交叉概率，如果遗传选择的个体满足交叉概率，则进行遗传交叉并保存到新的种群中；如果遗传选择的个体不满足交叉概率，则进入s46；本实施例选用多点交叉；s46、判断不满足交叉概率的遗传选择的个体是否满足变异概率，如果满足变异概率，则进行变异交叉并保存到新的种群中，否则去除；s47、在经过s45和s46操作得到的新种群的基础上增加新的随机个体，使得新种群保持原有种群的规模；s48、判断s47得到的种群个体是否达到完成条件，完成条件包括适应度函数值fit<ε或者s43-s47循环的次数在100-300次内，达到任意一个条件均视为达到完成条件，ε的取值范围为1*10-7-2*10-7；若没有达到完成条件，则用新的种群替换前一次迭代的种群，重复s43-s47的操作；若达到完成条件则退出；本实施例的完成条件为适应度函数值fit<ε，ε＝0.0000002，迭代次数为300次；s49、输出结果，即运动学逆解，如图5所示，横坐标为迭代次数，纵坐标为适应度函数值，通过图5能够发现，本实施例能够通过40次左右的迭代即可获得机械臂运动学逆解，且逆解的误差约等于零，即迭代后的数值的误差小于1％，因此本发明的可操作性强。综上，本发明一种6自由度机械臂逆运动学求解方法，通过采用蒙特卡洛方法生成遗传算法的初始样本，通过优化遗传算法的初始样本，提高了遗传算法收敛速度，又通过随机样本保持样本的多样性，保证了遗传算法的全局搜索能力，从而采用本发明方法求解逆运动学问题速度快，精度高，适用于离线与在线轨迹规划的情景。以上所揭露的仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本
技术领域：
的技术人员在本发明揭露的技术范围内，可轻易想到变化或变型，都应涵盖在本发明的保护范围之内。当前第1页12