何敏佳，孫永平，張 軍

（1.廣州數控設備有限公司，廣州 510535；2.哈爾濱工業(yè)大學電氣工程及自動化學院，哈爾濱 150001）

0 引言

目前，運動控制系統(tǒng)大多采用運動控制器＋伺服驅動器的方式，運動控制器與伺服驅動器進行信息交互的方式主要為脈沖指令、工業(yè)總線、工業(yè)以太網的方式，其具有應用廣泛、技術可靠等優(yōu)點。但隨著工業(yè)需求的發(fā)展，要求多軸運動控制產品具有小型化、低成本、高可靠性和靈活性[1]。采用傳統(tǒng)的通信方式，伺服驅動與運動控制之間傳輸的信息種類有限，無法滿足更多高性能運動控制算法的需求。

隨著工業(yè)界的需求以及微電子行業(yè)的發(fā)展，采用FPGA和ARM 的架構芯片Xilinx ZYNQ 全可編程片上系統(tǒng)Sys?tem－on－Chip（SoC）被廣泛應用在工業(yè)控制領域，其集成雙ARM核和FPGA核，非常適合伺服驅動與運動控制高集成技術。

FPGA 已經廣泛地應用于運動控制以及伺服驅動平臺[2]，通過FPGA高速運算能力實現(xiàn)伺服系統(tǒng)電流環(huán)運算，可以顯著提升電流環(huán)帶寬，進而提升整個伺服系統(tǒng)性能。對于電機驅動器來說要求其能實現(xiàn)高級的伺服算法[3－4]，例如諧振抑制、末端抖動抑制、參數辨識提高伺服系統(tǒng)的性能以及可靠性。本文采用Zynq7020 作為驅控一體硬件控制核心芯片，一個ARM核完成多軸的位置環(huán)、速度環(huán)算法，同時可以完成伺服高級算法如諧振抑制、參數自整定、參數辨識等高級算法，另一個ARM 核完成多軸軌跡生成、人機交互、示教在線功能，集成的FPGA實現(xiàn)多軸硬件電流環(huán)協(xié)同控制，提高系統(tǒng)整體帶寬[5－7]，簡化系統(tǒng)時序規(guī)劃，實現(xiàn)多軸ns級同步精度以及機器人更精確的位置軌跡，相比于分布式結構的基于CiA402運動控制協(xié)議工業(yè)以太網的數據傳輸[8－9]，驅控一體機內部數據通過共享內存以及高速內部總線的方式進行交換，可以達到Gb/s 級別，傳輸信息更加豐富，例如讀取機器人各個軸的位置、速度、力矩，以及伺服控制核心算法識別出的慣量等信息，便于機器人建模，更好地完成機器人的運動控制、柔順控制等算法，以及空間軌跡、人機交互等任務。同時，外部總線的減免，使得系統(tǒng)的體積和成本得到減小和降低的同時，提高了系統(tǒng)的可靠性。

1 基于Zynq－7020 SoC分析與設計

1.1 Zynq－7020 SoC控制系統(tǒng)結構

驅控一體機的框架如圖1 所示。核心的主控制芯片采用目前市場上主流的Xilinx Zynq 系列SoC，集成了兩個高性能ARM 芯片和FPGA 模塊，用于取代傳統(tǒng)的多DSP 控制器的分布式構架，避免復雜冗余的外部總線，縮小體積，提高可靠性。

圖1 Zynq－7020多軸控制系統(tǒng)機構Fig.1 Architecture of the Zynq－based multi－axis control system

在伺服電機驅動反面驅動器方面，借助于FPGA模塊，采用硬件電流環(huán)算法，在不增加算法整體復雜度的情況下，借助于FPGA高速并行運算能力，減少計算延時，顯著提高系統(tǒng)電流環(huán)帶寬。同時，考慮FPGA資源有限，為了實現(xiàn)多軸協(xié)同控制，采用時分復用算法，搭建流水線架構的多軸硬件電流環(huán)，以增加少量的計算時間為代價，使硬件資源占用量降低了一個維度。電流采樣部分采用AD7401芯片，其為模擬轉數字采樣，方便FPGA進行采樣處理，同時提高采樣精度，編碼器信號的協(xié)議解析同樣也采用FPGA完成，便于時序的規(guī)劃和信號的細分。涉及較多高級和擴展算法的位置環(huán)及速度環(huán)控制則由ARM芯片承擔，便于程序的拓展和調試。

雙ARM SoC中，一個CPU完成多軸的位置環(huán)、速度環(huán)和電流環(huán)的算法，同時可以完成伺服高級算法如諧振。另一個CPU完成交互功能、多軸軌跡規(guī)劃等，SoC內部ARM和FPGA之間的信息交流則由高速內部總線和共享內存空間加以實現(xiàn)。Advanced extensible interface（AXI）進一步地利用驅控一體化實現(xiàn)驅控信息共享，根據系統(tǒng)的空間位置和伺服電機各種狀態(tài)反饋，在線地調節(jié)系統(tǒng)各類參數，最大限度地優(yōu)化機器人軌跡，減小軌跡誤差。

1.2 Zynq－7020 SoC控制系統(tǒng)結構

多軸驅控一體雙核交互架構如圖2 所示，雙核數據交互為共享內存的方式，共享內存大小為64 kB，交互信息為實時信息，速度快且數據量大，為方便系統(tǒng)調度雙核架構雙核ARM 嵌入式系統(tǒng)μC/OS 嵌入式系統(tǒng)，可以實時調度系統(tǒng)資源加速度，本文雙核交互數據包括位置、速度、負載力矩、負載慣量等信息。

圖2 驅控一體化雙核ARM數據交互架構Fig.2 Architecture of the dual core data interaction

雙核交互數據時鐘同步信號以及ARM 與FPGA 之間數據交互時鐘由FPGA提供。雙核交互數據采用經典的乒乓緩沖策略，如圖3所示，伺服數據反饋區(qū)設置2個，數控指令區(qū)同樣設置2個。防止數據雙核數據讀取的時候產生沖突，在周期0主機讀AtBuf1，寫MdtBuf0，從機讀MdtBuf1，寫AtBuf0，在周期1 主機讀AtBuf0，寫MdtBuf1，從機讀MdtBuf0，寫AtBuf1，通過數據緩沖完成雙核交互運控與伺服數據內容的讀寫。

圖3 雙核兵乓緩沖數據交互Fig.3 Architecture of the dual core data interaction

1.3 單軸伺服控制器結構

單軸控制系統(tǒng)控制框圖如圖4 所示。位置環(huán)采用比例控制器WAPR(s)＝KP，速度環(huán)采用比例積分控制器，電流環(huán)采用比例積分控制器采用傳動的控制策略，由于控制平臺的優(yōu)越性可以提高系統(tǒng)的性能。

圖4 單軸控制框圖Fig.4 Block diagram of the single－axis control system

2 多軸電流環(huán)控制器設計

2.1 多軸流水線電流環(huán)控制策略

流水線結構的時序調度原理如圖5 所示，在進行流水線電流環(huán)計算之前，F(xiàn)PGA 部分先進行4 軸電流環(huán)采樣，得到4軸相電流信號。編碼器得到6軸角度信號，通過查表法得到各軸正弦值、余弦值，按照時分復用的思想，要求同一功能模塊在同一時刻只能進行某一個電機軸的控制運算，流水線電流環(huán)分為Clark 變換模塊、Park 變換模塊、PI 控制器、Park逆變換模塊、SVPWM 調制模塊。當1 軸的Clark 變換模塊結束后啟動1 軸的PARK 變換，之后進行1 軸的PI 控制器，接著進行PARK 逆變換，以此類推，直到啟動SVPWM 運算模塊對1 軸的運算，如果1 軸完成Clark 變換進行PARK 變換，則啟動Clark 變換模塊對2 軸的運算，直到完成SVPWM 運算。以此類推，直至完成4 軸電流環(huán)模塊，然后進行PWM輸出，這樣可以保證多軸同步性、電流環(huán)快速計算性能，又可以節(jié)約FPGA資源。

圖5 多軸流水線電流環(huán)Fig.5 Diagram of multi－axis current loop pipeline structure

2.2 基于FPGA雙采樣雙更新算法實現(xiàn)

目前伺服驅動器主流的控制芯片還是以應用DSP 單采樣單更新控制策略為主，但是由于DSP的運算特點，PWM零階保持器以及數字延遲的影響，使得執(zhí)行一次電流環(huán)需要時間較長，一般帶來1.5 倍的電流采樣延遲，限制了電流環(huán)的帶寬，通過減少采樣與運算延遲可以顯著地提高控制器電流環(huán)帶寬。

因此，近些來，基于FPGA電流環(huán)控制器的電機控制算法被廣泛研究[10]，由于FPGA具有快速的并行計算能力使得電流環(huán)運算速度非?？?，可以控制在幾十納秒內完成，并且采樣時間點和PWM更新點可以自由設置，其中較為經典的雙電流采樣和雙PWM占空比更新時序如圖6所示。圖中Dm(k)為第k個周期內第m次PWM占空比更新，im(k)為第k個周期內第m次電流采樣（m＝1,2），Ts為電流采樣周期，也表示電流環(huán)的運算周期，Tc為載波周期。對電流采樣和PWM 占空比更新進行分時處理。其中，PWM占空比更新仍然在三角載波的波峰和波谷進行，而電流的采樣位置則位于PWM占空比更新前的幾μs 處。由于FPGA并行數據處理的特點，在電流采樣之后，PWM占空比更新值能夠及時輸出。此時，電流環(huán)的電流采樣和計算延遲與電流環(huán)其他延時環(huán)節(jié)相比可以忽略不計。因此，電流環(huán)運算延遲T－delay≤0.5Ts，整體延遲小于0.75Ts。減小電流環(huán)閉環(huán)系統(tǒng)的延遲，能夠提升電流環(huán)帶寬，從而達到提升電流環(huán)的動態(tài)響應性能的效果。

圖6 雙采樣雙更新控制策略Fig.6 Strategy of double sampling and PWM duty cycle double update

3 伺服驅動控制器設計

3.1 電流環(huán)控制器設計及參數選擇

伺服電機為表貼式永磁同步電機，其電壓方程如下[11－12]：

式中：Ls為電機d-q 軸電感；Rs為定子電阻；為d-q 軸電壓；為轉子磁鏈；ωr為電機電角速度。

電流環(huán)采用PI 控制，其對電機參數變化有很強魯棒性，根據PI控制器輸出，電壓輸入如下：

系統(tǒng)的傳遞函數由系統(tǒng)的零極點以及帶寬決定。

3.2 速度環(huán)控制器設計及參數選擇

伺服電機機械方程如下：

式中：Tem為電機的電磁轉矩；J 為電機的負載慣量；ωr為電機轉速；B為系統(tǒng)的阻尼系數；Tload為負載轉矩。

速度環(huán)采用PI控制器以及系統(tǒng)的傳遞方程如下所示：

其中控制參數選擇KP_sr＝Jωc_sr，KI_sr＝Kv_scKP_scωc_sc，ωc_sc為速度環(huán)帶寬；Kv_sc為速度環(huán)帶寬設置系數，選擇0.2。

3.3 位置環(huán)環(huán)控制器設計及參數選擇

伺服系統(tǒng)電流環(huán)帶寬高一邊等效1，速度環(huán)等效一階低通環(huán)節(jié)，在此基礎上根據系統(tǒng)設定帶寬進行，伺服系統(tǒng)位置環(huán)普遍采用比例控制策略，本文也采用比例控制策略，控制參數選取如下：

4 實驗平臺及實驗驗證

實驗平臺為基于Zynq打造的驅控一體平臺，驅控一體集成4軸驅動與SCARA運動控制組成，實驗平臺，如圖7所示。

圖7 SCARA 驅控一體平臺Fig.7 SCARA experimental setup with SoC platform

4.1 掃頻帶寬測試

采用FPGA伺服驅動設計，控制器設計以及參數參考第3節(jié)進行設計，伺服系統(tǒng)帶寬提升顯著，下面就伺服系統(tǒng)位置環(huán)、速度環(huán)、電流環(huán)進行掃頻測。掃頻所用電機為多摩川750 W，測試條件為空載測試。

由于采用FPGA 雙電流采樣、雙PWM 更新策略，以及零極點匹配策略，如圖8所示，電流環(huán)帶寬可達2.1 Hz，速度環(huán)帶寬可達300 Hz，位置環(huán)帶寬為50 Hz。由于采用了FPGA電流環(huán)控制策略，電流環(huán)帶寬提升，進而三環(huán)帶寬同步得到了提升。

圖8 SCARA驅控一體平臺三環(huán)帶寬測試Fig.8 Three－loop bandwidth test of SCARA experimental platform

4.2 多軸數據反饋與監(jiān)測

圖9 所示為SCARA 機械臂在完成末端圓弧軌跡時，伺服關節(jié)信息通過ARM1核傳遞給運動控制ARM0核心。

圖9 SCARA 圓弧軌跡2軸伺服信息Fig.9 SCARA circular path 2 axis servo information

通過驅控一體化平臺可實現(xiàn)機械臂伺服關節(jié)狀態(tài)的檢測。由于采用基于Zynq－7020 作為驅控一體的控制核心。機器人的運動控制和伺服關節(jié)可以交互更多信息，不會局限于基于工業(yè)總線的通信方式，通信信息種類多以及通信速度快。后期進行更深層次算法開發(fā)可進行機械參數如關節(jié)慣量、控制參數在線調整傳輸等算法。

5 結束語

本文系統(tǒng)地介紹基于Zynq－7020 SoC驅控一體化平臺，系統(tǒng)介紹了芯片內部資源分配，通過合理分配雙核ARM資源以及FPGA資源，進行驅控一體化平臺搭建，并采用乒乓緩沖的數據交互策略，區(qū)別于以太網通訊，實現(xiàn)運動控制與伺服驅動之間高速通信。采用雙采樣雙更新硬件電流環(huán)控制策略，極大提高了電流環(huán)帶寬，由于電流環(huán)電流環(huán)帶寬提升，使得速度環(huán)、位置環(huán)帶寬也獲得了顯著提升，在多軸硬件電流環(huán)采用流水線硬件電流環(huán)的方案，節(jié)省FPGA資源。根據期望帶寬設計位置環(huán)控制器、速度環(huán)控制器、電流環(huán)控制器。最后進行了三環(huán)掃頻實驗，驗證平臺性能，以及多軸數據同時進行采集進行驗證，方便機器人算法開發(fā)。