CN115032949B - 支持运动中改变目标位置的曲线规划方法及系统 - Google Patents

支持运动中改变目标位置的曲线规划方法及系统 Download PDF

Info

Publication number
CN115032949B
CN115032949B CN202210964346.8A CN202210964346A CN115032949B CN 115032949 B CN115032949 B CN 115032949B CN 202210964346 A CN202210964346 A CN 202210964346A CN 115032949 B CN115032949 B CN 115032949B
Authority
CN
China
Prior art keywords
seg
deceleration
stage
abs
time
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210964346.8A
Other languages
English (en)
Other versions
CN115032949A (zh
Inventor
陈晓阳
孙海星
郭恩政
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hefei Anxin Precision Technology Co Ltd
Original Assignee
Hefei Anxin Precision Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hefei Anxin Precision Technology Co Ltd filed Critical Hefei Anxin Precision Technology Co Ltd
Priority to CN202210964346.8A priority Critical patent/CN115032949B/zh
Publication of CN115032949A publication Critical patent/CN115032949A/zh
Application granted granted Critical
Publication of CN115032949B publication Critical patent/CN115032949B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/416—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control of velocity, acceleration or deceleration
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/35—Nc in input of data, input till input file format
    • G05B2219/35473—Input limit values of speed, position, acceleration or force

Abstract

本发明涉及贴片机控制技术领域，具体公开了一种支持运动中改变目标位置的曲线规划方法及系统，所述方法包括以下步骤：设置参数；利用参数计算下一时刻开始减速的距离；根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。本发明在T型曲线规划的基础上实时更新的T型曲线规划，运动过程中支持变更目标位置、最大速度、加速度和减速度，能立刻响应参数变化，实时更新曲线规划，且曲线规划安全，不会超过设置的参数运行，不会超出设定的最大位置，既保证了到位精确，又缩短了曲线规划时间。

Description

支持运动中改变目标位置的曲线规划方法及系统
技术领域
本发明涉及控制技术领域，具体地涉及一种支持运动中改变目标位置的曲线规划方法及系统。
背景技术
电机运动的目标范围不是一个确定的数，而在一定范围内。为提高电机运动效率，先设置目标位置让电机运动，在电机运动的过程中再确定电机的准确目标位置，然后更新目标参数。因此新的目标位置和之前的目标位置大概率不完全相同，如果等上一次的曲线规划结束、运动到位后，再重新规划曲线重新运动，所需要的运动时间会增长。为了减小电机运动到位所需要的时间，要求曲线规划能实时响应目标位置的变化。
T型曲线规划，是工业界广泛采用的形式，算法的复杂度低，所占用的系统资源少且容易实现。但是一般在运动开始前规划，运动过程中如果想改变目标位置，不能立刻响应。因此在曲线规划过程中不能实时响应目标位置的变化，只能在上一个曲线规划结束后，才能重新规划曲线，因此运动时间长、不满足使用要求。
发明内容
本发明的目的是为了克服现有技术存在的问题，提出了一种支持运动中改变目标位置的曲线规划方法及系统，能够实时更新T型曲线规划，运动过程中支持变更目标位置、最大速度、加速度和减速度，实时更新曲线规划。
为了实现上述目的，本发明第一方面提供了一种支持运动中改变目标位置的曲线规划方法，包括以下步骤：
设置参数；
利用参数计算下一时刻开始减速的距离，计算公式如下：
若当前时刻处于加速阶段或等待阶段：
Figure 783626DEST_PATH_IMAGE001
若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；
根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
本发明第二方面提供了一种支持运动中改变目标位置的曲线规划系统，包括：
设置模块，用于设置参数；
计算模块，用于利用参数计算下一时刻开始减速的距离，计算公式如下：
若当前时刻处于加速阶段或等待阶段：
Figure 651088DEST_PATH_IMAGE001
若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
下一时刻判断模块，用于根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；
规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
本发明第三方面提供了一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器运行时执行上述支持运动中改变目标位置的曲线规划方法的步骤。
通过上述技术方案，本发明在T型曲线规划的基础上实时更新的T型曲线规划，运动过程中支持变更目标位置、最大速度、加速度和减速度，能立刻响应参数变化，实时更新曲线规划，且曲线规划安全，不会超过设置的参数运行，不会超出设定的最大位置，既保证了到位精确，又缩短了曲线规划时间。
附图说明
图1是本发明实施例支持运动中改变目标位置的曲线规划的流程示意图；
图2是本发明实施例1的位置曲线图；
图3是本发明实施例1的加速度曲线图；
图4是本发明实施例1的速度曲线图；
图5是本发明实施例2的位置曲线图；
图6是本发明实施例2的速度曲线图；
图7是本发明实施例2的加速度曲线图。
具体实施方式
以下结合附图对本发明实施例的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明实施例，并不用于限制本发明实施例。
在本发明的一种具体实施方式中，一种支持运动中改变目标位置的曲线规划方法，如图1所示，包括以下步骤：
S1、设置参数；
设置的参数包括目标位置PosSet、最大速度VeloSet、加速度AccSet、减速度DecSet和计算频率TGain（示例性地取1000Hz）、新的目标位置PosNew、新的最大速度VeloNew、新的加速度AccNew、新的减速度DecNew；以及曲线规划过程中使用的其他参数：当前时刻位置PosCurt、当前时刻位置与目标位置的距离差Dismove、当前时刻速度VeloCurt、下一时刻位置PosNext、下一时刻速度VeloNext、下一时刻加速度AccDirt、下一时刻开始减速的距离DisDec1、最小减速距离DisDec2、运动的阶段seg和计算频率TGain。
其中，运动的阶段包括以下7个，用阶段参数seg来表示：
seg=0：等待阶段，表示当前曲线规划刚启动、目标位置等相关参数更新或者上阶段曲线规划完成；
seg=1：加速阶段，表示当前速度小于设定速度，需要加速到设定速度；
seg=2：匀速阶段，表示当前处于匀速阶段；
seg=3：正常减速停止阶段，表示当前处于按照设定减速度进行减速并停止在目标位置的正常减速停止阶段；
seg=4：反向-减速停止阶段，表示当前位置和目标位置反向，亦即Dismove和VeloCurt反向，需要按照设定的减速度减速停止；
seg=5：同向但减速距离过小-减速停止阶段，表示当前位置和目标位置同向且Dismove<DisDec2，所以也需要先按照设定的减速度减速停止；
seg=6：实际速度大于设定速度-减速阶段，表示当前速度大于设定速度，需要减速到设定速度。
基于上述参数，进行设置参数包括：开始运动前，要设置目标位置PosSet、最大速度VeloSet、加速度AccSet、减速度DecSet和计算频率TGain。并根据目标位置和当前位置的相对关系，更新速度、加速度和减速度的符号，如下：
若当前时刻PosCurt位置在目标位置PosSet的前方，则
Figure 783123DEST_PATH_IMAGE002
；
若当前时刻PosCurt位置在目标位置PosSet的后方，则
Figure 685220DEST_PATH_IMAGE003
。
其它使用到的参数：当前时刻位置PosCurt、当前时刻速度VeloCurt、下一时刻位置PosNext、下一时刻速度VeloNext、下一时刻加速度AccDirt、运动的阶段seg，优选地，执行本发明方法前将这些参数都清零恢复原始状态，以防止出现意外情况。
进行设置参数还包括运动过程中设置新参数：运动过程中，可以设置新的目标位置PosNew、新的最大速度VeloNew、新的加速度AccNew、新的减速度DecNew。判断新设置的目标位置PosNew是否变化：
若无变化，最大速度VeloSet、加速度AccSet、减速度DecSet不更新成新设置的参数，不更改原有曲线规划；若有变化，令seg=0，目标位置、最大速度、加速度、减速度都更新成新设置的参数。
进一步地，考虑到某些情况：一开始的目标位置为运动边界位置，在减速停止过程中改变目标位置且减小了减速度。如果按照新设置的减速度减速停止，停止的位置会超出边界位置，会不安全。因此在更新减速度参数时，要判断新设置的减速度和之前的减速度的相对大小。
只有abs(DecNew)≥abs(DecSet)时，才将减速度DecSet更新成新设置的DecNew。否则就等“反向-减速停止阶段seg=4”或者“同向但减速距离过小-减速停止阶段seg=5”结束后，再更新到新设置的DecNew。
更新速度、加速度和减速度要根据当前位置和目标位置的相对大小，决定参数的符号，具体如下：
a、若当前时刻PosCurt位置在目标位置PosSet的前方，则：
VeloSet = -abs(VeloNew) ，AccSet = -abs(AccNew)，
当abs(DecNew) ≥ abs(DecSet)，则：DecSet = -abs(DecNew)；
否则：DecSet = -abs(DecSet)；
b、若当前时刻PosCurt位置在目标位置PosSet的后方，则：
VeloSet = abs(VeloNew) ，AccSet = abs(AccNew)，
当abs(DecNew) ≥ abs(DecSet)，则：DecSet = abs(DecNew)；
否则：DecSet = abs(DecSet) 。
S2、利用参数计算下一时刻开始减速的距离和最小减速距离；
首先计算最小减速距离，计算公式如下：
Figure 713350DEST_PATH_IMAGE004
其中，DisDec2为最小减速距离，VeloCurt为当前时刻速度，DecSet为当前时刻至目标位置的减速度。
如果Dismove<DisDec2，且此时正常减速停止，则计算的AccDirt>DecSet，不满足要求。所以必须先按照设定的减速度DecSet减速停止，再反向运动到目标位置。
然后计算下一时刻开始减速的距离，计算公式如下：
若当前时刻处于加速阶段：
Figure 68108DEST_PATH_IMAGE001
若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2，
其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，V2为k时刻的下一时刻速度，T为任意相邻两个时刻的时间间隔。
具体地，如果正常减速停止时计算出的下一时刻减速度，且要求AccDirt≤ DecSet，则
Figure 738255DEST_PATH_IMAGE005
。
对于DisDec1的选择，若选择不合适将产生以下影响：
A.如果DisDec1选择过大，例如DisDec1=DisDec2，很大可能会出现以下情况：
1.上一时刻Dismove>DisDec1=DisDec2，继续加速或匀速；
2.下一时刻Dismove<DisDec2，触发“同向但减速距离过小-减速停止阶段seg=5”，必须先按照设定的减速度减速停止，再反向运动到目标位置。
上述情况的出现导致不能一次规划到位，反而要在目标位置附近来回调整，会导致到位时间增长。极端情况下会来回摆荡，不能正常到位。
B.如果DisDec1选择过小，能够保证在正常情况下先触发正常减速停止，即满足DisDec1>Dismove>DisDec2。但是如果DisDec1过小，会导致曲线规划提前进入“正常减速停止阶段seg=3”，计算出的下一时刻减速度远小于设定的减速度，也会使到位时间增长。
因此，DisDec1的选择需满足以下条件：
a.在正常运动过程中（目标位置不变），一定要存在DisDec1>Dismove>DisDec2时刻，保证在触发“同向但减速距离过小-减速停止阶段seg=5”之前，先触发“正常减速停止阶段seg=3”。
b.同时DisDec1不宜太小，如果刚满足DisDec1>Dismove>DisDec2时不正常减速停止，尽量在下个时刻触发Dismove<DisDec2。
综上分析，DisDec1的推导过程如下（以下数学推导过程中，使用到的速度和加速度都是正值，不考虑方向）：
如果当前时刻处于加速阶段：设目标位置为P，加速度为α1，减速度为α2。当前时刻的速度为V1，前时刻的位置为P1。如果加速运动，则下一时刻的速度为V2，位置为P2。V1的减速距离为S1，V2的减速距离为S2。则存在以下关系：
Figure 494858DEST_PATH_IMAGE006
。
当前时刻DisMove(k)=P-P1，DisDec2(k)=S1。下一时刻DisMove(k+1)=P-P2，DisDec2(k+1)=S2。由上述的DisDec1的选择条件并考虑临界状态可知：
Figure 222118DEST_PATH_IMAGE007
，
对上式进行带入，并化简得到：
Figure 64172DEST_PATH_IMAGE008
。
由此可以得到：
Figure 272431DEST_PATH_IMAGE009
。
如果当前时刻处于匀速阶段：设目标位置为P，加速度为α1，减速度为α2。当前时刻的速度为V1，前时刻的位置为P1。如果匀速运动，则下一时刻的速度为V2，位置为P2。V1的减速距离为S1，V2的减速距离为S2。则存在以下关系：
Figure 883541DEST_PATH_IMAGE010
。
当前时刻DisMove(k)=P-P1，DisDec2(k)=S1。下一时刻DisMove(k+1)=P-P2， DisDec2(k+1)=S2。由上述的
Figure 784632DEST_PATH_IMAGE011
的选择条件并考虑临界状态可知：
Figure 113982DEST_PATH_IMAGE012
。
对上式进行带入，并化简得到：
Figure 125931DEST_PATH_IMAGE013
。
由此可以得到：DisDec1(k) = DisDec2(k) + V1T。
如果当前时刻处于减速阶段：设目标位置为P，加速度为α1，减速度为α2。当前时刻的速度为V1，前时刻的位置为P1。如果减速运动，则下一时刻的速度为V2，位置为P2。V1的减速距离为S1，V2的减速距离为S2。则存在以下关系：
Figure 325968DEST_PATH_IMAGE014
。
当前时刻DisMove(k)=P-P1，DisDec2(k)=S1。下一时刻DisMove(k+1)=P-P2， DisDec2(k+1)=S2。由上述的
Figure 660610DEST_PATH_IMAGE011
的选择条件并考虑临界状态可知：
Figure 211677DEST_PATH_IMAGE015
。
由此可以得到：DisDec1(k) = DisDec2(k)。
为简化分类，将减速阶段停止时的
Figure 761739DEST_PATH_IMAGE011
选择和匀速阶段时保持一致，即：
DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2。
进一步地，根据当前时刻所处的曲线阶段seg选择DisDec1，具体如下：
由以上的推导过程可知：DisDec1有当前时刻处于加速阶段和当前时刻处于匀速或减速阶段两种取值方法，可以根据当前时刻的曲线规划阶段选择DisDec1：
如果处于“加速阶段seg=1”或者处于“等待阶段seg=0”，则下一时刻可能是加速， 则选择
Figure 347441DEST_PATH_IMAGE009
；
如果处于“匀速阶段seg=2”、“正常减速停止阶段seg=3”、“反向-减速停止阶段seg=4”、“同向但减速距离过小-减速停止阶段seg=5”、“实际速度大于设定速度-减速阶段seg=6”，下一时刻不可能加速，只可能是匀速或减速，则选择DisDec1(k) = DisDec2(k) + V1T。
实际运动中速度是有符号的，要取绝对值。使用实际参数对上式做替换。综上所述，得到如下表达式：
当seg=0或seg=1
Figure 590334DEST_PATH_IMAGE016
表达式
Figure 363118DEST_PATH_IMAGE017
；
当seg=2、seg=3、seg=4、seg=5或seg=6，
Figure 966138DEST_PATH_IMAGE018
Figure 422658DEST_PATH_IMAGE019
。
S3、根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；
首先参数确定当前时刻是否满足曲线规划完成的退出条件，具体如下：
1) 当“正常减速停止阶段seg=3”完成后，下一时刻的目标位置等于设定的新的目标位置，此时完成曲线规划，满足退出条件，正常退出。
2) 当DisMove很小的同时，速度也很小：考虑到一些特殊情况：比如“等待阶段seg=0”，且设定的新的目标位置和当前目标位置之间的距离很小，如果此时速度也比较小，就直接更新到当前目标位置。当满足以下条件时，即表达式（3），也可以直接更新到设定位置，然后退出曲线规划：
Figure 820141DEST_PATH_IMAGE020
表达式（3）。
进一步地，判断DisMove和当前速度VeloCurt是否满足表达式(3)的退出条件，若满足，则退出曲线规划，下一时刻处于下一曲线规划的等待阶段seg=0；
若不满足，且当前时刻曲线规划处于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”；
若不满足，且当前时刻处于seg=1、seg=2或seg=4中的一种，则包括以下情况：
当DisMove和VeloCurt反向时，即DisMove×VeloCurt<0，要按照设定的减速度减速停止，下一时刻处于seg=4。
根据设定的新的目标位置和当前目标位置的关系，更新设定速度、设定加速度和设定减速度的符号：
若当前时刻PosCurt位置大于目标位置PosSet，则
Figure 695120DEST_PATH_IMAGE021
，
若当前时刻PosCurt位置小于目标位置PosSet，则：
Figure 836251DEST_PATH_IMAGE022
。
当新的目标位置和目标位置同向且abs(DisMove)>abs(DisDec1)时，若abs(VeloCurt)<abs(VeloSet)，需要按照设定的加速度，加速到设定速度，下一时刻处于seg=1，若abs(VeloCurt)=abs(VeloSet)，需要匀速运动，下一时刻处于seg=2，若abs(VeloCurt)>abs(VeloSet)，需要按照设定的减速度，减速到设定速度，下一时刻处于seg=6；
当新的目标位置和目标位置同向且abs(DisMove)≤abs(DisDec1)时，若abs(DisMove)≥abs(DisDec2)，需要继续正常的减速停止，下一时刻处于seg=3，若abs(DisMove)<abs(DisDec2)，减速距离过小，需要按照设定的减速度减速停止，下一时刻处于seg=5。
S4、根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
进一步地，具体为：
若下一时刻处于seg=0，则
Figure 412857DEST_PATH_IMAGE023
，退出曲线规划。
若下一时刻处于seg=1，则
Figure 715663DEST_PATH_IMAGE024
；
需要在计算下一时刻的位置PosNext之前，判断设定的下一时刻的速度VeloNext的大小。如果VeloNext>VeloSet，VeloNext=VeloSet。
若下一时刻处于seg=2，则
Figure 744930DEST_PATH_IMAGE025
。
若下一时刻处于seg=3，则
Figure 689752DEST_PATH_IMAGE026
；
需要在计算下一时刻的位置PosNext之前，判断设定的下一时刻的速度VeloNext的大小。如果VeloNext=0或者VeloNext与VeloCurt正负符号相反，则令VeloNext=VeloCurt，防止不能达到目标位置，维持同向最小速度。
计算出下一时刻的位置PosNext之后，计算设定位置与下一时刻的位置之间的位置差DisRev=PosSet-PosNext（图1中所述位置差均指设定位置与下一时刻的位置之间的位置差）。如果下一时刻位置差DisRev=0或者DisRev和当前时刻位置差DisMove符号相反，说明下一时刻位置PosNext越过目标位置PosSet，则令AccDirt=0，VeloNext=0，PosNext=PosEnd，并退出曲线规划。
若下一时刻处于seg=4或seg=5，则
Figure 855285DEST_PATH_IMAGE027
，
其中，若seg=4，AccDirt=DecSet，若seg=5，AccDirt=-DecSet，
需要在计算下一时刻位置PosNext之前，判断下一时刻的速度VeloNext的大小。如果VeloNext=0或者VeloNext与VeloCurt正负符号相反，说明减速停止过程已经完成，则令AccDirt=0，VeloNext=0，seg=0，并更新到最新的速度、加速度和减速度参数。
进一步地，参数更新如下：
如果当前位置PosCurt在设定位置PosSet之前，则
Figure 860150DEST_PATH_IMAGE028
，否则
Figure 108205DEST_PATH_IMAGE029
。
下一时刻位置计算：
Figure 591139DEST_PATH_IMAGE030
。
若下一时刻处于seg=6，则
Figure 611178DEST_PATH_IMAGE031
其中，PosEnd表示最终规划位置，即退出曲线规划，PosCurt为当前时刻位置。需要在计算下一时刻位置PosNext之前，判断下一时刻的速度VeloNext的大小。如果VeloNext<VeloSet，则令VeloNext=VeloSet。
本发明在T型曲线规划的基础上做出改进，运动过程中能实时响应目标位置的变化，并按照新设置的最大速度、加速度和减速度立刻调整曲线。为了方便使用和应用于更多的情况，将加速度和减速度分开，可以分别设置为不同值。此外在目标位置变化的前提下，还可以重新设置最大速度、加速度和减速度。
本发明对位置变化时可能的情况进行分类讨论，并增加了设置减速度是否生效判断，即提出了曲线规划完成的退出条件，能保证电机运行的安全，不会超过设定参数运行，不会超出设定的最大位置。本发明方案便于在计算机或芯片内实现，由当前时刻位置和当前时刻速度，迭代累加得到下一时刻的速度和下一时刻的位置。
基于上述支持运动中改变目标位置的曲线规划方法，本发明第二方面提供一种支持运动中改变目标位置的曲线规划系统，包括：
设置模块，用于设置参数；
计算模块，用于利用参数计算下一时刻开始减速的距离，计算公式如下：
若当前时刻处于加速阶段或等待阶段：
Figure 786945DEST_PATH_IMAGE001
若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
下一时刻判断模块，用于根据参数确定当前时刻是否满足曲线规划完成的退出条件，若不满足，则根据当前时刻所处阶段和下一时刻开始减速的距离判断下一时刻所处的阶段；
规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
进一步地，所述下一时刻判断模块中，所述根据参数确定当前时刻是否满足曲线规划完成的退出条件具体如下：
若当前时刻处于减速阶段的正常减速停止阶段，下一时刻的目标位置即为设定的 新的目标位置，或者若满足
Figure 525225DEST_PATH_IMAGE032
，则曲线规划完成，满足 退出条件，
其中，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)<abs(VeloSet)下一时刻处于seg=1，若abs(VeloCurt)=abs(VeloSet)下一时刻处于seg=2，若abs(VeloCurt)>abs(VeloSet)下一时刻处于seg=6；
当新的目标位置和目标位置同向且abs(DisMove)≤abs(DisDec1)时，若abs(DisMove)≥abs(DisDec2)下一时刻处于seg=3，若abs(DisMove)<abs(DisDec2)下一时刻处于seg=5，
其中，VeloSet表示设定速度，seg=1表示加速阶段，seg=2表示匀速阶段，seg=3表示正常减速停止阶段，seg=4表示当前位置和目标位置反向，需按照设定的减速度减速停止阶段，seg=5表示当前位置和目标位置同向且Dismove<DisDec2，需按照设定的减速度减速停止阶段，seg=6表示当前时刻速度大于设定速度，需减速到设定速度阶段。
进一步地，所述规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度具体为：
若下一时刻处于seg=0，则
Figure 811850DEST_PATH_IMAGE023
，退出曲线规划；
若下一时刻处于seg=1，则
Figure 951975DEST_PATH_IMAGE024
；
若下一时刻处于seg=2，则
Figure 298643DEST_PATH_IMAGE025
；
若下一时刻处于seg=3，则
Figure 255710DEST_PATH_IMAGE026
；
若下一时刻处于seg=4或seg=5，则
Figure 80446DEST_PATH_IMAGE027
Figure 809499DEST_PATH_IMAGE030
；
若下一时刻处于seg=6，则
Figure 327068DEST_PATH_IMAGE031
其中，AccDirt为下一时刻的加速度，VeloNext为下一时刻的速度，PosNext为下一时刻的位置，PosEnd表示最终规划位置，即退出曲线规划，PosCurt为当前时刻位置。
本发明第三方面提供一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器运行时执行上述支持运动中改变目标位置的曲线规划方法的步骤。
实施例1
电机的目标位置是500mm，最大速度为1.67m/s、加速度为2.2g。在开始运动的100ms时，收到下一条指令，目标位置是300mm、最大速度为1m/s、加速度为1g。根据本发明持运动中改变目标位置的曲线规划方法进行MATLAB仿真模拟（MATLAB是一种商业数学软件，用于数据分析、无线通信、深度学习、图像处理与计算机视觉、信号处理、量化金融与风险管理、机器人，控制系统等领域），步骤如下：
1、设置参数：
目标位置 PosSet为500mm；
最大速度 VeloSet为1.67m/s；
加速度 AccSet为22m/s2；
减速度 DecSet为22m/s2；
新设置的目标位置 PosNew为300mm；
新设置的最大速度 VeloNew为1m/s；
新设置的加速度 AccNew为10m/s2；
新设置的减速度 DecNew为10m/s2。
2、进行MATLAB仿真的规划计算：
位置曲线如图2所示，加速度曲线如图3所示，速度曲线如图4所示，0~76ms，此时当前速度小于设定速度，处于Seg=1的阶段，电机加速到设定速度；
76~100ms，此时电机以1.67m/s匀速运动，处于Seg=2的阶段；
在100ms这一时刻，运动到了103.6mm处，同时产生了位置更新的指令：目标位置是300mm、最大速度为1m/s、加速度为1g。由于：不满足退出条件、seg=1、位置差与当前速度同向、位置差（300-103.6=196.4mm）也大于开始减速距离（139.445mm）且当前速度（1.67m/s）大于新设定速度（1m/s），根据前述方法及流程图，将Seg的值改为6；
100~168ms，此时电机速度从1.67m/s减速到1m/s，处于Seg=6的阶段；
168~224ms，此时电机以1m/s匀速运动，处于Seg=2的阶段；
224~326ms，此时电机处于按照设定减速度（10m/s2）进行减速并停止在目标位置的正常减速停止阶段，处于Seg=3的阶段；
326ms以后运动结束，退出规划，Seg=0。
实施例2
电机的目标位置是500mm，最大速度为1.67m/s、加速度为2.2g。在开始运动的300ms时，收到下一条指令，目标位置是300mm、最大速度为1m/s、加速度为1g。MATLAB仿真的位置曲线如图5所示、速度曲线如图6所示和加速度曲线如图7所示，经过分析：300ms时，电机的理论位置是437.6mm，已经超过了下一条指令的目标位置，因此按照目前的减速度减速停止，并立即按照新的指令运动。
综上所述，本发明在T型曲线规划的基础上实时更新的T型曲线规划，运动过程中支持变更目标位置、最大速度、加速度和减速度，能立刻响应参数变化，实时更新曲线规划，且曲线规划安全，不会超过设置的参数运行，不会超出设定的最大位置，既保证了到位精确，又缩短了曲线规划时间。
以上结合附图详细描述了本发明的优选实施方式，但是，本发明并不限于此。在本发明的技术构思范围内，可以对本发明的技术方案进行多种简单变型包括各个具体技术特征以任何合适的方式进行组合。为了避免不必要的重复，本发明对各种可能的组合方式不再另行说明。但这些简单变型和组合同样应当视为本发明所公开的内容，均属于本发明的保护范围。

Claims (7)

1.一种支持运动中改变目标位置的曲线规划方法，其特征在于，包括以下步骤：
设置参数；
利用参数计算下一时刻开始减速的距离，计算公式如下：
若当前时刻处于加速阶段或等待阶段：
Figure 922042DEST_PATH_IMAGE001
若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
根据参数确定当前时刻是否满足曲线规划完成的退出条件，具体如下：若当前时刻处 于减速阶段的正常减速停止阶段，下一时刻的目标位置即为设定的新的目标位置，或者若 满足
Figure 347076DEST_PATH_IMAGE002
，则曲线规划完成，满足退出条件，其中， 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)<abs (VeloSet)下一时刻处于seg=1，若abs(VeloCurt)=abs(VeloSet)下一时刻处于seg=2，若 abs(VeloCurt)>abs(VeloSet)下一时刻处于seg=6；当新的目标位置和目标位置同向且abs (DisMove)≤abs(DisDec1)时，若abs(DisMove)≥abs(DisDec2)下一时刻处于seg=3，若abs (DisMove)<abs(DisDec2)下一时刻处于seg=5，其中，VeloSet表示设定速度，seg=1表示加 速阶段，seg=2表示匀速阶段，seg=3表示正常减速停止阶段，seg=4表示当前位置和目标位 置反向，需按照设定的减速度减速停止阶段，seg=5表示当前位置和目标位置同向且 Dismove<DisDec2，需按照设定的减速度减速停止阶段，seg=6表示当前时刻速度大于设定 速度，需减速到设定速度阶段；
根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
2.根据权利要求1所述的方法，其特征在于，所述根据参数确定当前时刻是否满足曲线规划完成的退出条件，若满足，则退出曲线规划，下一时刻处于下一曲线规划的等待阶段seg=0。
3.根据权利要求2所述的方法，其特征在于，所述根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度具体为：
若下一时刻处于seg=0，则
Figure 334623DEST_PATH_IMAGE003
，退出曲线规划；
若下一时刻处于seg=1，则
Figure 293352DEST_PATH_IMAGE004
；
若下一时刻处于seg=2，则
Figure 167898DEST_PATH_IMAGE005
；
若下一时刻处于seg=3，则
Figure 248987DEST_PATH_IMAGE006
；
若下一时刻处于seg=4或seg=5，则
Figure 973098DEST_PATH_IMAGE007
Figure 860152DEST_PATH_IMAGE008
；
若下一时刻处于seg=6，则
Figure 854783DEST_PATH_IMAGE009
其中，AccDirt为下一时刻的加速度，VeloNext为下一时刻的速度，PosNext为下一时刻的位置，PosEnd表示最终规划位置，即退出曲线规划，PosCurt为当前时刻位置。
4.根据权利要求1-3中任意一项所述的方法，所述最小减速距离的计算公式如下：
Figure 106773DEST_PATH_IMAGE010
其中，DisDec2为最小减速距离，VeloCurt为当前时刻速度，DecSet为当前时刻至目标位置的减速度。
5.一种支持运动中改变目标位置的曲线规划系统，其特征在于，包括：
设置模块，用于设置参数；
计算模块，用于利用参数计算下一时刻开始减速的距离，计算公式如下：
若当前时刻处于加速阶段或等待阶段：
Figure 52601DEST_PATH_IMAGE001
若当前时刻处于匀速阶段或减速阶段：DisDec1(k) = DisDec2(k) + V1T，且DisDec1≥DisDec2；其中，DisDec1为下一时刻开始减速的距离，k为当前时刻，α1为加速度，α2为减速度，V1为k时刻的速度，T为任意相邻两个时刻的时间间隔，DisDec2为最小减速距离；
下一时刻判断模块，用于根据参数确定当前时刻是否满足曲线规划完成的退出条件， 具体如下：若当前时刻处于减速阶段的正常减速停止阶段，下一时刻的目标位置即为设定 的新的目标位置，或者若满足
Figure 681029DEST_PATH_IMAGE002
，则曲线规划完 成，满足退出条件，其中，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)<abs(VeloSet)下一时刻处于seg=1，若abs(VeloCurt)=abs (VeloSet)下一时刻处于seg=2，若abs(VeloCurt)>abs(VeloSet)下一时刻处于seg=6；当新 的目标位置和目标位置同向且abs(DisMove)≤abs(DisDec1)时，若abs(DisMove)≥abs (DisDec2)下一时刻处于seg=3，若abs(DisMove)<abs(DisDec2)下一时刻处于seg=5，其中， VeloSet表示设定速度，seg=1表示加速阶段，seg=2表示匀速阶段，seg=3表示正常减速停止 阶段，seg=4表示当前位置和目标位置反向，需按照设定的减速度减速停止阶段，seg=5表示 当前位置和目标位置同向且Dismove<DisDec2，需按照设定的减速度减速停止阶段，seg=6 表示当前时刻速度大于设定速度，需减速到设定速度阶段；
规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度。
6.根据权利要求5所述的系统，其特征在于，所述规划模块，用于根据下一时刻所处的阶段计算下一时刻的位置、速度和加速度具体为：
若下一时刻处于seg=0，则
Figure 717118DEST_PATH_IMAGE003
，退出曲线规划；
若下一时刻处于seg=1，则
Figure 625162DEST_PATH_IMAGE004
；
若下一时刻处于seg=2，则
Figure 871336DEST_PATH_IMAGE005
；
若下一时刻处于seg=3，则
Figure 287143DEST_PATH_IMAGE006
；
若下一时刻处于seg=4或seg=5，则
Figure 912159DEST_PATH_IMAGE007
Figure 505951DEST_PATH_IMAGE008
；
若下一时刻处于seg=6，则
Figure 724574DEST_PATH_IMAGE009
其中，AccDirt为下一时刻的加速度，VeloNext为下一时刻的速度，PosNext为下一时刻的位置，PosEnd表示最终规划位置，即退出曲线规划，PosCurt为当前时刻位置。
7.一种计算机可读存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序被处理器运行时执行如权利要求1-4中任意一项所述的支持运动中改变目标位置的曲线规划方法的步骤。
CN202210964346.8A 2022-08-12 2022-08-12 支持运动中改变目标位置的曲线规划方法及系统 Active CN115032949B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210964346.8A CN115032949B (zh) 2022-08-12 2022-08-12 支持运动中改变目标位置的曲线规划方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210964346.8A CN115032949B (zh) 2022-08-12 2022-08-12 支持运动中改变目标位置的曲线规划方法及系统

Publications (2)

Publication Number Publication Date
CN115032949A CN115032949A (zh) 2022-09-09
CN115032949B true CN115032949B (zh) 2022-11-04

Family

ID=83131193

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210964346.8A Active CN115032949B (zh) 2022-08-12 2022-08-12 支持运动中改变目标位置的曲线规划方法及系统

Country Status (1)

Country Link
CN (1) CN115032949B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS63104573A (ja) * 1986-10-21 1988-05-10 Fuji Xerox Co Ltd 手駆動型複写機の制御装置
EP0949108A2 (en) * 1998-04-06 1999-10-13 Nissan Motor Company, Limited Cruise control apparatus and method, with regulation of inter-vehicle spacing
CN103713581A (zh) * 2013-12-12 2014-04-09 南京工程学院 运动控制系统s曲线加减速的实现方法
CN107844058A (zh) * 2017-11-24 2018-03-27 北京特种机械研究所 一种运动曲线离散动态规划方法
CN114740806A (zh) * 2022-05-11 2022-07-12 合肥安迅精密技术有限公司 兼容运动中目标位置更新的s型曲线规划方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS63104573A (ja) * 1986-10-21 1988-05-10 Fuji Xerox Co Ltd 手駆動型複写機の制御装置
EP0949108A2 (en) * 1998-04-06 1999-10-13 Nissan Motor Company, Limited Cruise control apparatus and method, with regulation of inter-vehicle spacing
CN103713581A (zh) * 2013-12-12 2014-04-09 南京工程学院 运动控制系统s曲线加减速的实现方法
CN107844058A (zh) * 2017-11-24 2018-03-27 北京特种机械研究所 一种运动曲线离散动态规划方法
CN114740806A (zh) * 2022-05-11 2022-07-12 合肥安迅精密技术有限公司 兼容运动中目标位置更新的s型曲线规划方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
始末速度不为零的 S 型加减速时间规划算法研究;杨亮亮;《机械工程学报》;20161231;第52卷(第23期);第199-207页 *
高速加工中的速度规划与段间连接;杨林;《上海交通大学学报》;20100131;第44卷(第1期);第40-45页 *

Also Published As

Publication number Publication date
CN115032949A (zh) 2022-09-09

Similar Documents

Publication Publication Date Title
US20170349168A1 (en) Vehicle collision avoidance
US9823660B2 (en) System for distance to stop
JP2013257856A (ja) スマートクルーズコントロールシステムの曲線路速度制御装置及び方法
EP4282724A1 (en) Method and apparatus for controlling vehicle following and stopping, and vehicle
CN110103957B (zh) 一种车辆控制方法、装置、设备及终端
CN111739342B (zh) 用于避让侧前方车辆的方法、装置、介质以及车辆
CN112965501A (zh) 一种自动驾驶速度规划方法及装置
CN109765888B (zh) 一种车辆控制方法、装置及系统
CN112254727B (zh) 基于teb的路径规划方法及装置
CN108674184B (zh) 车速控制方法、系统及汽车
CN109298713A (zh) 指令发送方法、装置及系统、自动驾驶车辆
CN115032949B (zh) 支持运动中改变目标位置的曲线规划方法及系统
CN113183962A (zh) 用于创造换道条件的自动驾驶纵向规划方法、系统及车辆
CN113428137A (zh) 泊车控制方法、装置及存储介质
CN115803216A (zh) 一种滑行能量回收方法、装置、电子设备及存储介质
CN113401182B (zh) 列车运行控制方法、装置和存储介质
CN114763140A (zh) 一种自适应巡航控制的方法、装置、设备及存储介质
CN110398993B (zh) 速度控制方法、装置及计算机可读存储介质
CN112757909B (zh) 一种车辆控制方法、装置、电子设备及存储介质
US20070150167A1 (en) Method for actuating a traffic-adaptive assistance system which is located in a vehicle
CN112622922A (zh) 具有自主车辆参考的非自主车辆速度预测
CN109747653B (zh) 一种车辆控制方法及装置
CN114394113B (zh) 车辆轨迹重规划方法、装置、电子设备及存储介质
CN112865627B (zh) 一种步进电机的控制方法及装置
JP2023072153A (ja) 経路予測装置、経路予測方法、および車両管制システム

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant