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

权利要求书

1.一种基于混合几何表征的双机械臂协同避障运动规划优化方法，其特征在于：该方法是采用以下技术手段实现的：

S1、对机械臂进行分解得到各机械臂的臂杆外形特征，采用圆柱体和凸多面体两种几何形式包络描述，并进行数学表征；

S2、基于空间线段间，线段与平面间空间最小距离建模函数，建立两凸多面体间、两圆柱体间，及凸多面体与圆柱体间的最短距离模型，并形成子函数包；

S3、构建双机械臂最短距离，并通过调用子函数包进行距离计算；

S4、基于目标位姿的进行双机械臂运动学逆解计算；

S5、采用五次多项式进行各机械臂关节空间插值规划；

S6、确定规划变量、运动学约束和安全距离约束条件，建立运动时间和能耗综合目标函数，构建双机械臂协同运动规划优化模型；

S7、采用粒子群算法，依据经验确定算法参数，进行优化模型求解，得到双机械臂优化运动轨迹。

2.根据权利要求1所述的一种基于混合几何表征的双机械臂协同避障运动规划优化方法，其特征在于：该方法的具体实施过程如下，

步骤(1)将机械臂进行简化分解，进行圆柱体和凸多面体包络描述与表征，机械臂的底座及其所在移动平台、末端夹爪简化为凸多面体描述，将各机械臂的臂杆简化为圆柱体描述，其中凸多面体PloyHD采用各凸多变形表面集合进行数学表征，其表征函数Express₁与所有表面顶点的有序集合相关，圆柱体CyLD采用轴线两端点及圆柱体半径进行数学表征，其表征函数Express₂与端点三维坐标和半径相关，具体如下，

PolyHD＝Express₁{Plane_i,1≤i≤N} (1)

CyLD＝Express₂{E₁,E₂,R} (2)

式中Plane_i为凸多面体第i个表面，N为表面总个数，各表面通过各顶点三维坐标ver_ij的有序集合描述，Plane_i＝Matrix{ver_ij,1≤j≤m_i}，m_i为第i个表面的顶点个数；E₁和E₂为圆柱体轴线的两个端点三维坐标，R为圆柱体半径；

则机械臂ROB可表征为圆柱体和凸多面体的集合，具体表示如下，

ROB＝COL{PolyHD_i,CyLD_j,1≤i≤I,1≤j≤J} (3)

式中，I和J分别为机械臂简化结构中凸多面体和圆柱体的个数；步骤(2)建立两凸多面体间、两圆柱体间，及凸多面体与圆柱体间的最短距离模型；

设两空间线段间、空间线段与凸多边形平面、两凸多边形平面间最短距离函数分别为Fun1(A,B)、Fun2(line,α)和Fun3(α,β)，其中A和B表示两个点，line表示线段，α和β表示凸多边形平面；基于以上函数建立两凸多面体间、两圆柱体间，及凸多面体与圆柱体间的最短距离模型分别如下：

dis1(PolyHD₁,PolyHD₂)＝min{Fun3(Plane_i,1,Plane_j,2),1≤i≤N₁,1≤j≤N₂} (4)

dis2(CyLD₁,CyLD₂)＝Fun1(E_1,1E_2,1,E_1,2E_2,2)-R₁-R₂ (5)

dis3(PolyHD,CyLD)＝min{Fun2(E₁E₂,Plane_i),1≤i≤N}-R (6)

基于计算公式(4)、(5)、(6)，分别形成子函数dis1、dis2和dis3；N₁和N₂分别为两个凸多边体PolyHD₁和PolyHD₂的表面个数，E_1,1E_2,1和E_1,2E_2,2分别为圆柱体CyLDj₁和CyLD₂的中心轴线段，R₁和R₂分别为两圆柱体的半径；

步骤(3)计算双机械臂最短距离DIS；

通过调用子函数，双机械臂最短距离由以下公式计算得到，

DIS(ROB₁,ROB₂)＝min{dis1(PolyHD_i,1,PolyHD_j,2),dis2(CyLD_i,1,CyLD_j,2),dis(PolyHD₁,CyLD₂)} (7)

式中，下标1和2分别表示机械臂1和机械臂2的几何体元素；

步骤(4)计算双机械臂运动学逆解θ₁和θ₂；

建立机械臂运动学逆解方程，并形成函数Inv(gsth)，当机械臂为冗余机械臂时，将冗余关节角位移设为已知进行求解，则基于该函数分别通过目标位姿gsth计算机械臂各关节角位移，表示如下，

θ＝{θ_k,1≤k≤K}＝Inv(gsth) (8)

式中θ_k为第k个关节的角位移，K表示机械臂关节个数；

步骤(5)采用五次多项式进行各机械臂关节空间插值规划；

以关节初始角位移和目标角位移θ_s和θ_e，则依据五次多项式插值方法将角位移方程θ(t)、角速度方程和角加速度方程分别定义如下，

式中t为运动时刻，t∈[0,T]，T为总运动时间，a₁,…,a₆为方程参数，为保证机械臂运动平稳性，应满足以下约束条件，

依据以上约束条件，可计算出各参数值；则机械臂1和机械臂2的角位移规划轨迹可分别表示为θ₁(t)和θ₂(t)；

步骤(6)构建双机械臂协同运动规划优化模型；

在该轨迹规划问题中，规划变量为两机械臂的运动时间T₁和T₂，若机械臂为冗余机械臂，规划变量还包括冗余关节角位移θ_re；根据各关节角位移允许范围D_i，角速度和角加速度允许最大值V_i,max和A_i,max，建立运动学约束条件；根据双机械臂间最短距离DIS建立安全距离约束条件；以运动时间和总能耗的加权综合定义优化目标函数如下：

式中w₁和w₂分别为运动时间与能耗在优化目标的权重，w₁+w₂＝1，根据任务实际需求确定；K₁和K₂分别为两个机械臂的关节个数；T₁和T₂分别为两个机械臂的运动时间；表示机械臂1的第i₁个关节角位移，表示机械臂2的第i₂个关节角位移；

则该轨迹规划优化模型可表示如下：

式中，下标1和2分别表示机械臂1和机械臂2的关节角变量；Safety_th是为保证机械臂运动避障绝对可靠设置的安全距离，可设置为Safety_th＝10mm；T_max为任务所允许的最大运动时间；D_i1,1、V_i1,max和A_i1,max分别表示机械臂1的第i₁个关节角位移区间、角速度允许最大值和角加速度允许最大值，和D_i2,2、V_i2,max和A_i2,max分别表示机械臂2的第i₂个关节角位移区间、角速度允许最大值和角加速度允许最大值；

步骤(7)进行双机械臂协同避障运动规划优化求解；

采用粒子群优化算法进行求解，基于粒子群优化算法，在每次迭代时，第m个粒子会根据现有位置X_m＝(x_m1,x_m2,…,x_mD)和V_m＝(v_m1,v_m2,…,v_mD)进行位置更新，其中D是变量个数；然后结合个体和全局极值，即各粒子本身最优解p_md和群体最优解p_gd，并通过以下规则更新其速度与位置信息：

式中ω为惯性权重，其值越大，粒子的全局搜索能力更强，反之，局部搜索能力更强，根据经验该优化问题中取ω＝0.01；c₁和c₂为加速常数，在区间[1,2]中取值，根据经验可取c₁＝c₂＝2；rand₁和rand₂为区间[0,1]中变化的随机值。