专利名称：实时运动控制算法仿真调试方法
技术领域：
本发明涉及一种计算机对运动部件的控制方法，特别是一种对运动控制算法进行高效、实时仿真调试的方法。
背景技术：
现代的高速精密机电设备(如半导体生产设备和数控机床等)中普遍采用主-从式计算机控制的模式，即由主机处理用户输入和协调各方向动作而由从机完成组成各动作的基本运动。从机一般由数字信号处理器(DSP)和外围输入输出接口组成并通过总线或网络与主机通讯。随着离散时间控制(数字控制)在硬件和软件两方面的充分发展，目前的运动控制都采用在DSP上运行控制算法的数字方式实现。从机的DSP上以中断服务例程(ISR)的形式运行着运动控制算法，每个时钟中断发生时，算法依次通过输入接口(编码器接口等)读取系统当前位置，经过计算后通过输出接口(数模转换器D/A等)更新驱动电机的电流信号而改变系统的输入，同时接受主机的运动命令并将执行结果和自身状态反馈给主机。随着时钟中断的不断发生，运动控制算法持续运行，算法在时钟的精确触发下按准确时间间隔循环往复运行是数字运动控制算法有效性的根本保证。由于从机是以嵌入式的方式包含在设备当中，不象主机那样具有完备的操作系统和输入/输出设备，无法方便高效地监测从机上运行程序的实时状态。另外，现代运动运动控制算法呈现出代码庞大、参数众多、关系繁杂的显著趋势，如何进行高效的调试以确保最短时间内获得运行正确的代码是一个极大的挑战。一种既能准确反映运动控制代码实时运行状况又可以方便高效地对代码进行各种常规调试的方法成为必需。
现有的仿真调试工具都无法全面满足前面提出的实时运动控制算法代码调试的需要。Matlab和Simulink等软件可以方便地建立控制对象的动态模型并与所用控制算法相连，进行仿真分析，但因为系统模型中的控制算法与DSP上运行的算法代码没有直接联系，结果并不反映实时运动控制代码执行的真实结果；而且这类仿真工具也无法有效体现运动控制代码中状态监测和逻辑流控制的部分。针对DSP的仿真调试环境，虽然可以对DSP上的代码进行单步运行等操作，但因为高速实时运行(每秒数千次)条件下执行元件(电机和运动部件)的真实响应无法随代码的运行定格在某一时刻而不具备真实运行条件下的调试能力；同时也缺乏必要的与DSP运行相协调的和主机间的数据交换机制。目前出现的硬件在回路(Hardware InLoop)仿真通过将模拟控制对象(执行元件)实时响应的可编程硬件仿真器接入控制回路的方法，一定程度上实现了控制算法脱离真实控制对象的实时仿真调试，但仍存在着硬件仿真器成本高昂、编程和设置复杂的问题，也无法达到完全自由灵活的代码实时单步、断点调试。主机代码的开发环境具有完善的调试机制，但无法和DSP的实时代码协调。
发明内容
本发明的目的是提供一种实时运动控制算法仿真调试方法，针对实时运动控制算法调试的特殊需要，解决完整不变的从机DSP运动控制代码和主机程序在同一计算机处理器上以真实的实时状态运行的问题；还解决目前代码调试工具并不能实现实时运动控制代码执行真实结果的问题，并解决控制算法与真实控制对象的实时仿真调试无法和DSP的实时代码协调的问题；同时解决利用现有的主机代码开发环境，对两部分程序进行简便、准确、有效的单步、断点等调试的问题。
本发明的技术方案该方法把从机DSP上运行的运动控制代码完整不变地转移到主机上运行并采用数值仿真的方法来准确模拟控制对象的真实响应；首先，把从机DSP上定义的用于与主机进行数据交换的内存区间转移定义到主机的内存映射中，并将相应的主机对DSP内存区间读写的函数修改为新的函数，新的函数对新定义的主机内存映射进行操作；其次，将运动控制代码中通过输出接口更新驱动电机电流信号的函数修改为根据代码计算的输出量用数值积分方法来计算控制对象的响应，并存储在相应的内存单元中；同时，把代码中通过输入接口读取系统当前位置的函数修改为新的函数，新的函数从存储控制对象响应的数值计算结果的内存单元中读取系统当前位置；最后，在主机中循环检测从机状态，等待从机完成运动命令的函数中加入对从机DSP代码ISR的调用，以达到运动控制算法的反复运行。
本发明通过以上措施实现了完整不变的从机DSP运动控制代码和主机程序在同一计算机处理器上以真实的实时状态运行，可以利用现有的主机代码开发环境对两部分程序进行方便有效的单步、断点等调试。该方法适用于各种独立运行于嵌入式处理器的运动控制算法的仿真调试。
图1是一个典型运动控制系统的硬件组成及代码运行关系示意图；图2是针对图1中系统的运动控制算法实时仿真调试方法示意图。
图3是本发明被应用在一个半导体生产设备工作台的示意图。
图4是对图3的设备在X方向典型的18毫秒内完成2.54mm的运动的仿真结果示意图。
具体实施例方式
如图1所示，一个典型的运动控制系统由主机、从机和控制对象组成。主机CPU中运行的主程序调用DSP内存写函数DSPWrite()向从机DSP的数据交换内存写入运动命令，之后运行运动完成检测函数MotionDone()来检测从机是否完成所写入的运动命令。MotionDone()调用函数DSP内存读函数DSPRead()自从机DSP的数据交换内存中读出从机状态，判断运动命令是否完成，MotionDone()循环运行直至运动完成为止。从机DSP中的中断服务例程ISR首先调用编码器读函数ReadEnc()通过编码器接口读取控制对象运动部件的当前位置，其后经过运动控制算法的计算得出新的电流控制输出量，然后运行D/A写函数WriteDA()通过D/A将这一电流信号传递给控制对象的电机。ISR运行的过程中通过DSP的数据交换内存接受主机CPU的运动命令并将包含命令执行结果的自身状态反馈给主机CPU。ISR在时钟中断的精确触发下，每隔一个中断周期TItr运行一次，一直循环持续下去。控制对象的电机以从机DSP规定的电流驱动运动部件到达当前位置。
图2给出了针对图1中系统的运动控制算法实时仿真调试方案。在原来主机CPU中运行的主程序基础之上，把从机DSP上运行的ISR代码转移到主机CPU上运行，同时将DSP中的数据交换内存也转移定义到主机的内存映射中。将主程序中的函数DSPWrite()和DSPRead()分别修改为对定义在主机上的数据交换内存进行写入和读出。在主程序的函数MotionDone()中加入对ISR()的调用，以使ISR()随函数MotionDone()反复循环运行直至运动完成为止。将ISR()中的函数WriteDA()修改为调用运动仿真函数MotionSim()。函数MotionSim()根据运动控制算法计算的电流控制输出量用四阶龙格-库塔法(Runge-Kutta)计算控制对象的响应并存储在位置内存中。把ISR()中的函数ReadEnc()修改为从位置内存中读取系统当前位置。ISR()中ReadEnc()与WriteDA()之间的代码是运动控制算法的主体，在从DSP到主机的转移中保持完整不变。仿真调试方案中内存定义和函数的少量修改可以通过条件编译命令实现，达到同一源文件用于DSP可执行文件编译和运动控制算法实时仿真调试。方案中位置内存里储存的控制对象位置及其导出量(速度、加速度等)可以与各种数据可视化手段相结合，形成完整、直观、高效的，完全脱离从机和控制对象的运动控制算法虚拟实时仿真调试环境。
方案中用于计算控制对象响应的四阶龙格-库塔法是最常用的准确数值积分方法，适用于任何可以用状态空间中表达(即动态特性可以写成一个线性一阶常微分方程组)的系统。一个运动控制系统中典型的单自由度质量-弹簧-阻尼运动部件的动力学方程可以写为md2xd2t+cdxdt+kx=F---(1)]]>其中x为运动部件的位置，t为时间，m为运动部件的质量，c为阻尼系数，k为弹簧刚度，F为作用于运动部件的电机驱动力。方程(1)可以重写为d2xd2t=Fmcmdxdtkmx---(2)]]>电机线圈内的电流和电机的驱动力之间存在以下关系F＝KfI (3)其中F为电机驱动力，I为运动控制算法计算得出的电流控制输出量，Kf为电机的力敏感度。将方程(3)代入方程(2)可以得到d2xd2t=KfImcmdxdtkmx---(4)]]>定义变量矢量如下y1y2=xdxdt---(5)]]>定义相应的变量导数矢量如下[dy1ddy2]dt=[dxdtd2xd2t]---(6)]]>将方程(4)代入方程(6)可以得到
方程(5)和方程(7)给出了单自由度二阶线性动态系统的状态空间表达。根据四阶龙格-库塔法的定义，控制对象的响应通过以下数值积分方法计算b1i＝hdydxi(y1，n，y2，n)b2i=hdydxi(y1,n+b112,y2,n+b122)]]>b3i=hdydxi(y1,n+b212,y2,n+b222)]]>b4i＝hdydxi(y1，n+b31，y2，n+b32)(8)yi,n+1=yi,n+b1i6+b2i3+b3i3+b4i6]]>其中i＝1，2，h为积分的时间步长，在ISR中断周期TItr足够小的情况下可以把h设为TItr，y1，n和y2，n分别为方程(5)中变量矢量两个分量在当前时刻的已知值，yi，n+1为相应变量矢量分量在与当前时刻相隔时间h的下一时刻的计算值。
实施例下面以主机和从机代码中关键函数的修改来说明该实时仿真调试方法的具体实施步骤。代码都采用类C语言的伪代码的方式表示，与方法实施无关而保持不变的代码部分用省略号“…”代表。
实施方法时，首先在头文件中定义两个编译变量#define DSPConfig#define SimulationConfigDSPConfig代表普通DSP可执行文件编译的配置，SimulationConfig代表实时仿真调试的配置，使用时根据需要将其中一行注释掉而保留另一行。
主机的主程序如下所示Main(){DSPWrite(CommandAddress，MotionCommand)；//向DSP的数据交换内存写入运动命令
MotionDone()；//检测从机是否完成所写入的运动命令}其中CommandAddress为从机DSP数据交换内存中运动命令的地址，MotionCommand为运动命令。
DSP内存写函数DSPWrite()如下所示DSPWrite(int Address，int Content){*(int*)DSPWriteAddressPort＝Address；//地址写入寄存器*(int*)DSPWriteContentPort＝Content；//内容写入寄存器}其中Address为写入地址，Content为写入内容，DSPWriteAddressPort为DSP内存写入地址寄存器的端口地址，DSPWriteContentPort为DSP内存写入内容寄存器的端口地址。方法实施后，DSPWrite()修改为DSPWrite(int Address，int Content){#ifdef DSP_config*(int*)DSPWriteAddressPort＝Address；//地址写入寄存器*(int*)DSPWriteContentPort＝Content；//内容写入寄存器#endif#ifdef simulation_config*(int*)Address＝Content；//内容直接写入地址指向的主机内存单元#endif}运动完成检测函数MotionDone()如下所示MotionDone(){ int Doneflag＝0；//将运动完成标志初始化为假while(！Doneflag){DSPRead(StatusAddress，&Doneflag)；//从DSP数据交换内存读出从机状态}
}其中StatusAddress为从机DSP数据交换内存中状态的地址，Doneflag为运动完成标志。方法实施后，MotionDone()修改为MotionDone(){ int Doneflag＝0；//将运动完成标志初始化为假while(！Doneflag){#ifdef simulation_configISR()；//调用中断服务例程#endifDSPRead(StatusAddress，&Doneflag)；//从DSP数据交换内存读出从机状态}}DSP内存读函数DSPRead()如下所示DSPRead(int Address，int*Destination){*(int*)DSPReadAddressPort＝Address；//地址写入寄存器*Destination＝*(int*)DSPReadContentPort；//寄存器内容读入目的地址}其中Address为写入地址，Destination为目的地址，DSPReadAddressPort为DSP内存读出地址寄存器的端口地址，DSPReadContentPort为DSP内存读出内容寄存器的端口地址。方法实施后，DSPRead()修改为DSPRead(int Address，int*Destination){#ifdef DSP_config*(int*)DSPReadAddressPort＝Address；//地址写入寄存器*Destination＝*(int*)DSPReadContentPort；//寄存器内容读入目的地址#endif#ifdef simulation_config*Destination＝*(int*)Address；//直接将地址指向的主机内存单元的内容读入目的地址
#endif}中断服务例程ISR()如下所示ISR(){ float x；x＝ReadEnc()；//通过编码器接口读取控制对象运动部件的当前位置？运动控制算法)WriteDA(I)；//通过D/A将电流信号传递给控制对象的电机}其中x为控制对象运动部件的当前位置，I为运动控制算法计算得出的电流控制输出量，ReadEnc()与WriteDA()之间的代码是运动控制算法。
编码器读函数ReadEnc()如下所示float ReadEnc(){ float EncValue；EncValue＝*(float*)EncValuePort；//读取编码器读数return EncValue；//返回编码器读数}其中EncValuePort为编码器计数值端口地址。方法实施后，ReadEnc()修改为float ReadEnc(){ float EncValue；#ifdef DSP_configEncValue＝*(float*)EncValuePort；//读取编码器读数return EncValue；//返回编码器读数#endif#ifdef simulation_configreturn xSim；//直接返回仿真计算的当前位置#endif}
其中xSim为存储仿真计算的当前位置的全局变量，将会在后面关于运动仿真函数MotionSim()的描述中详细解释。
D/A写函数WriteDA()如下所示WriteDA(float I){*(float*)DAValuePort＝I；//写入DA值寄存器}其中DAValuePort为DA值寄存器端口地址。方法实施后，WriteDA()修改为WriteDA(float I){#ifdef DSP_config*(float*)DAValuePort＝I；//写入DA值寄存器#endif#ifdef simulation_configMotionSim(I)；//调用运动仿真函数计算当前位置#endif}在定义MotionSim()前，首先定义两个全局变量float xSim，velSim；其中xSim为仿真计算的当前位置，velSim为仿真计算的当前速度。通常在仿真起始时xSim和velSim都初始化为0。
根据方程(5)和方程(7)定义由矢量[y1y2]和电流I计算导数矢量[dydt1dydt2]的函数derivative()如下derivative(float*y，I，float*dydt){dydt
＝y[1]；dydt[1]＝(Kf*I c*y[1]-k*y
)/m；}其中I，m，Kf，c，k的定义均与方程(7)中相同。
根据方程(8)定义运动仿真函数MotionSim()如下
MotionSim(float I){ int i；float y[2]，yt[2]，dydt[2]；float h；float b1[2]，b2[2]，b3[2]，b4[2]；h＝TItr；y
＝xSim；y[1]＝velSim；derivative(y，I，dydt)；for(i＝0；i＜2；i++)b1[i]＝h*dydt[i]；for(i＝0；i＜2；i++)yt[i]＝y[i]+0.5*b1[i]；derivative(yt，I，dydt)；for(i＝0；i＜2；i++)b2[i]＝h*dydt[i]；for(i＝0；i＜2；i++)yt[i]＝y[i]+0.5*b2[i]；derivative(yt，I，dydt)；for(i＝0；i＜2；i++)b3[i]＝h*dydt[i]；for(i＝0；i＜2；i++)yt[i]＝y[i]+b3[i]；derivative(yt，I，dydt)；for(i＝0；i＜2；i++)b4[i]＝h*dydt[i]；for(i＝0；i＜2；i++)y[i]＝y[i]+b1[i]/6+b2[i]/3+b3[i]/3+b4[i]/6；xSim＝y
；velSim＝y[1]；
}MotionSim()计算并最终更新当前仿真位置xSim和当前仿真速度velSim。
实施例参见图3实时仿真调试方法被应用在一个半导体生产设备的工作台上。图3给出了工作台的结构，它由沿相互垂直的方向运动的X滑块1、Y滑块2和各自的驱动电机X电机3和Y电机4组成。对于工作台X方向的运动，X滑块是运动控制系统中控制对象的运动部件，它的特征值如下所示m＝5kg，c＝0，k＝0。
X电机的特征值为Kf＝16.31N/A；按前面的说明对主机和DSP代码进行相应的修改，并将上列系统常数代入方程(7)，就可以对运动控制系统在主机上进行完全脱离DSP和控制对象(X滑块和X电机)的真实反映运动控制算法实时运行状况的仿真和调试。
参见图4，该图表示了对设备在X方向典型的18毫秒内完成2.54mm的运动的仿真结果。图中显示了仿真计算得到的运动过程中的X滑块的理想和实际位置以及两者之间的差值-跟随误差，X电机电流。各信号的显示比例是理想/实际位置1mm/格，跟随误差1mm/格，电机电流5A/格。实时仿真调试方法可以以同样的方式应用在工作台Y方向的运动上。
权利要求
1.一种实时运动控制算法仿真调试方法，包括主机、从机和控制对象，其特征在于该方法把从机DSP上运行的运动控制代码完整不变地转移到主机上运行，并采用数值仿真的方法来准确模拟控制对象的真实响应；首先，把从机DSP上定义的用于与主机进行数据交换的内存区间转移定义到主机的内存映射中，并将相应的主机对DSP内存区间读写的函数修改为新的函数，新的函数对新定义的主机内存映射进行操作；其次，将运动控制代码中通过输出接口更新驱动电机电流信号的函数修改为根据代码计算的输出量用数值积分方法来计算控制对象的响应，并存储在相应的内存单元中；同时，把代码中通过输入接口读取系统当前位置的函数修改为新的函数，新的函数从存储控制对象响应的数值计算结果的内存单元中读取系统当前位置；最后，在主机中循环检测从机状态，等待从机完成运动命令的函数中加入对从机DSP代码ISR的调用，以达到运动控制算法的反复运行。
2.一种实时运动控制算法仿真调试方法，包括主机、从机和控制对象，其特征在于主机CPU中运行的主程序调用DSP内存写函数DSPWrite()，向从机DSP的数据交换内存写入运动命令，之后，运行运动完成检测函数MotionDone()，来检测从机是否完成所写入的运动命令；MotionDone()调用函数DSP内存读函数DSPRead()自从机DSP的数据交换内存中读出从机状态，判断运动命令是否完成，MotionDone()循环运行直至运动完成为止；从机DSP中的中断服务例程ISR首先调用编码器读函数ReadEnc()通过编码器接口读取控制对象运动部件的当前位置，其后经过运动控制算法的计算得出新的电流控制输出量，然后运行D/A写函数WriteDA()通过D/A将这一电流信号传递给控制对象的电机；ISR运行的过程中通过DSP的数据交换内存接受主机CPU的运动命令并将包含命令执行结果的自身状态反馈给主机CPU；ISR在时钟中断的精确触发下，每隔一个中断周期TItr运行一次，一直循环持续下去；控制对象的电机以从机DSP规定的电流驱动运动部件到达当前位置。
3.一种实时运动控制算法仿真调试方法，包括主机、从机和控制对象，其特征在于在主机CPU中运行的主程序基础之上，把从机DSP上运行的ISR代码转移到主机CPU上运行，同时将DSP中的数据交换内存也转移定义到主机的内存映射中；将主程序中的函数DSPWrite()和DSPRead()分别修改为对定义在主机上的数据交换内存进行写入和读出；在主程序的函数MotionDone()中加入对ISR()的调用，以使ISR()随函数MotionDone()反复循环运行直至运动完成为止；将ISR()中的函数WriteDA()修改为调用运动仿真函数MotionSim()；函数MotionSim()根据运动控制算法计算的电流控制输出量用四阶龙格—库塔法计算控制对象的响应并存储在位置内存中；把ISR()中的函数ReadEnc()修改为从位置内存中读取系统当前位置；ISR()中ReadEnc()与WriteDA()之间的代码是运动控制算法的主体，在从DSP到主机的转移中保持完整不变；仿真调试方案中内存定义和函数的少量修改通过条件编译命令实现，达到同一源文件用于DSP可执行文件编译和运动控制算法实时仿真调试；方案中位置内存里储存的控制对象位置及其导出量与各种数据可视化手段相结合，形成完全脱离从机和控制对象的运动控制算法虚拟实时仿真调试环境。
4.根据权利要求3所述的实时运动控制算法仿真调试方法，其特征在于上述用于计算控制对象响应的四阶龙格—库塔法是准确数值积分方法，适用于任何可以用状态空间中表达，即动态特性可以写成一个线性一阶常微分方程组的系统；一个运动控制系统中典型的单自由度质量—弹簧—阻尼运动部件的动力学方程可以写为md2xd2t+cdxdt+kx=F----(1)]]>其中x为运动部件的位置，t为时间，m为运动部件的质量，c为阻尼系数，k为弹簧刚度，F为作用于运动部件的电机驱动力；方程(1)可以重写为d2xd2t=Fmcmdxdtkmx----(2)]]>电机线圈内的电流和电机的驱动力之间存在以下关系F＝KfI(3)其中F为电机驱动力，I为运动控制算法计算得出的电流控制输出量，Kf为电机的力敏感度；将方程(3)代入方程(2)可以得到d2xd2t=KfImcmdxdtkmx----(4)]]>定义变量矢量如下y1y2=xdxdt----(5)]]>定义相应的变量导数矢量如下 将方程(4)代入方程(6)可以得到 (7)方程(5)和方程(7)给出了单自由度二阶线性动态系统的状态空间表达；根据四阶龙格—库塔法的定义，控制对象的响应通过以下数值积分方法计算b1i＝hdydxi(y1，n，y2，n)b2i=hdydxi(y1,n+b112,y2,n+b122)]]>b3i=hdydxi(y1,n+b212,y2,n+b222)]]>b4i＝hdydxi(y1，n+b31，y2，n+b32)yi,n+1=yi,n+b1i6+b2i3+b3i3+b4i6]]>(8)其中i＝1，2，h为积分的时间步长，在ISR中断周期TItr足够小的情况下可以把h设为TItr，y1，n和y2，n分别为方程(5)中变量矢量两个分量在当前时刻的已知值，yi，n+1为相应变量矢量分量在与当前时刻相隔时间h的下一时刻的计算值。
5.根据权利要求3所述的实时运动控制算法仿真调试方法，其特征在于上述导出量是速度或加速度。
6.一种根据权利要求1-5任一项所述实时运动控制算法仿真调试方法的应用方法，其特征在于上述实时仿真调试方法被应用在一个半导体生产设备的工作台上，工作台的由沿相互垂直的方向运动的X滑块、Y滑块和各自的驱动电机X电机和Y电机组成，对于工作台X方向的运动，X滑块是运动控制系统中控制对象的运动部件，它的特征值如下所示m＝5kg，c＝0，k＝0；X电机的特征值为Kf＝16.31N/A；对主机和DSP代码进行相应的修改，并将上列系统常数代入方程(7)，就能对运动控制系统在主机上进行完全脱离DSP和控制对象，即X滑块和X电机，的真实反映运动控制算法实时运行状况的仿真和调试。
全文摘要
一种实时运动控制算法仿真调试方法，把从机上运行的运动控制代码完整不变地转移到主机上运行，并采用数值仿真的方法来模拟控制对象的真实响应；先把从机定义的内存区间转移定义到主机的内存映射中，并将主机对DSP内存区间读写的函数修改为对新定义的主机内存映射进行操作；然后将运动控制代码中通过输出接口更新驱动电机电流信号的函数修改为根据代码计算的输出量用数值积分方法计算控制对象的响应，并存储在内存单元中；同时把代码中通过输入接口读取系统当前位置的函数修改为从存储控制对象响应的数值计算结果的内存单元中读取系统当前位置；最后由主机循环检测从机状态，等待从机完成运动命令的函数中加入对从机DSP代码ISR的调用。
文档编号G06F9/00GK1770046SQ200510200639
公开日2006年5月10日 申请日期2005年10月24日 优先权日2005年10月24日
发明者何田 申请人:中国电子科技集团公司第四十五研究所