S_T曲线速度规划在定点DSP上的实现

1、Vol. 27 No. 6Jun.2006第27卷第6期增刊2006年6月仪器仪表学报Chinese Journal of Scientific InstrumentS/T曲线速度规划在定点DSP上的实现灰马艳歌2 贾凯I徐方】1（中国科学院沈阳自动化研究所沈阳110016)2（中国科学院研究生院 北京100039) 1994-2012 China Academic Journal Electronic Publishing Huusc. All rights reserved. http：/ki,nctVol. 27 No. 6Jun.2006第27卷第6期增刊2006年6月 1994-20

2、12 China Academic Journal Electronic Publishing Huusc. All rights reserved. http：/ki,nctVol. 27 No. 6Jun.2006第27卷第6期增刊2006年6月摘 要 本文将S/T曲线速度规划的思想引入全数字何服驱动系统中，通过提高速度的平滑性待别是高速启、制动状态来 提高何眼系统的整体控制性能.基于定点数字信号处理器DSP芯片对提出的算法进行了实现.由于定点运算的限制，算法 在实现中需要进行特殊的处理,本文对此进行了研究，并提出了一种余码补偿方芙.实验研究表明，使用本文提出的方法可 以提高系统运行的平稳

3、性和控制的精确度.关询 S曲线速度规划DSP定点运算余码补偿Implementation of S/T curve velocity profile based on fixed-point DSPMa Yange12 Jia Kai* Xu Fang1 Li Xiao*1 (Shenyang Institute of Automation Chinese Academy of Sciences Shenyang 110016 9China)z (Graduate School of Chinese Academy of Sciences Beijing 100039 C/i:na)Abstr

4、act This paper introduces the idea of S/T Curve Velocity Profile to full digital servo drive system. The proposed method improves the system control performance by improving the smoothness of velocity t especially the starting and braking period in high speed. It is based on fixed-point DSP chip. Du

5、e to the restriction of fixed-point operation of the chip, special treatment is needed to implement the method. The paper studies and proposes a remaining velocity compensation method. Finally the results of experiments show that the proposed method can increase the system smoothness and control acc

6、uracy. 1994-2012 China Academic Journal Electronic Publishing Huusc. All rights reserved. http：/ki,nctVol. 27 No. 6Jun.2006第27卷第6期增刊2006年6月Key words S-curve velocity profile DSP fixed-point operation remaining velocity compensation核心，因此定点运算的限制使速度规划算法的实现具 有一定的特殊性，在深入研究的基础上,提出了一种余 码补偿方案。实验结果表明，使用本文提出的

7、方法可 以达到理想的控制性能。1引 言对于一个高精度的运动控制系统来说，起动和停 止时不允许产生冲击、超程或振荡，因此需要设计专门 的加滅速控制規律，驱使加给电机的输入按照这个规 律变化，从而使系统运行平稳，控制精确度得到提高. 目前速度规划的基本方法是梯形速度曲线（以下简称 T曲线）和S形速度曲线（以下简称S曲线）方法。采 用这些方法可以使速度得到很好的平滑，从而避免柔 性冲击。本文将S/T曲线速度规划的思想引入了全 数字伺服驱动系统。由于全数字伺服驱动系统采用数 字信号处理器DSP芯片TMS320LF2407A作为控制2 S/T曲线加减速原理与简化的S/T曲 线速度规划2.1 T曲线加减速

8、原理T曲线是工业界广泛采用的形式，它是一种时间 最优的曲线。一般情况，曲线加速和减速的过程是 对称的，设给定速度上限为V”,加速度上限为如，被控 对象从A点运动到B点,要求生成的轨迹在这些条件 下时间最优，设加速时间长度为T.。如图1（a）所示, 1994-2012 China Academic Journal Electronic Publishing Huusc. All rights reserved. http：/ki,nctVol. 27 No. 6Jun.2006第27卷第6期增刊2006年6月 1994-2012 China Academic Journal Electronic

9、 Publishing Huusc. All rights reserved. http：/ki,nctVol. 27 No. 6Jun.2006第27卷第6期增刊2006年6月基金项目：中国科学院知识创新工程重大资助项目（KGCX-SW-15） 1994-2012 China Academic Journal Electronic Publishing Huusc. All rights reserved. http：/ki,nctZ366仪器仪表学报弟27卷v(t) =a(ntv. tWb+Te(6)对应关系如下：Ta=T,(1)在不考虑最大速度限制的条件下，到达B点所用 的最短时间为：v

10、=v04-amT.=T. = -(2)am设全程所用时间为T-如果考虑最大速度限制 V,h会相应的堆大。设V为最短时间下系统应该达 到的最大速度，有：gamT.(3)若VWVm，即没有超过速度上限，则Tw = T.8若 VVm，则系统在最大速度时经历的时间为：Tm = =(4)V(n这样系统到达给定速度时经历的时间为：1； =+ 几=二 + 匚：(5)S vm由此可以建立T.和几两段时间的速度曲线 方程：2.2 S曲线加減速原理S曲线是另一种应用广泛的加速曲线，特点是速度变化平滑，在接点处的位置、速度和加速度皆具有连 续性叭 正常情况下的S曲线加/减速如图1(c).(d) 所示。运行过程可分为

11、3段:加加/砂速段、恒加/减速 段、减加/减速段。假定规划的逵度为v,速度上限为V,加速度上限 为纭，引入加加速度上限L，要求生成的轨迹在这些上 限的约束下时间最优。考虑最一般的情况，设加加速 度的时间为T,恒加速度时间为T.,恒速度时间为T- 运行过程中的3段变化时间关系由式(7)给岀：(7)T. = T2文献3中将S曲线速度规划分4种情况进行了 讨论。悄况一:不考虑加速度限制和速度限制，仅考虑 加加速度的限制;情况二：考虑速度上限和加速度上 限I情况三:最大加速度出现在图1(c).(d)中C点对 应的时刻:情况四;考虑速度上限，最大速度出现在D 点对应的时刻。详细分析了各种情况下的运行过程

12、。 完整的S曲线速度规划的运算量是比较大的，在离实 时性的仝数字伺眼驱动系统中，速度闭环的控制周期 小于1ms,在速度規划阶段不允许有过于复杂的算法， 因此本文对S曲线速度规划算法进行了简化。d(l)S曲财1速b(2)S曲咖I图1 S/T曲线加截速规划2.3简化的$曲线速度规划算法在简化算法中，对完整的S曲线速度规划算法进 行了3个方面的简化：(1) 不考虑位置因索因为在设计的全数字伺服 驱动系统中，位置由位置闭环来控制，所以只考虑最短 时间内速度由起点速度达到终点速度vt；(2) 不考虑最大速度限制在所设计的伺服驱动 系统中，速度的给定值均小于系统允许的最大速 度vj(3) 加加速度j为恒定

13、值,加速度a初值为0。这样只需要确定两个基本的系统参数:加速度上 限值am和加加速度值j便可以确定整个运行过程。其 中加速度上限值反映了系统的最大加减速能力，加加 速度反映了系统的柔性，该参数与加加速度的时间E 成反比，若取大，则冲击大，极限情况下取无穷大,S曲 线加减速退化为直线加滅速。若取小，则系统的加减 速过程时间长，可以根据系统的需要及性能进行选取。 根据习惯，一般通过选取时间常数间接确定j,他们 之间定最的关系为：j = y(8)提出的算法中当给定的终点速度不等于起点速 度时，开始进行速度规划，若v.vc,经加加速段、匀 加速段、减加速段完成速度規划；反之，若V.v时的情况。根据以上

14、假设可以得到加加速度j、加速度a和 速度v的计算公式： 1购42012 China Academic Journal第6期增刊S/T曲线速度规划在定点DSP上的实现2367第6期增刊S/T曲线速度规划在定点DSP上的实现2367(1) tXKt】时,S曲线加加速段 ra(t)=j(tto) 卜(t) = v + *j(t-(2) t1t2时,S曲线匀加速段 a(t)=j(t1-to)=jT,=jTj = af# v(t) = v(t1)+am(t1!(3) t2tt3.S曲线滅加速段 a(t) = amj(t12)v v(t) = vf yj(t-t2-Tj)22.4 S曲线速度规划中的特殊悄

15、况根据几何知识，可以求得恒加速段/恒减速段的运 行时长，不妨以恒加速段为例：(10)(11)T二 一 -T戶 VeFo+aHT.+TJ(12)当T小于0,即VeVvHTj时，按照T形曲线进 行规划这里给岀图1(a)中恒加速段v的计算公式： aM7(t)= vo4-am(t-to)(13)3提出算法在定点DSP中的实现及实验结 果与分析3.1余码补偿TMS32OLF24O7A为16位定点微处理器,定点处 理器不能直接处理小数。在软件实现时用Q8格式 来表示数的定标，实现定点中的浮点运算提高数据运 算的精度，但是在时间，如T.和b的计算中，可能会存 在舍尾的现象，导致在规划结束时，终点速度小于v“

16、 以式(13)为例，设为0,人为100,a”为30,则匕进行 定点运算的结果为3,速度规划的结果v(3)为90,小于 终点设定速度。这里将规划终点速度和设定终点速 度的差值称为余码。余码的存在在高精度控制中是不允许的,本人通 过研究实验提出了一种补偿方案。在S曲线速度规划 中，以图1(c)的情况为例加以说明。由S曲线的连续性和对称性可知,AB段和CD段 的速度增量是相同的，设为Av,则，v=*jT；(将余码在恒加速段进行补偿。恒加速段的速度增Ti = VLZ2zlAy_T(若(ve-v0-2Av)不能整除a.,则将T加1,并将 恒加速段厳后时刻的速度值设为(ve-Av)#开始便J.15 眦T,

17、=T,+Ive.vc.AV4 YN严巧(讷v-ve1r图2软件设计流程图3.2软件实现综合以上所述，将本文所提出的算法和补偿方案 在TMS320LF2407A微处理器的编程环境CCS中进 行了实现。设前一时刻的速度值为pre.veL软件流 程如图2所示。3.3实验结果与分析图3所示为在VC卄环境中，使用16位定点运 算处理的方法得到的一组数据的仿真结果。图3(a)初 始速度为0,终点速度乂为1000,加速度上限值a,为 32,时间常数卫为10。图3(c)为终点速度v设为100 时规划结果，二者均用到了余码补偿方案。另外，图3 (b)和图3(d)给出了对应于加速情况的截速时的补偿 曲线。本算法在

18、全数字伺服驱动系统中进行了实验和应 用。图4所示为在速度闭环使用本文中提出的算法后 得到的实验结果.显然，使用速度规划算法不仅能够 使速度平滑过渡，而且能够使控制性能得到提高.第6期增刊S/T曲线速度规划在定点DSP上的实现2367量为(vt-v0-2Av),由此可得恒加速段的运行时本文对S/T曲线速度规划算法进行了深入研究,C hirw Academic Journal Electronic Publishing House. All rights reserved, 2368仪器仪表学报第27卷 1994-2012 China Academic Journal Electronic Pub

19、lishing House. All rights reserved, http. ki.nct2368仪器仪表学报第27卷200313201032 42t/ms询 Mt的S曲畑馄划01032 42(b滞曲的S曲袋加速规划 4020 /01234trms曲颇速规划25002000K1VTook妙601401tzms图3带补偿的S/T曲线规划500500并在全数字伺服驱动系统中进行了实验和应用，纵观 全文，具有两个创新点：(1) 将S/T曲线速度规划算法的思想引人全数字 伺服黑动系统的速度闭环控制中，通过提高速度的平 滑性，提高了系统的控制性能；(2) 针对16位定点DSP微处理钱在运算中的限 制，提出了一种余码补偿方案，并通过实验检验了该方