專利名稱：基于雙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日
發明者贛 周, 周勤博, 前 張, 暉 彭, 黃學良 申請人:東南大學