[发明专利]基于引力搜索粒子群算法的Delta机器人轨迹规划方法

摘要

本发明提供了一种基于引力搜索粒子群算法的Delta机器人轨迹规划方法，采用网格划分的方式将Delta机器人主传送带工作区域分割为M×N个网格并确定标准点坐标；构建笛卡尔空间“门”型抓取轨迹，推导出各主动轴角度与末端执行器位置的运动学逆解；采用引力搜索粒子群算法，以时间最优为适应度函数，在满足关节速度、加速度、脉动连续平滑且不超过伺服电机额定限制值的条件下，构造7次非均匀有理B样条函数，获取离线插值关节空间的角度‑时间节点序列；Delta机器人在线识别出目标点坐标后，计算关节空间的角度离散序列，查询离线存储在三维数组中的数据，获得时间节点序列，规划运动轨迹，完成既定运动。

主权项

1.一种基于引力搜索粒子群算法的Delta机器人轨迹规划方法，其特征在于，包含以下步骤：A.采用网格划分的方式将Delta机器人主传送带工作区域划分为M×N个正方形，每个正方形称为网格,其边长为dcm，d的数值为1或2，M的值为偶数，选取网格中心点为标准点；M′×N个网格分布在第一象限，余下M′×N个网格分布在第二象限，且它们关于Y轴对称，其中M′＝M/2；B.在笛卡尔空间，对步骤A中的标准点与固定放置点Q6通过直线与圆弧插值，生成笛卡尔空间中7个离散的位置序列以构建“门”型抓取路径，7个离散的位置序列坐标为：Q0＝(x0,y0,z0),Q1＝(x0,y0,z0+H),Q5＝(x6,y6,z6+H),Q6＝(x6,y6,z6)   (1)式中，Q0(x0,y0,z0)为标准点，Q6(x6,y6,z6)为固定放置点，位置提升高度为Hcm，数值根据Delta机器人实际抓取物体的要求设定；圆弧插值半径为Rcm，数值根据标准点Q0到固定放置点Q6的远近做微调，以消除由于笛卡尔空间中插值点选取过少而导致的“门”型轨迹圆弧过渡部分出现畸变的现象，R值的大小根据式(2)确定：式中，为当前抓取动作时笛卡尔空间中标准点Q₀与固定放置点Q₆之间的线段长度，R_max与R_min分别为圆弧过渡半径R的最大值与最小值，数值设定为：式中，Lmax与Lmin分别为固定放置点Q6与主传送带上M×N个网格标准点形成的最长线段长度与最短线段长度，数值根据Delta机器人工作区域离线测量获得；C.在Delta机器人固定平台的中心位置建立基坐标系O‑XYZ与求解坐标系O‑XiYiZ，考虑到Delta机器人三个支链完全相同，采用绕Z轴旋转坐标系的方式简化求解，得到Delta机器人各主动轴角度θ1,θ2,θ3与末端执行器位置E0的运动学逆解：其中，设定θi向下旋转为正方向，向上旋转为负方向，a、b、c为中间变量，e＝A′B′＝B′C′＝C′A′为移动平台三角形的边长，f＝AB＝BC＝CA为固定平台三角形的边长，r_e＝J₁E₁为从动臂的长度，r_f＝F₁J₁为主动臂的长度，为第i条支链的主动臂与从动臂的交点J_i在求解模型坐标系O‑X_iY_iZ的Y轴坐标与Z轴坐标，E₀(x₀,y₀,z₀)为机器人末端执行器在基坐标系中的坐标，E₁₀(x₁₀,y₁₀,z₁₀)、E₂₀(x₂₀,y₂₀,z₂₀)、E₃₀(x₃₀,y₃₀,z₃₀)分别为i＝1,2,3时点E₀(x₀,y₀,z₀)在求解坐标系O‑X_iY_iZ下的坐标，具体为：D.根据步骤C中的运动学逆解将步骤B中笛卡尔空间“门”型轨迹的位置离散序列{Q0,Q1,…Q6}转换成Delta机器人关节空间的角度离散序列{P0,P1,…P6}；E.初始化w组关节空间的时间节点序列{t0,t1,…t6}，每组关节空间的时间节点序列与笛卡尔空间中以标准点为起点的“门”型轨迹所对应的时间节点序列相一致，为简化说明，以下统称为“时间节点序列”，同时，定义每组时间节点序列的间隔Δti为：Δti＝ti+1‑ti(i＝0,1,…,5)   (7)式中，Δti为伺服电机由角度Pi转动到Pi+1的时间间隔,且Δti均为1s以内的随机数，ti为伺服电机转动到Pi所对应的时刻且ti为非递减数列；F.将步骤D中的关节空间的角度离散序列{P0,P1,…P6}与步骤E中任意一组时间节点序列{t0,t1,…t6}相对应，组成关节空间的角度‑时间节点序列{Pi，ti},i＝0,1,…6，构造7次非均匀有理B样条函数插值该关节空间的角度‑时间节点序列，控制各关节的运动形式，所述具体为：对于关节空间的角度‑时间节点序列{Pi，ti},i＝0,1,…6，其中Pi＝[P1i,P2i,P3i]T,分别对应第i个离散序列的关节1、2、3的角度，脉动连续轨迹要求各关节轨迹曲线函数Pm＝fm(t)至少三阶几何连续，且轨迹过关节空间的角度‑时间节点序列，即Pmi＝fm(ti)，m＝1,2,3对应于Delta机器人的3个关节；k次非均匀有理B样条曲线统一描述为：式中,d_j∈R^3×1为控制顶点矢量，j＝0,1,…n，为节点向量，P(u)∈R^3×1为u时刻处的关节角度矢量，N_j,k(u)为k次非均匀有理B样条的基函数，且：k次非均匀有理B样条曲线的r阶导数Pr(u),r＝1,2,3，由德布尔递推公式计算：根据关节轨迹经过关节空间的角度‑时间节点序列，反求7次非均匀有理B样条曲线控制顶点序列，采用累计弦长参数化方法对时间节点序列{t0,t1,…t6}归一化，确定节点向量u∈[u0,u1,…,u6+2×7]：u0＝u1＝…＝u7,u6+7＝u6+7+1＝…＝u6+2×7.   (12)列出关节空间的角度‑时间节点序列约束对应的7个方程：式中,i＝0,1,…,6，dj为B样条轨迹曲线的13个控制顶点矢量，j＝0,1,…,12，因此，还需要由边界条件增加6个方程，设定关节启停速度、加速度和脉动的数值，得到13个方程：式中vs，as，js分别表示关节起始速度、加速度和脉动矢量，ve，ae，je分别为关节终止速度、加速度、脉动矢量，P′(u)，P″(u)，P″′(u)分别为关节速度、加速度、脉动轨迹曲线；将上式描述成矩阵方程的形式：Cmdm＝Pm   (15)其中,Cm∈R13×13为系数矩阵且dm＝[dm0,dm1,…,dm12]T,Pm＝[Pm0,Pm1,…,Pm6,vms,vme,ams,ame,jms,jme]T，由此求出7次非均匀有理B样条轨迹的控制顶点矢量：dm＝Cm‑1Pm   (16)根据控制顶点矢量和时间节点矢量即可求出关节m的C6连续的7次非均匀有理B样条关节轨迹；G.构造时间最优适应度函数，在满足关节速度、加速度、脉动连续平滑且不超过伺服电机额定限制值的条件下，采用引力搜索粒子群算法，对w组时间节点序列{t0,t1,…t6}离线寻优，直到满足最大迭代次数，输出一组最优时间节点序列，具体步骤为：在Delta机器人运行平稳的前提下，要求运动总时间最短，构造目标函数为：式中，VCj,ACj,JCj分别表示机器人伺服电机的速度、加速度和脉动的额定限制值，θ′mj(t)、θ″mj(t)、θ″′mj(t)分别为各关节速度、加速度、脉动，由式(10)～(11)得到：采用引力搜索粒子群算法，在满足式(17)～(18)的条件下，对w组时间节点序列{t0,t1,…t6}进行离线寻优，寻优表达式为：式中，ω是介于0‑1之间的权重系数，rand₁与rand₂是介于0‑1之间均匀分布的随机函数，c'₁表示上一时刻粒子自身的加速度对下一时刻粒子速度的影响程度，c'₂为调节粒子飞向全局历史最优位置的步长，gbest^d(t)表示截止t时刻整个粒子群找到的最优解，d表示维度，取值为0,1,…5，表示在t时刻第i个粒子第d维的数值分别对应于时间节点序列的间隔Δt_i的6个数值；寻优算法满足最大迭代次数，则输出一组最优时间节点序列{t0,t1,…t6}；H.采用离线方式对步骤A中M×N个标准点，重复以上步骤B～G，获得M×N组以标准点为起点的“门”型轨迹所对应的最优时间节点序列{t0,t1,…t6}并存储在三维数组A与B中，具体步骤如下：针对X轴正方向与Y轴正方向所组成的第一象限网格标准点的分布情况建立三维数组A，第一维元素数量与沿X轴正方向单行分布的标准点数量一致，即为M′，第二维元素数量与沿Y轴正方向单行分布的标准点数量一致，即为N，第三维元素数量与每一组时间节点序列的数量一致，即为7，因此索引号分别为0‑(M'‑1)、0‑(N‑1)、0‑6，同理，针对X轴负方向与Y轴正方向组成的第二象限网格标准点的分布情况建立三维数组B，维数和索引号与三维数组A相同；在离线方式下，对步骤A中M×N个标准点，重复以上步骤B～G，获得M×N组最优时间节点序列{t0,t1,…t6}；根据各标准点在主传送带上的位置，确定以该标准点为起点的“门”型轨迹所对应的一组最优时间节点序列{t0,t1,…t6}在三维数组中的索引号；当标准点的横坐标x≥0时，则存储该组最优时间节点序列{t0,t1,…t6}于三维数组A中的相应位置；反之，则存储于三维数组B中的相应位置，为机器人在线运行做准备；I.当工业智能相机识别出主传送带上物件中心点，该点也称为目标点，通过坐标变换转化为机器人工作空间的坐标位置，通过横坐标与纵坐标的值确定该物体所在网格标准点的位置，计算出以该标准点为起点的“门”型轨迹所对应的一组最优时间节点序列{t0,t1,…t6}在三维数组中的索引号，利用在线查询的方式在步骤H建立的三维数组A与B中获得该组最优时间节点序列{t0,t1,…t6}，同时将该目标点采用类似于标准点的方法，重复以上步骤B～D快速规划机器人运动轨迹，完成既定运动。