一種掘進(jìn)式機(jī)器人控制電路的制作方法
【專利說(shuō)明】
[0001]技術(shù)領(lǐng)域:
本發(fā)明涉及一種控制電路，尤其涉及一種用于掘進(jìn)式機(jī)器人的控制電路。
[0002]【背景技術(shù)】:
掘進(jìn)式機(jī)器人主要應(yīng)用于勘測(cè)、采礦等領(lǐng)域，它可以根據(jù)特定的作業(yè)任務(wù)要求，通過(guò)切害J、鉆進(jìn)、挖取等方式，實(shí)現(xiàn)采掘、錨固等功能。由于復(fù)雜特殊的工作環(huán)境，大部分掘進(jìn)式機(jī)器人都需要人工操作，存在很大的安全隱患；一旦工作人員操作不當(dāng)，就有可能導(dǎo)致機(jī)器人工作的穩(wěn)定性和準(zhǔn)確性急劇降低。同時(shí)，在機(jī)器人發(fā)生故障時(shí)，現(xiàn)有的大部分控制電路都缺少必要的安全保護(hù)和狀態(tài)存儲(chǔ)功能，這樣不僅有可能導(dǎo)致機(jī)器人損壞，而且還要求工作人員不得不重新操作，效率較低。
[0003]
【發(fā)明內(nèi)容】
:
本發(fā)明提供了一種掘進(jìn)式機(jī)器人控制電路，該電路支持半自主式工作模式，可實(shí)現(xiàn)對(duì)機(jī)器人多個(gè)電機(jī)的獨(dú)立控制，同時(shí)它擁有電流保護(hù)和數(shù)據(jù)即時(shí)存儲(chǔ)的功能，穩(wěn)定性較好，工作效率較高。
[0004]為了實(shí)現(xiàn)上述目的，本發(fā)明的技術(shù)解決方案如下:
一種掘進(jìn)式機(jī)器人控制電路，其特征在于:包括MCU控制電路、上位機(jī)存儲(chǔ)和控制電路、FPGA運(yùn)動(dòng)控制電路、關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路、掘進(jìn)電機(jī)驅(qū)動(dòng)電路、電流檢測(cè)電路、信號(hào)處理電路、關(guān)節(jié)電機(jī)、關(guān)節(jié)編碼器和掘進(jìn)電機(jī)；所述MCU控制電路為主控制電路，分別與所述上位機(jī)存儲(chǔ)和控制電路和FPGA運(yùn)動(dòng)控制電路雙向通信連接；所述FPGA運(yùn)動(dòng)控制電路與所述關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路和關(guān)節(jié)編碼器相連，讀取關(guān)節(jié)編碼器信號(hào)并向關(guān)節(jié)驅(qū)動(dòng)電路發(fā)出驅(qū)動(dòng)脈沖；所述關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路與所述關(guān)節(jié)電機(jī)相連，控制關(guān)節(jié)電機(jī)工作；所述掘進(jìn)電機(jī)驅(qū)動(dòng)電路的輸入端與所述MCU控制電路相連，接收驅(qū)動(dòng)信號(hào)，其輸出端與所述掘進(jìn)電機(jī)相連，控制掘進(jìn)電機(jī)工作；所述電流檢測(cè)電路的輸入端分別與所述關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路和所述掘進(jìn)電機(jī)驅(qū)動(dòng)電路相連，實(shí)時(shí)檢測(cè)驅(qū)動(dòng)電流的變化；所述信號(hào)處理電路的輸入端、輸出端分別與所述電流檢測(cè)電路、MCU控制電路相連，將得到的驅(qū)動(dòng)電流信號(hào)進(jìn)行處理并反饋給MCU控制電路。
[0005]作為優(yōu)選，所述FPGA運(yùn)動(dòng)控制電路包括編碼器信號(hào)處理模塊、驅(qū)動(dòng)脈沖輸出模塊和串行口讀寫模塊，所述編碼器信號(hào)處理模塊對(duì)關(guān)節(jié)編碼器信號(hào)進(jìn)行處理，再通過(guò)所述串行口讀寫模塊將數(shù)據(jù)發(fā)送至所述MCU控制電路，所述串行口讀寫模塊還負(fù)責(zé)接收所述MCU控制電路發(fā)來(lái)的控制指令來(lái)控制所述驅(qū)動(dòng)脈沖輸出模塊輸出所述驅(qū)動(dòng)脈沖。
[0006]作為優(yōu)選，所述信號(hào)處理電路包括信號(hào)放大電路、AD轉(zhuǎn)換電路和光電隔離電路，所述信號(hào)放大電路將所述電流檢測(cè)電路得到的驅(qū)動(dòng)電流信號(hào)放大，提高分辨率，所述AD轉(zhuǎn)換電路將放大后的模擬信號(hào)轉(zhuǎn)換成數(shù)字信號(hào)，最后經(jīng)過(guò)所述的光電隔離電路傳給所述的MCU控制電路。
[0007]本發(fā)明的掘進(jìn)式機(jī)器人控制電路，以MCU控制電路為核心，通過(guò)上位機(jī)存儲(chǔ)和控制電路來(lái)指定相應(yīng)的工作參數(shù)，在工作過(guò)程中無(wú)需工作人員持續(xù)操作，可實(shí)現(xiàn)半自主式控制。同時(shí)，該控制電路還可以對(duì)機(jī)器人的各運(yùn)動(dòng)關(guān)節(jié)電機(jī)和掘進(jìn)電機(jī)進(jìn)行實(shí)時(shí)監(jiān)控并存儲(chǔ)當(dāng)前的工作狀態(tài)，包括控制模式和各關(guān)節(jié)的位置信息，從而有效的提高機(jī)器人的工作效率以及它的工作穩(wěn)定性和安全性。
【附圖說(shuō)明】:
圖1是本發(fā)明掘進(jìn)式機(jī)器人工作示意圖。
[0008]圖2是本發(fā)明控制電路原理框圖。
[0009]圖3是本發(fā)明控制電路中所述FPGA運(yùn)動(dòng)控制電路原理框圖。
[0010]圖4是本發(fā)明控制電路中所述信號(hào)處理電路原理框圖。
[0011]【具體實(shí)施方式】:
本發(fā)明一種掘進(jìn)式機(jī)器人控制電路的工作原理圖分別如圖1、圖2所示，包括MCU控制電路、上位機(jī)存儲(chǔ)和控制電路、FPGA運(yùn)動(dòng)控制電路、電流檢測(cè)電路、信號(hào)處理電路、關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路、掘進(jìn)電機(jī)驅(qū)動(dòng)電路、關(guān)節(jié)電機(jī)、關(guān)節(jié)編碼器和掘進(jìn)電機(jī)。其中，MCU控制電路為主控制電路，關(guān)節(jié)電機(jī)主要負(fù)責(zé)控制機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)，掘進(jìn)電機(jī)主要負(fù)責(zé)完成掘進(jìn)工作，所述關(guān)節(jié)編碼器采用增量式編碼器，本實(shí)施例具體采用334線光電增量式編碼器，每個(gè)關(guān)節(jié)電機(jī)都連接有該編碼器。
[0012]上位機(jī)存儲(chǔ)和控制電路的兩端分別連接PC機(jī)和MCU控制電路。操作者可以通過(guò)PC機(jī)設(shè)定機(jī)器人的工作參數(shù)(如掘進(jìn)角度、掘進(jìn)深度、掘進(jìn)速度等)并發(fā)送相關(guān)的控制指令(如啟動(dòng)，關(guān)閉等)，這些參數(shù)信息和指令信息首先會(huì)保存在上位機(jī)存儲(chǔ)和控制電路中，再通過(guò)USB接口發(fā)送至MCU控制電路，同時(shí)，MCU控制電路會(huì)將機(jī)器人的實(shí)時(shí)位置信息反饋給上位機(jī)存儲(chǔ)和控制電路。
[0013]如圖3所示，F(xiàn)PGA運(yùn)動(dòng)控制電路包括編碼器信號(hào)處理模塊、驅(qū)動(dòng)脈沖輸出模塊和串行口讀寫模塊，串行口讀寫模塊實(shí)現(xiàn)FPGA運(yùn)動(dòng)控制電路和MCU控制電路的雙向通信，MCU控制電路接收到上位機(jī)存儲(chǔ)和控制電路發(fā)送的控制指令后，通過(guò)該串行口讀寫模塊向FPGA運(yùn)動(dòng)控制電路發(fā)送相應(yīng)的控制指令(如關(guān)節(jié)的拉伸指令，收縮指令，運(yùn)行指令，停止指令等)；驅(qū)動(dòng)脈沖輸出模塊連接關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路，它根據(jù)接收到的控制指令，向關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路輸出相應(yīng)的驅(qū)動(dòng)脈沖，從而控制關(guān)節(jié)電機(jī)轉(zhuǎn)動(dòng)；編碼器信號(hào)處理模塊連接關(guān)節(jié)編碼器，通過(guò)處理關(guān)節(jié)編碼器信號(hào)來(lái)實(shí)時(shí)獲取關(guān)節(jié)電機(jī)的轉(zhuǎn)動(dòng)角度和轉(zhuǎn)動(dòng)方向，再通過(guò)串行口讀寫模塊反饋給MCU控制電路。
[0014]如圖4所示，信號(hào)處理電路包括信號(hào)放大電路、AD轉(zhuǎn)換電路和光電隔離電路。電流檢測(cè)電路的輸入端分別與關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路和掘進(jìn)電機(jī)驅(qū)動(dòng)電路相連，實(shí)時(shí)檢測(cè)驅(qū)動(dòng)電流的變化，并把電流信號(hào)轉(zhuǎn)換為電壓信號(hào)輸出，其輸出端連接信號(hào)放大電路，信號(hào)經(jīng)過(guò)放大處理后，再通過(guò)AD轉(zhuǎn)換電路進(jìn)行模數(shù)轉(zhuǎn)換，最后通過(guò)光電隔離電路傳給MCU控制電路。
[0015]以下介紹本發(fā)明控制電路的控制原理:
控制電路啟動(dòng)后，MCU控制電路同時(shí)開定時(shí)器I和定時(shí)器2，且定時(shí)器I的中斷優(yōu)先級(jí)小于定時(shí)器2。定時(shí)器I中斷服務(wù)程序負(fù)責(zé)每隔一段時(shí)間通過(guò)串口接收一次FPGA運(yùn)動(dòng)控制電路發(fā)送的關(guān)節(jié)電機(jī)實(shí)際轉(zhuǎn)動(dòng)信息，并通過(guò)PID調(diào)節(jié)實(shí)現(xiàn)閉環(huán)控制。當(dāng)關(guān)節(jié)電機(jī)控制機(jī)器人關(guān)節(jié)到達(dá)預(yù)定位置時(shí)，MCU控制電路立即輸出驅(qū)動(dòng)脈沖給掘進(jìn)電機(jī)驅(qū)動(dòng)電路，控制掘進(jìn)電機(jī)工作。當(dāng)任務(wù)完成，系統(tǒng)會(huì)自動(dòng)復(fù)位，等待下一個(gè)操作指令；定時(shí)器2中斷服務(wù)程序每隔一段時(shí)間會(huì)讀取一次由信號(hào)處理電路發(fā)送的各驅(qū)動(dòng)電路的電流值，并對(duì)這些電流值進(jìn)行判斷處理，如果有任意一個(gè)電流值超出設(shè)定好的閾值，控制電路就會(huì)立刻停止運(yùn)行。
[0016]每次停止運(yùn)行，上位機(jī)存儲(chǔ)和控制電路都會(huì)自動(dòng)保存上一個(gè)時(shí)刻的運(yùn)行數(shù)據(jù)，借此獲得上一時(shí)刻的運(yùn)行狀態(tài)。
[0017]每次啟動(dòng)機(jī)器人，操作者都可以通過(guò)PC機(jī)選擇是否加載存儲(chǔ)數(shù)據(jù)，上位機(jī)存儲(chǔ)和控制電路負(fù)責(zé)將該選擇狀態(tài)通過(guò)USB傳給MCU控制電路，由此確定是重新工作還是延續(xù)上一次的工作狀態(tài)。
[0018]本發(fā)明所述的掘進(jìn)式機(jī)器人控制電路，可以精確控制機(jī)器人各電機(jī)獨(dú)立運(yùn)動(dòng)，并能夠?qū)崿F(xiàn)對(duì)各電機(jī)驅(qū)動(dòng)電流的實(shí)時(shí)檢測(cè)，提高了機(jī)器人的穩(wěn)定性。同時(shí)，該控制電路提供了信息存儲(chǔ)功能，操作者根據(jù)實(shí)際情況確定好任務(wù)要求后，機(jī)器人控制電路即可自主完成該任務(wù)，實(shí)現(xiàn)半自主式控制。
【主權(quán)項(xiàng)】
1.一種掘進(jìn)式機(jī)器人控制電路，其特征在于:包括MCU控制電路、上位機(jī)存儲(chǔ)和控制電路、FPGA運(yùn)動(dòng)控制電路、關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路、掘進(jìn)電機(jī)驅(qū)動(dòng)電路、電流檢測(cè)電路、信號(hào)處理電路、關(guān)節(jié)電機(jī)、關(guān)節(jié)編碼器和掘進(jìn)電機(jī)；所述MCU控制電路為主控制電路，分別與所述上位機(jī)存儲(chǔ)和控制電路和FPGA運(yùn)動(dòng)控制電路雙向通信連接；所述FPGA運(yùn)動(dòng)控制電路與所述關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路和關(guān)節(jié)編碼器相連，讀取關(guān)節(jié)編碼器信號(hào)并向關(guān)節(jié)驅(qū)動(dòng)電路發(fā)出驅(qū)動(dòng)脈沖；所述關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路與所述關(guān)節(jié)電機(jī)相連，控制關(guān)節(jié)電機(jī)工作；所述掘進(jìn)電機(jī)驅(qū)動(dòng)電路的輸入端與所述MCU控制電路相連，接收驅(qū)動(dòng)信號(hào)，其輸出端與所述掘進(jìn)電機(jī)相連，控制掘進(jìn)電機(jī)工作；所述電流檢測(cè)電路的輸入端分別與所述關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路和所述掘進(jìn)電機(jī)驅(qū)動(dòng)電路相連，實(shí)時(shí)檢測(cè)驅(qū)動(dòng)電流的變化；所述信號(hào)處理電路的輸入端、輸出端分別與所述電流檢測(cè)電路、MCU控制電路相連，將得到的驅(qū)動(dòng)電流信號(hào)進(jìn)行處理并反饋給MCU控制電路。
2.根據(jù)權(quán)利要求1所述的掘進(jìn)式機(jī)器人控制電路，其特征在于:所述FPGA運(yùn)動(dòng)控制電路包括編碼器信號(hào)處理模塊、驅(qū)動(dòng)脈沖輸出模塊和串行口讀寫模塊，所述編碼器信號(hào)處理模塊對(duì)關(guān)節(jié)編碼器信號(hào)進(jìn)行處理，再通過(guò)所述串行口讀寫模塊將數(shù)據(jù)發(fā)送至所述MCU控制電路，所述串行口讀寫模塊還負(fù)責(zé)接收所述MCU控制電路發(fā)來(lái)的控制指令來(lái)控制所述驅(qū)動(dòng)脈沖輸出模塊輸出所述驅(qū)動(dòng)脈沖。
3.根據(jù)權(quán)利要求1所述的掘進(jìn)式機(jī)器人控制電路，其特征在于:所述信號(hào)處理電路包括信號(hào)放大電路、AD轉(zhuǎn)換電路和光電隔離電路，所述信號(hào)放大電路將所述電流檢測(cè)電路得到的驅(qū)動(dòng)電流信號(hào)放大，提高分辨率，所述AD轉(zhuǎn)換電路將放大后的模擬信號(hào)轉(zhuǎn)換成數(shù)字信號(hào)，最后經(jīng)過(guò)所述的光電隔離電路傳給所述的MCU控制電路。
【專利摘要】本發(fā)明公開一種掘進(jìn)式機(jī)器人控制電路，包括MCU控制電路、上位機(jī)存儲(chǔ)和控制電路、FPGA運(yùn)動(dòng)控制電路、關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路、掘進(jìn)電機(jī)驅(qū)動(dòng)電路、關(guān)節(jié)電機(jī)、關(guān)節(jié)編碼器、掘進(jìn)電機(jī)、電流檢測(cè)電路和信號(hào)處理電路；MCU控制電路與上位機(jī)存儲(chǔ)和控制電路、FPGA運(yùn)動(dòng)控制電路、掘進(jìn)電機(jī)驅(qū)動(dòng)電路、信號(hào)處理電路相連，F(xiàn)PGA運(yùn)動(dòng)控制電路還連接關(guān)節(jié)編碼器和關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路，電流檢測(cè)電路與關(guān)節(jié)電機(jī)驅(qū)動(dòng)電路、掘進(jìn)電機(jī)驅(qū)動(dòng)電路及信號(hào)處理電路相連，關(guān)節(jié)電機(jī)和掘進(jìn)電機(jī)分別連接相應(yīng)的驅(qū)動(dòng)電路。本發(fā)明可以控制機(jī)器人各電機(jī)獨(dú)立運(yùn)動(dòng)，對(duì)電流進(jìn)行實(shí)時(shí)檢測(cè)，提供了即時(shí)信息存儲(chǔ)功能，可實(shí)現(xiàn)半自主式控制，提高了機(jī)器人的工作效率，穩(wěn)定性和安全性。
【IPC分類】G05D1-02, G05B19-042
【公開號(hào)】CN104615136
【申請(qǐng)?zhí)枴緾N201510005543
【發(fā)明人】宋愛國(guó), 李松, 張軍, 文辭
【申請(qǐng)人】東南大學(xué)
【公開日】2015年5月13日
【申請(qǐng)日】2015年1月7日