机械臂点到点运动控制方法
【专利摘要】本发明属于数控系统领域，公开了机械臂点到点运动控制方法。现有技术的机械臂的点到点运动控制方法，采用较小的速度启动、停止，来抑制振动，导致运动效率低。本发明根据机械臂启动、停止的精度要求，确定加加速度比、速度比，对加速阶段的最大加加速度J1，减速阶段的最大加加速度J2，加速阶段的最大限制加速度A1，减速阶段的最大限制加速度A2根据实际运动需要进行调整，能适应诸多复杂的运动，尤其是能够满足对末位置状态有较高精度要求的情形，并且能保证运动时间最优。
【专利说明】
机械臂点到点运动控制方法
技术领域
[0001] 本发明涉及数控系统领域，特别是机械臂点到点运动控制方法。
【背景技术】
[0002] 随着技术的飞速发展，开放式的数控系统研究已经成为世界各国数控界研究的热 点，而我国目前在这一领域的研究相对落后，尤其是在加减速控制技术的研究上，与国外还 有很大的差距。在机械运动的控制上，运动过程的平稳、无冲击、曲线光滑、均匀等特点对机 械运动的质量、精度都产生了极大的影响，受限于机械零件材质、精度等因素，机械运动往 往达不到所预期的效果，运动过程可能存在较大振动，运动末位置精度不良，运动时间过长 效率低下等。采用对称的点到点运动控制方法，能较好的实现机械臂各轴的平滑启停和速 度切换，可做到无加速的突变，能够较好的减少机械臂的运动冲击和震荡，从而有效的提高 工件的加工质量，特别适用于高速高精加工。但是对称的点到点运动方法，但是如果加加速 度取得过大，在启动和停止时运动构件会有明显的振动，如果加加速度取得过小，就无法保 证加工的效率。对于机械臂的点到点运动规划，启动阶段可以不考虑振动问题，但是停止阶 段必须考虑振动的影响，简单的采用较小的速度策略，来抑制振动，必然会带来加工效率低 的结果。
【发明内容】
[0003] 为解决上述问题，本发明的目的在于提供一种在保证加工效率的情况，减少机械 振动，能够适应各种复杂运动情形的机械臂点到点运动控制方法。
[0004] 为实现上述目的，本发明的技术方案为：
[0005] 根据加加速度比以及机床硬件限制条件，进行时间规划;进而根据各阶段的时间 值求出的速度曲线以及起始点，进行插补运算计算出中间点的坐标值，根据坐标值变化向 相应坐标输出脉冲信号，控制各执行元件的进给速度、进给方向和进给长度量等，进而完成 工件的加工任务。
[0006] 对加速阶段的最大加加速度J1，减速阶段的最大加加速度J2,加速阶段的最大限 制加速度A1，减速阶段的最大限制加速度A2根据实际运动需要进行调整，能适应复杂极端 的运动，尤其是对末位置状态有较高精度要求的运动情况，在原有的S型轨迹规划的基础进 行改进，保留S型轨迹规划的优点。
[0007] 进一步地，包括以下步骤：
[0008] 第一步，首先确定匀加加速度阶段的时间t 根据末状态位移约束和速度约 束条件以及加加速度比，计算出t jl，t j2;
[0009] 第二步，判断规划出的最大速度是否超过了所要求的最大限制速度值，若超过最 大限制速度值则令规划出的最大速度等于最大限制速度值，根据速度公式重新计算tjl，并 根据tjl，tj2的关系式计算出tj2，
[0010] 判断规划出的最大加速度值是否满足最大限制加速度，若超过所要求的最大限制 加速度值，则重新计算tjl，tj2,且tjl，tj2最终确定；
[0011] 第三步，确定匀加速阶段的时间tal和匀减速阶段的时间ta2,先考虑不存在匀速 阶段的情况，根据末状态位移约束和速度约束条件以及加速度比，计算出tal，ta2;
[0012] 第四步，计算规划出的最大速度值是否超过最大限制速度值，若超出，则根据最大 限制速度值重新计算tal，并根据tal，ta2的关系式计算出ta2,此时，tal，ta2最终确定； [0013]第五步，根据已得到的幻1，幻2，丨 &1，丨&2来计算位移值，判断是否满足所要求的最 大位移值，若不满足，则计算匀速阶段的时间tv，此时所有时间量t j 1，t j2，tal，ta2，tv均已 确定。
[0014] 把整个运动控制过程分为5步，使得复杂的轨迹规划过程变得简单有序，方便编程 实现。
[0015] 进一步地，各时间节点的位移、速度、加速度、加加速度的表达公式如下：
[0016] T0 时刻
[0017] jerk〇 = 〇，Ao = 0，Vo = 0，So = 0
[0018] T1 时刻
[0053]其中：J1表示加速阶段的最大加加速度(Jerk); J2表示减速阶段的最大加加速度； A1表示加速阶段的最大限制加速度;A2表示减速阶段的最大限制加速度;V表示最大限制速 度;S表示运动所要求的位移，其中，Rl = J2/Jl，R2=A2/Al;tjl表示加速时匀加加速度阶段 的时间为;tj2表示减速时匀加加速度阶段的时间；tal为匀加速阶段时间；tj2表示匀减速 阶段的时间；tv.为匀速阶段的时间;A5表示减速阶段中最大加加速度；；V3表示最大速度； V7表示末位置时速度，S7表示末位置时位移。
[0054 ]进一步地，确定勾加加速度阶段的时间t j 1、t j 2:
[0055] 位移约束:S7 = S
[0056] 速度约束:V7 = 0
[0057] 则，利用S7和V7的公式可以组成方程组(其中，tal、ta2、tv均为0):
[0058] ， : \S1 = J^j; +
[0059] 解得 t j 1、t j 2 的关系式为tji = ^/.Rj^j.2 _，令
[0060] 然后，
[0062]判断最大速度V3与最大限制速度V的大小，若V3大于V，则令V3等于V，根据V3的表 达公式重新计算t jl:
[0064]分别判断加速时最大加速度Ai与最大限制加速度A1的大小，和减速时最大加速度 A5与最大限制加速度A2的大小，若M大于A1，则，令心等于A1，根据公式重新求得tjl;若A5大 于A2，则令A5等于A2，根据公式重新求得t j2;
[0066] 至此，tjl、tj2已经最终确定。
[0067] 进一步地，确定匀加速阶段的时间tal和匀减速阶段的时间ta2:
[0068] 首先考虑最大速度未到达最大限制速度的情况
[0069] 位移约束:S7 = S，速度约束:V7 = 0 [0070]由位移约束可知tal和ta2的关系式：
[0072]带入速度约束方程后可得一个关于tal的一元二次方程，其中tal取方程的非负 根，并根据tal解得ta2的值；
[0078]判断最大速度V3与最大限制速度V的大小，若V3大于V，则令V3等于V，根据V3的表 达公式重新计算tal，ta2
[0080] 至此，tal，ta2已最终确定。
[0081] 与现有技术相比，本发明具有以下有益效果：
[0082] 本发明与现有技术相比有如下优点和效果：
[0083] 现有的点到点3阶对称运动轨迹规划控制方法中，加速段和减速段曲线对称，各运 动参数也对称，不能进行调节，不能适应复杂的、极端的运动轨迹。本发明可对加速阶段的 最大加加速度J1，减速阶段的最大加加速度J2,加速阶段的最大限制加速度A1，减速阶段的 最大限制加速度A2根据实际运动需要进行调整，能适应更多复杂极端的运动，尤其是对末 位置状态有较高精度要求的情况，并且能保证运动时间最优。同时，本发明兼容了 3阶对称 运动控制方法的计算速度短，运动平稳、减少冲击、震荡等优点，特别适合机械臂等高速高 精运动领域，提高了运动的精度和效率。
【附图说明】
[0084] 图1为本发明机械臂加速度变化图；
[0085]图2为本发明机械臂运行状态图。
【具体实施方式】
[0086] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对 本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并 不用于限定本发明。
[0087] 相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修 改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，在下文对本发明的细 节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的 描述也可以完全理解本发明。
[0088] 机械臂点到点运动控制方法，始末状态的速度，加速度，加加速度均为零，即始末 状态均相对静止，且位移能达到所要求的值并保证精度，保证运动时间最短。其加速阶段和 减速阶段并不对称，且可根据情况调整参数以达到实际运动的目标要求。具体可调参数包 括加速阶段的最大加加速度，减速阶段的最大加加速度，加速阶段的最大限制加速度，减速 阶段的最大限制加速度，最大限制速度。
[0089] 已知加工轨迹位移、机床硬件限制条件:最大限制速度、最大限制加速度、最大加 加速度，根据机床硬件限制，对时间进行轨迹规划，为满足各种运动情形，根据机械臂启动、 停止的精度要求，确定加加速度比R1，加速度比R2，R1=J2/J1，，R2=A2/A1，
[0090] 其中：J1是加速段的最大加加速度，J2是减速段的最大加加速度，A1是加速段的最 大限制加速度，A2是减速段的最大限制加速度，同时兼顾运动效率最优。
[0091] 各时间节点的位移、速度、加速度、加加速度的表达公式如下：
[0092] T0 时刻
[0093] Jerko = 0 ,Ao = 0 ,Vo = 0 ,So = 0
[0129]其中：J1表示加速阶段的最大加加速度(Jerk); J2表示减速阶段的最大加加速度； A1表示加速阶段的最大限制加速度;A2表示减速阶段的最大限制加速度;V表示最大限制速 度;S表示运动所要求的位移，其中，Rl = J2/Jl，R2=A2/Al;tjl表示加速时匀加加速度阶段 的时间为;tj2表示减速时匀加加速度阶段的时间；tal为匀加速阶段时间；tj2表示匀减速 阶段的时间；tv.为匀速阶段的时间;A5表示减速阶段中最大加加速度;V3表示最大速度;V7 表不末位置时速度;7表不末位置时位移。
[0130]本发明的工作步骤为：
[0131] 第一步，从机械臂控制系统中，得到各线段轨迹的起点和终点坐标值等数据，进而 计算出各线段轨迹的长度，根据控制系统以及工件的工艺要求分别确定加减速阶段的加加 速度、最大限制加速度和最大限制速度。
[0132] 第二步，确定已知参数:J1，表示加速阶段的最大加加速度(Jerk); J2,表示减速阶 段的最大加加速度;A1，表示加速阶段的最大限制加速度;A2,表示减速阶段的最大限制加 速度;V，表示最大限制速度;S，表示运动所要求的位移。其中，Rl = J2/J1，R2=A2/A1.
[0133] 第三步，假设各运动阶段的时间量:设加速时匀加加速度阶段的时间为t j 1;设减 速时匀加加速度阶段的时间为t j 2 ;设匀加速阶段是时间为t a 1;设匀减速阶段的时间为 t j2;设匀速阶段的时间为tv.
[0134] 第四步，求解各运动阶段时间量，具体如下：
[0135]按先后顺序求解tjl，tj2，tal，ta2，tv
[0136] 求解tjl，tj2
[0137] 首先考虑仅有tj l，tj2的情况
[0138] 位移约束:S7 = S
[0139] 速度约束:V7 = 0
[0140] 则，利用S7和V7的公式可以组成方程组(其中，tal、ta2、tv均为0):
[0141] \ , ' ,
[0142] 解得tjl、tj2的关系式为tj:1 =：
[0143]然后，
[0145]判断最大速度V3与最大限制速度V的大小，若V3大于V，则令V3等于V，根据V3的表 达公式重新计算tjl
[0147]分别判断加速时最大加速度Ai与最大限制加速度A1的大小，和减速时最大加速度 A5与最大限制加速度A2的大小。若&大于A1，则，令&等于A1，根据公式重新求得tjl;若A5大 于A2，则令A5等于A2，根据公式重新求得t j2.
[0149] 至此，tjl、tj2已经最终确定，可作为已知量在之后的计算中使用。
[0150] 求解 tal，ta2
[0151] 首先考虑最大速度未到达最大限制速度的情况
[0152] 位移约束:S7 = S，速度约束:V7 = 0
[0153] 由位移约束可知tal和ta2的关系式：
[0155]带入速度约束方程后可得一个关于tal的一元二次方程，其中tal取方程的非负 根，并根据tal解得ta2的值。
[0161]判断最大速度V3与最大限制速度V的大小，若V3大于V，则令V3等于V，根据V3的表 达公式重新计算tal，ta2
[0163] 至此，tal，ta2已最终确定。
[0164] 此时，tjl，tj2，tal，ta2已确定，代入S7的表达公式中，并令S7等于S，求出tv。
[0165] 至此第一段待加工轨迹的加减速变化时间都已经求出，根据加减速变化时间，可 以得到此待加工轨迹的速度曲线。
[0166] 第五步:判断是否还存在其他加工轨迹线段如果还有则重复第二、三、四步，如果 没有其他线段则进行第六步。
[0167] 第六步:根据求出的速度曲线以及起始点，进行插补运算计算出中间点的坐标值， 根据坐标值变化向相应坐标输出脉冲信号，控制各执行元件的进给速度、进给方向和进给 长度量等，进而完成工件的加工任务。
【主权项】
1. 机械臂点到点运动控制方法，包括加工轨迹位移、机床硬件限制条件：最大限制速 度、最大限制加速度、最大加加速度，根据机床硬件限制，对轨迹进行时间规划，其特征在 于:为满足各种运动情形，根据机械臂启动、停止的精度要求，确定加加速度比R1，加速度比 R2，R1=J2/J1，，R2=A2/A1， 其中：Jl是加速段的最大加加速度，J2是减速段的最大加加速度，Al是加速段的最大限 制加速度，A2是减速段的最大限制加速度，同时兼顾运动效率最优； 根据加加速度比以及机床硬件限制条件，进行时间规划;进而根据各阶段的时间值求 出的速度曲线以及起始点，进行插补运算计算出中间点的坐标值，根据坐标值变化向相应 坐标输出脉冲信号，控制各执行元件的进给速度、进给方向和进给长度量等，进而完成工件 的加工任务。2. 根据权利要求1所述的机械臂点到点运动控制方法，其特征在于，包括以下步骤： 第一步，首先确定匀加加速度阶段的时间tj I、tj2，根据末状态位移约束和速度约束条 件以及加加速度比，计算出tjl，tj2; 第二步，判断规划出的最大速度是否超过了所要求的最大限制速度值，若超过最大限 制速度值则令规划出的最大速度等于最大限制速度值，根据速度公式重新计算tjl，并根据 tjl，tj2的关系式计算出tj2， 判断规划出的最大加速度值是否满足最大限制加速度，若超过所要求的最大限制加速 度值，则重新计算tjl，tj2,且tjl，tj2最终确定； 第三步，确定匀加速阶段的时间tal和匀减速阶段的时间ta2,先考虑不存在匀速阶段 的情况，根据末状态位移约束和速度约束条件以及加速度比，计算出tal，ta2; 第四步，计算规划出的最大速度值是否超过最大限制速度值，若超出，则根据最大限制 速度值重新计算tal，并根据tal，ta2的关系式计算出ta2,此时，tal，ta2最终确定； 第五步，根据已得到的幻1，幻2，丨&1，丨&2来计算位移值，判断是否满足所要求的最大位 移值，若不满足，则计算匀速阶段的时间tv，此时所有时间量t jI，t j2，tal，ta2，tv均已确 定。3. 根据权利要求1所述的机械臂点到点运动控制方法，其特征在于，各时间节点的位 移、速度、加速度、加加速度的表达公式如下： TO时刻 Jerko = O, A〇 = 0,Vo = O,So = O Tl时刻 Jerki = Ji Ai = Jitji T2时刻Jerk2 = 0 A2 = Jltjl其中：Jl表示加速阶段的最大加加速度(Jerk) ;J2表示减速阶段的最大加加速度;Al表 示加速阶段的最大限制加速度;A2表示减速阶段的最大限制加速度;V表示最大限制速度;S 表示运动所要求的位移，其中，Rl=J2/Jl，R2=A2/Al;tjl表示加速时匀加加速度阶段的时 间为;tj2表示减速时匀加加速度阶段的时间；tal为匀加速阶段时间；tj2表示匀减速阶段 的时间；tv.为匀速阶段的时间;A5表示减速阶段中最大加加速度；；V3表示最大速度;V7表 不末位置时速度，S7表不末位置时位移。4. 根据权利要求2所述的机械臂点到点运动控制方法，其特征在于，确定匀加加速度阶 段的时间tjl、tj2: 位移约束:S7 = S 速度约束:V7 = O 贝IJ，利用S7和V7的公式可以组成方程组(其中，tal、ta2、tv均为0):解得的关j 然后， 判断最大速度V3与最大限制速度V的大小，若V3大于V，则令V3等于V，根据V3的表达公 式重新计算tjl:分别判断加速时最大加速度A1与最大限制加速度Al的大小，和减速时最大加速度A5与 最大限制加速度A2的大小，若A1大于Al，则，令A1等于Al，根据公式重新求得tjl;若A5大于 A2，则令A5等于A2，根据公式重新求得t.i2; 至此，tjl、tj2已经最终确定。5. 根据权利要求2所述的机械臂点到点运动控制方法，其特征在于，确定匀加速阶段的 时间tal和匀减速阶段的时间ta2: 首先考虑最大速度未到达最大限制速度的情况 位移约束:S7 = S，速度约束:V7 = 0 由位務约東可知tal和ta2的关系式：至此，tal，ta2已最终确定
【文档编号】G05B19/19GK105892402SQ201610444859
【公开日】2016年8月24日
【申请日】2016年6月17日
【发明人】杨亮亮, 沈波, 吴达伟, 胡建, 时军, 欧阳博
【申请人】浙江理工大学