專利名稱：基于fpga的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)器人領(lǐng)域，具體涉及一種基于FPGA的模塊化雙關(guān)節(jié)控制系統(tǒng)。
背景技術(shù)：
隨著機(jī)器人技術(shù)發(fā)展，對機(jī)器人系統(tǒng)的性能要求越來越高，不僅要求具有快速的響應(yīng)特性，較高的跟蹤精度，而且要求具有良好的通用性和擴(kuò)展性。作為機(jī)器人執(zhí)行機(jī)構(gòu)的直接驅(qū)動者，關(guān)節(jié)控制器是整個系統(tǒng)中關(guān)鍵的一點。目前關(guān)節(jié)控制系統(tǒng)大多采用基于ARM、 DSP平臺實現(xiàn)，這些控制系統(tǒng)集成度低、系統(tǒng)復(fù)雜，已經(jīng)不能滿足現(xiàn)代工業(yè)和社會發(fā)展的要求。隨著機(jī)器人體系結(jié)構(gòu)的發(fā)展，機(jī)器人模塊化的研究也得到了長足的發(fā)展，模塊化機(jī)器人是由一個個獨立的功能相同或者相似的模塊重組而成，要求每個模塊的適用性高， 抗干擾性強(qiáng)；機(jī)器人小型化也是發(fā)展的一個重要趨勢，傳統(tǒng)的單關(guān)節(jié)控制器使得機(jī)器人體型龐大、控制不靈活；傳統(tǒng)的PID單閉環(huán)控制方法收非線性因素影響嚴(yán)重，適應(yīng)性差。僅用電機(jī)驅(qū)動器進(jìn)行PID參數(shù)自整定不能滿足系統(tǒng)的適用性、穩(wěn)定性要求。

發(fā)明內(nèi)容
為了解決機(jī)器人的控制器部分結(jié)構(gòu)復(fù)雜、體積龐大、集成度低的問題，本發(fā)明提供了一種基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)的設(shè)計方法。本發(fā)明的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)，包括關(guān)節(jié)核心控制單元、CAN 總線通信模塊、電機(jī)驅(qū)動輸出模塊、旋變測角模塊、力矩測量模塊、DC/DC模塊，關(guān)節(jié)核心控制單元包括NIOSII軟核處理器、Avalon總線、CAN總線接口電路、串口接口電路I、PID控制器I、旋變解算接口電路、A/D接口電路、FIFO和看門狗電路，CAN總線通信模塊包括CAN 總線控制器I和總線驅(qū)動器I，電機(jī)驅(qū)動模塊包括電機(jī)電源開關(guān)控制電路、串口通信電路， 旋變測角模塊包括旋變勵磁產(chǎn)生單元和旋變信號解算單元，力矩測量模塊包括電橋測量電路、放大器、A/D轉(zhuǎn)換電路。上位機(jī)通過CAN總線和關(guān)節(jié)核心控制單元進(jìn)行通信，關(guān)節(jié)核心控制單元通過電機(jī)驅(qū)動模塊的串口向電機(jī)驅(qū)動器發(fā)送指令，再由電機(jī)驅(qū)動器控制電機(jī)，同時反饋電機(jī)的位置信息。關(guān)節(jié)核心控制單元用于數(shù)據(jù)的存儲、交換等處理，同時設(shè)計了與外設(shè)的接口邏輯，如與CAN總線控制器之間的訪問時序、雙通道旋變解算、力矩測量信號濾波等，均在片內(nèi)采用硬件描述語言Verilog HDL實現(xiàn)；CAN總線通信模塊用于接收上位機(jī)的指令信息，同時向上位機(jī)反饋各種狀態(tài)信息；電機(jī)驅(qū)動模塊用于接收控制器的命令來控制電機(jī)驅(qū)動器；力矩測量模塊用來測量轉(zhuǎn)矩；旋變測角模塊用作角度傳感器，包括旋變勵磁產(chǎn)生和解算模塊，通過產(chǎn)生旋變的勵磁信號和對旋變輸出的正余弦信號進(jìn)行解算獲得電機(jī)和關(guān)節(jié)的角度信息；DC/DC模塊用于將直流電源變換為關(guān)節(jié)控制器所需要的各種二次電源。本發(fā)明的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)，它是基于上述控制系統(tǒng)實現(xiàn)的，所述控制方法的控制過程為
在系統(tǒng)中通過關(guān)節(jié)核心控制單元的雙閉環(huán)PID控制器，實現(xiàn)對關(guān)節(jié)電機(jī)的控制。上位機(jī)給出位置指令，經(jīng)過雙閉環(huán)的最外環(huán)位置環(huán)控制器后，轉(zhuǎn)化成速度指令通過串口通信電路發(fā)送給電機(jī)驅(qū)動器，同時利用神經(jīng)元網(wǎng)絡(luò)對速度環(huán)的PID參數(shù)進(jìn)行在線整定。本發(fā)明的有益效果本發(fā)明提供了一種可靠性高和小型化的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)；本發(fā)明集機(jī)械系統(tǒng)、通信系統(tǒng)、驅(qū)動系統(tǒng)和傳感系統(tǒng)為一體組成了關(guān)節(jié)控制器；本發(fā)明的兩路電機(jī)驅(qū)動輸出模塊、雙通道多級旋轉(zhuǎn)變壓器等實現(xiàn)了一個控制器對雙關(guān)節(jié)的控制；本發(fā)明的雙閉環(huán)PID控制器實現(xiàn)了對關(guān)節(jié)電機(jī)的伺服控制；本發(fā)明的基于神經(jīng)元網(wǎng)絡(luò)的PID參數(shù)自整定，提高了系統(tǒng)的適用性和穩(wěn)定性。


圖1是應(yīng)用本發(fā)明的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)的結(jié)構(gòu)示意圖，其中，8和12為電機(jī)驅(qū)動器，9和13為電機(jī)，10為雙通道旋轉(zhuǎn)變壓器，10-1為雙通道旋轉(zhuǎn)變壓器I，10-2為雙通道旋轉(zhuǎn)變壓器II，圖2為雙閉環(huán)PID控制策略圖，圖3為基于BP神經(jīng)網(wǎng)絡(luò)的PID整定。
具體實施方法具體實施方式
一根據(jù)說明書附圖1具體說明本實施方式，本實施方式所述的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)，包括關(guān)節(jié)核心控制單元1、CAN總線通信模塊2、電機(jī)驅(qū)動輸出模塊3、旋變測角模塊4、力矩測量模塊5、DC/DC模塊6，關(guān)節(jié)核心控制單元1包括 NIOSII軟核處理器1-1、Avalon總線1-2、CAN總線接口電路1-3、旋變解算接口電路1-6、 A/D接口電路1-7、FIF01-8, CAN總線通信模塊2包括CAN總線控制器12-1和總線驅(qū)動器 12-2，電機(jī)驅(qū)動模塊3包括電機(jī)電源開關(guān)控制電路3-1和串口通信電路3-2，力矩測量模塊 5包括電橋測量電路5-1、放大器5-2、A/D轉(zhuǎn)換電路5-3 ；上位機(jī)C通過CAN總線2和關(guān)節(jié)核心控制單元1進(jìn)行通信，關(guān)節(jié)核心控制單元1通過電機(jī)驅(qū)動模塊3的串口 3-2向電機(jī)驅(qū)動器8發(fā)送指令，電機(jī)驅(qū)動器8最終控制電機(jī)9，同時反饋電機(jī)9的位置信息；關(guān)節(jié)核心控制單元1用于數(shù)據(jù)的存儲、交換等處理，同時設(shè)計了與外設(shè)的接口邏輯，如與CAN總線接口電路1-3、旋變解算接口電路1-6、A/D接口電路1-7等，均在片內(nèi)利用Verilog HDL實現(xiàn)； CAN總線通信模塊2用于控制CAN總線，實現(xiàn)關(guān)節(jié)控制器1和上位機(jī)C的通信，CAN總線用于接收上位機(jī)C的指令信息，同時反饋各種狀態(tài)信息；電機(jī)驅(qū)動模塊3用于接收控制器1的命令來控制電機(jī)驅(qū)動器8 ；力矩測量模塊5利用電橋測量電路5-1、放大器5-2、A/D轉(zhuǎn)換電路5-3來測量轉(zhuǎn)矩；旋變測角模塊4用作角度傳感器；DC/DC模塊6用于將直流電源變換為關(guān)節(jié)控制器1所需要的各種二次電源。本具體實施方式
中，在FPGA板上實現(xiàn)了關(guān)節(jié)空間的位置控制、CAN總線電路的控制，同時用電源板實現(xiàn)電機(jī)驅(qū)動輸出模塊，采用PCB板間電連接器進(jìn)行互聯(lián)，旋變測角模塊、力矩測量模塊、DC/DC模塊同時分布于FPGA板、電源板上，減少了雙關(guān)節(jié)控制器中不同特性電路之間的耦合和干擾。本實施方式中，中央控制計算機(jī)作為上位機(jī)，雙關(guān)節(jié)控制器作為下位機(jī)，通過CAN總線互聯(lián)，組成上下位機(jī)、分布式的控制網(wǎng)絡(luò)。本具體實施方式
中，旋變測角模塊4包括旋變勵磁產(chǎn)生單元4-1和旋變信號解算單元4-2，旋變勵磁產(chǎn)生單元4-1產(chǎn)生部分需要的正弦波，作為雙通道旋轉(zhuǎn)變壓器10-1的輸入信號，同時為旋變解算單元4-2提供用于相位同步的參考信號，雙通道旋轉(zhuǎn)變壓器10-1 輸出信號到旋變解算單元4-2中，再經(jīng)過旋變解算接口電路1-6的轉(zhuǎn)換把采集到的電機(jī)和關(guān)節(jié)的角度信息輸出給關(guān)節(jié)核心控制單元1中。雙通道旋轉(zhuǎn)變壓器I是由一個多極旋轉(zhuǎn)變壓器(精機(jī))和一個單機(jī)旋轉(zhuǎn)變壓器(粗機(jī))集成在一起，粗機(jī)和精機(jī)各對應(yīng)一個旋變信號解算器，即旋變解算單元4-2中包含兩個旋變信號解算器，旋變解算接口電路1-6同時把兩個旋變信號解算器的信息采集過來，經(jīng)過分析運(yùn)算后得到角度信息。本具體實施方式
中，關(guān)節(jié)核心控制單元1中的A/D接口電路1-7、FIF01-8用來轉(zhuǎn)換從力矩測量模塊5中采集到的轉(zhuǎn)矩信息。力矩測量模塊5中的電橋測量電路15-1用于采集關(guān)節(jié)力矩，信號經(jīng)過放大器15-2放大后發(fā)送給A/D轉(zhuǎn)換電路5-3，一方面可以信號可以經(jīng)過A/D接口電路1-7，轉(zhuǎn)換成NIOSII可以使用的信號，另一方面也可以經(jīng)過A/D接口電路 1-7和FIF01-8求和得到NIOSII可以使用的信號。
具體實施方式
二，本具體實施方式
與具體實施方式
一的區(qū)別在于，關(guān)節(jié)控制器實現(xiàn)了對雙關(guān)節(jié)的控制。CAN總線通信模塊2還包括CAN總線控制器12-1和CAN總線驅(qū)動器12-2，每個關(guān)節(jié)包含一個CAN總線控制器和驅(qū)動器，上位機(jī)通過發(fā)送數(shù)據(jù)來選擇不同的關(guān)節(jié)通道，實現(xiàn)對雙關(guān)節(jié)的控制。本具體實施方式
中，旋變測角模塊4還包括多路模擬量開關(guān)14-3和多路模擬量開關(guān)114-4，雙通道旋轉(zhuǎn)變壓器10中還包括一個雙通道旋轉(zhuǎn)變壓器II 10-2，每一個關(guān)節(jié)中含有一個雙通道旋轉(zhuǎn)變壓器。旋變測角模塊4通過旋變解算單元4-2中的一片旋變解算器與一個多路模擬量開關(guān)14-3或者多路模擬量開關(guān)II4-4組成兩個獨立的通路，每個通路都可以完成關(guān)節(jié)的旋變精機(jī)和粗機(jī)的信號解算，多路模擬量開關(guān)完成雙通道旋轉(zhuǎn)變壓器之間的切換，即完成關(guān)節(jié)之間的切換。該設(shè)計雖然增加了多路模擬量開關(guān)，但極大地簡化了系統(tǒng)的設(shè)計，減少了體積、質(zhì)量和功耗。本具體實施方式
中，力矩測量模塊5還包括電橋測量電路II5-4和放大器115-5， 來完成對關(guān)節(jié)二的力矩信息采集。本具體實施方式
中，PID控制器111-9、串口接口電路111-10、電機(jī)驅(qū)動輸出模塊 1111、電機(jī)驅(qū)動器1112和電機(jī)1113同樣用來控制關(guān)節(jié)II。本具體實施方式
，對雙關(guān)節(jié)的控制集成在一個控制器上，減少了電路板的數(shù)量，實現(xiàn)了一體化雙關(guān)節(jié)的小型化設(shè)計。
具體實施方式
三，本具體實施方式
與實施方式一、二的區(qū)別在于添加了看門狗電路7，集成在FPGA芯片上?？撮T狗實質(zhì)上是一個定時器電路，看門狗的設(shè)置分兩步進(jìn)行，包括初始化狗和喂狗操作，用軟件操作來實現(xiàn)。在系統(tǒng)參數(shù)初始化以后初始化狗，在主控周期模塊喂狗。當(dāng)軟件工作不正常時，定時器會非正常工作。在規(guī)定的周期內(nèi)中斷喂狗操作，則系統(tǒng)自動復(fù)位。本具體實施方式
中，在軟件中添加看門狗模塊是為了監(jiān)視軟件的運(yùn)行情況，當(dāng)軟件運(yùn)行不正常時系統(tǒng)會自動復(fù)位，恢復(fù)正常工作，防止程序發(fā)生死循環(huán)，增加了系統(tǒng)的可靠性和穩(wěn)定性。
具體實施方式
四，本具體實施方式
與具體實施方式
一、二或三的區(qū)別在于，關(guān)節(jié)核心控制單元1還包括串口接口電路11-4和PID控制器11-5。在系統(tǒng)中通過關(guān)節(jié)核心控制單元1的雙閉環(huán)PID控制器11-5，實現(xiàn)對關(guān)節(jié)電機(jī)的控制。上位機(jī)C給出位置指令，經(jīng)過雙閉環(huán)的最外環(huán)位置環(huán)控制器后，轉(zhuǎn)化成速度指令通過串口通信電路3-2發(fā)送給電機(jī)驅(qū)動器，電機(jī)驅(qū)動器直接控制電機(jī)，實現(xiàn)對電機(jī)的伺服控制。其中串口接口電路11-4是串口通信電路3-2的驅(qū)動電路。本具體實施方式
中，雙閉環(huán)PID控制策略如圖2所示。上位機(jī)C給出位置指令后， 在關(guān)節(jié)控制器中和旋轉(zhuǎn)變壓器反饋的位置信息進(jìn)行比較，得到一個位置信號作為位置環(huán)控制器的輸入，位置環(huán)控制器輸出速度指令，再和光電編碼器采集到的速度信息進(jìn)行比較后， 得到一個速度信號作為電流環(huán)控制器的輸入，電流環(huán)實現(xiàn)對電機(jī)電磁轉(zhuǎn)矩的控制。內(nèi)環(huán)對速度和電流進(jìn)行直接控制，外環(huán)控制位置，諧波減速器放在外環(huán)，減少了對速度環(huán)的影響， 提高了系統(tǒng)的穩(wěn)定性。本具體實施方式
中，雙閉環(huán)控制中的電流環(huán)控制器和PWM的作用是由電機(jī)驅(qū)動器來實現(xiàn)。
具體實施方式
五，本具體實施方式
是對具體實施方式
四的進(jìn)一步說明，PID控制器 11-5中，利用神經(jīng)元網(wǎng)絡(luò)對PID控制器11-5的位置環(huán)和速度環(huán)進(jìn)行PID參數(shù)在線整定。本具體實施方式
中，基于BP神經(jīng)網(wǎng)絡(luò)的PID整定如圖3中所示，位置誤差和誤差偏方作為BP神經(jīng)元網(wǎng)絡(luò)的輸入，輸出為位置環(huán)的參數(shù)KPP，速度環(huán)的參數(shù)KP和KI。通過神經(jīng)網(wǎng)絡(luò)自身的學(xué)習(xí)、加權(quán)系數(shù)調(diào)整，從而使系統(tǒng)達(dá)到最佳穩(wěn)定狀態(tài)。本具體實施方式
中，利用BP神經(jīng)網(wǎng)絡(luò)對PID參數(shù)進(jìn)行整定，提高了系統(tǒng)的適用性， 實現(xiàn)了系統(tǒng)的高性能穩(wěn)定機(jī)制。
權(quán)利要求
1.基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)，其特征在于它包括關(guān)節(jié)核心控制單元1、 CAN總線通信模塊2、電機(jī)驅(qū)動輸出模塊3、旋變測角模塊4、力矩測量模塊5、DC/DC模塊6， 關(guān)節(jié)核心控制單元1包括NIOSII軟核處理器l-l、Avalon總線1_2、CAN總線接口電路1_3、 旋變解算接口電路1-6、A/D接口電路1-7、FIF01-8, CAN總線通信模塊2包括CAN總線控制器12-1和總線驅(qū)動器12-2，電機(jī)驅(qū)動模塊3包括電機(jī)電源開關(guān)控制電路3-1和串口通信電路3-2，力矩測量模塊5包括電橋測量電路5-1、放大器5-2、A/D轉(zhuǎn)換電路5_3 ；上位機(jī)C通過CAN總線2和關(guān)節(jié)核心控制單元1進(jìn)行通信，關(guān)節(jié)核心控制單元1通過電機(jī)驅(qū)動模塊3的串口 3-2向電機(jī)驅(qū)動器8發(fā)送指令，電機(jī)驅(qū)動器8最終控制電機(jī)9，同時反饋電機(jī)9的位置信息；關(guān)節(jié)核心控制單元1用于數(shù)據(jù)的存儲、交換等處理，同時設(shè)計了與外設(shè)的接口邏輯， 如與CAN總線接口電路1-3、旋變解算接口電路1-6、A/D接口電路1_7等，均在片內(nèi)利用 Verilog HDL 實現(xiàn)；CAN總線通信模塊2用于控制CAN總線，實現(xiàn)關(guān)節(jié)控制器1和上位機(jī)C的通信，CAN總線用于接收上位機(jī)C的指令信息，同時反饋各種狀態(tài)信息；電機(jī)驅(qū)動模塊3用于接收控制器1的命令來控制電機(jī)驅(qū)動器8 ；力矩測量模塊5利用電橋測量電路5-1、放大器5-2、A/D轉(zhuǎn)換電路5-3來測量轉(zhuǎn)矩；旋變測角模塊4用作角度傳感器；DC/DC模塊6用于將直流電源變換為關(guān)節(jié)控制器1所需要的各種二次電源。
2.根據(jù)權(quán)利要求1所述的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)，其特征在于，CAN 總線通信模塊2還包括CAN總線控制器12-1和CAN總線驅(qū)動器12_2，每個關(guān)節(jié)包含一個 CAN總線控制器和驅(qū)動器，上位機(jī)通過發(fā)送數(shù)據(jù)來選擇不同的關(guān)節(jié)通道，實現(xiàn)對雙關(guān)節(jié)的控制。
3.權(quán)利要求1所述的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)，其特征在于，它還包括看門狗電路7，集成在FPGA芯片上?？撮T狗實質(zhì)上是一個定時器電路，看門狗的設(shè)置分兩步進(jìn)行，包括初始化狗和喂狗操作，用軟件操作來實現(xiàn)。在系統(tǒng)參數(shù)初始化以后初始化狗，在主控周期模塊喂狗。當(dāng)軟件工作不正常時，定時器會非正常工作。在規(guī)定的周期內(nèi)中斷喂狗操作，則系統(tǒng)自動復(fù)位。
4.權(quán)利要求1所述的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)，其特征在于，關(guān)節(jié)核心控制單元1還包括串口接口電路11-4和PID控制器11-5。在系統(tǒng)中通過關(guān)節(jié)核心控制單元1的雙閉環(huán)PID控制器11-5，實現(xiàn)對關(guān)節(jié)電機(jī)的控制。上位機(jī)C給出位置指令，經(jīng)過雙閉環(huán)的最外環(huán)位置環(huán)控制器后，轉(zhuǎn)化成速度指令通過串口通信電路3-2發(fā)送給電機(jī)驅(qū)動器， 電機(jī)驅(qū)動器直接控制電機(jī)，實現(xiàn)對電機(jī)的伺服控制。其中串口接口電路11-4是串口通信電路3-2的驅(qū)動電路。
5.權(quán)利要求1或4所述的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)，其特征在于，PID 控制器11-5中，利用神經(jīng)元網(wǎng)絡(luò)對PID控制器11-5的位置環(huán)和速度環(huán)進(jìn)行PID參數(shù)在線整定。
全文摘要
本發(fā)明的基于FPGA的模塊化雙關(guān)節(jié)伺服控制系統(tǒng)，包括關(guān)節(jié)核心控制單元、CAN總線通信模塊、電機(jī)驅(qū)動輸出模塊、旋變測角模塊、力矩測量模塊、DC/DC模塊，關(guān)節(jié)核心控制單元包括NIOSII軟核處理器、Avalon總線、CAN總線接口電路、串口接口電路I、PID控制器I、旋變解算接口電路、A/D接口電路、FIFO和看門狗電路，CAN總線通信模塊包括CAN總線控制器I和總線驅(qū)動器I，電機(jī)驅(qū)動模塊包括電機(jī)電源開關(guān)控制電路、串口通信電路，旋變測角模塊包括旋變勵磁產(chǎn)生單元和旋變信號解算單元，力矩測量模塊包括電橋測量電路、放大器、A/D轉(zhuǎn)換電路。上位機(jī)通過CAN總線和關(guān)節(jié)核心控制單元進(jìn)行通信，關(guān)節(jié)核心控制單元通過電機(jī)驅(qū)動模塊的串口向電機(jī)驅(qū)動器發(fā)送指令，再由電機(jī)驅(qū)動器控制電機(jī)，同時反饋電機(jī)的位置信息。
文檔編號G05B19/04GK102354134SQ20111025892
公開日2012年2月15日 申請日期2011年9月2日 優(yōu)先權(quán)日2011年9月2日
發(fā)明者史翰林, 葉平, 孫漢旭, 張丹, 張延恒, 曾祥宇, 李思, 賈慶軒, 魏楠哲 申請人:北京郵電大學(xué)