CN107765639B - 一种s曲线加减速的圆整误差补偿方法 - Google Patents

Abstract

本发明属于数控***运动控制领域，具体是一种针对S曲线加减速的圆整误差补偿方法，在初次进行S曲线规划后，加速段、匀速段和减速段可能不同时存在，针对不同组合类型，根据前述运动参数关系的分析，需要采用不同的误差补偿策略。当存在匀速段时选择位移调整方式；当不存在匀速段时且同时存在加速段和减速段时，选择最大速度调整方式；当存在匀速段但是匀速段的位移比较短无法满足调整要求时，先使用位移调整，再使用速度调整。本发明不调整单段插补时间，只调整插补总时间，尽可能地降低补偿的插补时间，提高插补效率，基于调整后的插补时间，使用位移和速度调整的方式重新计算全部的运动参数，保证调整后的运动参数均不超限。

Description

一种S曲线加减速的圆整误差补偿方法

技术领域

本发明属于数控***运动控制领域，具体是一种针对S曲线加减速的圆整误差补偿方法。

背景技术

在数控***中，为避免各轴产生冲击、失步、超程和振荡，以保证运动部件的平稳和准确定位，必须进行加减速控制。同时，良好的加减速控制能够实现数控机床的快速反应，短时间内达到指定速度，缩短加减速时间，提高生产效率。常用的加减速控制方法有直线加减速、指数加减速、S曲线加减速等方法。直线加减速和指数加减速虽然计算量小，编程简单，但是在加减速阶段存在加速度突变的现象，导致机床产生剧烈振动，不适合用于数控机床的高速加工。S曲线加减速方法可实现加减速过程中加速度的连续变化，能够有效的减少冲击和振荡，适合高速高精加工的应用。

目前数控***大都基于数据采用控制***，其数据采用周期即为插补周期，均为一固定时间周期常数，从1-8ms不等。在数据采用控制***中，所有不同处理步骤的时间理论上必须为该周期的整数倍，但实际上会有误差，因此会带来响应的量化误差问题，即圆整误差。采样周期越大，误差也越大。该误差会影响严重影响加工的精度。

现有圆整误差补偿方法的核心思想是对规划中的各时间段，在段与段衔接的地方调整求得的时间值，然后再根据调整后的时间进行速度规划。时间调整方法主要分为两种:一种是向下圆整法。向下圆整法的步骤包括：保留各段时间内插补周期的整数倍部分；然后根据圆整的时间计算各段间衔接点的速度和各段的插补位移；根据实际待插补位移与圆整后计算的位移可计算出因圆整造成的位移误差，然后通过采用在起点、终点、匀速段增加匀速运动周期的方法进行误差补偿。该方法降低了原规划中所能达到的最大加速度、最大速度，增加了运动时间，同时无法做到理论无差，最后一个周期可能会存在加速度和速度的冲击。另一种方法是向上圆整法。通过将后一段的时间补偿到前一段，使得前一段时间向上圆整。但该方法的最后一段t7无法是插补周期的整数倍，也会造成一个周期的速度波动，同时因前几段时间向上圆整之后，仍采用原有的运动参数进行运动，导致段间衔接处出现位置、速度和加速度误差，也对后续时间段的计算造成误差，严重影响精度。

中国专利文件(申请号201310027747.1)，公开了一种用于数控***的加减速规划方法，方法包括：对数控机床操作设备的加工轨迹进行T型曲线速度规划，获取T型曲线中匀加速、匀速和匀减速的时间；对T型曲线中匀加速、匀速和匀减速三个时间段单独调整，将不满一个插补周期的步长补足为一个完整的插补周期；该方法将T型曲线中每个时间段都补足为整数周期，这样造成了插补时间和位移的增大，降低插补效率；同时，T型曲线规划没有考虑加加速度变化，不能保证调整后的加加速度不超限。

中国专利文件(申请号201410280826.8)，公开了一种连续多类型曲线段混合插补三次多项式进给速度前瞻规划方法，包括五大步骤：步骤1：连续多类型曲线段生成；步骤2：段间衔接点临界速度求解；步骤3：单段曲线进给速度规划；步骤4：单段速度规划残差补偿；步骤5：进给速度曲线生成。为了补偿圆整误差，该规划方法将补偿的运动参数直接与圆整的运动参数叠加，通过圆整插补时间计算得到的运动参数是满足要求的，但是不能保证叠加后的运动参数满足要求。

发明内容

针对上述问题，目前采用的圆整方法都力图使t₁-t₇每一个时间段都成为插补周期的整数倍。但实际上，在S曲线加减速中，加速度、速度和位移都是时间的连续函数，变量t在取值范围内可以任意取值，因此并没有必要保证每一段时间都是插补周期的整数倍，只需要保证总时间T是插补周期的整数倍即可。本发明提出一种S曲线加减速误差补偿方法，能够精确解决圆整误差，实现理论无差，消除因圆整误差造成的速度波动问题。

定义：

其曲线按时间顺序包括七个阶段，分别是：加加速段L₁、匀加速段L₂、减加速段L₃、匀速段L₄、加减速段L₅、匀减速段L₆和减减速段L₇；对应这7个运动阶段，总运动时间T也被分成了7部分。t为时间变量，t_i(i＝1,2,...,7)为时间未圆整时各段终点时刻，Δt_i为时间未圆整时第i段的时间，有Δt_i＝t_i-t_i-1，T为未圆整时的插补总时间，T_s为插补周期，j(t)、a(t)、v(t)、s(t)分别为加加速度、加速度、速度和位移随时间的变化关系，J_com、a_com、v_com分别为数控设备允许最大加加速度、最大加速度和最大速度，v_max为未进行时间圆整时规划得到的最大速度，v_s为起点速度，v_e为终点速度，S为待规划的位移。

本发明的技术方案如下：

本发明的基本思路是：当总时间T不是插补周期T_s整数倍时，通过对各段运动参数的调整，实现总时间的圆整，所述运动参数包括时间、加加速度、加速度、速度和位移。因此本发明对整个运动过程中的三部分，即加速段、匀速段和减速段，进行运动参数的综合调整，使得总插补时间为插补周期的整数倍，实现理论无差，解决圆整误差的补偿问题。

根据运动参数关系的分析可知，为满足各项约束，调整插补时间后可对应调整的参数包括运动位移、最大速度、起点速度和终点速度等参数，但同时调整多项参数，逻辑分析困难且计算量大，难以实现。初次进行S曲线规划后，加速段、匀速段和减速段可能不同时存在，针对不同组合类型，根据前述运动参数关系的分析，需要采用不同的误差补偿策略。即有整体思路如下：当存在匀速段时选择位移调整方式；当不存在匀速段时且同时存在加速段和减速段时，为保证首末点速度为规划要求的速度，选择最大速度调整方式；当存在匀速段但是匀速段的位移比较短无法满足调整要求时，先使用位移调整，再使用速度调整(属于第一种方式中的特殊情况)。

一种S曲线加减速的圆整误差补偿方法，曲线按时间顺序分为加速段acc、匀速段L₄、减速段dec，加速段包括加加速段L₁、匀加速段L₂和减加速段L₃，减速段包括加减速段L₅、匀减速段L₆和减减速段L₇；设S曲线规划后的总插补时间为t_whole1＝t_acc1+t_con+t_dec1＝m₁T_s+Δt，其中t_acc1为加速段时间，t_con为匀速段时间，t_dec1为减速段时间，T_s为插补周期，m₁为非负整数，Δt∈[0,T_s)，Δt为非完整周期；

若Δt＝0，则无需进行插补时间调整；若Δt∈(0,T_s)，则需进行调整，进入如下判断：

(1)当加速段、匀速段、减速段同时存在，选择位移调整方式；

保持v_max与v_s、v_e不变，对t_acc1与t_dec1进行增大调整，对应的加速段和减速段的运动位移也会相应的增加，产生一个位移增量区间同时为保证总位移不变，需要减小t_con，对应减小的位移量ΔS_con，应当在保证总插补时间为插补周期的整数倍的同时，使得ΔS_con位于内部，则表示调整完成；

(2)当不存在匀速段时且同时存在加速段和减速段时，为保证首末点速度为规划要求的速度，选择最大速度调整方式：此时需要保持加速段和减速段的位移及v_s、v_e不变，通过减小最大速度v_max、增大加速段的插补时间t_acc1与减速段的插补时间t_dec1，以实现总插补时间为插补周期的整数倍，当调整时间后的加速段与减速段的

最大速度调整范围有交集时，即表示调整完成。

根据本发明优选的，步骤(1)中，

设Δt′为加速段和减速段增加的时间之和，将Δt′分配到加速段和减速段中，不同的分配方式，会有不同的位移增量区间假设将Δt′全部补偿到加速段，获得新的加速段的时间，求得位移增量区间ΔS_accL为加速段增量区间的下限、ΔS_accH为加速段增量区间的上限；假设将Δt′全部补偿到减速段，获得新的减速段的时间，获得位移增量区间ΔS_decL为减速段增量区间的下限、ΔS_decH为减速段增量区间的上限；假设将Δt′平均分配到加速段与减速段，获得新的加速段和减速段的时间，根据(式18)、(式35)--(式70)分别获得加速段和减速段的位移增量区间为[ΔS′_accL,ΔS′_accH]、[ΔS′_decL,ΔS′_decH]，则位移增量区间为ΔS′_accL为均分加速段增量区间下限、ΔS′_accH为均分加速段增量区间上限、ΔS′_decL为均分减速段增量区间下限、ΔS′_decH为均分减速段增量区间上限；

认为最大范围区间与最小范围区间就存在中，同时可获得范围：

初始化n₁＝n₂＝1，t′_con＝t_con，ΔS_con＝0，n₁、n₂为插补周期的增量倍数，t′_con为调整后的匀速段时间，按如下过程进行位移与时间调整：

Step1-1：增加加速段和减速段总时间，时间增量为Δt′＝T_s-Δt，获得位移增量区间ΔS_L、ΔS_H分别为位移增量区间的下限、上限，ΔS_L＝min(ΔS′_accL+ΔS′_decL，ΔS_accL，ΔS_decL)、ΔS_H＝max(ΔS′_accH+ΔS′_decH，ΔS_accH，ΔS_decH)；判断是否存在ΔS_L≤ΔS_con≤ΔS_H：若存在，则进入Step1-6；若不存在，则进入Step1-2；

Step1-2：缩短匀速段时间，将t′_con更新为t′_con-n₁T_s，则匀速段位移变化量为ΔS_con＝n₁T_sv_max；比较ΔS_con与原匀速段位移S_con的关系：若ΔS_con＜S_con，则进入Step1-3；若ΔS_con≥S_con，则使ΔS_con＝S_con，进入Step1-4；

Step1-3：比较ΔS_con与的关系：若ΔS_con＜ΔS_L，则将n₁更新为n₁+1，则返回Step1-2；若则进入Step1-6；若ΔS_con＞ΔS_H，则进入Step1-5；

Step1-4：比较ΔS_con与的关系：

若ΔS_con＜ΔS_L，则说明位移调整无法满足调整要求，需将匀速段位移补偿到加速段和减速段，然后进行最大速度调整，进入步骤(2)；

具体的：匀速段位移补偿到加速段与减速段的具体方法是：在(0,Δt′)内以二分法方式减小时间t_con并重新计算直到在(0,Δt′)内必有解。然后按照ΔS_con所处的区间类型进行判断并将位移和时间进行补偿，判断ΔS_con处于或或则将匀速段减小的位移量ΔS_con全部补偿到加速段，或全部补偿到减速段，或平均补偿到加速段和减速段，然后退出位移调整，进入步骤(2)。

若则进入Step1-6；若ΔS_con＞ΔS_H，则进入Step1-5；

Step1-5：增加加速段和减速段总时间，将增量Δt′更新为Δt′+n₂T_s，重新更新位移增量区间比较ΔS_con与的关系：若ΔS_con＜ΔS_L，则将n₁更新为n₁+1，进入Step1-2；若则进入Step1-6；若ΔS_con＞ΔS_H，则将n₂更新为n₂+1，返回Step1-5；

Step1-6：此时说明存在Δt′满足调整要求，有匀速段时间为t′_con，进行如下判断：

若则可根据ΔS_con所属的具体区间类型计算出调整后的加速段时间t′_acc1和调整后的减速段时间t′_dec1，当存在多区间交集时可任选一个区间类型；具体的，根据判断ΔS_con是属于还是还是来增加相应的时间，如果ΔS_con属于增量Δt′全部加到加速段的原始时间t_acc1上，得到调整后的加速段时间t′_acc1；如果ΔS_con属于增量Δt′全部加到减速段的原始时间t_dec1上，得到调整后的减速段时间t′_dec1，如果ΔS_con属于0.5倍的增量Δt′加到加速段，0.5倍的增量Δt′加到减速段，得到调整后的加速段时间t′_acc1和调整后的减速段时间t′_dec1；

若则需要对Δt′的分配做出调整；调整方法是根据与ΔS_con相邻的两个位移增量区间类型和单调性关系，使用二分法寻找新的Δt′的分配方案，求得新的分配给加速段的时间增量Δt′_acc1、新的分配给减速段的时间增量Δt′_dec1；得到调整后的加速段时间为t′_acc1＝t_acc1+Δt′_acc1，调整后的减速段时间为t′_dec1＝t_dec1+Δt′_dec1，其中Δt′＝Δt′_acc1+Δt′_dec1；

利用调整后的各段时间t_a′_cc1、t′_dec1和t′_con，即可计算加速段、减速段各段时间及各运动参数，运动参数包括加加速度、加速度、速度、位移，完成圆整误差的补偿。

具体的，运动参数之间的关系如下：利用Step1-6调整后的各段时间，求出加速段、减速段的加加速度，再利用加加速度与加速度、速度、位移的对应关系求得各段的加速度、速度、位移参数，完成圆整误差的补偿。

根据本发明优选的，步骤(2)中，当不存在匀速段时且同时存在加速段和减速段时，

在进行最大速度调整之前可能已经进行了位移调整，因此重新假设当前的插补总时间为t_whole2＝t_acc2+t_dec2＝m₂T_s+Δt″，其中t_acc2为当前加速段时间，t_dec2为当前减速段时间，m₂为非负整数，Δt″∈[0,T_s)；Δt″为本步骤中的非完整周期，Δt″包括两种情况：一种是经过了步骤(1)位移调整后余下的非完整周期，另一种是不存在匀速段直接进入本步骤时的非完整周期；需要补偿进的时间为Δt″′，Δt″′＝T_s-Δt″，根据需要补偿进的时间，求得加速段时间调整后的最大速度取值范围为[v_aL,v_aH]，v_aL为加速段最大速度调整区间的下限、v_aH为加速段最大速度调整区间的上限，根据需要补偿进的时间，求得减速段时间调整后的最大速度取值范围为[v_dL,v_dH]，v_dL为加速段最大速度调整区间的下限、v_dH为加速段最大速度调整区间的上限，下面以v_s≥v_e为例给出调整策略，

Step2-1：计算加速段最大调整时间Δt_{a max}，计算减速段最大调整时间为Δt_{d max}，有Δt_{a max}≥Δt_{d max}；

Step2-2：Δt″′＝T_s-Δt″，比较Δt″′与Δt_{a max}的关系：若Δt″′≥Δt_{a max}，则进入Step2-4；若Δt″′＜Δt_{a max}，则进入Step2-3；

Step2-3：使用二分法调整Δt″′在加速段和减速段内的分配量，求得加速段、减速段的时间调整后的最大速度v′_max的取值范围区间，直到加速段与减速段的最大速度取值范围有交集，此时必有解，可取交集内的任意速度为最大速度，完成调整并退出；

Step2-4：Δt″′₂＝Δt″′-Δt_{a max}，Δt″′₂为在保证加速段完成最大速度调整前提下的剩余非完整时间，比较Δt″′₂与Δt_dmax的关系：若Δt″′₂≤Δt_{d max}，则进入Step2-5；若Δt″′₂＞Δt_{d max}，无解，退出调整；

Step2-5：将Δt_{a max}补偿给加速段，加速段的最大速度取值仅为v_s；将Δt″′₂＝Δt″′-Δt_{a max}补偿给减速段，获得减速段最大速度的调整范围[v_dL,v_dH]，比较v_s与[v_dL,v_dH]：

若v_s＜v_dL，同Step2-3中的二分法调整方法，使得取交集内的任意速度为最大速度，完成调整并退出；

若v_s∈[v_dL,v_dH]，取最大速度为v_s，完成调整并退出；

若v_s＞v_dH，进入Step2-6；

Step2-6：将补偿给减速段获得[v_dL,v_dH]，将补偿给加速段得到[v_aL,v_aH]，比较[v_dL,v_dH]与[v_aL,v_aH]：

若取交集内的任意值作为最大速度，完成调整并退出；

若停止调整并退出；

进一步优选的，步骤(2)中，退出最大速度调整后，若有解，将调整后的加速段时间、调整后的减速段时间及调整后的最大速度代回(式19)-(式24)、(式1)-(式10)，求出加速段、减速段的加加速度。(完成最大速度的调整即完成了圆整误差补偿，求解加加速度视为验证)

本发明的有益效果如下：

1、本申请方案基于S曲线各段时间取值可任意的事实，不调整单段插补时间，只调整插补总时间，将插补总时间补足为插补周期的整数倍，尽可能地降低补偿的插补时间，提高插补效率；本方案根据调整后的时间，采用位移调整和速度调整的方式重新计算计算加加速度、加速度、速度和位移，能够保证调整后的运动参数不超限。

2、本申请方案在S曲线加减速规划方法的基础上，调整插补时间，理论上消除圆整误差，提高了插补效率；基于调整后的插补时间，使用位移和速度调整的方式重新计算全部的运动参数，保证调整后的运动参数均不超限。

附图说明

图1为实施例1调整之后的加加速度曲线图，图2为实施例1调整之后的加速度曲线图，图3为实施例1调整之后的速度曲线图；图4为实施例2调整之后的加加速度曲线图；图5为实施例2调整之后的加速度曲线图；图6为实施例2调整之后的速度曲线图；图7为实施例3调整之后的加加速度曲线图；图8为实施例3调整之后的加速度曲线图；图9为实施例3调整之后的速度曲线图。

具体实施方式：

下面通过具体的实施例对本发明的技术方案做进一步的说明，但不限于此。

实施例1：

本发明方案的原理论证如下：

在S曲线加减速中，各段的加加速度为常数且不连续，匀加速段和匀减速段加加速度为0，但经过参数调整后可能不为0，因此设各段加加速度分别为J₁、J₂、J₃，J₅，J₆，J₇，a_i(i＝1,2,...,7)为t_i时刻的加速度值，v_i为t_i时刻的速度值，ΔS_i为第i段内的位移，根据S曲线加减速的性质，有J₁＝J_com，J₂＝0，J₃＝J_com J₅＝J_com，J₆＝0，J₇＝J_com，Δt₁＝Δt₃，Δt₅＝Δt₇；

加加速度与加速度、速度、位移之间的积分关系为：

其中τ_i＝t-t_i-1(i＝1,2,..,7；t₀＝0)；

有如下关系：

a₁＝J₁Δt₁；a₂＝a₁+J₂Δt₂；a₃＝a₂+J₃Δt₃；a₄＝0

a₅＝J₅Δt₅；a₆＝a₅+J₆Δt₆；a₇＝a₆+J₇Δt₇ (式4)

v₄＝v₃ (式5)

因此加速段和减速段运动总位移分别为

S_acc＝ΔS₁+ΔS₂+ΔS₃；S_dec＝ΔS₅+ΔS₆+ΔS₇ (式7)

加速段结束时加速度为零，即：a₃＝0 (式8)

加速段结束时速度达到最大值，即：v₃＝v_max (式9)

减速段结束时速度为终点速度值，即：v₇＝v_e (式10)

联立(式4)-(式10)并做变换，可得以下关系

由(式11)-(式16)可知，存在如下单调性关系：

a)v_s、v_max、Δt₁、Δt₂、Δt₃保持不变时，J₁、J₃与S_acc是单调递增关系，J₂与S_acc是单调递减关系。

b)v_s、S_acc、Δt₁、Δt₂、Δt₃保持不变时，J₁、J₃与v_max是单调递减关系，J₂与v_max是单调递增关系。

c)v_max、S_acc、Δt₁、Δt₂、Δt₃保持不变时，J₁、J₃与v_s是单调递减关系，J₂与v_s是单调递增关系。

d)v_max、S_acc、v_s保持不变时，J₁、J₃与Δt₁、Δt₂、Δt₃是单调递减关系。

根据单调性关系，可通过增大插补时间Δt₁、Δt₂、Δt₃，同时增大S_acc、减小v_max与v_s，

e)v_e、v_max、Δt₅、Δt₆、Δt₇保持不变时，J₅、J₇与S_dec是单调递增关系，J₂与S_dec是单调递减关系。

f)v_e、S_dec、Δt₅、Δt₆、Δt₇保持不变时，J₅、J₇与v_max是单调递减关系，J₆与v_max是单调递增关系。

g)v_max、S_dec、Δt₅、Δt₆、Δt₇保持不变时，J₅、J₇与v_e是单调递减关系，J₆与v_e是单调递增关系。

h)v_max、S_dec、v_e保持不变时，J₅、J₇与Δt₅、Δt₆、Δt₇是单调递减关系。

根据单调性关系，可通过增大插补时间Δt₅、Δt₆、Δt₇，同时增大S_dec、减小v_max与v_e，

保证调整之后的J₁、J₂、J₃、J₅、J₆、J₇及中间过程能够达到的最大加速度a_max在约束范围之内，连同减速段的调整，使得总插补时间为插补周期的整数倍，在原理上是可行的。下面将对加速段和减速段时间调整策略及各运动参数调整可行性进行分析。

(1)时间调整策略如下

设调整之后的时间为t′_acc、t′_dec，有t′_acc＝Δt′₁+Δt′₂+Δt′₃，t′_dec＝Δt′₅+Δt′₆+Δt′₇ (式17)

为简化计算，总保证调整后的加加速段时间与减加速段时间相等，则各段时间分别为

Δt′₁＝Δt₁+k[t′_acc-(Δt₁+Δt₂+Δt₃)]，Δt′₃＝Δt′₁，Δt′₂＝t′_acc-Δt′₁-Δt′₃

Δt′₅＝Δt₅+k[t′_dec-(Δt₅+Δt₆+Δt₇)]，Δt′₇＝Δt′₅，Δt′₆＝t′_dec-Δt′₅-Δt′₇ (式18)

其中Δt′₁为补偿后的加加速段时间，以此类推，(式18)中，t′_acc统指补偿之后加速段的时间(包括原始加速段时间和补偿进的时间)，t′_dec统指补偿之后减速段的时间(包括原始减速段时间和补偿进的时间)，这是个原则性的公式，不同的时间分配情况都可根据本公式求得各段时间，例如，如果是步骤(1)中将Δt′全部补入到加速段的情况，则此时t′_acc＝Δt₁+Δt₂+Δt₃+Δt′，此时t′_dec未变、仍为原来的时间值。

由以上条件可计算得到时间调整之后的各段加速度，经化简可得

(1.1)J′₁需要满足的约束

J′₁对应加加速段，为保证满足最大加加速度限制和最大加速度限制，同时为缩短运动时间，应保证J₁′为正值。因此需要满足以下约束：

a)最大加加速度约束：0≤J′₁≤J_com (式25)

b)最大加速度约束：a′_max1＝J′₁Δt′₁≤a_com，即

(1.2)J′₂需要满足的约束

J′₂对应原匀加速段，经时间调整后，J′₂可能不为0，需要满足最大加加速度约束，即

-J_com≤J′₂≤J_com (式27)

(1.3)J′₃需要满足的约束

J′₃对应减加速段，为保证满足最大加加速度限制和最大加速度限制，同时为缩短运动时间，应保证J′₃为负值。因此需要满足以下约束：

a)最大加加速度约束：-J_com≤J′₃≤0 (式28)

b)最大加速度约束：a′_max3＝J′₃Δt′₃≥-a_com，即

(1.4)J′₅需要满足的约束

J′₅对应加减速段，为保证满足最大加加速度限制和最大加速度限制，同时为缩短运动时间，应保证J′₅为正值。因此需要满足以下约束：

a)最大加加速度约束：-J_com≤J′₅≤0 (式30)

b)最大加速度约束：a′_max5＝J′₅Δt′₅≥-a_com，即

(1.5)J′₆需要满足的约束

J′₆对应原匀减速段，经时间调整后，J′₆可能不为0，需要满足最大加加速度约束，即

-J_com≤J′₆≤J_com (式32)

(1.6)J′₇需要满足的约束

J′₇对应减减速段，为保证满足最大加加速度限制和最大加速度限制，同时为缩短运动时间，应保证J′₇为正值。因此需要满足以下约束：

a)最大加加速度约束：0≤J′₇≤J_com (式33)

b)最大加速度约束：a′_max7＝J′₇Δt′₇≤a_com，即

(2)关于位移调整范围计算

(2.1)位移S_acc调整范围计算

保持v_max、v_s不变，在一定的调整时间Δt′₁+Δt′₂+Δt′₃下，通过修改加速段位移S_acc能够实现对各段加加速度和最大加速度的调整，但为满足(式25)-(式29)所示的约束，调整后的S′_acc需满足一定的范围要求，如下

a)J′₁约束下的位移调整范围

由(式19)与(式25)左边，可得即

S_{accL 1}为加加速度J′₁约束下位移调整区间的下限，S′_acc为补偿后的加速段位移；

由(式19)与(式25)右边，可得即S_{accH 1}为加加速度J′₁约束下位移调整区间的下限；

由(式19)与(式26)，可得即S_{accH 2}为加速度a′_{max 1}约束下位移调整区间上限；a_{lim 1}为加速段加速度的约束值；

对S_{accH 1}与S_{accH 2}，设存在a_{lim 1}，有如下关系：a_{lim 1}＝min(a_com,J_comΔt′₁) (式38)

此时(式36)与(式37)可综合为：

S_{accH 3}为J′₁约束下位移调整区间的上限；

由(式35)-(式39)，可得在满足J′₁约束下的加速段位移调整范围为

S′_acc∈[S_{accL 1},S_{accH 3}] (式40)，且S_{accH 3}-S_{accL 1}＞0 (式41)，

同时，取值范围上限S_{accH 3}随Δt′₁+Δt′₂+Δt′₃的增大而增大。

b)J′₂约束下的位移调整范围

由(式20)与(式27)左边，可得即S_{accH 4}为加加速度J′₂约束下位移调整区间的上限；

由(式20)与(式27)右边，可得即S_{accL 4}为加加速度J′₂约束下位移调整区间的下限；

综合(式42)与(式43)，可得J′₂约束下的位移调整范围为

S′_acc∈[S_{accL 4},S_{accH 4}] (式44)，且S_{accH 4}-S_{accL 4}＞0 (式45)

同时，取值范围上限S_{accH 4}随Δt′₁+Δt′₂+Δt′₃的增大而增大。

c)J′₃约束下的位移调整范围

由(式21)与(式28)右边，有即

S_{accH 5}为加加速度J′₃约束下位移调整区间的上限；

由(式21)与(式28)左边，有即

S_{accL 2}为加加速度J′₃约束下位移调整区间的下限；

由(式21)与(式29)，有即

S_{accL 3}为加速度a′_max3约束下位移调整区间的下限；

对S_{accL 2}与S_{accL 3}，根据(式38)，(式47)与(式48)可综合为

S_{accL 5}为比较后的J′₃约束下位移调整区间的下限；

综合(式46)-(式49)，可得J′₃约束下的位移调整范围为

S′_acc∈[S_{accL 5},S_{accH 5}] (式50)，且S_{accH 5}-S_{accL 5}＞0 (式51)，

同时，取值范围上限S_{accH 5}随Δt′₁+Δt′₂+Δt′₃的增大而增大。

综合(式40)、(式44)、(式50)，可知在满足约束条件下的位移调整取值范围为S′_acc∈[S_{accL 1},S_{accH 3}]∩[S_{accL 4},S_{accH 4}]∩[S_{accL 5},S_{accH 5}] (式52)

(2.2)位移S_dec调整范围计算

保持v_max、v_e不变，在一定的调整时间Δt′₅+Δt′₆+Δt′₇下，通过修改加速段位移S_dec能够实现对各段加加速度和加速度的调整，但为满足(式30)-(式34)所示的约束，调整后的S′_dec需满足一定的范围要求，如下

a)J′₅约束下的位移调整范围

由(式22)与(式30)右边，可得即

S_{decH 1}为加加速度J′₅约束下的位移调整区间上限；

由(式22)与(式30)左边，可得即

S_{decL 1}为为加加速度J′₅约束下的位移调整区间下限；

由(式22)与(式31)，可得即

S_{decL 2}为加速度a′_{max 5}约束下的位移调整区间下限；

对S_{decL 1}与S_{decL 2}，设存在a_{lim 2}，a_{lim 2}为减速段的加速度约束值，有如下关系：a_{lim 2}＝min(a_com,J_comΔt′₅) (式56)

此时(式54)与(式55)可综合为

S_{decL 3}为比较后的J′₅约束下的位移调整区间下限；

由(式53)-(式57)，可得在满足J′₅约束下的加速段位移调整范围为

S′_dec∈[S_{decL 3},S_{decH 1}] (式58)，且S_{decH 1}-S_{decL 3}＞0 (式59)，

同时，取值范围上限S_{decH 1}随Δt′₅+Δt′₆+Δt′₇的增大而增大。

b)J′₆约束下的位移调整范围

由(式23)与(式32)左边，可得即

S_{decH 4}为加加速度J′₆约束下的位移调整区间上限；

由(式23)与(式32)右边，可得

S_{decL 4}为加加速度J′₆约束下的位移调整区间下限；

综合(式60)与(式61)，可得J′₆约束下的位移调整范围为

S′_dec∈[S_{decL 4},S_{decH 4}] (式62)，且S_{decH 4}-S_{decL 4}＞0 (式63)，

同时，取值范围上限S_{decH 4}随Δt′₅+Δt′₆+Δt′₇的增大而增大。

c)J′₇约束下的位移调整范围

由(式24)与(式33)左边，有即

S_{decL 5}为加加速度J′₇约束下的位移调整区间下限；

由(式24)与(式33)右边，有即

S_{decH 2}为加加速度J′₇约束下的位移调整区间上限；

由(式24)与(式34)，有即

S_{decH 3}为加速度a＇_{max 7}约束下的位移调整区间上限；

对S_{decH 2}与S_{decH 3}，根据(式56)，(式65)与(式66)可综合为

S_{decH 5}为比较后的J′₇约束下位移调整区间上限；

综合(式64)-(式67)，可得J′₇约束下的位移调整范围为

S′_dec∈[S_{decL 5},S_{decH 5}] (式68)，且S_{decH 5}-S_{decL 5}＞0 (式69)，

同时，取值范围上限S_{decH 5}随Δt′₅+Δt′₆+Δt′₇的增大而增大。

综合(式58)、(式62)、(式68)，可知在满足约束条件下的位移调整取值范围为

S′_dec∈[S_{decL 3},S_{decH 1}]∩[S_{decL 4},S_{decH 4}]∩[S_{decL 5},S_{decH 5}] (式70)

本部分中，acc指代加速段，L指代下限，H指代上限；

(2.3)位移调整范围存在性进行证明

需要对位移调整范围存在性进行证明，即证明(式52)和(式70)的取值区间非空，证明如下：

由于(Δt₁+Δt₂)a_lim1＝(v_max-v_s)且Δt′₁+Δt′₂＞Δt₁+Δt₂

因此S_{accH 3}-S_{accL 5}＞0 (式73)

综合(式71)-(式77)，可知(式52)所示的加速段位移调整区间总是存在且非空，同时，随Δt′₁+Δt′₂+Δt′₃的增大，位移取值区间的上限及区间范围也增大。位移的调整范围为S′_acc∈(S_acc,+∞)。

综合(式78)-(式83)，可知(式70)所示的减速段位移调整区间总是存在且非空，同时，随Δt′₅+Δt′₆+Δt′₇的增大，位移取值区间的上限及区间范围也增大。位移的调整范围为S′_dec∈(S_dec,+∞)。

(3)最大速度v_max调整范围计算

(3.1)加速段调整

保持S_acc、v_s不变，在一定的调整时间Δt′₁+Δt′₂+Δt′₃下，通过修改加速段位移v_max能够实现对各段加加速度和最大加速度的调整，但为满足(式25)-(式29)所示的约束，调整后的最大速度v′_max需满足一定的范围要求，如下

a)J′₁约束下的最大速度调整范围

由(式19)与(式25)左边，可得S_acc为当前步骤下的加速段的位移值，因为在最大速度调整策略中，位移是不用调整、不变的；即v_{max H 1}为加加速度J′₁约束下的最大速度调整上限；

由(式19)与(式25)右边，可得即

v_{max L 1}为加加速度J′₁约束下的最大速度调整下限；

由(式19)与(式26)，可得即

v_{max L 2}为最大加速度a′_{max 1}约束下的最大速度调整下限；

对v_{max L 1}与v_{max L 2}，根据(式38)，(式85)与(式86)可综合为

v_{max L 3}为J′₁约束下的最大速度调整下限；

由(式84)-(式87)，可得在满足J′₁约束下的加速段最大速度调整范围为

v′_max∈[v_{max L 3},v_{max H 1}] (式88)，且v_{max H 1}-v_{accL 3}＞0 (式89)，

同时，取值范围下限v_{max L 3}随Δt′₁+Δt′₂+Δt′₃的增大而减小。

b)J′₂约束下的最大速度调整范围

由(式20)与(式27)左边，可得即

v_{max L 4}为J′₂约束下的最大速度调整下限；

由(式20)与(式27)右边，可得即

v_{max H 4}为J′₂约束下的最大速度调整上限；

综合(式90)与(式91)，可得J′₂约束下的最大速度调整范围为

v′_max∈[v_{max L 4},v_{max H 4}] (式92)，且v_{max H 4}-v_{max L 4}＞0 (式93)，

同时，取值范围下限v_{max L 4}随Δt′₁+Δt′₂+Δt′₃的增大而减小。

c)J′₃约束下的速度调整范围

由(式21)与(式28)右边，有即v_{max L 5}为J′₃约束下的最大速度调整下限；

由(式21)与(式28)左边，有即

v_{max H 2}为加加速度J′₃约束下的最大速度调整上限；

由(式21)与(式29)，有即

v_{max H 3}为最大加速度a′_{max 3}约束下的最大速度调整上限；

对v_{max H 2}与v_{max H 3}，根据(式38)，(式95)与(式96)可综合为

v_{max H 5}为比较后的J′₃约束下的最大速度调整上限；

综合(式94)-(式97)，可得J′₃约束下的最大速度调整范围为

v′_max∈[v_{max L 5},v_{max H 5}] (式98)，且v_{max H 5}-v_{max L 5}＞0 (式99)，

同时，取值范围下v_{max L 5}随Δt′₁+Δt′₂+Δt′₃的增大而减小。

综合(式88)、(式92)、(式98)，可知在满足约束条件下的最大速度调整取值范围为v′_max∈[v_{max L 3},v_{max H 1}]∩[v_{max L 4},v_{max H 4}]∩[v_{max L 5},v_{max H 5}] (式100)

(3.2)减速段调整

保持S_dec、v_s不变，在一定的调整时间Δt′₅+Δt′₆+Δt′₇下，通过修改加速段位移v_max能够实现对各段加加速度和最大加速度的调整，但为满足(式30)-(式34)所示的约束，调整后的v′_max需满足一定的范围要求，如下

a)J′₅约束下的最大速度调整范围

由(式22)与(式30)右边，可得即

v_{max L 1}为加加速度J’₅约束下的最大速度调整区间下限；

由(式22)与(式30)左边，可得即

v_{max H 1}为加加速度J’₅约束下的最大速度调整区间上限；

由(式22)与(式31)，可得即

v_{max H 2}为加速度a’_{max 5}约束下的最大速度调整区间上限；

对v_{max H 1}与v_{max H 2}，根据(式56)，(式102)与(式103)可综合为

v_{max H 3}为比较后的J’₅约束下的最大速度调整区间上限；

由(式101)-(式104)，可得在满足J′₅约束下的加速段最大速度调整范围为

v′_max∈[v_{max L 1},v_{max H 3}] (式105)，且v_{max H 3}-v_{max L 1}＞0 (式106)，

同时，取值范围下限v_{max L 1}随Δt′₅+Δt′₆+Δt′₇的增大而减小。

b)J′₆约束下的最大速度调整范围

由(式23)与(式32)左边，可得即

v_{max L 4}为加加速度J’₆约束下的最大速度调整区间下限；

由(式23)与(式32)右边，可得

v_{max H 4}为加加速度J’₆约束下的最大速度调整区间上限；

综合(式107)与(式108)，可得J′₆约束下的最大速度调整范围为

v′_max∈[v_{max L 4},v_{max H 4}] (式109)，且v_{max H 4}-v_{max L 4}＞0 (式110)，

同时，取值范围下限v_{max L 4}随Δt′₅+Δt′₆+Δt′₇的增大而减小。

c)J′₇约束下的最大速度调整范围

由(式24)与(式33)左边，可得即

v_{max H 5}为加加速度J’₇约束下的最大速度调整区间上限；

由(式24)与(式33)右边，可得即

v_{maxL 2}为加加速度J’₇约束下的最大速度调整区间下限；

由(式24)与(式34)，可得即

v_{max L 3}为加速度a’_{max 7}约束下的最大速度调整区间下限；

对v_{max L 2}与v_{max L 3}，根据(式56)，(式112)与(式113)可综合为

v_{max L 5}为比较后的J’₇约束下的最大速度调整区间下限；

综合(式111)-(式114)，可得J′₇约束下的最大速度调整范围为

v′_max∈[v_{max L 5},v_{max H 5}] (式115)，且v_{max H 5}-v_{max L 5}＞0 (式116)，

同时，取值范围下v_{max L 5}随Δt′₅+Δt′₆+Δt′₇的增大而减小。

综合(式105)、(式109)、(式115)，可知在满足约束条件下的最大速度调整取值范围为v′_max∈[v_{max L 1},v_{max H 3}]∩[v_{max L 4},v_{max H 4}]∩[v_{max L 5},v_{max H 5}] (式117)。

本部分中，L指代下限、H指代上限。

(3.3)最大速度调整范围存在性进行证明

a)需要对最大速度调整范围存在性进行证明，即证明(式100)的取值区间非空，证明如下：

由于且Δt′₁+Δt′₂＞Δt₁+Δt₂，因此有v_{max H 5}-v_{maxL 3}＞0 (式119)，

当满足有v_{max H 1}-v_{max L 5}≥0 (式122)，

对(式124)、(式125)，当(式121)满足时，有

v_{max H 4}-v_{max L 5}＞0 (式127)，v_{max H 1}-v_{max L 4}＞0 (式128)，

综合(式118)-(式128)，可知，当(式121)满足时，(式100)所示的最大速度调整区间总是存在且非空。

极限情况下，当时，v′_max仅能取一个值，即v′_max＝v_s (式129)，此时，最大速度的调整范围为v′_max∈[v_s,v_max]。

b)减速段证明同理可得，证明如下：

条件：S_dec≥(2Δt′₅+Δt′₆)v_e (式136)

基于以上，本实施例提供一种S曲线加减速的圆整误差补偿方法，曲线按时间顺序分为加速段acc、匀速段L₄、减速段dec，加速段包括加加速段L₁、匀加速段L₂和减加速段L₃，减速段包括加减速段L₅、匀减速段L₆和减减速段L₇；设S曲线规划后的总插补时间为t_whole1＝t_acc1+t_con+t_dec1＝m₁T_s+Δt，其中t_acc1为加速段时间，t_con为匀速段时间，t_dec1为减速段时间，T_s为插补周期，m₁为非负整数，Δt∈[0,T_s)；

若Δt＝0，则无需进行插补时间调整；若Δt∈(0,T_s)，则需进行调整，以使总插补时间为插补周期的整数倍；进入如下判断：

(1)当加速段、匀速段、减速段同时存在，选择位移调整方式；保持v_max与v_s、v_e不变，根据前述分析可知，对于加速段与减速段，在保证加加速度及最大加速度约束条件下，对t_acc1与t_dec1只能进行增大调整，对应的加速段和减速段的运动位移也会相应的增加，产生一个位移增量区间同时为保证总位移不变，需要减小t_con，对应减小的位移量ΔS_con，应当在保证总插补时间为插补周期的整数倍的同时，使得ΔS_con位于内部；则表示调整完成；

步骤(1)中，设Δt′为加速段和减速段增加的时间之和，将Δt′分配到加速段和减速段中，不同的分配方式，会有不同的位移增量区间的最大与最小对应着Δt′分配的最优化问题。考虑到计算的复杂性及算法执行的实时性问题，采用如下策略：假设将Δt′全部补偿到加速段，获得新的加速段的时间，根据(式18)、(式35)--(式52)，获得位移增量区间假设将Δt′全部补偿到减速段，获得新的减速段的时间，根据(式18)、(式53)--(式70)，获得位移增量区间假设将Δt′平均分配到加速段与减速段，获得新的加速段和减速段的时间，根据(式18)、(式35)--(式70)分别获得加速段和减速段的位移增量区间为[ΔS′_accL,ΔS′_accH]、[ΔS′_decL,ΔS′_decH]，则位移增量区间为

认为最大范围区间与最小范围区间就存在中，同时可获得范围：

当时，若ΔS_con属于中的任何一个，则完成时间调整，若ΔS_con不属于 中的任何一个，根据单调性关系，则可通过二分法调整Δt′的时间分配使得ΔS_con落入调整范围区间内。因此当时，总能存在合理的Δt′分配方法，使得总插补周期为整数倍，达到误差补偿的目的。

初始化n₁＝n₂＝1，t′_con＝t_con，ΔS_con＝0，n₁、n₂为插补周期的增量倍数，t′_con为调整后的匀速段时间，按如下过程进行位移与时间调整：

Step1-1：增加加速段和减速段总时间，时间增量为Δt′＝T_s-Δt，获得位移增量范围ΔS_L＝min(ΔS′_accL+ΔS′_decL，ΔS_accL，ΔS_decL)、ΔS_H＝max(ΔS′_accH+ΔS′_decH，ΔS_accH，ΔS_decH)；判断是否存在ΔS_L≤ΔS_con≤ΔS_H：若存在，则进入Step1-6；若不存在，则进入Step1-2；

Step1-2：缩短匀速段时间，将t′_con更新为t′_con-n₁T_s(在程序架构里，可写成等式：t′_con＝t′_con-n₁T_s，等号两边的两个t′_con指代相同，目的是给t′_con赋值，t′_con只包含一个最新的值，这样写也是为了循环调用方便。如果等会左右改成两个变量，第二次减时就不方便说明关系了)，则匀速段位移变化量为ΔS_con＝n₁T_sv_max；比较ΔS_con与原匀速段位移S_con的关系：若ΔS_con＜S_con，则进入Step1-3；若ΔS_con≥S_con，则使ΔS_con＝S_con，进入Step1-4；

Step1-3：比较ΔS_con与的关系：若ΔS_con＜ΔS_L，则将n₁更新为n₁+1(同上理，在程序架构里，可写成等式：n₁＝n₁+1，等号左右两边的两个n₁指代相同，写成一样是与算法结构保持一致)，则返回Step1-2；若则进入Step1-6；若ΔS_con＞ΔS_H，则进入Step1-5；

Step1-4：比较ΔS_con与的关系：

若ΔS_con＜ΔS_L，则说明位移调整无法满足调整要求，需将匀速段位移补偿到加速段和减速段(此时的匀速段位移＝匀速段减小的位移量ΔS_con，因为此时是将匀速段所有的位移都补入到其他段中了)，即：根据单调性关系，在(0,Δt′)内以二分法方式减小时间t_con并重新计算直到在(0,Δt′)内必有解。然后按照ΔS_con所处的区间类型进行判断并将位移和时间进行补偿，判断ΔS_con处于或或则将匀速段减小的位移量ΔS_con全部补偿到加速段，或全部补偿到减速段，或平均补偿到加速段和减速段，(所处的区间类型是指中某一个，因为包含这三种情况)，然后退出位移调整，然后进行最大速度调整，进入步骤(2)；

若则进入Step1-6；若ΔS_con＞ΔS_H，则进入Step1-5；

Step1-5：增加加速段和减速段总时间，将增量Δt′更新为Δt′+n₂T_s，重新更新位移增量区间比较ΔS_con与的关系：若ΔS_con＜ΔS_L，则将n₁更新为n₁+1，进入Step1-2；若则进入Step1-6；若ΔS_con＞ΔS_H，则将n₂更新为n₂+1，返回Step1-5；

Step1-6：此时说明存在Δt′满足调整要求，有匀速段时间为t′_con，进行如下判断：

若则可根据ΔS_con所属的具体区间类型计算出调整后的加速段时间t′_acc1和调整后的减速段时间t′_dec1，当存在多区间交集时可任选一个区间类型；具体的，根据判断ΔS_con是属于还是还是来增加相应的时间，如果ΔS_con属于增量Δt′全部加到加速段的原始时间t_acc1上，得到调整后的加速段时间t′_acc1；如果ΔS_con属于增量Δt′全部加到减速段的原始时间t_dec1上，得到调整后的减速段时间t′_dec1，如果ΔS_con属于0.5倍的增量Δt′加到加速段，0.5倍的增量Δt′加到减速段，得到调整后的加速段时间t′_acc1和调整后的减速段时间t′_dec1；

若则需要对Δt′的分配做出调整；调整方法是根据与ΔS_con相邻的两个位移增量区间类型和单调性关系，使用二分法寻找新的Δt′的分配方案，求得新的分配给加速段的时间增量Δt′_acc1、新的分配给减速段的时间增量Δt′_dec1；得到调整后的加速段时间为t′_acc1＝t_acc1+Δt′_acc1，调整后的减速段时间为t′_dec1＝t_dec1+Δt′_dec1，其中Δt′＝Δt′_acc1+Δt′_dec1；

利用Step1-6调整后的各段时间t′_acc1、t′_dec1和t′_con，求出加速段、减速段的加加速度，再利用加加速度与加速度、速度、位移的对应关系求得各段的加速度、速度、位移参数，代回(式19)-(式24)、(式1)-(式10)，即可计算加速段、减速段各段时间及各运动参数，运动参数包括加加速度、加速度、速度、位移，完成圆整误差的补偿。

(2)当不存在匀速段时且同时存在加速段和减速段时，为保证首末点速度为规划要求的速度，选择最大速度调整方式：此时需要保持加速段和减速段的位移及v_s、v_e不变，通过减小最大速度v_max、增大加速段的插补时间t_acc1与减速段的插补时间t_dec1，以实现总插补时间为插补周期的整数倍，当调整时间后的加速段与减速段的最大速度调整范围有交集时，即表示调整完成。

步骤(2)中，在进行最大速度调整之前可能已经进行了位移调整，因此重新假设当前的插补总时间为t_whole2＝t_acc2+t_dec2＝m₂T_s+Δt″，其中t_acc2为当前加速段时间，t_dec2为当前减速段时间，m₂为非负整数，Δt″∈[0,T_s)；Δt″为本步骤中的非完整周期，Δt″包括两种情况：一种是经过了步骤(1)位移调整后余下的非完整周期，另一种是不存在匀速段直接进入本步骤时的非完整周期；需要补偿进的时间为Δt″′，Δt″′＝T_s-Δt″，根据需要补偿进的时间，再根据(式18)、(式84)--(式100)求得加速段时间调整后的最大速度取值范围为[v_aL,v_aH]，v_aL为加速段最大速度调整区间的下限、v_aH为加速段最大速度调整区间的上限，根据需要补偿进的时间，利用(式18)、(式101)--(式117)求得减速段时间调整后的最大速度取值范围为[v_dL,v_dH]，v_dL为加速段最大速度调整区间的下限、v_dH为加速段最大速度调整区间的上限，下面以v_s≥v_e为例给出调整策略，

Step2-1：根据(式18)、(式121)计算加速段最大调整时间Δt_{a max}，根据(式18)、(式136)计算减速段最大调整时间为Δt_{d max}，有Δt_{a max}≥Δt_{d max}；

Step2-2：Δt″′＝T_s-Δt″，比较Δt″′与Δt_{a max}的关系：若Δt″′≥Δt_{a max}，则进入Step2-4；若Δt″′＜Δt_{a max}，则进入Step2-3；

Step2-3：根据单调性关系，使用二分法调整Δt″′在加速段和减速段内的分配量，求得加速段、减速段的时间调整后的最大速度v′_max的取值范围区间，直到加速段与减速段的最大速度取值范围有交集，此时必有解，可取交集内的任意速度为最大速度，完成调整并退出；

Step2-4：Δt″′₂＝Δt″′-Δt_{a max}，Δt″′₂为在保证加速段完成最大速度调整前提下的剩余非完整时间，比较Δt″′₂与Δt_{d max}的关系：若Δt″′₂≤Δt_{d max}，则进入Step2-5；若Δt″′₂＞Δt_{d max}，则表示在运动参数约束下，无法通过调整最大速度以达到总插补时间是插补周期整数倍的目的，即无解，退出调整；

Step2-5：将Δt_{a max}补偿给加速段，此时处于临界情况，加速段的最大速度取值仅为v_s；将Δt″′₂＝Δt″′-Δt_{a max}补偿给减速段，根据(式18)、(式101)--(式117)可获得减速段最大速度的调整范围[v_dL,v_dH]，比较v_s与[v_dL,v_dH]：

若v_s＜v_dL，则此时必有解，可根据单调性关系，同Step2-3中的调整方法，使得可取交集内的任意速度为最大速度，完成调整并退出；

若v_s∈[v_dL,v_dH]，则Δt_amax与Δt″′₂的分配方式恰好能满足要求，即可取最大速度为v_s，完成调整并退出；

若v_s＞v_dH，此时可能无解，但考虑到v_dH与减速段插补时间的单调性不确定，可进行一次计算尝试，进入Step2-6；

Step2-6：将补偿给减速段、利用(式18)、(式101)--(式117)获得[v_dL,v_dH]，将补偿给加速段、利用(式18)、(式84)--(式100)得到[v_aL,v_aH]，比较[v_dL,v_dH]与[v_aL,v_aH]：

若则表示有解，可取交集内的任意值作为最大速度，完成调整并退出；

若则考虑到计算的复杂度及算法执行的实时性，停止调整并退出；

步骤(2)中，退出最大速度调整后，若有解，将调整后的加速段时间、调整后的减速段时间及调整后的最大速度代回(式19)-(式24)、(式1)-(式10)，求出加速段、减速段的加加速度。

本实施例中，取S＝400mm，v_s＝30mm/s，v_e＝10mm/s，v_com＝400mm/s，a_com＝1000mm/s^2，J_com＝10000mm/s^3，T_s＝0.001s

首先根据S曲线加减速算法规划出原始的7段时间和位移，其中：

t1＝0.1s，t2＝0.27s，t3＝0.1s，t4＝0.49625s，t5＝0.1s，t6＝0.29s，t7＝0.1s；

s1＝4.66667mm；s2＝58.05mm；s3＝38.3333mm；s4＝198.5mm，s5＝38.3333mm；s6＝59.45mm；s7＝2.66667mm；

不难看出，总的规划时间T＝t1+t2+t3+t4+t5+t6+t7＝1.45625s，不是插补周期的整数倍。

若设T＝t_acc1+t_con+t_dec1＝m₁T_s+Δt，其中m₁为非负整数，则Δt＝0.00025s，为了保证总的规划时间T是插补周期的整数倍，需将Δt补足为一个插补周期，则加速段和减速段需要增加的时间之和为Δt′＝0.00075s。因为本例中，加速段，匀速段和减速段同时存在，故选择位移调整方式。根据本发明详细的步骤(1)进行计算。得新的规划时间和位移为：

t1＝0.100583s，t2＝0.270583s，t3＝0.100583s，t4＝0.49525s，t5＝0.1s，t6＝0.29s，t7＝0.1s；

s1＝4.69974mm；s2＝58.1964mm；s3＝38.5538mm；s4＝198.1mm，s5＝38.3333mm；s6＝59.45mm；s7＝2.66667mm；

T＝t1+t2+t3+t4+t5+t6+t7＝1.457s，S＝s1+s2+s3+s4+s5+s6+s7＝400mm

故经过该算法调整之后，总规划时间是插补周期整数倍，无圆整误差，且位移满足要求。

同时，可以计算出：v_max＝400mm/s；

a1＝997.671mm/s^2，a2＝996.043mm/s^2，a3＝0mm/s^2，a5＝-1000mm/s^2，a6＝-1000mm/s^2，a7＝0mm/s^2；

J1＝9918.85mm/s^3，J2＝-6.01538mm/s^3，J3＝-9902.66mm/s^3，J5＝-10000mm/s^3，J6＝0mm/s^3，J7＝10000mm/s^3；

加加速度、加速度和速度的曲线分别绘制如图1-3所示。

综合上述数据和曲线，可以得出：经过本发明调整，能保证总规划时间是插补周期的整数倍，位移满足要求，且速度、加速度和加加速度满足约束、均未超过初始设定的值。

实施例2：

一种S曲线加减速的圆整误差补偿方法，方法步骤如实施例1所述，所不同的是，本例中，设：S＝200mm，v_s＝30mm/s，v_e＝10mm/s，v_com＝400mm/s，a_com＝1000mm/s^2，J_com＝10000mm/s^3，T_s＝0.001s

首先根据S曲线加减速算法规划出原始的7段时间和位移，其中：

t1＝0.1s，t2＝0.26833s，t3＝0.1s，t4＝0s，t5＝0.1s，t6＝0.28833s，t7＝0.1s；

s1＝4.66667mm；s2＝57.467mm；s3＝38.1664mm；s4＝0mm，s5＝38.1664mm；s6＝58.867mm；s7＝2.66667mm；

不难看出，总的规划时间T＝t1+t2+t3+t4+t5+t6+t7＝0.95666s，不是插补周期的整数倍。

若设T＝t_acc1+t_con+t_dec1＝m₁T_s+Δt，其中m1为非负整数，则Δt＝0.00066s，为了保证总的规划时间T是插补周期的整数倍，需将Δt补足为一个插补周期，则加速段和减速段需要增加的时间之和为Δt′＝0.00034s。因为本例中，不存在匀速段，故选择最大速度调整方式。根据本发明详细的步骤(2)进行计算。

得新的规划时间和位移为：

t1＝0.100057s，t2＝0.268387s，t3＝0.100057s，t4＝0s，t5＝0.100057s，t6＝0.288387s；t7＝0.100057s；

s1＝4.6693mm；s2＝57.4602mm；s3＝38.1706mm；s4＝0mm，s5＝38.1706mm；s6＝58.8610mm；s7＝2.6684mm；

T＝t1+t2+t3+t4+t5+t6+t7＝0.957s，S＝s1+s2+s3+s4+s5+s6+s7＝200mm

同时，可以计算出：v_max＝398.1490mm/s；

a1＝999.413mm/s^2，a2＝998.989mm/s^2，a3＝0mm/s^2，a5＝-998.938mm/s^2，a6＝-999.546mm/s^2，a7＝0mm/s^2；

J1＝9988.48mm/s^3，J2＝-1.58147mm/s^3，J3＝-9984.24mm/s^3，J5＝-9983.73mm/s^3，J6＝-2.10864mm/s^3，J7＝9989.81mm/s^3；

加加速度、加速度和速度的曲线分别绘制如图4-图6所示。

综合上述数据和曲线，可以得出：经过本发明调整，能保证总规划时间是插补周期的整数倍，位移满足要求，且速度，加速度和加加速度满足约束、均未超过初始设定的值。

实施例3：

一种S曲线加减速的圆整误差补偿方法，方法步骤如实施例1所述，所不同的是本例中，设：S＝201.6mm，v_s＝30mm/s，v_e＝10mm/s，v_com＝400mm/s，a_com＝1000mm/s^2，J_com＝10000mm/s^3，T_s＝0.001s

首先根据S曲线加减速算法规划出原始的7段时间和位移，其中：

t1＝0.1s，t2＝0.27s，t3＝0.1s，t4＝0.00025s，t5＝0.1s，t6＝0.29s，t7＝0.1s；

s1＝4.66667mm；s2＝58.05mm；s3＝38.3333mm；s4＝0.1mm，s5＝38.3333mm；s6＝59.45mm；s7＝2.66667mm；

不难看出，总的规划时间T＝t1+t2+t3+t4+t5+t6+t7＝0.96025s，不是插补周期的整数倍。

若设T＝t_acc1+t_con+t_dec1＝m₁T_s+Δt，其中m1为非负整数，则Δt＝0.00025s，为了保证总的规划时间T是插补周期的整数倍，需将Δt补足为一个插补周期，则加速段和减速段需要增加的时间之和为Δt′＝0.00075s。因为本例中，存在匀速段但是匀速段不足一个插补周期，位移调整不能消除圆整误差，故先采用位移调整方式，将匀速段全部补偿到加速段和减速段，再采用最大速度调整方式，根据本发明详细的步骤(3)进行计算。

得新的规划时间和位移为：

t1＝0.10025s，t2＝0.27025s，t3＝0.10025s，t4＝0s，t5＝0.100083s，t6＝0.290083s，t7＝0.100083s；

s1＝4.6795mm；s2＝58.0719mm；s3＝38.3986mm；s4＝0mm，s5＝38.3366mm；s6＝59.4439mm；s7＝2.6695mm；

T＝t1+t2+t3+t4+t5+t6+t7＝0.961s，S＝s1+s2+s3+s4+s5+s6+s7＝201.6mm

同时，可以计算出：v_max＝399.6944mm/s；

a1＝998.193mm/s^2，a2＝997.458mm/s^2，a3＝0mm/s^2，a5＝-998.047mm/s^2，a6＝-999.532mm/s^2，a7＝0mm/s^2；

J1＝9957.04mm/s^3，J2＝-2.72202mm/s^3，J3＝-9949.7mm/s^3，J5＝-9972.16mm/s^3，J6＝-5.11947mm/s^3，J7＝9987mm/s^3；

加加速度、加速度和速度的曲线分别绘制如图7-图9所示：

综合上述数据和曲线，可以得出：经过本发明调整，能保证总规划时间是插补周期的整数倍，位移满足要求，且速度，加速度和加加速度满足约束、均未超过初始设定的值。

Claims (6)

1.一种S曲线加减速的圆整误差补偿方法，其特征在于，曲线按时间顺序分为加速段acc、匀速段L₄、减速段dec，加速段包括加加速段L₁、匀加速段L₂和减加速段L₃，减速段包括加减速段L₅、匀减速段L₆和减减速段L₇；设S曲线规划后的总插补时间为t_whole1＝t_acc1+t_con+t_dec1＝m₁T_s+Δt，其中t_acc1为加速段时间，t_con为匀速段时间，t_dec1为减速段时间，T_s为插补周期，m₁为非负整数，Δt∈[0,T_s)，Δt为非完整周期；定义v_s为起点速度，v_e为终点速度；

若Δt＝0，则无需进行插补时间调整；若Δt∈(0,T_s)，则需进行调整，进入如下判断：

(1)当加速段、匀速段、减速段同时存在，选择位移调整方式；

保持v_max与v_s、v_e不变，对t_acc1与t_dec1进行增大调整，对应的加速段和减速段的运动位移也相应的增加，产生一个位移增量区间同时减小t_con，对应减小的位移量ΔS_con，在保证总插补时间为插补周期的整数倍的同时，使得ΔS_con位于内部，则表示调整完成；

(2)当不存在匀速段时且同时存在加速段和减速段时，选择最大速度调整方式：保持加速段和减速段的位移及v_s、v_e不变，通过减小最大速度v_max、增大加速段的插补时间t_acc1与减速段的插补时间t_dec1，以实现总插补时间为插补周期的整数倍，当调整时间后的加速段与减速段的最大速度调整范围有交集时，即表示调整完成。

2.根据权利要求1所述的S曲线加减速的圆整误差补偿方法，其特征在于，步骤(1)中，

设Δt′为加速段和减速段增加的时间之和，将Δt′分配到加速段和减速段中，不同的分配方式，会有不同的位移增量区间假设将Δt′全部补偿到加速段，获得新的加速段的时间，求得位移增量区间ΔS_accL为加速段增量区间的下限、ΔS_accH为加速段增量区间的上限；假设将Δt′全部补偿到减速段，获得新的减速段的时间，获得位移增量区间ΔS_decL为减速段增量区间的下限、ΔS_decH为减速段增量区间的上限；假设将Δt′平均分配到加速段与减速段，获得新的加速段和减速段的时间，分别获得加速段和减速段的位移增量区间为[ΔS′_accL,ΔS′_accH]、[ΔS′_decL,ΔS′_decH]，则位移增量区间为ΔS′_accL为均分加速段增量区间下限、ΔS′_accH为均分加速段增量区间上限、ΔS′_decL为均分减速段增量区间下限、ΔS′_decH为均分减速段增量区间上限；

获得范围：

初始化n₁＝n₂＝1，t′_con＝t_con，ΔS_con＝0，n₁、n₂为插补周期的增量倍数，t′_con为调整后的匀速段时间，按如下过程进行位移与时间调整：

Step1-1：增加加速段和减速段总时间，时间增量为Δt′＝T_s-Δt，获得位移增量区间ΔS_L、ΔS_H分别为位移增量区间的下限、上限，ΔS_L＝min(ΔS′_accL+ΔS′_decL，ΔS_accL，ΔS_decL)、ΔS_H＝max(ΔS′_accH+ΔS′_decH，ΔS_accH，ΔS_decH)；判断是否存在ΔS_L≤ΔS_con≤ΔS_H：若存在，则进入Step1-6；若不存在，则进入Step1-2；

Step1-2：缩短匀速段时间，将t′_con更新为t′_con-n₁T_s，则匀速段位移变化量为ΔS_con＝n₁T_sv_max；比较ΔS_con与原匀速段位移S_con的关系：若ΔS_con＜S_con，则进入Step1-3；若ΔS_con≥S_con，则使ΔS_con＝S_con，进入Step1-4；

Step1-3：比较ΔS_con与的关系：若ΔS_con＜ΔS_L，则将n₁更新为n₁+1，则返回Step1-2；若则进入Step1-6；若ΔS_con＞ΔS_H，则进入Step1-5；

Step1-4：比较ΔS_con与的关系：

若ΔS_con＜ΔS_L，则说明位移调整无法满足调整要求，需将匀速段位移补偿到加速段和减速段，然后进行最大速度调整，进入步骤(2)；

若则进入Step1-6；若ΔS_con＞ΔS_H，则进入Step1-5；

Step1-5：增加加速段和减速段总时间，将增量Δt′更新为Δt′+n₂T_s，重新更新位移增量区间比较ΔS_con与的关系：若ΔS_con＜ΔS_L，则将n₁更新为n₁+1，进入Step1-2；若则进入Step1-6；若ΔS_con＞ΔS_H，则将n₂更新为n₂+1，返回Step1-5；

Step1-6：此时说明存在Δt′满足调整要求，有调整后的匀速段时间为t′_con，进行如下判断：

若则可根据ΔS_con所属的具体区间类型计算出调整后的加速段时间t′_acc1和调整后的减速段时间t′_dec1，当存在多区间交集时任选一个区间类型；具体的，根据判断ΔS_con是属于还是还是来增加相应的时间，如果ΔS_con属于增量Δt′全部加到加速段的原始时间t_acc1上，得到调整后的加速段时间t′_acc1；如果ΔS_con属于增量Δt′全部加到减速段的原始时间t_dec1上，得到调整后的减速段时间t′_dec1，如果ΔS_con属于0.5倍的增量Δt′加到加速段，0.5倍的增量Δt′加到减速段，得到调整后的加速段时间t′_acc1和调整后的减速段时间t′_dec1；

若则需要对Δt′的分配做出调整；调整方法是根据与ΔS_con相邻的两个位移增量区间类型和单调性关系，使用二分法寻找新的Δt′的分配方案，求得新的分配给加速段的时间增量Δt′_accl、新的分配给减速段的时间增量Δt′_decl；得到调整后的加速段时间为t′_accl＝t_accl+Δt′_accl，调整后的减速段时间为t′_decl＝t_decl+Δt′_decl，其中Δt′＝Δt′_accl+Δt′_decl；

利用调整后的各段时间t′_acc1、t′_dec1和t′_con，即计算加速段、减速段各段时间及各运动参数，运动参数包括加加速度、加速度、速度、位移，完成圆整误差的补偿。

3.根据权利要求2所述的S曲线加减速的圆整误差补偿方法，其特征在于，步骤(2)中，当不存在匀速段时且同时存在加速段和减速段时，

重新假设当前的插补总时间为t_whole2＝t_acc2+t_dec2＝m₂T_s+Δt″，其中t_acc2为当前加速段时间，t_dec2为当前减速段时间，m₂为非负整数，Δt″∈[0,T_s)；Δt″为本步骤中的非完整周期，Δt″包括两种情况：一种是经过了步骤(1)位移调整后余下的非完整周期，另一种是不存在匀速段直接进入本步骤时的非完整周期；需要补偿进的时间为Δt″′，Δt″′＝T_s-Δt″，根据需要补偿进的时间，求得加速段时间调整后的最大速度取值范围为[v_aL,v_aH]，v_aL为加速段最大速度调整区间的下限、v_aH为加速段最大速度调整区间的上限，根据需要补偿进的时间，求得减速段时间调整后的最大速度取值范围为[v_dL,v_dH]，v_dL为减速段最大速度调整区间的下限、v_dH为减速段最大速度调整区间的上限，

Step2-1：计算加速段最大调整时间Δt_amax，计算减速段最大调整时间为Δt_dmax，有Δt_amax≥Δt_dmax；

Step2-2：Δt″′＝T_s-Δt″，比较Δt″′与Δt_amax的关系：若Δt″′≥Δt_amax，则进入Step2-4；若Δt″′＜Δt_amax，则进入Step2-3；

Step2-3：使用二分法调整Δt″′在加速段和减速段内的分配量，求得加速段、减速段的时间调整后的最大速度v′_max的取值范围区间，直到加速段与减速段的最大速度取值范围有交集，取交集内的任意速度为最大速度，完成调整并退出；

Step2-4：Δt″′₂＝Δt″′-Δt_amax，Δt″′₂为在保证加速段完成最大速度调整前提下的剩余非完整时间，比较Δt″′₂与Δt_dmax的关系：若Δt″′₂≤Δt_dmax，则进入Step2-5；若Δt″′₂＞Δt_dmax，无解，退出调整；

Step2-5：将Δt_amax补偿给加速段，加速段的最大速度取值仅为v_s；将Δt″′₂＝Δt″′-Δt_amax补偿给减速段，获得减速段最大速度的调整范围[v_dL,v_dH]，比较v_s与[v_dL,v_dH]：

若v_s＜v_dL，同Step2-3中的二分法调整方法，使得取交集内的任意速度为最大速度，完成调整并退出；

若v_s∈[v_dL,v_dH]，取最大速度为v_s，完成调整并退出；

若v_s＞v_dH，进入Step2-6；

Step2-6：将补偿给减速段获得[v_dL,v_dH]，将补偿给加速段得到[v_aL,v_aH]，比较[v_dL,v_dH]与[v_aL,v_aH]：

若取交集内的任意值作为最大速度，完成调整并退出；

若停止调整并退出。

4.根据权利要求2所述的S曲线加减速的圆整误差补偿方法，其特征在于，Step1-4中，匀速段位移补偿到加速段与减速段的具体方法是：在(0,Δt′)内以二分法方式减小时间t_con并重新计算直到然后按照ΔS_con所处的区间类型进行判断并将位移和时间进行补偿，判断ΔS_con处于或或则将匀速段减小的位移量ΔS_con全部补偿到加速段，或全部补偿到减速段，或平均补偿到加速段和减速段，然后退出位移调整，进入步骤(2)。

5.根据权利要求2所述的S曲线加减速的圆整误差补偿方法，其特征在于，利用Step1-6调整后的各段时间，求出加速段、减速段的加加速度，再利用加加速度与加速度、速度、位移的对应关系求得各段的加速度、速度、位移参数，完成圆整误差的补偿。

6.根据权利要求3所述的S曲线加减速的圆整误差补偿方法，其特征在于，步骤(2)中，退出最大速度调整后，若有解，利用调整后的加速段时间、调整后的减速段时间及调整后的最大速度求出加速段、减速段的加加速度。