CN101114166A - 一种复杂轨迹的轮廓控制方法 - Google Patents

Abstract

本发明公开了一种复杂轨迹的轮廓控制方法，该方法结合一种具有轮廓误差预补偿功能的交叉耦合控制框架，对参与伺服运动的各轴建立自适应数据模型，根据当前目标位置点和若干历史位置点值，确定伺服被控对象待辨识参数，通过极点配置算法实时整定控制参数。这种根据历史控制量和未来控制量对当前控制输出加以调整的方法，有效地抑制了有界过程干扰，提高了轮廓控制的精度及过程的稳定性。

Description

一种复杂轨迹的轮廓控制方法

技术领域

本发明涉及一种轮廓控制方法，特别是一种复杂轨迹的轮廓控制方法。

背景技术

现有提高轨迹运动精度的方法主要有两种：1.通过分别提高各单轴轨迹伺服跟踪控制性能，间接减小各轴合成后的轨迹轮廓误差；2.通过协调处理各单轴伺服动态，以提高平面轨迹轮廓控制性能为目标，兼顾单轴伺服动态特性，直接减小两轴平面轨迹轮廓误差。

通常意义上的轮廓控制是在不改变单轴伺服控制结构的基础上，通过对各伺服轴间信息的共享处理，即采用交叉耦合控制策略，向各轴提供附加的轮廓误差信息，实现轨迹跟踪的闭环控制。不采用前者是因为：当其中一个轴受到扰动影响的时候，其他轴并未得到相应的反馈信息，仍然认为其是正常工作的而未采取有效的补偿措施以减少轮廓性能的恶化。对于不同的跟踪轨迹，如非线性轮廓及大曲率转角，单轴伺服跟踪控制也未考虑几何特性对轨迹跟踪过程所产生的影响。

Koren于1980年提出的基于时间等态方法的交叉耦合控制(Cross-Coupled Control，简称CCC)，是最基本的处理多轴耦合控制的方法。其主要思想是通过获取各轴跟踪误差信息，实时估计轮廓误差，设计耦合控制律对轮廓误差进行反馈补偿，将补偿量解耦到各轴上，得到轮廓误差的线性计算模型ε＝-C_xE_x+C_yE_y，如图10、11所示。由于采用的是P型控制器u_c＝w_pε，其不能控制非线性轮廓。继而Koren等改善P(比例)型控制器，提出可变增益的非线性轮廓控制，如：圆弧，采用直线逼近的方式，则依照不同的增益系数解耦分配轮廓误差，从而独立控制各单轴。这种基本的交叉耦合控制器在固定的坐标系下估算跟踪直线或圆弧的轮廓误差，所得轮廓误差为各轴跟踪误差的线性组合，补偿器采用固定参数的P型或PID型控制器，所以仅在低速或曲率变化不大的情况下，较单轴跟踪控制在轮廓精度方面有较大提高。

另外，为了降低轮廓运动耦合控制的计算复杂性，提高耦合控制变增益的稳定性，增强耦合控制器的鲁棒性，T.C.Chiu等人于1998年设计出了基于轮廓误差传递函数(ContourError Transfer Function，简称CETF)的控制方式。其描述了由耦合控制***产生的轮廓误差与未解耦***产生的轮廓误差之间的动态关系，从而把CETF看成是一个等价***的传递函数，把交叉耦合控制器转化为一个等价时变的单输入单输出***进行设计。

基于轮廓误差传递函数的交叉耦合控制器设计，本质上还是传统交叉耦合控制的***化设计方法，通过把轮廓误差增益C_x、C_y近似在[-1，1]范围内，采用定量反馈技术(QFT)设计等价的单输入单输出***，可以得到满***叉耦合增益变化的稳定性与鲁棒性的交叉耦合控制补偿器。

此外，于1999年Chin J.H.等人改变一般轮廓误差计算基于笛卡儿坐标系的设置，将***动力学方程变换到任务坐标系(Task Coordiante Frame，简称TCF)上，通过在各单轴环节附加前馈预补偿器使各轴的动态特性匹配，从而使在任务坐标系下***的传递函数矩阵是对角化的，用法向误差分量近似轮廓误差，即可把交叉耦合控制器的设计简化为两个独立单回路控制器设计，实现对法向误差分量与切向误差分量的解耦控制。这种耦合控制器是基于频域的方法设计的，通过坐标变换保持***特性不变，且假定轨迹轮廓是微段直线构成，则该方法限制于低角频率的轮廓运动与分段线性的参考轮廓下使用。其本质是用任务坐标系上的误差法向分量来近似轮廓误差，着重提高法向误差分量的带宽，避免因无选择的提高各轴单方向的带宽而带来的***潜在不稳定因素的影响，同时考虑了速度与曲率方向的影响，作为控制器前馈信息对***动力学性能予以补偿，从而可以提高***在高速大曲率下的轮廓性能。除了直线和圆，对于任意曲率变化的复杂几何特征平面曲线建立TCF，在线计算量较大；用TCF标架下的法向误差分量近似代替轮廓误差，未能可靠实现轮廓误差解耦控制；当然，控制器对于被控对象模型不确定性和外部扰动的处理能力也较弱。

其他应用于轮廓控制的方法，如无源耦合控制器设计，是一种基于Lyapunov函数的CCC控制律来实现连续轨迹运动中的多轴协调控制。通过调整权值对轮廓性能与跟踪性能进行不同的配置，采用积分Backstepping技术设计控制律，该控制率实际为包含参考轨迹的速度加速度信息的时变PD控制，可实现局部稳定性，通过增加权值以牺牲跟踪性能为代价提高轮廓性能。

显然，基于无源性CCC设计的本质是把最优能量控制的概念引入到控制目标设计中，通过调整设定能量函数的权值以适应不同的控制需求。在实际应用过程中，对于复杂轮廓曲线，不易找到其隐式表达方式，且在误差接近于零时由于控制率的不连续切换会降低轮廓性能，则在很大程度上限制了其应用的范围。

本发明针对平面或空间复杂轨迹的轮廓控制问题，提供了完备的解决方案，并提供了超出现有技术的其他优点。

发明内容

本发明即是为适应工业生产中平面/空间轮廓控制领域，不断提高的廓型精度要求及可靠控制稳定性需求，而设计出的针对复杂轨迹轮廓控制过程的高性能伺服控制算法。目的在于：从轮廓误差和轮廓控制稳定性两方面考虑，基于复杂几何特征轨迹跟踪控制，提出一种高精度、高稳定性的轮廓控制方法。

实现本发明的技术方案，包含以下具体步骤：

(1)对位置伺服***建模，即进行位置控制***的模型辨识，确定模型阶次、得到位置控制***传递函数、线性回归数据模型及其待辨识的模型参数J_eq、B_eq；

(2)设计基于PD型的位置控制器，根据辨识所得的被控对象伺服特性，确定稳定的控制极点，实时调节控制器参数，以获得微超调、无相位滞后的稳定控制器；

(3)将插补进给率与增量式位置输出值比较得到的伺服延迟输入到位置控制器中，得到实时控制量的输出；

(4)如图3所示，根据控制器硬件FIFO队列中存储的插补数据，采用运动命令队列输出缓冲机制，基于步骤(1)辨识获得的数据模型，产生预见前馈输入；由本发明所设定的自适应预见控制器APC，对输入量301、伺服延迟302、控制量304进行处理，存在外部扰动306的情况下，得到稳定的位置输出308；

(5)如图13所示，对各伺服轴输出位置值进行交叉耦合处理，计算实时耦合轮廓误差1330，通过耦合补偿控制器1350、1350’，对速度环内控制量一次修正；经过设定的预估补偿增益处理在1320进行解耦计算，计算结果经过v_kx、v_ky处理，实现进给率的实时修调；

(6)在步骤(5)的基础上，结合步骤(4)所实现的自适应预见控制器，如图14，各轴自适应预见控制器1410对各伺服轴的控制量U_x、U_y输出做进一步调整。根据历史位置数据及外部干扰信息改善实时控制量的稳定输出；轴间协调控制采用交叉耦合预补偿机制1330，同时对前端输入进给率1315及控制输出量U_x、U_y进行修调处理。

上述步骤(1)所述具体实现方式为：如图1所示，建立平面运动XY轴的线性伺服控制模型。忽略电气时间延迟，确定电机伺服***102、106辨识阶数为2，推导出简化的线性***微分方程模型： $J\stackrel{..}{y}+B\stackrel{.}{y}+F\left(\stackrel{.}{y}\right)+T_1=u,$

其中： $J=\frac{J_1{R}_a}{K_g{K}_T}\·\frac{2\mathrm{\π}}{P},$ $B=\frac{K_T{K}_g+R_{a}D}{K_g{K}_T}\·\frac{2\mathrm{\π}}{P},$ $F_{\mathrm{fn}}\left(\stackrel{.}{y}\right)=\frac{R_a}{K_g{K}_T}{F}_{\mathrm{fn}}^{\′}\left(\frac{2\mathrm{\πy}}{P}\right),$ $T_l=\frac{R_a}{K_g{K}_T}{T}_l^{\′},$ P

为丝杠螺距，y、

分别为丝杠传动滑块的位移、速度和加速度。若先不考虑非线性摩擦项

及外部载荷T_l′，得到从伺服输入参考电压u到位移输出y的传递函数，以J_eq、B_eq为待辨识的模型参数， $G\left(s\right)=\frac{Y\left(s\right)}{U\left(s\right)}=\frac{1}{J_{\mathrm{ep}}{s}^2+B_{\mathrm{ep}}s},$ 其中： $J_{\mathrm{ep}}=\frac{J}{K_t{K}_{\mathrm{\ωp}}{K}_{\mathrm{eq}}R},$ $B_{\mathrm{eq}}=\frac{B+K_{\mathrm{\ωp}}{k}_{\mathrm{eq}}}{K_t{K}_{\mathrm{\ωp}}{K}_{\mathrm{eq}}R}.$ 所进行的扫频辨识实验是对***u端输入持续激励正弦扫频信号，采集输出端y的位置信息，即可得到从参考电压输入端到位置输出端的线性辨识模型。图2所示为竖直轴Z轴的物理模型，Z轴的运动是工件与配重同时运动的动态过程，两者之间由钢丝绳通过两个滑轮传递位移，根据对XY轴的分析，可得Z轴的动态模型为：

$\left\{\begin{array}{c}{m}_1{\stackrel{..}{x}}_1+B_1{\stackrel{.}{x}}_1+B_3({\stackrel{.}{x}}_1-{\stackrel{.}{x}}_2)+K(x_1-x_2)=\frac{\mathrm{\τ}}{R},\\ m_2{\stackrel{..}{x}}_2+B_2{\stackrel{.}{x}}_2+B_3({\stackrel{.}{x}}_2-{\stackrel{.}{x}}_1)+K(x_2-x_1)=0\end{array}\right.$ 经拉氏变换后可得： $\frac{x_1\left(s\right)}{\mathrm{\τ}\left(s\right)}=\frac{p_2{s}^2+p_{1}s+p_0}{R^{2}s(s^3+q_2{s}^2+q_{1}s+q_0)},$ 其中： $p_2=\frac{1}{m},$ $p_1=\frac{B_2+B_3}{m_1{m}_2},$ $p_0=\frac{K}{m_1{m}_2},$ $q_2=\frac{m_1(B_2+B_3)+m_2(B_1+B_3)}{R{m}_1{m}_2},$ $q_1=\frac{K(m_1+m_2)+B_1{B}_2+B_1{B}_3+B_2{B}_3}{R{m}_1{m}_2},$ $q_0=\frac{K(B_1+B_2)}{R{m}_1{m}_2}$ 即为待辨识的模型参数。

上述步骤(2)所述之具体实现方式为，如图3所示，设定二阶被控对象305： $\left\{\begin{array}{c}A\left(z^{-1}\right)y\left(k\right)=z^{-1}B\left(z^{-1}\right)u\left(k\right)+y_d\\ A\left(z^{-1}\right)=1+a_1{z}^{-1}+a_2{z}^{-1}\\ B\left(z^{-1}\right)=b_0+b_1{z}^{-1}\end{array}\right.,$ 其中：y_d为常值扰动306。采用PD控制器303，设进行极点配置的闭环特征多项式为：A_m(z^-1)，控制器中包含积分环节，取： $\left\{\begin{array}{c}F\left(z^{-1}\right)=(1-z^{-1})(1+f_2{z}^{-1})=1+(f_2-1)z^{-1}-f_2{z}^{-2},(-1<f_2<0),\\ G\left(z^{-1}\right)=g_0+g_1{z}^{-1}+g_2{z}^{-2}\end{array}\right.$ 设

由图4传递函数有：F(z^-1)u(k)＝H(z^-1)r(k)-G(z^-1)y(k)，405为计算得Fu(k)，因此可得控制量410表达式u(k)为：

$u\left(k\right)=\frac{k_D(1-z^{-1})}{1+f_2{z}^{-1}}\&CenterDot;y\left(k\right)+[k_p+\frac{k_1}{(1-z^{-1})(1-f_2{z}^{-1})}]\&CenterDot;[r\left(k\right)-y\left(k\right)],$ 见图5，303经过Z变换后得到PD控制器501，其输出为505。其中待求量为：f₂，g₀，g₁，g₂或f，K_P，K_I，K_D；需要满足的极点配置条件：即闭环特征多项式A_m(z^-1)，从而有：(AF+z^-1BG)y(k)＝z^-1BHr(k)，显然特征方程式为：

(AF+z^-1BG)＝A_m(z^-1)，其中：AF和z^-1BG都是4次多项式，比较同次幂系数可得4个独立方程，从而解出未知量f₂，g₀，g₁，g₂，反解得到K_P，K_i，K_d。将F(z^-1)设成二次多项式，选择Am(z^-1)时取degA_m＝2，A_m＝1+a_m1z^-1+a_m2z^-2，对应的连续二阶多项式为：

$s^2+2\mathrm{\&xi;}{\mathrm{\&omega;}}_{n}s+{\mathrm{\&omega;}}_n^2=(s+s_1)(s+s_2).$

由伺服被控对象特性ξ，ω_n值及设定的伺服采样周期T₀，可得此二次多项式的a_m1，a_m2的值满足下列等式： $\left\{\begin{array}{c}{a}_{m1}=-2\exp (-\mathrm{\&xi;}{\mathrm{\&omega;}}_n{T}_0)\cos \left({\mathrm{\&omega;}}_n{T}_0\sqrt{1-{\mathrm{\&xi;}}^2}\right)\\ a_{m2}=\exp (-2\mathrm{\&xi;}{\mathrm{\&omega;}}_n{T}_0)\end{array}.\right.$ 此时，自适应特性即体现在反应被控对象模型特性的A、B变化时，F、G也同时随动；在被控对象参数在线辨识的基础上，配置稳定的控制极点，自适应整定PD控制参数。

上述步骤(3)所述具体实现方式为：实现运动命令FIFO队列传输机制，如图6所示，由背景程序产生的运动命令依次存放(601)在预设的运动命令缓冲区(即，硬件存储区预设的FIFO队列)内。各命令次序由指针指定偏置位置，设置一基地址，在设定的伺服周期内从队列中依偏置地址的设定，取出(602)插补位置数据，调用运动函数库执行相应的伺服运动任务。

上述步骤(4)所述具体实现方式为：由辨识所得I型伺服***，给出以下二次型加权目标函数， $J=\underset{j=-M_R}{\overset{\&infin;}{\mathrm{\&Sigma;}}}[e^T\left(j\right)\mathrm{Qe}\left(j\right)+\mathrm{\&Delta;}{u}^T\left(j\right)\mathrm{H\&Delta;u}\left(j\right)],$ 其中e(j)与u(j)分别为伺服跟踪误差和实时控制量，Q、H为待设计的加权系数矩阵，z则加入干扰值的控制量计算公式为：u(k)＝F_e∑e(i)+F_xx(k)+F_pr(z)P(k)+F_pd(z)d(k)，其中：

$F_{\mathrm{pr}}\left(z\right)=F_r\left(1\right)z+F_r\left(2\right)z^2+\&CenterDot;\&CenterDot;\&CenterDot;+F_r\left(M_r\right)z^{M_r},$ $F_{\mathrm{pd}}\left(z\right)=F_d\left(0\right)z+F_d\left(1\right)z^2+\&CenterDot;\&CenterDot;\&CenterDot;+F_d\left(M_d\right)z^{M_d},$

则最优预见伺服控制***构成如图7所示。715、725为与预见前馈目标值与预见前馈干扰值。基于单变量的SISO控制***，反馈控制710与前馈控制矩阵715一维时不变向量，则u(k)式中一、二项相同于典型伺服控制***，三、四项体现预见伺服控制***特点。F_pr(z)与F_pd(z)的待辨识过程参数是根据上步的输入、输出、M_R(M_d)阶未来目标值与干扰值所做出最优预见控制动作的。这种预见伺服控制方法应用在实时伺服控制***中解决的最大问题是合理有效使用未来信息的阶数，信息量越大则伺服控制的精度越高，但DSP处理资源消耗也越多，理论上仿真的数据是：预见超过阶数30次也不会影响伺服控制的性能。图8即示出了具体的算法实现，其中301、308为***的输入/输出，302为伺服延迟，与预见前馈干扰值720、反馈控制输出840共同作用于自适应预见控制器830，经过前馈模块820处理，作用于反馈控制器810，整体控制输出为850，作用于被控对象710获得最终的预见位置输出值。图9示出了在图1伺服控制结构下的具体测控流程，其中910为通过电机带编码器反馈的半闭环速度控制控制模式，920为直线伺服机构所装光栅尺反馈之位置控制模式。

上述步骤(5)所述具体实现方式是：首先计算实时轮廓运动误差，如图10所示，1010为跟踪目标轨迹，y_d(k)与y_d(k-1)为跟踪目标轨迹上的相邻两点，y(k)与y(k-1)为实时跟踪位置目标点，则图中几何关系中，ε_k与ε_k-1 1020为目标轨迹轮廓误差，e_k与e_k-1 1030为目标轨迹跟踪误差。相应的，在图11中变换坐标系，在跟踪轨迹1010’下，1102与1105为实时任务坐标系下的切向与法向跟踪误差，相似的，1130’为跟踪误差，此时1102可认同为1020的轮廓误差。由此可得几何关系：

$\left\{\begin{array}{c}{\stackrel{\&RightArrow;}{e}}_k=|y\left(k\right)-y_d\left(k\right)|={\stackrel{\&RightArrow;}{e}}_{\mathrm{kx}}+{\stackrel{\&RightArrow;}{e}}_{\mathrm{ky}}\\ {\stackrel{\&RightArrow;}{\mathrm{\&epsiv;}}}_k=\underset{\mathrm{\&tau;}\&Element;[k_0,k_f]}{\min }\left|\right|y\left(k\right)-y_d\left(\mathrm{\&tau;}\right)\left|\right|\end{array}\right.,$ 若将法向偏差1102等效为轮廓误差进行处理，则有： $\left\{\begin{array}{c}{\stackrel{\&RightArrow;}{e}}_k={\stackrel{\&RightArrow;}{e}}_{\mathrm{kx}}+{\stackrel{\&RightArrow;}{e}}_{\mathrm{ky}}\\ {\stackrel{\&RightArrow;}{\mathrm{\&epsiv;}}}_k={\stackrel{\&RightArrow;}{\mathrm{\&epsiv;}}}_{\mathrm{kx}}+{\stackrel{\&RightArrow;}{\mathrm{\&epsiv;}}}_{\mathrm{ky}}\end{array}\right.\text{\&DoubleRightArrow;}\left\{\begin{array}{c}{e}_{\mathrm{kx}}=x_r-x_m\\ e_{\mathrm{ky}}=y_r-y_m\\ {\mathrm{\&epsiv;}}_{\mathrm{kx}}=-\mathrm{\&epsiv;}\sin \text{\&theta;}\\ {\mathrm{\&epsiv;}}_{\mathrm{ky}}=\mathrm{\&epsiv;}\cos \mathrm{\&theta;}\end{array},\right.$ 其中y(x_m，y_m)，y_d(x_r，y_r)，其中： $\mathrm{\&theta;}\left(k\right)={\tan }^{-1}\frac{f_x\left(k\right)}{f_y\left(k\right)}={\tan }^{-1}\frac{x_r\left(k\right)-x_r(k-1)}{y_r\left(k\right)-y_r(k-1)},$ 即该时刻指令位置轨迹切线与X轴夹角，f_x(k)、f_y(k)为每个伺服周期τ内XY轴的进给率，轮廓误差ε可用下式近似表示为：ε_k＝e_kycosθ-e_kxsinθ，则其中即体现了跟踪误差与轮廓误差的关系。由位控***速度控制模式，I型***速度跟随误差 $e(\&infin;)=\frac{1}{K_v},$ 即： $K_v=\frac{v_{\max }}{e},$ 可得速度环跟随误差计算公式： $E=\frac{v}{K},$ 则可改写轮廓误差一般计算公式为：

$\mathrm{\&epsiv;}=\frac{K_x-K_y}{K_x{K}_y}\&CenterDot;[y_r\left(t\right)-y_r(t-1)]\&CenterDot;\cos \mathrm{\&theta;}\&DoubleLeftRightArrow;\mathrm{\&epsiv;}=\frac{K_x-K_y}{K_x{K}_y}\&CenterDot;[x_r\left(t\right)-x_r(t-1)\&CenterDot;\sin \mathrm{\&theta;}.$

另外，如图12，由几何型值点目标轨迹曲线由参数矢函数描述：r(u)＝{x(u)，y(u)}，令 $x^{\&prime;}=\frac{\mathrm{dx}}{\mathrm{du}},$ $y^{\&prime;}=\frac{\mathrm{dy}}{\mathrm{du}},$ 则：k₀是曲线在r₀点的相对曲率： $k_0={\left[\frac{x^{\&prime;}{y}^{\&prime;\&prime;}+x^{\&prime;\&prime;}{y}^{\&prime;}}{{(x^{\&prime;2}+y^{\&prime;2})}^{3/2}}\right]}_{u=u_0},$ 其中：1010”为任意跟踪迹线，跟踪型值点P_i的坐标为：

距离为：D_i(s)＝[(X_i-X(s))²+(Y_i-Y(s))²]^1/2，轨迹跟踪误差为1230，故可推得任意平面曲线的轮廓误差1220计算式：

在路径预估补偿算法(PM)和交叉耦合(CCC)的基础上，对伺服进给率(一次脉冲粗插补值)及控制量输出进行修调。如图13所示，1305表示的是前端插补器提供的粗插补进给率，1310表示的是X轴输入指令位置，经过一次微分环节得到粗插补的初始进给率1315、1315’，根据伺服输出1380、1380’得到轮廓误差计算值1330，经过速度环调节系数1360之后，由1320解耦于XY轴的进给率1315、1315’输入端，对其进行1325修调，经过一次积分环节得到参考指令位置输入1335。其中，1340为各轴被控对象，1370为各轴位置控制干扰分量；此外，轮廓误差计算1330还对速度内环1350进行一次控制量调整。

上述步骤(6)所述具体实现方式是：设p为当前采样点，e(k)＝R(k)-y(k)，y为***输出，R为目标值，e为跟踪误差，u(k)为被控对象的输入，设F_R＝[F_R1，F_R2，...，F_Rj，...，F_RM]，F_d＝[F_d0，F_d1，...，F_dj，...，F_dM]，控制量u(k)可表示为： $u\left(k\right)=\underset{j=0}{\overset{M}{\mathrm{\&Sigma;}}}{F}_{\mathrm{Rj}}\text{R}(k+j)+F_{\mathrm{dj}}d(k+j),$ 以增量形式表达为：

$\mathrm{\&Delta;u}\left(k\right)=\underset{j=0}{\overset{M}{\mathrm{\&Sigma;}}}{F}_{\mathrm{Rj}}(k+j)+F_{\mathrm{dj}}\mathrm{\&Delta;}(k+j).$ 由图8之预见控制APC模块，可调参数F_Rj、F_dj的自适应控制率，可通过J_p对其求偏导使其沿负梯度方向下降，得到：

$F_{\mathrm{Rj}}=2B_{\mathrm{Rj}}T\underset{k=-M+1}{\overset{p}{\mathrm{\&Sigma;}}}\frac{y\left(k\right)}{u\left(k\right)}R(k+j)\mathrm{Qe}\left(k\right)+\mathrm{\&Delta;R}(k+j)\mathrm{H\&Delta;u}\left(k\right),$ $F_{\mathrm{dj}}=2B_{\mathrm{bj}}T\underset{k=-M+1}{\overset{p}{\mathrm{\&Sigma;}}}\frac{y\left(k\right)}{u\left(k\right)}d(k+j)\mathrm{Qe}\left(k\right)+\mathrm{\&Delta;d}(k+j)\mathrm{H\&Delta;u}\left(k\right),$ 使用过程中，被控对象G_p(k)＝y(k)/u(k)及线性辨识模型是稳定收敛的，否则可能导致输出振荡，图14所示即为在图13交叉耦合预补偿控制的基础上，加入各轴之预见控制模块1410，利用交叉耦合预补偿控制对粗插补一次进给率1315、1315’及控制器1345、1345’输出进行修调，辅以各轴APC模块对控制量的修正，使得交叉耦合控制过程更加稳定、控制结果更精确。

与现有的轨迹跟踪、轮廓控制技术相比，本发明所体现的显著效果是：

(1)通过伺服***线性辨识，得到位置控制数据模型，确定在线辨识模型的参数及自适应律，从而可以得到精确、稳定的预见控制输出。这种方法较常用的无被控对象模型的伺服控制***，控制量输出更稳定、跟踪目标位置输出更精确。

(2)本发明所使用的预见自适应控制律及交叉耦合预补偿控制的方法，尤其适合于复杂几何特征轮廓控制，相比于典型前馈PD位置跟踪算法，在减小轮廓误差方面有良好的表现。

(3)本发明中所采用的预见自适应的实时控制方法，充分利用未来与历史控制信息，采用二次型最优控制方法，与早前提出的一些轨迹轮廓控制算法相比，通过设定合适的自适应预见控制率，可以得到更稳定的控制量输出与更精确的轮廓精度。

通过以下有关示例实施例的详细描述并参考相关附图，可以表征本发明以上特征且使其他优点也显而易见。

附图说明

图1示出了进行轨迹轮廓控制实验的伺服装置。

图2是关于Z轴运动原理的分析图。

图3示出了采用极点配置的方法进行PD控制的控制框图。

图4是带扰动的离散控制***框图。

图5示出了进行PID极点配置的离散控制***框图。

图6示出了前端插补数据传输的队列方式。

图7是预见前馈控制的原理图。

图8示出了带有外界干扰的自适应预见控制框图。

图9示出了基于图1的测控配置流程图。

图10示出了笛卡儿坐标系下的轮廓误差及跟踪误差几何关系。

图11示出了实时任务坐标系下的廓误差及跟踪误差几何关系。

图12示出了具有几何型值点的任意曲线廓误差及跟踪误差几何关系。

图13为一典型的应用交叉耦合轮廓误差预补偿机制的控制结构图。

图14为基于预见控制的交叉耦合轮廓误差预补偿控制结构图。

具体实施方式

一个具体的示例实施例为，参考图1的执行机构，进行的平面复杂轨迹跟踪及轮廓控制实例。

图1的伺服控制机构图，即示出了执行此实例的实际控制平台。102与106分别为XY轴直线运动导轨，以相互垂直的方式连接在一起，且固定于101机构平台支座上；XY轴行程位置信息由光栅尺量测，106的运动部件承载实际工作任务；各运动轴上均装有前后限位开关104及回原点开关。直线运动部件由交流伺服电机105通过伺服放大器驱动滚珠丝杠传动，速度信息由电机编码器返回，位置信息则由103(X轴)光栅尺获得(三轴均配)。竖直轴Z轴109的伺服运动结构相同于XY轴，直线运动轴上搭载运动部件，由于是竖直轴结构，则Z轴的停止靠电机抱闸或加装前后配重，108即是本实验装置所加装的配重，108与Z轴运动部件207以刚性绳索203通过固定于Z轴顶部的同轴线滑轮110、110’相连接，配重108由直线光轴导杆约束运动方向，206为竖直轴传动滚珠丝杠，107为Z轴固定支乘，208、209分别为Z轴驱动电机及编码器。运动机构由伺服控制器(DSP-Based)驱动控制，如图9所示，控制***配置均可依照基本伺服控制模式，其中910回路是速度环编码器信息反馈，920回路是位置环光栅尺信息反馈。

实现本发明所述之高性能轨迹轮廓控制算法，采用位置控制模式。依照以下控制流程即可获得较佳的控制效果和轮廓控制精度。图14所示的双轴交叉耦合控制框图即体现了本发明的基本精神，其核心主要体现了一种基于被控对象模型的实时伺服控制算法。

对各轴被控对象的具体辨识流程是：

首先，构造出各轴的标称模型，其主要由一组线性微分方程表述了轮廓控制工作台的结构动力学特性。辨识被控对象标称模型，需要对伺服机构的机电特性有准确的了解，可利用经验数据/理论公式/测量数据获得这方面最贴切的认识，这类信息用来确定模型的结构和定义模型的参数；

然后，设计伺服***的控制性能指标，使得运行于控制器的控制算法的设计具有可行性、达到最优化；

最后，对模型进行验证，以确保辨识出的模型是具有鲁棒性的控制***模型。其不仅包含***动力学的理论模型，而且包含不确定性描述和噪声描述。对本发明而言，模型验证问题明确地表述成为一种线性时不变***，其具有范数有界构造的不确定性和实验数据，在频域中进行模型验证，确定模型是否与输入输出数据匹配以及控制器是否与模型匹配。

以X轴为例，采用正弦脉冲扫频的方式辨识出从输入参考电压到输出位置的辨识模型，在被控对象输入端持续输入0.1～500Hz、幅值为1.5V的正弦扫频信号，描述为：

k＝0，1，2…，N，其中：Ts为采样周期，k为辨识采样点数，ω为输入角频率，α为输入幅值，

为摩擦力矩补偿项，实时采集输出端的位置信息，在Matlab/Simulink的***辨识工具箱中应用最小二乘的方法对辨识对象进行离线辨识。得到各轴从参考电压输入到位置输出的传递函数。对于竖直轴Z轴的辨识，由于Z轴较XY轴有较大的相位滞后，确定Z轴模型结构的时候需要加一个纯滞后环节，设定合适的时间常数，以同样的方法可得到其在低频段范围内的简化模型。

得到各运动轴的线性辨识模型之后，则需建立与之联系的实时控制模型。本发明采用极点配置的方式对实时位置控制参数进行整定。

设定二阶被控对象 $\left\{\begin{array}{c}A\left(z^{-1}\right)y\left(k\right)=z^{-1}B\left(z^{-1}\right)u\left(k\right)+y_d\\ A\left(z^{-1}\right)=1+a_1{z}^{-1}+a_2{z}^{-1}\\ B\left(z^{-1}\right)=b_0+b_1{z}^{-1}\end{array},\right.$ 其中，y_d为常值扰动，A_m(z^-1)即是进行极点配置的特征多项式，取：

$\left\{\begin{array}{c}F\left(z^{-1}\right)=(1-z^{-1})(1+f_2{z}^{-1})=1+(f_2-1)z^{-1}-f_2{z}^{-2},(-1<f_2<0),\\ G\left(z^{-1}\right)=g_0+g_1{z}^{-1}+g_2{z}^{-2}\end{array}\right.$ 则有：

由于F(z^-1)u(k)＝H(z^-1)r(k)-G(z^-1)y(k)，可得控制量表达式为：

$u\left(k\right)=\frac{k_D(1-z^{-1})}{1+f_2{z}^{-1}}\&CenterDot;y\left(k\right)+[K_p+\frac{K_I}{(1-z^{-1})(1-f_2{z}^{-1})}\&CenterDot;[r\left(k\right)-y\left(k\right)],$ 可见比例和积分控制作用于偏差信号，微分作用用于输出信号y(k)。则相应的控制参数实时更新量为： $\left\{\begin{array}{c}{K}_p=-\frac{g_1+2g_2}{1+f_2}\\ T_d=\frac{(g_1+g_2)f_2-g_2}{g_1+2g_2}\&CenterDot;T_0,\\ T_i=-\frac{(g_1+2g_2)\&CenterDot;T_0}{(1+f_2)(g_0+g_1+g_2)}\end{array}\right.$ 自适应特性体现在反应被控对象模型的A、B变化时，F、G也同时随动。在以上确定***的极点配置PID参数自适应调整的基础上，对被控对象参数在线辨识，从而求取控制量u(k)的步骤如下：

(1)设定控制量初值，读取***的输入、输出数值；

(2)根据被控对象的输入控制量u(k)、输出位置值y(k)估计被控对象数据模型平移算子系数

(3)根据极点配置自适应PID控制算法解出F、G并求出H；

(4)求解出实时u(k)的值。

其中，

的在线辨识方法采用对LTI***的线性参数模型的辨识方法。最后，使用遗忘因子法(RFF)可实时辨识出模型参数：

$\left\{\begin{array}{c}\mathrm{\&Theta;}(k+1)=\frac{P\left(k\right)\mathrm{\&psi;}(k+1)}{\mathrm{\&lambda;}+{\mathrm{\&psi;}}^T(k+1)P\left(k\right)\mathrm{\&psi;}(k+1)}\\ P(k+1)=\frac{1}{\mathrm{\&lambda;}}(P\left(k\right)-\mathrm{\&Theta;}(k+1)\mathrm{\&psi;}(k+1)P\left(k\right))\\ \widehat{\mathrm{\&theta;}}(k+1)=\widehat{\mathrm{\&theta;}}\left(k\right)+\mathrm{\&Theta;}(k+1)(y(k+1)-{\mathrm{\&psi;}}^T(k+1)\widehat{\mathrm{\&theta;}}\left(k\right))\end{array}\right.,$ 其中Θ(k)与P(k)为中间变量，λ为遗忘因子选择接近于1的小数，若λ＝1则表示同等处理以往数据。

图6示出了运动控制命令队列操作的功能流图。在本发明中使用队列操作的意图在于为了解决并不均衡的插补迭代计算时间，将插补计算任务放置于DSP的背景处理程序中，从而减小实时伺服控制任务，保证位置伺服控制任务的硬实时特征。具体实现方式是，在背景处理程序中，每计算得一个位置目标点，均传送至伺服指令中，作为一笔运动控制指令存放入FIFO中(函数调用)压栈，即601操作。每个伺服周期来临之时，均需从FIFO中取得一笔运动指令执行伺服操作，即602操作，弹栈。由于背景程序中需要管理若干作业任务，则在作业任务管理之外的硬件资源全部调配给复杂曲线的插补计算之用。为确保运动的连续性，则需保证FIFO队列栈中有足够的运动控制指令供伺服调用，在运动控制器的DRAM中或片外RAM开辟一段空间，实现运动指令的动态存储过程。设定存储量上、下限，在运动开始阶段首先确保FIFO队列中有不低于下限容量的指令数量，方可启动整个伺服运动，在运行过程中定时监控栈内容量，一旦存储容量超过上限则停止插补动作，仅在每个伺服周期提供给实时运动控制指令。FIFO队列栈上、下限的确定及FIFO队列栈的长度设定需视插补计算任务复杂性而定，但其基本原则则是不能在实时伺服控制过程中中断运动指令的供给。在本发明中，FIFO栈容量设定为1K×16字节，上限即是队列的栈顶，下限设定为队列容量的25％。若队列内运动控制指令容量小于最低容限，则需增加插补周期以提高运动命令容量。

进行轨迹轮廓控制采用图13所示的交叉耦合控制方法，实时计算轮廓误差1330，将其作为反馈值对控制量进行调整。轮廓误差反馈值的补偿，一般情况是作用在速度环反馈，对被控对象的控制量进行校正，即将1330通过耦合补偿控制器(图13仅包含比例环节)1350及1350’作用于位置控制器1345及1345’输出端，1345及1345’体现了运动轴的伺服动态，简化成为比例环节处理。耦合控制器1350及1350’的值根据1345及1345’的匹配特性确定。其中耦合轮廓误差1330的计算由图10和图11所示的平面几何关系求取，在发明步骤(2)中有较详细的理论推导，具体实施例采用公式：

$\mathrm{\&epsiv;}=\frac{K_x-K_y}{K_x{K}_y}\&CenterDot;[y_r\left(t\right)-y_r(t-1)]\&CenterDot;\cos \mathrm{\&theta;}\&DoubleLeftRightArrow;\mathrm{\&epsiv;}=\frac{K_x-K_y}{K_x{K}_y}\&CenterDot;[x_r\left(t\right)-x_r(t-1)]\&CenterDot;\sin \mathrm{\&theta;},$ 或公式：

参数说明可对照图13。速度环比例增益K_px及K_py的确定参照前述自适应比例增益系数K_p、T_i、T_d的方式获得，由一般增量型PID控制律采用梯形积分近似法，得到 $\left\{\begin{array}{c}{p}_1=K_p\&CenterDot;(1+\frac{T_d}{T_0}+\frac{T_0}{2\&CenterDot;T_i})\\ P_2=-K_p\&CenterDot;(1+\frac{2T_d}{T_0}-\frac{T_0}{2\&CenterDot;T_i})\\ p_3=K_p\&CenterDot;\frac{T_d}{T_0}\end{array}\right.,$ 从而得到各运动轴的PID自适应控制律u(k)＝u(k-1)+p₁e(k)+p₂e(k-1)+p₃e(k-2)。K_εx及K_εy参数需根据不同运动轴的伺服特性和轨迹轮廓几何特征来设定。

若将轮廓误差反馈值补偿作用于位置环，可应用轮廓误差预补偿机制，设置预估解耦增益值1360，通过各轴误差解耦计算1320，对各运动轴的粗插补进给率进行修调，K_v值的设定可参照：

其中ρ为曲率。

这样即可体现本发明之基本耦合补偿精神。

图14示出了一种包含各单运动轴预见前馈控制的交叉耦合预补偿运动控制策略。基于步骤(1)所辨识出来的位置控制模型，将其转化为伺服***的状态空间模型， $\left\{\begin{array}{c}x(k+1)=\mathrm{Ax}\left(k\right)+\mathrm{Bu}\left(k\right)+\mathrm{Ed}\left(k\right)\\ y\left(k\right)=\mathrm{Cx}\left(k\right)\end{array}\right.,$ 其中A_x、B_x、C_x、A_y、B_y、C_y的值可依照***线性传递函数确定。式中：d(k)为已知干扰信号，在满足可控性可观性条件时，设定目标值信号为R(k)，则误差信号为：e(k)＝R(k)-y(k)，进而推导出以下误差***方程：

$\left[\begin{array}{c}e[k+1]\\ \mathrm{\&Delta;x}[k+1]\end{array}\right]=\left[\begin{array}{cc}{I}_M& -\mathrm{CA}\\ 0& A\end{array}\right]\left[\begin{array}{c}e\left[k\right]\\ \mathrm{\&Delta;x}\left[k\right]\end{array}\right]+\left[\begin{array}{c}-\mathrm{CB}\\ B\end{array}\right]\mathrm{\&Delta;u}\left(k\right)+\left[\begin{array}{c}{I}_M\\ 0\end{array}\right]\mathrm{\&Delta;R}(k+1)\left[\begin{array}{c}-\mathrm{CE}\\ E\end{array}\right]\mathrm{\&Delta;d}\left(k\right),$ 或X₀(k+1)＝φX₀(k)+GΔu(k)+G_RΔR(k+1)+G_dΔd(k)，其中φ为动态数据模型的待辨识系数{a₁，a₂，…，a_m；b₁，b₂，…，b_n}，辨识方法参照前述遗忘因子法(RFF)。图8即示出了应用自适应预见控制器作用于反馈控制器及被控对象的示意图，在参考指令位置301及外界干扰725的输入下，自适应预见控制器830、预见控制模块820及反馈控制模块810共同作用与被控对象710，得到预见控制输出308。相应图14，自适应控制模块1410输出进一步调整各单轴控制量U_x、U_y，与交叉耦合控制器1350、1350’的控制调节量共同对单轴作用以达到更佳的控制效果。各单轴调节的另一个方面在于对1315及1315’的进给率修调，修调量通过解耦控制器1320决定，调节后的进给率1325、1325’通过一个积分环节输出指令位置。

由此，对本发明的一个具体实施方式进行了描述，具体的实施流程需结合不同的执行部件。所确定的被控对象标称模型，是经过线性化处理的数据模型，模型的不确定性描述应最大限度涵盖模型结构与参数的属性信息，提供在标称模型周围的包络。在本实施例中，各运动轴伺服回路均包含多个前馈、反馈的输入、控制量调整，因此所构成的控制***描述应采用状态空间法。

应理解，即使在上述示例实施例中已提出本发明所涉及的多个实施步骤及实现方法，包括控制结构及具体的功能细节，但这种揭示仅是说明性的，而其细节方面的诸多变化，特别是在本发明原理范围内的部分结构及布局的调整，应延拓至所附权利要求书所表达之术语的上位概念所明示的最大范围。其特殊的应用实施例将根据特殊的驱动器类型和执行部件来变化，同时基本上保持相同的功能类型，而不偏离本发明的范围和构思。

Claims (4)

1.一种复杂轨迹的轮廓控制方法，包括以下步骤：

(1)对位置伺服***建模，即进行位置控制***的模型辨识，确定模型阶次、得到位置控制***传递函数、线性回归数据模型及其待辨识的模型参数J_eq、B_eq；

(2)设计基于PD型的位置控制器，根据辨识所得的被控对象伺服特性，确定稳定的控制极点，实时调节控制器参数，获得微超调、无相位滞后的稳定控制器；

(3)将插补进给率与增量式位置输出值比较得到的伺服延迟输入到位置控制器中，得到实时控制量的输出；

(4)根据控制器硬件FIFO队列中存储的插补数据，采用运动命令队列输出缓冲机制，基于步骤(1)辨识获得的数据模型，产生预见前馈输入；由自适应预见控制器对输入量(301)、伺服延迟(302)、控制量(304)进行处理，得到稳定的位置输出(308)；

(5)对各伺服轴输出位置值进行交叉耦合处理，计算实时耦合轮廓误差(1330)，通过耦合补偿控制器(1350、1350’)，对速度环内控制量进行修正；经过预补偿增益处理，进行解耦计算，计算结果经过XY轴向进给率校正分量v_kx、v_ky处理，实现对进给率的实时修调；

(6)在步骤(5)的基础上，结合步骤(4)的自适应预见控制器，各轴自适应预见控制器(1410)对各伺服轴的控制量U_x、U_y输出做进一步调整；根据历史位置数据及外部干扰信息改善实时控制量的稳定输出；轴间协调控制采用交叉耦合预补偿机制，同时对前端输入进给率(1315)及控制输出量U_x、U_y进行修调处理。

2.根据权利要求1所述的复杂轨迹的轮廓控制方法，其特征在于步骤(2)中所述确定稳定的控制极点，体现于下列实现步骤：

(a)设定控制量u(k)初值U_x、U_y，读取控制***输入、输出数值；

(b)根据当前被控对象的输入控制量u(k)、输出位置y(k)，在权利要求1所描述之自适应模型作用下，实时估算模型平移算子系数

(c)根据极点配置算法调整被控对象辨识所得特性参数ξ，ω_n值及采样周期T₀，求得极点配置特征多项式的系数F、G及其前馈模块H；

(d)根据以上所求之中间变量求得实时控制量u(k)的值；

3.根据权利要求1所述的复杂轨迹的轮廓控制方法，其特征在于步骤(4)的自适应预见控制器可用的控制过程信息包括当前位置目标点、未来目标点及外部扰动变化，在全局控制目标范围内，引入给定之加权函数，使之在最佳可控之理论范围内；通过如下方法实现：

(a)计算实时伺服控制延迟，初始化控制量，将控制目标函数设定为二次型表达：

$J\left(k\right)=\underset{j=k}{\overset{k+M_R}{\mathrm{\&Sigma;}}}[e^T\left(j\right)\mathrm{Qe}\left(j\right)+u^T\left(j\right)\mathrm{Hu}\left(j\right)];$

(b)加权系数矩阵Q、H的优化模型可通过相关数学软件得到；

(c)设置控制增量值Δu(k)，加入干扰未来值对控制量的影响，可得控制量处理公式为：

u(k)＝F_e∑e(i)+F_xx(k)+F_pr(z)P(k)+F_pd(z)d(k)；

(d)根据上步输入、输出、M_R(M_d)阶未来目标值与干扰值做出最优预见动作，对其中F_pr、F_pd进行向量系数辨识。

4.根据权利要求1所述的复杂轨迹的轮廓控制方法，其特征在于步骤(5)的交叉耦合处理包含以下两方面：

(a)采用交叉耦合预补偿控制方法，对各单轴前端输入进给率进行一次修调，使得***进给率输入既包含轨迹几何特征，又包含轴动态特性；

(b)通过耦合补偿控制器(1350)对耦合轮廓误差实时计算值(1330)之实时轮廓误差计算值进行解耦，得到U_x、U_y作为控制量输出补偿。

Images