梯形加速度轨迹ZVD输入整形python仿真实现

介绍

输入整形技术作为一种开环主动抑振技术，在自动化领域的振动抑制方面一直广受关注。指令整形理论最早是由 OJM Smith 于1950年代提出的。Smith 的理论被称作 posicast control，在指令下发给系统之前，将指令拆分成为基本指令和部分延迟指令。其中部分被延迟的指令被用于抵消系统响应中因基本指令而引起的抖动。
根据采用的约束方程及个数的不同，输入整形器可分为 ZV，ZVD，ZVDD，EI等，由于整形器的参数是依赖于对应的系统模态振动参数（频率，阻尼比）的，系统的振动参数往往无法精确获得，不精确的振动参数设计出的整形器对其指令的抑振效果存在影响，此外不同的输入整形器或多或少都会对系统的输入指令中引入延迟。不同整形器的设计一般是考虑引入的指令延迟与对系统参数误差的鲁棒性间的均衡考虑。

梯型加速度轨迹规划指令

梯形加速度形式的轨迹规划，既保证了在固定运动约束限制（位置，速度，加速度，Jerk）下的最快规划方式，同时保证了加速度连续，Jerk有界，相比于角速度跳变的Bang-Bang控制，能够降低引入的振动。这里以梯形加速度规划形式为例进行指令的输入整形

形式如上图所示，具体的理论基础可参见书籍：
《Trajectory Planning for Automatic Machines and Robots》

指令输入整形

输入整形可以看做一系列特定时刻的脉冲信号与原始指令经过卷积运算后进行叠加

为了保证整形后的输入指令幅值没有变化，要求所有的脉冲信号的幅值之和为1.

这里以ZVD输入整形器为例，进行指令处理。该整形器的脉冲序列可根据系统参数计算并描述为：

其中：$T_d$为系统有阻尼振动周期
$T_d=\frac{\pi }{\omega \sqrt{1-{\xi }^2}}$
$K=e^{\frac{-\xi \pi }{\sqrt{1-{\xi }^2}}}$

代码实现

from tg import TgPy
import math
import numpy as np
s1=0
s2=100
vm=50
am=300
jm=5000
Ts=0.004
W=1*2*math.pi
Cr=0.2
### set IS ###

wd=W*math.sqrt(1-Cr**2)
K=math.exp(-Cr*math.pi/math.sqrt(1-Cr**2))
Td=2*math.pi/wd
A1=1/(1+K)**2
A2=2*K/(1+K)**2
A3=K**2/(1+K)**2
A=[A1,A2,A3]
T=[0,0.5*Td,Td]

### shape command trajectory ###
q=TgPy().doubleSVel(s1,s2,vm,am,jm,Ts)
sumT=t=q[-1,-1]
dataList=[]
originList=[]
tList=[]
print("total run time ={}".format(q[-1,-1]))
for t in np.arange(0,sumT+Td+500*Ts,Ts):
    t1=t-T[0]
    t2=t-T[1]
    t3=t-T[2]
    if t1<0:
        t1=0
    if t2<0:
        t2=0
    if t3<0:
        t3=0
    data0=TgPy().calculateInterpolation(q,Ts,s1,s2,jm,t)
    data1=TgPy().calculateInterpolation(q,Ts,s1,s2,jm,t1)
    data2=TgPy().calculateInterpolation(q,Ts,s1,s2,jm,t2)
    data3=TgPy().calculateInterpolation(q,Ts,s1,s2,jm,t3)
    data1[1]=data1[1]*A[0]
    data2[1]=data2[1]*A[1]
    data3[1]=data3[1]*A[2]
    data=np.array(data1)+np.array(data2)+np.array(data3)
    tList.append(t)
    dataList.append(data)
    originList.append(data0)

## plot data ###
import matplotlib.pyplot as plt
dataList=np.array(dataList)
originList=np.array(originList)
originPos=originList[:,1]
originVel=originList[:,2]
originAcc=originList[:,3]
shapedPos=dataList[:,1]
velList=np.diff(shapedPos)/0.004
accList=np.diff(velList)/0.004

tList=np.array(tList)
fig,ax=plt.subplots()
fig.suptitle('trajectory with zvd input shaping')
plt.subplot(311)
plt.plot(tList,dataList[:,1],label='shaped pos')
plt.plot(tList,originPos,label='origin pos')
plt.xlabel('time(s)')
plt.ylabel('deg')
plt.legend(loc='lower right')
plt.subplot(312)
plt.plot(tList,originVel,label='origin vel')
plt.plot(tList[0:-1],velList,label='shaped vel')
plt.xlabel('time(s)')
plt.ylabel('deg/s')
plt.legend()
plt.subplot(313)
plt.plot(tList,originAcc,label='origin acc')
plt.plot(tList[0:-2],accList,label='shaped acc')
plt.xlabel('time(s)')
plt.ylabel('deg/s^2')
plt.legend()
plt.show()

## build system and calculate response
from scipy.signal import lti,step2,impulse2,lsim,lsim2
Ws=W
Crs=Cr
Num=np.array([Ws**2])
Den=np.array([1,2*Ws*Crs,Ws*Ws])
tout1, yout1,xout1 =lsim((Num,Den),originPos,tList)
tout2, yout2,xout2 =lsim((Num,Den),shapedPos,tList)
plt.figure(2)
plt.title('system response')
plt.plot(tout1,yout1,label='origin response')
plt.plot(tout2,yout2,label='shaped response')
plt.xlabel('time(s)')
plt.ylabel('deg')
plt.legend(loc='lower right')
plt.show()

结果

整形前后系统输入指令：

整形前后系统输出响应：