基于蜣螂优化算法求解冗余自由度机器人逆解的方法

本发明属于机器人运动求解，具体涉及基于蜣螂优化算法求解冗余自由度机器人逆解的方法。

背景技术：

1、随着冗余自由度机器人在工业生产线上的广泛应用,用冗余机械臂多余的自由度来实现机器人本体避障、避奇异、避关节限位、关节力矩优化、增加操作度等附加任务。为了更好地对冗余自由度机器人进行位置控制，需要快速求解冗余自由度机器人逆解。

2、目前，求解冗余自由度机器人逆解的方法主要有几何解析法，伪逆法，智能算法等，几何解析法需要满足特殊构型，伪逆法存在累积误差，传统的智能算法虽然不受机器人结构影响，具有通用性，但其收敛速度不够快、易陷入局部极值的缺陷，为提高冗余自由度机器人逆解的方法求解速度以及精度，简化求解难度，因此提出求解冗余自由度机器人逆解的方法是很有必要的。

技术实现思路

1、本发明的目的是提供基于蜣螂优化算法求解冗余自由度机器人逆解的方法，解决了传统智能算法在冗余自由度机器人逆解求解时收敛速度不够快、易陷入局部极值的缺陷，速度慢，准确性不高的问题。

2、本发明所采用的技术方案是基于蜣螂优化算法求解冗余自由度机器人逆解的方法，具体按照以下步骤实施：

3、步骤1、使用d-h方法建立冗余自由度机器人的正向运动学模型；

4、步骤2、制定机器人逆运动学求解的评价标准；

5、步骤3、改进蜣螂优化算法；

6、步骤4、将步骤2机器人逆运动学求解的评价标准作为优化目标，利用步骤3改进蜣螂优化算法求解冗余自由度机器人的逆运动学。

7、本发明的特点还在于，

8、步骤1根据d-h法将机器人各关节的变换矩阵依次相乘，得到机器人的末端相对于基坐标的变换矩阵，构建正向运动学模型；

9、

10、式中，m为机器人第m个连杆，第m个连杆的变换矩阵，为机器人的末端相对于基坐标的变换矩阵，d为总自由度数，当d≥7时，机器人为冗余自由度机器人。

11、步骤2具体过程为：

12、为了求取机器人实际位姿与目标位姿之间的误差，以机器人的接近矢量与位置矢量加权和来构建适应度函数；

13、f＝η||pd-pc||+β||ad-ac||        (2)

14、式中，pd是目标位姿，pc是实际位姿，ad和ac分别为目标接近矢量和实际接近矢量，η和β分别为权值系数；

15、冗余机械臂的逆运动学存在无数组解，而式(3)每次只求得一组点到点的解；为了空间运动中找到平滑连续运动轨迹，采用式(4)构建适应度函数进行求解：

16、

17、f2＝min(f+γf1)       (4)

18、为第k个关节初始关节角，θk为第k个关节当前关节角，f1为关节运动最小变化量，γ为权值系数。

19、步骤3的具体过程为：

20、步骤3.1、加入tent混沌映射；

21、步骤3.2、计算所有蜣螂初始适应度值，确定当前最优蜣螂位置；

22、步骤3.3、引入正弦策略，更新普通蜣螂局部最优位置；

23、步骤3.4，更新其他角色蜣螂局部最优位置，

24、步骤3.5，计算蜣螂群当前的全局最优位置，比较当前的全局最优位置与前一代全局最优位置，如果当前的全局最优位置适应度值优于前一代，则将蜣螂群的最优位置更新为当前的全局最优位置；

25、步骤3.6，若没有达到最大迭代次数t，则循环重复步骤3.3～步骤3.5，直至结束。

26、步骤3.1根据式(5)扩大初始种群解空间搜索范围；

27、

28、式中，xn为第n次迭代产生的序列，ω为混沌参数，ω不取0.5；

29、在(0，1)之间随机生成一个数组序列x0，此时n为0，然后根据式(5)进行迭代计算，生成一个新序列，当n达到最大迭代次数时，将生成的序列做为蜣螂种群个体初始分布。

30、步骤3.3的具体过程为：

31、

32、式中，t表示当前迭代次数，就表示第i只蜣螂在第t次迭代时的位置；ωt为惯性权重，k∈(0,0.2]表示偏转系数，b是(0,1)之间的一个常值；表示当前种群中的最差位置，用于模拟光强的变化，α模拟了自然因素，使蜣螂偏离它们原来的方向，为第t次迭代中最优个体位置变量分量；r1为非线性递减函数，r2是区间[0，2π]上的随机数，r3是区间[-2,2]上的随机数，δ为(0,1)之间的一个随机常值，st是一个属于0.5到1之间的随机数；

33、非线性递减函数r1为：

34、

35、式中，ωmin和ωmax表示ωt的最小值和最大值，tmax为最大迭代次数；

36、

37、对于普通蜣螂，当δ＜st时，表明当前的蜣螂有指定目标，正处于全局探索阶段，反之，代表蜣螂没有指定目标，要通过正弦函数的方式进行搜索移动。

38、步骤3.4中对于雌性蜣螂利用公式(9)更新卵球位置；

39、bj(t+1)＝x*+b1×(bj(t)-lb*)+b2×(bj(t)-ub*)          (9)

40、其中，bj(t)是第j个卵球在第t次迭代的位置信息，b1和b2是两个独立的大小为1×d的随机向量，d是优化问题的维度，其中x*表示当前局部最优位置，lb*和ub*分别表示产卵范围的下界和上界。

41、对于小蜣螂更新觅食位置；

42、

43、其中，c1为服从正态分布的随机数，即c1～n(0,1)，c2为1×d的属于(0,1)之间的随机向量，xl为当前种群的小蜣螂局部最优位置；用lbl和ubl分别定义了小蜣螂觅食范围的下界和上界。

44、对于小偷蜣螂更新位置；

45、

46、其中，g表示大小为1×d的随机向量，服从正态分布，s表示一个常数值。

47、步骤4的具体过程为：

48、步骤4.1、确定机器人各关节的角度范围，初始化参数，包括蜣螂数量、不同角色蜣螂之间比例、最大迭代次数或阈值；

49、步骤4.2、在关节范围内利用tent混沌序列生成初始种群；

50、步骤4.3、计算出当前每只蜣螂的适应度值；

51、步骤4.4、利用改进蜣螂优化算法迭代产生新的解，更新普通蜣螂、卵球、小蜣螂、偷窃蜣螂的位置，对每只蜣螂进行边界判定，每经过一次寻优迭代后，每只蜣螂的适应度值和种群平均适应度值会重新计算，并进行比较，更新机器人各关节角度

52、步骤4.5、判断是否满足输出条件，满足则输出结果，否则重复步骤4.4。

53、本发明的有益效果是：本发明基于蜣螂优化算法求解冗余自由度机器人逆解的方法，蜣螂优化算法提高了全局探索能力，减少陷入局部最优的可能性，蜣螂优化算法提升求解准确性能；本发明基于蜣螂优化算法求解冗余自由度机器人逆解的方法，将改进后的蜣螂优化算法应用到了冗余自由度机器人逆解的求解，解决了冗余自由度机器人逆解求解时速度慢，准确性不高的问题，增强求解通用性，满足机器人控制中对于快速性、精确性的要求。