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