基于高阶s型运动轨迹的位置与速度控制方法
【技术领域】
[0001] 本发明属于精密运动控制领域。
【背景技术】
[0002] 在精密数控加工系统、机器人系统及精密工作台系统等精密运动控制系统中，为 了避免各轴产生的冲击、失步、超程和振荡，W保证在高加速度、高速度的情况下，各运动机 构的平稳定位，常需要进行加减速控制，常用的方法有两种，一种是梯形加速度运动轨迹， 一种是S型运动轨迹。梯形加速度运动轨迹计算量小、易于实现，但是由于加速度轨迹不平 滑，存在突变，易激起系统振荡，对系统造成冲击，因此在实际工程中很少应用。S型运动轨 迹能够保证加速度曲线的连续变化，而且阶数越高，轨迹越平滑，对系统的冲击越小。3阶、 4阶、5阶S曲线在实际工程中得到了大量应用。
[0003] S曲线的阶数越高，轨迹规划算法越复杂。传统的轨迹规划算法往往针对给定的 行程、速度、加速度等参数值进行设计，当参数值发生改变时，需重新修改算法程序。为了在 任意给定行程、速度、加速度等参数值的情况下，不需重新更改算法程序而进行运动轨迹规 划，一种方法是求出各阶曲线关于行程、速度、加速度等参数的分段解析表达式，然后将表 达式写成函数，行程、速度、加速度等参数作为形参，运种方法程序设计非常简单，但是运动 轨迹解析表达式的求解非常复杂，易出错，S曲线阶数越高越复杂，3阶S曲线有7个分段， 4阶S曲线有15个分段，对于更高阶的S曲线，运种方法是几乎不被采用的。
【发明内容】
[0004] 本发明的目的是为了解决利用传统的轨迹规划算法的位置与速度控制方法的可 移植性差的问题，本发明提供一种基于高阶S型运动轨迹的位置与速度控制方法。
[0005] 本发明的基于高阶S型运动轨迹的位置与速度控制方法，所述高阶S型运动轨迹 为N阶S曲线；所述控制方法包括如下步骤：
[000引步骤一：待规划的N阶S曲线共有2^个分段点，撕i为各阶S曲线限定值，表示位 置的第n阶导数最大值，n= 0,1，，N;根据第N阶S曲线的限定值巧f"，求取第N阶S曲线 在各分段点的值SW比]，k= 0,1，2,，;
[0007] 步骤二：求取第k个分段点对应的时间值T比]和第n阶S曲线在第k个分段点的 值S(n)比]，k= 0，l，2，，2N-l，n= 0，1，，N-1 ;
[0008] 步骤S:对确定T比]和SW比]的S曲线进行采样，获得规划后的N阶S型运动轨 迹；
[0009] 步骤四：根据规划后的N阶S型运动轨迹，利用位置的采样值、位置的第1阶导数 的采样值分别对位置与速度进行控制。
[0010]所述步骤一中，根据第N阶S曲线的限定值巧，求取第N阶S曲线在各分段点的 值SW比]的方法为：
[0011]
[0012] S(N)[1]=0;
[0013]根据S(N)[2ii+i] =H)*S(N)[i];j= 2，. . .，N，i= 0，1，. . .，2!1，确定当k= 2，， 2^-1时的SW比]。
[0014] 所述步骤二中，求取第k个分段点对应的时间值T比]和第n阶S曲线在第k个分 段点的值SW比]的方法为：
[001 引（1)当k= 0 时：
[0016] S(n)[0] = 0,n= 0，1，. . .，N-1 ;
[0017] T[0] = 0 ；
[001 引 似当k= 1，2,. . .jN-l时：
[0019]首先，求取T比]的值：
[0022] T[3] =T[l]+T[2]；
[0023]k> 4 时，
[0024]根据
和T[2W] =T[2]巧山，j= 2，， N-1，i=l，，2.i-l，获得T比];
[002引根据获得的T比]、T比-1]和SW比-1]，获得第n阶S曲线在第k个分段点的值SW比]:
[0026]
[0027]其中，t=T比]。
[0028] 所述步骤S中，对确定T比]和SW比]的S曲线进行采样，获得规划后的N阶S型 运动轨迹的方法为：
[0029]根据采样频率f，求取各阶S曲线在第1 (1 = 1，2,...）个采样点的值：
[003引当巧竭《J冶rf透兹-1;!时，首先从T[o]至T[2M]判断如属的分段区间，对于其 所
[0033]属的区间段
各阶S曲线在f处的采样值为：
[0036] 当1 = 1至1 =T巧W-1]xf+1，共T巧W-1]xf+1个采样点的值均求取完成后，N阶 S型运动轨迹规划完成。
[0037] 本发明的有益效果在于，本发明针对给定行程、速度、加速度及加速度各阶导数限 定值的情况下，提出了一种高阶S型运动轨迹规划算法，该算法可移植性强，可应用于任意 阶S曲线的轨迹规划，具有较强的使用价值。
【附图说明】
[0038] 图1为【具体实施方式】一所述的控制方法的流程示意图。
[003引图2为4阶S曲线的示意图，Sm、Vm、Am、Jm和Dm所标注的5条曲线依次表示位置、 速度、加速度、加速度一阶导数、加速度二阶导数的轨迹。
【具体实施方式】
[0040]结合图1和图2说明本实施方式，本实施方式所述的基于高阶S型运动轨迹的位 置与速度控制方法，本实施方式高阶S型运动轨迹为4阶S曲线，4阶S曲线位置、速度（位 置一阶导数）、加速度（位置二阶导数）、加速度一阶导数（位置=阶导数）和加速度二阶 导数（位置四阶导数）的限定值分别为式。巧f.)、K，，议！i)、(货1)、人，(巧門和公
[00川 4阶S曲线共有24= 16个分段点，设第k个分段点对应的时间值为T比]，各阶曲 线在第k个分段点的值分别为S比]、V比]、A比]、J比]和D比]，k= 0，1，2，，15 ;
[0042] 所述方法包括如下步骤：
[004引步骤一：根据加速度二阶导数A,(识'），求取D比]:
[0044] D[0]=Dm;
[0045] D[l]=0；
[0046]根据S(N)[2"+i] = H)*S(N)[i];其中j= 2，...，N，i =0，1，...，2"，确定D= 化，0, -Dm, 0, -Dm, 0,Dm, 0, -Dm, 0,Dm, 0,Dm, 0, -Dm, 0};
[0047]步骤二：T比]、S比]、V比]、A比]、J比]，k= 0，1，2，，15:
[0048] (1)当k= 0时：
[0049]T[0] =S[0] =V[0] =A[0] =J[0] = 0；
[0050]似当k= 1，2, . . .，15 时：
[0051] 首先，求取T比]的值：
[0052] T[0] = 0 ;
[005引T山=Jm/Dm;
[0054]T巧]=Am/Jm;
[005引T閒=T凹巧山；
[005引TW] =T閒+ (Vm-2V[3]) /Am;
[0057] T閒=TW]巧山；
[0058]T[6] =T[4]巧巧];
[005引T[7]=TW]巧巧];
[0060]T閒=T[7] + (Sm-2S[7])Am;
[006。T脚=T閒巧山；
[0062] T[10] =T巧]+T[2];
[006引T[山=T閒巧閒；
[0064] T[12] =T閒巧[4];
[006引T[13]=T閒巧巧];
[0066]T[14] =T閒巧[6];
[0067]T[15] =T閒巧[7];
[006引根据获得的T比]、T比-1]和SW比-1]，获得第n阶S曲线在第k个分段点的值SW比]:
[0069]
[0070]其中，t=T比];
[0071] 步骤S:对确定T比]和SW比]的S曲线进行采样，获得规划后的N阶S型运动轨 迹：
[0072] 根据采样频率f，求取各阶S曲线在第1 (1=1，2,...）个采样点的值：
[0073]当y<r脚时，d=j=a=v=s=0;
[0074]当y〉r[巧时，d=j=a = V = 0, s =Sm;
[0075] 当巧0! < ^ < 了[2"-非时，首先从T[0]至T巧w-u判断^所属的分段区间，对于其 所属的区间段f运約域巧衾木销，各阶S曲线在I处的采样值为：
[0076]d=D比];
[0081] 当1 = 1至1 =T[1引Xf+1，共T[1引Xf+1个采样点的值均求取完成后，4阶S 型运动轨迹规划完成，如图2所示；
[0082] 步骤四：根据规划后的N阶S型运动轨迹，利用位置的采样值与位置的第1阶导数 的采样值分别对位置与速度进行控制。
【主权项】
1. 基于高阶S型运动轨迹的位置与速度控制方法，其特征在于，所述高阶S型运动轨迹 为N阶S曲线；所述控制方法包括如下步骤： 步骤一：待规划的N阶S曲线共有乂个分段点，笔Γ*为各阶S曲线限定值，表示位置的 第η阶导数最大值，η = 0, 1，，N ;根据第N阶S曲线的限定值，:求取第N阶S曲线在各 分段点的值 S(N) [k]，k = 0, 1，2,，2Ν-1 ; 步骤二：求取第k个分段点对应的时间值T[k]和第η阶S曲线在第k个分段点的值 S(n) [k]，k = 0, 1，2,，2Ν-1，η = 0, 1，，N-I ; 步骤三：对确定T[k]和S(n) [k]的S曲线进行采样，获得规划后的N阶S型运动轨迹； 步骤四：根据规划后的N阶S型运动轨迹，利用位置的采样值、位置的第1阶导数的采 样值分别对位置与速度进行控制。2. 根据权利要求1所述的基于高阶S型运动轨迹的位置与速度控制方法，其特征在于， 所述步骤一中，根据第N阶S曲线的限定值，求取第N阶S曲线在各分段点的值S (N) [k] 的方法为：S(N)[1] = 0 ; 根据 S(N)[2.jl+i] = (_l)*S(N)[i] ;j = 2，...，N，i = 0，1，...，2"，确定当1^ = 2，，2[1 时的 S(N)[k]。3. 根据权利要求2所述的基于高阶S型运动轨迹的位置与速度控制方法，其特征在于， 所述步骤二中，求取第k个分段点对应的时间值T [k]和第η阶S曲线在第k个分段点的值 S(n) [k]的方法为： (1) 当 k = 0 时： S(n) [0] = 0, η = 0,1，· · ·，N-1 ; T[0] = 0 ； (2) 当 k = 1，2, · · ·，2N-1 时： 首先，求取T [k]的值：T[3] = T[l]+T[2]； k彡4时， 根据；和了⑵+丨]=!^]』]+!^]，」=]，，. 1，i = 1，，2Μ，获得 T[k]; 根据获得的T [k]、T [k-1]和S(n) [k-1]，获得第η阶S曲线在第k个分段点的值S(n) [k]:其中，t = T[k]。4.根据权利要求3所述的基于高阶S型运动轨迹的位置与速度控制方法，其特征在于， 所述步骤三中，对确定T[k]和S(n) [k]的S曲线进行采样，获得规划后的N阶S型运动轨迹 的方法为： 根据采样频率f，求取各阶S曲线在第1(1 = 1，2，...）个采样点的值：，首先从T[0]至T[2N-1]判断$所属的分段区间，对于其所 属的区间g各阶S曲线在§处的采样值为：当I = 1至I = T[2N-l]Xf+l，共T[2N-l]Xf+l个采样点的值均求取完成后，N阶S型 运动轨迹规划完成。
【专利摘要】基于高阶S型运动轨迹的位置与速度控制方法，属于精密运动控制领域。为了解决利用传统的轨迹规划算法的位置与速度控制方法的可移植性差的问题。所述控制方法包括：一：根据第N阶S曲线的限定值求取第N阶S曲线在各分段点的值S(N)[k]；二：求取第k个分段点对应的时间值T[k]和第n阶S曲线在第k个分段点的值S(n)[k]；三：对确定T[k]和S(n)[k]的S曲线进行采样，获得规划后的N阶S型运动轨迹；四：根据规划后的N阶S型运动轨迹，利用位置的采样值、位置的第1阶导数的采样值分别对位置与速度进行控制。本发明用于运动机构位置与速度的控制。
【IPC分类】G05B19/19
【公开号】CN105182906
【申请号】CN201510615768
【发明人】陈兴林, 宋法质, 王一光, 董岳, 刘杨, 陈震宇, 张常江, 万勇利, 刘洋, 何良辰
【申请人】哈尔滨工业大学
【公开日】2015年12月23日
【申请日】2015年9月24日