基于改进的天牛须搜索算法的机械臂避障路径规划方法

基于改进的天牛须搜索算法的机械臂避障路径规划方法


技术实现要素：

1.本发明涉及自动控制技术领域，具体涉及一种机械臂避障路径规划方法。


背景技术：

2.近年来，随着科技的进步，我国制造业掀起了以机器人为核心的智能化、无人化的研究热潮。根据发展需要，很多情况下要求机器人能够根据复杂环境情况自主避障，这给机器人避障路径规划技术带来了广泛的应用前景。
3.将现有路径规划算法改进甚至结合成更高效的复合算法以适应更加复杂的路径规划任务，是目前的主流研究方向。目前用于路径规划优化的智能算法包括萤火虫算法(firefly algorithm,fa)、粒子群算法(particle swarm optimization,pso)、遗传算法(genetic algorithm,ga)、人工蜂群算法(artificial bee colony,abc)、快速探索随机树算法(rrt)、蚁群算法(aco)等。姜向远在模仿自然界中天牛捕食行为的基础上，提出了一种元启发式、高随机性、快速收敛的优化算法——天牛须搜索算法(beetle antennae search,bas)。如图3所示，在自然界中，天牛的捕食主要依靠头部两侧的触角，触角内的气味细胞能够感知空气中分散的信息素的浓度。由于不同触角上的气味细胞感知到两个方向上的浓度差异，天牛就会向浓度更大的一侧移动，从而不断更新位置，最终找到食物。根据天牛捕食行为，bas算法的搜索过程主要由方向和步长两个参数控制。通过设置随机方向和减小搜索步长，可以保证在较大的搜索范围内进行优化，并在优化结束时快速收敛。同时，由于姿态随机性强，步长自衰减的限制，初始参数设置对优化结果影响较大，在机器人姿态规划领域应用时需要进行调整和改进。
4.六自由度机械臂在工业生产中有着非常广泛的应用，具有较大的研究价值。但目前使用的路径规划算法大多以随机采样的路径规划算法为主，虽然可以成功找到合适的运动路径，但由于进行了大范围的随机搜索，导致算法运算效率不高。
5.发明内容
6.本发明的目的是提供一种基于改进的bas算法的机械臂避障轨迹规划方法，有效提高了算法在三维姿态规划中的路径探索和避障能力。
7.为实现上述目的，本发明采用的技术方案是：基于改进的天牛须搜索算法的机械臂避障路径规划方法，包括以下几个步骤：
8.建立姿态规划结果的优化评估模型；
9.获取机械臂末端起点和终点位姿，利用逆运动学解析法求解起点关节角和终点关节角，确定机械臂在空间中的位置；
10.建立机械臂和障碍物碰撞检测模型；
11.根据碰撞检测获得的试验参数，结合优化评估模型，利用改进后的bas4-ao算法生成机械臂避障路径。
12.进一步地，优化评估模型包括用于优化机械臂关节角度的函数，其表达式为：
38.其中，l(p)作为四个判断点的适应度评价标准，符号函数min(
·
)是用于提取四个判断点中适应度函数值最小的点；矩阵rg为目标点的旋转矩阵，r
t
为当前点的旋转矩阵，以λ2作为匹配系数，δ
*
作为rg到r
t
的逼近步长；
39.(4)、碰撞检测及搜索步长调整
40.进行机械臂轴体与障碍物的碰撞检验，若无碰撞，则直接转入步骤(2)执行下一次迭代循环；若发生碰撞，则须衰减全局搜索步长，清除此次路径点信息并再次搜索新的路径点。
41.碰撞检测过程是将机械臂凸包简化模型和障碍物模型在三个坐标平面上投影，通过判断投影是否重合来确定是否发生碰撞。
42.(5)、进行关节角度寻优判断
43.根据机械臂末端执行器空间坐标求解机械臂关节角度，判断当前机械臂末端执行器关节角度θ
t
和目标点关节角度θg之间的角度差是否小于阈值τ，若不小于阈值τ，则返回步骤(2)继续生成路径点；若小于阈值，则转入步骤(6)；
44.(6)、关节角度局部变步长搜索：
[0045][0046][0047][0048][0049][0050]
其中，θ(θ
t
)作为关节角度的适应度评价标准；
[0051]
局部变步长的更新如下所示：
[0052][0053][0054]
式中为最小搜索步长，m为该局部衰减迭代的最大迭代次数；
[0055]
(7)、路径点精度判断
[0056]
判断当前路径点与目标路径点的空间位置距离，若该距离不小于精度阈值μ，则返回步骤(6)，若小于精度阈值μ，则进入步骤(8)；
[0057]
(8)、路径修剪
[0058]
计算每个路径点各关节角度变化下的模量，确定最小关节变化所在的点，具体如下式所示：
[0059][0060]
其中，表示在对规划路径点进行正序或逆序计算过程中，与当前点关节角度最接近的规划路径中的第i个点；
[0061]
将当前计算的路径点和具有最小关节变化的点进行标注，对标注点之间的路径点进行平滑处理，并使用当前最小关节变化的点的关节角替换标注点之间路径点的关节角。
[0062]
本发明提供的基于改进的天牛须搜索算法的机械臂避障路径规划方法，生成两个相互正交的搜索方向并得出四个判断点的空间位置坐标，能够提高全局搜索能力，可以有效简化初始路径，消除路径中的抖动，确保机械臂运行过程的安全性。
附图说明
[0063]
图1为本发明实施例中基于改进的天牛须搜索算法的机械臂避障路径规划方法的流程图；
[0064]
图2是具有6个自由度的安川制造motoman-es165d机械手模型图，a模型图，b是其坐标系模型图；
[0065]
图3是本发明的机械臂位置和障碍物分布示意图；
[0066]
图4是本发明采用凸包结构近似的机械臂建模的示意图；
[0067]
图5是本发明采用轴体包络法近似的障碍物建模的示意图；
[0068]
图6是本发明采用的判断点优化部分简化图；
[0069]
图7是初始路径生成的路径信息图；
[0070]
图8是初始路径生成的关节角度变化趋势图，a是靠近底座的前3个关节角度变化情况，b是靠近末端执行器的后3个关节角度变化情况；
[0071]
图9是经过修剪的路径信息图；
[0072]
图10是修剪后路径的关节角度变化趋势图，a是靠近底座的前3个关节角度变化情况，b是靠近末端执行器的后3个关节角度变化情况。
具体实施方式
[0073]
为了便于理解本发明，下面结合附图和具体实施例，对本发明进行更详细的说明。附图中给出了本发明的较佳的实施例。但是，本发明可以以许多不同的形式来实现，并不限于本说明书所描述的实施例。相反地，提供这些实施例的目的是使对本发明公开内容的理解更加透彻全面。
[0074]
实施例1本发明提供了一种基于改进的天牛须搜索算法的机械臂避障路径规划方法，可应用于机械臂姿态规划，流程如图1所示，具体包括以下步骤：
[0075]
步骤一、利用旋量理论建立机械臂数学模型，建立姿态规划结果的优化评估模型，以获得最优的机械臂关节角度及运行路径长度
[0076]
本实施例中，以安川制造motoman-es165d型机械臂为研究对象，利用旋量理论建立数学模型，具体参数见图2。
[0077]
对于一个具有6个自由度的串联机械臂，将它的各个关节运动整合成一个整体，就可以获得用于表征该机械臂正运动学模型的指数积公式：
[0078][0079]
式中,g
st
(θ)表示机械臂经过关节运动后末端执行器的刚体坐标系在基坐标系中的位姿矩阵，g
st
(0)表示各关节在零时刻下末端执行器的刚体坐标系在基坐标系中的初始位姿矩阵,θ和ξ分别表示对应关节的关节角度与关节旋量。通过指数积公式，可以建立起由
机械臂所有关节的运动旋量表示的正运动学模型。
[0080]
机械臂的初始位姿表示为：
[0081][0082]
即可求取该机械臂的指数积公式并可以表示为如下形式：
[0083][0084]
其中齐次坐标的数学表达公式如下：
[0085]nx
＝c1c
23c5-s5(c1c4s
23
+s1s4)
[0086]
ny＝c
23
c5s
1-s5(c1c
4-c4s1s
23
)
[0087]
nz＝-c3(c5s2+c2c4s5)-s3(c2c
5-c4s2s5)
[0088]ox
＝c6(c1s
23s4-c4s1)+s6(c5s1s4+c1(c4c5s
23
+c
23
s5))
[0089]
oy＝c1(c4c
6-c5s4s6)+s1(c6s
23
s4+s6(c4c5s
23
+c
23
s5))
[0090]
oz＝c
23
(c6s4+c4c5s6)-s
23
s5s6[0091]ax
＝s1(c5c6s4+c4s6)+c1(c4c5c6s
23-s4s6s
23
+c
23
c6s5)
[0092]ay
＝c6(c4c5s1s
23-c1c5s4+c
23
s1s5)-s6(c1c4+s1s
23
s4)
[0093]az
＝c
23
(c4c5c
6-s4s6)-c6s
23
s5[0094]
p
x
＝d6(c1s2s
3-c1c2c3)c4s
5-d6s1s4s
5-(c1c2s3+c1s2c3)(d4+d6c5)+a3c1c2c
3-a3c1s2s3+a2c1c2[0095]
py＝d6(s1s2s
3-s1c2c3)c4s
5-d6c1s4s
5-(s1c2s3+s1s2c3)(d4+d6c5)+a3s1c2c
3-a3s1s2s3+a2s1c2[0096]
pz＝d6(s2c3+c2s3)c4s5+d1+(s2s
3-c2c3)(d4+d6c5)-a3c2s
3-a2s2[0097]
式中，符号c表示三角函数中的余弦函数，s符号表示正弦函数,d表示对应关节之间的相对距离，s
23
表示sin(θ2+θ3)和c
23
表示cos(θ2+θ3)。
[0098]
同理，由于为六个自由度的串联机械臂，适应度评价函数如下：
[0099][0100][0101]
式中，θ(θ
t
)表示六自由度机械臂的六个关节经过n次搜索迭代后的角度差之和，就是所有路径点角度差值的和；l(p)表示机械臂末端执行器在空间中的移动距离，k表示路径中包含的路径点个数。m为路径点的总个数。
[0102]
r6指该变量为一个有6个元素向量，每个元素都是实数，属于实数域r(r6可以解为包含6个元素的向量，其中每个元素是实数)，r3同理。
[0103]
步骤二、读取机械臂末端起点和终点位姿，利用逆运动学解析法求解起点关节角和终点关节角
[0104]
在获得机械臂末端位姿后，需要获取机械臂的各个关节角才能结合之前建立的机械臂模型，确定机械臂在空间中的位置。因此，通过逆运动学求解关节角是十分重要的。
[0105]
步骤三、建立机械臂和障碍物碰撞检测模型
[0106]
在实际实施过程中，机械臂和障碍物的空间位置关系见图3所示。障碍物数据如下表1所示。
[0107]
表1障碍物模型数据
[0108][0109]
由于实际机械臂模型较为复杂，实施过程中建立了对应的凸包模型，具体见图4所示。障碍物简化模型采用了轴体包络，考虑到实际的误差还进行了膨化，具体见图5所示。
[0110]
步骤四、利用改进后的bas4-ao算法生成有效的机械臂避障路径，具体如下：
[0111]
(1)、初始化各项参数
[0112]
根据碰撞试验的机械臂参数，各个关节角度范围见下表2所示。
[0113]
表2机械臂各个关节角度有效区间
[0114][0115]
根据试验经验，算法相关参数的选择见下表3所示。
[0116]
表3路径规划参数配置组合
[0117][0118]
(2)、路径点搜索方向生成
[0119]
与传统bas算法的两个判断点相比，改进后的bas4-ao算法拥有四个位置不同的判断点，如图6所示，因此需要两个相互正交的方向向量。
[0120]
根据以下公式计算每个路径点的搜索方向：
[0121][0122][0123]
式中，a1和a2是两个比例常数，rand(
·
)为均匀分布于[0,1]上的随机值。本实施例中，由于a1为1，a2为10，则试验中搜索方向受当前点到目标点的连线方向的影响较大。表示从左到右的方向向量；表示从下到上的方向向量。
[0124]
将方向向量归一化处理：
[0125][0126][0127][0128][0129]
其中，为具有目标点处关节信息的目标导向部分，inυ(
·
)为三维坐标转换为关节角的逆运动学方程；其中原始随机方向仍然保留，常数向量xg表示目标点的坐标。
[0130]
(3)、路径点空间位置生成
[0131]
进行第t时刻迭代时，围绕空间坐标p的四个判断点的空间位置坐标为：
[0132][0133][0134][0135][0136]
其中，搜索步长δ
*
是一个动态调整因子，随着搜索迭代循环衰减。本实施例中，搜索步长δ
*
初始值为0.8。
[0137]
然后，生成下一次迭代的位姿信息：
[0138][0139]rt+1
＝r
t
+δ
*
*(r
g-r
t
)*λ2[0140]
其中，l(p)作为四个判断点的适应度评价标准，符号函数min(
·
)是用于提取四个判断点中适应度函数值最小的点。矩阵rg为目标点的旋转矩阵，r
t
为当前点的旋转矩阵，以λ2作为匹配系数，δ
*
在这里作为rg到r
t
的逼近步长和全局步长相等。
[0141]
(4)、碰撞检测及搜索步长调整
[0142]
进行机械臂轴体与障碍物的碰撞检验，若无碰撞，则直接转入步骤(2)执行下一次迭代循环；若发生碰撞，则须衰减全局搜索步长，清除此次路径点信息并再次搜索新的路径点。碰撞检测过程是将机械臂凸包简化模型和障碍物模型在三个坐标平面上投影，通过判断投影是否重合来确定是否发生碰撞。
[0143]
全局搜索步长数值衰减模型表示如下：
[0144][0145]
其中，λ1是一个常数，该试验设置为0.998。
[0146]
(5)、进行关节角度寻优判断
[0147]
根据机械臂末端执行器空间坐标求解机械臂关节角度，判断当前机械臂末端执行器关节角度θ
t
和目标点关节角度θg之间的角度差是否小于阈值τ；若不小于阈值τ，则返回步骤(2)继续生成路径点；若小于阈值τ，则转入步骤(6)开始关节角度寻优。
[0148]
(6)、关节角度局部变步长探索
[0149]
在当前机械臂末端执行器关节角度θ
t
和目标点关节角度θg之间的角度差小于阈值τ时，搜索转移到以角度优化为主导的姿态控制部分，具体如下：
[0150][0151][0152][0153][0154][0155]
其中迭代原理与坐标点的迭代相似，将空间坐标p
t
替换为关节角θ
t
，适应度评价函数根据前文将l(p
t
)替换为θ(θ
t
)。
[0156]
局部变步长的更新如下所示：
[0157][0158][0159]
式中为最小搜索步长，m为该局部衰减迭代的最大迭代次数。在进行这种局部迭代探索时，一旦搜索到一个有效的路径点，就跳出局部迭代，并重用全局步长进行优化。
[0160]
(7)、路径点精度判断
[0161]
判断当前路径点与目标路径点的空间位置距离，若该距离不小于精度阈值μ，则返回步骤(6)，若小于精度阈值μ，则进入步骤(8)。
[0162]
(8)、路径修剪
[0163]
试验生成的路径信息和关节变化信息见图7和图8所示。图8中可以明显发现，关节角度存在剧烈的波动。因此，为了确保机械臂运行的安全性，在迭代生成路径后，还需要通过预处理去除路径中可能存在的奇异点。
[0164]
本实施例中，首先依次计算了每个路径点和后续点各关节角度变化下的模量，确定了最小关节变化所在的点。具体如下式所示：
[0165][0166]
其中，表示在对规划路径点进行正序或逆序计算过程中，与当前点关节角度最接近的规划路径中的第i个点。
[0167]
将当前计算的路径点和具有最小关节变化的点进行标注，对标注点之间的路径点进行平滑处理，并使用当前最小关节变化的点的关节角替换标注点之间路径点的关节角。该方法对路径修剪后路径信息和关节变化信息见图9和图10所示。
[0168]
实施例2为了比较本发明改进后的bas4-ao算法和现有rrt算法以及bas算法的区别，本实施例统计了多次相同情况下，使用三种算法对机械臂进行路径规划的实验数据，并进行比较，结果见表4。
[0169]
表4改进后的bas4-ao算法和rrt算法、bas算法的优化比较
[0170][0171]
与传统的rrt算法相比，本发明改进的bas4-ao算法的时间效率缩短了87.62％，路径长度缩短了29.83％，整体性能优于rrt算法在机械手姿态规划中的应用，表明该算法具有较好的路径优化能力。并且，本发明中的“四须”(四个判断点),相对于bas算法原有的“两须”具有可选择的路径方向更多，全局寻优效果更好，使其路径长度缩短了18.87％，并使其全局搜索能力与原bas算法相比提升了13％。
[0172]
六自由度机械臂在工业生产中有着非常广泛的应用，具有较大的研究价值。但目前使用的路径规划算法大多以随机采样的路径规划算法为主，虽然可以成功找到合适的运动路径，但由于进行了大范围的随机搜索，导致算法运算效率不高。本发明在提出一种改进的bas算法，简称bas4-ao算法。首先，引入适应度函数和目标位置搜索导向，使搜索更有目标性，减少算法运行时间；然后，采用三维笛卡尔坐标和关节角度搜索相结合的方式，从多角度保证机械臂的运行稳定。由实际试验结果可以看出，相比于rrt算法，bas4-ao不仅能形成一个合适的运动规划路径，而且具有更快运行效率和更好的运行结果。

技术特征：
1.基于改进的天牛须搜索算法的机械臂避障路径规划方法，其特征在于，包括以下步骤：建立姿态规划结果的优化评估模型；获取机械臂末端起点和终点位姿，利用逆运动学解析法求解起点关节角和终点关节角，确定机械臂在空间中的位置；建立机械臂和障碍物碰撞检测模型；根据碰撞检测结果，结合优化评估模型，利用改进后的bas4-ao算法生成机械臂避障路径。2.根据权利要求1所述的基于改进的天牛须搜索算法的机械臂避障路径规划方法，其特征在于，所述优化评估模型包括用于优化机械臂关节角度的函数，其表达式为：其中，为机械臂关节经过n次搜索迭代后的角度差之和；变量表示在路径点i上采样时刻t处的关节角度；m表示路径规划过程中所有路径点的总数。3.根据权利要求1所述的基于改进的天牛须搜索算法的机械臂避障路径规划方法其特征在于，所述优化评估模型包括用于优化运行路径长度的函数，其表达式为：式中，l(p)表示机械臂末端执行器在空间中的移动距离，变量p
it
表示在路径点i上采样时刻t处的机械手端点位置；k表示路径中包含的路径点个数。4.根据权利要求2所述的基于改进的天牛须搜索算法的机械臂避障路径规划方法，其特征在于，算法完成的时间优化函数表达式为：t＝t
end-t
start
其中，t
start
为算法优化的起始点，t
end
为机械手操作完成的时间点；搜索迭代次数其中t
s
为采样区间。5.根据权利要求1所述的基于改进的天牛须搜索算法的机械臂避障路径规划方法，其特征在于，碰撞检测模型中，使用凸包简化模型作为机械臂模型，使用轴体包络模型作为障碍物模型。6.根据权利要求1-5任一项所述的基于改进的天牛须搜索算法的机械臂避障路径规划方法，其特征在于，利用改进后的bas4-ao算法生成机械臂避障路径包含以下几个步骤：(1)、初始化各项参数；(2)、生成两个相互正交的路径点搜索方向，其中搜索方向的计算公式如下：(2)、生成两个相互正交的路径点搜索方向，其中搜索方向的计算公式如下：
a1和a2是两个比例常数，rand(
·
)为均匀分布于[0,1]上的随机值；为随机单位向量，为目标点相对于当前路径点的方向向量；表示从左到右的方向向量；表示从下到上的方向向量；(3)、路径点空间位置生成进行第t时刻迭代时，围绕空间坐标p的四个判断点的空间位置坐标为：进行第t时刻迭代时，围绕空间坐标p的四个判断点的空间位置坐标为：进行第t时刻迭代时，围绕空间坐标p的四个判断点的空间位置坐标为：进行第t时刻迭代时，围绕空间坐标p的四个判断点的空间位置坐标为：其中，搜索步长δ
*
是一个动态调整因子，随着搜索迭代循环衰减；然后，生成下一次迭代的位姿信息：r
t+1
＝r
t
+δ
*
*(r
g-r
t
)*λ2其中，l(p)作为四个判断点的适应度评价标准，符号函数min(
·
)是用于提取四个判断点中适应度函数值最小的点；矩阵r
g
为目标点的旋转矩阵，r
t
为当前点的旋转矩阵，以λ2作为匹配系数，δ
*
作为r
g
到r
t
的逼近步长；(4)、碰撞检测及搜索步长调整进行机械臂轴体与障碍物的碰撞检验，若无碰撞，则直接转入步骤(2)执行下一次迭代循环；若发生碰撞，则须衰减全局搜索步长，清除此次路径点信息并再次搜索新的路径点；(5)、进行关节角度寻优判断根据机械臂末端执行器空间坐标求解机械臂关节角度，判断当前机械臂末端执行器关节角度θ
t
和目标点关节角度θ
g
之间的角度差是否小于阈值τ，若不小于阈值τ，则返回步骤(2)继续生成路径点；若小于阈值τ，则转入步骤(6)；(6)、关节角度局部变步长搜索：(6)、关节角度局部变步长搜索：(6)、关节角度局部变步长搜索：(6)、关节角度局部变步长搜索：(6)、关节角度局部变步长搜索：其中，作为关节角度的适应度评价标准；局部变步长的更新如下所示：
式中为最小搜索步长，m为该局部衰减迭代的最大迭代次数；(7)、路径点精度判断判断当前路径点与目标路径点的空间位置距离，若该距离不小于精度阈值μ，则返回步骤(6)，若小于精度阈值μ，则进入步骤(8)；(8)、路径修剪计算每个路径点各关节角度变化下的模量，确定最小关节变化所在的点，具体如下式所示：其中，表示在对规划路径点进行正序或逆序计算过程中，与当前点关节角度最接近的规划路径中的第i个点；将当前计算的路径点和具有最小关节变化的点进行标注，对标注点之间的路径点进行平滑处理，并使用当前最小关节变化的点的关节角替换标注点之间路径点的关节角。7.根据权利要求6所述的基于改进的天牛须搜索算法的机械臂避障路径规划方法，其特征在于，步骤(2)中，计算搜索方向之前，需要将方向向量归一化，公式如下：征在于，步骤(2)中，计算搜索方向之前，需要将方向向量归一化，公式如下：征在于，步骤(2)中，计算搜索方向之前，需要将方向向量归一化，公式如下：征在于，步骤(2)中，计算搜索方向之前，需要将方向向量归一化，公式如下：其中，为具有目标点处关节信息的目标导向部分，inυ(
·
)为三维坐标转换为关节角的逆运动学方程；其中原始随机方向仍然保留，常数向量x
g
表示目标点的坐标。

技术总结
本发明涉及自动控制技术领域，具体涉及一种机械臂避障路径规划方法。基于改进的BAS算法的机械臂避障轨迹规划方法，包括以下步骤：建立姿态规划结果的优化评估模型；获取机械臂末端起点和终点位姿，利用逆运动学解析法求解起点关节角和终点关节角，确定机械臂在空间中的位置；建立机械臂和障碍物碰撞检测模型；根据碰撞检测结果，结合优化评估模型，利用BAS4-AO算法生成机械臂避障路径。本发明提供的基于改进的天牛须搜索算法的机械臂避障路径规划方法，可以有效简化初始路径，消除路径中的抖动，确保机械臂运行过程的安全性。确保机械臂运行过程的安全性。确保机械臂运行过程的安全性。


技术研发人员：季雅文 姜向远 马思乐 栾义忠 马晓静 陈纪旸 孙文旭 张振强 李进
受保护的技术使用者：山东大学
技术研发日：2021.11.26
技术公布日：2022/3/8