CN104062902A - Delta机器人时间最优轨迹规划方法 - Google Patents

Abstract

本发明提供了一种视觉引导的基于Delta机器人关节空间的轨迹规划方法，用于Delta机器人的最优时间运动。求解Delta机器人的运动学关系，构建末端执行器到各关节的逆运动学方程，工业智能相机用于采集目标位置，编码器用于实时更新目标位置；将Delta机器人的工作区域划分为9*13个子区域，利用B样条曲线离线对不同区域各关节的运动路径规划，保证速度、加速度和加加速度的平滑与连续，减小伺服电机对机械结构的冲击；对经典的粒子群算法进行改进，采用分数阶粒子群算法加快最优解的搜寻时间节点的速度，避免陷入局部最优解；最后采用二维模糊方法在线选择机器人工作区域对应的时间最优节点，完成控制。

Description

Delta机器人时间最优轨迹规划方法

技术领域

本发明涉及Delta机器人关节空间轨迹规划领域，具体是指一种基于分数阶粒子群的时间节点离线寻优方案，实现对Delta机器人关节空间的轨迹规划，并提出一种二维模糊的方法，在线选择时间节点，以改善机器人动作时间，属于机器人轨迹规划领域。

背景技术

在工业现场中，只需要给定机器人运动起点与终点，机器人就可以完成相应动作。为保证起止点间机器人动作的柔顺性，减少机械结构的磨损，并且在满足电机各项指标的前提下快速动作，就需要对机器人各关节的动作过程进行轨迹规划。

目前，轨迹规划的方案根据优化目标的不同可以分为能量最优、时间最优、最小震动等类型，通过关节空间规划机器人运动路径以提高效率、保护机械结构。其中，时间最优的轨迹规划方案由于其满足工业现场对生产效率的需求，应用较为广泛。时间最优轨迹规划主要将求取时间节点转化为非线性规划问题，当前，针对非线性规划问题国内外提出了很多解决算法，如遗传算法及其改进算法(GA、EA、DE)、信赖域算法、二次规划等。

在采用上述的优化算法时，由于现阶段硬件计算速度的约束，实时性成为在工业现场阻碍其应用的障碍之一。为满足工业现场的实时性需求，大多方案在规划的数学模型上进行简化，采用多项式插值的方案以减少计算量。可是，在工业现场路径存在大量不确定因素，特别高阶多项式插值会出现龙格现象，使得运动难以达到最优的要求。

发明内容

本发明的目的是以时间最优为指标要求，提出一种关节空间轨迹规划方法，采用基于分数阶粒子群算法对时间节点寻优，对于机器人各关节的运动轨迹进行规划，以减小震动和动作时间为目标，实现机器人运动的优化。

为达到此目的，本发明的技术方案如下：建立Delta机器人的运动学模型，根据各轴角度和末端执行器的位置建立运动学方程；采用5次非均匀B样条函数拟合机器人的各关节运动方程，控制各关节的运动方式；采用多区域划分的方式将Delta机器人工作区域划分为9*13个矩形区域，根据在传送带上的位置不同，每个区域采取不同的动作方式；采用基于分数阶粒子群算法，考虑机器人运动时各关节的速度、加速度和加加速度的物理极限，对B样条的时间节点进行离线寻优。采用二维模糊控制方式给出目标权重的计算模型，构建模糊区域，使得机器人在工业现场在线实时得到轨迹曲线时间节点。

第一步，系统的硬件结构由Delta机器人、立体钢结构支架、工业智能相机、传送带和编码器组成。在立体钢结构支架内安装Delta机器人，所述的Delta机器人能够在立体支架内运动。在钢结构立体支架前端安装工业智能相机，Delta机器人下部安装垂直于机器人本体的传送带。

第二步，当传送带上有物件传送时，工业智能相机识别物件中心的像素坐标，并将其转化到相机坐标系。由相机坐标系通过外参数矩阵转化到Delta机器人基坐标，得到物件中心基于机器人基坐标的位置。在运动过程中，通过编码器将传送带上的物件位置实时更新，在物件到达机器人工作区域时，机器人完成将传送带的物件抓取到另一固定位置。

第三步，建立Delta机器人的运动学模型，确定各运动轴位置(θ₁，θ₂，θ₃)与Delta机器人末端位置坐标(x，y)的对应关系。通过已知Delta机器人末端的位置，求解主动臂的转角。

第四步，构建基于分数阶的粒子群算法对时间节点进行离线寻优，并采用多区域划分方式将Delta机器人的工作区域划分为9*13个部分，由Delta机器人运动学模型求解各区域笛卡尔空间轨迹对应机器人各关节的角度，由这些角度拟合B样条曲线，得到时间最优的轨迹。

第五步，对Delta机器人操作区域建立模糊规则，通过二维的模糊规则在线对Delta机器人动作时间节点进行选择，选取最优的动作时间策略完成Delta机器人的动作。

本发明的有益效果：提供了一种基于分数阶的粒子群寻优算法快速寻找时间节点的最优分布，用于提高Delta机器人的控制效率和稳定性。首先从Delta机器人的结构入手，求解机器人的运动学关系，构建Delta机器人末端到各关节的逆运动学方程；采用多区域划分方式将Delta机器人的工作区域划分为9*13个部分，利用B样条曲线离线拟合各关节的运动路径，使得运动保证速度、加速度和加加速度的平滑与连续，减小伺服电机对机械结构的冲击；对经典的粒子群算法进行改进，采用分数阶的粒子群算法加快最优解的搜寻速度，并避免陷入局部最优解；构建在线寻优策略，采用二维模糊方法选择机器人工作区域对应的时间最优节点。

附图说明

图1是本发明的整体算法流程

图2是本发明整体结构框架

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下面结合具体实施例，并参照附图，对本发明作进一步详细说明。

本发明的基本目的是以Delta机器人为运行平台，对于抓取传送带上物件的动作进行关节空间的轨迹规划，在机器人机械结构可以承受的范围内以时间最优为目标，对Delta机器人各关节进行控制。主要流程分为目标采集、基于分数阶粒子群算法的B样条轨迹规划、二维模糊算法离线寻找对应最优时间节点，如图1所示。

进一步的，具体步骤为：

(1)在立体钢结构支架内安装Delta机器人，如图2所示，建立相机坐标系与机器人基坐标系关系，将工业智能相机识别到的物件坐标转换到机器人的基坐标。

(1.1)在立体钢结构支架130内安装Delta机器人110，所述Delta机器人110能在立体钢结构支架130内运动；在立体钢结构支架130的前端采用全局方式安装工业智能相机100，立体钢结构支架130内设置传送带120，传送带120的运动方向垂直于四自由度Delta机器人110的运动平面。

(1.2)在智能工业相机100所在的位置建立相机坐标系，坐标原点为光轴中心位置，在Delta机器人110的顶部中心建立机器人基坐标系，坐标原点为机器人顶部中心位置。

(1.3)通过上述设置后，完成视觉引导Delta机器人系统环境的搭建。工业智能相机坐标系的原点与Delta机器人基坐标系的原点有一段平移距离与旋转角度；工业智能相机100获取传送带120上的轨迹点超前于Delta机器人110的运动，从而工业智能相机100获取的轨迹点时，机器人110能够完成与工业智能相机100的通信，将视觉捕获物件形心的像素坐标发送到机器人。

(1.4)对于智能工业摄像机模型：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{k}_x& 0& u_0\\ 0& k_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{x_c}{z_c}\\ \frac{y_c}{z_c}\\ 1\end{array}\right]$

其中，(u，v)为成像平面上的像素坐标，原点建立在图像左上角。(x_c，y_c，z_c)为景物点在相机坐标系下的表示。k_x，k_y分别为x和y方向的放大倍数。(u₀，v₀)为图像中心点的像素坐标。通过张正友标定的方法，计算出矩阵 $\left[\begin{array}{ccc}{k}_x& 0& u_0\\ 0& k_y& v_0\\ 0& 0& 1\end{array}\right],$ 在通过乘以逆矩阵获得基于相机坐标系的位置(x_c，y_c，z_c)。

另有景物坐标在相机坐标中的描述：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=M_w\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

(x_w，y_w，z_w)表示景物点在景物坐标系的坐标，M_w是外参数矩阵。这里，外参数矩阵就是相机坐标系的原点与Delta机器人基坐标原点的关系矩阵，可以通过测量得到。

设传送带120的速度恒定，定义(x_c，y_c，z_c)为物件某时刻在相机坐标系中的坐标，在Delta机器人实时抓取的时刻，同时需要考虑到传送带速度。传送带运送速度的补偿可以采用加入旋转编码器的反馈得到。即在传送带末端添加一个与传送带紧密结合的旋转编码器140，将编码器140捕捉到的传送带速度信息实时反馈到机器人110，对物件的位置坐标进行补偿。

本发明实施例中传送带120的线速度为300mm/s，相机坐标系到机器人末端工具坐标系的外参数矩阵为M_w为：

$M_w=\left[\begin{array}{cccc}1& 0& 0& -618\\ 0& 1& 0& -65\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

(2)生成运动轨迹

(2.1)Delta机器人的工作区域为一40cm*60cm的矩形区域，将其划分为9*13个子区域，针对不同区域，根据上述方案得到的基于机器人基坐标系的物件位置，生成7个对应的轨迹控制点，以构建抓取动作路径。设获取的坐标为(x，y，z)，终点设置坐标为(x′，y′，z′)，生成对应的7个点坐标：

(x，y，z)，

(x，y，z+15)，

$(x+1,\frac{x-x^{\′}+2}{x-x^{\′}}(y-y^{\′})+y^{\′},z+17),$

$(\frac{x^{\′}-x}{2},\frac{y^{\′}-y}{2},z+17),$

$(x-2,-\frac{2}{x-x^{\′}}(y-y^{\′})+y^{\′},z+17),$

(x′，y′，z′+15)，

(x′，y′，z′)

由上式可以得到轨迹经过的点，经过Delta机器人的逆运算，可以得到关节空间的控制点坐标。

所谓逆运算，即已知Delta机器人末端的位置，求解主动臂的转角。其目的主要是为Delta机器人控制提供必要的数学模型。考虑到Delta机械手的动平台作平动，且平行四边形支链中两从动臂运动完全相同，可以得到关于Delta机器人主动臂旋转角度与动平台的关系为：

${\mathrm{\θ}}_i=2\arctan \frac{-E_i-\sqrt{E_i^2-G_i^2+F_i^2}}{G_i-F_i},i=\mathrm{1,2,3}$

$E_i=2l_1{(r-e_i)}^T\hat{z}$

$F_i=-2l_1{(r-e)}^T(\cos {\mathrm{\β}}_i\hat{x}+\sin {\mathrm{\β}}_i\hat{y})$

$G_i={(r-e_i)}^T(r-e_i)+1_1^2-l_2^2$

r＝e_i+l₁u_i+l₂w_i，i＝1，2，3

在上式子中，e_i是为动平台与静平台的外接圆半径差，u_i＝[cosβ_icosθ_i sinβ_icosθ_i -sinθ_i]和w_i分别为支链i主动臂和从动臂的杆长和单位矢量。

(2.2)通过上述方法得到多区域的关节空间点序列，利用改进的分数阶粒子群算法对时间节点进行离线寻优。

粒子群算法的初始化数据值为1s以内的一群随机粒子，然后通过迭代找到最优解。在每一次迭代中，粒子通过跟踪两个“极值”来更新自己，第一个就是粒子本身所找到的最优解，另一个极值是整个种群目前找到的最优解。在找到这两个最优值时，粒子根据如下的公式来更新自己的速度和新的位置：

$v_{\mathrm{id}}^{t+1}=\mathrm{\ω}\·v_{\mathrm{id}}^t+c_1{r}_1(p_{\mathrm{id}}^t-x_{\mathrm{id}}^t)+c_2{r}_2(p_{(i-1)d}^t-x_{\mathrm{id}}^t)$

$x_{\mathrm{id}}^{t+1}=x_{\mathrm{id}}^t+v_{\mathrm{id}}^{t+1}$

式中，ω是非负常数，c₁，c₂为学习因子，r₁，r₂为随机数，范围在0-1之间。通过迭代，最终粒子寻找到全局最优解。

设时域分数阶方程为：

$D^{\mathrm{\α}}\left[x\left(t\right)\right]=\underset{h\→0}{l\mathrm{im}}\left[\frac{1}{h^{\mathrm{\α}}}\underset{k=0}{\overset{+\∞}{\mathrm{\Σ}}}\frac{{(-1)}^k(\mathrm{\α}+1)x(t-\mathrm{kh})}{\mathrm{\Γ}(k+1)\mathrm{\Γ}(\mathrm{\α}-k+1)}\right]$

将上述包含无限项的式子近似为关于时间的导数：

$D^{\mathrm{\α}}\left[x\left(t\right)\right]=\frac{1}{T^{\mathrm{\α}}}\underset{k=0}{\overset{r}{\mathrm{\Σ}}}\frac{{(-1)}^k\mathrm{\Γ}(\mathrm{\α}+1)x(t-\mathrm{kh})}{\mathrm{\Γ}(k+1)\mathrm{\Γ}(\mathrm{\α}-k+1)}$

T表示离散的采样周期，r表示最终终止阶数。在本发明中，取α＝0.5。由上式可知，这是一个无限项的和，r控制终止阶数，取r＝4，则上式变为：

$\begin{array}{c}{v}^{t+1}-{\mathrm{\αv}}^t-\frac{1}{2}\mathrm{\α}{v}^{t-1}-\frac{1}{6}\mathrm{\α}(1-\mathrm{\α})v^{t-2}-\frac{1}{24}\mathrm{\α}(1-\mathrm{\α})(2-\mathrm{\α})v^{t-3}\\ =c_1{r}_1(p_{\mathrm{id}}^t-x_{\mathrm{id}}^t)+c_2{r}_2(p_{(i-1)d}^t-x_{\mathrm{id}}^t)\end{array}$

则：

$\begin{array}{c}{v}^{t+1}={\mathrm{\αv}}^t+\frac{1}{2}\mathrm{\α}{v}^{t-1}+\frac{1}{6}\mathrm{\α}(1-\mathrm{\α})v^{t-2}+\frac{1}{24}\mathrm{\α}(1-\mathrm{\α})(2-\mathrm{\α})v^{t-3}\\ +c_1{r}_1(p_{\mathrm{id}}^t-x_{\mathrm{id}}^t)+c_2{r}_2(p_{(i-1)d}^t-x_{\mathrm{id}}^t)\end{array}$

与经典粒子群算法相比，利用分数阶微分算子，可以提高粒子的收敛速度，记忆多步速度，以提高全局搜索能力。

(2.3)采用B样条方法完成关节空间的轨迹规划。B样条计算效率高且平滑性足够好，又由于B样条曲线的局部可控性和凸包性质等特点，可以进行轨迹优化。

对于关节空间位置-时间节点序列{p_i，t_i}，i＝0，1，…n，其中p_i为关节角度矢量，t_i为时间节点矢量，要使得k次B样条轨迹在时间节点t_i处经过关节位置节点p_i，需要规划B样条插值轨迹。B样条曲线方程为：

$P\left(u\right)=\underset{i=0}{\overset{n}{\mathrm{\Σ}}}{d}_i{N}_{i,k}\left(u\right),u_{\min }\≤u\≤u_{\max },2\≤k\≤n+1$

其中，d_i(i＝0，1，…n)是n+1个控制顶点，N_i，k(u)(i＝0，1，…，n)为k次规范B样条基函数，也称之为混合函数，按照Cox-deBoor递归公式定义为：

$N_{i,k}\left(u\right)=\frac{u-u_i}{u_{i+k}-u_i}{N}_{i,k-1}\left(u\right)+\frac{u_{i+k+1}-u}{u_{i+k+1}-u_{i+1}}{N}_{i+1,k-1}\left(u\right)$

k表示B样条次数，i表示B样条序号，u_i表示节点向量，为非递减序列。由B样条性质可知，N_i，k(u)在区间[u_i，u_i+k+1]上；B样条的节点矢量为U＝[u₀，u₁，…，u_n+k+1]，且所生成的B样条曲线仅定义在节点子区间[u_k，u_n+1]上；在轴上u∈[u_i，u_i+1]处，最多只有k+1个非零的k次B样条基函数N_j，k(u)(j＝i-k，i-k+1，…，i)，其它k次B样条基函数在该处均为零，因此，B样条曲线可以表示为：

$P\left(u\right)=\underset{j=i-k}{\overset{j}{\mathrm{\Σ}}}{d}_j{N}_{j,k}\left(u\right),u\∈[u_i,u_{i+1}]$

其中，

u₀＝u₁＝…＝u₅＝0

u_n+5＝u_n+6＝…＝u_n+10＝1

$u_i=u_{i-1}+\frac{\left|\mathrm{\Δ}{t}_{i-k-1}\right|}{\underset{j=0}{\overset{n-1}{\mathrm{\Σ}}}\left|\mathrm{\Δ}{t}_j\right|},i=\mathrm{6,7},...,n+4$

Δt_j，j＝0，1，…，n-1通过上述的粒子群算法求得。

最终可以通过B样条插值方案获取一系列角度值，并将这些角度值输入到Delta机器人，完成机器人的运动。在控制机器人运动的同时，还需要考虑其速度、加速度和加加速度在限定的范围内：

$\left\{\begin{array}{cc}\left|{\mathrm{\θ}}_{\mathrm{ij}}^{\′}\left(t\right)\right|\≤V_{\mathrm{Cj}}& \\ \left|{\mathrm{\θ}}_{\mathrm{ij}}^{\′\′}\left(t\right)\right|\≤A_{\mathrm{Cj}}& t\∈[t_i,t_{i+1}]\\ \left|{\mathrm{\θ}}_{\mathrm{ij}}^{\′\′\′}\left(t\right)\right|\≤J_{\mathrm{Cj}}& \end{array}\right.$

V_Cj，A_Cj，J_Cj分别表示机器人本体的限制速度、限制加速度和限制脉动。

(3)利用二维模糊算法自动在线寻找最优的方案

(3.1)以Delta机器人坐标系原点为中心，将机器人在传送带上的工作区域进行模糊区域的划分。采用二维模糊控制规则，将目标在传送带上的水平位置和垂直位置作为输入。

(3.2)设输入的数字量为当前传送带上物件坐标(x，y)，Delta机器人的工作区域为40cm*60cm。在x方向上，将40cm的工作区域模糊集定义为{N4，N3，N2，N1，Z0，P1，P2，P3，P4}，在y方向上，将60cm的工作区域模糊集定义为{N6，N5，N4，N3，N2，N1，Z0，P1，P2，P3，P4，P5，P6}。

经模糊化后分别得到系统的模糊化变量(X，Y)，然后将输入数据信息送入到模糊推理机，当前位置进行检测，论域采用三角形隶属函数，结合推理机中存储的模糊规则进行推理，确定对当前归属于的区域，并选取相应的时间节点，生成轨迹。

Claims (5)

1.提出了一种视觉引导的基于Delta机器人的轨迹规划方法，其特征是从Delta机器人特殊系统结构入手，构建工业智能相机和Delta机器人的坐标装换关系，利用B样条差值对Delta机器人各关节的轨迹进行规划，并以时间最优的为目标；采用基于分数阶粒子群算法寻找最优时间节点；采用二维模糊的方法对Delta机器人工作区域进行模糊划分，由模糊控制器自动识别所属区域；包含以下几个步骤：

(1)在Delta机器人系统中采用编码器实时更新目标位置；

(2)将Delta机器人的工作区域进行划分，并对不同的运动轨迹进行5次B样条曲线的平滑轨迹规划；

(3)采用基于分数阶的粒子群算法搜寻最优时间节点；

(4)对机器人工作区域进行模糊划分，选择合适的时间节点。

2.根据权利要求1所述的视觉引导的基于Delta机器人的轨迹规划方法，其特征是：所述步骤(1)中在Delta机器人系统中采用编码器实时更新目标位置，包含以下步骤：

第一步、在所述的Delta机器人系统中，安装在立体钢结构支架上的工业智能相机实时获取目标位置，目标位置默认选用传送带上物件的中心坐标；

第二步、在获取目标位置后，由于传送带在不断运动，通过安装在传送带末端的编码器实时更新目标位置；编码器将传送带走过的距离实时反馈给机器人，物件获取的初始位置加上当前做过的距离即为更新后的目标位置。

3.根据权利要求1所述的视觉引导的基于Delta机器人的轨迹规划方法，所述步骤(2)中将Delta机器人的工作区域进行划，分并对不同的运动轨迹进行5次B样条曲线的平滑轨迹规划，包含以下步骤：

第一步、将Delta机器人40cm*60cm的工作区域划分为9*13个子区域，不同的区域内近似具有相同的运动路径；

第二步、初始化时间节点，针对每一组区域生成一组任意的时间节点，每个时间点的范围在1s以内的随机数；

第二步、对于求取到的最优时间节点通过5阶B样条插值确定运动路径，路径求取公式为： $P\left(u\right)=\underset{j=i-k}{\overset{j}{\mathrm{\Σ}}}{d}_j{N}_{j,k}\left(u\right),u\∈[u_i,u_{i+1}];$

第三步、求取已知路径的速度、加速度、加加速度，通过对上式求导得到，如果三个变量有任意一个超过了设置的条件限制，将回到第一步重新搜索，直到符合限制条件算法停止。

4.根据权利要求1所述的视觉引导的基于Delta机器人的轨迹规划方法，所述步骤(3)中机器人运动轨迹进行轨迹规划，包含以下步骤：

第一步、对于符合速度、加速度、加加速度限制条件的初始时间节点，作为算法输入参量；

第二步、利用基于分数阶粒子群算法寻找最优时间节点，通过迭代更新初始值，搜寻全局最优的时间节点值，目标函数为总时间最短，并且满足机械结构对速度、加速度、加加速度的要求。

5.根据权利要求1所述的视觉引导的基于Delta机器人的轨迹规划方法，所述的步骤(4)中对机器人工作区域进行模糊划分，包含以下步骤：

第一步、以Delta机器人坐标系原点为中心，将机器人在传送带上的工作区域进行模糊区域的划分，即通过模糊方式选择已划分好的对应区域；采用二维模糊控制规则，将目标在传送带上的水平位置和垂直位置作为输入；

第二步、设输入的数字量为当前传送带上物件坐标(x，y)，经模糊化后分别得到系统的模糊化变量(X，Y)，然后将输入数据信息送入到模糊推理机，当前位置进行检测，结合推理机中存储的模糊规则进行推理，选取相应的时间节点，生成轨迹。