CN104460518A - 基于模糊扰动补偿直接驱动xy平台轮廓控制装置及方法 - Google Patents

Abstract

本发明提供一种基于模糊扰动补偿直接驱动XY平台轮廓控制装置及方法，该装置包括主电路、控制电路和控制对象三部分,主电路分别连接控制电路和控制对象，控制电路也连接控制对象；本发明的优点在于当系统轮廓误差模型不准确，以及参数不确定时，系统仍然可以实现精确的位置跟踪。本发明利用了模糊系统以任意精度逼近非线性函数的能力，发明了模糊扰动补偿器。通过补偿器对未知干扰和参数不确定性进行估计及补偿,同时利用状态观测器逼近并补偿建模、求逆误差，降低了控制器对干扰器和模型精确度的要求，提高了跟踪精度。

Description

基于模糊扰动补偿直接驱动XY平台轮廓控制装置及方法

技术领域：本发明属于数控技术领域，尤其涉及一种基于模糊扰动补偿直接驱动XY平台轮廓控制装置及方法。

背景技术：XY平台系统的精密轮廓跟踪控制在数控机床中具有代表性，对提高数控系统加工精度和性能具有重要的作用。在XY平台伺服系统中,直线电机直接驱动方式避免了“滚珠+丝杠”的中间传动环节，负载受到的仅是永磁直线同步电动机提供的直接推力，消除了传统传动机构产生的问题。

为了减小跟踪误差，许多方法应用在改进单轴跟踪性能的控制器设计上。跟踪误差大都是由伺服延迟和受到负载或外界不确定干扰造成的，为了消除这些扰动，学者们提出了单轴控制策略，如PID控制、神经网络控制、自适应控制、鲁棒控制等控制方法，一般来说，应用这些方法能有效的减小单轴或解耦运动的轮廓误差。但是当系统轮廓误差模型不准确，以及参数不确定时，由于传统控制方法都要求有精确的模型对象，因此很难实现精确的位置跟踪。在过去的十几年中，模糊逻辑的应用越来越广泛，模糊逻辑系统可以任意的逼近高阶非线性系统并被认为是对非线性系统最有效的控制手段之一。同时状态观测器具有较强的抗干扰能力而得到了极为广泛且成功的应用，然而这种状态观测器不能消除非连续干扰，如静摩擦，因此它通常与摩擦补偿相结合才能较好地抑制静摩擦。实际上，单轴跟踪控制与双轴协调性是影响XY平台系统轮廓精度的两个重要因素。为了提高双轴协调性，常常采用交叉耦合控制(cross-coupled control，CCC)减小轮廓误差，但其分析与设计都限制于使用线性的轮廓误差。

发明内容：

发明目的：本发明提供一种基于模糊扰动补偿直接驱动XY平台轮廓控制装置及方法，其目的是解决以往的方式所存在的效果不理想的问题。

技术方案：本发明是通过以下技术方案来实现的：

一种基于模糊扰动补偿直接驱动XY平台轮廓控制装置，其特征在于：该装置包括主电路、控制电路和控制对象三部分,主电路分别连接控制电路和控制对象，控制电路也连接控制对象；控制电路包括DSP处理器、电流采样电路、动子位置采样电路、IPM隔离驱动保护电路；主电路包括调压电路、整流滤波单元和IPM逆变单元；控制对象为三相永磁直线同步电机，机身装有光栅尺；电流采样电路、动子位置采样电路、IPM隔离驱动保护电路均连接至DSP处理器，DSP处理器通过电压调整电路连接至整流滤波单元，整流滤波单元连接IPM逆变单元，IPM逆变单元连接三相永磁直线同步电机，三相永磁直线同步电机连接动子位置采样电路，IPM逆变单元连接IPM隔离驱动保护电路，电流采样电路通过霍尔传感器连接至IPM逆变单元与三相永磁直线同步电机之间。

利用上述的基于模糊扰动补偿直接驱动XY平台轮廓控制装置所实施的基于模糊扰动补偿直接驱动XY平台轮廓控制方法，其特征在于：该方法包括以下的具体步骤：

步骤一：建立XY平台轮廓误差模型

PMLSM的机械运动方程式表示如下

$F_e=K_f{i}_q=M\stackrel{\·}{v}+\mathrm{Bv}+F---\left(1\right)$

式中:F_e为电磁推力；K_f是推力系数；i_q为动子q轴电流，M是动子与所带负载质量和；为动子加速度；B描述系统粘滞摩擦系数；v是动子速度；F描述系统总扰动力；

选取x(t)和v(t)为系统状态变量,即PMLSM的状态方程可改写为

$\left\{\begin{array}{c}\stackrel{\·}{x}\left(t\right)=v\left(t\right)\\ \stackrel{\·}{v}\left(t\right)=-\frac{B}{M}\stackrel{\·}{x}\left(t\right)+\frac{K_f}{M}u+\frac{F}{M}\end{array}\right.---\left(2\right)$

式中:v(t)是电机的运动速度；u＝i_q表示电机的控制输入量；x(t)则为直线电机的位置输出，因此,直接驱动XY平台的传递函数为

$G_X\left(s\right)=\frac{v\left(s\right)}{i_q\left(s\right)}=\frac{K_f}{\mathrm{Ms}+B}---\left(3\right)$

$G_y\left(s\right)=\frac{v\left(s\right)}{i_q\left(s\right)}=\frac{K_f}{\mathrm{Ms}+B}---\left(4\right)$

在XY平台任意轨迹的轮廓跟踪任务中，轮廓误差模型的精度将直接影响轮廓加工的性能,其中R₁为指令路径，P₁为实际路径，R₁(t)为指令位置点，P₁(t)为实际位置点，而R₂(t)是指令路径上距离P₁(t)最近的一个点，为P₁(t)点的平均速度,为R₁(t)点的平均速度,V₁和V₂分别为R₁(t)和R₂(t)两点的切线速度,下角标X,Y分别代表X轴和Y轴,E_x与E_y分别为X轴和Y轴的跟踪误差，X轴与直线R₁R₂的夹角是α，轮廓误差E_c′为点P₁(t)到直线R₁R₂的距离,为直线R₁R₂与X轴的夹角；

由以上这些推出

$R_2\left(t\right)\≈\left[\begin{array}{c}{R}_{1x}\left(t\right)-\left(\frac{V_{R_{1x}}\left(t\right)+V_{P_{1x}}\left(t\right)}{2}\right)\·\mathrm{\Δt}\\ R_{1y}\left(t\right)-\left(\frac{V_{R_{1y}}\left(t\right)+V_{P_{1y}}\left(t\right)}{2}\right)\·\mathrm{\Δt}\end{array}\right]---\left(5\right)$

得出

最终推导出任意轨迹轮廓误差公式为

步骤二：模糊扰动补偿器设计

轮廓加工精度易受到系统动态非线性、不确定性因素以及曲线轨迹的轮廓误差模型相对复杂等问题的影响，为了解决上述问题，本发明在单轴上设计了基于状态观测器技术及模糊控制方法的模糊扰动补偿器；

模糊扰动补偿器的位置控制系统中1/(J_ms+B_m)为实际被控对象，为通过系统辨识得到的名义模型，T_m和T_d分别为转矩指令和实际扰动负载转矩，是模糊扰动补偿器所提供的补偿值，ω_cmd,ω,和Δω分别是速度指令，实际被控对象的速度，辨识出的名义模型的输出速度，实际被控对象与辨识出的名义模型之间的速度差，R,P,和ΔP分别是速度指令，实际被控对象的速度，辨识出的名义模型的输出速度，实际被控对象与辨识出的名义模型之间的速度差，R,P,和ΔP分别是参考位置命令，被控对象的实际位置，辨识出的名义模型输出位置，被控对象的实际位置和辨识出的名义模型输出位置间的位置差，此外，是速度环路控制器的比例增益和积分增益，而K_p是P型位置控制器；在状态反馈控制中，负载转速、转矩，轴矩等状态量直接测量存在困难，因此需要建立模型对这些系统状态进行估计，即系统状态重构问题,本申请采用反馈原理，构造与实际平台具有相同结构的估计模型，由于估计模型为人为建立，因此负载转速、转矩，轴矩等状态量都较易测量，然后利用观测偏差消除状态偏差；通过计算实际模型与估计模型输出的位置误差ΔP以及速度误差Δω，并将ΔP和Δω作为两个输入信号反馈回模糊控制器，模糊控制器再利用模糊规则对两个输入信号进行修正，逐渐使估计的状态值逼近实际的状态值，从而消除误差达到抑制扰动的目的；为了抑制噪声ξ，本发明估计模型获得的速度信息作为了反馈信号，而不使用实际被控对象的速度ω。

本申请设计了模糊扰动补偿器来抑制外部干扰；模糊干扰控制器有两个模糊输入和一个模糊输出,如图6所示；G_Δp为位置差比例因子，G_Δω为速度差比例因子，G_c为输出比例因子，而y^*分别为输入和输出的语言变量，在本申请中，将五个模糊语言变量值分别用于和共有25个规则用于模糊规则库，另外，本申请采用的每个模糊规则的一般形式可以描述为：

Rule_ij:Ifis A_i and ifis B_j,theny^*is C_ij,i＝1,2,3,4,5,j＝1,2,3,4,5,  (8)

其中A_i，B_j和C_ij分别是语言变量和y^*的语言学术语，图7为相关联的隶属度函数；

本发明采用了Mamdani的模糊推理方法。此外，下面的去模糊化公式是用来计算的输出y^*值

$y^{*}=\frac{\underset{n=1}{\overset{p}{\mathrm{\Σ}}}{\mathrm{\μ}}_n\left(C_n\right)\·C_n}{\underset{n=1}{\overset{p}{\mathrm{\Σ}}}{\mathrm{\μ}}_n\left(C_n\right)}---\left(9\right)$

其中μ(C_n)是隶属度，C_n是每个模糊集的基础，p代表控制输入的模糊集数量；

步骤三：轮廓控制器设计

本申请采用前馈补偿控制与交叉耦合控制器相结合的改进型交叉耦合控制方法来改善轮廓跟踪特性，减小轮廓误差；

其中，K_fx，K_fy为前馈补偿增益；C_x，C_y为交叉耦合增益；K_px，K_py分别为X、Y轴的位置控制器。X、Y轴的控制率分别为

U_x＝E_c′K_cC_x+E_xK_px+P_pecxK_pecx  (10)

U_y＝E_c′K_cC_y+E_yK_py+P_pecyK_pecy  (11)

式中，X、Y轴的跟踪误差E_x，E_y分别为

E_x＝R_x-P_x  (12)

E_y＝R_y-P_y  (13)

式中:R_x，R_y分别为X、Y轴的给定位置；P_x，P_y分别为X、Y轴的实际位置。

步骤2中状态观测器本质上是一个状态估计器或称动态补偿器，其基本思路是利用容易量测的被控对象的输入和输出对状态进行估计和推测；

状态估计的开环处理方法:

考虑线性时不变系统

$\stackrel{\·}{x}=\mathrm{Ax}+\mathrm{Bu},y=\mathrm{Cx}---\left(8\right)$

基于式(8)人为地构造一个观测器，观测器的输出为如果能满足

$\underset{t\→\∞}{\lim }(\tilde{x}-x)=0---\left(9\right)$

则观测器的输出可以作为内部状态x(t)的估值，从而实现“状态重构-即重新构造“状态”来作为“原状态x”的估值。观测器的输出应该能由系统输入u和系统输出y综合而成(系统输入u和系统输出y在工程实际中容易检测到)。

t→∞只是数学上的表述，实际工程中是很快的过程(<1s)。为了得到估计值一个很自然的想法是构造一个模拟系统

$\stackrel{\&CenterDot;}{\tilde{x}}=A\tilde{x}+\mathrm{Bu},\tilde{y}=C\tilde{x}---\left(10\right)$

用式(10)表示的模拟部件去再现式(8)表示的系统。因为式(10)的模拟系统是构造的，故是可测量的信息，若以作为x的估值。其估计误差为式(10)减式(8)，满足方程

$\stackrel{\&CenterDot;}{e}=\mathrm{Ae}---\left(11\right)$

讨论：①若A存在不具有负实部的特征值，将不会稳定，则当初始误差e(0)≠0，即时，有这样就不能作为x的估计值，即不能作为一个观测器。原因是他是一个开环系统，当估计值产生误差时，由于没有反馈，不能消除误差。

改进措施如图4所示，利用输出的估计误差作为反馈。此时构造的动态系统，即“D·G·Luenberger状态观测器”的状态方程为

$\stackrel{\&CenterDot;}{\tilde{x}}=A\tilde{x}+\mathrm{Bu}+L(y-C\tilde{x})=(A-\mathrm{LC})\tilde{x}+\mathrm{Ly}+\mathrm{Bu}---\left(12\right)$

观测器的输入为系统输出y和输入u的综合观测器的输出为式中L～n×p称为反馈增益阵。此时估计误差满足的方程为

$\begin{array}{c}\stackrel{\&CenterDot;}{\tilde{e}}=\stackrel{\&CenterDot;}{\tilde{x}}-\stackrel{\&CenterDot;}{x}=[(A-\mathrm{LC})\tilde{x}+\mathrm{Ly}+\mathrm{Bu}]-(\mathrm{Ax}+\mathrm{Bu})=(A-\mathrm{LC})\tilde{x}+\mathrm{LCx}-\mathrm{Ax}\\ =A(\tilde{x}-x)-\mathrm{LC}(\tilde{x}-x)=(A-\mathrm{LC})\tilde{e}\end{array}$

即 $\begin{array}{cc}\stackrel{\&CenterDot;}{\tilde{e}}={\tilde{A}}_L\tilde{e}=(A-\mathrm{LC})\tilde{e}& {\tilde{A}}_L=A-\mathrm{LC}\end{array}---\left(13\right)$

(13)表明系统存在观测器，且观测器的极点可以任意配置的充要条件是该系统完全能观，即可以选择L，通过C阵来改变A的特征值，使得原det(sI-A)＝0的非负实部的极点 $\&DoubleRightArrow;\det (\mathrm{sI}-{\tilde{A}}_L)=\det [\mathrm{sI}-(A-\mathrm{LC})]=0$ 都具有负实部的极点。

可以选择合适的L，使稳定，即特征值σ(A-LC)都具有负实部，则对任意初值x(0)以及任意输入u均有

$\underset{t\&RightArrow;\&infin;}{\lim }\tilde{e}=\underset{t\&RightArrow;\&infin;}{\lim }[\tilde{x}\left(t\right)-x\left(t\right)]=0---\left(14\right)$

因而可以作为x的估值，故可作为y＝Cx的一个观测器。

步骤二中：为了确定一个合适的干扰补偿值，模糊干扰控制器是在下面规则的基础上设计的；

规则一：如果位置差的是PB，PS，ZR，或NS并且速度差为PB，PS，ZR，或NS，那么所需的干扰补偿值将增至PB或PS；

规则二：如果位置差的是NB，NS，ZR，或PS并且速度差为NB，NS，ZR，或PS，那么所需的干扰补偿值将增至NB或NS；

规则三：如果位置差和速度差的具有相反的符号或者位置差和速度差都接近于零，那么所需的干扰补偿值将在ZR处保持不变；

步骤二中：本方法最终由嵌入DSP处理器中的控制程序实现，其控制过程按以下步骤执行：

步骤1 系统初始化；

步骤2 允许TN1、TN2中断；

步骤3 启动T1下溢中断；

步骤4 程序数据初始化；

步骤5 开总中断；

步骤6 中断等待；

步骤7 TN1中断处理子控制程序；

步骤8 结束。

其中步骤7中T1中断处理子控制程序按以下步骤：

步骤1 T1中断子控制程序；

步骤2 保护现场；

步骤3 判断是否已初始定位；是进入步骤4，否则进入步骤10；

步骤4 电流采样，CLARK变换，PARK变换；

步骤5 判断是否需要位置调节；否则进入步骤7；

步骤6 位置调节中断处理子控制程序；

步骤7 d q轴电流调节；

步骤8 PARK逆变换；

步骤9 计算CMPPx及PWM输出；

步骤10 位置采样；

步骤11 初始定位程序；

步骤12 恢复现场；

步骤13 中断返回。

其中步骤6中位置调节中断处理子控制程序按以下步骤：

步骤1 位置调节中断子控制程序；

步骤2 读取编码器值；

步骤3 判断角度；

步骤4 计算已走距离；

步骤5 执行位置控制器；

步骤6 执行模糊扰动补偿器补偿外部扰动；

步骤7 计算电流命令并输出；

步骤8 中断返回。

优点及效果：本发明提供一种基于模糊扰动补偿直接驱动XY平台轮廓控制装置及方法，针对现有控制技术中存在的实际问题，本发明在单轴上基于状态观测器技术及模糊控制方法提出了一种新的非线性控制结构。

本发明所设计的控制器应用到一个直线驱动电机的X-Y数控平台，实验系统如图1所示。该平台的位置被连接到每个驱动轴的线性编码器，线性编码器的传感器分辨率是0.1微米。每个驱动轴的速度被位置测量的反向差计算出来，这个采样周期为2毫秒。

本发明包括XY平台实时误差模型设计，使系统适用于任意曲线的轮廓跟踪任务；状态观测器设计，使系统能够有效抑制外部扰动；模糊扰动补偿器设计，降低系统对模型精确度的要求；轮廓控制器的设计，减小系统的轮廓误差。任意轨迹实时轮廓误差模型，如图2所示；状态观测器开环处理方法，如图3所示；状态观测器闭环处理方法，如图4所示；模糊控制结构框图如图5所示；位置差分输入()，速度差分输入()和控制输出(y^*)的隶属函数如图6所示；模糊扰动补偿器设计如图7所示；轮廓控制器的设计如图8所示。

针对直接驱动数控平台,本发明的优点在于当系统轮廓误差模型不准确，以及参数不确定时，系统仍然可以实现精确的位置跟踪。本发明利用了模糊系统以任意精度逼近非线性函数的能力，发明了模糊扰动补偿器。通过补偿器对未知干扰和参数不确定性进行估计及补偿,同时利用状态观测器逼近并补偿建模、求逆误差，降低了控制器对干扰器和模型精确度的要求，提高了跟踪精度。本发明同时采用适用于多轴非线性轮廓控制的实时轮廓误差计算法则，建立可用于自由曲线跟踪的XY平台的实时轮廓误差模型。将模糊扰动补偿控制器与前馈补偿交叉耦合控制相结合的方法得到的系统轮廓误差趋近于零。

附图说明：

图1为本发明所设计的实验系统；

图2为任意轨迹轮廓误差模型；

图3状态观测器开环处理方法；

图4状态观测器闭环处理方法；

图5为模糊控制器结构框图；

图6为位置差分输入()，速度差分输入()和控制输出(y^*)的隶属函数；

图7为模糊扰动补偿器设计原理图；

图8为实时轮廓误差补偿框图；

图9为实现本发明所设计的永磁直线同步电机矢量控制系统硬件结构硬件框图；

图10为本发明方法中矢量控制系统程序流程图；

图11为本发明方法位置调节中断处理子控制程序流程图；

图12为实现本发明的控制系统原理图；

(a)电机控制系统主电路原理图

(b)A、B相电流采样电路原理图

(c)光栅尺信号采样电路原理图

(d)IPM硬件驱动电路原理图。

具体实施方式：下面结合附图对本发明做进一步的说明：

本发明提供一种基于模糊扰动补偿直接驱动XY平台轮廓控制装置，其特征在于：该装置包括主电路、控制电路和控制对象三部分,主电路分别连接控制电路和控制对象，控制电路也连接控制对象；控制电路包括DSP处理器、电流采样电路、动子位置采样电路、IPM隔离驱动保护电路；主电路包括调压电路、整流滤波单元和IPM逆变单元；控制对象为三相永磁直线同步电机，机身装有光栅尺；电流采样电路、动子位置采样电路、IPM隔离驱动保护电路均连接至DSP处理器，DSP处理器通过电压调整电路连接至整流滤波单元，整流滤波单元连接IPM逆变单元，IPM逆变单元连接三相永磁直线同步电机，三相永磁直线同步电机连接动子位置采样电路，IPM逆变单元连接IPM隔离驱动保护电路，电流采样电路通过霍尔传感器连接至IPM逆变单元与三相永磁直线同步电机之间。下面结合附图对本申请做进一步描述：

(一)系统硬件结构

实现本发明的控制系统主电路如图12(a)所示，调压电路采用反向调压模块EUV-25A-II，可实现0～220V隔离调压。整流滤波单元采用桥式不可控整流，大电容滤波，配合适当的阻容吸收电路，可以获得IPM工作所需的恒定直流电压。IPM采用富士公司6MBP50RA060智能功率模块，耐压600V，最大电流50A，最高工作频率20kHz。IPM用四组独立的15V驱动电源供电。主电源输入端子(P，N)，输出端子(U，V，W)，主端子用自带的螺钉固定，可实现电流传输。P、N为变频器的整流变换平滑滤波后的主电源输入端子，P为正端，N为负端，逆变器输出的三相交流电通过输出端子U、V、W接至电机。

本发明的控制电路的核心为TMS320F2812处理器，其配套的开发板包括目标只读存储器、模拟接口、eCAN接口、串行引导ROM、用户指示灯、复位电路、可配置为RS232/RS422/RS485的异步串口、SPI同步串口和片外256K*16位RAM。

实际控制系统中电流采样采用LEM公司霍尔电流传感器LT58-S7。由两个霍尔电流传感器检测A、B相电流，得到电流信号，经过电流采样电路，转换成0～3.3V的电压信号，最后由TMS320LF2812的A/D转换模块转换成12位精度的二进制数，并保存在数值寄存器中。A、B相的电流采样电路如图12(b)所示。可调电阻VR2调节信号幅值，可调电阻VR1调节信号偏移量，通过对这两个电阻的调节，可以将信号调整到0～3.3V，再将其送入DSP的AD0、AD1管脚。图中的稳压管是为了防止送入DSP的信号超过3.3V，导致DSP被高压损坏。运算放大器采用OP27，电源接正负15V电压，在电压和地间接去耦电容。电路输入端接电容滤波，以去除高频信号干扰，提高采样精度。

光栅尺输出的A相和B相脉冲信号要通过快速光耦6N137对信号进行隔离，然后经过分压电路将信号电平由5V转换为3.3V，最后连接到DSP的两路正交编码脉冲接口QEP1和QEP2。电路原理如图12(c)所示。图12(d)给出了六路隔离驱动电路的原理图。需要指出的是IPM故障保护信号针对的是非重复瞬态故障，在本系统中通过如下措施来实现：IPM的故障输出信号通过光耦接到DSP的引脚，以确保IPM发生故障时DSP及时将所有事件管理器输出脚置高阻态。

(二)系统软件实现

本发明方法中矢量控制系统程序流程图如图10所示。软件的主程序包括系统初始化；开INT1、INT2中断；允许定时器中断；定时器中断处理子程序。其中初始化程序包括关闭所有中断，DSP系统初始化，变量初始化，事件管理器初始化、AD初始化和正交编码脉冲QEP初始化。中断服务子程序包括保护中断子程序和T1下溢中断服务子程序。其他部分如动子初始化定位，PID调节，矢量变换等都在定时器TI下溢中断处理子程序中执行。

IPM保护信号产生的保护中断响应属外部中断，INT1中断优先级比定时器T1的高。IPM会在过流、过压等异常情况自动发出保护信号，这一信号经转换连接到DSP的功率驱动保护引脚一旦有异常情况发生，DSP会进入保护中断子程序，首先禁止所有中断，然后封锁PWM输出使得电机马上停转，起到保护电机和IPM的作用。

矢量控制系统的顺利启动，需要知道动子的初始位置，利用软件可以给电机的动子通一个幅值恒定的直流电，使定子产生一个恒定的磁场，这个磁场与转子的恒定磁场相互作用，使电机动子运动到两个磁链重合的位置。而动子初始定位、AD采样值的读取、电机动子位置的计算、坐标变换、PID调节、SVPWM波形比较值的产生都在T1下溢中断服务子程序中完成。

本发明的方法包括以下的具体步骤：

步骤一：建立XY平台轮廓误差模型

PMLSM的机械运动方程式表示如下

$F_e=K_f{i}_q=M\stackrel{\&CenterDot;}{v}+\mathrm{Bv}+F---\left(1\right)$

式中:F_e为电磁推力；K_f是推力系数；i_q为动子q轴电流，M是动子与所带负载质量和；为动子加速度；B描述系统粘滞摩擦系数；v是动子速度；F描述系统总扰动力；

选取x(t)和v(t)为系统状态变量,即PMLSM的状态方程可改写为

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{x}\left(t\right)=v\left(t\right)\\ \stackrel{\&CenterDot;}{v}\left(t\right)=-\frac{B}{M}\stackrel{\&CenterDot;}{x}\left(t\right)+\frac{K_f}{M}u+\frac{F}{M}\end{array}\right.---\left(2\right)$

式中:v(t)是电机的运动速度；u＝i_q表示电机的控制输入量；x(t)则为直线电机的位置输出。因此,直接驱动XY平台的传递函数为

$G_X\left(s\right)=\frac{v\left(s\right)}{i_q\left(s\right)}=\frac{K_f}{\mathrm{Ms}+B}---\left(3\right)$

$G_y\left(s\right)=\frac{v\left(s\right)}{i_q\left(s\right)}=\frac{K_f}{\mathrm{Ms}+B}---\left(4\right)$

在XY平台任意轨迹的轮廓跟踪任务中，轮廓误差模型的精度将直接影响轮廓加工的性能,图2为任意轨迹实时轮廓误差模型，其中R₁为指令路径，P₁为实际路径，R₁(t)为指令位置点，P₁(t)为实际位置点，而R₂(t)是指令路径上距离P₁(t)最近的一个点，为P₁(t)点的平均速度,为R₁(t)点的平均速度,V₁和V₂分别为R₁(t)和R₂(t)两点的切线速度,下角标X,Y分别代表X轴和Y轴,E_x与E_y分别为X轴和Y轴的跟踪误差，X轴与直线R₁R₂的夹角是α，轮廓误差为P₁(t)到直线R₁R₂的距离E_c′,为通过R₂(t)与R₁(t)的直线与X轴的夹角为

由以上这些推出

$R_2\left(t\right)\&ap;\left[\begin{array}{c}{R}_{1x}\left(t\right)-\left(\frac{V_{R_{1x}}\left(t\right)+V_{P_{1x}}\left(t\right)}{2}\right)\&CenterDot;\mathrm{\&Delta;t}\\ R_{1y}\left(t\right)-\left(\frac{V_{R_{1y}}\left(t\right)+V_{P_{1y}}\left(t\right)}{2}\right)\&CenterDot;\mathrm{\&Delta;t}\end{array}\right]---\left(5\right)$

得出

最终推导出任意轨迹轮廓误差公式为

步骤二：模糊扰动补偿器设计

轮廓加工精度易受到系统动态非线性、不确定性因素以及曲线轨迹的轮廓误差模型相对复杂等问题的影响。为了解决上述问题，本发明在单轴上设计了基于状态观测器技术及模糊控制方法的模糊扰动补偿器。

干扰观测器可以将系统的外部干扰以及模型参数变化引起的实际被控对象与名义模型输出的差值等效到输入端，从而补偿外部干扰和模型参数的变化。干扰观测器的控制框图如图3所示。图中G_p(s)为被控对象的传递函数；G_n(s)为名义模型传递函数；Q(s)为低通滤波器。d为等效干扰；R为控制输入；d′为干扰的估计值。

状态观测器本质上是一个状态估计器(或称动态补偿器)，其基本思路是利用容易量测的被控对象的输入和输出对状态进行估计(和推测)。

如图3为状态估计的开环处理方法。

考虑线性时不变系统

$\stackrel{\&CenterDot;}{x}=\mathrm{Ax}+\mathrm{Bu},y=\mathrm{Cx}---\left(8\right)$

基于式(8)人为地构造一个观测器，观测器的输出为如果能满足

$\underset{t\&RightArrow;\&infin;}{\lim }(\tilde{x}-x)=0---\left(9\right)$

则观测器的输出可以作为内部状态x(t)的估值，从而实现“状态重构-即重新构造“状态”来作为“原状态x”的估值。观测器的输出应该能由系统输入u和系统输出y综合而成(系统输入u和系统输出y在工程实际中容易检测到)。

t→∞只是数学上的表述，实际工程中是很快的过程(<1s)。为了得到估计值一个很自然的想法是构造一个模拟系统

$\stackrel{\&CenterDot;}{\tilde{x}}=A\tilde{x}+\mathrm{Bu},\tilde{y}=C\tilde{x}---\left(10\right)$

用式(10)表示的模拟部件去再现式(8)表示的系统。因为式(10)的模拟系统是构造的，故是可测量的信息，若以作为x的估值。其估计误差为式(10)减式(8)，满足方程

$\stackrel{\&CenterDot;}{e}=\mathrm{Ae}---\left(11\right)$

讨论：①若A存在不具有负实部的特征值，将不会稳定，则当初始误差e(0)≠0，即时，有这样就不能作为x的估计值，即不能作为一个观测器。原因是他是一个开环系统，当估计值产生误差时，由于没有反馈，不能消除误差。

改进措施如图4所示，利用输出的估计误差作为反馈。此时构造的动态系统，即“D·G·Luenberger状态观测器”的状态方程为

$\stackrel{\&CenterDot;}{\tilde{x}}=A\tilde{x}+\mathrm{Bu}+L(y-C\tilde{x})=(A-\mathrm{LC})\tilde{x}+\mathrm{Ly}+\mathrm{Bu}---\left(12\right)$

观测器的输入为系统输出y和输入u的综合观测器的输出为式中L～n×p称为反馈增益阵。此时估计误差满足的方程为

$\begin{array}{c}\stackrel{\&CenterDot;}{\tilde{e}}=\stackrel{\&CenterDot;}{\tilde{x}}-\stackrel{\&CenterDot;}{x}=[(A-\mathrm{LC})\tilde{x}+\mathrm{Ly}+\mathrm{Bu}]-(\mathrm{Ax}+\mathrm{Bu})=(A-\mathrm{LC})\tilde{x}+\mathrm{LCx}-\mathrm{Ax}\\ =A(\tilde{x}-x)-\mathrm{LC}(\tilde{x}-x)=(A-\mathrm{LC})\tilde{e}\end{array}$

即 $\begin{array}{cc}\stackrel{\&CenterDot;}{\tilde{e}}={\tilde{A}}_L\tilde{e}=(A-\mathrm{LC})\tilde{e}& {\tilde{A}}_L=A-\mathrm{LC}\end{array}---\left(13\right)$

(13)表明系统存在观测器，且观测器的极点可以任意配置的充要条件是该系统完全能观，即可以选择L，通过C阵来改变A的特征值，使得原det(sI-A)＝0的非负实部的极点 $\&DoubleRightArrow;\det (\mathrm{sI}-{\tilde{A}}_L)=\det [\mathrm{sI}-(A-\mathrm{LC})]=0$ 都具有负实部的极点。

可以选择合适的L，使稳定，即特征值σ(A-LC)都具有负实部，则对任意初值x(0)以及任意输入u均有

$\underset{t\&RightArrow;\&infin;}{\lim }\tilde{e}=\underset{t\&RightArrow;\&infin;}{\lim }[\tilde{x}\left(t\right)-x\left(t\right)]=0---\left(14\right)$

因而可以作为x的估值，故可作为y＝Cx的一个观测器。

在轮廓控制问题中经常使用干扰观测器来抑制外部干扰。在许多方法中，干扰观测器是在伺服控制机构的逆模型或固定增益常数基础上设计的。但干扰观测器在实际应用建模时，经常由于存在的摩擦力或齿轮间隙等未知干扰和非线性问题，而达不到较好的控制效果。因此，为了满足轮廓精度的要求，本发明设计了模糊扰动补偿器来抑制外部干扰。图4为模糊控制器的原理图。

模糊干扰控制器有两个模糊输入和一个模糊输出；G_Δp为位置差比例因子，G_Δω为速度差比例因子，G_c为输出比例因子，而y^*分别为输入和输出的语言变量，在本发明中，将五个模糊语言变量值分别用于和共有25个规则用于模糊规则库。另外，本发明采用的每个模糊规则的一般形式可以描述为：

Rule_ij:Ifis A_i and ifis B_j,theny^*isC_ij,i＝1,2,3,4,5,j＝1,2,3,4,5,  (8)

其中A_i，B_j和C_ij分别是语言变量和y^*的语言学术语。此外，图5阐述了相关联的隶属函数

本发明采用了Mamdani的模糊推理方法。此外，下面的去模糊化公式是用来计算的输出y^*值

$y^{*}=\frac{\underset{n=1}{\overset{p}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_n\left(C_n\right)\&CenterDot;C_n}{\underset{n=1}{\overset{p}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_n\left(C_n\right)}---\left(9\right)$

其中μ(C_n)是隶属度，C_n是每个模糊集的基础，p代表控制输入的模糊集数量。

在本发明中，为了确定一个合适的干扰补偿值，模糊干扰控制器是在下面规则的基础上设计的。

规则一：如果位置差的是PB，PS，ZR，或NS并且速度差为PB，PS，ZR，或NS，那么所需的干扰补偿值将增至PB或PS。

规则二：如果位置差的是NB，NS，ZR，或PS并且速度差为NB，NS，ZR，或PS，那么所需的干扰补偿值将增至NB或NS。

规则三：如果位置差和速度差的具有相反的符号或者位置差和速度差都接近于零(如是PB,是NB，和都是ZR)，那么所需的干扰补偿值将在ZR处保持不变。

模糊扰动补偿器主要利用模糊逻辑范式来调节扰动观测器的增益常数从而改进干扰抑制性能，模糊扰动补偿器是在实际被控对象与名义模型之间的速度差和位置差基础上设计的。图5为模糊扰动补偿器的位置控制系统框图，其中1/(J_ms+B_m)为实际被控对象，为通过系统辨识得到的名义模型。T_m和T_d分别为转矩指令和实际扰动负载转矩，是模糊扰动补偿器所提供的补偿值。ω_cmd,ω,和Δω分别是速度指令，实际被控对象的速度，辨识出的名义模型的输出速度，实际被控对象与辨识出的名义模型之间的速度差。R,P,和ΔP分别是速度指令，实际被控对象的速度，辨识出的名义模型的输出速度，实际被控对象与辨识出的名义模型之间的速度差。R,P,和ΔP分别是参考位置命令，被控对象的实际位置，辨识出的名义模型输出位置，被控对象的实际位置和辨识出的名义模型输出位置间的位置差。此外，是速度环路控制器的比例增益和积分增益，而K_p是P型位置控制器。在状态反馈控制中，负载转速、转矩，轴矩等状态量直接测量存在困难，因此需要建立模型对这些系统状态进行估计，即系统状态重构问题。本发明采用反馈原理，构造图5中与实际平台具有相同结构的估计模型，由于估计模型为人为建立，因此负载转速、转矩，轴矩等状态量都较易测量，然后利用观测偏差消除状态偏差；如图5，通过计算实际模型与估计模型输出的位置误差ΔP以及速度误差Δω，并将ΔP和Δω作为两个输入信号反馈回模糊控制器，模糊控制器再利用模糊规则对两个输入信号进行修正，逐渐使估计的状态值逼近实际的状态值，从而消除误差达到抑制扰动的目的；为了抑制噪声ξ，本发明估计模型获得的速度信息作为了反馈信号，而不使用实际被控对象的速度ω。

此外，为了抑制噪声ξ，通过辨识出的名义模型获得的速度信息作为了反馈信号，而不使用实际被控对象的速度。设计模糊扰动补偿器的关键在于设计一个能够提供合适扰动补偿值的模糊干扰控制器。

步骤三：轮廓控制器设计

传统轮廓加工中，一般采用交叉耦合控制策略就能很好地减小轮廓误差，但局限于轮廓轨迹为线性轨迹，而在自由形态曲线跟踪时，由于轨迹的时变性以及各轴动态不一致，虽然采用交叉耦合控制器，仍存在很大的轮廓误差。针对这一问题，本发明采用前馈补偿控制与交叉耦合控制器相结合的改进型交叉耦合控制方法来改善轮廓跟踪特性，减小轮廓误差。其补偿原理如图7所示。

其中，K_fx，K_fy为前馈补偿增益；C_x，C_y为交叉耦合增益；K_px，K_py分别为X、Y轴的位置控制器。X、Y轴的控制率分别为

U_x＝E_c′K_cC_x+E_xK_px+P_pecxK_pecx  (10)

U_y＝E_c′K_cC_y+E_yK_py+P_pecyK_pecy  (11)

式中，X、Y轴的跟踪误差E_x，E_y分别为

E_x＝R_x-P_x  (12)

E_y＝R_y-P_y  (13)

式中:R_x，R_y分别为X、Y轴的给定位置；P_x，P_y分别为X、Y轴的实际位置。

实现本发明所设计的直接驱动数控平台模糊扰动补偿控制系统及方法硬件框图如图8所示。整个系统包括主电路、控制电路和控制对象三部分。控制电路包括DSP处理器、电流采样电路、动子位置采样电路、IPM隔离驱动电路和IPM保护电路。主电路包括调压电路、整流滤波单元和IPM逆变单元。控制对象为三相永磁直线同步电机，机身装有光栅尺。

本发明方法最终由嵌入DSP处理器中的控制程序实现，其控制过程按以下步骤执行：

步骤1 系统初始化；

步骤2 允许TN1、TN2中断；

步骤3 启动T1下溢中断；

步骤4 程序数据初始化；

步骤5 开总中断；

步骤6 中断等待；

步骤7 TN1中断处理子控制程序；

步骤8 结束；

其中步骤7中T1中断处理子控制程序按以下步骤：

步骤1 T1中断子控制程序；

步骤2 保护现场；

步骤3 判断是否已初始定位；是进入步骤4，否则进入步骤10；

步骤4 电流采样，CLARK变换，PARK变换；

步骤5 判断是否需要位置调节；否则进入步骤7；

步骤6 位置调节中断处理子控制程序；

步骤7 d q轴电流调节；

步骤8 PARK逆变换；

步骤9 计算CMPPx及PWM输出；

步骤10 位置采样；

步骤11 初始定位程序；

步骤12 恢复现场；

步骤13 中断返回；

其中步骤6中位置调节中断处理子控制程序按以下步骤：

步骤1 位置调节中断子控制程序；

步骤2 读取编码器值；

步骤3 判断角度；

步骤4 计算已走距离；

步骤5 执行位置控制器；

步骤6 执行模糊扰动补偿器补偿外部扰动；

步骤7 计算电流命令并输出；

步骤8 中断返回；

传统轮廓加工中，一般采用交叉耦合控制策略就能很好地减小轮廓误差，但局限于轮廓轨迹为线性轨迹，而在自由形态曲线跟踪时，由于轨迹的时变性以及各轴动态不一致，虽然采用交叉耦合控制器，仍存在很大的轮廓误差。针对这一问题，本申请采用前馈补偿控制与交叉耦合控制器相结合的改进型交叉耦合控制方法来改善轮廓跟踪特性，减小轮廓误差。

模糊扰动补偿器利用模糊系统具有以任意精度逼近非线性函数的能力对未知干扰和参数不确定性进行估计及补偿，同时利用状态观测器逼近并补偿建模、求逆误差，降低了控制器对干扰器和模型精确度的要求，提高了跟踪精度。

为了加强CCC方法控制轮廓精度的作用，近年来不少研究从交叉耦合控制器的结构入手来消除其本身的一些缺点。本发明在实时轮廓误差计算法则的基础上提出了改进型交叉耦合控制方法，这种方法适用于任意曲线的轮廓跟踪任务。

Claims (7)

1.一种基于模糊扰动补偿直接驱动XY平台轮廓控制装置，其特征在于：该装置包括主电路、控制电路和控制对象三部分,主电路分别连接控制电路和控制对象，控制电路也连接控制对象；控制电路包括DSP处理器、电流采样电路、动子位置采样电路、IPM隔离驱动保护电路；主电路包括调压电路、整流滤波单元和IPM逆变单元；控制对象为三相永磁直线同步电机，机身装有光栅尺；电流采样电路、动子位置采样电路、IPM隔离驱动保护电路均连接至DSP处理器，DSP处理器通过电压调整电路连接至整流滤波单元，整流滤波单元连接IPM逆变单元，IPM逆变单元连接三相永磁直线同步电机，三相永磁直线同步电机连接动子位置采样电路，IPM逆变单元连接IPM隔离驱动保护电路，电流采样电路通过霍尔传感器连接至IPM逆变单元与三相永磁直线同步电机之间。

2.利用权利要求1所述的基于模糊扰动补偿直接驱动XY平台轮廓控制装置所实施的基于模糊扰动补偿直接驱动XY平台轮廓控制方法，其特征在于：该方法包括以下的具体步骤：

步骤一：建立XY平台轮廓误差模型

PMLSM的机械运动方程式表示如下

式中:F_e为电磁推力；K_f是推力系数；i_q为动子q轴电流，M是动子与所带负载质量和；为动子加速度；B描述系统粘滞摩擦系数；v是动子速度；F描述系统总扰动力；

选取x(t)和v(t)为系统状态变量,即PMLSM的状态方程可改写为

式中:v(t)是电机的运动速度；u＝i_q表示电机的控制输入量；x(t)则为直线电机的位置输出，因此,直接驱动XY平台的传递函数为

在XY平台任意轨迹的轮廓跟踪任务中，轮廓误差模型的精度将直接影响轮廓加工的性能,其中R₁为指令路径，P₁为实际路径，R₁(t)为指令位置点，P₁(t)为实际位置点，而R₂(t)是指令路径上距离P₁(t)最近的一个点，为P₁(t)点的平均速度,为R₁(t)点的平均速度,V₁和V₂分别为R₁(t)和R₂(t)两点的切线速度,下角标X,Y分别代表X轴和Y轴,E_x与E_y分别为X轴和Y轴的跟踪误差，X轴与直线 R₁R₂的夹角是α，轮廓误差E′_c为点P₁(t)到直线R₁R₂的距离,为直线R₁R₂与X轴的夹角；

由以上这些推出

得出

最终推导出任意轨迹轮廓误差公式为

步骤二：模糊扰动补偿器设计

轮廓加工精度易受到系统动态非线性、不确定性因素以及曲线轨迹的轮廓误差模型相对复杂等问题的影响，为了解决上述问题，本发明在单轴上设计了基于状态观测器技术及模糊控制方法的模糊扰动补偿器；

模糊扰动补偿器的位置控制系统中1/(J_ms+B_m)为实际被控对象，为通过系统辨识得到的名义模型，T_m和T_d分别为转矩指令和实际扰动负载转矩， 是模糊扰动补偿器所提供的补偿值，ω_cmd,ω,和Δω分别是速度指令，实际被控对象的速度，辨识出的名义模型的输出速度，实际被控对象与辨识出的名义模型之间的速度差，R,P,和ΔP分别是速度指令，实际被控对象的速度，辨识出的名义模型的输出速度，实际被控对象与辨识出的名义模型之间的速度差，R,P,和ΔP分别是参考位置命令，被控对象的实际位置，辨识出的名义模型输出位置，被控对象的实际位置和辨识出的名义模型输出位置间的位置差，此外， 是速度环路控制器的比例增益和积分增益，而K_p是P型位置控制器；在状态反馈控制中，负载转速、转矩，轴矩等状态量直接测量存在困难，因此需要建立模型对这些系统状态进行估计，即系统状态重构问题,本申请采用反馈原理，构造与实际平台具有相同结构的估计模型，由于估计模型为人为建立，因此负载转速、转矩，轴矩等状态量都较易测量，然后利用观测偏差消除状态 偏差；通过计算实际模型与估计模型输出的位置误差ΔP以及速度误差Δω，并将ΔP和Δω作为两个输入信号反馈回模糊控制器，模糊控制器再利用模糊规则对两个输入信号进行修正，逐渐使估计的状态值逼近实际的状态值，从而消除误差达到抑制扰动的目的；为了抑制噪声ξ，本发明估计模型获得的速度信息作为了反馈信号，而不使用实际被控对象的速度ω；

本申请设计了模糊扰动补偿器来抑制外部干扰；模糊干扰控制器有两个模糊输入和一个模糊输出,如图6所示；G_Δp为位置差比例因子，G_Δω为速度差比例因子，G_c为输出比例因子，而y^*分别为输入和输出的语言变量，在本申请中，将五个模糊语言变量值分别用于和共有25个规则用于模糊规则库，另外，本申请采用的每个模糊规则的一般形式可以描述为：

Rule_ij:If  is A_i and if  is B_j,then y^*is C_ij,i＝1,2,3,4,5,j＝1,2,3,4,5,                                   (8) 

其中A_i，B_j和C_ij分别是语言变量和y^*的语言学术语，图7为相关联的隶属度函数；

本发明采用了Mamdani的模糊推理方法；此外，下面的去模糊化公式是用来计算的输出y^*值

其中μ(C_n)是隶属度，C_n是每个模糊集的基础，p代表控制输入的模糊集数量；

步骤三：轮廓控制器设计

本申请采用前馈补偿控制与交叉耦合控制器相结合的改进型交叉耦合控制方法来改善轮廓跟踪特性，减小轮廓误差；

其中，K_fx，K_fy为前馈补偿增益；C_x，C_y为交叉耦合增益；K_px，K_py分别为X、Y轴的位置控制器；X、Y轴的控制率分别为

U_x＝E_c′K_cC_x+E_xK_px+P_pecxK_pecx                  (10) 

U_y＝E_c′K_cC_y+E_yK_py+P_pecyK_pecy                   (11) 

式中，X、Y轴的跟踪误差E_x，E_y分别为

E_x＝R_x-P_x                          (12) 

E_y＝R_y-P_y                          (13) 

式中:R_x，R_y分别为X、Y轴的给定位置；P_x，P_y分别为X、Y轴的实际位置。

3.根据权利要求2所述的基于模糊扰动补偿直接驱动XY平台轮廓控制方法，其特征在于：步骤2中状态观测器本质上是一个状态估计器或称动态补偿器，其基本思路是利用容易量测的被控对象的输入和输出对状态进行估计和推测；

状态估计的开环处理方法:

考虑线性时不变系统

基于式(8)人为地构造一个观测器，观测器的输出为如果能满足

则观测器的输出可以作为内部状态x(t)的估值，从而实现“状态重构-即重新构造“状态”来作为“原状态x”的估值；观测器的输出应该能由系统输入u和系统输出y综合而成(系统输入u和系统输出y在工程实际中容易检测到)；

t→∞只是数学上的表述，实际工程中是很快的过程(<1s)；为了得到估计值，一个很自然的想法是构造一个模拟系统

用式(10)表示的模拟部件去再现式(8)表示的系统；因为式(10)的模拟系统是构造的，故是可测量的信息，若以作为x的估值；其估计误差为式(10)减式(8)，满足方程

讨论：①若A存在不具有负实部的特征值，将不会稳定，则当初始误差e(0)≠0，即时，有这样就不能作为x的估计值，即不能作为一个观测器；原因是他是一个开环系统，当估计值产生误差时，由于没有反馈，不能消除误差；

改进措施如图4所示，利用输出的估计误差作为反馈；此时构造的动态系统，即“D·G·Luenberger状态观测器”的状态方程为

观测器的输入为系统输出y和输入u的综合观测器的输出为式中L～n×p称为反馈增益阵；此时估计误差满足的方程为

即

(13)表明系统存在观测器，且观测器的极点可以任意配置的充要条件是该系统完全能观，即可以选择L，通过C阵来改变A的特征值，使得原det(sI-A)＝0的非负实部的极点都具有负实部的极点；

可以选择合适的L，使稳定，即特征值σ(A-LC)都具有负实部，则对任意初值x(0)以及任意输入u均有

因而可以作为x的估值，故可作为y＝Cx的一个观测器。

4.根据权利要求2所述的基于模糊扰动补偿直接驱动XY平台轮廓控制方法，其特征在于：步骤二中：为了确定一个合适的干扰补偿值，模糊干扰控制器是在下面规则的基础上设计的；

规则一：如果位置差的是PB，PS，ZR，或NS并且速度差为PB，PS，ZR，或NS，那么所需的干扰补偿值将增至PB或PS；

规则二：如果位置差的是NB，NS，ZR，或PS并且速度差为NB，NS，ZR，或PS，那么所需的干扰补偿值将增至NB或NS；

规则三：如果位置差和速度差的具有相反的符号或者位置差和速度差都接近于零，那么所需的干扰补偿值将在ZR处保持不变。

5.根据权利要求4所述的基于模糊扰动补偿直接驱动XY平台轮廓控制方法，其特征在于：步骤二中：本方法最终由嵌入DSP处理器中的控制程序实现，其控制过程按以下步骤执行：

步骤1  系统初始化；

步骤2  允许TN1、TN2中断；

步骤3  启动T1下溢中断；

步骤4  程序数据初始化；

步骤5  开总中断；

步骤6  中断等待；

步骤7  TN1中断处理子控制程序；

步骤8  结束。

6.根据权利要求5所述的基于模糊扰动补偿直接驱动XY平台轮廓控制方法，其特征在于：

其中步骤7中T1中断处理子控制程序按以下步骤：

步骤1  T1中断子控制程序；

步骤2  保护现场；

步骤3  判断是否已初始定位；是进入步骤4，否则进入步骤10；

步骤4  电流采样，CLARK变换，PARK变换；

步骤5  判断是否需要位置调节；否则进入步骤7；

步骤6  位置调节中断处理子控制程序；

步骤7  d q轴电流调节；

步骤8  PARK逆变换；

步骤9  计算CMPPx及PWM输出；

步骤10 位置采样；

步骤11 初始定位程序；

步骤12 恢复现场；

步骤13 中断返回。

7.根据权利要求6所述的基于模糊扰动补偿直接驱动XY平台轮廓控制方法，其特征在于：

其中步骤6中位置调节中断处理子控制程序按以下步骤：

步骤1  位置调节中断子控制程序；

步骤2  读取编码器值；

步骤3  判断角度；

步骤4  计算已走距离；

步骤5  执行位置控制器；

步骤6  执行模糊扰动补偿器补偿外部扰动；

步骤7  计算电流命令并输出；

步骤8  中断返回。