一种伺服运动控制系统加减速轨迹规划方法及装置

专利名称：一种伺服运动控制系统加减速轨迹规划方法及装置

专利类型：发明专利

专利申请号：CN202211124326.6

专利申请（专利权）人：中国北方车辆研究所
权利人地址：北京市丰台区槐树岭四号院

专利发明（设计）人：田德文,刘华源,何淑贤,王震,陈嘉伟,王亚庆,邢彦斌,刘泓,姜茹,马亚青,孙文欣

专利摘要：本发明公开了一种伺服运动控制系统加减速轨迹规划方法及装置，所述方法包括：根据当前位置Pf及当前位置及当前速度Vf计算得到加减速判断阈值Perr1，基于加减速判断阈值Perr1，得到输出速度Vact；若所述输出速度Vact大于最大转速限制Vmax，则令Vact等于Vmax，进行输出速度限幅；若所述输出速度Vact小于最大负转速限制‑Vmax，则令Vact等于‑Vmax，进行输出速度限幅；基于所述位置步进量Pstep与当前位置Pf，确定位置规划输出Pact。本发明的方法，在计算过程中无需时间变量进行速度、位置和分段计算，能够根据指定总移动距离、加速度、减速度以及最高转速直接计算出速度和位置指令输出，方法简单、占用时间少、响应快。

主权利要求：
1.一种伺服运动控制系统加减速轨迹规划方法，其特征在于，包括如下步骤：
步骤S1：获取指示新位置的指令，若所述新位置Pr与当前位置Pf之间误差Perr小于到位判断位置阈值Perr0，则轨迹规划位置输出已到达目标位置，方法结束；否则，进入步骤S2；
步骤S2：根据当前位置Pf及当前位置处的输出速度Vf计算得到加减速判断阈值Perr1，若当前位置Pf的值小于加减速判断阈值Perr1，则将当前位置Pf处的当前速度Vf累加速度增量Vacc，得到输出速度Vact；否则，将前位置Pf处的当前速度Vf减少速度变化量Vdec，得到输出速度Vact；输出速度Vact是所述伺服运动控制系统基于轨迹规划计算得到的输出速度；
步骤S3：若所述输出速度Vact大于最大转速限制Vmax，则令Vact等于Vmax，进行输出速度限幅；若所述输出速度Vact小于最大负转速限制‑Vmax，则令Vact等于‑Vmax，进行输出速度限幅；
步骤S4：对所述输出速度Vact进行积分计算，确定当前计算周期位置步进量Pstep，基于所述位置步进量Pstep与当前位置Pf，确定位置规划输出Pact；所述位置步进量Pstep是指当前计算周期轨迹规划的位置增量，所述位置规划输出Pact是指最终位置输出；
所述步骤S2，若所述新位置Pr与当前位置Pf之间误差Perr大于0，则以当前位置Pf及此处速度Vact为初速度，匀减速至零，从而计算加减速判断阈值Perr1；若所述新位置Pr与当前位置Pf之间误差Perr小于0，则以当前位置Pf及此处速度Vact为初速度，匀加速至零，从而计算加减速判断阈值Perr1；所述速度增量Vacc基于伺服运动控制系统最大加速度确定，取值为Vacc＝ΔTaacc；所述速度变化量Vdec基于伺服运动控制系统最大减速度确定，取值为Vdec＝ΔTadec，其中，ΔT为轨迹规划的计算周期。
2.如权利要求1所述的方法，其特征在于，所述步骤S1，所述新位置是指当前计算周期的目标位置给定值，所述到位判断位置阈值Perr0的取值基于伺服运动控制系统的指标确定。
3.如权利要求1所述的方法，其特征在于，所述步骤S3，所述最大转速限制Vmax基于实际伺服运动控制系统的指标确定；所述最大负转速限制与所述最大转速限制Vmax互为相反数。
4.如权利要求1‑3中任一项所述的方法，其特征在于，所述步骤S4，基于所述输出速度Vact的积分确定位置步进量Pstep；若所述位置步进量Pstep小于位置偏差Perr，则将位置规划输出Pact累加位置步进量Pstep，输出Pact；否则，将所述新位置Pr直接赋值给位置规划输出Pact，输出Pact；所述位置步进量Pstep基于输出速度Vact积分确定，取值为Pstep＝ΔTVacc；位置偏差Perr是指新位置Pr与当前位置Pf之间误差，其中，ΔT为轨迹规划的计算周期。
5.一种伺服运动控制系统加减速轨迹规划装置，其特征在于，包括：
第一判断模块：配置为获取指示新位置的指令，若所述新位置Pr与当前位置Pf之间误差Perr小于到位判断位置阈值Perr0，则轨迹规划位置输出已到达目标位置；否则，触发第二判断模块；
第二判断模块：配置为根据当前位置Pf及当前位置处的输出速度Vf计算得到加减速判断阈值Perr1，若当前位置Pf的值小于加减速判断阈值Perr1，则将当前位置Pf处的当前速度Vf累加速度增量Vacc，得到输出速度Vact；否则，将前位置Pf处的当前速度Vf减少速度变化量Vdec，得到输出速度Vact；输出速度Vact是所述伺服运动控制系统基于轨迹规划计算得到的输出速度；
第三判断模块：配置为若所述输出速度Vact大于最大转速限制Vmax，则令Vact等于Vmax，进行输出速度限幅；若所述输出速度Vact小于最大负转速限制‑Vmax，则令Vact等于‑Vmax，进行输出速度限幅；
输出模块：配置为对所述输出速度Vact进行积分计算，确定当前计算周期位置步进量Pstep，基于所述位置步进量Pstep与当前位置Pf，确定位置规划输出Pact；所述位置步进量Pstep是指当前计算周期轨迹规划的位置增量，所述位置规划输出Pact是指最终位置输出；
其中，所述第二判断模块，若所述新位置Pr与当前位置Pf之间误差Perr大于0，则以当前位置Pf及此处速度Vact为初速度，匀减速至零，从而计算加减速判断阈值Perr1；若所述新位置Pr与当前位置Pf之间误差Perr小于0，则以当前位置Pf及此处速度Vact为初速度，匀加速至零，从而计算加减速判断阈值Perr1；所述速度增量Vacc基于伺服运动控制系统最大加速度确定，取值为Vacc＝ΔT·aacc；所述速度变化量Vdec基于伺服运动控制系统最大减速度确定，取值为Vdec＝ΔT·adec，其中，ΔT为轨迹规划的计算周期。
6.一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如权利要求1‑4中任一项所述方法。
7.一种电子设备，其特征在于，所述电子设备，包括：
处理器，用于执行多条指令；
存储器，用于存储多条指令；
其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如权利要求
1‑4中任一项所述方法。 说明书 : 一种伺服运动控制系统加减速轨迹规划方法及装置技术领域[0001] 本发明涉及伺服控制领域，具体涉及一种伺服运动控制系统加减速轨迹规划方法及装置。背景技术[0002] 一直以来，动态响应的快速性、稳态跟踪的高精度以及系统的鲁棒性是伺服控制系统追求的主要目标。为了提高伺服系统的跟踪性能，一方面，需要引入较为先进的控制算法，如模糊PID控制、自抗扰控制、滑模变结构控制及自适应鲁棒控制等，以提高闭环系统的带宽；另一方面，需要有优化的轨迹规划算法，以尽可能降低伺服系统输入信号的带宽。随着数控、机器人等领域的快速发展，系统对轨迹规划方法的要求越来越高。例如，在高精度直驱转台的运行过程中，为了使其按照预定的速度和位置实现快速准确地运动，需要利用轨迹规划在起、停、轨迹转接过程中进行加减速控制来减少冲击、超程或振荡。由于在快速启停时的轨迹规划要兼顾实时性与稳定性，因此，寻求简单、能满足精度要求和实时性的轨迹规划方法，已成为目前高速高精度运动控制系统的关键问题之一。[0003] 轨迹规划方法是伺服系统具有良好的跟踪性能的前提。好的轨迹规划方法除了需保证机械本体的运动平稳性和在启、停和插补段之间速度变化时不产生冲击、失步、超程、振荡外，还应算法简单、实时性强，并具有良好的通用性，便于在不同配置的系统中使用。直线加减速规划算法因在给定位置、速度、加速度的条件下定位时间最优而得到广泛应用。[0004] 理想的直线加减速规划方法分为加速段、匀速段和减速段三个阶段。第一阶段由初始速度以恒定加速度向最大速度递增；第二阶段以恒定的速度运动；第三阶段以恒定的加速度向初始速度减速，被控对象刚好到达目标位置。直线加减速规划方法的曲线如图1所示。[0005] 当前，业界普遍采用基于时间分段的速度变化曲线型式实现轨迹规划，程序需要指定总移动距离、加速度、减速度以及最高转速，需要实时根据当前指令周期运行时间，计算对应时间段的速度和位置指令输出。这种基于时间分段加减速控制算法的主要特点是算法简单，但是占用时间相对较多、执行响应相对较慢很难适应快速起停定位系统需要。发明内容[0006] 有鉴于此，本发明提供了一种伺服运动控制系统加减速轨迹规划方法及装置，能够解决现有的加减速轨迹规划的技术问题。[0007] 为了解决上述技术问题，本发明是这样实现的。[0008] 一种伺服运动控制系统加减速轨迹规划方法，包括：[0009] 步骤S1：获取指示新位置的指令，若所述新位置Pr与当前位置Pf之间误差Perr小于到位判断位置阈值Perr0，则轨迹规划位置输出已到达目标位置，方法结束；否则，进入步骤S2；[0010] 步骤S2：根据当前位置Pf及当前位置处经轨迹规划得到的输出速度Vf计算得到加减速判断阈值Perr1，若当前位置值Pf小于加减速判断阈值Perr1，则将当前位置Pf处的当前速度Vf累加速度增量Vacc，得到输出速度Vact；否则，将前位置Pf处的当前速度Vf减少速度变化量Vdec，得到输出速度Vact；输出速度Vact是所述伺服运动控制系统基于轨迹规划计算得到的输出速度；[0011] 步骤S3：若所述输出速度Vact大于最大转速限制Vmax，则令Vact等于Vmax，进行输出速度限幅；若所述输出速度Vact小于最大负转速限制‑Vmax，则令Vact等于‑Vmax，进行输出速度限幅；[0012] 步骤S4：对所述输出速度Vact进行积分计算，确定当前计算周期位置步进量Pstep，基于所述位置步进量Pstep与当前位置Pf，确定位置规划输出Pact；所述位置步进量Pstep是指当前计算周期轨迹规划的位置增量，所述位置规划输出Pact是指最终位置输出。[0013] 优选地，所述步骤S1，所述新位置是指当前计算周期的目标位置给定值，所述到位判断位置阈值Perr0的取值基于伺服运动控制系统的指标确定。[0014] 优选地，所述步骤S2，若所述新位置Pr与当前位置Pf之间误差Perr大于0，则以当前位置Pf及此处速度Vact为初速度，匀减速至零，从而计算预测目标位置Perr1；若所述新位置Pr与当前位置Pf之间误差Perr小于0，则以当前位置Pf及此处速度Vact为初速度，匀加速至零，从而计算预测目标位置Perr1；所述速度增量Vacc基于伺服运动控制系统最大加速度确定，取值为Vacc＝ΔT·aacc；所述速度变化量Vdec基于伺服运动控制系统最大减速度确定，取值为Vdec＝ΔT·adec，其中，ΔT为轨迹规划的计算周期。[0015] 优选地，所述步骤S3，所述最大转速限制Vmax基于实际伺服运动控制系统的指标确定；所述最大负转速限制与所述最大转速限制Vmax互为相反数。[0016] 优选地，所述步骤S4，基于所述输出速度Vact的积分确定位置步进量Pstep；若所述位置步进量Pstep小于位置偏差Perr，则将位置规划输出Pact累加位置步进量Pstep，输出Pact；否则，将所述新位置Pr直接赋值给位置规划输出Pact，输出Pact。所述位置步进量Pstep基于输出速度Vact积分确定，取值为Pstep＝ΔT·Vacc；位置偏差Perr是指新位置Pr与当前位置Pf之间误差，其中，ΔT为轨迹规划的计算周期。[0017] 本发明所提供的一种伺服运动控制系统加减速轨迹规划装置，所述装置包括：[0018] 第一判断模块：配置为获取指示新位置的指令，若所述新位置Pr与当前位置Pf之间误差Perr小于到位判断位置阈值Perr0，则轨迹规划位置输出已到达目标位置；否则，触发第二判断模块；[0019] 第二判断模块：配置为根据当前位置Pf及当前位置处经轨迹规划得到的输出速度Vf计算得到加减速判断阈值Perr1，若当前位置值Pf小于加减速判断阈值Perr1，则将当前位置Pf处的当前速度Vf累加速度增量Vacc，得到输出速度Vact；否则，将前位置Pf处的当前速度Vf减少速度变化量Vdec，得到输出速度Vact；输出速度Vact是所述伺服运动控制系统基于轨迹规划计算得到的输出速度；[0020] 第三判断模块：配置为若所述输出速度Vact大于最大转速限制Vmax，则令Vact等于Vmax，进行输出速度限幅；若所述输出速度Vact小于最大负转速限制‑Vmax，则令Vact等于‑Vmax，进行输出速度限幅；[0021] 输出模块：配置为对所述输出速度Vact进行积分计算，确定当前计算周期位置步进量Pstep，基于所述位置步进量Pstep与当前位置Pf，确定位置规划输出Pact；所述位置步进量Pstep是指当前计算周期轨迹规划的位置增量，所述位置规划输出Pact是指最终位置输出。[0022] 本发明所提供的一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如前所述方法。[0023] 本发明所提供的一种电子设备，其特征在于，所述电子设备，包括：[0024] 处理器，用于执行多条指令；[0025] 存储器，用于存储多条指令；[0026] 其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如前所述方法。[0027] 有益效果：[0028] (1)本发明提供了一种伺服运动控制系统加减速轨迹规划方法，它在计算过程中无需时间变量进行速度、位置和分段计算，能够根据指定总移动距离、加速度、减速度以及最高转速直接计算出速度和位置指令输出，方法简单、占用时间少、响应快。[0029] (2)本发明特别适合高速、快速起停伺服运动控制系统。[0030] (3)本发明对硬件平台约束较小，易于移植，实现通用化设计。[0031] (4)本发明简单、能满足精度要求和实时性的伺服运动控制系统。在高精度直驱转台的运行过程中，为了使其按照预定的速度和位置实现快速准确地运动，利用轨迹规划在起、停、轨迹转接过程中进行加减速控制来减少冲击、超程或振荡。在快速启停时能够兼顾实时性与稳定性。附图说明[0032] 图1为现有技术的直线加减速规划方法的曲线示意图；[0033] 图2为本发明提供的伺服运动控制系统加减速轨迹规划方法流程示意图；[0034] 图3为本发明提供的又一伺服运动控制系统加减速轨迹规划方法流程示意图；[0035] 图4为本发明提供的转速变化曲线示意图；[0036] 图5为本发明提供的轨迹规划位置指令计算示意图；[0037] 图6为本发明提供的轨迹规划位置指令到位处理示意图；[0038] 图7为本发明提供的伺服运动控制系统加减速轨迹规划装置结构示意图。具体实施方式[0039] 下面结合附图和实施例，对本发明进行详细描述。[0040] 如图2‑3所示，本发明提出了一种伺服运动控制系统加减速轨迹规划方法，包括如下步骤：[0041] 步骤S1：获取指示新位置的指令，若所述新位置Pr与当前位置Pf之间误差Perr小于到位判断位置阈值Perr0，则轨迹规划位置输出已到达目标位置，方法结束；否则，进入步骤S2；[0042] 步骤S2：根据当前位置Pf及当前位置处经轨迹规划得到的输出速度Vf计算得到加减速判断阈值Perr1，若当前位置值Pf小于加减速判断阈值Perr1，则将当前位置Pf处的当前速度Vf累加速度增量Vacc，得到输出速度Vact；否则，将前位置Pf处的当前速度Vf减少速度变化量Vdec，得到输出速度Vact；输出速度Vact是所述伺服运动控制系统基于轨迹规划计算得到的输出速度；[0043] 步骤S3：若所述输出速度Vact大于最大转速限制Vmax，则令Vact等于Vmax，进行输出速度限幅；若所述输出速度Vact小于最大负转速限制‑Vmax，则令Vact等于‑Vmax，进行输出速度限幅；[0044] 步骤S4：对所述输出速度Vact进行积分计算，确定当前计算周期位置步进量Pstep，基于所述位置步进量Pstep与当前位置Pf，确定位置规划输出Pact；所述位置步进量Pstep是指当前计算周期轨迹规划的位置增量，所述位置规划输出Pact是指最终位置输出。[0045] 进一步地，所述步骤S1，所述新位置是指当前计算周期的目标位置给定值，所述到位判断位置阈值Perr0的取值基于伺服运动控制系统的指标确定，如选择0.001°。[0046] 进一步地，所述步骤S2，若所述新位置Pr与当前位置Pf之间误差Perr大于0，则以当前位置Pf及此处速度Vact为初速度，匀减速至零，从而计算预测目标位置Perr1；若所述新位置Pr与当前位置Pf之间误差Perr小于0，则以当前位置Pf及此处速度Vact为初速度，匀加速至零，从而计算预测目标位置Perr1；所述速度增量Vacc基于伺服运动控制系统最大加速度确定，取值为Vacc＝ΔT·aacc；所述速度变化量Vdec基于伺服运动控制系统最大减速度确定，取值为Vdec＝ΔT·adec，其中，ΔT为轨迹规划的计算周期。[0047] 进一步地，所述步骤S3，所述最大转速限制Vmax基于实际伺服运动控制系统的指标确定；所述最大负转速限制与所述最大转速限制Vmax互为相反数。[0048] 进一步地，所述步骤S4，基于所述输出速度Vact的积分确定位置步进量Pstep；若所述位置步进量Pstep小于位置偏差Perr，则将位置规划输出Pact累加位置步进量Pstep，输出Pact；否则，将所述新位置Pr直接赋值给位置规划输出Pact，输出Pact。所述位置步进量Pstep基于输出速度Vact积分确定，取值为Pstep＝ΔT·Vacc；位置偏差Perr是指新位置Pr与当前位置Pf之间误差。[0049] 以下结合具体实施例说明伺服运动控制系统加减速轨迹规划方法，如图3‑6所示。[0050] 如图3所示，当接收到新的位置指令，轨迹规划方法开始执行，首先计算位置指令Pr与当前位置Pf之间偏差Perr。[0051] Perr＝Pr‑Pf(1)[0052] 如果绝对位置偏差Perr小于到位判断位置阈值Perr0的绝对值，即|Perr|≤|Perr0|，轨迹规划位置指令输出已经落入目标位置输出的误差带内，则认为轨迹规划已到达目标指令位置，轨迹规划结束。如果位置偏差Perr的绝对值大于到位判断位置阈值Perr0的绝对值，即|Perr|＞Perr0|，则认为轨迹规划位置指令输出未到达目标指令输出，则进入轨迹规划程序，继续规划速度和位置指令输出。到位判断位置阈值Perr0取值，根据实际伺服运动控制系统的指标确定，如选择0.001°。[0053] 接下来进行加减速判断。在轨迹规划位置输出未到达目标位置时，首先进行加减速条件判断，如图4所示。[0054] 加减速规划算法分为加速段、匀速段和减速段三个阶段。第一阶段t0～t1由初始速度以恒定加速度向最大速度递增，每个程序执行周期速度增量为Vacc。第三阶段t2～t3以恒定的减速度向初始速度减速，每个程序执行周期速度减量为Vdec。[0055] Vacc＝ΔT·aacc(2)[0056] Vdec＝ΔT·adec(3)[0057] 式中，ΔT为轨迹规划程序的执行周期，aacc为伺服运动控制系统运行的最大加速度，adec为伺服运动控制系统运行的最大减速度。[0058] 本发明轨迹规划速度输出处于加速、减速还是恒速阶段，不再采用基于时间分段的形式，即如果t1>t>t0,则认为处于加速段这种判断条件，而是采用一种新颖的判断方式。当轨迹规划速度输出运行到图4的a点时，不关注运行至a点的时间，此时计算以a点为起点，速度沿a～c减速运行时，位置规划输出的位移。如果当前位置加上沿a～c减速运行时理论输出的位移大于或等于位置目标指令指定的位置，则规划程序进入减速段。[0059] Vact＝Vact‑Vdec(4)[0060] 如果当前位置加上沿a～c减速运行时理论输出的位移仍然小于目标位置，则继续进行加速。[0061] Vact＝Vact+Vacc(5)[0062] 恒速段的处理较为简单，当速度输出Vact大于最大转速限制Vmax时，则令Vact＝Vmax自动进入恒速运动段；当速度输出Vact小于最大负转速限制Vmax时，则令Vact＝‑Vmax自动进入恒速运动段。[0063] 轨迹规划程序实时计算当前位置点，判断沿着加或减速曲线运行是否可以到的目标点，如果能够达到则置位相应标志位，进入加速或者减速段。当位置指令为反方向时，结论仍然成立，此处不再赘述。[0064] 再进行到位判断。轨迹规划程序计算出速度输出指令后，对速度进行积分，计算出位置规划指令Pact，如图5所示。[0065] 轨迹规划程序对速度积分计算，当前计算周期位置增量如下：[0066] Pstep＝ΔT·Vacc(6)[0067] 如图6所示，如果位置步进量小于位置偏差Perr，即当前执行周期后无法到达指令目标位置，则继续累加位置输出。[0068] Pact＝Pact+Pstep(7)[0069] 如果位置步进量大于位置偏差Perr，即当执行Pact＝Pact+Pstep后，有Pact＞Pr，将造成轨迹规划位置指令超过给定值，此时将位置指令Pr直接赋值给位置规划输出Pact，即：[0070] Pact＝Pr(8)[0071] 本发明还提供了一种伺服运动控制系统加减速轨迹规划装置，如图7所示，该装置包括：[0072] 第一判断模块：配置为获取指示新位置的指令，若所述新位置Pr与当前位置Pf之间误差Perr小于到位判断位置阈值Perr0，则轨迹规划位置输出已到达目标位置；否则，触发第二判断模块；[0073] 第二判断模块：配置为根据当前位置Pf及当前位置处经轨迹规划得到的输出速度Vf计算得到加减速判断阈值Perr1，若当前位置值Pf小于加减速判断阈值Perr1，则将当前位置Pf处的当前速度Vf累加速度增量Vacc，得到输出速度Vact；否则，将前位置Pf处的当前速度Vf减少速度变化量Vdec，得到输出速度Vact；输出速度Vact是所述伺服运动控制系统基于轨迹规划计算得到的输出速度；[0074] 第三判断模块：配置为若所述输出速度Vact大于最大转速限制Vmax，则令Vact等于Vmax，进行输出速度限幅；若所述输出速度Vact小于最大负转速限制‑Vmax，则令Vact等于‑Vmax，进行输出速度限幅；[0075] 输出模块：配置为对所述输出速度Vact进行积分计算，确定当前计算周期位置步进量Pstep，基于所述位置步进量Pstep与当前位置Pf，确定位置规划输出Pact；所述位置步进量Pstep是指当前计算周期轨迹规划的位置增量，所述位置规划输出Pact是指最终位置输出。[0076] 以上的具体实施例仅描述了本发明的设计原理，该描述中的部件形状，名称可以不同，不受限制。所以，本发明领域的技术人员可以对前述实施例记载的技术方案进行修改或等同替换；而这些修改和替换未脱离本发明创造宗旨和技术方案，均应属于本发明的保护范围。

专利地区：北京

专利申请日期：2022-09-15

专利公开日期：2024-06-18

专利公告号：CN115542732B