支持运动中改变目标位置的曲线规划方法及系统转让专利
申请号 : CN202210964346.8
文献号 : CN115032949B
文献日 : 2022-11-04
发明人 : 陈晓阳 , 孙海星 , 郭恩政
申请人 : 合肥安迅精密技术有限公司
摘要 :
本发明涉及贴片机控制技术领域，具体公开了一种支持运动中改变目标位置的曲线规划方法及系统，所述方法包括以下步骤：设置参数；利用参数计算下一时刻开始减速的距离；根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。本发明在T型曲线规划的基础上实时更新的T型曲线规划，运动过程中支持变更目标位置、最大速度、加速度和减速度，能立刻响应参数变化，实时更新曲线规划，且曲线规划安全，不会超过设置的参数运行，不会超出设定的最大位置，既保证了到位精确，又缩短了曲线规划时间。
权利要求 :
1.一种支持运动中改变目标位置的曲线规划方法，其特征在于，包括以下步骤：设置参数；
利用参数计算下一时刻开始减速的距离，计算公式如下：
若当前时刻处于加速阶段或等待阶段：
若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
根据参数确定当前时刻是否满足曲线规划完成的退出条件，具体如下：若当前时刻处于减速阶段的正常减速停止阶段，下一时刻的目标位置即为设定的新的目标位置，或者若满足 ，则曲线规划完成，满足退出条件，其中，DisMove为当前时刻位置与目标位置的距离差，AccSet为当前时刻加速度，DecSet为当前时刻减速度，TGain为计算频率，VeloCurt为当前时刻速度，abs为绝对值函数；若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段，具体为：若当前时刻处于seg=3或seg=5，则下一时刻维持当前时刻所处阶段；若当前时刻处于seg=1、seg=2或seg=4中的一种，则包括以下情况：当DisMove和VeloCurt反向时，下一时刻处于seg=4；
当新的目标位置和目标位置同向且abs(DisMove)>abs(DisDec1)时，若abs(VeloCurt)
根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
2.根据权利要求1所述的方法，其特征在于，所述根据参数确定当前时刻是否满足曲线规划完成的退出条件，若满足，则退出曲线规划，下一时刻处于下一曲线规划的等待阶段seg=0。
3.根据权利要求2所述的方法，其特征在于，所述根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度具体为：若下一时刻处于seg=0，则
，退出曲线规划；
若下一时刻处于seg=1，则
；
若下一时刻处于seg=2，则
；
若下一时刻处于seg=3，则
；
若下一时刻处于seg=4或seg=5，则
；
若下一时刻处于seg=6，则
其中，AccDirt为下一时刻的加速度，VeloNext为下一时刻的速度，PosNext为下一时刻的位置，PosEnd表示最终规划位置，即退出曲线规划，PosCurt为当前时刻位置。
4.根据权利要求1‑3中任意一项所述的方法，所述最小减速距离的计算公式如下：其中，DisDec2为最小减速距离，VeloCurt为当前时刻速度，DecSet为当前时刻至目标位置的减速度。
5.一种支持运动中改变目标位置的曲线规划系统，其特征在于，包括：设置模块，用于设置参数；
计算模块，用于利用参数计算下一时刻开始减速的距离，计算公式如下：若当前时刻处于加速阶段或等待阶段：
若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
下一时刻判断模块，用于根据参数确定当前时刻是否满足曲线规划完成的退出条件，具体如下：若当前时刻处于减速阶段的正常减速停止阶段，下一时刻的目标位置即为设定的新的目标位置，或者若满足 ，则曲线规划完成，满足退出条件，其中，DisMove为当前时刻位置与目标位置的距离差，AccSet为当前时刻加速度，DecSet为当前时刻减速度，TGain为计算频率，VeloCurt为当前时刻速度，abs为绝对值函数；若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段，具体为：若当前时刻处于seg=3或seg=5，则下一时刻维持当前时刻所处阶段；
若当前时刻处于seg=1、seg=2或seg=4中的一种，则包括以下情况：当DisMove和VeloCurt反向时，下一时刻处于seg=4；当新的目标位置和目标位置同向且abs(DisMove)>abs(DisDec1)时，若abs(VeloCurt)
规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
6.根据权利要求5所述的系统，其特征在于，所述规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度具体为：若下一时刻处于seg=0，则
，退出曲线规划；
若下一时刻处于seg=1，则
；
若下一时刻处于seg=2，则
；
若下一时刻处于seg=3，则
；
若下一时刻处于seg=4或seg=5，则
；
若下一时刻处于seg=6，则
其中，AccDirt为下一时刻的加速度，VeloNext为下一时刻的速度，PosNext为下一时刻的位置，PosEnd表示最终规划位置，即退出曲线规划，PosCurt为当前时刻位置。
7.一种计算机可读存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序被处理器运行时执行如权利要求1‑4中任意一项所述的支持运动中改变目标位置的曲线规划方法的步骤。
说明书 :
支持运动中改变目标位置的曲线规划方法及系统
技术领域
[0001] 本发明涉及控制技术领域，具体地涉及一种支持运动中改变目标位置的曲线规划方法及系统。
背景技术
[0002] 电机运动的目标范围不是一个确定的数，而在一定范围内。为提高电机运动效率，先设置目标位置让电机运动，在电机运动的过程中再确定电机的准确目标位置，然后更新目标参数。因此新的目标位置和之前的目标位置大概率不完全相同，如果等上一次的曲线规划结束、运动到位后，再重新规划曲线重新运动，所需要的运动时间会增长。为了减小电机运动到位所需要的时间，要求曲线规划能实时响应目标位置的变化。
[0003] T型曲线规划，是工业界广泛采用的形式，算法的复杂度低，所占用的系统资源少且容易实现。但是一般在运动开始前规划，运动过程中如果想改变目标位置，不能立刻响应。因此在曲线规划过程中不能实时响应目标位置的变化，只能在上一个曲线规划结束后，才能重新规划曲线，因此运动时间长、不满足使用要求。
发明内容
[0004] 本发明的目的是为了克服现有技术存在的问题，提出了一种支持运动中改变目标位置的曲线规划方法及系统，能够实时更新T型曲线规划，运动过程中支持变更目标位置、最大速度、加速度和减速度，实时更新曲线规划。
[0005] 为了实现上述目的，本发明第一方面提供了一种支持运动中改变目标位置的曲线规划方法，包括以下步骤：
[0006] 设置参数；
[0007] 利用参数计算下一时刻开始减速的距离，计算公式如下：
[0008] 若当前时刻处于加速阶段或等待阶段：
[0009] 若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
[0010] 根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；
[0011] 根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
[0012] 本发明第二方面提供了一种支持运动中改变目标位置的曲线规划系统，包括：
[0013] 设置模块，用于设置参数；
[0014] 计算模块，用于利用参数计算下一时刻开始减速的距离，计算公式如下：
[0015] 若当前时刻处于加速阶段或等待阶段：
[0016] 若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
[0017] 下一时刻判断模块，用于根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；
[0018] 规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
[0019] 本发明第三方面提供了一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器运行时执行上述支持运动中改变目标位置的曲线规划方法的步骤。
[0020] 通过上述技术方案，本发明在T型曲线规划的基础上实时更新的T型曲线规划，运动过程中支持变更目标位置、最大速度、加速度和减速度，能立刻响应参数变化，实时更新曲线规划，且曲线规划安全，不会超过设置的参数运行，不会超出设定的最大位置，既保证了到位精确，又缩短了曲线规划时间。
附图说明
[0021] 图1是本发明实施例支持运动中改变目标位置的曲线规划的流程示意图；
[0022] 图2是本发明实施例1的位置曲线图；
[0023] 图3是本发明实施例1的加速度曲线图；
[0024] 图4是本发明实施例1的速度曲线图；
[0025] 图5是本发明实施例2的位置曲线图；
[0026] 图6是本发明实施例2的速度曲线图；
[0027] 图7是本发明实施例2的加速度曲线图。
具体实施方式
[0028] 以下结合附图对本发明实施例的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明实施例，并不用于限制本发明实施例。
[0029] 在本发明的一种具体实施方式中，一种支持运动中改变目标位置的曲线规划方法，如图1所示，包括以下步骤：
[0030] S1、设置参数；
[0031] 设置的参数包括目标位置PosSet、最大速度VeloSet、加速度AccSet、减速度DecSet和计算频率TGain（示例性地取1000Hz）、新的目标位置PosNew、新的最大速度VeloNew、新的加速度AccNew、新的减速度DecNew；以及曲线规划过程中使用的其他参数：当前时刻位置PosCurt、当前时刻位置与目标位置的距离差Dismove、当前时刻速度VeloCurt、下一时刻位置PosNext、下一时刻速度VeloNext、下一时刻加速度AccDirt、下一时刻开始减速的距离DisDec1、最小减速距离DisDec2、运动的阶段seg和计算频率TGain。
[0032] 其中，运动的阶段包括以下7个，用阶段参数seg来表示：
[0033] seg=0：等待阶段，表示当前曲线规划刚启动、目标位置等相关参数更新或者上阶段曲线规划完成；
[0034] seg=1：加速阶段，表示当前速度小于设定速度，需要加速到设定速度；
[0035] seg=2：匀速阶段，表示当前处于匀速阶段；
[0036] seg=3：正常减速停止阶段，表示当前处于按照设定减速度进行减速并停止在目标位置的正常减速停止阶段；
[0037] seg=4：反向‑减速停止阶段，表示当前位置和目标位置反向，亦即Dismove和VeloCurt反向，需要按照设定的减速度减速停止；
[0038] seg=5：同向但减速距离过小‑减速停止阶段，表示当前位置和目标位置同向且Dismove
[0039] seg=6：实际速度大于设定速度‑减速阶段，表示当前速度大于设定速度，需要减速到设定速度。
[0040] 基于上述参数，进行设置参数包括：开始运动前，要设置目标位置PosSet、最大速度VeloSet、加速度AccSet、减速度DecSet和计算频率TGain。并根据目标位置和当前位置的相对关系，更新速度、加速度和减速度的符号，如下：
[0041] 若 当 前时 刻P o s C u r t 位置 在 目 标 位 置 P os S e t 的 前 方 ，则；
[0042] 若 当 前时 刻P o s C u r t 位置 在 目 标 位 置 P os S e t 的 后 方 ，则。
[0043] 其它使用到的参数：当前时刻位置PosCurt、当前时刻速度VeloCurt、下一时刻位置PosNext、下一时刻速度VeloNext、下一时刻加速度AccDirt、运动的阶段seg，优选地，执行本发明方法前将这些参数都清零恢复原始状态，以防止出现意外情况。
[0044] 进行设置参数还包括运动过程中设置新参数：运动过程中，可以设置新的目标位置PosNew、新的最大速度VeloNew、新的加速度AccNew、新的减速度DecNew。判断新设置的目标位置PosNew是否变化：
[0045] 若无变化，最大速度VeloSet、加速度AccSet、减速度DecSet不更新成新设置的参数，不更改原有曲线规划；若有变化，令seg=0，目标位置、最大速度、加速度、减速度都更新成新设置的参数。
[0046] 进一步地，考虑到某些情况：一开始的目标位置为运动边界位置，在减速停止过程中改变目标位置且减小了减速度。如果按照新设置的减速度减速停止，停止的位置会超出边界位置，会不安全。因此在更新减速度参数时，要判断新设置的减速度和之前的减速度的相对大小。
[0047] 只有abs(DecNew)≥abs(DecSet)时，才将减速度DecSet更新成新设置的DecNew。否则就等“反向‑减速停止阶段seg=4”或者“同向但减速距离过小‑减速停止阶段seg=5”结束后，再更新到新设置的DecNew。
[0048] 更新速度、加速度和减速度要根据当前位置和目标位置的相对大小，决定参数的符号，具体如下：
[0049] a、若当前时刻PosCurt位置在目标位置PosSet的前方，则：
[0050] VeloSet = ‑abs(VeloNew) ，AccSet = ‑abs(AccNew)，
[0051] 当abs(DecNew) ≥ abs(DecSet)，则：DecSet = ‑abs(DecNew)；
[0052] 否则：DecSet = ‑abs(DecSet)；
[0053] b、若当前时刻PosCurt位置在目标位置PosSet的后方，则：
[0054] VeloSet = abs(VeloNew) ，AccSet = abs(AccNew)，
[0055] 当abs(DecNew) ≥ abs(DecSet)，则：DecSet = abs(DecNew)；
[0056] 否则：DecSet = abs(DecSet) 。
[0057] S2、利用参数计算下一时刻开始减速的距离和最小减速距离；
[0058] 首先计算最小减速距离，计算公式如下：
[0059]
[0060] 其中，DisDec2为最小减速距离，VeloCurt为当前时刻速度，DecSet为当前时刻至目标位置的减速度。
[0061] 如果DismoveDecSet，不满足要求。所以必须先按照设定的减速度DecSet减速停止，再反向运动到目标位置。
[0062] 然后计算下一时刻开始减速的距离，计算公式如下：
[0063] 若当前时刻处于加速阶段：
[0064] 若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2，
[0065] 其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，V2为k时刻的下一时刻速度，T为任意相邻两个时刻的时间间隔。
[0066] 具体地，如果正常减速停止时计算出的下一时刻减速度，且要求AccDirt≤DecSet，则 。
[0067] 对于DisDec1的选择，若选择不合适将产生以下影响：
[0068] A.如果DisDec1选择过大，例如DisDec1=DisDec2，很大可能会出现以下情况：
[0069] 1.上一时刻Dismove>DisDec1=DisDec2，继续加速或匀速；
[0070] 2.下一时刻Dismove
[0071] 上述情况的出现导致不能一次规划到位，反而要在目标位置附近来回调整，会导致到位时间增长。极端情况下会来回摆荡，不能正常到位。
[0072] B.如果DisDec1选择过小，能够保证在正常情况下先触发正常减速停止，即满足DisDec1>Dismove>DisDec2。但是如果DisDec1过小，会导致曲线规划提前进入“正常减速停止阶段seg=3”，计算出的下一时刻减速度远小于设定的减速度，也会使到位时间增长。
[0073] 因此，DisDec1的选择需满足以下条件：
[0074] a.在正常运动过程中（目标位置不变），一定要存在DisDec1>Dismove>DisDec2时刻，保证在触发“同向但减速距离过小‑减速停止阶段seg=5”之前，先触发“正常减速停止阶段seg=3”。
[0075] b.同时DisDec1不宜太小，如果刚满足DisDec1>Dismove>DisDec2时不正常减速停止，尽量在下个时刻触发Dismove
[0076] 综上分析，DisDec1的推导过程如下（以下数学推导过程中，使用到的速度和加速度都是正值，不考虑方向）：
[0077] 如果当前时刻处于加速阶段：设目标位置为P，加速度为α1，减速度为α2。当前时刻的速度为V1，前时刻的位置为P1。如果加速运动，则下一时刻的速度为V2，位置为P2。V1的减速距离为S1，V2的减速距离为S2。则存在以下关系：
[0078] 。
[0079] 当前时刻DisMove(k)=P‑P1，DisDec2(k)=S1。下一时刻DisMove(k+1)=P‑P2，DisDec2(k+1)=S2。由上述的DisDec1的选择条件并考虑临界状态可知：
[0080] ，
[0081] 对上式进行带入，并化简得到：
[0082] 。
[0083] 由此可以得到：
[0084] 。
[0085] 如果当前时刻处于匀速阶段：设目标位置为P，加速度为α1，减速度为α2。当前时刻的速度为V1，前时刻的位置为P1。如果匀速运动，则下一时刻的速度为V2，位置为P2。V1的减速距离为S1，V2的减速距离为S2。则存在以下关系：
[0086] 。
[0087] 当前时刻DisMove(k)=P‑P1，DisDec2(k)=S1。下一时刻DisMove(k+1)=P‑P2，DisDec2(k+1)=S2。由上述的 的选择条件并考虑临界状态可知：
[0088] 。
[0089] 对上式进行带入，并化简得到：
[0090] 。
[0091] 由此可以得到：DisDec1(k) = DisDec2(k) + V1T。
[0092] 如果当前时刻处于减速阶段：设目标位置为P，加速度为α1，减速度为α2。当前时刻的速度为V1，前时刻的位置为P1。如果减速运动，则下一时刻的速度为V2，位置为P2。V1的减速距离为S1，V2的减速距离为S2。则存在以下关系：
[0093] 。
[0094] 当前时刻DisMove(k)=P‑P1，DisDec2(k)=S1。下一时刻DisMove(k+1)=P‑P2，DisDec2(k+1)=S2。由上述的 的选择条件并考虑临界状态可知：
[0095] 。
[0096] 由此可以得到：DisDec1(k) = DisDec2(k)。
[0097] 为简化分类，将减速阶段停止时的 选择和匀速阶段时保持一致，即：
[0098] DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2。
[0099] 进一步地，根据当前时刻所处的曲线阶段seg选择DisDec1，具体如下：
[0100] 由以上的推导过程可知：DisDec1有当前时刻处于加速阶段和当前时刻处于匀速或减速阶段两种取值方法，可以根据当前时刻的曲线规划阶段选择DisDec1：
[0101] 如果处于“加速阶段seg=1”或者处于“等待阶段seg=0”，则下一时刻可能是加速，则选择 ；
[0102] 如果处于“匀速阶段seg=2”、“正常减速停止阶段seg=3”、“反向‑减速停止阶段seg=4”、“同向但减速距离过小‑减速停止阶段seg=5”、“实际速度大于设定速度‑减速阶段seg=6”，下一时刻不可能加速，只可能是匀速或减速，则选择DisDec1(k) = DisDec2(k) + V1T。
[0103] 实际运动中速度是有符号的，要取绝对值。使用实际参数对上式做替换。综上所述，得到如下表达式：
[0104] 当seg=0或seg=1
[0105] 表达式 ；
[0106] 当seg=2、seg=3、seg=4、seg=5或seg=6，
[0107] 。
[0108] S3、根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；
[0109] 首先参数确定当前时刻是否满足曲线规划完成的退出条件，具体如下：
[0110] 1) 当“正常减速停止阶段seg=3”完成后，下一时刻的目标位置等于设定的新的目标位置，此时完成曲线规划，满足退出条件，正常退出。
[0111] 2) 当DisMove很小的同时，速度也很小：考虑到一些特殊情况：比如“等待阶段seg=0”，且设定的新的目标位置和当前目标位置之间的距离很小，如果此时速度也比较小，就直接更新到当前目标位置。当满足以下条件时，即表达式（3），也可以直接更新到设定位置，然后退出曲线规划：
[0112] 表达式（3）。
[0113] 进一步地，判断DisMove和当前速度VeloCurt是否满足表达式(3)的退出条件，若满足，则退出曲线规划，下一时刻处于下一曲线规划的等待阶段seg=0；
[0114] 若不满足，且当前时刻曲线规划处于seg=3或seg=5，则下一时刻维持当前时刻所处阶段。原因如下：当前时刻曲线规划处于seg=3时，实时计算过程中DisMove和开始减速距离DisDec1、最小减速距离DisDec2的大小可能会发生相对变化，所以维持当前阶段不变，以防止由“正常减速停止阶段seg=3”错误切换成“匀速阶段seg=2”或者“同向但减速距离过小‑减速停止阶段seg=5”；当前时刻曲线规划处于seg=5时，在同向但减速距离过小‑减速停止阶段seg=5中，会存在DisMove和VeloCurt正负符号相反的情况，所以维持当前阶段不变，以防止由“同向但减速距离过小‑减速停止阶段seg=5”错误切换成“反向‑减速停止阶段seg=4”；
[0115] 若不满足，且当前时刻处于seg=1、seg=2或seg=4中的一种，则包括以下情况：
[0116] 当DisMove和VeloCurt反向时，即DisMove×VeloCurt<0，要按照设定的减速度减速停止，下一时刻处于seg=4。
[0117] 根据设定的新的目标位置和当前目标位置的关系，更新设定速度、设定加速度和设定减速度的符号：
[0118] 若当前时刻PosCurt位置大于目标位置PosSet，则 ，
[0119] 若当前时刻PosCurt位置小于目标位置PosSet，则： 。
[0120] 当新的目标位置和目标位置同向且abs(DisMove)>abs(DisDec1)时，若abs(VeloCurt)abs(VeloSet)，需要按照设定的减速度，减速到设定速度，下一时刻处于seg=6；
[0121] 当新的目标位置和目标位置同向且abs(DisMove)≤abs(DisDec1)时，若abs(DisMove)≥abs(DisDec2)，需要继续正常的减速停止，下一时刻处于seg=3，若abs(DisMove)
[0122] S4、根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
[0123] 进一步地，具体为：
[0124] 若下一时刻处于seg=0，则
[0125] ，退出曲线规划。
[0126] 若下一时刻处于seg=1，则
[0127] ；
[0128] 需要在计算下一时刻的位置PosNext之前，判断设定的下一时刻的速度VeloNext的大小。如果VeloNext>VeloSet，VeloNext=VeloSet。
[0129] 若下一时刻处于seg=2，则
[0130] 。
[0131] 若下一时刻处于seg=3，则
[0132] ；
[0133] 需要在计算下一时刻的位置PosNext之前，判断设定的下一时刻的速度VeloNext的大小。如果VeloNext=0或者VeloNext与VeloCurt正负符号相反，则令VeloNext=VeloCurt，防止不能达到目标位置，维持同向最小速度。
[0134] 计算出下一时刻的位置PosNext之后，计算设定位置与下一时刻的位置之间的位置差DisRev=PosSet‑PosNext（图1中所述位置差均指设定位置与下一时刻的位置之间的位置差）。如果下一时刻位置差DisRev=0或者DisRev和当前时刻位置差DisMove符号相反，说明下一时刻位置PosNext越过目标位置PosSet，则令AccDirt=0，VeloNext=0，PosNext=PosEnd，并退出曲线规划。
[0135] 若下一时刻处于seg=4或seg=5，则
[0136] ，
[0137] 其中，若seg=4，AccDirt=DecSet，若seg=5，AccDirt=‑DecSet，
[0138] 需要在计算下一时刻位置PosNext之前，判断下一时刻的速度VeloNext的大小。如果VeloNext=0或者VeloNext与VeloCurt正负符号相反，说明减速停止过程已经完成，则令AccDirt=0，VeloNext=0，seg=0，并更新到最新的速度、加速度和减速度参数。
[0139] 进一步地，参数更新如下：
[0140] 如果当前位置PosCurt在设定位置PosSet之前，则
[0141] ，否则
[0142] 。
[0143] 下一时刻位置计算：
[0144] 。
[0145] 若下一时刻处于seg=6，则
[0146]
[0147] 其中，PosEnd表示最终规划位置，即退出曲线规划，PosCurt为当前时刻位置。需要在计算下一时刻位置PosNext之前，判断下一时刻的速度VeloNext的大小。如果VeloNext
[0148] 本发明在T型曲线规划的基础上做出改进，运动过程中能实时响应目标位置的变化，并按照新设置的最大速度、加速度和减速度立刻调整曲线。为了方便使用和应用于更多的情况，将加速度和减速度分开，可以分别设置为不同值。此外在目标位置变化的前提下，还可以重新设置最大速度、加速度和减速度。
[0149] 本发明对位置变化时可能的情况进行分类讨论，并增加了设置减速度是否生效判断，即提出了曲线规划完成的退出条件，能保证电机运行的安全，不会超过设定参数运行，不会超出设定的最大位置。本发明方案便于在计算机或芯片内实现，由当前时刻位置和当前时刻速度，迭代累加得到下一时刻的速度和下一时刻的位置。
[0150] 基于上述支持运动中改变目标位置的曲线规划方法，本发明第二方面提供一种支持运动中改变目标位置的曲线规划系统，包括：
[0151] 设置模块，用于设置参数；
[0152] 计算模块，用于利用参数计算下一时刻开始减速的距离，计算公式如下：
[0153] 若当前时刻处于加速阶段或等待阶段：
[0154] 若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
[0155] 下一时刻判断模块，用于根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；
[0156] 规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
[0157] 进一步地，所述下一时刻判断模块中，所述根据参数确定当前时刻是否满足曲线规划完成的退出条件具体如下：
[0158] 若当前时刻处于减速阶段的正常减速停止阶段，下一时刻的目标位置即为设定的新的目标位置，或者若满足 ，则曲线规划完成，满足退出条件，
[0159] 其中，DisMove为当前时刻位置与目标位置的距离差，AccSet为当前时刻加速度，DecSet为当前时刻减速度，TGain为计算频率，VeloCurt为当前时刻速度，abs为绝对值函数。
[0160] 进一步地，所述下一时刻判断模块中，所述根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段具体为：
[0161] 若当前时刻处于seg=3或seg=5，则下一时刻维持当前时刻所处阶段；
[0162] 若当前时刻处于seg=1、seg=2或seg=4中的一种，则包括以下情况：
[0163] 当DisMove和VeloCurt反向时，下一时刻处于seg=4；
[0164] 当新的目标位置和目标位置同向且abs(DisMove)>abs(DisDec1)时，若abs(VeloCurt)abs(VeloSet)下一时刻处于seg=6；
[0165] 当新的目标位置和目标位置同向且abs(DisMove)≤abs(DisDec1)时，若abs(DisMove)≥abs(DisDec2)下一时刻处于seg=3，若abs(DisMove)
[0166] 其中，VeloSet表示设定速度，seg=1表示加速阶段，seg=2表示匀速阶段，seg=3表示正常减速停止阶段，seg=4表示当前位置和目标位置反向，需按照设定的减速度减速停止阶段，seg=5表示当前位置和目标位置同向且Dismove
[0167] 进一步地，所述规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度具体为：
[0168] 若下一时刻处于seg=0，则
[0169] ，退出曲线规划；
[0170] 若下一时刻处于seg=1，则
[0171] ；
[0172] 若下一时刻处于seg=2，则
[0173] ；
[0174] 若下一时刻处于seg=3，则
[0175] ；
[0176] 若下一时刻处于seg=4或seg=5，则
[0177]
[0178] ；
[0179] 若下一时刻处于seg=6，则
[0180]
[0181] 其中，AccDirt为下一时刻的加速度，VeloNext为下一时刻的速度，PosNext为下一时刻的位置，PosEnd表示最终规划位置，即退出曲线规划，PosCurt为当前时刻位置。
[0182] 本发明第三方面提供一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器运行时执行上述支持运动中改变目标位置的曲线规划方法的步骤。
[0183] 实施例1
[0184] 电机的目标位置是500mm，最大速度为1.67m/s、加速度为2.2g。在开始运动的100ms时，收到下一条指令，目标位置是300mm、最大速度为1m/s、加速度为1g。根据本发明持运动中改变目标位置的曲线规划方法进行MATLAB仿真模拟（MATLAB是一种商业数学软件，用于数据分析、无线通信、深度学习、图像处理与计算机视觉、信号处理、量化金融与风险管理、机器人，控制系统等领域），步骤如下：
[0185] 1、设置参数：
[0186] 目标位置 PosSet为500mm；
[0187] 最大速度 VeloSet为1.67m/s；
[0188] 加速度 AccSet为22m/s2；
[0189] 减速度 DecSet为22m/s2；
[0190] 新设置的目标位置 PosNew为300mm；
[0191] 新设置的最大速度 VeloNew为1m/s；
[0192] 新设置的加速度 AccNew为10m/s2；
[0193] 新设置的减速度 DecNew为10m/s2。
[0194] 2、进行MATLAB仿真的规划计算：
[0195] 位置曲线如图2所示，加速度曲线如图3所示，速度曲线如图4所示，0 76ms，此时当~前速度小于设定速度，处于Seg=1的阶段，电机加速到设定速度；
[0196] 76 100ms，此时电机以1.67m/s匀速运动，处于Seg=2的阶段；~
[0197] 在100ms这一时刻，运动到了103.6mm处，同时产生了位置更新的指令：目标位置是300mm、最大速度为1m/s、加速度为1g。由于：不满足退出条件、seg=1、位置差与当前速度同向、位置差（300‑103.6=196.4mm）也大于开始减速距离（139.445mm）且当前速度（1.67m/s）大于新设定速度（1m/s），根据前述方法及流程图，将Seg的值改为6；
[0198] 100 168ms，此时电机速度从1.67m/s减速到1m/s，处于Seg=6的阶段；~
[0199] 168 224ms，此时电机以1m/s匀速运动，处于Seg=2的阶段；~
[0200] 224 326ms，此时电机处于按照设定减速度（10m/s2）进行减速并停止在目标位置~的正常减速停止阶段，处于Seg=3的阶段；
[0201] 326ms以后运动结束，退出规划，Seg=0。
[0202] 实施例2
[0203] 电机的目标位置是500mm，最大速度为1.67m/s、加速度为2.2g。在开始运动的300ms时，收到下一条指令，目标位置是300mm、最大速度为1m/s、加速度为1g。MATLAB仿真的位置曲线如图5所示、速度曲线如图6所示和加速度曲线如图7所示，经过分析：300ms时，电机的理论位置是437.6mm，已经超过了下一条指令的目标位置，因此按照目前的减速度减速停止，并立即按照新的指令运动。
[0204] 综上所述，本发明在T型曲线规划的基础上实时更新的T型曲线规划，运动过程中支持变更目标位置、最大速度、加速度和减速度，能立刻响应参数变化，实时更新曲线规划，且曲线规划安全，不会超过设置的参数运行，不会超出设定的最大位置，既保证了到位精确，又缩短了曲线规划时间。
[0205] 以上结合附图详细描述了本发明的优选实施方式，但是，本发明并不限于此。在本发明的技术构思范围内，可以对本发明的技术方案进行多种简单变型包括各个具体技术特征以任何合适的方式进行组合。为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。但这些简单变型和组合同样应当视为本发明所公开的内容，均属于本发明的保护范围。