CN104834809A - 基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法 - Google Patents

Abstract

本发明公开了一种基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法，该方法用于求取七自由度关节串联型机械臂运动路径中位姿插补点的逆向运动学方程的数值解，该方法基于七自由度机械臂正向运动学方程建立目标函数，采用人工蜂群算法三段式搜索策略，对运动路径中的位姿插补点的各个关节绝对角度值在从起点到终点的角度变化范围内进行最优值搜索，并将搜索结果作为该位姿插补点逆向运动学方程的数值解。通过本发明述及的三段式搜索策略可增强人工蜂群算法的搜索能力、提高算法的收敛速度和计算精度，从而简化七自由度机械臂逆向运动学求解的复杂性，提高求解的速度和精度。

Description

基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法

技术领域

本发明属于工业机器人控制技术领域，具体涉及一种基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法。

背景技术

目前，工业领域中使用最多的机械臂是六自由度或少于六自由度的关节串联型机械臂。随着工作环境和任务的日益复杂化，要求新一代的机械臂需要具备更高的灵活性和适应性。六自由度机械臂的末端位姿与各关节角之间存在着一一对应关系，即在末端位姿确定的情况下，只存在着有限的关节位置与之对应，如果某指定位姿下对应的关节位置存在障碍或处于奇异位形，机械臂的关节角就没有调整的余地，缺乏灵活性。由于多了一个冗余自由度，七自由度机械臂的自由度数目大于确定空间某一位姿所需的六个，其末端位姿与关节角空间的关系变成一对多，即在末端位姿指定的情况下，关节空间中存在着无数组解与指定末端位姿相对应。因此，七自由度机械臂有了更多的姿势形态，并可利用这一特点克服机械臂的奇异性、避免受限关节角、躲避障碍物及获得最小关节力矩等，可以很好地满足某些环境、任务和工艺的特殊要求。但是，七自由度机械臂逆向运动学方程的解也由六自由度机械臂的一组变成无穷多组，如何在无穷多组解中确定一个合理且最优的解是七自由度机械臂控制技术的关键问题，其求解难度和复杂程度远大于六自由度机械臂的逆向运动学求解，如何快速准确地求取七自由度机械臂逆向运动学的解仍然是该领域的一大难题。

人工蜂群算法是由土耳其学者提出的一种基于蜜蜂觅食行为的群智能优化算法，与蚁群算法、粒子群算法等其他智能算法相比，人工蜂群不仅参数少，鲁棒性强，易于实现，而且在每次迭代过程中都会进行全局和局部搜索，增加了找到最优解的概率，并在一定程度上避免了陷入局部最优的可能。对于七自由度机械臂的逆向运动学求解问题，可采用人工蜂群算法对某一位姿下逆向运动学对应的无穷多组解进行寻优，并确定一组适用的最优解，通过这一方法可极大简化运算设计的复杂性，提高求解的速度和精度。

但标准的人工蜂群算法也存在缺陷，主要表现在初始解随机性强，存在早熟收敛，后段收敛速度慢，缺乏局部精细搜索能力，计算精度不高等问题，这主要是因为在标准的人工蜂群算法中，蜜源初始位置的设定和更新蜜源的搜索策略不够理想。

在标准的人工蜂群算法中，蜜源的位置被抽象成解空间中的点并代表问题的潜在解，如果求解问题的维数为D，蜜源数为N，则蜜源i初始位置依照以下公式在搜索空间中随机产生： $x_i^d=x_{\min }^d+\mathrm{rand}\left(\mathrm{0,1}\right)(x_{\max }^d-x_{\min }^d);$

其中，i∈{1,2,…,N}，d∈{1,2,…,D}，表示蜜源i的初始位置中第d维分量，表示搜索空间中第d维分量的下限值，表示搜索空间中第d维分量的上限值，rand(0,1)表示[0,1]内的一个随机数。

在蜜源搜索过程中，引领蜂和跟随蜂在蜜源i周围随机选择一个参考蜜源进行搜索，并根据以下公式产生一个新蜜源：

其中，i∈{1,2,…,N}，m∈{1,2,…,D}，j∈{1,2,…,N}，m和j为其取值范围内的随机数，且i≠j，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示随机选择的目标蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数。

由上述两式可知，在蜜源初始化时，由于每个蜜源位置的初始值是随机设置，当蜜源数量较少时，其初始位置分布不均匀，导致搜索范围受到限制，影响全局搜索能力，并易导致早熟收敛。在引领蜂和跟随蜂进行新蜜源搜索时，由于搜索公式中位置的维度分量m、蜜源j和都为随机数，搜索过程中不确因素较多，搜索的收敛速度和精度较难保证。

发明内容

针对现有技术中存在的上述技术问题，本发明提出了一种基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法，其采用如下技术方案：

基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法，包括如下步骤：

设七自由度关节串联型机械臂的逆向运动学方程的解的维数为7，第1维至第7维分别对应机械臂底座第1个旋转关节到末端第7个旋转关节的绝对角度值；

设机械臂末端的运动路径中位姿插补点P的位姿矩阵T_p为：

$T_p=\left[\begin{array}{cccc}{n}_{\mathrm{px}}& o_{\mathrm{px}}& a_{\mathrm{px}}& p_{\mathrm{px}}\\ n_{\mathrm{py}}& o_{\mathrm{py}}& a_{\mathrm{py}}& p_{\mathrm{py}}\\ n_{\mathrm{pz}}& o_{\mathrm{pz}}& a_{\mathrm{pz}}& p_{\mathrm{pz}}\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

其中，子矩阵 $\left[\begin{array}{ccc}{n}_{\mathrm{px}}& o_{\mathrm{px}}& a_{\mathrm{px}}\\ n_{\mathrm{py}}& o_{\mathrm{py}}& a_{\mathrm{py}}\\ n_{\mathrm{pz}}& o_{\mathrm{pz}}& a_{\mathrm{pz}}\end{array}\right]$ 描述了在基础坐标系中机械臂末端在位姿插补点P时的姿态，其由三个单位矢量构成；

其中，[n_px n_py n_pz]^T为末端法线矢量，[o_px o_py o_pz]^T为末端方向矢量，[a_px a_py a_pz]^T为末端接近矢量；

子矩阵[p_px p_py p_pz]^T描述了在基础坐标系中机械臂末端在位姿插补点P时的位置；

设人工蜂群算法完成1次搜索机械臂逆向运动学方程数值解的循环后，各关节角的搜索值为θ_i，i＝1,2,…,7；

根据在基础坐标系下建立的机械臂各关节及末端的坐标系模型，利用θ_i并结合机械臂的机械参数建立机械臂的正向运动学方程，求得位姿矩阵T_o为：

$T_o=\left[\begin{array}{cccc}{n}_{\mathrm{ox}}& o_{\mathrm{ox}}& a_{\mathrm{ox}}& p_{\mathrm{ox}}\\ n_{\mathrm{oy}}& o_{\mathrm{oy}}& a_{\mathrm{oy}}& p_{\mathrm{oy}}\\ n_{\mathrm{op}}& o_{\mathrm{oz}}& a_{\mathrm{oz}}& p_{\mathrm{oz}}\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

其中，子矩阵 $\left[\begin{array}{ccc}{n}_{\mathrm{ox}}& o_{\mathrm{ox}}& a_{\mathrm{ox}}\\ n_{\mathrm{oy}}& o_{\mathrm{oy}}& a_{\mathrm{oy}}\\ n_{\mathrm{oz}}& o_{\mathrm{oz}}& a_{\mathrm{oz}}\end{array}\right]$ 描述了在基础坐标系中机械臂末端在位姿插补点P时的姿态，其由三个单位矢量构成；

其中，[n_ox n_oy n_oz]^T为末端法线矢量，[o_ox o_oy o_oz]^T为末端方向矢量，[a_ox a_oy a_oz]^T为末端接近矢量；

子矩阵[p_ox p_oy p_oz]^T描述了在基础坐标系中机械臂末端在位姿插补点P时的位置；

设置人工蜂群搜索的目标函数f(T_o,T_p)，表达式如下：

f(T_o,T_p)＝(|n_ox|-|n_px)²+(|o_ox|-|o_px|)²+(|a_ox|-|a_px|)²+(|n_oy|-|n_py|)²+(|o_oy|-|o_py|)²+(|a_oy|-|a_py|)²+(|n_oz|-|n_pz|)²+(|o_oz|-|o_pz)²+(|a_oz|-|a_pz|)²+(p_ox-p_px)²+(p_oy-p_py)²+(p_oz-p_pz)²  (3)

人工蜂群中蜜源的位置代表七自由度机械臂逆向运动学方程的解，蜜源适应度值的计算采用标准的人工蜂群算法中的计算公式，如下式(4)：

$\mathrm{fit}{n}_i\text{=}\left\{\begin{array}{cc}\frac{1}{1+f_i(T_o,T_p)}& f_i(T_o,T_p)\&GreaterEqual;0\\ 1+\left|f_i(T_o,T_p)\right|& f_i(T_o,T_p)<0\end{array}\right.---\left(4\right)$

其中，fitn_i表示第i个蜜源的适应度值，f_i(T_o,T_p)表示第i个蜜源的目标函数值；

采用人工蜂群三段式搜索策略对机械臂末端某一运动路径中的位姿插补点P的各个关节绝对角度值在从路径起点到终点的角度变化范围进行最优值搜索，并将搜索结果作为该位姿插补点P的逆向运动学方程的数值解，具体搜索步骤如下：

将整个搜索过程分为三个阶段，即前段搜索阶段、中段搜索阶段和后段搜索阶段；

a前段搜索阶段

前段搜索阶段包括蜜源初始化和前段搜索过程；

蜜源初始化包括如下步骤：

设初始蜜源的数量为N，将逆向运动学方程的解空间划分为N个大小相等的子空间，1个子空间内包含1个蜜源，蜜源的初始位置在子空间内随机确定；

定义分别表示解空间中第d维分量对应的关节在机械臂末端某一运动路径中的起点和终点的绝对角度值；其中，d∈{1,2,…,7}，解空间的每维分量均分为长度相等的m个数据段，且m⁷＝N，每个数据段的长度为

各维分量中均分的数据段用表达；其中，上标d∈{1,2,…,7}表示维度序列号，下标i∈{1,2,…,m}表示数据段序列号；

每次抽取各维中的1个数据段构建1个7维的子空间，每个子空间中至少有1个数据段与其他子空间不同，共有m⁷种不同的组合方式，即可构建N个子空间；

子空间用X_i表达，其中，i∈{1,2,…,N}；子空间的表达式为其中，分别为从第1至第7维中各维的1个数据段；

在每个子空间中随机产生1个蜜源的初始位置，即蜜源的初始位置的各维分量是其被包含的子空间的各维分量取值范围内的随机数，公式表达如下：

$x_i^d=s_{i\min }^d+\mathrm{rand}\left(\mathrm{0,1}\right)(s_{i\max }^d-s_{i\min }^d)---\left(5\right)$

其中，表示第i个子空间中的蜜源初始位置的第d维分量，表示第i个子空间中第d维分量的下限值，表示第i个子空间中第d维分量的上限值，rand(0,1)表示[0,1]内的1个随机数；

所有子空间内都产生蜜源的初始位置后，即开始进行前段搜索；

前段搜索采用标准的人工蜂群算法的搜索策略，即N个引领蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(6)产生1个新蜜源：

其中，i∈{1,2,…,N}，m∈{1,2,…,7}，j∈{1,2,…,N}，m和j为其取值范围内的随机数，且i≠j，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示随机选择的参考蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数；

然后根据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当引领蜂全部完成1次搜索后，跟随蜂将依据蜜源的选择概率P_i选择蜜源进行跟随，选择概率P_i的表达式如下： $P_i=\frac{\mathrm{fit}{n}_i}{\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}\mathrm{fit}{n}_i}---\left(7\right)$

然后N个跟随蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(6)产生1个新蜜源；

跟随蜂依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当跟随蜂全部完成1次搜索后，前段搜索即完成了1次循环；

当完成总循环中的前20％次循环后，前段搜索阶段结束；

b中段搜索阶段

将中段搜索阶段划分为N-1个循环数相同的调整段，设中段搜索阶段的循环数为L，调整段的循环数为l_A，且满足l_A＝L/(N-1)；

在中段搜索阶段，引领蜂在某一蜜源周围搜索新蜜源时的规则为：在不断搜索过程中，逐渐缩小可供随机选择的参考蜜源的数量；

中段搜索开始后，可供随机选择的参考蜜源的数量从第1个调整段开始循环前的N-1个，每经过1个调整段减少1个，一直减少到最后1个调整段开始循环前的1个；

设n为中段搜索中调整段的序列号，n＝1,2,…,N-1；在第n个调整段开始循环前，依据公式(4)计算当前的N个蜜源的适应度值并从大到小排序，排除掉适应度值最小的n个蜜源，将其余N-n个蜜源作为可供随机选择的参考蜜源；

在第n个调整段的l_A次循环中，N个引领蜂将以选择好的N-n个蜜源作为参考蜜源进行新蜜源的搜索，其表达式为：

其中，i∈{1,2,…,N}，m∈{1,2,…,7}，j∈{1,2,…,N-n}，m和j为其取值范围内的随机数，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第m维分量，表示在经过排序选择后的N-n个参考蜜源中随机选择的参考蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数；

如果某一蜜源属于可供随机选择的参考蜜源，则当引领蜂在该蜜源周围搜索新蜜源时，不能选择该蜜源本身作为参考蜜源；

然后依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当引领蜂全部完成1次搜索后，跟随蜂将依据公式(7)的选择概率P_i选择蜜源进行跟随；

N个跟随蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(8)产生1个新蜜源；

然后跟随蜂依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当N个跟随蜂全部完成1次搜索后，中段搜索的第n个调整段即完成了1次循环；

当完成l_A次循环后，中段搜索的第n个调整段的搜索结束；

当中段搜索的最后1个调整段开始循环时，此时可供选择的参考蜜源的数量为1个，引领蜂和跟随蜂在依照公式(8)在某一蜜源周围进行新蜜源搜索时，如果该蜜源同时也为参考蜜源，则停止该次搜索，蜜源的位置保持不变；

当中段搜索的N-1个调整段都完成循环后，即完成了总循环中的中间70％次循环，中段搜索阶段结束；

c后段搜索阶段

在后段搜索开始前，首先基于中段搜索阶段结束时的N个蜜源扩展N个新蜜源，使得蜜源总数达到2N个；

N个引领蜂依次进行1次搜索，生成N个新蜜源，搜索公式如下：

引领蜂在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(9)产生1个新蜜源：

其中，i∈{1,2,…,N}，m∈{1,2,…,7}，j∈{1,2,…,N}，m和j为其取值范围内的随机数，且i≠j，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示随机选择的目标蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数；

蜜源扩展成后，引领蜂和跟随蜂的数量也相应变为2N个，后段搜索将延续中段搜索的策略，即在后段搜索过程中，每次循环开始前，计算2N个蜜源的适应度值并寻找适应度值最高的蜜源，2N个引领蜂依次以适应度值最高的蜜源作为参考蜜源进行新蜜源的搜索，其表达式为：

其中，i∈{1,2,…,2N}，m∈{1,2,…,7}，m为其取值范围内的随机数，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示适应度值最高的蜜源的位置的第m维分量，表示[-1,1]内的一个随机数；

当引领蜂在参考蜜源周围搜索新蜜源时，则停止该次搜索，参考蜜源的位置保持不变；

然后依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当引领蜂全部完成1次搜索后，跟随蜂将依据蜜源公式(7)的选择概率P_i选择蜜源进行跟随；2N个跟随蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(10)产生1个新蜜源；

然后跟随蜂依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当跟随蜂全部完成1次搜索后，后段搜索即完成了1次循环；

当完成总循环中的后10％次循环后，后段搜索阶段结束；

整个搜索过程结束后，依据公式(4)分别计算2N个蜜源的适应度值，适应度值最高的蜜源是人工蜂群搜索出来的最优解，即为位姿插补点P的逆向运动学方程的数值解，其第1维至第7维的数值分别对应机械臂末端在该位姿插补点P时机械臂底座第1个旋转关节到末端第7个旋转关节的绝对角度值，求解过程结束。

本发明具有如下优点：

本发明方法用于求取七自由度关节串联型机械臂运动路径中位姿插补点的逆向运动学方程的数值解，该方法基于七自由度机械臂正向运动学方程建立目标函数，采用人工蜂群算法三段式搜索策略，对运动路径中的位姿插补点的各个关节绝对角度值在从起点到终点的角度变化范围内进行最优值搜索，并将搜索结果作为该位姿插补点逆向运动学方程的数值解。通过本发明述及的三段式搜索策略可增强人工蜂群算法的搜索能力、提高算法的收敛速度和计算精度，从而简化七自由度机械臂逆向运动学求解的复杂性，提高求解的速度和精度。

附图说明

图1为本发明中人工蜂群搜索方法的流程示意图。

具体实施方式

下面结合附图以及具体实施方式对本发明作进一步详细说明：

如图1所示，基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法，包括如下步骤：

设七自由度关节串联型机械臂的逆向运动学方程的解的维数为7，第1维至第7维分别对应机械臂底座第1个旋转关节到末端第7个旋转关节的绝对角度值；

设机械臂末端的运动路径中位姿插补点P的位姿矩阵T_p为：

$T_p=\left[\begin{array}{cccc}{n}_{\mathrm{px}}& o_{\mathrm{px}}& a_{\mathrm{px}}& p_{\mathrm{px}}\\ n_{\mathrm{py}}& o_{\mathrm{py}}& a_{\mathrm{py}}& p_{\mathrm{py}}\\ n_{\mathrm{pz}}& o_{\mathrm{pz}}& a_{\mathrm{pz}}& p_{\mathrm{pz}}\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

其中，子矩阵 $\left[\begin{array}{ccc}{n}_{\mathrm{px}}& o_{\mathrm{px}}& a_{\mathrm{px}}\\ n_{\mathrm{py}}& o_{\mathrm{py}}& a_{\mathrm{py}}\\ n_{\mathrm{pz}}& o_{\mathrm{pz}}& a_{\mathrm{pz}}\end{array}\right]$ 描述了在基础坐标系中机械臂末端在位姿插补点P时的姿态，其由三个单位矢量构成；

其中，[n_px n_py n_pz]^T为末端法线矢量，[o_px o_py o_pz]^T为末端方向矢量，[a_px a_py a_pz]^T为末端接近矢量；

子矩阵[p_px p_py p_pz]^T描述了在基础坐标系中机械臂末端在位姿插补点P时的位置；

设人工蜂群算法完成1次搜索机械臂逆向运动学方程数值解的循环后，各关节角的搜索值为θ_i，i＝1,2,…,7；

根据在基础坐标系下建立的机械臂各关节及末端的坐标系模型，利用θ_i并结合机械臂的机械参数建立机械臂的正向运动学方程，求得位姿矩阵T_o为：

$T_o=\left[\begin{array}{cccc}{n}_{\mathrm{ox}}& o_{\mathrm{ox}}& a_{\mathrm{ox}}& p_{\mathrm{ox}}\\ n_{\mathrm{oy}}& o_{\mathrm{oy}}& a_{\mathrm{oy}}& p_{\mathrm{oy}}\\ n_{\mathrm{op}}& o_{\mathrm{oz}}& a_{\mathrm{oz}}& p_{\mathrm{oz}}\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

其中，子矩阵 $\left[\begin{array}{ccc}{n}_{\mathrm{ox}}& o_{\mathrm{ox}}& a_{\mathrm{ox}}\\ n_{\mathrm{oy}}& o_{\mathrm{oy}}& a_{\mathrm{oy}}\\ n_{\mathrm{oz}}& o_{\mathrm{oz}}& a_{\mathrm{oz}}\end{array}\right]$ 描述了在基础坐标系中机械臂末端在位姿插补点P时的姿态，其由三个单位矢量构成；

其中，[n_ox n_oy n_oz]^T为末端法线矢量，[o_ox o_oy o_oz]^T为末端方向矢量，[a_ox a_oy a_oz]^T为末端接近矢量；

子矩阵[p_ox p_oy p_oz]^T描述了在基础坐标系中机械臂末端在位姿插补点P时的位置；

设置人工蜂群搜索的目标函数f(T_o,T_p)，表达式如下：

f(T_o,T_p)＝(|n_ox|-|n_px|)²+(|o_ox|-|o_px|)²+(|a_ox|-|a_px|)²+(|n_oy|-|n_py|)²+(|o_oy|-|o_py|)²+(|a_oy|-|a_py|)²+(|n_oz|-|n_pz|)²+(|o_oz|-|o_pz|)²+(|a_oz|-|a_pz|)²+(p_ox-p_px)²+(p_oy-p_py)²+(p_oz-p_pz)²  (3)

人工蜂群中蜜源的位置代表七自由度机械臂逆向运动学方程的解，蜜源适应度值的计算采用标准的人工蜂群算法中的计算公式，如下式(4)：

$\mathrm{fit}{n}_i\text{=}\left\{\begin{array}{cc}\frac{1}{1+f_i(T_o,T_p)}& f_i(T_o,T_p)\&GreaterEqual;0\\ 1+\left|f_i(T_o,T_p)\right|& f_i(T_o,T_p)<0\end{array}\right.---\left(4\right)$

其中，fitn_i表示第i个蜜源的适应度值，f_i(T_o,T_p)表示第i个蜜源的目标函数值；

采用人工蜂群三段式搜索策略对机械臂末端某一运动路径中的位姿插补点P的各个关节绝对角度值在从路径起点到终点的角度变化范围进行最优值搜索，并将搜索结果作为该位姿插补点P的逆向运动学方程的数值解，具体搜索步骤如下：

将整个搜索过程分为三个阶段，即前段搜索阶段、中段搜索阶段和后段搜索阶段；

a前段搜索阶段

前段搜索阶段包括蜜源初始化和前段搜索过程；

蜜源初始化包括如下步骤：

设初始蜜源的数量为N，将逆向运动学方程的解空间划分为N个大小相等的子空间，1个子空间内包含1个蜜源，蜜源的初始位置在子空间内随机确定；

定义分别表示解空间中第d维分量对应的关节在机械臂末端某一运动路径中的起点和终点的绝对角度值；其中，d∈{1,2,…,7}，解空间的每维分量均分为长度相等的m个数据段，且m⁷＝N，每个数据段的长度为

各维分量中均分的数据段用表达；其中，上标d∈{1,2,…,7}表示维度序列号，下标i∈{1,2,…,m}表示数据段序列号；

每次抽取各维中的1个数据段构建1个7维的子空间，每个子空间中至少有1个数据段与其他子空间不同，共有m⁷种不同的组合方式，即可构建N个子空间；

子空间用X_i表达，其中，i∈{1,2,…,N}；子空间的表达式为其中，分别为从第1至第7维中各维的1个数据段；

在每个子空间中随机产生1个蜜源的初始位置，即蜜源的初始位置的各维分量是其被包含的子空间的各维分量取值范围内的随机数，公式表达如下：

$x_i^d=s_{i\min }^d+\mathrm{rand}\left(\mathrm{0,1}\right)(s_{i\max }^d-s_{i\min }^d)---\left(5\right)$

其中，表示第i个子空间中的蜜源初始位置的第d维分量，表示第i个子空间中第d维分量的下限值，表示第i个子空间中第d维分量的上限值，rand(0,1)表示[0,1]内的1个随机数；

所有子空间内都产生蜜源的初始位置后，即开始进行前段搜索；

前段搜索采用标准的人工蜂群算法的搜索策略，即N个引领蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(6)产生1个新蜜源：

其中，i∈{1,2,…,N}，m∈{1,2,…,7}，j∈{1,2,…,N}，m和j为其取值范围内的随机数，且i≠j，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示随机选择的参考蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数；

然后根据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当引领蜂全部完成1次搜索后，跟随蜂将依据蜜源的选择概率P_i选择蜜源进行跟随，选择概率P_i的表达式如下： $P_i=\frac{\mathrm{fit}{n}_i}{\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}\mathrm{fit}{n}_i}---\left(7\right)$

然后N个跟随蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(6)产生1个新蜜源；

跟随蜂依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当跟随蜂全部完成1次搜索后，前段搜索即完成了1次循环；

当完成总循环中的前20％次循环后，前段搜索阶段结束；

b中段搜索阶段

将中段搜索阶段划分为N-1个循环数相同的调整段，设中段搜索阶段的循环数为L，调整段的循环数为l_A，且满足l_A＝L/(N-1)；

在中段搜索阶段，引领蜂在某一蜜源周围搜索新蜜源时的规则为：在不断搜索过程中，逐渐缩小可供随机选择的参考蜜源的数量；

中段搜索开始后，可供随机选择的参考蜜源的数量从第1个调整段开始循环前的N-1个，每经过1个调整段减少1个，一直减少到最后1个调整段开始循环前的1个；

设n为中段搜索中调整段的序列号，n＝1,2,…,N-1；在第n个调整段开始循环前，依据公式(4)计算当前的N个蜜源的适应度值并从大到小排序，排除掉适应度值最小的n个蜜源，将其余N-n个蜜源作为可供随机选择的参考蜜源；

在第n个调整段的l_A次循环中，N个引领蜂将以选择好的N-n个蜜源作为参考蜜源进行新蜜源的搜索，其表达式为：

其中，i∈{1,2,…,N}，m∈{1,2,…,7}，j∈{1,2,…,N-n}，m和j为其取值范围内的随机数，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第m维分量，表示在经过排序选择后的N-n个参考蜜源中随机选择的参考蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数；

如果某一蜜源属于可供随机选择的参考蜜源，则当引领蜂在该蜜源周围搜索新蜜源时，不能选择该蜜源本身作为参考蜜源；

然后依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当引领蜂全部完成1次搜索后，跟随蜂将依据公式(7)的选择概率P_i选择蜜源进行跟随；

N个跟随蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(8)产生1个新蜜源；

然后跟随蜂依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当N个跟随蜂全部完成1次搜索后，中段搜索的第n个调整段即完成了1次循环；

当完成l_A次循环后，中段搜索的第n个调整段的搜索结束；

当中段搜索的最后1个调整段开始循环时，此时可供选择的参考蜜源的数量为1个，引领蜂和跟随蜂在依照公式(8)在某一蜜源周围进行新蜜源搜索时，如果该蜜源同时也为参考蜜源，则停止该次搜索，蜜源的位置保持不变；

当中段搜索的N-1个调整段都完成循环后，即完成了总循环中的中间70％次循环，中段搜索阶段结束；

c后段搜索阶段

在后段搜索开始前，首先基于中段搜索阶段结束时的N个蜜源扩展N个新蜜源，使得蜜源总数达到2N个；

N个引领蜂依次进行1次搜索，生成N个新蜜源，搜索公式如下：

引领蜂在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(9)产生1个新蜜源：

其中，i∈{1,2,…,N}，m∈{1,2,…,7}，j∈{1,2,…,N}，m和j为其取值范围内的随机数，且i≠j，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示随机选择的目标蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数；

蜜源扩展成后，引领蜂和跟随蜂的数量也相应变为2N个，后段搜索将延续中段搜索的策略，即在后段搜索过程中，每次循环开始前，计算2N个蜜源的适应度值并寻找适应度值最高的蜜源，2N个引领蜂依次以适应度值最高的蜜源作为参考蜜源进行新蜜源的搜索，其表达式为：

其中，i∈{1,2,…,2N}，m∈{1,2,…,7}，m为其取值范围内的随机数，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示适应度值最高的蜜源的位置的第m维分量，表示[-1,1]内的一个随机数；

当引领蜂在参考蜜源周围搜索新蜜源时，则停止该次搜索，参考蜜源的位置保持不变；

然后依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当引领蜂全部完成1次搜索后，跟随蜂将依据蜜源公式(7)的选择概率P_i选择蜜源进行跟随；2N个跟随蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(10)产生1个新蜜源；

然后跟随蜂依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当跟随蜂全部完成1次搜索后，后段搜索即完成了1次循环；

当完成总循环中的后10％次循环后，后段搜索阶段结束；

整个搜索过程结束后，依据公式(4)分别计算2N个蜜源的适应度值，适应度值最高的蜜源是人工蜂群搜索出来的最优解，即为位姿插补点P的逆向运动学方程的数值解，其第1维至第7维的数值分别对应机械臂末端在该位姿插补点P时机械臂底座第1个旋转关节到末端第7个旋转关节的绝对角度值，求解过程结束。

本发明中人工蜂群搜索的三段式策略可在搜索空间内较为均匀的分布初始蜜源，避免了初始解的随机性；中段搜索策略可有效增强算法的整体搜索能力，提高收敛速度，并避免陷入早熟收敛；后段搜索策略可增强算法的局部精细搜索能力，提高计算精度。采用该算法求解七自由度机械臂逆向运动学方程时可简化其复杂性，提高求解的速度和精度。

当然，以上说明仅仅为本发明的较佳实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本说明书的教导下，所做出的所有等同替代、明显变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。

Claims (1)

1.基于人工蜂群搜索的七自由度机械臂逆向运动学求解方法，其特征在于，包括如下步骤：

设七自由度关节串联型机械臂的逆向运动学方程的解的维数为7，第1维至第7维分别对应机械臂底座第1个旋转关节到末端第7个旋转关节的绝对角度值；

设机械臂末端的运动路径中位姿插补点P的位姿矩阵T_p为：

$T_p=\left[\begin{array}{cccc}{n}_{\mathrm{px}}& o_{\mathrm{px}}& a_{\mathrm{px}}& p_{\mathrm{px}}\\ n_{\mathrm{py}}& o_{\mathrm{py}}& a_{\mathrm{py}}& p_{\mathrm{py}}\\ n_{\mathrm{pz}}& o_{\mathrm{pz}}& a_{\mathrm{pz}}& p_{\mathrm{pz}}\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

其中，子矩阵 $\left[\begin{array}{ccc}{n}_{\mathrm{px}}& o_{\mathrm{px}}& a_{\mathrm{px}}\\ n_{\mathrm{py}}& o_{\mathrm{py}}& a_{\mathrm{py}}\\ n_{\mathrm{pz}}& o_{\mathrm{pz}}& a_{\mathrm{pz}}\end{array}\right]$ 描述了在基础坐标系中机械臂末端在位姿插补点P时的姿态，其由三个单位矢量构成；

其中，[n_px n_py n_pz]^T为末端法线矢量，[o_px o_py o_pz]^T为末端方向矢量，[a_px a_py a_pz]^T为末端接近矢量；

子矩阵[p_px p_py p_pz]^T描述了在基础坐标系中机械臂末端在位姿插补点P时的位置；

设人工蜂群算法完成1次搜索机械臂逆向运动学方程数值解的循环后，各关节角的搜索值为θ_i，i＝1,2,…,7；

根据在基础坐标系下建立的机械臂各关节及末端的坐标系模型，利用θ_i并结合机械臂的机械参数建立机械臂的正向运动学方程，求得位姿矩阵T_o为：

$T_o=\left[\begin{array}{cccc}{n}_{\mathrm{ox}}& o_{\mathrm{ox}}& a_{\mathrm{ox}}& p_{\mathrm{ox}}\\ n_{\mathrm{oy}}& o_{\mathrm{oy}}& a_{\mathrm{oy}}& p_{\mathrm{oy}}\\ n_{\mathrm{op}}& o_{\mathrm{oz}}& a_{\mathrm{oz}}& p_{\mathrm{oz}}\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

其中，子矩阵 $\left[\begin{array}{ccc}{n}_{\mathrm{ox}}& o_{\mathrm{ox}}& a_{\mathrm{ox}}\\ n_{\mathrm{oy}}& o_{\mathrm{oy}}& a_{\mathrm{oy}}\\ n_{\mathrm{oz}}& o_{\mathrm{oz}}& a_{\mathrm{oz}}\end{array}\right]$ 描述了在基础坐标系中机械臂末端在位姿插补点P时的姿态，其由三个单位矢量构成；

其中，[n_ox n_oy n_oz]^T为末端法线矢量，[o_ox o_oy o_oz]^T为末端方向矢量，[a_ox a_oy a_oz]^T为末端接近矢量；

子矩阵[p_ox p_oy p_oz]^T描述了在基础坐标系中机械臂末端在位姿插补点P时的位置；

设置人工蜂群搜索的目标函数f(T_o,T_p)，表达式如下：

f(T_o,T_p)＝(|n_ox|-|n_px|)²+(|o_ox|-|o_px|)²+(|a_ox|-|a_px|)²+(|n_oy|-|n_py|)²+(|o_oy|-|o_py|)²+(|a_oy|-|a_py|)²+(|n_oz|-|n_pz|)²+(|o_oz|-|o_pz|)²+(|a_oz|-|a_pz|)²+(p_ox-p_px)²+(p_oy-p_py)²+(p_oz-p_pz)²  (3)

人工蜂群中蜜源的位置代表七自由度机械臂逆向运动学方程的解，蜜源适应度值的计算采用标准的人工蜂群算法中的计算公式，如下式(4)：

${\mathrm{fitn}}_i=\left\{\begin{array}{cc}\frac{1}{1+f_i(T_o,T_p)}& f_i(T_o,T_p)\&GreaterEqual;0\\ 1+\left|f_i(T_o,T_p)\right|& f_i(T_o,T_p)<0\end{array}\right.---\left(4\right)$

其中，fitn_i表示第i个蜜源的适应度值，f_i(T_o,T_p)表示第i个蜜源的目标函数值；

采用人工蜂群三段式搜索策略对机械臂末端某一运动路径中的位姿插补点P的各个关节绝对角度值在从路径起点到终点的角度变化范围进行最优值搜索，并将搜索结果作为该位姿插补点P的逆向运动学方程的数值解，具体搜索步骤如下：

将整个搜索过程分为三个阶段，即前段搜索阶段、中段搜索阶段和后段搜索阶段；

a前段搜索阶段

前段搜索阶段包括蜜源初始化和前段搜索过程；

蜜源初始化包括如下步骤：

设初始蜜源的数量为N，将逆向运动学方程的解空间划分为N个大小相等的子空间，1个子空间内包含1个蜜源，蜜源的初始位置在子空间内随机确定；

定义分别表示解空间中第d维分量对应的关节在机械臂末端某一运动路径中的起点和终点的绝对角度值；其中，d∈{1,2,…,7}，解空间的每维分量均分为长度相等的m个数据段，且m⁷＝N，每个数据段的长度为

各维分量中均分的数据段用表达；其中，上标d∈{1,2,…,7}表示维度序列号，下标i∈{1,2,…,m}表示数据段序列号；

每次抽取各维中的1个数据段构建1个7维的子空间，每个子空间中至少有1个数据段与其他子空间不同，共有m⁷种不同的组合方式，即可构建N个子空间；

子空间用X_i表达，其中，i∈{1,2,…,N}；子空间的表达式为其中，分别为从第1至第7维中各维的1个数据段；

在每个子空间中随机产生1个蜜源的初始位置，即蜜源的初始位置的各维分量是其被包含的子空间的各维分量取值范围内的随机数，公式表达如下：

$x_i^d=s_{i\min }^d+\mathrm{rand}\left(\mathrm{0,1}\right)(s_{i\max }^d-s_{i\min }^d)---\left(5\right)$

其中，表示第i个子空间中的蜜源初始位置的第d维分量，表示第i个子空间中第d维分量的下限值，表示第i个子空间中第d维分量的上限值，rand(0,1)表示[0,1]内的1个随机数；

所有子空间内都产生蜜源的初始位置后，即开始进行前段搜索；

前段搜索采用标准的人工蜂群算法的搜索策略，即N个引领蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(6)产生1个新蜜源：

其中，i∈{1,2,…,N}，m∈{1,2,…,7}，j∈{1,2,…,N}，m和j为其取值范围内的随机数，且i≠j，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示随机选择的参考蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数；

然后根据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当引领蜂全部完成1次搜索后，跟随蜂将依据蜜源的选择概率P_i选择蜜源进行跟随，选择概率P_i的表达式如下： $P_i=\frac{{\mathrm{fitn}}_i}{\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{fitn}}_i}---\left(7\right)$

然后N个跟随蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(6)产生1个新蜜源；

跟随蜂依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当跟随蜂全部完成1次搜索后，前段搜索即完成了1次循环；

当完成总循环中的前20％次循环后，前段搜索阶段结束；

b中段搜索阶段

将中段搜索阶段划分为N-1个循环数相同的调整段，设中段搜索阶段的循环数为L，调整段的循环数为l_A，且满足l_A＝L/(N-1)；

在中段搜索阶段，引领蜂在某一蜜源周围搜索新蜜源时的规则为：在不断搜索过程中，逐渐缩小可供随机选择的参考蜜源的数量；

中段搜索开始后，可供随机选择的参考蜜源的数量从第1个调整段开始循环前的N-1个，每经过1个调整段减少1个，一直减少到最后1个调整段开始循环前的1个；

设n为中段搜索中调整段的序列号，n＝1,2,…,N-1；在第n个调整段开始循环前，依据公式(4)计算当前的N个蜜源的适应度值并从大到小排序，排除掉适应度值最小的n个蜜源，将其余N-n个蜜源作为可供随机选择的参考蜜源；

在第n个调整段的l_A次循环中，N个引领蜂将以选择好的N-n个蜜源作为参考蜜源进行新蜜源的搜索，其表达式为：

其中，i∈{1,2,…,N}，m∈{1,2,…,7}，j∈{1,2,…,N-n}，m和j为其取值范围内的随机数，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第m维分量，表示在经过排序选择后的N-n个参考蜜源中随机选择的参考蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数；

如果某一蜜源属于可供随机选择的参考蜜源，则当引领蜂在该蜜源周围搜索新蜜源时，不能选择该蜜源本身作为参考蜜源；

然后依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当引领蜂全部完成1次搜索后，跟随蜂将依据公式(7)的选择概率P_i选择蜜源进行跟随；

N个跟随蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(8)产生1个新蜜源；

然后跟随蜂依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当N个跟随蜂全部完成1次搜索后，中段搜索的第n个调整段即完成了1次循环；

当完成l_A次循环后，中段搜索的第n个调整段的搜索结束；

当中段搜索的最后1个调整段开始循环时，此时可供选择的参考蜜源的数量为1个，引领蜂和跟随蜂在依照公式(8)在某一蜜源周围进行新蜜源搜索时，如果该蜜源同时也为参考蜜源，则停止该次搜索，蜜源的位置保持不变；

当中段搜索的N-1个调整段都完成循环后，即完成了总循环中的中间70％次循环，中段搜索阶段结束；

c后段搜索阶段

在后段搜索开始前，首先基于中段搜索阶段结束时的N个蜜源扩展N个新蜜源，使得蜜源总数达到2N个；

N个引领蜂依次进行1次搜索，生成N个新蜜源，搜索公式如下：

引领蜂在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(9)产生1个新蜜源：

其中，i∈{1,2,…,N}，m∈{1,2,…,7}，j∈{1,2,…,N}，m和j为其取值范围内的随机数，且i≠j，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示随机选择的目标蜜源j的位置的第m维分量，表示[-1,1]内的一个随机数；

蜜源扩展成后，引领蜂和跟随蜂的数量也相应变为2N个，后段搜索将延续中段搜索的策略，即在后段搜索过程中，每次循环开始前，计算2N个蜜源的适应度值并寻找适应度值最高的蜜源，2N个引领蜂依次以适应度值最高的蜜源作为参考蜜源进行新蜜源的搜索，其表达式为：

其中，i∈{1,2,…,2N}，m∈{1,2,…,7}，m为其取值范围内的随机数，表示搜索到的新蜜源的位置的第m维分量，表示蜜源i的位置的第d维分量，表示适应度值最高的蜜源的位置的第m维分量，表示[-1,1]内的一个随机数；

当引领蜂在参考蜜源周围搜索新蜜源时，则停止该次搜索，参考蜜源的位置保持不变；

然后依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当引领蜂全部完成1次搜索后，跟随蜂将依据蜜源公式(7)的选择概率P_i选择蜜源进行跟随；2N个跟随蜂依次在蜜源i周围随机选择1个参考蜜源进行搜索，并根据公式(10)产生1个新蜜源；

然后跟随蜂依据公式(4)分别计算参考蜜源和新蜜源的适应度值；

如果新蜜源的适应度值大于参考蜜源的适应度值，则放弃该参考蜜源，新蜜源成为可供随机选择的参考蜜源；否则，保留参考蜜源并放弃新蜜源；

当跟随蜂全部完成1次搜索后，后段搜索即完成了1次循环；

当完成总循环中的后10％次循环后，后段搜索阶段结束；

整个搜索过程结束后，依据公式(4)分别计算2N个蜜源的适应度值，适应度值最高的蜜源是人工蜂群搜索出来的最优解，即为位姿插补点P的逆向运动学方程的数值解，其第1维至第7维的数值分别对应机械臂末端在该位姿插补点P时机械臂底座第1个旋转关节到末端第7个旋转关节的绝对角度值，求解过程结束。