[发明专利]基于双DSP的3自由度平面电机伺服控制器

权利要求书

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轴位置参考值X_pr、平面电机动子平台质心Y轴位置参考值Y_pr、平面电机动子平台绕Z轴旋转的角度位置参考值θ_Zpr，通过CAN总线发送同步标志F_syn给从DSP(21)，并等待从DSP(21)发送的同步确认标志A_syn；从DSP(21)等待并收到同步标志F_syn后，通过CAN总线发送同步确认标志A_syn给主DSP(11)，读取2号、4号直线电机执行器的电流采样值，读取1号Y轴激光位置传感器(26)的采样值S_Y1，对采样值进行数字滤波、校正，接着等待主DSP(11)发送的平面电机动子平台质心Y轴位置参考值Y_pr和平面电机动子平台质心Y轴位置当前值校正项Y_pcali；主DSP(11)等待并收到同步确认标志A_syn后，读取1号、3号直线电机执行器的电流采样值，读取1号、2号X轴激光位置传感器的采样值S_X1、S_X2，对采样值进行数字滤波、校正，根据1号、2号X轴激光位置传感器的采样值S_X1、S_X2和传感器方程计算平面电机动子平台质心X轴位置当前值X_p、平面电机动子平台绕Z轴旋转的角度位置当前值θ_Zp、平面电机动子平台质心Y轴位置当前值校正项Y_pcali，再通过CAN总线发送平面电机动子平台质心Y轴位置参考值Y_pr和平面电机动子平台质心Y轴位置当前值校正项Y_pcali给从DSP(21)，接着等待从DSP(21)发送的平面电机动子平台质心Y轴位置当前值Y_p；从DSP(21)等待并收到平面电机动子平台质心Y轴位置参考值Y_pr和平面电机动子平台质心Y轴位置当前值校正项Y_pcali后，根据1号Y轴激光位置传感器(26)的采样值S_Y1、平面电机动子平台质心Y轴位置当前值校正项Y_pcali和传感器方程计算平面电机动子平台质心Y轴位置当前值Y_p，再通过CAN总线发送Y_p给主DSP(11)，并进行Y轴自由度位置、速度控制，进行2号、4号直线电机执行器电流控制，接着等待下一个控制周期的同步标志F_syn；主DSP(11)等待并收到平面电机动子平台质心Y轴位置当前值Y_p后，进行X轴和θ_Z自由度位置、速度控制，并进行1号、3号直线电机执行器电流控制，接着定时等待下一个控制周期的开始，所述的传感器方程为：

θZp=(SX2-SX1)/(lS2+lS1)Xp=X0-(lS2SX1+lS1SX2)/(lS2+lS1)Ypcali=(lS3-Xp)×θZpYp=-Y0+SY1-Ypcali,]]>

其中，l_s1、l_s2、l_s3为激光位置传感器激光束和动子平台质心初始位置C1的距离，X₀等于XYZ坐标系的原点固结于空间O点，平面电机动子平台处于初始位置P1，动子平台质心初始位置C1和O点重合时，X轴激光位置传感器的采样值，Y₀等于XYZ坐标系的原点固结于空间O点，平面电机动子平台处于初始位置P1，动子平台质心初始位置C1和O点重合时，Y轴激光位置传感器的采样值。

2.根据权利要求1所述的基于双DSP的3自由度平面电机伺服控制器，其特征在于所述的主控单元(1)：主DSP(11)型号为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，

主DSP(11)的片上PWM模块的PWM信号输出端“PWM1～PWM6”分别与主缓冲驱动模块(12)的主第一缓冲驱动器(U1)的输入端“1A1～1A4”、2A1、2A2连接，主DSP(11)片上PWM模块的PWM信号输出端“PWM7～PWM12”分别与主缓冲驱动模块(12)的主第二缓冲驱动器(U2)的输入端“1A1～1A4”、2A1、2A2连接，

主缓冲驱动模块(12)的主第一缓冲驱动器(U1)的输出端“1Y1～1Y4”、2Y1、2Y2分别与主光耦驱动隔离模块(13)的主第一光电耦合器(U5)、主第二光电耦合器(U6)、主第三光电耦合器(U7)的输入端VF1-、VF2-连接，主缓冲驱动模块(12)的主第二缓冲驱动器(U2)的输出端“1Y1～1Y4”、2Y1、2Y2分别与主光耦驱动隔离模块(13)的主第四光电耦合器(U8)、主第五光电耦合器(U9)、主第六光电耦合器(U10)的输入端VF1-、VF2-连接，

主光耦驱动隔离模块(13)的主第一光电耦合器(U5)、主第二光电耦合器(U6)、主第三光电耦合器(U7)、主第四光电耦合器(U8)、主第五光电耦合器(U9)、主第六光电耦合器(U10)的输出端Vo1、Vo2分别与主全桥功率驱动模块(14)的主第一全桥功率驱动器(U17)、主第二全桥功率驱动器(U18)、主第三全桥功率驱动器(U19)、主第四全桥功率驱动器(U20)、主第五全桥功率驱动器(U21)、主第六全桥功率驱动器(U22)的输入端IN1、IN2连接，

主全桥功率驱动模块(14)的主第一全桥功率驱动器(U17)、主第二全桥功率驱动器(U18)、主第三全桥功率驱动器(U19)、主第四全桥功率驱动器(U20)、主第五全桥功率驱动器(U21)、主第六全桥功率驱动器(U22)的输出端OUT1分别与主霍尔电流传感器模块(15)的主第一霍尔电流传感器(U29)、主第二霍尔电流传感器(U30)、主第三霍尔电流传感器(U31)、主第四霍尔电流传感器(U32)、主第五霍尔电流传感器(U33)、主第六霍尔电流传感器(U34)的输入端1连接，主全桥功率驱动模块(14)的主第一全桥功率驱动器(U17)、主第二全桥功率驱动器(U18)、主第三全桥功率驱动器(U19)的输出端OUT2分别与1号直线电机执行器(L1)的输入端“-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)、主第二霍尔电流传感器(U30)、主第三霍尔电流传感器(U31)的输出端4分别与1号直线电机执行器(L1)的输入端“+IN1～+IN3”连接，主霍尔电流传感器模块(15)的主第四霍尔电流传感器(U32)、主第五霍尔电流传感器(U33)、主第六霍尔电流传感器(U34)的输出端4分别与3号直线电机执行器(L3)的输入端“+IN1～+IN3”连接，

1号X轴激光位置传感器(16)、2号X轴激光位置传感器(17)的输出端Wh分别连接到主DSP(11)的片上ADC模块的输入端ADCINA6、ADCINA7，

主DSP(11)的片上CAN模块连接到主控制器局域网通讯接口(D2)，

所述的从控单元(2)包括：从DSP(21)型号为TMS320F2812，从缓冲驱动模块(22)由从第一缓冲驱动器(U3)、从第二缓冲驱动器(U4)组成，型号为SN74LS244，从光耦驱动隔离模块(23)由从第一光电耦合器(U11)、从第二光电耦合器(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)的从第一光电耦合器(U11)、从第二光电耦合器(U12)、从第三光电耦合器(U13)的输入端VF1-、VF2-连接，从缓冲驱动模块(22)的从第二缓冲驱动器(U4)的输出端“1Y1～1Y4”、2Y1、2Y2分别与从光耦驱动隔离模块(23)的从第四光电耦合器(U14)、从第五光电耦合器(U15)、从第六光电耦合器(U16)的输入端VF1-、VF2-连接，

从光耦驱动隔离模块(23)的从第一光电耦合器(U11)、从第二光电耦合器(U12)、从第三光电耦合器(U13)、从第四光电耦合器(U14)、从第五光电耦合器(U15)、从第六光电耦合器(U16)的输出端Vo1、Vo2分别与从全桥功率驱动模块(24)的从第一全桥功率驱动器(U23)、从第二全桥功率驱动器(U24)、从第三全桥功率驱动器(U25)、从第四全桥功率驱动器(U26)、从第五全桥功率驱动器(U27)、从第六全桥功率驱动器(U28)的输入端IN1、IN2连接，

从全桥功率驱动模块(24)的从第一全桥功率驱动器(U23)、从第二全桥功率驱动器(U24)、从第三全桥功率驱动器(U25)、从第四全桥功率驱动器(U26)、从第五全桥功率驱动器(U27)、从第六全桥功率驱动器(U28)的输出端OUT1分别与从霍尔电流传感器模块(25)的从第一霍尔电流传感器(U35)、从第二霍尔电流传感器(U36)、从第三霍尔电流传感器(U37)、从第四霍尔电流传感器(U38)、从第五霍尔电流传感器(U39)、从第六霍尔电流传感器(U40)的输入端1连接，从全桥功率驱动模块(24)的从第一全桥功率驱动器(U23)、从第二全桥功率驱动器(U24)、从第三全桥功率驱动器(U25)的输出端OUT2分别与2号直线电机执行器(L2)的输入端“-IN1～-IN3”连接，从全桥功率驱动模块(24)的从第四全桥功率驱动器(U26)、从第五全桥功率驱动器(U27)、从第六全桥功率驱动器(U28)的输出端OUT2分别与4号直线电机执行器(L4)的输入端“-IN1～-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)的输入端“+IN1～+IN3”连接，

1号Y轴激光位置传感器(26)的输出端Wh连接到从DSP(21)的片上ADC模块输入端ADCINA6，

从DSP(21)的片上CAN模块连接到从控制器局域网通讯接口(D3)，

主控制器局域网通讯接口(D2)和从控制器局域网通讯接口(D3)通过双绞线互连。