一种工业机器人应用的过渡轨迹规划方法与流程

本发明涉及一种工业机器人应用的过渡轨迹规划方法。

背景技术：
工业机器人已经被广泛应用于多个领域，在现代工业自动化、智能化的发展中发挥着越来越突出的作用。运动轨迹规划是机器人控制系统的一项基本任务，工业机器人的运动轨迹通常有笛卡尔空间的直线、圆弧，以及关节空间的点到点运动轨迹。实际应用场合中，一般是由多条轨迹顺序衔接完成一项加工任务，然而，相邻两条轨迹在衔接处存在拐角，而许多应用要求轨迹平滑没有拐角，即末端点从一条轨迹平滑过渡到下一条轨迹。两条轨迹之间的切换轨迹称为过渡轨迹。目前，对过渡段的轨迹规划有部分研究，常以圆弧、多项式曲线来构造过渡轨迹。其中，圆弧过渡可以保证轨迹平滑、速度匀速，有较广泛的应用，如：中国发明专利“一种焊接机器人过渡轨迹规划方法”(申请号为201110000264.3)，就是针对焊接机器人，过渡段焊缝采用圆弧来衔接直线段焊缝和圆弧段焊缝，但圆弧过渡在过渡段与轨迹段衔接处存在加速度的跃变，影响轨迹的平滑性，可能导致机械振动，且圆弧过渡无法实现相邻两段仅有姿态变化的轨迹过渡，对其他有此需求的应用场合就不适用了；多项式曲线过渡，包括样条函数、Hermite函数、五次多项式等，借助高阶连续性的特性可以保证加速度的平滑，减少振动，有利于高速运动，其中五次多项式过渡算法的研究较多，如：文献《机械手笛卡尔空间轨迹规划研究[J].》(林仕高,刘晓麟,欧元贤《机械设计与制造》,2013(3):49-52)对位姿6个自由度分别用五次多项式构造过渡曲线，不仅能保证轨迹、速度、加速度的平滑，且能完成仅有姿态变化的轨迹间的过渡，但五次曲线的几何形状难以控制，而且缺乏工程约束条件。以上研究是基于笛卡尔空间两条轨迹之间的轨迹过渡，对关节空间轨迹到笛卡尔空间轨迹的过渡研究较少，而不同轨迹之间的不停顿运动对于提高工作效率是很有必要的。

技术实现要素：
本发明的目的在于，克服现有技术存在的缺陷，提出了一种工业机器人应用的过渡轨迹规划方法，构造一条能保证轨迹、速度、加速度平滑的过渡曲线，实现关节空间轨迹与笛卡尔空间轨迹的过渡、笛卡尔空间两条轨迹的过渡，对于仅姿态变化的轨迹也能实现过渡，过渡轨迹形状能够受控。同时从工程应用角度，对过渡段的边界路径速度和姿态旋转速度进行约束。本发明的基本技术方案包括以下步骤：步骤1：将过渡轨迹规划需要的运动参数导入机器人过渡轨迹规划模块机器人末端点位姿由位置矢量(x,y,z)和RPY姿态矢量(α,β,γ)共同描述，组合成一个6自由度的复合矢量(x,y,z,α,β,γ)，过渡轨迹规划涉及的运动参数有：第一条轨迹起点位姿P0、终点位姿P1，第二条轨迹终点位姿P2，过渡参数百分比a，工程约束条件包括：系统最大速度Vmax、系统最大加速度Amax、系统允许的最大弓高误差Emax。步骤2：确定轨迹P0P1与轨迹P1P2之间过渡轨迹的起点和终点的位姿当轨迹P0P1是笛卡尔空间的直线轨迹时，过渡起点Ps到拐点P1的直线长度是轨迹P0P1直线长度的一半乘以过渡参数百分比a；当轨迹P0P1是笛卡尔空间的圆弧轨迹时，过渡起点Ps到拐点P1的弧长是轨迹P0P1弧长的一半乘以过渡参数百分比a；设定过渡起点Ps到拐点P1的RPY姿态矢量变化对应轨迹P0P1的RPY姿态矢量变化的一半乘以过渡参数百分比a。当轨迹P0P1是关节空间轨迹时，设定过渡起点Ps到拐点P1的各关节位置变化对应轨迹P0P1各关节位置变化的一半乘以过渡参数百分比a，进一步根据正向运动学计算出过渡起点Ps的位姿。当轨迹P1P2是笛卡尔空间的直线轨迹时，拐点P1到过渡终点Pe的直线长度是轨迹P1P2直线长度的一半乘以过渡参数百分比a；当轨迹P1P2是笛卡尔空间的圆弧轨迹时，拐点P1到过渡终点Pe的弧长是轨迹P1P2弧长的一半乘以过渡参数百分比a；设定拐点P1到过渡终点Pe的RPY姿态矢量变化对应轨迹P1P2的RPY姿态矢量变化的一半乘以过渡参数百分比a。当轨迹P1P2是关节空间轨迹时，设定拐点P1到过渡终点Pe的各关节位置变化对应轨迹P1P2各关节位置变化的一半乘以过渡参数百分比a，进一步根据正向运动学计算出过渡终点Pe的位姿。步骤3：确定过渡轨迹的边界速度由外部软件模块机器人的加减速轨迹规划算法，输入轨迹P0P1在过渡起点Ps处的速度，轨迹P1P2在过渡终点Pe处的速度，为简化计算，约定过渡轨迹起、终点的边界速度相等。笛卡尔空间轨迹对应末端点的路径速度Vpath、末端点的姿态旋转速度Vori，关节空间轨迹对应各关节的速度，此情况下，借助雅可比矩阵，可由各关节的速度直接计算出机器人末端点位姿6个自由度的速度。步骤4：利用弓高误差来约束过渡轨迹的边界速度从工程应用角度，当相邻轨迹切向速度矢量的夹角很大时，过大的轨迹衔接速度会对机械系统产生较大的冲击，因此有必要对轨迹衔接速度进行约束。本发明利用工程上的弓高误差参数来约束轨迹衔接速度，保证过渡轨迹边界速度Vpath和Vori不超过允许的轨迹衔接速度，具体方法如下：假定轨迹P0P1与轨迹P1P2之间有个假想的小圆弧，通过弓高误差来调整曲率半径r，然后利用r对轨迹衔接速度进行约束，从而达到约束过渡轨迹边界速度Vpath和Vori的目的。假想圆弧只是用来获取速度的约束条件，并不影响实际轨迹。由于关节空间轨迹不直观，用过渡点与拐点之间的直线连线替代原轨迹计算拐点处的夹角。轨迹P0P1与轨迹P1P2在拐点P1处的切向速度矢量的夹角为θ，对路径速度Vpath的约束公式如下：其中，Vmax为系统最大速度，Amax为系统最大加速度，Emax系统允许的弓高误差参数，定义一个角度到毫米的量纲转换系数λ，将姿态旋转速度等效为一个长度上的速度矢量λVori，参照路径速度Vpath的约束方式，对λVori进行约束，即可达到对姿态旋转速度Vori的约束目的。轨迹P0P1与轨迹P1P2在拐点P1处的切向姿态旋转速度矢量的夹角为θori，对姿态旋转速度Vori的约束公式如下：其中，步骤5：将笛卡尔空间轨迹在过渡轨迹边界点的速度Vpath和Vori分解到位姿6个自由度上若轨迹P0P1是笛卡尔空间轨迹，将过渡起点Ps处的速度Vpath和Vori分解到位姿6个自由度上；若轨迹P1P2是笛卡尔空间轨迹，将过渡终点Pe处的速度Vpath和Vori分解到位姿6个自由度上。对于关节空间轨迹，已在步骤3中求出过渡起点Ps和过渡终点Pe处的位姿6个自由度上的速度。笛卡尔空间轨迹分直线和圆弧两种轨迹类型，以下分别阐述速度Vpath和Vori的分解方法：对于直线轨迹类型，根据直线轨迹在笛卡尔坐标系中的单位向量，可直接将Vpath分解到各轴上；对于圆弧轨迹类型，利用文献《工业机器人笛卡尔空间轨迹规划[J].》(机械工程与自动化,2014(5):141-143)基于局部坐标系的空间圆弧插补方案中的计算步骤可得出圆弧轨迹圆心点Oarc的位置坐标、及局部坐标系的Z轴的单位方向向量Z′，由向量Z′与圆心点Oarc指向圆弧上某点的单位方向向量O′叉乘可计算出该点处的单位速度切向矢量，即可将Vpath分解到各轴上。然后以直线的位置处理方式来等效处理姿态轨迹，将姿态旋转速度Vori分解到姿态轴3个自由度上。步骤6：计算过渡轨迹起点Ps和终点Pe的位姿6个自由度上的加速度加速度由速度差分计算得到，步骤5已将边界速度Vpath和Vori分解到6个自由度上，假定过渡起点Ps处的瞬间是匀速，过渡终点Pe处的瞬间是匀速，可计算过渡轨迹起点Ps和终点Pe的位姿6个自由度上的加速度。步骤7：计算过渡轨迹运行时间T假定过渡轨迹的执行时间与从过渡起点Ps匀速运动到拐点P1再匀速运动到过渡终点Pe的直线段时间相等，依此设定，分别计算位置过渡的运行时间T1和姿态过渡的运行时间T2，则T选取较长的时间：若其中存在位置或姿态不过渡，只需将对应的运行时间置为零即可。步骤8：对机器人过渡轨迹末端点位姿的6个自由度分别构造过渡曲线方程P(σ)，采用两条抛物线运动的叠加融合为过渡轨迹的运动，矩阵方程如下：P(σ)＝P1(σ)+η(σ)[P2(σ)-P1(σ)](4)其中，σ是过渡时间t的参数化变量；P1(σ)是与过渡起点Ps相连的抛物线，是关于变量σ的二次函数，与第一段轨迹相切于过渡起点Ps；P2(σ)是与过渡终点Pe相连的抛物线，是关于变量σ的二次函数，与第二段轨迹相切于过渡终点Pe；η(σ)是保证曲线P1(σ)过渡到曲线P2(σ)的过渡函数，可设计η(σ)＝6σ5-15σ4+10σ3，以保证P(σ)边界在轨迹、速度、加速度上连续。将过渡轨迹起点Ps和终点Pe的边界条件：位姿、速度、加速度，带入公式(4)的矩阵方程，即可确定过渡轨迹的位姿6个自由度的曲线方程。设计的过渡轨迹保证了轨迹、速度、加速度的平滑性，对于仅姿态变化的轨迹也能实现过渡，过渡曲线由两条抛物线融合而成，曲线形状可控。根据以上的技术方案，可以实现以下的有益效果：1)可实现关节空间轨迹与笛卡尔空间轨迹之间的过渡，以及笛卡尔空间两条轨迹之间的过渡，笛卡尔空间轨迹包括：直线、圆弧；过渡轨迹统一在笛卡尔空间内规划，曲线直观，边界条件求取只涉及正向运动学，避免了逆向运动学的多解问题。2)过渡曲线由两条抛物线融合而成，保证了轨迹、速度、加速度的平滑性，曲线形状可控；过渡轨迹由独立的6条曲线构成，对于无位置变化仅有姿态变化的轨迹也能实现过渡。3)从工程应用角度对过渡轨迹的边界路径速度和姿态旋转速度进行约束，防止当相邻轨迹的夹角很大时，过大的衔接速度可能会对机械系统产生较大的冲击。关键点：1)笛卡尔空间轨迹与关节空间轨迹的衔接统一处理成在笛卡尔空间下直观地规划过渡轨迹；2)采用两条抛物线融合成过渡曲线的算法，以保证轨迹、速度、加速度的平滑，以及曲线形状可控；3)从工程应用角度利用轨迹间的夹角、弓高误差对边界路径速度进行约束，以及以类似的方式对边界姿态旋转速度进行约束。附图说明图1是本发明中过渡轨迹规划方法的流程图。图2是示教盒示教出的直线和圆弧轨迹示意图。图3是弓高误差约束边界速度方法中插入假想圆弧的示意图。图4是直线过渡到圆弧上的机器人末端点的路径图。图5是过渡轨迹上的机器人末端点的位置曲线图。具体实施方式下面结合SCARA机器人系统对本发明作进一步说明。该系统由一台SCARA工业机器人、控制柜、示教盒组成，其中SCARA机器人的大臂长300mm，小臂长300mm，关节3上下行程是200mm。SCARA机器人仅有γ轴姿态，为统一描述，保留α轴姿态和β轴姿态，但使其数值为零，不影响计算过程。利用示教盒示教直线轨迹P0P1，再示教圆弧轨迹P1P2，PM是圆弧轨迹上一点，如图2所示，示教出的P0位姿为(-200,-300,200,0,0,-45)，示教出的P1位姿为(0,-300,200,0,0,45)，示教出的P2位姿为(100,-400,200,0,0,45)，示教出的PM位姿为(50.2429,-386.7423,200,0,0,45)，其中，位置单位是毫米，姿态单位是度。系统参数设定如下：系统最大速度Vmax＝500mm/s、系统最大加速度Amax＝2000mm/s2、系统允许的最大弓高误差Emax＝10mm，角度到毫米的量纲转换系数λ＝2mm/°，过渡参数百分比a＝100％。确定过渡轨迹的过渡起点Ps的位姿和过渡终点Pe的位姿。轨迹P0P1是笛卡尔空间的直线轨迹，根据步骤2所述，过渡起点Ps到拐点P1的直线长度是轨迹P0P1直线长度的一半乘以过渡参数百分比a，过渡起点Ps到拐点P1的γ轴姿态变化对应轨迹P0P1的γ轴姿态变化的一半乘以过渡参数百分比a，可计算出过渡起点Ps的位姿为(-100,-300,200,0,0,0)。轨迹P1P2是笛卡尔空间的圆弧轨迹，根据步骤2所述，拐点P1到过渡终点Pe的弧长是轨迹P1P2弧长的一半乘以过渡参数百分比a，拐点P1到过渡终点Pe的γ轴姿态变化对应轨迹P1P2的γ轴姿态变化的一半乘以过渡参数百分比a，可计算出过渡终点Pe的位姿为(29.2893,-370.7107,200,0,0,45)。根据步骤3的约定，由外部软件模块机器人的加减速轨迹规划算法，输入过渡轨迹的边界速度Vpath＝500mm/s和Vori＝150°/s。从工程应用角度，当相邻轨迹切向速度矢量的夹角很大时，过大的轨迹衔接速度会对机械系统产生较大的冲击，因此有必要对轨迹衔接速度进行约束。本发明利用工程上的弓高误差参数来约束轨迹衔接速度，保证过渡轨迹边界速度Vpath和Vori不超过允许的轨迹衔接速度。根据步骤4所述，θ是直线P0P1在拐点P1处切向速度矢量与圆弧P1P2在拐点P1处切向速度矢量的夹角，如图3所示，θ＝90°，带入弓高误差Emax，可得到r＝24.1421mm，进一步由公式(1)计算出弓高误差限制的最大速度为219.7368mm/s，Vpath超过了该限制，因此需要约束Vpath＝219.7368mm/s。根据步骤4所述，参考路径速度Vpath的限制对姿态旋转速度Vori进行限制，轨迹P0P1的γ轴姿态有变化，轨迹P1P2的γ轴姿态无变化，即轨迹P0P1与轨迹P1P2在拐点P1处的切向姿态旋转速度矢量的夹角为θori＝0°，理论上对Vori的限制速度是无穷大，因此Vori＝150°/s。将笛卡尔空间轨迹在过渡轨迹边界点的速度Vpath和Vori分解到位姿6个自由度上。根据步骤5所述，轨迹P0P1是直线轨迹类型，在笛卡尔坐标系中的单位向量Vpath分解到x、y、z轴上的速度表示为单位是mm/s。轨迹P1P2是圆弧轨迹类型，利用文献《工业机器人笛卡尔空间轨迹规划[J].》(机械工程与自动化,2014(5):141-143)基于局部坐标系的空间圆弧插补方案中的计算步骤可得出圆弧轨迹圆心点Oarc的位置坐标为(100,-300,200)，及局部坐标系的Z轴的单位方向向量Z′＝(0,0,1)，圆心点Oarc指向圆弧上Pe点的单位方向向量由向量Z′与向量O′叉乘可计算出Pe点处的单位速度切向矢量可将Vpath分解到x、y、z轴上的速度表示为单位是mm/s。SCARA机器人仅有γ轴姿态，因此Vori的速度即是γ轴的旋转速度，过渡起点Ps处的姿态旋转速度分解到α、β、γ轴上的旋转速度表示为(0,0,150)，单位是°/s，轨迹P1P2的γ轴姿态无变化，过渡终点Pe处的姿态旋转速度分解到α、β、γ轴上的旋转速度表示为(0,0,0)，单位是°/s。计算过渡轨迹起点Ps和终点Pe的位姿6个自由度上的加速度。根据步骤6所述，假定过渡起点Ps处的瞬间是匀速，轨迹P0P1是直线轨迹，直线上任一点处单位速度切线矢量均是加速度由速度差分计算得到，可计算出过渡起点Ps处的x、y、z轴上的加速度为(0,0,0)，单位是mm/s2。轨迹P1P2是圆弧轨迹类型，存在向心加速度，根据步骤6所述，假定过渡终点Pe处的瞬间是匀速，通过速度差分，计算出点Pe处的x、y、z轴上的加速度为(341.0459,341.7961,0)，单位是mm/s2。SCARA机器人仅有γ轴姿态，因此仅有γ轴的旋转加速度，根据步骤6，假定过渡起点Ps处和过渡终点Pe处的瞬间是匀速，因此得出过渡起点Ps处和过渡终点Pe处的姿态旋转速度分解到α、β、γ轴上的旋转加速度表示为(0,0,0)，单位是°/s2。计算过渡轨迹运行时间T。根据步骤7所述，假定过渡轨迹的执行时间与从过渡起点Ps匀速运动到拐点P1再匀速运动到过渡终点Pe的直线段时间是一样的，依此设定，分别计算位置过渡的运行时间T1＝0.8034s和姿态过渡的运行时间T2＝0.3s，T选取较长的时间为0.8034s。利用矩阵方程(4)式，对机器人过渡轨迹末端点位姿的6个自由度分别构造过渡曲线方程P(σ)，其中，σ是时间t的参数化变量，0≤t≤T，0≤σ≤1，P1(σ)是与过渡起点Ps相连的抛物线，是关于变量σ的二次函数，设P1(σ)＝A1σ2+B1σ+C1，P2(σ)是与过渡终点Pe相连的抛物线，是关于变量σ的二次函数，设P2(σ)＝A2σ2+B2σ+C2，根据步骤8，η(σ)＝6σ5-15σ4+10σ3，将过渡轨迹起点Ps、终点Pe的边界条件：位姿、速度、加速度，带入公式(4)的矩阵方程，计算出系数矩阵A1＝(0,0,0,0,0,0)、B1＝(132.4025,0,0,0,0,90.3826)、C1＝(-100,-300,200,0,0,0)、A2＝(11.0064,11.0307,0,0,0,0)、B2＝(77.113,-110.1687,0,0,0,0)、C2＝(-58.8302,-271.5726,200,0,0,45)，系数矩阵确定后，即确定了过渡轨迹的位姿6个自由度的曲线方程。图4利用MATLAB软件绘制了直线过渡到圆弧上的机器人末端点的路径图，其中点Ps到点Pe之间的实线就是过渡轨迹，图5是过渡轨迹运行期间位姿随时间变化的曲线。