专利名称：基于双dsp的3自由度平面电机伺服控制器的制作方法
技术领域：
本发明是一种3自由度平面电机的伺服控制器。
二背景技术：
随着先进制造业的快速发展，高精度定位平台技术得到了深入的研究和发 展，在半导体产业、微立体光刻、纳米工作台、高精度绘图仪等领域具有广阔 的应用前景。高精度定位平台除需实现水平x、 y轴的大行程运动外，往往还
需实现绕坐标轴的微小转动，例如绕z轴的转动ez，本发明的控制对象3自由 度平面电机就是一种能够进行x、 y、 ez三自由度快速精确定位的工作平台。
平面电机的电磁执行器为4组3相直线电机，平面电机控制器需实现4组 直线电机执行器和3个运动自由度的同步控制，需采集12路电流信号、3路位 置信号，输出24路PWM信号，还需实现轨迹规划、数字滤波、位置伺服、电 流控制等算法，单DSP控制器结构不但无法满足AD采样和PWM输出所需通 道数目，且无法在规定的控制周期内完成所需的控制算法，所以提出了双DSP 的平面电机控制器结构和3自由度运动的双DSP同步控制策略；双DSP间的 信息交换通过CAN网络实现，和传统的RS232和RS485等通讯方式比较，CAN 通讯速度快，抗干扰能力强，便于实现网络联结，为平面电机进入复杂系统提 供了条件。
三自由度平面电机控制需实现x轴大行程位移、y轴大行程位移、绕z轴 微小转动ez的非接触测量，国外的定位平台常采用激光干涉测距系统，分辨率 达纳米级别，价格约6万美金，对一般定位平台应用来说，价格过于昂贵，平 面电机控制器使用3组激光位置传感器和对应传感器方程实现3个运动自由度 位置的非接触测量，分辨率达到lpm，价格约0.3万美元；平面电机控制器需 对12路绕组电流进行精密控制，国外定位平台控制系统常采用功率运放电路实 现的电流PI (比例积分)控制电路，缺点是模拟电路参数难以确定，且易受环 境干扰，平面电机控制器中电流采样由霍尔电流传感器完成，功率放大由PWM 斩波实现，电流PI控制算法由DSP实现。
三
发明内容
本发明提供一种基于双DSP的3自由度平面电机伺服控制器，控制平面电
8机进行X轴、Y轴、6z三个运动自由度的快速精确定位。为达上述目的，本发 明采用的技术方案是-
一种基于双DSP的3自由度平面电机伺服控制器，其特征在于包括主控单 元和从控单元。所述的主控单元包括主DSP、主缓冲驱动模块、十二路主光耦 隔离驱动模块、六相主全桥功率驱动模块、用于采样主控单元输出的六相平面 电机驱动电流的主霍尔电流传感器模块及用于采样第一路、第二路平面电机X 轴位置的1号X轴激光位置传感器、2号X轴激光位置传感器，主DSP包括主 DSP片上ADC模块、主DSP片上PWM模块及主DSP片上CAN模块；主DSP 片上PWM模块的PWM信号输出端与主缓冲驱动模块的输入端连接，主缓冲 驱动模块的输出端与主光耦隔离驱动模块的输入端连接，主光耦隔离驱动模块 的输出端与主全桥功率驱动模块的输入端连接，主全桥功率驱动模块的六相输 出电流为主控单元输出的六相平面电机驱动电流，主霍尔电流传感器模块的六 路电流采样信号输出端分别与主DSP片上ADC模块的第一到第六输入端连接， 1号X轴激光位置传感器的位置采样信号输出端与主DSP片上ADC模块的第 七输入端连接，2号X轴激光位置传感器的位置采样信号输出端与主DSP片上 ADC模块的第八输入端连接。所述的从控单元包括从DSP、从缓冲驱动模块、 十二路从光耦隔离驱动模块、六相从全桥功率驱动模块、用于采样从控单元输 出的六相平面电机驱动电流的从霍尔电流传感器模块及用于采样第一路平面电 机Y轴位置的1号Y轴激光位置传感器，从DSP包括从DSP片上ADC模块、 从DSP片上PWM模块及从DSP片上CAN模块；从DSP片上PWM模块的 PWM信号输出端与从缓冲驱动模块的输入端连接，从缓冲驱动模块的输出端 与从光耦隔离驱动模块的输入端连接，从光耦隔离驱动模块的输出端与从全桥 功率驱动模块的输入端连接，从全桥功率驱动模块的六相输出电流为从控单元
输出的六相平面电机驱动电流，从霍尔电流传感器模块的六路电流采样信号输 出端分别与从DSP片上ADC模块的第一到第六输入端连接，1号Y轴激光位 置传感器的位置采样信号输出端与从DSP片上ADC模块的第七输入端连接。 主控单元的主DSP片上CAN模块通过CAN总线与从控单元的从DSP片上CAN 模块连接。
所述的主DSP定时等待控制周期开始后，根据轨迹规划算法计算平面电机 动子平台质心X轴位置参考值Xpr、平面电机动子平台质心Y轴位置参考值ypr、 平面电机动子平台绕Z轴旋转的角度位置参考值&pr，通过CAN总线发送同步
标志Fsyn给从DSP，并等待从DSP发送的同步确认标志Asyn;从DSP等待并收到同步标志Fsyn后，通过CAN总线发送同步确认标志A，给主DSP，读取2 号、4号直线电机执行器的电流采样值，读取1号Y轴激光位置传感器的采样 但Sy,，对采样值进行数字滤波、校正，接着等待主DSP发送的平面电机动子 平台质心Y轴位置参考值r^和平面电机动子平台质心Y轴位置当前值校正项 ypcali;主DSP等待并收到同步确认标志Asyn后，读取1号、3号直线电机执行 器的电流采样值，读取1号、2号X轴激光位置传感器的采样值5^、 5k2，对 采样值进行数字滤波、校正，根据1号、2号X轴激光位置传感器的采样值5^、 Sxi和传感器方程计算平面电机动子平台质心X轴位置当前值Xp、平面电机动 子平台绕Z轴旋转的角度位置当前值&p、平面电机动子平台质心Y轴位置当 前值校正项！^ah，再通过CAN总线发送平面电机动子平台质心Y轴位置参考
值！^和平面电机动子平台质心Y轴位置当前值校正项Fpeau给从DSP，接着等
待从DSP发送的平面电机动子平台质心Y轴位置当前值Ip;从DSP等待并收 到平面电机动子平台质心Y轴位置参考值]V和平面电机动子平台质心Y轴位 置当前值校正项yp。au后，根据1号Y轴激光位置传感器的采样值SY1、平面电
机动子平台质心Y轴位置当前值校正项FpcaH和传感器方程计算平面电机动子平
台质心Y轴位置当前值yp，再通过CAN总线发送Fp给主DSP，并进行Y轴自 由度位置、速度控制，进行2号、4号直线电机执行器电流控制，接着等待下 一个控制周期的同步标志Fsyn;主DSP等待并收到平面电机动子平台质心Y轴 位置当前值yp后，进行X轴和6z自由度位置、速度控制，并进行1号、3号直 线电机执行器电流控制，接着定时等待下一个控制周期的开始。 与现有技术相比，本发明具有如下优点-
本发明可控制平面电机进行水平X、 Y轴的平移运动和绕Z轴的微小转动 9z，行程为40mmX40画X士20mrad，误差为i5pmX士5nmX士0.05mrad。
对本发明采用的双DSP控制器结构及双DSP同步控制策略的优点分析如 下平面电机的电磁执行器为4组3相直线电机执行器，平面电机控制器需实 现4组直线电机执行器和3个运动自由度的同步控制，需采集12路电流信号、 3路位置信号，输出24路PWM信号，另外还需实现轨迹规划、数字滤波、位 置伺服、电流控制等算法，单DSP控制器结构不但无法满足AD采样和PWM 输出所需的通道数目，且无法在规定的控制周期内完成所需的控制算法，本发 明提出了双DSP控制器结构及双DSP同步控制策略，其中，双DSP的片上ADC 和PWM路数满足了平面电机控制的需求，通过双DSP的并行计算和同步控制， 可实现平面电机三个自由度的快速精确定位，三自由度定位实验曲线参见图6，其中X轴10mm阶跃响应的调节时间小于750ms,稳态误差的平均值小于 ±5pm; Y轴10mm阶跃响应的调节时间小于750ms，稳态误差的平均值小于 ±5拜；ez自由度lOmrad阶跃响应的调节时间小于750ms，稳态误差的平均值 小于士0.05mrad。
双DSP的信息交换通过CAN网络实现，和传统的RS232、 RS485等通讯 方式比较，CAN通讯速度快，抗干扰能力强，便于实现网络联结，为平面电机 进入复杂系统提供了条件。
平面电机控制需实现X轴大行程位移、Y轴大行程位移、绕Z轴微小转动 6z的非接触测量，国外高精度定位平台常采用激光干涉测距系统，分辨率达纳 米级别，价格约6万美金，对一般应用来说，价格过于昂贵，本发明使用3组 激光位置传感器和对应的传感器方程实现3个自由度位置的非接触测量，3个 自由度分辨率达到lpm、 lpm、 0.02mrad，价格约0.3万美元，大幅降低了位置 传感器系统的成本。
平面电机控制器需对4组直线电机执行器的12路绕组电流进行精密控制， 国外定位平台控制系统常采用功率运放电路实现的电流PI控制电路，缺点是模 拟电路参数难以确定，且易受环境干扰，本发明中电流采样由霍尔电流传感器 完成，功率放大由PWM斩波实现，电流PI控制算法由DSP实现，其中使用 霍尔电流传感器进行电流测量与反馈，相比串入反馈电阻进行电流测量，反馈 精度高，温漂小；使用PWM方式进行电流控制，精度高，响应快，抗干扰能 力强；使用光电耦合器实现控制电路和功率驱动电路之间的电气隔离。 四
图l是本发明的结构简图。
图2是本发明的电路框图。
图3是本发明的主从DSP同步控制策略流程图。
图4是本发明的电路原理图，其中图4a是主控单元电路原理图，图4b是 从控单元电路原理图。
图5是本发明的位置传感器阵列图。 图6是本发明的三自由定位实验曲线。 附图中的主要标号有-
I 主控单元 2 从控单元
II 主DSP 21 从DSP
12 主缓冲驱动模块 22 从缓冲驱动模块13主光耦隔离驱动模块23从光耦隔离驱动模块
14主全桥功率驱动模块24从全桥功率驱动模块
15主霍尔电流传感器模块25从霍尔电流传感器模块
161号X轴激光位置传感器261号Y轴激光位置传感器
172号X轴激光位置传感器M平面电机
Ll 丄41 4号直线电机执行器
Dl串行通讯接口D2 控制器局域网通讯接口
Ul -U4缓冲驱动器U5 U16光电耦合器
U17' U28全桥功率驱动器U29'、U40霍尔电流传感器
PI动子平台初始位置Cl动子平台质心初始位置
P2动子平台平移后位置C2动子平台质心平移后位置
P3动子平台最终位置
五具体实施例方式
下面结合附图对本发明做详细说明。
参见图l， 一种基于双DSP的3自由度平面电机伺服控制器，其特征在于 包括主控单元1和从控单元2。
参见图2，所述的主控单元1包括主DSPll、主缓冲驱动模块12、十二路 主光耦隔离驱动模块13、六相主全桥功率驱动模块14、用于采样主控单元输出 的六相平面电机驱动电流的主霍尔电流传感器模块15及用于采样第一路、第二 路平面电机X轴位置的1号X轴激光位置传感器16、 2号X轴激光位置传感 器17，主DSP11包括主DSP片上ADC模块、主DSP片上PWM模块及主DSP 片上CAN模块；主DSP片上PWM模块的PWM信号输出端与主缓冲驱动模 块12的输入端连接，主缓冲驱动模块12的输出端与主光耦隔离驱动模块13的 输入端连接，主光耦隔离驱动模块13的输出端与主全桥功率驱动模块14的输 入端连接，主全桥功率驱动模块14的六相输出电流为主控单元输出的六相平面 电机驱动电流，主霍尔电流传感器模块15的六路电流采样信号输出端分别与主 DSP片上ADC模块的第一到第六输入端连接，1号X轴激光位置传感器16的 位置采样信号输出端与主DSP片上ADC模块的第七输入端连接，2号X轴激 光位置传感器17的位置采样信号输出端与主DSP片上ADC模块的第八输入端 连接。所述的从控单元2包括从DSP21、从缓冲驱动模块22、十二路从光耦隔 离驱动模块23、六相从全桥功率驱动模块24、用于采样从控单元输出的六相平 面电机驱动电流的从霍尔电流传感器模块25及用于采样第一路平面电机Y轴位置的1号Y轴激光位置传感器26，从DSP21包括从DSP片上ADC模块、 从DSP片上PWM模块及从DSP片上CAN模块；从DSP片上PWM模块的 PWM信号输出端与从缓冲驱动模块22的输入端连接，从缓冲驱动模块22的 输出端与从光耦隔离驱动模块23的输入端连接，从光耦隔离驱动模块23的输 出端与从全桥功率驱动模块24的输入端连接，从全桥功率驱动模块24的六相 输出电流为从控单元输出的六相平面电机驱动电流，从霍尔电流传感器模块25 的六路电流采样信号输出端分别与从DSP片上ADC模块的第一到第六输入端 连接，1号Y轴激光位置传感器26的位置采样信号输出端与从DSP片上ADC 模块的第七输入端连接。主控单元1的主DSP片上CAN模块通过CAN总线与 从控单元2的从DSP片上CAN模块连接。
参见图3，主从DSP的同步控制策略可描述如下主从DSP通过CAN总 线交换同步控制信号，也可通过双端口 RAM交换同步控制信号；所述的主 DSP11定时等待控制周期开始后，根据轨迹规划算法计算平面电机动子平台质
心X轴位置参考值Xp。平面电机动子平台质心Y轴位置参考值7pr、平面电机
动子平台绕Z轴旋转的角度位置参考值&pr，通过CAN总线发送同步标志Fsyn 给从DSP21，并等待从DSP21发送的同步确认标志Asyn;从DSP21等待并收 到同步标志Fsyn后，通过CAN总线发送同步确认标志A^给主DSP11 ，读取2 号、4号直线电机执行器的电流采样值，读取1号Y轴激光位置传感器26的采 样值Sw,对采样值进行数字滤波、校正，接着等待主DSPll发送的平面电机 动子平台质心Y轴位置参考值JV和平面电机动子平台质心Y轴位置当前值校
正项7peali;主DSPll等待并收到同步确认标志Asyn后，读取1号、3号直线电
机执行器的电流采样值，读取1号、2号X轴激光位置传感器的采样值Sx"Sx2， 对采样值进行数字滤波、校正，根据1号、2号X轴激光位置传感器的采样值
SX1、 Sx2和传感器方程计算平面电机动子平台质心X轴位置当前值Xp、平面电
机动子平台绕Z轴旋转的角度位置当前值0zp、平面电机动子平台质心Y轴位 置当前值校正项Fpcali，再通过CAN总线发送平面电机动子平台质心Y轴位置 参考值！^和平面电机动子平台质心Y轴位置当前值校正项Fpcaii给从DSP21， 接着等待从DSP21发送的平面电机动子平台质心Y轴位置当前值&从DSP21 等待并收到平面电机动子平台质心Y轴位置参考值1V和平面电机动子平台质 心Y轴位置当前值校正项ypeali后，根据1号Y轴激光位置传感器26的采样值
SY1、平面电机动子平台质心Y轴位置当前值校正项Fpdi和传感器方程计算平
面电机动子平台质心Y轴位置当前值yp，再通过CAN总线发送&给主DSP11 ，并进行Y轴自由度位置、速度控制，进行2号、4号直线电机执行器电流控制， 接着等待下一个控制周期的同步标志F，；主DSPll等待并收到;Tp后，进行X 轴和ez自由度位置、速度控制，并进行1号、3号直线电机执行器电流控制， 接着定时等待下一个控制周期的开始。提到的轨迹规划算法可采用美国德州农 机大学博士学位论文《DEVELOPMENT OF NOVEL HIGH-PERFORMANCE SIX-AXIS MAGNETICALLY LEVITATED INSTRUMENTS FOR NANOSCALE APPLICATIONS))所公开的方法；传感器方程可采用美国MIT大学博士学位论 文《Precision six陽degree-of-freedom magnetically levitated photolithography stage》 所公开的方法；电流、位置、速度控制可采用专著《电动机的DSP控制》所公 开的方法。
参见图4a，所述的主控单元l包括主DSPll，型号为TMS320F2812，主 缓冲驱动模块12由主第一缓冲驱动器U1、主第二缓冲驱动器U2组成，型号 为SN74LS244，主光耦驱动隔离模块13由主第一光电耦合器U5、主第二光电 耦合器U6、主第三光电耦合器U7、主第四光电耦合器U8、主第五光电耦合器 U9、主第六光电耦合器U10组成，型号为HCPL0631，主全桥功率驱动模块14 由主第一全桥功率驱动器U17、主第二全桥功率驱动器U18、主第三全桥功率 驱动器U19、主第四全桥功率驱动器U20、主第五全桥功率驱动器U21、主第 六全桥功率驱动器U22组成，型号为L298，主霍尔电流传感器模块15由主第 一霍尔电流传感器U29、主第二霍尔电流传感器U30、主第三霍尔电流传感器 U31、主第四霍尔电流传感器U32、主第五霍尔电流传感器U33、主第六霍尔 电流传感器U34组成，型号为LTSR6-NP， 1号X轴激光位置传感器16， 2号 X轴激光位置传感器17，型号为LG10A65PI。其中主DSPll的片上PWM模 块的PWM信号输出端"PWM1 PWM6"分别与主缓冲驱动模块12的主第一缓冲 驱动器U1的输入端"1A1 1A4"、 2A1、 2A2连接，主DSPll片上PWM模块的 PWM信号输出端"PWM7 PWM12"分别与主缓冲驱动模块12的主第二缓冲驱 动器U2的输入端"1A1 1A4"、 2A1、 2A2连接；主缓冲驱动模块12的主第一 缓冲驱动器U1的输出端"1Y1 1Y4"、 2Y1、 2Y2分别与主光耦驱动隔离模块13 的主第一光电耦合器U5、主第二光电耦合器U6、主第三光电耦合器U7的输 入端VFl-、 VF2-连接，主缓冲驱动模块12的主第二缓冲驱动器U2的输出端 "1Y1 1Y4"、2Y1、2Y2分别与主光耦驱动隔离模块13的主第四光电耦合器U8、 主第五光电耦合器U9、主第六光电耦合器U10的输入端VFl-、 VF2-连接；主 光耦驱动隔离模块13的主第一光电耦合器U5、主第二光电耩合器U6、主第三光电耦合器U7、主第四光电耦合器U8、主第五光电耦合器U9、主第六光电耦 合器UIO的输出端Vol、 Vo2分别与主全桥功率驱动模块14的主第一全桥功率 驱动器U17、主第二全桥功率驱动器U18、主第三全桥功率驱动器U19、主第 四全桥功率驱动器U20、主第五全桥功率驱动器U21、主第六全桥功率驱动器 U22的输入端IN1、 IN2连接；主全桥功率驱动模块14的主第一全桥功率驱动 器U17、主第二全桥功率驱动器U18、主第三全桥功率驱动器U19、主第四全 桥功率驱动器U20、主第五全桥功率驱动器U21、主第六全桥功率驱动器U22 的输出端0UT1分别与主霍尔电流传感器模块15的主第一霍尔电流传感器 U29、主第二霍尔电流传感器U30、主第三霍尔电流传感器U31、主第四霍尔 电流传感器U32、主第五霍尔电流传感器U33、主第六霍尔电流传感器U34的 输入端1连接，主全桥功率驱动模块14的主第一全桥功率驱动器U17、主第二 全桥功率驱动器U18、主第三全桥功率驱动器U19的输出端OUT2分别与1号 直线电机执行器L1的输入端"-INl—IN3"连接，主全桥功率驱动模块14的主第 四全桥功率驱动器U20、主第五全桥功率驱动器U21、主第六全桥功率驱动器 U22的输出端OUT2分别与3号直线电机执行器L3的输入端"-INl -IN3"连接； 主霍尔电流传感器模块15的主第一霍尔电流传感器U29、主第二霍尔电流传感 器U30、主第三霍尔电流传感器U31、主第四霍尔电流传感器U32、主第五霍 尔电流传感器U33、主第六霍尔电流传感器U34的3号脚分别与各自的5号脚 连接，2号脚分别与各自的6号脚连接，输出端OUT分别与主DSPll的片上 ADC模块的输入端"ADCINA0 ADCINA5"连接，主霍尔电流传感器模块15的 主第一霍尔电流传感器U29、主第二霍尔电流传感器U30、主第三霍尔电流传 感器U31的输出端4分别与1号直线电机执行器L1的输入端"+INl—IN3"连接， 主霍尔电流传感器模块15的主第四霍尔电流传感器U32、主第五霍尔电流传感 器U33、主第六霍尔电流传感器U34的输出端4分别与3号直线电机执行器L3 的输入端"+INl +IN3"连接；1号X轴激光位置传感器16、 2号X轴激光位置 传感器17的输出端Wh分别连接到主DSP11的片上ADC模块的输入端 ADCINA6、 ADCINA7;主DSP11的片上CAN模块连接到主控制器局域网通 讯接口 D2。
参见图4b，所述的从控单元2包括从DSP21，型号为TMS320F2812，
从缓冲驱动模块22由从第一缓冲驱动器U3、从第二缓冲驱动器U4组成，型 号为SN74LS244，从光耦驱动隔离模块23由从第一光电耦合器Ull、从第二 光电耦合器U12、从第三光电耦合器U13、从第四光电耦合器U14、从第五光电耦合器U15、从第六光电耦合器U16组成，型号为HCPL0631，从全桥功率 驱动模块24由从第一全桥功率驱动器U23、从第二全桥功率驱动器U24、从第 三全桥功率驱动器U25、从第四全桥功率驱动器U26、从第五全桥功率驱动器 U27、从第六全桥功率驱动器U28组成，型号为L298，从霍尔电流传感器模块 25由从第一霍尔电流传感器U35、从第二霍尔电流传感器U36、从第三霍尔电 流传感器U37、从第四霍尔电流传感器U38、从第五霍尔电流传感器U39、从 第六霍尔电流传感器U40组成，型号为LTSR6-NP， 1号Y轴激光位置传感器 26，型号为LG10A65PI。其中从DSP21的片上PWM模块的PWM信号输出 端"PWM1 PWM6"分别与从缓冲驱动模块22的从第一缓冲驱动器U3的输入端 "1A1 1A4"、 2A1、 2A2连接，从DSP21的片上PWM模块的PWM信号输出 端"PWM7 PWM12"分别与从缓冲驱动模块22的从第二缓冲驱动器U4的输入 端"1A1 1A4"、 2A1、 2A2连接；从缓冲驱动模块22的从第一缓冲驱动器U3 的输出端"1Y1 1Y4"、 2Y1、 2Y2分别与从光耦驱动隔离模块23的从第一光电 耦合器Ull、从第二光电耦合器U12、从第三光电耦合器U13的输入端VFl-、 VF2-连接，从缓冲驱动模块22的从第二缓冲驱动器U4的输出端"l Y1 1Y4"、 2Y1、 2Y2分别与从光耦驱动隔离模块23的从第四光电耦合器U14、从第五光 电耦合器U15、从第六光电耦合器U16的输入端VFl-、 VF2-连接；从光耦驱 动隔离模块23的从第一光电耦合器Ull、从第二光电耦合器U12、从第三光电 耦合器U13、从第四光电耦合器U14、从第五光电耦合器U15、从第六光电耦 合器U16的输出端Vol 、 Vo2分别与从全桥功率驱动模块24的从第一全桥功率 驱动器U23、从第二全桥功率驱动器U24、从第三全桥功率驱动器U25、从第 四全桥功率驱动器U26、从第五全桥功率驱动器U27、从第六全桥功率驱动器 U28的输入端IN1 、 IN2连接；从全桥功率驱动模块24的从第一全桥功率驱动 器U23、从第二全桥功率驱动器U24、从第三全桥功率驱动器U25、从第四全 桥功率驱动器U26、从第五全桥功率驱动器U27、从第六全桥功率驱动器U28 的输出端0UT1分别与从霍尔电流传感器模块25的从第一霍尔电流传感器 U35、从第二霍尔电流传感器U36、从第三霍尔电流传感器U37、从第四霍尔 电流传感器U38、从第五霍尔电流传感器U39、从第六霍尔电流传感器U40的 输入端1连接，从全桥功率驱动模块24的从第一全桥功率驱动器U23、从第二 全桥功率驱动器U24、从第三全桥功率驱动器U25的输出端OUT2分别与2号 直线电机执行器L2的输入端"-INl -IN3"连接，从全桥功率驱动模块24的从第 四全桥功率驱动器U26、从第五全桥功率驱动器U27、从第六全桥功率驱动器U28的输出端0UT2分别与4号直线电机执行器L4的输入端"-INl -IN3"连接； 从霍尔电流传感器模块25的从第一霍尔电流传感器U35、从第二霍尔电流传感 器U36、从第三霍尔电流传感器U37、从第四霍尔电流传感器U38、从第五霍 尔电流传感器U39、从第六霍尔电流传感器U40的3号脚分别与各自的5号脚 连接，2号脚分别与各自的6号脚连接，输出端OUT分别与从DSP21的片上 ADC模块的输入端"ADCINA0 ADCINA5"连接，从霍尔电流传感器模块25的 从第一霍尔电流传感器U35、从第二霍尔电流传感器U36、从第三霍尔电流传 感器U37的输出端4分别与2号直线电机执行器L2的输入端"+INl4lN3"连接， 从霍尔电流传感器模块25的从第四霍尔电流传感器U38、从第五霍尔电流传感 器U39、从第六霍尔电流传感器U40的输出端4分别与4号直线电机执行器L4 的输入端"+INl +IN3"连接；1号Y轴激光位置传感器26的输出端Wh连接到 从DSP21的片上ADC模块输入端ADCINA6;从DSP21的片上CAN模块连接 到从控制器局域网通讯接口 D3;主控制器局域网通讯接口 D2和从控制器局域 网通讯接口 D3通过双绞线互连。
参见图5，所述的1号X轴激光位置传感器16、 2号X轴激光位置传感器 17、 1号Y轴激光位置传感器26构成位置传感器阵列；1号X轴激光位置传感 器16的采样值为S^， 2号X轴激光位置传感器17的采样值为Sx2， 1号Y轴 激光位置传感器26的采样值为&1; XYZ坐标系的原点固结于空间O点，平面 电机动子平台处于初始位置Pl时，动子平台质心初始位置Cl和O点重合，此 时Sxl=Sx2=X()， SY1=YQ，激光位置传感器激光束和动子平台质心初始位置Cl 的距离为/sl、 /s2、 /s3;动子平台的运动可由平移运动和旋转运动合成，动子平 台首先平移到平移后位置P2，动子平台质心移动到平移后位置C2，然后绕过 C2且和Z轴平行的轴旋转到最终位置P3，此时动子平台质心X轴、Y轴位置 当前值为Xp、 rp，绕Z轴旋转的角度位置当前值为&p;将Sx" Sx2代入传感 器方程可得A、 &p、平面电机动子平台质心Y轴位置当前值校正项ypeaii;将
SY1、 Fpeau代入传感器方程可得yp。激光位置传感器阵列的传感器方程为
0Zp = (^X2 — SX1 )/(7S2 + ^S1) 。
、,《广A)x
= -K + SY1 — J^a!i
权利要求
1. 一种基于双DSP的3自由度平面电机伺服控制器，其特征在于，包括主控单元(1)和从控单元(2)，所述的主控单元(1)包括主DSP(11)、主缓冲驱动模块(12)、十二路主光耦隔离驱动模块(13)、六相主全桥功率驱动模块(14)、用于采样主控单元输出的六相平面电机驱动电流的主霍尔电流传感器模块(15)及用于采样第一路、第二路平面电机X轴位置的1号X轴激光位置传感器(16)、2号X轴激光位置传感器(17)，主DSP(11)包括主DSP片上ADC模块、主DSP片上PWM模块及主DSP片上CAN模块，主DSP片上PWM模块的PWM信号输出端与主缓冲驱动模块(12)的输入端连接，主缓冲驱动模块(12)的输出端与主光耦隔离驱动模块(13)的输入端连接，主光耦隔离驱动模块(13)的输出端与主全桥功率驱动模块(14)的输入端连接，主全桥功率驱动模块(14)的六相输出电流为主控单元输出的六相平面电机驱动电流，主霍尔电流传感器模块(15)的六路电流采样信号输出端分别与主DSP片上ADC模块的第一到第六输入端连接，1号X轴激光位置传感器(16)的位置采样信号输出端与主DSP片上ADC模块的第七输入端连接，2号X轴激光位置传感器(17)的位置采样信号输出端与主DSP片上ADC模块的第八输入端连接，所述的从控单元(2)包括从DSP(21)、从缓冲驱动模块(22)、十二路从光耦隔离驱动模块(23)、六相从全桥功率驱动模块(24)、用于采样从控单元输出的六相平面电机驱动电流的从霍尔电流传感器模块(25)及用于采样第一路平面电机Y轴位置的1号Y轴激光位置传感器(26)，从DSP(21)包括从DSP片上ADC模块、从DSP片上PWM模块及从DSP片上CAN模块，从DSP片上PWM模块的PWM信号输出端与从缓冲驱动模块(22)的输入端连接，从缓冲驱动模块(22)的输出端与从光耦隔离驱动模块(23)的输入端连接，从光耦隔离驱动模块(23)的输出端与从全桥功率驱动模块(24)的输入端连接，从全桥功率驱动模块(24)的六相输出电流为从控单元输出的六相平面电机驱动电流，从霍尔电流传感器模块(25)的六路电流采样信号输出端分别与从DSP片上ADC模块的第一到第六输入端连接，1号Y轴激光位置传感器(26)的位置采样信号输出端与从DSP片上ADC模块的第七输入端连接，主控单元(1)的主DSP片上CAN模块通过CAN总线与从控单元(2)的从DSP片上CAN模块连接，所述的主DSP(11)定时等待控制周期开始后，根据轨迹规划算法计算平面电机动子平台质心X轴位置参考值Xpr、平面电机动子平台质心Y轴位置参考值Ypr、平面电机动子平台绕Z轴旋转的角度位置参考值θZpr，通过CAN总线发送同步标志Fsyn给从DSP(21)，并等待从DSP(21)发送的同步确认标志Asyn；从DSP(21)等待并收到同步标志Fsyn后，通过CAN总线发送同步确认标志Asyn给主DSP(11)，读取2号、4号直线电机执行器的电流采样值，读取1号Y轴激光位置传感器(26)的采样值SY1，对采样值进行数字滤波、校正，接着等待主DSP(11)发送的平面电机动子平台质心Y轴位置参考值Ypr和平面电机动子平台质心Y轴位置当前值校正项Ypcali；主DSP(11)等待并收到同步确认标志Asyn后，读取1号、3号直线电机执行器的电流采样值，读取1号、2号X轴激光位置传感器的采样值SX1、SX2，对采样值进行数字滤波、校正，根据1号、2号X轴激光位置传感器的采样值SX1、SX2和传感器方程计算平面电机动子平台质心X轴位置当前值Xp、平面电机动子平台绕Z轴旋转的角度位置当前值θZp、平面电机动子平台质心Y轴位置当前值校正项Ypcali，再通过CAN总线发送平面电机动子平台质心Y轴位置参考值Ypr和平面电机动子平台质心Y轴位置当前值校正项Ypcali给从DSP(21)，接着等待从DSP(21)发送的平面电机动子平台质心Y轴位置当前值Yp；从DSP(21)等待并收到平面电机动子平台质心Y轴位置参考值Ypr和平面电机动子平台质心Y轴位置当前值校正项Ypcali后，根据1号Y轴激光位置传感器(26)的采样值SY1、平面电机动子平台质心Y轴位置当前值校正项Ypcali和传感器方程计算平面电机动子平台质心Y轴位置当前值Yp，再通过CAN总线发送Yp给主DSP(11)，并进行Y轴自由度位置、速度控制，进行2号、4号直线电机执行器电流控制，接着等待下一个控制周期的同步标志Fsyn；主DSP(11)等待并收到平面电机动子平台质心Y轴位置当前值Yp后，进行X轴和θz自由度位置、速度控制，并进行1号、3号直线电机执行器电流控制，接着定时等待下一个控制周期的开始。
2.根据权利要求1所述的基于双DSP的3自由度平面电机伺服控制器，其 特征在于所述的主控单元(1):主DSP (11)型号为TMS320F2812，主缓冲驱 动模块(12)由主第一缓冲驱动器(Ul)、主第二缓冲驱动器(U2)组成，型号 为SN74LS244，主光耦驱动隔离模块(13)由主第一光电耦合器(U5)、主第二 光电耦合器(U6)、主第三光电耦合器(U7)、主第四光电耦合器(U8)、主第五光电耦合器(U9)、主第六光电耦合器(U10)组成，型号为HCPL0631，主 全桥功率驱动模块(14)由主第一全桥功率驱动器(U17)、主第二全桥功率驱 动器(U18)、主第三全桥功率驱动器(U19)、主第四全桥功率驱动器(U20)、 主第五全桥功率驱动器(U21)、主第六全桥功率驱动器(U22)组成，型号为 L298，主霍尔电流传感器模块(15)由主第一霍尔电流传感器(U29)、主第二 霍尔电流传感器(U30)、主第三霍尔电流传感器(U31)、主第四霍尔电流传感 器(U32)、主第五霍尔电流传感器(U33)、主第六霍尔电流传感器(U34)组 成，型号为LTSR6-NP， 1号X轴激光位移传感器(16)， 2号X轴激光位移传 感器(17)，型号为LG10A65PI,主DSP (11)的片上PWM模块的PWM信号输出端"PWM1 PWM6"分 别与主缓冲驱动模块(12)的主第一缓冲驱动器(Ul)的输入端"1A1 1A4"、 2A1 、2A2连接，主DSP( 11 )片上PWM模块的PWM信号输出端"PWM7 PWM12 " 分别与主缓冲驱动模块(12)的主第二缓冲驱动器(U2)的输入端"1A1 1A4"、 2A1、 2A2连接，主缓冲驱动模块(12)的主第一缓冲驱动器(Ul)的输出端"1Y1 1Y4"、 2Y1、 2Y2分别与主光耦驱动隔离模块(13)的主第一光电耦合器(U5)、主第 二光电耦合器(U6)、主第三光电耦合器(U7)的输入端VFl-、 VF2-连接，主 缓冲驱动模块(12)的主第二缓冲驱动器(U2)的输出端"1Y1 1Y4"、 2Y1、 2Y2分别与主光耦驱动隔离模块(13)的主第四光电耦合器(U8)、主第五光电 耦合器(U9)、主第六光电耦合器(U10)的输入端VFl-、 VF2-连接，主光耦驱动隔离模块(13)的主第一光电耦合器(U5)、主第二光电耦合器 (U6)、主第三光电耦合器(U7)、主第四光电耦合器(U8)、主第五光电耦合 器(U9)、主第六光电耦合器(U10)的输出端Vol、 Vo2分别与主全桥功率驱 动模块(14)的主第一全桥功率驱动器(U17)、主第二全桥功率驱动器(U18)、 主第三全桥功率驱动器(U19)、主第四全桥功率驱动器(U20)、主第五全桥功 率驱动器(U21)、主第六全桥功率驱动器(U22)的输入端IN1、 IN2连接，主全桥功率驱动模块(14)的主第一全桥功率驱动器(U17)、主第二全桥 功率驱动器(U18)、主第三全桥功率驱动器(U19)、主第四全桥功率驱动器 (U20)、主第五全桥功率驱动器(U21)、主第六全桥功率驱动器(U22)的输 出端0UT1分别与主霍尔电流传感器模块(15)的主第一霍尔电流传感器(U29)、 主第二霍尔电流传感器(U30)、主第三霍尔电流传感器(U31)、主第四霍尔电流传感器(U32)、主第五霍尔电流传感器(U33)、主第六霍尔电流传感器(U34) 的输入端l连接，主全桥功率驱动模块(14)的主第一全桥功率驱动器(U17)、 主第二全桥功率驱动器(U18)、主第三全桥功率驱动器(U19)的输出端OUT2 分别与1号直线电机执行器(LI)的输入端"-IN1~-IN3"连接，主全桥功率驱 动模块(14)的主第四全桥功率驱动器(U20)、主第五全桥功率驱动器(U21)、 主第六全桥功率驱动器(U22)的输出端OUT2分别与3号直线电机执行器(L3) 的输入端"-IN1~-IN3"连接，主霍尔电流传感器模块(15)的主第一霍尔电流传感器(U29)、主第二霍 尔电流传感器(U30)、主第三霍尔电流传感器(U31)、主第四霍尔电流传感器 (U32)、主第五霍尔电流传感器(U33)、主第六霍尔电流传感器(U34)的3 号脚分别与各自的5号脚连接，2号脚分别与各自的6号脚连接，输出端OUT 分别与主DSP (11)的片上ADC模块的输入端"ADCINA0 ADCINA5"连接， 主霍尔电流传感器模块(15)的主第一霍尔电流传感器(U29)、主第二霍尔电 流传感器(IBO)、主第三霍尔电流传感器(U31)的输出端4分别与1号直线电 机执行器(LI)的输入端"+INl—IN3"连接，主霍尔电流传感器模块(15)的 主第四霍尔电流传感器(U32)、主第五霍尔电流传感器(U33)、主第六霍尔电 流传感器(U34)的输出端4'分别与3号直线电机执行器(L3)的输入端 "+IN1~+IN3"连接，l号X轴激光位置传感器(16)、 2号X轴激光位置传感器(17)的输出端 Wh分别连接到主DSP (11)的片上ADC模块的输入端ADCINA6、 ADCINA7，主DSP (11)的片上CAN模块连接到主控制器局域网通讯接口 (D2)，所述的从控单元(2)包括从DSP (21)型号为TMS320F2812，从缓冲驱 动模块(22)由从第一缓冲驱动器(U3)、从第二缓冲驱动器(U4)组成，型号 为SN74LS244，从光耦驱动隔离模块(23)由从第一光电耦合器(Ull)、从第 二光电耦合器(U12)、从第三光电耦合器(U13)、从第四光电耦合器(U14)、 从第五光电耦合器(U15)、从第六光电耦合器(U16)组成，型号为HCPL0631， 从全桥功率驱动模块(24)由从第一全桥功率驱动器(U23)、从第二全桥功率 驱动器(U24)、从第三全桥功率驱动器(U25)、从第四全桥功率驱动器(U26)、 从第五全桥功率驱动器(U27)、从第六全桥功率驱动器(U28)组成，型号为 L298，从霍尔电流传感器模块(25)由从第一霍尔电流传感器(U35)、从第二 霍尔电流传感器(U36)、从第三霍尔电流传感器(U37)、从第四霍尔电流传感器(U38)、从第五霍尔电流传感器(U39)、从第六霍尔电流传感器(U40)组 成，型号为LTSR6-NP， 1号Y轴激光位移传感器(26)，型号为LG10A65PI，从DSP (21)的片上PWM模块的PWM信号输出端"PWM1 PWM6"分 别与从缓冲驱动模块(22)的从第一缓冲驱动器(U3)的输入端"1A1 1A4"、 2A1、 2A2连接，从DSP (21)的片上PWM模块的PWM信号输出端"PWM7 PWM12"分别与从缓冲驱动模块(22)的从第二缓冲驱动器(U4)的 输入端"1A1 1A4"、 2A1、 2A2连接，从缓冲驱动模块(22)的从第一缓冲驱动器(U3)的输出端"1Y1 1Y4"、 2Y1、 2Y2分别与从光耦驱动隔离模块(23)的从第一光电耦合器(Ull)、从第 二光电耦合器(U12)、从第三光电耦合器(U13)的输入端VFl-、 VF2-连接， 从缓冲驱动模块(22)的从第二缓冲驱动器(U4)的输出端"1Y1 1Y4"、 2Y1、 2Y2分别与从光耦驱动隔离模块(23)的从第四光电耦合器(U14)、从第五光 电耦合器(U15)、从第六光电耦合器(U16)的输入端VFl-、 VF2-连接，从光耦驱动隔离模块(23)的从第一光电耦合器(Ull)、从第二光电耦合 器(U12)、从第三光电耦合器(U13)、从第四光电耦合器(U14)、从第五光电 耦合器(U15)、从第六光电耦合器(U16)的输出端Vol、 Vo2分别与从全桥功 率驱动模块(24)的从第一全桥功率驱动器(U23)、从第二全桥功率驱动器(U24)、 从第三全桥功率驱动器(U25)、从第四全桥功率驱动器(U26)、从第五全桥功 率驱动器(U27)、从第六全桥功率驱动器(U28)的输入端IN1、 IN2连接，从全桥功率驱动模块(24)的从第一全桥功率驱动器(U23)、从第二全桥 功率驱动器(U24)、从第三全桥功率驱动器(U25)、从第四全桥功率驱动器(U26)、从第五全桥功率驱动器(U27)、从第六全桥功率驱动器(U28)的输 出端0UT1分别与从霍尔电流传感器模块(25)的从第一霍尔电流传感器(U35)、 从第二霍尔电流传感器(U36)、从第三霍尔电流传感器(U37)、从第四霍尔电 流传感器(U38)、从第五霍尔电流传感器(U39)、从第六霍尔电流传感器(U40) 的输入端l连接，从全桥功率驱动模块(24)的从第一全桥功率驱动器(U23)、 从第二全桥功率驱动器(U24)、从第三全桥功率驱动器(U25)的输出端OUT2 分别与2号直线电机执行器(L2)的输入端"-INl -IN3"连接，从全桥功率驱 动模块(24)的从第四全桥功率驱动器(U26)、从第五全桥功率驱动器(U27)、 从第六全桥功率驱动器(U28)的输出端OUT2分别与4号直线电机执行器(L4) 的输入端"-INl -IN3"连接，从霍尔电流传感器模块(25)的从第一霍尔电流传感器(U35)、从第二霍 尔电流传感器(U36)、从第三霍尔电流传感器(U37)、从第四霍尔电流传感器 (U38)、从第五霍尔电流传感器(U39)、从第六霍尔电流传感器(U40)的3 号脚分别与各自的5号脚连接，2号脚分别与各自的6号脚连接，输出端OUT 分别与从DSP (21)的片上ADC模块的输入端"ADCINA0 ADCINA5"连接， 从霍尔电流传感器模块(25)的从第一霍尔电流传感器(U35)、从第二霍尔电 流传感器(U36)、从第三霍尔电流传感器(U37)的输出端4分别与2号直线电 机执行器(L2)的输入端"+IN1~+IN3"连接，从霍尔电流传感器模块(25)的 从第四霍尔电流传感器(U38)、从第五霍尔电流传感器(U39)、从第六霍尔电 流传感器(U40)的输出端4分别与4号直线电机执行器(L4)的输入端 "+INl +IN3"连接，1号Y轴激光位置传感器(26)的输出端Wh连接到从DSP (21)的片上 ADC模块输入端ADCINA6，从DSP (21)的片上CAN模块连接到从控制器局域网通讯接口 (D3)，主控制器局域网通讯接口 (D2)和从控制器局域网通讯接口 (D3)通过双 绞线互连。
全文摘要
基于双DSP的3自由度平面电机伺服控制器包括主控和从控单元，主控和从控单元包括各自的DSP芯片、光耦驱动隔离模块、全桥功率驱动模块、电流传感器模块、位置传感器模块。主控单元的DSP芯片给出PWM信号，经光耦隔离后，控制全桥功率驱动模块输出六相平面电机驱动电流，通过激光位置传感器采集两路平面电机X轴位置；从控单元的DSP芯片给出PWM信号，经光耦隔离后，控制全桥功率驱动模块输出六相平面电机驱动电流，通过激光位置传感器采集一路平面电机Y轴位置；主从DSP通过CAN总线实时交换同步控制信号，使用3自由度同步控制策略控制平面电机实现X轴、Y轴、θ<sub>Z</sub>自由度的快速准确定位。
文档编号H02P21/14GK101510749SQ200910029828
公开日2009年8月19日 申请日期2009年3月18日 优先权日2009年3月18日
发明者赣 周, 周勤博, 前 张, 晖 彭, 黄学良 申请人:东南大学