基于改进麻雀搜索算法的机械臂路径规划*

2023-02-04沈孝龙王吉芳郭子昇

组合机床与自动化加工技术 2023年1期

沈孝龙，王吉芳，郭子昇

(北京信息科技大学机电工程学院，北京 100192)

0 引言

随着机器人在生产制造领域的大范围应用，工业机械臂在生产线的装机量逐年上升，稠密布置的机械臂在有限的工作空间内暴露出机器人与现场障碍物物理接触时产生的安全问题。在即将碰撞瞬间前能否对碰撞进行检测避免设备撞击损坏与安全事故发生，使机械臂可以安全避过潜在障碍物是该类机器人能否被进一步推广应用与提高单位空间装机量的关键所在，因此，研究一种使机械臂可在三维空间避障的路径规划算法具有极高的工程应用与学术研究价值。

目前，避碰研究分全局避碰与局部避碰两种[1]。前者需获知大量周围环境信息后才能对其进行规划，有A*搜索、马尔科夫决策法与随机树形法[2]等。此类方法缺点是：无法约束关节位姿、易陷入局部灾难、耗时等。局部避碰使用碰撞检测，能根据物体当前运动状态与障碍物关系实时给出避碰方案，受到广泛研究。曹博等[3]利用改进人工势场法解决冗余机械臂无碰运动。季晓明等[4]把无碰规划问题变为最优化问题，设计天牛须搜索机制的循环神经网络解决避碰问题。刘建春等[5]对机械臂建立球体和胶囊体包围盒模型，使用二次投影法完成对双机械臂的快速碰撞检测。王刚等[6]利用人工势场法修改蚁群算法的启发值参数并提出吸引素修改信息素参数更新规则，实现了机器人三维路径规划。王猛等[7]设计指引函数改进蚁群算法初期搜索的盲目性与随机性，通过增加阀值和路径权重改造信息素更新规则，改进算法性能。朱晨曦等[8]改造蚁群算法启发值和信息素更新规则，保留前代最优路径，提高了人机象棋实用性与效率。

目前蚁群算法在机械臂路径规划研究较多，改进的策略基本上是对启发值、信息素及选择概率方面改进，或者融合人工势场算法，但此类方法耗时较长，在求解最优路径时较易陷入局部最优，对机械臂三维避碰路径规划研究少。实际应用的机械臂，工作场景多变复杂，同时存在障碍物，给路径规划增加了难度。所以，针对复杂三维空间的避障路径规划有重要探索价值。

1 机械臂运动学分析

1.1 正运动学分析

仿真实验以6-DOF串联机械臂为对象，进行算法的验证。机械臂的连杆坐标系如图1所示。

图1 机械臂连杆坐标系

由D-H法，连杆坐标系i-1到i的变换由关节角θi、连杆扭角αi、连杆偏距di、连杆长ai这4个参数描述，结合产品手册可得其D-H参数如表1所示。

表1 机械臂D-H参数表

相邻两连杆位姿变换可用i-1Ti表示，如式(1)所示。

(1)

式中，cαi-1=cosαi-1；sαi-1=sinαi-1；cθi=cosθi；sθi=sinθi；i=1,2,…,n。

则末端执行器相对基坐标系位姿矩阵0T6如下：

(2)

式中，R=[n,o,a]T系末端工具的姿态信息；P=[px,py,pz]T系末端工具的位置信息。把表1各参数带入式(1)和式(2)，即可求得各位姿参数方程，从而得出机械臂正运动学描述。

1.2 逆运动学分析

由图1知，机械臂符合Pieper准则,可根据其正运动学方程，通过多次左乘逆矩阵将其进行逆变换，最终可以求到8组逆解，具体计算过程不在本文论述。对于轨迹规划这些逆解并不都满足，因各个关节的转动范围有约束，自身构型问题也会存在奇异解。另外，我们常常希望在满足轨迹规划的前提下，使机械臂的运动具有优异的第二特性，如最佳柔顺或最低能量消耗等。我们需要在满足机械臂工作要求前提下，去除非法解，然后根据某准则得出一组最优解，本文期望各相邻路径节点上的关节转角差的绝对值和最小以满足较低能量消耗。

2 碰撞检测分析

由于障碍物是已知且静止的，故碰撞检测时只需保证臂自身在运动时避免与障碍物碰撞即可，处理手段是利用层次包围盒法[9]，即用规则几何体包裹形状复杂不规则障碍物。

2.1 包络方法

使用长方体包裹空间中的障碍物并满足其各棱边均与坐标轴平行。数学描述为：

R={(x,y,z)|lx≤x≤mx,ly≤y≤my,lz≤z≤mz}

(3)

式中，(lx,ly,lz)表示长方体棱边沿坐标轴X、Y、Z方向的最小值；(mx,my,mz)表示长方体棱边沿坐标轴X、Y、Z方向的最大值。

本文研究的是多自由度串联机械臂，因其本体各个连杆的形状与圆柱体类似，使用圆柱体包络法能够对机身简化，同时较好留存机械臂的结构特征。这里底座在其运动中位姿并不变化，只需对余下的连杆包络简化。

在使用圆柱体简化机械臂，长方体简化障碍物后，要想在空间中判断两者间的位置状态依旧不易，为进一步简化计算，将机械臂连杆抽象为直线，把原圆柱体半径加到障碍物模型中进行扩大[10]，最终变为检测直线与平面相交的问题，就可判断机械臂与障碍物是否碰撞。简易化模型如图2所示。

图2 机械臂与障碍物简化模型

假设连杆i直线方程为Ri，障碍物模型Ii，膨胀后的障碍物模型用符号Oi表示。我们假定Ii的8个顶点坐标依次为(x1,y1,z1)…(x8,y8,z8)。膨胀厚度设为r，顶点1沿x轴的单位向量Px为：

(4)

顶点1沿y轴的单位向量Py为：

(5)

顶点1沿z轴的单位向量Pz为：

(6)

综上所述，求得膨胀后第一个顶点的坐标：

O1=[x1,y1,z1]+Px+Py+Pz

(7)

重复上述过程，易求得Oi的其余7个顶点坐标。

2.2 碰撞检测

方法如下：假设连杆i两端关节i和i+1的坐标为p1(x1,y1,z1)和p2(x2,y2,z2)，则连杆i所在直线方向H=(x2-x1,y2-y1,z2-z1)。假设某空间障碍物的某一平面S其4个端点为s1(xs1,ys1,zs1)、s2(xs2,ys2,zs2)、s3(xs3,ys3,zs3)、s4(xs4,ys4,zs4)，法向量为F(f1,f2,f3)。由空间几何原理知，当H·F=0时，线与面平行，无交点；反之，线与面相交，有某交点p0(x0,y0,z0)。由几何关系知，交点为碰撞点需要满足：①交点在连杆线段上，②交点在空间平面内。障碍物抽象为长方体，其表面方向有3种：分别平行于x轴上下表面、y轴左右表面、z轴前后表面。下面以平行于x轴上表面平面S举例，其4个端点为s1(xs1,ys1,zs1)、s2(xs2,ys1,zs1)、s3(xs2,ys3,zs1)、s4(xs1,ys3,zs1)，连杆i若与平面S相交，需满足如下约束：

min(z1,z2)min(xs1,xs2)min(ys1,ys3)

(8)

在检测完连杆i与面S是否发生碰撞后，同样的方法检测连杆i与长方体障碍物下面是否碰撞，以及其余连杆是否与其碰撞。

3 三维空间建模

与二维平面的机械臂避障研究比，三维空间的避障研究较少，下面重点研究如何实现在三维空间做避障轨迹规划，不可避免的涉及到三维空间的建模。具体方法为：创建一个包含机械臂与障碍物及起终点的三维立体区域，首先我们沿X轴方向将空间进行等距划分，得到n个平面，然后沿Y轴方向对这n个平面进行n等分，最后沿Z轴方向进行n等分，把这些交点提取出来就可以把空间离散成(n+1)*(n+1)*(n+1)的一系列三维空间点集合，同时也可以得到n*n*n个立方体，用0表示安全，1表示障碍物。栅格化空间路径节点如图3所示。

图3 三维空间点图

在有了三维轨迹规划所需的路径节点后，我们把机械臂末端执行器的移动轨迹看作是依次路过规划路径点的集合，由运动学逆解求解可能的解，经过逆解选择与碰撞检测无碰后的逆解，得出对应关节角，求得系列路径节点，再用高次样条曲线平滑路径点实现路径规划任务。

4 基于改进麻雀搜索的机械臂避障路径规划

麻雀搜索(sparrow search algorithm，SSA)是一种基于群体社会化特征优化的群智能算法[11]。相比传统群智能算法，结构简单易实现、控制参数较少，局部寻优本领较强。在单、多峰等基准函数测试中表现性能胜于粒子群、蚁群等算法。原作者将算法运用在三维无人机航迹优化中，效果显著，与本文探讨的机械臂避障路径规划类似。对基本麻雀算法，通过融合柯西变异和反向学习策略对其控制参数进行优化改进[12]，最终实现了机械臂避障路径规划。

4.1 基本麻雀算法

在麻雀搜索算法里，一般把种群内的个体分为发现者、跟随者和警戒者3种，每个个体位置代表一个解。根据作者设定，警戒者一般占种群的10%～20%，发现者与跟随者为动态变化量，所以当有一个体变为发现者代表着另一个体将转变为跟随者。根据分工，发现者为种群搜集觅食方位和区域，跟随者跟随发现者进行觅食，警戒者负责对觅食区域的安全监视与预警。在一个完整的觅食过程中，三者位置动态更新，完成对食物资源的获取。限于篇幅，有关基本麻雀算法的具体理论与数学模型可以阅读文献[13]。

基本麻雀搜索在处理复杂组合优化问题时，易陷入局部灾难、收敛速度减慢、寻优结果变差或搜索不全等。因机械臂是应用于实际生产的，工作场合复杂多变，希望在较短时间内寻到最优解，便于实时控制，故需对基本麻雀算法进行改进。

4.2 融合柯西变异和反向学习的麻雀算法

4.2.1 Sin混沌初始化种群

混沌运动的随机性和遍历不重复性有助于建构多样的种群个体，使群落内个体均匀分布于搜索空间。Sin混沌是一种映射折叠次数无穷的模型，杨海东等[14]研究证明Sin混沌比其它混沌有更优的混沌特性，为提升最优解质量和保持群落里个体多样性，采用Sin混沌映射对麻雀搜索算法的初始种群进行改造,以提高算法寻优能力。其一维自映射表达式为：

(9)

4.2.2 动态自适应权重

基本麻雀搜索的发现者在迭代初期就向全局最优解逼近，会导致搜索范围不足，陷入局部最优，使搜索失败。为克服这一弊端，在原发现者位置更新公式中加入上一代全局最优解，使当前发现者位置同时受上代发现者位置与最优解影响，有利于降低算法陷入局部最优解概率。此外，再在其位置更新公式中加入动态权重系数ω[15]，让它在迭代初期具有较大值，促进全局搜索，迭代后期自适应变小，促进局部搜索并加快收敛速度。权重因子ω与新的发现者位置更新公式为：

(10)

(11)

4.2.3 改进的警戒者公式：

(12)

该式表示：若该麻雀是最优位置麻雀，它会逃到最优与最差位置间的任一随机位置，反之，它会在当前位置与最优位置间的任一随机位置。Xbest为当前全局最优位置；β为步长参数，是服从均值为0，方差为1的正态分布随机数；fg为当前全局最优适应值；fi为当前种群某个体适应值。

4.2.4 融合柯西变异和反向学习机制

反向学习的目的是以当前解为基础，经反向学习找到对应的反向解，再经过判断比较保留更好的解。为确保个体寻到的解是最优解，将反向学习机制融入麻雀搜索中，其数学表达式为：

(13)

(14)

(15)

一维柯西分布密度函数如下：

(16)

当a取1时，式(16)变为标准柯西分布。把式(16)加入目标位置更新公式里，展现柯西算子的干扰能力，有利于算法全局寻优性能的提高。

(17)

式中，cauchy(0,1)为标准柯西分布。柯西分布随机变量生成的函数为η=tan[(ξ-0.5)π]。

为找到最优性能解，这里设计一种分段选择策略来更新目标位置，把反向学习与柯西扰动依某概率分段执行，使目标位置动态更新。由反向学习求得反向解，增加算法搜索区域。通过柯西变异策略在最优解位置产生干扰得出新解，降低算法陷入局部最优解。其选择概率表达式[16]如下：

(18)

式中，θ为调节系数。选择原理如下：若rand

(19)

式中，f(x)表示x位置的适应值。

4.2.5 算法步骤

步骤1：初始化参数设置，如种群量NP，最高迭代数itermax，发现者比率PD，安全值ST等，并用Sin混沌映射产生初始麻雀种群；

步骤2：求各个体适应值，选出当前最优与最差适应值个体及相对应的位置；

步骤3：从较优适应度值的麻雀中选取PD比例作为发现者，按照式(11)更新位置。其余麻雀为跟随者，按照基本麻雀搜索位置更新公式更新；

步骤4：从种群随机选取一些麻雀为警戒者，按式(12)更新位置；

步骤5：由概率Ps判断是否用柯西扰动或反向学习对当前最优解干扰，产生新解；

步骤6：根据式(19)确定是否进行位置更新；

步骤7：判断是否到达结束条件，如是执行下一步，反之跳转到步骤2；

步骤8：程序执行完，输出结果。

5 仿真结果与分析

5.1 算法实现流程

机械臂无碰路径规划是在已知障碍物信息的工作空间中，给定起终两个位置点使其完成两点间运动，以最短路径为评价原则构建适应度函数，并要求轨迹运行连续，各关节角度变化连续平缓无冲击且能避开所有障碍物的最优无碰路径。轨迹规划仿真是在笛卡尔空间中对末端执行器的移动路径及自身位姿状态进行规划，以防机身与障碍物碰撞。让机械臂做往复运动，观察全过程有无与障碍物发生碰撞。算法实现流程如图4所示。

图4 算法实现流程图

5.2 仿真结果分析

仿真环境为MATLAB 2020b，计算机配置为：AMD5800H、3.2 GHz、16 G，分别对改进前后的麻雀搜索进行路径优化效果对比。另外，为了体现新颖的麻雀搜索算法的出色优化性能，选取文献[10]的改进蚁群算法与之对比，算法所需的参数如表2所示，α为信息素启发值，ρ为挥发值，β1为适应度启发值，为保证对比公平性，种群数量与迭代次数设置相同。离散点划分量为21*21*21，起位置(1,10,4)，终位置(21,8,6)，设置5个障碍物，仿真结果对比如图5～图7所示。