專利名稱：基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，涉及先進(jìn)機(jī)器人制造技術(shù)領(lǐng)
域，是在高壓輸電線路線上作業(yè)中使用的巡檢系統(tǒng)的自動(dòng)控制系統(tǒng)。利用分布式硬件結(jié)構(gòu) 組成整個(gè)機(jī)器人的控制系統(tǒng)，形成了適用于高壓輸電線路線上巡檢、除冰等一系列作業(yè)的 先進(jìn)機(jī)器人控制系統(tǒng)。
背景技術(shù)：
高壓輸電線路作為電力輸送的主要方式，是國(guó)民經(jīng)濟(jì)的大動(dòng)脈，其安全可靠的運(yùn) 行是社會(huì)生產(chǎn)和人民生活的重要保障。但是，輸電線路長(zhǎng)期暴露在日曬、雨淋、大風(fēng)等惡劣 氣候條件下，極易發(fā)生斷股、銹蝕、脫線等損傷，在某些天氣條件下，如雨雪冰凍等，甚至可 能由于線路結(jié)冰太厚造成輸電線路斷裂，直接影響電力的輸送。因此，電力公司需要經(jīng)常對(duì) 輸電線路及其設(shè)備進(jìn)行檢查，發(fā)現(xiàn)并修復(fù)缺陷，清除結(jié)冰，以保證可靠的電力輸送。
目前，這些工作基本靠人工來(lái)完成，這樣工作效率低下，且工作環(huán)境險(xiǎn)惡，很難保 證工人的人身安全。隨著機(jī)器人技術(shù)，人工智能和現(xiàn)代機(jī)械設(shè)計(jì)的不斷發(fā)展，設(shè)計(jì)一種可以 代替人進(jìn)行輸電線路巡檢和除冰的自動(dòng)化裝置有了可能。巡線機(jī)器人就是以此為出發(fā)點(diǎn)而 設(shè)計(jì)的一款自動(dòng)化裝置。通過(guò)加裝不同的功能模塊，巡線機(jī)器人可以代替人工完成線路巡 檢和除冰等工作，從而減輕工人勞動(dòng)強(qiáng)度，降低工作風(fēng)險(xiǎn)，提高工作效率，保證輸電線路穩(wěn) 定可靠的運(yùn)行。目前，國(guó)際上巡線機(jī)器人研究現(xiàn)狀如下 日本Tokyo Electric Power公司的Sawada等人研制了用于檢測(cè)光纖復(fù)合架空 線路的自主移動(dòng)機(jī)器人，實(shí)際運(yùn)行時(shí)安裝在地線上，能跨越障礙物。當(dāng)它碰到障礙物時(shí)，打 開自身攜帶的弧形臂，臂的兩端握住障礙物的兩側(cè)，構(gòu)成一個(gè)導(dǎo)軌，本體順著導(dǎo)軌滑過(guò)障礙 物?？缭竭^(guò)障礙物后，自動(dòng)將弧形臂折疊起來(lái)，收在本體的下方。機(jī)器人裝有自行研制的驅(qū) 動(dòng)設(shè)備，具有自驅(qū)動(dòng)能力。 日本法政大學(xué)的Hideo Nakamura等開發(fā)了電氣列車饋電電纜巡線機(jī)器人，采用多 關(guān)節(jié)小車結(jié)構(gòu)和"頭部決策，尾部跟隨"的仿生控制體系，能跨越分支線、絕緣子等障礙物。 機(jī)器人由六對(duì)左右對(duì)稱、相互聯(lián)結(jié)的小車組成，每個(gè)單體小車有兩個(gè)電機(jī)，左右小車采用磁 鎖系統(tǒng)聯(lián)結(jié)。當(dāng)機(jī)器人遇到障礙物時(shí)，每對(duì)小車順次將磁鎖打開，機(jī)器人再改變兩側(cè)旋轉(zhuǎn) 關(guān)節(jié)的關(guān)節(jié)角，使左右小車分開；小車依次通過(guò)障礙物后，控制兩側(cè)旋轉(zhuǎn)關(guān)節(jié)使左右小車合 攏，磁鎖再次鎖緊，機(jī)器人恢復(fù)正常行走狀態(tài)。 日本靜岡大學(xué)的Tsu jimura等人開發(fā)了一種架空通訊線纜巡線機(jī)器人。機(jī)器人設(shè) 計(jì)成四臂仿生式結(jié)構(gòu)，四只手臂分為前后兩組，各自鉸接在一起，分別固定在前后兩個(gè)盤型 連接器上，并通過(guò)皮帶傳動(dòng)與電機(jī)相連。在運(yùn)行時(shí)，由電機(jī)帶動(dòng)盤型連接器轉(zhuǎn)動(dòng)，每組手臂 由于相互鉸接的作用，其末端的就會(huì)產(chǎn)生弧形運(yùn)動(dòng)，在線纜上形成四臂交替行走的動(dòng)作，從 而實(shí)現(xiàn)機(jī)器人行走和障礙跨越。但這種機(jī)器人只能跨越等間距排列的障礙，對(duì)于不規(guī)則障 礙和桿塔是無(wú)法跨越的。 美國(guó)TRC公司研制了一臺(tái)懸臂自治巡線機(jī)器人，它采用三臂懸掛結(jié)構(gòu)，能沿架空
4損耗、絕緣子、結(jié)合點(diǎn)、壓接頭等視覺檢查任務(wù)，對(duì)探測(cè)到的線路 故障數(shù)據(jù)預(yù)處理后，傳送給地面人員。當(dāng)機(jī)器人遇到桿塔時(shí)，利用手臂采用仿人攀援的方法 從側(cè)面越過(guò)桿塔。 武漢大學(xué)吳功平等研制了 220kV單分裂導(dǎo)線雙臂巡線機(jī)器人。該機(jī)器人結(jié)構(gòu)緊 湊，重量輕，采用絲杠連接兩只手臂與本體，越障時(shí)通過(guò)絲杠聯(lián)動(dòng)實(shí)現(xiàn)兩只手臂的交互滑移 異位，再通過(guò)轉(zhuǎn)動(dòng)副越過(guò)障礙物。機(jī)器人具有較好的抱卡機(jī)構(gòu)，在一只手臂越障時(shí)，可利用 電力線的局部剛性，維持機(jī)器人本體的位姿不發(fā)生大的變化，進(jìn)而保證各種動(dòng)作的順利完 成。 中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所研制了沿500kV地線巡檢機(jī)器人，機(jī)器人采用小車 式結(jié)構(gòu)，可以在兩塔檔間自主行走，不能跨越桿塔障礙物。利用分布式專家系統(tǒng)構(gòu)建機(jī)器人 控制系統(tǒng)，實(shí)現(xiàn)機(jī)器人運(yùn)行的智能化。利用攜帶的攝像機(jī)或紅外熱像儀等檢測(cè)裝置，可以檢 測(cè)輸電線、防震錘、絕緣子和桿塔等輸電設(shè)備的損傷情況。實(shí)現(xiàn)了機(jī)器人和地面基站遠(yuǎn)程通 訊，基站對(duì)機(jī)器人運(yùn)行狀態(tài)的遠(yuǎn)程控制。 本發(fā)明的目的是在已有的先進(jìn)機(jī)器人控制技術(shù)基礎(chǔ)上提供一種基于現(xiàn)場(chǎng)總線的
雙臂巡線機(jī)器人控制系統(tǒng)，使用該控制系統(tǒng)能夠安全可靠的控制巡線機(jī)器人的各種運(yùn)動(dòng)，
并通過(guò)在機(jī)器人上掛載相應(yīng)的設(shè)備，可以實(shí)現(xiàn)輸電線路的自動(dòng)巡檢和除冰等作業(yè)，減輕工
人勞動(dòng)強(qiáng)度，降低工作風(fēng)險(xiǎn)，提高工作效率，保證輸電線路的安全可靠運(yùn)行。 為達(dá)到上述目的，本發(fā)明的技術(shù)解決方案是 —種基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，包括機(jī)器人控制主機(jī)、運(yùn)動(dòng)控制
單元、傳感器單元、系統(tǒng)狀態(tài)監(jiān)測(cè)單元、檢測(cè)設(shè)備控制單元和地面基站，機(jī)器人控制主機(jī)包
括嵌入式PC104計(jì)算機(jī)、圖像采集卡、無(wú)線網(wǎng)卡，是機(jī)器人控制系統(tǒng)的核心部分； 機(jī)器人控制主機(jī)和各單元分別經(jīng)現(xiàn)場(chǎng)總線電連接，并通過(guò)現(xiàn)場(chǎng)總線相互進(jìn)行實(shí)時(shí)
通訊；機(jī)器人各單元都有獨(dú)立的微處理器，能夠?qū)Ρ締卧畔⑦M(jìn)行預(yù)處理，并通過(guò)現(xiàn)場(chǎng)總線
將預(yù)處理結(jié)果發(fā)送給機(jī)器人控制主機(jī)，控制主機(jī)也通過(guò)現(xiàn)場(chǎng)總線實(shí)現(xiàn)對(duì)各個(gè)單元的實(shí)時(shí)訪
問(wèn)與控制； 其在運(yùn)動(dòng)控制單元，傳感器單元、系統(tǒng)狀態(tài)監(jiān)控單元和檢測(cè)設(shè)備控制單元中包括 ARM (Advanced Rise Machines,高級(jí)精簡(jiǎn)指令機(jī))控制板、FPGA (Field Programmable Gate Array,現(xiàn)場(chǎng)可編程門陣列)控制板禾口 CPLD (Complex Programmable Logic Device,復(fù)雜可 編程邏輯器件)伺服驅(qū)動(dòng)板； ARM控制板包括ARM9芯片、FLASH存儲(chǔ)器、時(shí)鐘電路、現(xiàn)場(chǎng)總線接口 、PC104總線接 口和RS232接口 ；其中，ARM9芯片分別與時(shí)鐘電路、PC104總線、RS232總線、現(xiàn)場(chǎng)總線控制 器電連接，現(xiàn)場(chǎng)總線控制器與現(xiàn)場(chǎng)總線電連接；并經(jīng)地址總線、數(shù)據(jù)總線與FLASH存儲(chǔ)器電 連接； FPFA控制板包括現(xiàn)場(chǎng)可編程邏輯器件FPGA、 FPGA配置電路、時(shí)鐘電路、模式選擇 電路、電源模塊和PC104接口 ；其中，現(xiàn)場(chǎng)可編程邏輯器件FPGA分別與時(shí)鐘電路、PC104總 線、模式選擇電路、電源模塊、FPGA配置電路電連接；FPGA配置電路包括JTAG電路和PR0M， 其中PROM和現(xiàn)場(chǎng)可編程邏輯器件FPGA串行連接在JTAG電路上；現(xiàn)場(chǎng)可編程邏輯器件FPGA

發(fā)明內(nèi)容
5并經(jīng)電機(jī)速度接口和電機(jī)方向接口與CPLD伺服驅(qū)動(dòng)板9電連接； CPLD伺服驅(qū)動(dòng)板包括CPLD芯片、時(shí)鐘電路、H橋控制電路、光耦隔離電路及過(guò)流保 護(hù)電路。其中，CPLD芯片分別與時(shí)鐘電路、現(xiàn)場(chǎng)可編程邏輯器件FPGA、 JTAG(Joined Test Action Group,聯(lián)合測(cè)試行動(dòng)組)電路、H橋控制電路、光耦隔離電路電連接；H橋控制電路 與直流電機(jī)電連接；光耦隔離電路與比較器輸出端電連接，比較器兩輸入端，一接設(shè)定值信 號(hào)，另一接驅(qū)動(dòng)器電流； 傳感器單元包括ARM控制板和多種傳感器，ARM控制板完成對(duì)各傳感器信號(hào)的采 集與預(yù)處理，并通過(guò)現(xiàn)場(chǎng)總線實(shí)現(xiàn)與運(yùn)動(dòng)控制單元的實(shí)時(shí)通信；傳感器主要有超聲傳感器、 紅外傳感器、霍爾傳感器、觸點(diǎn)開關(guān)； 系統(tǒng)狀態(tài)監(jiān)控單元和檢測(cè)設(shè)備控制單元都有ARM控制板及檢測(cè)設(shè)備，ARM控制板 用來(lái)采集系統(tǒng)及檢測(cè)設(shè)備的狀態(tài)并通過(guò)現(xiàn)場(chǎng)總線實(shí)現(xiàn)與控制主機(jī)的實(shí)時(shí)通信；檢測(cè)設(shè)備包 括機(jī)器人電源檢測(cè)單元、系統(tǒng)運(yùn)行檢測(cè)單元和設(shè)備檢測(cè)單元； 傳感器單元采集輸電線路信息并經(jīng)過(guò)本單元微處理器進(jìn)行預(yù)處理后，通過(guò)現(xiàn)場(chǎng)總 線傳送到機(jī)器人控制主機(jī)，控制主機(jī)綜合傳感器信息、系統(tǒng)狀態(tài)和檢測(cè)設(shè)備信息，做出動(dòng)作 規(guī)劃，向機(jī)器人的運(yùn)動(dòng)控制單元發(fā)送控制命令，控制各關(guān)節(jié)電機(jī)的轉(zhuǎn)動(dòng)，實(shí)現(xiàn)機(jī)器人在輸電 線路上越障和行走，達(dá)到對(duì)高壓輸電線路巡檢的目的； 機(jī)器人控制主機(jī)通過(guò)無(wú)線網(wǎng)絡(luò)與地面基站進(jìn)行實(shí)時(shí)通訊，地面基站操控人員依據(jù) 信息做遠(yuǎn)程處理，給出指令，通過(guò)控制主機(jī)調(diào)動(dòng)各單元，實(shí)現(xiàn)機(jī)器人在輸電線路上越障和行 走，完成高壓輸電線路巡檢任務(wù)。 所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其所述控制系統(tǒng)采用分布式的 硬件結(jié)構(gòu)，各控制單元既相互獨(dú)立又通過(guò)現(xiàn)場(chǎng)總線聯(lián)系在一起，各單元信息通過(guò)現(xiàn)場(chǎng)總線 發(fā)送到機(jī)器人控制主機(jī)，控制主機(jī)通過(guò)現(xiàn)場(chǎng)總線實(shí)現(xiàn)對(duì)各單元的實(shí)時(shí)訪問(wèn)與控制。
所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其所述機(jī)器人控制主機(jī)，作為 機(jī)器人控制器的核心部分，主要完成行為層次上的越障規(guī)劃、圖像采集與傳輸、障礙信息融 合與識(shí)別、各單元之間的組織管理等。所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其 所述運(yùn)動(dòng)控制單元中，ARM控制板完成越障動(dòng)作規(guī)劃和各關(guān)節(jié)電機(jī)的協(xié)調(diào)運(yùn)動(dòng)，F(xiàn)PGA控制 板和CPLD伺服驅(qū)動(dòng)板完成電機(jī)的伺服控制和過(guò)流、過(guò)載保護(hù)。 所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其所述傳感器單元，包括超聲 傳感器、紅外傳感器、霍爾傳感器、觸點(diǎn)開關(guān)和負(fù)責(zé)信息融合的ARM處理器；其中，超聲傳感 器陣列探測(cè)電力線上方和下方的障礙物，紅外傳感器陣列檢測(cè)電力線與手爪的相對(duì)位置， 霍爾傳感器檢測(cè)各運(yùn)動(dòng)關(guān)節(jié)的機(jī)械限位，觸點(diǎn)開關(guān)用于其他方式障礙物檢測(cè)失敗后的障礙 備用檢測(cè)手段，所有這些傳感器信息經(jīng)過(guò)ARM處理器融合后得到初步的外部環(huán)境信息。
所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其所述系統(tǒng)狀態(tài)監(jiān)控單元能夠 實(shí)時(shí)監(jiān)測(cè)PC104主機(jī)的工作狀態(tài)， 一旦發(fā)現(xiàn)PC104主機(jī)死機(jī)或重啟，立即向運(yùn)動(dòng)控制器發(fā)出 停止運(yùn)動(dòng)命令，避免因PC104主機(jī)故障而帶來(lái)的危險(xiǎn)。 所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其所述檢測(cè)設(shè)備控制單元利用 ARM9處理器采集各檢測(cè)設(shè)備的工作狀態(tài)并進(jìn)行實(shí)時(shí)分析，將預(yù)處理的結(jié)果通過(guò)現(xiàn)場(chǎng)總線傳 送給機(jī)器人控制主機(jī)，控制主機(jī)綜合各中信息通過(guò)檢測(cè)設(shè)備控制單元的ARM9處理器對(duì)各 檢測(cè)設(shè)備進(jìn)行實(shí)時(shí)控制。
—種所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)的控制流程，其包括步驟
C)開啟機(jī)器人控制主機(jī)，經(jīng)現(xiàn)場(chǎng)總線對(duì)各單元初始化； D)檢測(cè)設(shè)備控制單元采集系統(tǒng)、設(shè)備信息，傳感器單元采集傳感器信息，處理后， 經(jīng)現(xiàn)場(chǎng)總線返回控制主機(jī)； C)控制主機(jī)發(fā)出運(yùn)動(dòng)控制指令，綜合各單元返回的信息ARM控制板作動(dòng)作規(guī)劃， 現(xiàn)場(chǎng)可編程邏輯器件FPGA產(chǎn)生直流電機(jī)的速度、方向控制信號(hào)；
D)直流電機(jī)的速度、方向控制信號(hào)傳遞給CPLD伺服驅(qū)動(dòng)板； E)經(jīng)過(guò)CPLD芯片的內(nèi)部邏輯轉(zhuǎn)換成直接控制電機(jī)驅(qū)動(dòng)器的速度和方向信號(hào)，驅(qū) 動(dòng)直流電機(jī)轉(zhuǎn)動(dòng)； F)若直流電機(jī)轉(zhuǎn)動(dòng)到位，直流電機(jī)停止轉(zhuǎn)動(dòng)，若直流電機(jī)轉(zhuǎn)動(dòng)不到位，返回E)步， 循環(huán)動(dòng)作，直到到位為止。 所述的的控制流程，其所述E)步中，同時(shí)，CPLD芯片經(jīng)H橋控制電路及過(guò)流保護(hù) 電路采集直流電機(jī)電流信號(hào)，將直流電機(jī)電流信號(hào)與比較器提供的設(shè)定值信號(hào)比較，并由 光耦隔離電路傳送到CPLD芯片中 A)若采集到的電流值沒有超過(guò)設(shè)定值，驅(qū)動(dòng)直流電機(jī)轉(zhuǎn)動(dòng)； B)若采集到的電流值超過(guò)設(shè)定值，則說(shuō)明直流電機(jī)過(guò)流或過(guò)載，信號(hào)馬上傳送到
CPLD芯片，通過(guò)CPLD芯片的內(nèi)部邏輯發(fā)出停機(jī)命令，使直流電機(jī)停止轉(zhuǎn)動(dòng)。 本發(fā)明提出的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)的主要優(yōu)點(diǎn)如下采用分
布式硬件體系結(jié)構(gòu)，通過(guò)現(xiàn)場(chǎng)總線系統(tǒng)將機(jī)器人控制主機(jī)和各控制單元聯(lián)系在一起，實(shí)現(xiàn)
整個(gè)控制系統(tǒng)的實(shí)時(shí)通訊與控制；采用可編程邏輯器件實(shí)現(xiàn)對(duì)各關(guān)節(jié)電機(jī)的速度與開停控
制及過(guò)流過(guò)載控制，保證系統(tǒng)的實(shí)時(shí)性與準(zhǔn)確性；采用超聲、紅外、霍爾元件和攝像裝置實(shí)
現(xiàn)對(duì)輸電線路及線上裝置的全面檢測(cè)，保證機(jī)器人自主、準(zhǔn)確地在線上行走并跨越障礙；采
用系統(tǒng)狀態(tài)監(jiān)控單元和檢測(cè)設(shè)備控制單元實(shí)現(xiàn)對(duì)整個(gè)機(jī)器人控制系統(tǒng)及檢測(cè)設(shè)備總體運(yùn)
行情況的實(shí)時(shí)監(jiān)控監(jiān)測(cè)，保證系統(tǒng)和各檢測(cè)設(shè)備的正常運(yùn)行，保證系統(tǒng)運(yùn)行安全，提高了巡
檢效率。


圖1為本發(fā)明基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)的結(jié)構(gòu)框圖；
圖2為本發(fā)明基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)中運(yùn)動(dòng)控制單元原理圖；
圖3為本發(fā)明基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)中ARM控制板的電路原理圖；
圖4為本發(fā)明基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)中FPGA控制板的電路原 理圖； 圖5為本發(fā)明基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)中CPLD伺服驅(qū)動(dòng)板的電 路原理圖； 圖6為本發(fā)明基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)的軟件結(jié)構(gòu)圖；
圖7為本發(fā)明基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)的控制流程圖。
具體實(shí)施例方式
本發(fā)明一種基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，是基于嵌入式微處理器和工控機(jī)的，適用于高壓輸電線路巡檢作業(yè)，由機(jī)器人控制主機(jī)、運(yùn)動(dòng)控制單元、傳感器單元、 系統(tǒng)狀態(tài)監(jiān)測(cè)單元、檢測(cè)設(shè)備控制單元組成。 其中，機(jī)器人控制主機(jī)由嵌入式PC 104計(jì)算機(jī)、圖像采集卡、無(wú)線網(wǎng)卡構(gòu)成，是機(jī) 器人控制器的核心部分；運(yùn)動(dòng)控制單元由ARM控制板、FPGA控制板和CPLD伺服驅(qū)動(dòng)板構(gòu) 成，ARM控制板主要包括ARM9微處理器、現(xiàn)場(chǎng)總線接口 、 PC104接口和RS232接口 ， FPFA控 制板主要包括FPGA芯片及ROM配置芯片和PC104接口 ， CPLD伺服驅(qū)動(dòng)板主要包括CPLD芯 片及時(shí)鐘芯片、H橋控制電路及過(guò)流保護(hù)電路；傳感器單元由ARM控制板和各類傳感器組 成，ARM控制板主要包括ARM9微處理器、現(xiàn)場(chǎng)總線接口 ，傳感器主要有超聲傳感器、紅外傳 感器、霍爾傳感器、觸點(diǎn)開關(guān)等；系統(tǒng)狀態(tài)監(jiān)控單元和檢測(cè)設(shè)備控制單元都由ARM控制板構(gòu) 成，ARM控制板上主要包括ARM9微處理器和現(xiàn)場(chǎng)總線接口 。 圖1為該機(jī)器人控制系統(tǒng)的結(jié)構(gòu)框圖?？刂葡到y(tǒng)采用分布式硬件結(jié)構(gòu)，控制主機(jī) 通過(guò)現(xiàn)場(chǎng)總線和各控制單元進(jìn)行數(shù)據(jù)傳輸；控制主機(jī)包括PC104主機(jī)、圖像采集卡和無(wú)線 網(wǎng)卡等，圖像采集卡通過(guò)PC104總線與PC104主機(jī)連接，無(wú)線網(wǎng)卡通過(guò)PC104主機(jī)上的無(wú) 線網(wǎng)卡插槽與其連接；運(yùn)動(dòng)控制單元包括ARM控制板、FPGA控制板和CPLD伺服驅(qū)動(dòng)板等， FPGA控制板和CPLD伺服驅(qū)動(dòng)板通過(guò)PC104插針與ARM控制板連接，F(xiàn)PGA控制板信號(hào)通過(guò) P麗和DIR接口與CPLD伺服驅(qū)動(dòng)板進(jìn)行連接；傳感器單元包括ARM控制板和各類傳感器， 各類傳感器信號(hào)通過(guò)A/D轉(zhuǎn)換，連接到ARM控制器的I/O 口 ；系統(tǒng)狀態(tài)監(jiān)控單元和檢測(cè)設(shè)備 控制單元主要由ARM控制板和相關(guān)傳感器組成，其傳感器信號(hào)通過(guò)A/D轉(zhuǎn)換連接到ARM控 制器的I/O 口 ；運(yùn)動(dòng)控制單元、傳感器單元、系統(tǒng)狀態(tài)監(jiān)控單元和檢測(cè)設(shè)備控制單元通過(guò)各 自的現(xiàn)場(chǎng)總線接口與控制主機(jī)連接，進(jìn)行實(shí)時(shí)通信。 其中，PC104、 ARM9微處理器、圖像采集卡、無(wú)線網(wǎng)卡、FPGA和CPLD芯片都是從市 場(chǎng)購(gòu)買的。 圖1的工作原理如下傳感器單元中的紅外、超聲等傳感器陣列及攝像裝置探測(cè) 線路損傷及障礙物情況，將傳感器信息及圖像經(jīng)過(guò)本單元ARM9微處理器進(jìn)行預(yù)處理后(主 要是對(duì)傳感器陣列所探測(cè)到的障礙物信息進(jìn)行匯總并簡(jiǎn)單處理)，通過(guò)現(xiàn)場(chǎng)總線傳送給機(jī) 器人控制主機(jī)，作為控制主機(jī)進(jìn)行障礙信息融合與識(shí)別、行為層次上的越障規(guī)劃的依據(jù)；運(yùn) 動(dòng)控制單元通過(guò)現(xiàn)場(chǎng)總線接收到控制主機(jī)傳來(lái)的關(guān)于關(guān)節(jié)電機(jī)運(yùn)動(dòng)速度和方向的信息，首 先進(jìn)入運(yùn)動(dòng)控制單元的ARM控制板，經(jīng)過(guò)地址選擇器選擇需要控制的電機(jī)，同時(shí)將控制信 息通過(guò)PC104總線接口發(fā)送給相應(yīng)的FPGA控制板，由FPGA控制板上的程序產(chǎn)生需要的P麗 控制信息和電機(jī)轉(zhuǎn)向信息，之后P麗信號(hào)和轉(zhuǎn)向信號(hào)經(jīng)過(guò)CPLD伺服驅(qū)動(dòng)板的組合邏輯傳送 到H橋控制電路對(duì)相關(guān)電機(jī)進(jìn)行啟停及速度控制；系統(tǒng)狀態(tài)監(jiān)控單元實(shí)時(shí)監(jiān)測(cè)控制主機(jī)的 工作狀態(tài)信息，并由單元內(nèi)的ARM控制板對(duì)信息進(jìn)行預(yù)處理， 一旦發(fā)現(xiàn)計(jì)算機(jī)死機(jī)或重啟， ARM控制板立即將信息通過(guò)現(xiàn)場(chǎng)總線發(fā)送到控制主機(jī)，由控制主機(jī)向運(yùn)動(dòng)控制器發(fā)出停止 運(yùn)動(dòng)命令；檢測(cè)設(shè)備控制單元主要對(duì)機(jī)器人攜帶的各種檢測(cè)設(shè)備進(jìn)行實(shí)時(shí)監(jiān)控，并將檢測(cè) 結(jié)果通過(guò)現(xiàn)場(chǎng)總線傳送到控制主機(jī)；機(jī)器人控制主機(jī)匯總各單元信息，完成行為層次上的 越障規(guī)劃、圖像采集與傳輸、障礙信息融合與識(shí)別、各單元之間的組織管理等。
圖2為基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)中運(yùn)動(dòng)控制單元結(jié)構(gòu)圖。運(yùn)動(dòng)控 制單元主要包括ARM控制板、FPGA控制板、CPLD伺服驅(qū)動(dòng)板、H橋電路和過(guò)流過(guò)載保護(hù)電 路。在單元外，ARM控制板通過(guò)現(xiàn)場(chǎng)總線實(shí)現(xiàn)和控制主機(jī)的通訊，在單元內(nèi)，ARM控制板通過(guò)PC104接口實(shí)現(xiàn)與各控制板的通訊ARM控制板產(chǎn)生的電機(jī)控制信號(hào)通過(guò)PC104總線傳 送給FPGA控制板，通過(guò)FPGA內(nèi)部程序產(chǎn)生相應(yīng)電機(jī)的速度信號(hào)P麗和電機(jī)方向信號(hào)DIR， P麗和DIR信號(hào)通過(guò)P麗和DIR接口傳送給CPLD伺服驅(qū)動(dòng)板，經(jīng)過(guò)CPLD芯片的內(nèi)部邏輯產(chǎn) 生H橋電路的驅(qū)動(dòng)信號(hào)，控制H橋的通斷。同時(shí)H橋臂上的電流信號(hào)經(jīng)過(guò)過(guò)流過(guò)載保護(hù)電 路的處理反饋給CPLD伺服驅(qū)動(dòng)板。 圖2的工作原理如下ARM控制板作為運(yùn)動(dòng)控制單元的主控板，通過(guò)現(xiàn)場(chǎng)總線實(shí) 現(xiàn)和控制主機(jī)的實(shí)時(shí)通訊，通過(guò)PC104接口實(shí)現(xiàn)與FPGA控制板的實(shí)時(shí)通訊，它負(fù)責(zé)將上位 機(jī)傳來(lái)的控制信息譯碼發(fā)送給相應(yīng)的寄存器，實(shí)現(xiàn)對(duì)不同電機(jī)的準(zhǔn)確控制；機(jī)器人控制主 機(jī)通過(guò)信息融合及各種現(xiàn)場(chǎng)控制命令，將具體關(guān)節(jié)電機(jī)的運(yùn)行參數(shù)通過(guò)現(xiàn)場(chǎng)總線傳送給運(yùn) 動(dòng)控制單元的ARM控制板，ARM控制板將參數(shù)進(jìn)行相應(yīng)格式化處理通過(guò)PC104總線傳送給 FPGA控制板，并將參數(shù)存儲(chǔ)在相應(yīng)寄存器中。在接收到ARM控制板傳來(lái)的信息后，F(xiàn)PGA控 制板根據(jù)相應(yīng)寄存器內(nèi)容，產(chǎn)生P麗波形，并通過(guò)PC104總線傳送給CPLD伺服驅(qū)動(dòng)板；CPLD 伺服驅(qū)動(dòng)板上的CPLD芯片根據(jù)P麗信號(hào)通過(guò)邏輯運(yùn)算得到電機(jī)轉(zhuǎn)向信號(hào)，并產(chǎn)生H橋電路 的導(dǎo)通和截止信號(hào)，實(shí)現(xiàn)對(duì)關(guān)節(jié)電機(jī)的速度和方向控制。 圖3為本發(fā)明的ARM控制板的電路原理圖。ARM控制板主要包括ARM9芯片1、FLASH 存儲(chǔ)器2、時(shí)鐘電路3、現(xiàn)場(chǎng)總線接口、PC104總線接口和RS232接口。其中，ARM9芯片l分 別與時(shí)鐘電路3、PC104總線5、RS232總線6、現(xiàn)場(chǎng)總線控制器7電連接，現(xiàn)場(chǎng)總線控制器7 與現(xiàn)場(chǎng)總線4電連接；并經(jīng)地址總線、數(shù)據(jù)總線與FLASH存儲(chǔ)器2電連接。
圖3的工作原理是通過(guò)RS232接口對(duì)ARM9芯片1進(jìn)行編程及通信。時(shí)鐘電路3 為ARM9芯片1提供基頻時(shí)鐘，通過(guò)ARM9芯片1內(nèi)部的倍頻模塊產(chǎn)生芯片正常工作所需要 的時(shí)鐘頻率。系統(tǒng)運(yùn)行所需要的程序及數(shù)據(jù)信息存儲(chǔ)在FLASH存儲(chǔ)器2中。ARM9芯片1通 過(guò)PC104總線5實(shí)現(xiàn)其與機(jī)器人控制主機(jī)、FPGA控制板、CPLD伺服驅(qū)動(dòng)板之間的通信。通 過(guò)現(xiàn)場(chǎng)總線4實(shí)現(xiàn)各ARM控制板之間的通信，現(xiàn)場(chǎng)總線4由現(xiàn)場(chǎng)總線控制器7掌控。
圖4為FPGA控制板的電路原理圖。包括現(xiàn)場(chǎng)可編程邏輯器件FPGA8、FPGA配置電 路、時(shí)鐘電路3、模式選擇電路12、電源模塊13和PC104接口。其中，現(xiàn)場(chǎng)可編程邏輯器件 FPGA8分別與時(shí)鐘電路3、PC104總線5、模式選擇電路12、電源模塊13、FPGA配置電路電連 接，F(xiàn)PGA配置電路包括JTAG電路11和PR0M10，其中PR0M10和現(xiàn)場(chǎng)可編程邏輯器件FPGA8 串行連接在JTAG電路11上；現(xiàn)場(chǎng)可編程邏輯器件FPGA8并經(jīng)電機(jī)速度接口 (P麗)和電機(jī) 方向接口 (DIR)與CPLD芯片9電連接。 圖4的工作原理是電源模塊13為L(zhǎng)DO電源，為FPGA控制板的正常工作提供合適 的電平。時(shí)鐘電路3為現(xiàn)場(chǎng)可編程邏輯器件FPGA8提供高頻工作時(shí)鐘?，F(xiàn)場(chǎng)可編程邏輯 器件FPGA8的配置電路包括JTAG電路11和PR0M10，其中PR0M10和現(xiàn)場(chǎng)可編程邏輯器件 FPGA8串行連接在JTAG電路11上，在配置FPGA時(shí)通過(guò)模式選擇電路12將現(xiàn)場(chǎng)可編程邏輯 器件FPGA8的配置模式選為邊緣掃描模式，通過(guò)JTAG電路11實(shí)現(xiàn)對(duì)PR0M10和現(xiàn)場(chǎng)可編程 邏輯器件FPGA8的配置。在斷電后，由于現(xiàn)場(chǎng)可編程邏輯器件FPGA8是易失性器件，原先的 配置將不復(fù)存在，而PR0M10是非易失性器件，此時(shí)將現(xiàn)場(chǎng)可編程邏輯器件FPGA8的配置模 式選擇為主串模式，通過(guò)PR0M10實(shí)現(xiàn)對(duì)現(xiàn)場(chǎng)可編程邏輯器件FPGA8的配置。從ARM控制板 經(jīng)PC104總線5傳來(lái)的控制信息達(dá)到現(xiàn)場(chǎng)可編程邏輯器件FPGA8后，經(jīng)過(guò)內(nèi)部時(shí)序邏輯和 組合邏輯，現(xiàn)場(chǎng)可編程邏輯器件FPGA8產(chǎn)生P麗波形和電機(jī)運(yùn)行方向控制信號(hào)DIR，并通過(guò)
9PC104總線5傳送給CPLD伺服驅(qū)動(dòng)板。 圖5為CPLD伺服驅(qū)動(dòng)板的電路原理圖。包括CPLD芯片9、時(shí)鐘電路3、 H橋控制 電路14、光耦隔離電路15及過(guò)流保護(hù)電路。其中，CPLD芯片9分別與時(shí)鐘電路3、現(xiàn)場(chǎng)可 編程邏輯器件FPGA8、JTAG電路11、H橋控制電路14、光耦隔離電路15電連接；H橋控制電 路14與直流電機(jī)17電連接；光耦隔離電路15與比較器16輸出端電連接，比較器16兩輸 入端，一接設(shè)定值信號(hào)18，另一接驅(qū)動(dòng)器電流19。 圖5的工作原理是通過(guò)時(shí)鐘電路3為CPLD芯片9提供可靠的工作時(shí)鐘。由于CPLD 芯片9是非易失性器件，所以只需要通過(guò)JTAG電路11對(duì)其進(jìn)行一次性配置即可。從FPGA 控制板傳來(lái)的P麗和DIR信號(hào)經(jīng)過(guò)CPLD芯片9的內(nèi)部邏輯轉(zhuǎn)換成可以直接控制電機(jī)驅(qū)動(dòng) 器的速度和方向信號(hào)，驅(qū)動(dòng)直流電機(jī)17轉(zhuǎn)動(dòng)。同時(shí)采集H橋制電路14橋臂上的電流，通過(guò) 與設(shè)定值信號(hào)18的電流值進(jìn)行比較，如果采集到的電流值超過(guò)設(shè)定值，則說(shuō)明直流電機(jī)17 過(guò)流或者過(guò)載，信號(hào)馬上傳送到CPLD芯片9，通過(guò)CPLD芯片9的內(nèi)部邏輯發(fā)出停機(jī)命令，達(dá) 到防止直流電機(jī)17過(guò)流過(guò)載的目的。 本發(fā)明的技術(shù)核心在于將分布式硬件體系結(jié)構(gòu)應(yīng)用到機(jī)器人運(yùn)動(dòng)控制系統(tǒng)中，并 通過(guò)現(xiàn)場(chǎng)總線技術(shù)將各硬件單元有效的聯(lián)系在一起，實(shí)現(xiàn)整個(gè)控制系統(tǒng)的實(shí)時(shí)高效通訊， 形成了適應(yīng)于高壓輸電線路線上作業(yè)的雙臂巡線機(jī)器人控制系統(tǒng)。 本發(fā)明采用雙臂式機(jī)械結(jié)構(gòu)、分布式硬件結(jié)構(gòu)和現(xiàn)場(chǎng)總線通訊系統(tǒng)，能夠?qū)崿F(xiàn)對(duì) 機(jī)器人各關(guān)節(jié)的有效實(shí)時(shí)控制，為高壓輸電線路上的作業(yè)提供了很好的操作平臺(tái)，通過(guò)在 巡線機(jī)器人本體上加裝巡檢模塊，可以實(shí)現(xiàn)對(duì)輸電線路破損、銹蝕、斷股的自動(dòng)檢測(cè)；在雨 雪冰凍天氣時(shí)，可以加裝除冰模塊，清除線路積冰，防止因積冰嚴(yán)重而導(dǎo)致輸電線被壓斷等 惡性事故的發(fā)生。從而有利于國(guó)家電力系統(tǒng)的安全運(yùn)行，保障人民生活和國(guó)家經(jīng)濟(jì)的順利 進(jìn)行。 本發(fā)明的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)的軟件結(jié)構(gòu)，如圖6所示。
本發(fā)明的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)的控制流程，如圖7所示，包 括步驟 E)開啟機(jī)器人控制主機(jī)，經(jīng)現(xiàn)場(chǎng)總線4對(duì)各單元初始化； F)檢測(cè)設(shè)備控制單元采集系統(tǒng)、設(shè)備信息，傳感器單元采集傳感器信息，處理后， 經(jīng)現(xiàn)場(chǎng)總線4返回控制主機(jī)； C)控制主機(jī)發(fā)出運(yùn)動(dòng)控制指令，綜合各單元返回的信息ARM控制板作動(dòng)作規(guī)劃，
現(xiàn)場(chǎng)可編程邏輯器件FPGA8產(chǎn)生直流電機(jī)17的速度、方向控制信號(hào)； D)直流電機(jī)17的速度、方向控制信號(hào)傳遞給CPLD伺服驅(qū)動(dòng)板； E)經(jīng)過(guò)CPLD芯片9的內(nèi)部邏輯轉(zhuǎn)換成直接控制電機(jī)驅(qū)動(dòng)器的速度和方向信號(hào)，驅(qū)
動(dòng)直流電機(jī)17轉(zhuǎn)動(dòng)； F)若直流電機(jī)17轉(zhuǎn)動(dòng)到位，直流電機(jī)17停止轉(zhuǎn)動(dòng)，若直流電機(jī)17轉(zhuǎn)動(dòng)不到位，返 回E)步，循環(huán)動(dòng)作，直到到位為止。 在E)步同時(shí)，CPLD芯片9經(jīng)H橋控制電路14及過(guò)流保護(hù)電路采集直流電機(jī)17電 流信號(hào)，將直流電機(jī)17電流信號(hào)與比較器16提供的設(shè)定值信號(hào)18比較，并由光耦隔離電 路傳送到CPLD芯片中； A)若采集到的電流值沒有超過(guò)設(shè)定值，驅(qū)動(dòng)直流電機(jī)17轉(zhuǎn)動(dòng)。
B)若采集到的電流值超過(guò)設(shè)定值，則說(shuō)明直流電機(jī)17過(guò)流或過(guò)載，信號(hào)馬上傳送 到CPLD芯片9，通過(guò)CPLD芯片9的內(nèi)部邏輯發(fā)出停機(jī)命令，使直流電機(jī)17停止轉(zhuǎn)動(dòng)，達(dá)到 防止直流電機(jī)17過(guò)流過(guò)載的目的。
權(quán)利要求
一種基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，包括機(jī)器人控制主機(jī)、運(yùn)動(dòng)控制單元、傳感器單元、系統(tǒng)狀態(tài)監(jiān)測(cè)單元、檢測(cè)設(shè)備控制單元和地面基站，機(jī)器人控制主機(jī)包括嵌入式PC104計(jì)算機(jī)、圖像采集卡、無(wú)線網(wǎng)卡，是機(jī)器人控制系統(tǒng)的核心部分；機(jī)器人控制主機(jī)和各單元分別經(jīng)現(xiàn)場(chǎng)總線電連接，并通過(guò)現(xiàn)場(chǎng)總線相互進(jìn)行實(shí)時(shí)通訊；機(jī)器人各單元都有獨(dú)立的微處理器，能夠?qū)Ρ締卧畔⑦M(jìn)行預(yù)處理，并通過(guò)現(xiàn)場(chǎng)總線將預(yù)處理結(jié)果發(fā)送給機(jī)器人控制主機(jī)，控制主機(jī)也通過(guò)現(xiàn)場(chǎng)總線實(shí)現(xiàn)對(duì)各個(gè)單元的實(shí)時(shí)訪問(wèn)與控制；其特征在于在運(yùn)動(dòng)控制單元，傳感器單元、系統(tǒng)狀態(tài)監(jiān)控單元和檢測(cè)設(shè)備控制單元中包括ARM/Advanced Risc Machines，高級(jí)精簡(jiǎn)指令機(jī)控制板、FPGA/Field Programmable Gate Array，現(xiàn)場(chǎng)可編程門陣列控制板和CPLD/Complex Programmable Logic Device，復(fù)雜可編程邏輯器件伺服驅(qū)動(dòng)板；ARM控制板包括ARM9芯片、FLASH存儲(chǔ)器、時(shí)鐘電路、現(xiàn)場(chǎng)總線接口、PC104總線接口和RS232接口；其中，ARM9芯片分別與時(shí)鐘電路、PC104總線、RS232總線、現(xiàn)場(chǎng)總線控制器電連接，現(xiàn)場(chǎng)總線控制器與現(xiàn)場(chǎng)總線電連接；并經(jīng)地址總線、數(shù)據(jù)總線與FLASH存儲(chǔ)器電連接；FPFA控制板包括現(xiàn)場(chǎng)可編程邏輯器件FPGA、FPGA配置電路、時(shí)鐘電路、模式選擇電路、電源模塊和PC104接口；其中，現(xiàn)場(chǎng)可編程邏輯器件FPGA分別與時(shí)鐘電路、PC104總線、模式選擇電路、電源模塊、FPGA配置電路電連接；FPGA配置電路包括JTAG/Joined Test Action Group，聯(lián)合測(cè)試行動(dòng)組電路和PROM/Programmable Read Only Memory，可編程只讀存儲(chǔ)器，其中PROM和現(xiàn)場(chǎng)可編程邏輯器件FPGA串行連接在JTAG電路上；現(xiàn)場(chǎng)可編程邏輯器件FPGA并經(jīng)電機(jī)速度接口和電機(jī)方向接口與CPLD芯片9進(jìn)行電連接；CPLD伺服驅(qū)動(dòng)板包括CPLD芯片、時(shí)鐘電路、H橋控制電路、光耦隔離電路及過(guò)流保護(hù)電路。其中，CPLD芯片分別與時(shí)鐘電路、現(xiàn)場(chǎng)可編程邏輯器件FPGA、JTAG電路、H橋控制電路、光耦隔離電路電連接；H橋控制電路與直流電機(jī)電連接；光耦隔離電路與比較器輸出端電連接，比較器兩輸入端，一接設(shè)定值信號(hào)，另一接驅(qū)動(dòng)器電流；傳感器單元包括ARM控制板和多種傳感器，ARM控制板完成對(duì)各傳感器信號(hào)的采集與預(yù)處理，并通過(guò)現(xiàn)場(chǎng)總線實(shí)現(xiàn)與運(yùn)動(dòng)控制單元的實(shí)時(shí)通信；傳感器主要有超聲傳感器、紅外傳感器、霍爾傳感器、觸點(diǎn)開關(guān)；系統(tǒng)狀態(tài)監(jiān)控單元和檢測(cè)設(shè)備控制單元都有ARM控制板及檢測(cè)設(shè)備，ARM控制板用來(lái)采集系統(tǒng)及檢測(cè)設(shè)備的狀態(tài)并通過(guò)現(xiàn)場(chǎng)總線實(shí)現(xiàn)與控制主機(jī)的實(shí)時(shí)通信；檢測(cè)設(shè)備包括機(jī)器人電源檢測(cè)單元、系統(tǒng)運(yùn)行檢測(cè)單元和設(shè)備檢測(cè)單元；傳感器單元采集輸電線路信息并經(jīng)過(guò)本單元微處理器進(jìn)行預(yù)處理后，通過(guò)現(xiàn)場(chǎng)總線傳送到機(jī)器人控制主機(jī)，控制主機(jī)綜合傳感器信息、系統(tǒng)狀態(tài)和檢測(cè)設(shè)備信息，做出動(dòng)作規(guī)劃，向機(jī)器人的運(yùn)動(dòng)控制單元發(fā)送控制命令，控制各關(guān)節(jié)電機(jī)的轉(zhuǎn)動(dòng)，實(shí)現(xiàn)機(jī)器人在輸電線路上越障和行走，達(dá)到對(duì)高壓輸電線路巡檢的目的；機(jī)器人控制主機(jī)通過(guò)無(wú)線網(wǎng)絡(luò)與地面基站進(jìn)行實(shí)時(shí)通訊，地面基站操控人員依據(jù)信息做遠(yuǎn)程處理，給出指令，通過(guò)控制主機(jī)調(diào)動(dòng)各單元，實(shí)現(xiàn)機(jī)器人在輸電線路上越障和行走，完成高壓輸電線路巡檢任務(wù)。
2. 如權(quán)利要求1所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其特征在于所述控制系統(tǒng)采用分布式的硬件結(jié)構(gòu)，各控制單元既相互獨(dú)立又通過(guò)現(xiàn)場(chǎng)總線聯(lián)系在一起，各單元信息通過(guò)現(xiàn)場(chǎng)總線發(fā)送到機(jī)器人控制主機(jī)，控制主機(jī)通過(guò)現(xiàn)場(chǎng)總線實(shí)現(xiàn)對(duì)各單元的實(shí) 時(shí)訪問(wèn)與控制。
3. 如權(quán)利要求1所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其特征在于所述 機(jī)器人控制主機(jī)，作為機(jī)器人控制器的核心部分，主要完成行為層次上的越障規(guī)劃、圖像采 集與傳輸、障礙信息融合與識(shí)別、各單元之間的組織管理等。
4. 如權(quán)利要求1所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其特征在于所述運(yùn)動(dòng)控制單元中，ARM控制板完成越障動(dòng)作規(guī)劃和各關(guān)節(jié)電機(jī)的協(xié)調(diào)運(yùn)動(dòng)，F(xiàn)PGA控制板和 CPLD伺服驅(qū)動(dòng)板完成電機(jī)的伺服控制和過(guò)流、過(guò)載保護(hù)。
5. 如權(quán)利要求1所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其特征在于所述 傳感器單元，包括超聲傳感器、紅外傳感器、霍爾傳感器、觸點(diǎn)開關(guān)和負(fù)責(zé)信息融合的ARM9 處理器；其中，超聲傳感器陣列探測(cè)電力線上方和下方的障礙物，紅外傳感器陣列檢測(cè)電力線與手爪的相對(duì)位置，霍爾傳感器檢測(cè)各運(yùn)動(dòng)關(guān)節(jié)的機(jī)械限位，觸點(diǎn)開關(guān)用于其他方式障 礙物檢測(cè)失敗后的障礙備用檢測(cè)手段，所有這些傳感器信息經(jīng)過(guò)ARM9處理器融合后得到 初步的外部環(huán)境信息。
6. 如權(quán)利要求1所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其特征在于所述 系統(tǒng)狀態(tài)監(jiān)控單元能夠?qū)崟r(shí)監(jiān)測(cè)PC104主機(jī)的工作狀態(tài)， 一旦發(fā)現(xiàn)PC104主機(jī)死機(jī)或重啟， 立即向運(yùn)動(dòng)控制器發(fā)出停止運(yùn)動(dòng)命令，避免因PC104主機(jī)故障而帶來(lái)的危險(xiǎn)。
7. 如權(quán)利要求1所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，其特征在于所述 檢測(cè)設(shè)備控制單元利用ARM9處理器采集各檢測(cè)設(shè)備的工作狀態(tài)并進(jìn)行實(shí)時(shí)分析，將預(yù)處 理的結(jié)果通過(guò)現(xiàn)場(chǎng)總線傳送給控制主機(jī)，控制主機(jī)綜合各種信息通過(guò)檢測(cè)設(shè)備控制單元的 ARM9處理器對(duì)各檢測(cè)設(shè)備進(jìn)行實(shí)時(shí)控制。
8. —種如權(quán)利要求1所述的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)的控制流程，其特征在于包括步驟A) 開啟機(jī)器人控制主機(jī)，經(jīng)現(xiàn)場(chǎng)總線對(duì)各單元初始化；B) 檢測(cè)設(shè)備控制單元采集系統(tǒng)、設(shè)備信息，傳感器單元采集傳感器信息，處理后，經(jīng)現(xiàn) 場(chǎng)總線返回控制主機(jī)；C) 控制主機(jī)發(fā)出運(yùn)動(dòng)控制指令，綜合各單元返回的信息由ARM控制板作動(dòng)作規(guī)劃，現(xiàn) 場(chǎng)可編程邏輯器件FPGA8產(chǎn)生直流電機(jī)的速度、方向控制信號(hào)；D) 直流電機(jī)的速度、方向控制信號(hào)傳遞給CPLD伺服驅(qū)動(dòng)板；E) 經(jīng)過(guò)CPLD芯片的內(nèi)部邏輯轉(zhuǎn)換成直接控制電機(jī)驅(qū)動(dòng)器的速度和方向信號(hào)，驅(qū)動(dòng)直 流電機(jī)轉(zhuǎn)動(dòng)；F) 若直流電機(jī)轉(zhuǎn)動(dòng)到位，直流電機(jī)停止轉(zhuǎn)動(dòng)，若直流電機(jī)轉(zhuǎn)動(dòng)不到位，返回E)步，循環(huán) 動(dòng)作，直到到位為止。
9. 如權(quán)利要求8所述的的控制流程，其特征在于所述E)步中，同時(shí)，CPLD芯片經(jīng)H橋 控制電路及過(guò)流保護(hù)電路采集直流電機(jī)電流信號(hào)，將直流電機(jī)電流信號(hào)與比較器提供的設(shè) 定值信號(hào)比較，并由光耦隔離電路傳送到CPLD芯片中A) 若采集到的電流值沒有超過(guò)設(shè)定值，驅(qū)動(dòng)直流電機(jī)轉(zhuǎn)動(dòng)；B) 若采集到的電流值超過(guò)設(shè)定值，則說(shuō)明直流電機(jī)過(guò)流或過(guò)載，信號(hào)馬上傳送到CPLD 芯片，通過(guò)CPLD芯片的內(nèi)部邏輯發(fā)出停機(jī)命令，使直流電機(jī)停止轉(zhuǎn)動(dòng)。
全文摘要
本發(fā)明公開了一種基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)，涉及機(jī)器人制造技術(shù)，是在高壓輸電線路線上作業(yè)中使用的巡檢系統(tǒng)的自動(dòng)控制系統(tǒng)。該系統(tǒng)由機(jī)器人控制主機(jī)、運(yùn)動(dòng)控制單元、傳感器單元、系統(tǒng)狀態(tài)監(jiān)測(cè)單元、檢測(cè)設(shè)備控制單元和地面基站組成。機(jī)器人控制主機(jī)根據(jù)傳感器單元采集的信息進(jìn)行動(dòng)作規(guī)劃，由運(yùn)動(dòng)控制單元對(duì)各關(guān)節(jié)進(jìn)行控制，同時(shí)實(shí)時(shí)監(jiān)控整個(gè)控制系統(tǒng)和各檢測(cè)設(shè)備的運(yùn)行狀況，并通過(guò)無(wú)線網(wǎng)絡(luò)與地面基站進(jìn)行聯(lián)系，各單元和機(jī)器人主控機(jī)通過(guò)現(xiàn)場(chǎng)總線進(jìn)行實(shí)時(shí)通訊。本發(fā)明的基于現(xiàn)場(chǎng)總線的雙臂巡線機(jī)器人控制系統(tǒng)采用分布式硬件結(jié)構(gòu)，適用于高壓輸電線路線上巡檢、除冰等一系列作業(yè)，有利于保障國(guó)家電力系統(tǒng)的安全可靠運(yùn)行。
文檔編號(hào)G05D1/02GK101770221SQ20081024674
公開日2010年7月7日 申請(qǐng)日期2008年12月30日 優(yōu)先權(quán)日2008年12月30日
發(fā)明者李恩, 楊國(guó)棟, 梁自澤, 范長(zhǎng)春, 譚民 申請(qǐng)人:中國(guó)科學(xué)院自動(dòng)化研究所