基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法
【技术领域】
[0001] 本发明属于工业机器人控制技术领域，具体涉及一种基于人工蜂群搜索的七自由 度机械臂逆向运动学求解方法。
【背景技术】
[0002] 目前，工业领域中使用最多的机械臂是六自由度或少于六自由度的关节串联型机 械臂。随着工作环境和任务的日益复杂化，要求新一代的机械臂需要具备更高的灵活性和 适应性。六自由度机械臂的末端位姿与各关节角之间存在着一一对应关系，即在末端位姿 确定的情况下，只存在着有限的关节位置与之对应，如果某指定位姿下对应的关节位置存 在障碍或处于奇异位形，机械臂的关节角就没有调整的余地，缺乏灵活性。由于多了一个冗 余自由度，七自由度机械臂的自由度数目大于确定空间某一位姿所需的六个，其末端位姿 与关节角空间的关系变成一对多，即在末端位姿指定的情况下，关节空间中存在着无数组 解与指定末端位姿相对应。因此，七自由度机械臂有了更多的姿势形态，并可利用这一特点 克服机械臂的奇异性、避免受限关节角、躲避障碍物及获得最小关节力矩等，可以很好地满 足某些环境、任务和工艺的特殊要求。但是，七自由度机械臂逆向运动学方程的解也由六自 由度机械臂的一组变成无穷多组，如何在无穷多组解中确定一个合理且最优的解是七自由 度机械臂控制技术的关键问题，其求解难度和复杂程度远大于六自由度机械臂的逆向运动 学求解，如何快速准确地求取七自由度机械臂逆向运动学的解仍然是该领域的一大难题。
[0003] 人工蜂群算法是由土耳其学者提出的一种基于蜜蜂觅食行为的群智能优化算法， 与蚁群算法、粒子群算法等其他智能算法相比，人工蜂群不仅参数少，鲁棒性强，易于实现， 而且在每次迭代过程中都会进行全局和局部搜索，增加了找到最优解的概率，并在一定程 度上避免了陷入局部最优的可能。对于七自由度机械臂的逆向运动学求解问题，可采用人 工蜂群算法对某一位姿下逆向运动学对应的无穷多组解进行寻优，并确定一组适用的最优 解，通过这一方法可极大简化运算设计的复杂性，提高求解的速度和精度。
[0004] 但标准的人工蜂群算法也存在缺陷，主要表现在初始解随机性强，存在早熟收敛， 后段收敛速度慢，缺乏局部精细搜索能力，计算精度不高等问题，这主要是因为在标准的人 工蜂群算法中，蜜源初始位置的设定和更新蜜源的搜索策略不够理想。
[0005] 在标准的人工蜂群算法中，蜜源的位置被抽象成解空间中的点并代表问题的潜在 解，如果求解问题的维数为D，蜜源数为N，则蜜源i初始位置依照以下公式在搜索空间中随
【主权项】
1.基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法，其特征在于，包括如下 步骤： 设七自由度关节串联型机械臂的逆向运动学方程的解的维数为7,第1维至第7维分别 对应机械臂底座第1个旋转关节到末端第7个旋转关节的绝对角度值； 设机械臂末端的运动路径中位姿插补点P的位姿矩阵Tp为：
其中，子矩卩
§述了在基础坐标系中机械臂末端在位姿插补点P时的 姿态，其由三个单位矢量构成； 其中，[npx npy ηρζ]τ为末端法线矢量，[opx opy ορζ]τ为末端方向矢量，[apx apy apz]T为末 端接近矢量； 子矩阵[Ppx Ppy PpJt描述了在基础坐标系中机械臂末端在位姿插补点P时的位置； 设人工蜂群算法完成1次搜索机械臂逆向运动学方程数值解的循环后，各关节角的搜 索值为Θ i，i = 1，2,…，7 ; 根据在基础坐标系下建立的机械臂各关节及末端的坐标系模型，利用Θ i并结合机械 臂的机械参数建立机械臂的正向运动学方程，求得位姿矩阵T。为：
其中，子矩￥
苗述了在基础坐标系中机械臂末端在位姿插补点P时的 姿态，其由三个单位矢量构成； 其中，[nM、nJT为末端法线矢量，[oM Otjy oJT为末端方向矢量，[aM、aJT为末 端接近矢量； 子矩阵[PM Ptjy pJT描述了在基础坐标系中机械臂末端在位姿插补点P时的位置； 设置人工蜂群搜索的目标函数f(T。，Tp)，表达式如下： f (Τ〇, τρ) = (I nox I -1 npx I)2+ (I Oox I -1 Opx I)2+ (I aox I -1 apx I)2+ (I noy I -1 npy I)2+ (I Ooy I -1 opy |) 2+ (I aoy I - I apy I)2+ (I noz I -1 npJ )2+ (I OozI -1 opz I)2+ (I aoz I - I apz I)2+ (pox-ppx)2+ (poy-ppy)2+ (poz~p pZ)2 (3) 人工蜂群中蜜源的位置代表七自由度机械臂逆向运动学方程的解，蜜源适应度值的计 算采用标准的人工蜂群算法中的计算公式，如下式（4): J
其中，Htni表不苐i个蜜源的迠应度值，f i (T。，Tp)表不苐i个蜜源的目标函数值； 采用人工蜂群三段式搜索策略对机械臂末端某一运动路径中的位姿插补点P的各个 关节绝对角度值在从路径起点到终点的角度变化范围进行最优值搜索，并将搜索结果作为 该位姿插补点P的逆向运动学方程的数值解，具体搜索步骤如下： 将整个搜索过程分为三个阶段，即前段搜索阶段、中段搜索阶段和后段搜索阶段； a前段搜索阶段 前段搜索阶段包括蜜源初始化和前段搜索过程； 蜜源初始化包括如下步骤： 设初始蜜源的数量为N，将逆向运动学方程的解空间划分为N个大小相等的子空间，1 个子空间内包含1个蜜源，蜜源的初始位置在子空间内随机确定； 定义分别表示解空间中第d维分量对应的关节在机械臂末端某一运动路径 中的起点和终点的绝对角度值；其中，d e {1，2,…，7}，解空间的每维分量均分为长度相等 的m个数据段，且m7= N，每个数据段的长度为（X1Ix -; 各维分量中均分的数据段用 < 表达；其中，上标d e {1，2,…，7}表示维度序列号，下 标i e {1，2,…，m}表不数据段序列号； 每次抽取各维中的1个数据段构建1个7维的子空间，每个子空间中至少有1个数据 段与其他子空间不同，共有m7种不同的组合方式，即可构建N个子空间； 子空间用Xi表达，其中，i e {1，2，···，Ν};子空间的表达式为足={4,4「，~，\7}，其中， ?，…,?7分别为从第1至第7维中各维的1个数据段； 在每个子空间中随机产生1个蜜源的初始位置，即蜜源的初始位置的各维分量是其被 包含的子空间的各维分量取值范围内的随机数，公式表达如下： = sL·+rand (°5 - sLm ) (5 ) 其中，x/表示第i个子空间中的蜜源初始位置的第d维分量，Siin表示第i个子空间中 第d维分量的下限值，^nax表示第i个子空间中第d维分量的上限值，rand(0, 1)表示[0, 1] 内的1个随机数； 所有子空间内都产生蜜源的初始位置后，即开始进行前段搜索； 前段搜索采用标准的人工蜂群算法的搜索策略，即N个引领蜂依次在蜜源i周围随机 选择1个参考蜜源进行搜索，并根据公式（6)产生1个新蜜源： y;!!二 xXx;"-xJ) (6) 其中，ie {1，2,…，N}，me {1，2,…，7}，je {1，2,…，N}，m和j为其取值范围内的 随机数，且i辛