何敏佳，孫永平，張 軍

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

0 引言

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

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

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

1 基于Zynq－7020 SoC分析與設(shè)計(jì)

1.1 Zynq－7020 SoC控制系統(tǒng)結(jié)構(gòu)

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

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

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

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

1.2 Zynq－7020 SoC控制系統(tǒng)結(jié)構(gòu)

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

圖2 驅(qū)控一體化雙核ARM數(shù)據(jù)交互架構(gòu)Fig.2 Architecture of the dual core data interaction

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

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

1.3 單軸伺服控制器結(jié)構(gòu)

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

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

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

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

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

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

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

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

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

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

3 伺服驅(qū)動(dòng)控制器設(shè)計(jì)

3.1 電流環(huán)控制器設(shè)計(jì)及參數(shù)選擇

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

式中：Ls為電機(jī)d-q 軸電感；Rs為定子電阻；為d-q 軸電壓；為轉(zhuǎn)子磁鏈；ωr為電機(jī)電角速度。

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

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

3.2 速度環(huán)控制器設(shè)計(jì)及參數(shù)選擇

伺服電機(jī)機(jī)械方程如下：

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

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

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

3.3 位置環(huán)環(huán)控制器設(shè)計(jì)及參數(shù)選擇

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

4 實(shí)驗(yàn)平臺(tái)及實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)平臺(tái)為基于Zynq打造的驅(qū)控一體平臺(tái)，驅(qū)控一體集成4軸驅(qū)動(dòng)與SCARA運(yùn)動(dòng)控制組成，實(shí)驗(yàn)平臺(tái)，如圖7所示。

圖7 SCARA 驅(qū)控一體平臺(tái)Fig.7 SCARA experimental setup with SoC platform

4.1 掃頻帶寬測(cè)試

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

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

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

4.2 多軸數(shù)據(jù)反饋與監(jiān)測(cè)

圖9 所示為SCARA 機(jī)械臂在完成末端圓弧軌跡時(shí)，伺服關(guān)節(jié)信息通過ARM1核傳遞給運(yùn)動(dòng)控制ARM0核心。

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

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

5 結(jié)束語(yǔ)

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