基于S 曲線的前饋三閉環(huán)PID 伺服控制器設(shè)計(jì)

李萬(wàn)寶，田 達(dá)

（中電海康集團(tuán)有限公司，浙江 杭州 310012）

0 引言

隨著科學(xué)技術(shù)水平的不斷提高，伺服驅(qū)動(dòng)器的性能有了極大地提高。 伺服驅(qū)動(dòng)器也向著數(shù)字化、智能化、小型化、通用化方向發(fā)展[1]。目前，伺服控制技術(shù)在智能機(jī)器人、機(jī)械加工、數(shù)字機(jī)床等方面越來(lái)越重要，面向機(jī)器人關(guān)節(jié)、智能制造等小型化伺服控制器也逐漸成為研究的重點(diǎn)[2]。與此同時(shí)，這些領(lǐng)域也對(duì)小型化伺服控制器的響應(yīng)速度、控制精度等提出了更高的要求。目前普通伺服控制器一般采用位置環(huán)、速度環(huán)、電流環(huán)的三閉環(huán)PID 控制，但是存在超調(diào)大、響應(yīng)速度慢等缺點(diǎn)[3]。文獻(xiàn)[4]提出采用速度加速度雙前饋控制技術(shù)，提高了系統(tǒng)響應(yīng)速度和跟蹤特性，但采用位置指令進(jìn)行微分運(yùn)算，計(jì)算復(fù)雜度較大。

本文提出了采用S 曲線結(jié)果提取速度及速度前饋補(bǔ)償?shù)姆椒ǎ?并由此研究出基于ARM7 內(nèi)核的低壓交流伺服驅(qū)動(dòng)器，具有以下特點(diǎn)：采用microchip32 位數(shù)字控制器ATSAME70N21B， 運(yùn)算速度400MHz， 并支持Ethernet，CAN，UASRT 等多種通訊方式；低電壓輸入，最高支持500W 輸出；采用三閉環(huán)控制，其中基于S 曲線優(yōu)化前饋控制，提高控制器的實(shí)時(shí)響應(yīng)速度和控制精度。

1 系統(tǒng)硬件設(shè)計(jì)

系統(tǒng)整體硬件方案如圖1 所示。整個(gè)系統(tǒng)包括了控制模塊、電機(jī)驅(qū)動(dòng)模塊、信號(hào)采集模塊、通訊模塊、電源模塊等。 控制模塊選擇ARM7 內(nèi)核3ATSAME70N21B 進(jìn)行開(kāi)發(fā)。通過(guò)電機(jī)驅(qū)動(dòng)電路實(shí)現(xiàn)整體的回路控制功能，包括：三相永磁直流伺服電機(jī)的驅(qū)動(dòng)，主控輸出PWM 后通過(guò)專用的驅(qū)動(dòng)電路驅(qū)動(dòng)MOS 管實(shí)現(xiàn)通、斷功能，根據(jù)電機(jī)不同的位置驅(qū)動(dòng)電機(jī)運(yùn)轉(zhuǎn)；專用的反饋電路，實(shí)現(xiàn)電機(jī)閉環(huán)控制功能，支持相對(duì)編碼器和霍爾傳感器；通訊、調(diào)試端口，用于控制和調(diào)試，包括SWD，USB，CAN，RS232等；溫度傳感器、E2PROM 等實(shí)現(xiàn)溫度讀取、數(shù)據(jù)存儲(chǔ)等。

圖1 伺服驅(qū)動(dòng)硬件設(shè)計(jì)Figure 1 Servo drive hardware design

1.1 伺服電機(jī)驅(qū)動(dòng)電路設(shè)計(jì)

三相永磁直流伺服電機(jī)的驅(qū)動(dòng)采用MIC4605芯片，主控輸出PWM 后通過(guò)專用的驅(qū)動(dòng)電路驅(qū)動(dòng)MOS 管實(shí)現(xiàn)通、斷功能，根據(jù)電機(jī)不同的位置驅(qū)動(dòng)電機(jī)運(yùn)轉(zhuǎn)。 伺服電機(jī)驅(qū)動(dòng)電路設(shè)計(jì)如圖2 所示。

圖2 伺服電機(jī)驅(qū)動(dòng)電路Figure 2 Servo motor drive circuit

1.2 反饋電路設(shè)計(jì)

反饋電路包含電流電壓檢測(cè)、傳感器、編碼器等，實(shí)現(xiàn)電機(jī)閉環(huán)控制功能。

第一，三相電機(jī)的相電流檢測(cè)、總電流檢測(cè)。通過(guò)高精度采樣電阻采樣各相流過(guò)的電流，反饋給主控實(shí)現(xiàn)FOC 電流環(huán)控制功能，如圖3 所示。

圖3 電流檢測(cè)示意圖Figure 3 Schematic diagram of current detection

第二，HALL 傳感器的反饋。HALL 傳感器集成在電機(jī)內(nèi)部， 通過(guò)給主控提供起始位置信息等，實(shí)現(xiàn)電機(jī)啟動(dòng)端的控制。霍爾傳感器反饋電路設(shè)計(jì)如圖4 所示。

圖4 霍爾傳感器反饋電路設(shè)計(jì)Figure 4 Hall sensor feedback circuit design

第三，編碼器的反饋。編碼器集成于電機(jī)內(nèi)部，通過(guò)給主控提供電機(jī)精準(zhǔn)的絕對(duì)位置信息等，實(shí)現(xiàn)電機(jī)運(yùn)轉(zhuǎn)過(guò)程中的精確位置反饋功能。編碼器反饋電路設(shè)計(jì)如圖5 所示。

圖5 編碼器反饋電路設(shè)計(jì)Figure 5 Encoder feedback circuit design

2 系統(tǒng)軟件架構(gòu)設(shè)計(jì)

2.1 軟件整體架構(gòu)

伺服器軟件架構(gòu)整體分為應(yīng)用層、 設(shè)備驅(qū)動(dòng)層、平臺(tái)驅(qū)動(dòng)層和通用庫(kù)四大模塊。 從上到下依次為應(yīng)用層、設(shè)備驅(qū)動(dòng)層和平臺(tái)驅(qū)動(dòng)層。 最上層為應(yīng)用層，它是根據(jù)設(shè)備驅(qū)動(dòng)層提供的數(shù)據(jù)，依據(jù)既定邏輯進(jìn)行判斷和控制，包含電機(jī)控制模塊、外部軟件監(jiān)控模塊、通信控制模塊、故障與處理模塊等；中間層為設(shè)備驅(qū)動(dòng)層，它是根據(jù)平臺(tái)驅(qū)動(dòng)層提供的信息為應(yīng)用層提供數(shù)據(jù)，或者從應(yīng)用層接收指令控制平臺(tái)驅(qū)動(dòng)層的模塊，包含三環(huán)控制模塊、位置采集、電流電壓溫度采集、FOC 控制算法和SVPWM 控制算法；最底層為平臺(tái)驅(qū)動(dòng)層，它是直接使用硬件資源的模塊，如UART，QDEC，IO，ADC，PWM 和定時(shí)器等模塊。 另外還有通用庫(kù)模塊，為某些功能提供集成的調(diào)用，包括CRC 校驗(yàn)、PID 控制和三角函數(shù)等模塊。 如圖6 所示為伺服驅(qū)動(dòng)邏輯架構(gòu)。

圖6 伺服驅(qū)動(dòng)邏輯架構(gòu)Figure 6 Servo drive logic architecture

2.2 模塊對(duì)應(yīng)關(guān)系

功能模塊關(guān)系如圖7 所示， 主要分為四條線路。

圖7 伺服驅(qū)動(dòng)功能模塊關(guān)聯(lián)圖Figure 7 Servo drive function module correlation diagram

第一， 存儲(chǔ)線路： 主要需要用到EEPROM 和DataSaveLoad 等模塊，電機(jī)控制通過(guò)這些模塊對(duì)數(shù)據(jù)進(jìn)行讀取和存儲(chǔ)。

第二，模數(shù)轉(zhuǎn)換線路：對(duì)傳輸進(jìn)來(lái)的數(shù)據(jù)量進(jìn)行模數(shù)轉(zhuǎn)換，并進(jìn)行驅(qū)動(dòng)控制。

第三， 通訊線路： 主要通過(guò)CAN 通訊、UART通訊等對(duì)上位機(jī)，電機(jī)等設(shè)備進(jìn)行數(shù)據(jù)傳輸和指令接發(fā)。

第四，控制線路：電機(jī)控制模塊通過(guò)各種信息進(jìn)行S 曲線生成、路徑規(guī)劃和FOC 控制等。

3 控制算法設(shè)計(jì)

3.1 控制框架

伺服控制系統(tǒng)如圖8 所示， 包括坐標(biāo)變換、SVPWM 空間矢量算法、三閉環(huán)控制技術(shù)、S 曲線運(yùn)動(dòng)控制算法和前饋控制算法等。 首先，通過(guò)S 曲線運(yùn)動(dòng)控制算法計(jì)算出位置信息；然后將給定的位置信息和編碼器采集到的位置信息的差值，輸入到位置環(huán)中，形成位置閉環(huán)反饋控制；再將給定速度值和反饋速度的差值送入速度環(huán)中，速度環(huán)的輸出結(jié)果會(huì)通過(guò)電流環(huán)計(jì)算出d 軸q 軸電流。 最后，再通過(guò)坐標(biāo)變換、SVPWM 空間矢量完成對(duì)電機(jī)的輸出。其中在速度環(huán)和電流環(huán)中加入了由S 曲線規(guī)劃產(chǎn)生的速度和加速度信息，用于前饋控制，從而提高各個(gè)環(huán)的精度和響應(yīng)速度。

圖8 伺服驅(qū)動(dòng)軟件平臺(tái)架構(gòu)Figure 8 Servo drive software platform architecture

3.2 S 曲線控制

伺服電機(jī)在啟動(dòng)和停止階段分別會(huì)出現(xiàn)加速和減速的過(guò)渡過(guò)程，加入S 曲線可以保證伺服系統(tǒng)平穩(wěn)運(yùn)動(dòng)，避免沖擊和振蕩。 在伺服控制中，S 曲線具有啟動(dòng)時(shí)需要伺服電機(jī)輸出轉(zhuǎn)矩小，加減速時(shí)高次諧波分量少，速度平滑性較好等優(yōu)點(diǎn)，本項(xiàng)目中加減速運(yùn)行過(guò)程可分為3 段：加速段（T1）、勻速段（T2）、減速段（T3），S 曲線加減速過(guò)程中位置、速度、加速度、加加速度變化情況如圖9 所示。

圖9 S 曲線示意圖Figure 9 Schematic diagram of S curve

本系統(tǒng)中加加速度使用正弦函數(shù)來(lái)代替正負(fù)函數(shù)，這樣的目的是可以合并加加速階段和減加速階段，合并加減速階段和減減速階段，計(jì)算加速度、速度和位置時(shí)更簡(jiǎn)便。 另外，本系統(tǒng)中的S 曲線考慮了在有初始速度情況下的運(yùn)動(dòng)規(guī)劃，解決在有速度時(shí)接受位置指令后的規(guī)劃問(wèn)題。

S 曲線加減速過(guò)程中位置s、 速度v、 加速度a、加加速度j在三個(gè)階段的值分別為：（初始速度為v0）

加速階段（T1），t∈[0，T1]：

勻速階段（T2），t∈[T1，T2]：

減速階段（T3），t∈[T2，T3]：

3.3 前饋優(yōu)化

在伺服系統(tǒng)中，前饋控制可以用來(lái)提高系統(tǒng)的響應(yīng)特性和跟蹤性能，前饋控制的特點(diǎn)是無(wú)須等到被控變量出現(xiàn)偏差，而是直接按照預(yù)先給定的指令給出控制量[5]。

傳統(tǒng)前饋控制如圖10 所示，其中G（s）是PID控制器與系統(tǒng)結(jié)合的傳遞函數(shù)，R（s）是系統(tǒng)輸入量，Y（s）是系統(tǒng)輸出。N（s）就是前饋控制的傳遞函數(shù)。

圖10 前饋控制示意圖Figure 10 Schematic diagram of feedforward control

由圖可得，前饋控制系統(tǒng)的系統(tǒng)輸出Y（s）等于：

可得：

我們期望的理想系統(tǒng)，就是輸出量可以準(zhǔn)確、快速地跟隨系統(tǒng)輸入量，所以理想情況下Y（s）=R（s）。 由此可得N（s）=1/G（s）

所以對(duì)于系統(tǒng)而言，當(dāng)N（s）=1/G（s）時(shí)可以實(shí)現(xiàn)輸出量對(duì)輸入量的完全跟隨，達(dá)到理想的前饋控制。 但在實(shí)際的情況中由于系統(tǒng)比較復(fù)雜，一般采用部分補(bǔ)償?shù)姆绞綄?duì)系統(tǒng)進(jìn)行補(bǔ)償。

由于普通的前饋補(bǔ)償需要將輸入的位置信息再進(jìn)行兩次微分， 實(shí)現(xiàn)對(duì)速度和加速度信號(hào)的補(bǔ)償，但微分會(huì)導(dǎo)致系統(tǒng)計(jì)算時(shí)間上升和復(fù)雜度。 在本系統(tǒng)中， 我們采用將S 曲線規(guī)劃中計(jì)算得到的速度和加速度信息直接按一定比例補(bǔ)償給速度環(huán)和電流環(huán)，實(shí)現(xiàn)更簡(jiǎn)便的前饋補(bǔ)償，具體原理如圖11 所示。

圖11 S 曲線輸出優(yōu)化前饋控制示意圖Figure 11 Schematic diagram of S-curve output optimized feedforward control

對(duì)于速度環(huán)和電流環(huán)來(lái)說(shuō)，對(duì)應(yīng)的輸入除上一環(huán)解算的數(shù)據(jù)外還要加上S 曲線計(jì)算出的速度或加速度。 公式如式（4）和式（5）所示：

式中：Vref——速度環(huán)的輸入?yún)⒖妓俣龋?/p>

pidpos——位置環(huán)的輸出結(jié)果；

vsref——S 曲線計(jì)算出的速度值；

cofv——速度值的系數(shù)；

iqref——電流環(huán)的輸入?yún)⒖妓俣龋?/p>

pidvel——速度環(huán)的輸出結(jié)果；

asref——S 曲線計(jì)算出的加速度值；

cofa——加速度值的系數(shù)。

4 試驗(yàn)結(jié)果分析

4.1 試驗(yàn)環(huán)境搭建

本文設(shè)計(jì)的伺服驅(qū)動(dòng)器所使用的電機(jī)采用maxon 公司的EC90flat 型號(hào)電機(jī)，電機(jī)的主要參數(shù)為額定電壓24V， 額定電流0.658A， 額定轉(zhuǎn)速3179rpm，每相電阻0.216Ω。 每相電感0.232mh，極對(duì)數(shù)11，轉(zhuǎn)矩系數(shù)71.2nNm/A，轉(zhuǎn)動(dòng)慣量3170gcm2。

本文主要驗(yàn)證在旋轉(zhuǎn)一定角度情況下伺服驅(qū)動(dòng)器的控制效果。

4.2 結(jié)果對(duì)比

本試驗(yàn)分別對(duì)比了之前S 曲線輸出和分別加入速度前饋、 加入速度和加速度前饋情況下的位置輸出曲線。 圖12 為原始S 曲線輸出示意圖，圖13 為加入速度前饋后S 曲線輸出示意圖，圖14 為加入加速度和速度前饋后S 曲線輸出示意圖。 其中灰色線代表電機(jī)參考位置， 黑色線代表電機(jī)實(shí)際轉(zhuǎn)動(dòng)位置。

圖12 原始S 曲線輸出示意圖Figure 12 Schematic diagram of the original S-curve output

圖13 加入速度前饋后S 曲線輸出示意圖Figure 13 Schematic diagram of S-curve output after speed feedforward is added

圖14 加入加速度和速度前饋后S 曲線輸出示意圖Figure 14 Schematic diagram of S-curve output after acceleration and velocity feedforward are added

在為伺服驅(qū)動(dòng)器供電后， 控制位置環(huán)由0°旋轉(zhuǎn)到3600°， 圖12、13、14 分別為三種情況下的S曲線輸出。 比較三者曲線可以得出，采用普通S 曲線控制時(shí)，超調(diào)量為0.3°，穩(wěn)定時(shí)的誤差為0.146°，采用速度前饋時(shí)超調(diào)量為和穩(wěn)定時(shí)的誤差均為0.0766°， 采用速度和加速度前饋時(shí)超調(diào)量和穩(wěn)定時(shí)的誤差為0.01416°。另外通過(guò)對(duì)比啟動(dòng)后旋轉(zhuǎn)到720°時(shí)的曲線， 我們可以得出三種方法的跟隨效果：采用單S 曲線時(shí)，到達(dá)720°的目標(biāo)和實(shí)際曲線誤差在0.8ms；而加入速度前饋后為0.093ms，加入速度和加速度前饋后為0.0003ms。 由試驗(yàn)可以看出，在加入速度前饋后，超調(diào)減少74.5%；穩(wěn)態(tài)誤差減少47.5%，跟隨時(shí)間減少88.4%；另外加入加速度前饋后，超調(diào)減少95.2%，穩(wěn)態(tài)誤差減少90.3%，減少了99%的跟隨時(shí)間，提高了電機(jī)的響應(yīng)速度。

5 結(jié)語(yǔ)

本文設(shè)計(jì)了基于S 曲線的前饋三閉環(huán)PID 控制低壓伺服驅(qū)動(dòng)器，采用了ARM7 內(nèi)核芯片，具有低電壓大功率、運(yùn)算速度快、控制精度高等特點(diǎn)。通過(guò)電機(jī)驅(qū)動(dòng)電路實(shí)現(xiàn)整體的回路控制功能，反饋電路支持相對(duì)編碼器和霍爾傳感器，并有SWD，USB，CAN，RS232 等多種通訊方式； 通過(guò)電流讀取和溫度傳感器等實(shí)現(xiàn)過(guò)流保護(hù)、溫度保護(hù)等功能。 伺服驅(qū)動(dòng)器軟件分為應(yīng)用層、設(shè)備驅(qū)動(dòng)層、平臺(tái)驅(qū)動(dòng)層和通用庫(kù)四大模塊，通過(guò)各個(gè)模塊之間的通訊和調(diào)用實(shí)現(xiàn)存儲(chǔ)、模數(shù)轉(zhuǎn)換、通訊、控制等功能。

本文在使用S 曲線平穩(wěn)運(yùn)動(dòng)的基礎(chǔ)上，在速度環(huán)和電流環(huán)中加入S 曲線計(jì)算生成的速度和加速度作為前饋補(bǔ)償。 試驗(yàn)證明，加入前饋補(bǔ)償有效提高了電機(jī)實(shí)際的跟隨性能，同時(shí)電機(jī)超調(diào)和精度也有所提高。 與單獨(dú)在速度環(huán)加入前饋補(bǔ)償相比，速度環(huán)和電流環(huán)都加入前饋補(bǔ)償在超調(diào)量、 穩(wěn)態(tài)誤差、跟隨效果上都有更好的表現(xiàn)。 該驅(qū)動(dòng)器與傳統(tǒng)前饋控制相比，無(wú)需進(jìn)行微分計(jì)算，減少了計(jì)算量，具有較大的應(yīng)用價(jià)值。