[发明专利]一种6自由度机械臂逆运动学求解方法

权利要求书

1.一种6自由度机械臂逆运动学求解方法，其特征在于：包括以下步骤：

S1、根据机械臂的参数建立机械臂的D-H矩阵；

S2、根据机械臂的结构和机械臂的参数，确定S3的阈值，即机械臂末端位置与目标位置P^E的最大距离；

S3、采用蒙特卡洛方法为机械臂全部关节赋予随机角度生成随机样本，通过所述D-H矩阵计算每个所述随机样本对应的机械臂末端位置，保留满足条件的机械臂末端位置为样本；

所述S3具体包括以下步骤：

S31、为机械臂全部关节赋予随机角度，生成随机样本；

S32、将所述随机样本代入S1的D-H矩阵，计算对应的机械臂末端的位置

S33、判断保留P^M同时满足如下条件，如公式(1)所示；

式(1)中，th_x，th_y，th_z为所述阈值在x，y，z三个方向的标识；

S34、若所述随机样本对应的末端位置P^M满足S33的条件，则将所述对应样本保存至种群数组D1；

S35、循环S32-S34的操作，直至完成全部随机样本的计算；

S36、循环结束，得到种群数组D1；

S4、以S3保留的样本为种群组，增加新的随机种群组合成遗传算法的初始种群，利用遗传算法求得运动学逆解；

所述S4具体包括以下步骤：

S40、在种群数组D1的基础，增加新的关节角度的随机样本组D2，组成遗传算法的初始种群D；

S41、为初始种群D编码；

S42、设定遗传算法的参数；

S43、计算种群中每个个体的对应的机械臂末端位姿矩阵的适应度函数值；

S44、根据S43的适应度函数值，对S41种群中的个体进行遗传选择；

所述S44中的适应度的函数的构造如公式(3)所示：

Fit＝|ΔP|+|ΔO| (3)

式(3)中，|ΔP|为位置偏移量，|ΔO|为姿态偏移量；

式(4)中，为遗传个体对应的机械臂末端位置，为遗传个体对应的机械臂末端位置的x，y，z坐标，P^E为机械臂末端的目标位置；

式(5)中，为遗传个体对应的机械臂末端姿态矩阵的第j列第i行，为机械臂末端目标体姿态矩阵的第j列第i行；

S45、判断S44得到的遗传选择的个体是否满足交叉概率，如果所述遗传选择的个体满足交叉概率，则进行遗传交叉并保存到新的种群中；如果所述遗传选择的个体不满足交叉概率，则进入S46；

S46、判断不满足交叉概率的遗传选择的个体是否满足变异概率，如果满足变异概率，则进行变异交叉并保存到新的种群中，否则去除；

S47、在经过S45和S46操作得到的新种群的基础上增加新的随机个体，使得新种群保持原有种群的规模；

S48、判断S47得到的种群是否达到完成条件，完成条件包括种群中最佳个体的适应度函数值Fitε或者S43-S47循环的次数在100-300次内，达到任意一个条件均视为达到完成条件，ε的取值范围为1*10^-7-2*10^-7；若没有达到完成条件，则用S47所得新种群替换前一次迭代的种群，重复S43-S47的操作；若达到完成条件则退出；

S49、输出结果，即运动学逆解。

2.根据权利要求1所述的6自由度机械臂逆运动学求解方法，其特征在于：所述机械臂的参数包括：每个所述机械臂的长度、扭转角度、偏移量及关节角度。

3.根据权利要求1所述的6自由度机械臂逆运动学求解方法，其特征在于：所述S41的编码方式为实数编码。

4.根据权利要求1所述的6自由度机械臂逆运动学求解方法，其特征在于：所述S44中的选择采用竞赛选择法，舍弃适应度函数值过大的个体。

5.根据权利要求1所述的6自由度机械臂逆运动学求解方法，其特征在于：所述S45交叉选用多点交叉。

6.根据权利要求1所述的6自由度机械臂逆运动学求解方法，其特征在于：所述S40中，设定种群D中个体的数量为T，随机种群D2中个体的数量T₂的计算公式为：

式(2)中，T₁为S36所得的种群数组D1中个体的数量。