一种基于混合几何表征的双机械臂协同避障运动规划优化方法

1.本发明属于机械臂运动规划领域，涉及一种双机械臂协同避障运动规划优化方法，该方法通过混合几何表征建立机械臂间避障距离模型，进行考虑运动学和空间约束的双机械臂运动规划优化建模与求解，能够为双机协同安全控制提供一定的方法基础。


背景技术：

2.为提高作业效率和任务适应性，多机械臂系统被越来越多地用于协同执行同一作业任务，双机避障运动控制是保证机器人操作安全性的关键途径，已成为当前研究热点。避障轨迹规划是指在给定的环境条件下，规划一条由起始位置指向目标位置的无障碍路径，保证机器人运动的安全性与可靠性。现有的双机械臂协同作业主要通过示教编程和基于a*算法的运动规划两种方法。其中示教编程法具有效率低、任务或环境适应性差等缺点。为了提高作业效率，a*算法是一种在静态网路中求解最短路径的直接搜索算法，结合a*算法和最短距离模型可实现双机械臂离线运动规划优化。然而该方法须通过离散化求得c空间，一般为满足精度要求，所占内存量较大，尤其当机器人关节较多时，会降低自由路径搜索效率，且该方法不适用于解决动态环境下机器人运动规划，也难以考虑其他运动性能需求，其实际应用受到限制。随着智能优化算法的发展，为机器人运动规划优化建模与求解提供了实现途径。在该方法的执行过程中，双机械臂最短距离建模是必要环节。现有研究中有基于椭球体、长方体和球形层次包围盒、及圆柱体描述的最小距离建模方法，而工业机器人结构中存在各种机械结构，具有不同的外形特征，单一采用某种建模方法，其距离计算精度和效率均无法达到最优。如何通过混合几何体描述构建更准确的双机械臂安全距离约束，同时考虑运动效率、运动平稳性、能耗等因素进行双臂协同运动规划优化，是本专利要解决的关键问题。


技术实现要素：

3.本发明旨在提供一种双机械臂协同避障运动规划优化方法。该方法的主要特点是采用圆柱体和凸多面体混合表征的机械臂包络模型，建立双机械臂最短距离模型，从而构建安全距离约束，考虑关节运动平稳性，以能耗和运动时间综合指标为目标函数，进行双机械臂运动规划优化建模与求解，从而为双机械臂协同控制提供方法支撑。
4.本发明是采用以下技术手段实现的：
5.s1、对机械臂进行分解得到各机械臂的臂杆外形特征，采用圆柱体和凸多面体两种几何形式包络描述，并进行数学表征。
6.s2、基于空间线段间，线段与平面间空间最小距离建模函数，建立两凸多面体间、两圆柱体间，及凸多面体与圆柱体间的最短距离模型，并形成子函数包。
7.s3、构建双机械臂最短距离，并通过调用子函数包进行距离计算。
8.s4、基于目标位姿的进行双机械臂运动学逆解计算。
9.s5、采用五次多项式进行各机械臂关节空间插值规划。
10.s6、确定规划变量、运动学约束和安全距离约束条件，建立运动时间和能耗综合目标函数，构建双机械臂协同运动规划优化模型。
11.s7、采用粒子群算法，依据经验确定算法参数，进行优化模型求解，得到双机械臂优化运动轨迹。
12.本发明的特点在于建立了基于混合几何表征的双机械臂最短距离模型，提高了距离计算精度和效率，考虑运动平稳性、运动时间和能耗因素进行双机械臂协同避障规划，提高了双机械臂作业综合性能，为双机械臂安全应用提供方法支撑。
附图说明
13.图1基于混合几何包络的机械臂结构示意简图。
具体实施方式
14.以下结合附图和实施例对本发明进行详细说明。
15.本发明采用的技术方案为一种基于混合几何表征的双机械臂协同避障运动规划优化方法，该方法的具体实施过程如下，
16.步骤(1)将机械臂进行简化分解，进行圆柱体和凸多面体包络描述与表征，如图1所示。
17.如图1所示，机械臂的底座及其所在移动平台、末端夹爪简化为凸多面体描述，将各机械臂的臂杆简化为圆柱体描述，其中凸多面体polyhd可采用各凸多变形表面集合进行数学表征，其表征函数express1与所有表面顶点的有序集合相关，圆柱体cyld可采用轴线两端点及圆柱体半径进行数学表征，其表征函数express2与端点三维坐标和半径相关，具体如下，
18.polyhd＝express1{planei,1≤i≤n}
ꢀꢀꢀ
(1)
19.cyld＝express2{e1,e2,r}
ꢀꢀꢀ
(2)
20.式中planei为凸多面体第i个表面，n为表面总个数，各表面通过各顶点三维坐标ver
ij
的有序集合描述，planei＝matrix{ver
ij
,1≤j≤mi}，mi为第i个表面的顶点个数；e1和e2为圆柱体轴线的两个端点三维坐标，r为圆柱体半径。
21.则机械臂rob可表征为圆柱体和凸多面体的集合，具体表示如下，
22.rob＝col{polyhdi,cyldj,1≤i≤i,1≤j≤j}
ꢀꢀꢀ
(3)
23.式中，i和j分别为机械臂简化结构中凸多面体和圆柱体的个数。
24.步骤(2)建立两凸多面体间、两圆柱体间，及凸多面体与圆柱体间的最短距离模型。
25.设两空间线段间、空间线段与凸多边形平面、两凸多边形平面间最短距离函数分别为fun1(a,b)、fun2(line,α)和fun3(α,β)，其中a和b表示两个点，line表示线段，α和β表示凸多边形平面。基于以上函数建立两凸多面体间、两圆柱体间，及凸多面体与圆柱体间的最短距离模型分别如下：
26.dis1(polyhd1,polyhd2)＝min{fun3(plane
i,1
,plane
j,2
),1≤i≤n1,1≤j≤n2}
ꢀꢀ
(4)
27.dis2(cyld1,cyld2)＝fun1(e
1,1e2,1
,e
1,2e2,2
)-r
1-r2ꢀꢀꢀ
(5)
28.dis3(polyhd,cyld)＝min{fun2(e1e2,planei),1≤i≤n}-r
ꢀꢀꢀ
(6)
29.基于计算公式(4)、(5)、(6)，分别形成子函数dis1、dis2和dis3。n1和n2分别为两个凸多边体polyhd1和polyhd2的表面个数，e
1,1e2,1
和e
1,2e2,2
分别为圆柱体cyld1和cyld2的中心轴线段，r1和r2分别为两圆柱体的半径。
30.步骤(3)计算双机械臂最短距离dis。
31.通过调用子函数，双机械臂最短距离由以下公式计算得到，
32.dis(rob1,rob2)＝min{dis1(polyhd
i,1
,polyhd
j,2
),dis2(cyld
i,1
,cyld
j,2
),dis(polyhd1,cyld2)}
ꢀꢀꢀ
(7)式中，下标1和2分别表示机械臂1和机械臂2的几何体元素。
33.步骤(4)计算双机械臂运动学逆解θ1和θ2。
34.建立机械臂运动学逆解方程，并形成函数inv(gsth)，当机械臂为冗余机械臂时，将冗余关节角位移设为已知进行求解即可，则基于该函数可分别通过目标位姿gsth计算机械臂各关节角位移，可表示如下，
35.θ＝{θk,1≤k≤k}＝inv(gsth)
ꢀꢀꢀ
(8)
36.式中θk为第k个关节的角位移，k表示机械臂关节个数。
37.步骤(5)采用五次多项式进行各机械臂关节空间插值规划。
38.以某一关节初始角位移和目标角位移θs和θe，则依据五次多项式插值方法将角位移方程θ(t)、角速度方程和角加速度方程分别定义如下，
[0039][0040]
式中t为运动时刻，t∈[0,t]，t为总运动时间，a1,
…
,a6为方程参数，为保证机械臂运动平稳性，应满足以下约束条件，
[0041][0042]
依据以上约束条件，可计算出各参数值。则机械臂1和机械臂2的角位移规划轨迹可分别表示为θ1(t)和θ2(t)。
[0043]
步骤(6)构建双机械臂协同运动规划优化模型。
[0044]
在该轨迹规划问题中，规划变量为两机械臂的运动时间t1和t2，若机械臂为冗余机械臂，规划变量还应包括冗余关节角位移θ
re
。根据各关节角位移允许范围di，角速度和角加速度允许最大值v
i,max
和a
i,max
，建立运动学约束条件；根据双机械臂间最短距离dis建立安全距离约束条件；以运动时间和总能耗的加权综合定义优化目标函数如下：
[0045][0046]
式中w1和w2分别为运动时间与能耗在优化目标的权重，w1+w2＝1，可根据任务实际需求确定；k1和k2分别为两个机械臂的关节个数；t1和t2分别为两个机械臂的运动时间；表示机械臂1的第i1个关节角位移，表示机械臂2的第i2个关节角位移。
[0047]
则该轨迹规划优化模型可表示如下：
[0048][0049]
式中，下标1和2分别表示机械臂1和机械臂2的关节角变量；safety
th
是为保证机械臂运动避障绝对可靠设置的安全距离，可设置为safety
th
＝10mm；t
max
为任务所允许的最大运动时间；和分别表示机械臂1的第i1个关节角位移区间、角速度允许最大值和角加速度允许最大值，和和分别表示机械臂2的第i2个关节角位移区间、角速度允许最大值和角加速度允许最大值。
[0050]
步骤(7)进行双机械臂协同避障运动规划优化求解。
[0051]
采用粒子群优化算法进行求解，基于粒子群优化算法，在每次迭代时，第m个粒子会根据现有位置xm＝(x
m1
,x
m2
,
…
,x
md
)和vm＝(v
m1
,v
m2
,
…
,v
md
)进行位置更新，其中d是变量个数；然后结合个体和全局极值，即各粒子本身最优解p
md
和群体最优解p
gd
，并通过以下规则更新其速度与位置信息：
[0052][0053]
式中ω为惯性权重，其值越大，粒子的全局搜索能力更强，反之，局部搜索能力更强，根据经验该优化问题中可取ω＝0.01；c1和c2为加速常数，可在区间[1,2]中取值，根据经验可取c1＝c2＝2；rand1和rand2为区间[0,1]中变化的随机值。此外，优化过程粒子最大速度v
max
，研究表明，其值较小时可提升收敛速度，却易陷入局部最优，一般根据变量区间大小确定；该优化问题中变量为运动时间和冗余关节角位移，可取v
max
＝0.01；最后根据经验可设置群体粒子个数为10，迭代次数为100进行优化求解。