(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210869380.7 (22)申请日 2022.07.21 (71)申请人 北京工业大 学 地址 100124 北京市朝阳区平乐园10 0号 (72)发明人 许静静　裴艳虎　刘志峰　陈建洲　 (74)专利代理 机构 北京思海天达知识产权代理 有限公司 1 1203 专利代理师 王兆波 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于混合几何表征的双机械臂协同避 障运动规划优化方法 (57)摘要 本发明公开了一种基于混合几何表征的双 机械臂协同避障运动规划优化方法， 更具体的是 为保证协同作业安全性， 提出了一种基于混合几 何表征的双机械臂最短距离模型和无碰撞运动 轨迹优化方法。 该方法采用圆柱体和凸多面体两 种几何体简化描述机械臂结构， 建立了双机械臂 最短距离计算模 型， 及基于模型的安全距离约束 条件， 通过关节空间五次多项式插值提高了机械 臂关节运动平稳性， 建立运动时间和能耗综合目 标函数， 进行了双机械臂协同运动规划优化建 模， 采用粒子群算法优化求解， 为双机械臂安全 应用提供了方法支撑 。 权利要求书3页 说明书4页 附图1页 CN 115008475 A 2022.09.06 CN 115008475 A 1.一种基于混合几何表征的双机械臂协同避障运动规划优化方法， 其特征在于： 该方 法是采用以下技 术手段实现的： S1、 对机械臂进行分解得到各机械臂的臂杆外形特征， 采用圆柱体和凸多面体两种 几 何形式包络描述， 并进行 数学表征； S2、 基于空间线段间， 线段与平面间空间最小距离建模函数， 建立两凸多面体间、 两圆 柱体间， 及凸多面体与圆柱体间的最短距离模型， 并形成子函数包； S3、 构建双机 械臂最短距离， 并通过调用子函数包进行距离计算； S4、 基于目标位姿的进行双机 械臂运动学逆解计算； S5、 采用五次多 项式进行 各机械臂关节空间插值 规划； S6、 确定规划变量、 运动学约束和安全距离约束条件， 建立运动时间和能耗综合目标函 数， 构建双机 械臂协同运动规划优化模型； S7、 采用粒子群算法， 依据经验确定算法参数， 进行优化模型求解， 得到双机械臂优化 运动轨迹。 2.根据权利要求1所述的一种基于混合几何表征的双机械臂协同避障运动 规划优化方 法， 其特征在于： 该 方法的具体实施过程如下， 步骤(1)将机械臂进行简化分解， 进行圆柱体和凸多面体包络描述与表征， 机械臂的底 座及其所在移动平台、 末端夹爪简化为凸多面体描述， 将各机械臂的臂杆简化为圆柱体描 述， 其中凸多面体PloyHD采用各凸多变形表面集合进行数学表征， 其表征函数Express1与 所有表面顶点的有序集合相关， 圆柱体CyLD采用轴线两端点及圆柱体半径进行数学表征， 其表征函数Expres s2与端点三维坐标和半径相关， 具体如下， PolyHD＝Expres s1{Planei,1≤i≤N}           (1) CyLD＝Expres s2{E1,E2,R}        (2) 式中Planei为凸多面体第i个表面， N为表面总个数， 各表面通过各顶点三维坐标verij 的有序集合描述， Planei＝Matrix{verij,1≤j≤mi}， mi为第i个表面的顶点个数； E1和E2为 圆柱体轴线的两个端点 三维坐标， R为圆柱体半径； 则机械臂ROB可表征为圆柱体和凸多面体的集 合， 具体表示如下， ROB＝COL{Po lyHDi,CyLDj,1≤i≤I,1≤j≤J}       (3) 式中， I和J分别为机械臂简化结构中凸多面体和圆柱体的个数； 步骤(2)建立两凸多面 体间、 两圆柱体间， 及凸多面体与圆柱体间的最短距离模型； 设两空间线段间、 空间线段与凸多边形平面、 两凸多边形平面间最短距离函数分别为 Fun1(A,B)、 Fun2(line, α )和Fun3( α, β )， 其中A和B表示两个点， line表示线段， α和β 表示凸 多边形平面； 基于以上函数建立两凸多面体间、 两圆柱体间， 及凸多面体与圆柱体间的最短 距离模型分别如下： dis1(PolyHD1,PolyHD2)＝min{Fun3(Planei,1,Planej,2),1≤i≤N1,1≤j≤N2}    (4) dis2(CyLD1,CyLD2)＝Fun1(E1,1E2,1,E1,2E2,2)‑R1‑R2        (5) dis3(PolyHD,CyLD)＝mi n{Fun2(E1E2,Planei),1≤i≤N} ‑R        (6) 基于计算 公式(4)、 (5)、 (6)， 分别形成子函数dis1、 dis2和dis3； N1和N2分别为两个凸多 边体PolyHD1和PolyHD2的表面个数， E1,1E2,1和E1,2E2,2分别为圆柱体CyLDj1和CyLD2的中心轴 线段， R1和R2分别为两圆柱体的半径；权　利　要　求　书 1/3 页 2 CN 115008475 A 2步骤(3)计算双机 械臂最短距离DIS； 通过调用子函数， 双机 械臂最短距离由以下公式计算得到， DIS(ROB1,ROB2)＝min{dis1(PolyHDi,1,PolyHDj,2),dis2(CyLDi,1,CyLDj,2),dis (PolyHD1,CyLD2)}  (7) 式中， 下标1和2分别表示机 械臂1和机 械臂2的几何体元 素； 步骤(4)计算双机 械臂运动学逆解θ1和 θ2； 建立机械臂运动学逆解方程， 并形成函数Inv(gsth)， 当机械臂为冗余机械臂时， 将冗 余关节角位移设为已知 进行求解， 则基于该函数分别通过目标位姿gsth计算机械臂各关节 角位移， 表示如下， θ ＝{ θk,1≤k≤K}＝ Inv(gsth)        (8) 式中θk为第k个关节的角位移， K表示机 械臂关节个数； 步骤(5)采用五次多 项式进行 各机械臂关节空间插值 规划； 以关节初始角 位移和目标角 位移θs和θe， 则依据五次多项式插值方法将角 位移方程θ (t)、 角速度方程 和角加速度方程 分别定义如下， 式中t为运动时刻， t∈[0,T]， T为总运动时间， a1,…,a6为方程参数， 为保证机械臂运动 平稳性， 应满足以下约束条件， 依据以上约束条件， 可计算出各参数值； 则机械臂1和机械臂2的角位移规划轨迹可分 别表示为θ1(t)和 θ2(t)； 步骤(6)构建双机 械臂协同运动规划优化模型； 在该轨迹规划问题中， 规划变量为两机械臂的运动时间T1和T2， 若机械臂为冗余机械 臂， 规划变量还包括冗余关节角位移θre； 根据各关节角位移允许范围Di， 角速度和角加速度 允许最大值Vi,max和Ai,max， 建立运动学约束条件； 根据双机械臂间最短距离DIS建立安全距 离约束条件； 以运动时间和总能耗的加权综合定义优化目标函数如下： 式中w1和w2分别为运动时间与能耗在优化目标的权重， w1+w2＝1， 根据任务实际需求确 定； K1和K2分别为两个机械臂的关节 个数； T1和T2分别为两个机械臂的运动时间； 表示机 械臂1的第i1个关节角位移， 表示机械臂2的第i2个关节角位移； 则该轨迹规划优化模型 可表示如下：权　利　要　求　书 2/3 页 3 CN 115008475 A 3