楊洪濤，胡婷婷，范祿源，沈梅，程晶晶，張夢(mèng)遙

安徽理工大學(xué)機(jī)械工程學(xué)院

1 引言

自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)是參考手動(dòng)式關(guān)節(jié)臂測(cè)量機(jī)的結(jié)構(gòu)，并借鑒串聯(lián)機(jī)器人控制系統(tǒng)，在測(cè)量機(jī)各關(guān)節(jié)處加入伺服電機(jī)、諧波減速器和編碼器等器件組成的閉環(huán)控制系統(tǒng)，可以自動(dòng)實(shí)現(xiàn)高精度的實(shí)時(shí)定位和測(cè)量[1]。

自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)對(duì)絕對(duì)定位精度有很高的要求，尤其是對(duì)多電機(jī)的高精度同步控制[2]，因此國(guó)內(nèi)外學(xué)者對(duì)多電機(jī)的同步運(yùn)動(dòng)控制系統(tǒng)進(jìn)行了深入研究。李俊敏[3]使用STM32設(shè)計(jì)了可以協(xié)調(diào)運(yùn)動(dòng)的六足機(jī)器人運(yùn)動(dòng)控制系統(tǒng)。Cho J.U.等[4]使用FPGA作為多軸運(yùn)動(dòng)控制器的核心，實(shí)現(xiàn)了對(duì)多接口模塊開發(fā)。陳亞等[5]以STM32+FPGA為核心設(shè)計(jì)了一款六自由度機(jī)器人運(yùn)動(dòng)控制器。王成瓊等[6]使用ARM+DSP設(shè)計(jì)了一種高精度三軸運(yùn)動(dòng)控制器，實(shí)現(xiàn)了機(jī)器人的精確運(yùn)動(dòng)控制。劉大偉等[7]以ARM+DSP+FPGA為架構(gòu)并結(jié)合梯形加減速控制策略，通過控制插補(bǔ)速度，從而提高了控制精度。李靜嵐[8]基于CAN總線、董漢卿[9]基于EtherCAT實(shí)現(xiàn)了分布式多電機(jī)的同步控制。由于通信系統(tǒng)會(huì)產(chǎn)生延時(shí)，張謙[10]通過模糊PID結(jié)合交叉耦合控制算法，降低了延時(shí)對(duì)同步精度的影響。

為使自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)的多電機(jī)同步運(yùn)動(dòng)擁有更高的精度，根據(jù)上述研究現(xiàn)狀，以FPGA+STM32為核心運(yùn)動(dòng)控制器，對(duì)雙關(guān)節(jié)的同步控制系統(tǒng)進(jìn)行了研究與開發(fā)。

2 系統(tǒng)結(jié)構(gòu)和硬件設(shè)計(jì)

自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)的雙關(guān)節(jié)結(jié)構(gòu)如圖1所示，關(guān)節(jié)1為橫滾關(guān)節(jié)，關(guān)節(jié)2為俯仰關(guān)節(jié)，本測(cè)控系統(tǒng)即針對(duì)此關(guān)節(jié)模塊展開。各關(guān)節(jié)內(nèi)部由伺服電機(jī)、編碼器和諧波減速器等器件組成，伺服電機(jī)選用表貼式三相永磁同步電機(jī)，該電機(jī)具有控制精度較好和電磁干擾小等優(yōu)點(diǎn)。為使自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)在線測(cè)量精度更高，要求轉(zhuǎn)角誤差≤0.2′，同步控制時(shí)間精度≤1ms。

圖1 雙關(guān)節(jié)結(jié)構(gòu)

自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)的雙關(guān)節(jié)同步控制系統(tǒng)框架如圖2所示，主要包括上位機(jī)、運(yùn)動(dòng)控制器、伺服電機(jī)系統(tǒng)、諧波減速器和關(guān)節(jié)末端編碼器等。上位機(jī)提供人機(jī)交互界面，實(shí)現(xiàn)雙電機(jī)的角度和速度參數(shù)設(shè)置、指令收發(fā)與顯示、與FPGA進(jìn)行雙向串口通信等功能。系統(tǒng)以FPGA和STM32為核心運(yùn)動(dòng)控制器，F(xiàn)PGA型號(hào)為EP4CE10F17C8，主要實(shí)現(xiàn)雙關(guān)節(jié)末端編碼器信號(hào)采集、雙電機(jī)同步控制、與上位機(jī)和STM32通信等功能；STM32型號(hào)為STM32F407IGT6，主要實(shí)現(xiàn)對(duì)單個(gè)電機(jī)的三閉環(huán)控制、與FPGA通信等功能。伺服電機(jī)驅(qū)動(dòng)板主要實(shí)現(xiàn)ADC電流采樣轉(zhuǎn)換、坐標(biāo)系變換、矢量控制算法以及編碼器數(shù)據(jù)處理等功能。

圖2 雙關(guān)節(jié)同步控制系統(tǒng)框架

2.1 關(guān)節(jié)末端編碼器信號(hào)采集模塊

關(guān)節(jié)末端編碼器采用NPN集電極開路信號(hào)的E6B2-CWZ6C旋轉(zhuǎn)編碼器，24V直流電供電。為了實(shí)現(xiàn)關(guān)節(jié)末端編碼器的高精度實(shí)時(shí)數(shù)據(jù)采集，消除編碼器輸出信號(hào)的干擾，將求解后的轉(zhuǎn)動(dòng)角度和速度傳輸至上位機(jī)，在Quartus Ⅱ軟件中設(shè)計(jì)了采集關(guān)節(jié)末端編碼器信號(hào)的電路(見圖3a)。

2.2 CAN總線通信模塊

CAN總線通信模塊采用雙絞線連接方式，在FPGA和STM32之間以差分信號(hào)進(jìn)行雙向傳輸。FPGA的高速CAN收發(fā)器選擇JA1050芯片(見圖3b)，終端電阻R2=120Ω。為減少回波反射現(xiàn)象，保證系統(tǒng)的阻抗匹配，STM32的高速CAN收發(fā)器選擇SN65HVD230芯片，其最大波特率為1Mbps(見圖3c)，CAN_H和CAN_L之間同樣采用120Ω的終端電阻。

2.3 電機(jī)同步控制模塊

雙電機(jī)同步控制信號(hào)電路連接如圖3d所示。FPGA與STM32之間需要進(jìn)行數(shù)據(jù)的傳輸，先配置IP核，保證FPGA能夠接收上位機(jī)的指令控制，再通過CAN總線轉(zhuǎn)發(fā)至兩個(gè)STM32實(shí)現(xiàn)雙電機(jī)的同步控制。反之，F(xiàn)PGA通過CAN接收兩個(gè)STM32實(shí)時(shí)傳回的數(shù)據(jù)，再通過串口通信將雙電機(jī)的角度和速度等數(shù)據(jù)傳輸至上位機(jī)。

(a)關(guān)節(jié)末端編碼器信號(hào)采集電路

2.4 整體硬件裝置

根據(jù)雙關(guān)節(jié)的系統(tǒng)框架和相應(yīng)的硬件電路設(shè)計(jì)，搭建雙電機(jī)同步控制系統(tǒng)的整體硬件裝置(見圖4)。上位機(jī)通過串口通信與FPGA相連，F(xiàn)PGA與兩個(gè)STM32通過CAN總線通信連接，兩個(gè)STM32分別連接伺服電機(jī)驅(qū)動(dòng)板，電機(jī)驅(qū)動(dòng)板與永磁同步電機(jī)相連構(gòu)成伺服電機(jī)系統(tǒng)，電機(jī)驅(qū)動(dòng)板采用直流電源24V供電。

圖4 系統(tǒng)硬件實(shí)物

3 系統(tǒng)軟件設(shè)計(jì)

3.1 電機(jī)控制算法

如圖5a所示，采用三閉環(huán)(位置環(huán)、速度環(huán)和電流環(huán))精確控制電機(jī)，采用id=0矢量控制，既提高了電機(jī)的電磁轉(zhuǎn)矩，又降低了由磁阻轉(zhuǎn)矩導(dǎo)致的電能損耗。根據(jù)電機(jī)id=0控制算法原理，在MATLAB/Simulink中搭建如圖5b所示的電機(jī)控制仿真模型，并驗(yàn)證該算法的可行性。

(a)三閉環(huán)PID控制算法

3.2 上位機(jī)軟件設(shè)計(jì)

在LabVIEW軟件中開發(fā)和設(shè)計(jì)雙電機(jī)同步控制系統(tǒng)的上位機(jī)軟件，上位機(jī)人機(jī)操作界面如圖6所示。根據(jù)自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)的定位測(cè)量需求建立上位機(jī)與運(yùn)動(dòng)控制器，以串口通信方式傳輸數(shù)據(jù)，對(duì)電機(jī)進(jìn)行初始化參數(shù)等，主要實(shí)現(xiàn)對(duì)串口號(hào)的選擇、啟動(dòng)/停止電機(jī)、電機(jī)復(fù)位、急停、設(shè)定雙電機(jī)的角度、速度值以及實(shí)時(shí)顯示波形等功能。

圖6 上位機(jī)人機(jī)交互界面

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

為了驗(yàn)證雙關(guān)節(jié)同步控制系統(tǒng)的穩(wěn)定性以及同步性，根據(jù)雙關(guān)節(jié)同步控制系統(tǒng)的軟硬件設(shè)計(jì)進(jìn)行雙電機(jī)的多次運(yùn)轉(zhuǎn)試驗(yàn)，將所采集的角度和速度數(shù)據(jù)與仿真實(shí)驗(yàn)進(jìn)行對(duì)比分析。

在上位機(jī)操作界面中選擇相應(yīng)的串口號(hào)，設(shè)定雙電機(jī)的目標(biāo)角度分別為400rad和500rad，并將其分別發(fā)送至ID號(hào)不同的兩個(gè)STM32中，再設(shè)定雙電機(jī)的轉(zhuǎn)速為3240r/min，2000r/min和1000r/min，在三種速度下進(jìn)行雙電機(jī)的正、反轉(zhuǎn)試驗(yàn)。根據(jù)上位機(jī)顯示的數(shù)據(jù)所得試驗(yàn)結(jié)果見圖7和圖8。

(a)3240r/min時(shí)角度數(shù)據(jù)界面

(a)3240r/min時(shí)角度數(shù)據(jù)界面

取采集得到的角度誤差最大值進(jìn)行計(jì)算，得到電機(jī)輸出轉(zhuǎn)角的最大誤差值為0.684rad，經(jīng)過諧波減速器(減速比為101:1)后，電機(jī)轉(zhuǎn)角的最大誤差值為0.39°，保證了電機(jī)運(yùn)動(dòng)的穩(wěn)定性。

如圖9所示，為了得到雙電機(jī)的同步時(shí)間，放大圖7a的角度波形圖。觀察可知，雙電機(jī)的同步時(shí)間控制精度為1ms，保證了雙電機(jī)的同步控制，滿足自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)雙關(guān)節(jié)同步控制系統(tǒng)的技術(shù)指標(biāo)要求。

圖9 雙電機(jī)時(shí)間同步精度

5 結(jié)語

通過分析自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)的雙關(guān)節(jié)結(jié)構(gòu)，設(shè)計(jì)了以上位機(jī)和FPGA+STM32為架構(gòu)的雙關(guān)節(jié)同步控制系統(tǒng)。通過實(shí)驗(yàn)實(shí)現(xiàn)了雙電機(jī)的同步運(yùn)動(dòng)，且同步時(shí)間控制在1ms內(nèi)。所設(shè)計(jì)的自驅(qū)動(dòng)關(guān)節(jié)臂測(cè)量機(jī)雙關(guān)節(jié)同步控制系統(tǒng)能夠滿足控制需求，并且具有很好的穩(wěn)定性和同步性。