本發(fā)明屬于電力技術(shù)領(lǐng)域，具體涉及一種基于雙機(jī)械臂和輔助臂的帶電作業(yè)機(jī)器人控制系統(tǒng)。

背景技術(shù)：

帶電作業(yè)是在高壓電氣設(shè)備上不停電進(jìn)行檢查、維修及部件更換的一種特殊的工程技術(shù)。長期以來,我國普遍釆用的是人工帶電作業(yè)或者主從操作的作業(yè)方式。對于作業(yè)對象是10KV及以下的高壓架空線路,這些作業(yè)方式要求作業(yè)人員長時間處在高空、高壓、強(qiáng)電磁場的環(huán)境中,極易發(fā)生人身傷亡的事故。

雖然國內(nèi)已研發(fā)有帶電作業(yè)機(jī)器人，但仍然需要操作人員在絕緣斗內(nèi)隨機(jī)器人升至線路附近，并沒有從根本上解決操作人員的安全問題。并且，該作業(yè)機(jī)器人完全由操作人員控制，并不能自主完成帶電作業(yè)，較傳統(tǒng)的絕緣手套作業(yè)法反而降低了工作效率。

面向這些較為復(fù)雜且危險的作業(yè)任務(wù)，需要自主作業(yè)機(jī)器人具有很高的智能，帶電作業(yè)安全可靠，這對整個控制系統(tǒng)的設(shè)計提出了很高的要求。為使作業(yè)機(jī)械手具有一定的自主作業(yè)能力，需具備多種關(guān)鍵技術(shù)，使其能夠安全可靠地自主完成帶電作業(yè)任務(wù)。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明提出一種基于雙機(jī)械臂和輔助臂的帶電作業(yè)機(jī)器人控制系統(tǒng)，基于雙目視覺結(jié)合坐標(biāo)變換的方法自主式地控制機(jī)械臂完成配電線路帶電作業(yè)的任務(wù)，減輕作業(yè)人員的勞動強(qiáng)度，提高了安全性。

為了解決上述技術(shù)問題，本發(fā)明提供一種基于雙機(jī)械臂和輔助臂的帶電作業(yè)機(jī)器人控制系統(tǒng)，包括絕緣斗臂車，搭載在絕緣斗臂車上的機(jī)器人平臺，安裝在機(jī)器人平臺上的機(jī)械臂，還包括數(shù)據(jù)處理和控制系統(tǒng)；所述機(jī)械臂包括第一機(jī)械臂、第二機(jī)械臂和輔助機(jī)械臂，所述攝像機(jī)包括雙目攝像頭，所述第一機(jī)械臂、第二機(jī)械臂和輔助機(jī)械臂上均搭載有雙目攝像頭；所述雙目攝像頭用于采集機(jī)械臂作業(yè)場景圖像，并將所述作業(yè)場景圖像發(fā)送給數(shù)據(jù)處理和控制系統(tǒng)；所述數(shù)據(jù)處理和控制系統(tǒng)根據(jù)所述作業(yè)場景圖像規(guī)劃出機(jī)械臂空間路徑，具體方法為：將機(jī)械臂末端位置與作業(yè)目標(biāo)位置轉(zhuǎn)換到同一參考坐標(biāo)系下，獲取械臂末端位置和作業(yè)目標(biāo)位置在同一參考坐標(biāo)系下的坐標(biāo)之差；根據(jù)所述坐標(biāo)之差使用運(yùn)動學(xué)逆解計算方法解算出機(jī)械臂每個關(guān)節(jié)的角度期望值。

進(jìn)一步，使用D-H參數(shù)法以機(jī)械臂基座的中心點(diǎn)為原點(diǎn)建立基坐標(biāo)系，將機(jī)械臂末端位置與作業(yè)目標(biāo)位置均轉(zhuǎn)換到基坐標(biāo)系下；

將機(jī)械臂末端位置轉(zhuǎn)換到基坐標(biāo)系下的過程為：

1-1：在機(jī)械臂的每一個關(guān)節(jié)處建立連桿坐標(biāo)系，并在機(jī)械臂末端執(zhí)行工器具處建立工具坐標(biāo)系；以雙目攝像頭連線的中垂點(diǎn)為原點(diǎn)建立雙目坐標(biāo)系；

1-2：由機(jī)械臂各關(guān)節(jié)處的編碼器采集每個關(guān)節(jié)當(dāng)前的角度數(shù)據(jù)并發(fā)送給第二工控機(jī)，第二工控機(jī)根據(jù)關(guān)節(jié)角度確定機(jī)械臂基座到末端執(zhí)行工器具間的總連桿變換矩陣；

1-3：根據(jù)所述總連桿變換矩陣獲得機(jī)械臂末端在基坐標(biāo)系下的坐標(biāo)。

將作業(yè)目標(biāo)的位置轉(zhuǎn)換到基坐標(biāo)系下的過程為：

2-1：通過模板匹配的方法獲取作業(yè)目標(biāo)在雙目攝像機(jī)左、右圖像中的位置，并根據(jù)立體視覺三維測量原理計算獲得作業(yè)目標(biāo)在雙目坐標(biāo)系下的坐標(biāo)；

2-2：將作業(yè)目標(biāo)在雙目坐標(biāo)系下的坐標(biāo)左乘平移坐標(biāo)變換矩陣得到目標(biāo)在機(jī)械臂腕旋轉(zhuǎn)關(guān)節(jié)坐標(biāo)系下的坐標(biāo)，再左乘腕旋轉(zhuǎn)關(guān)節(jié)坐標(biāo)系到基坐標(biāo)系的連桿變換矩陣得到作業(yè)目標(biāo)在基坐標(biāo)系下的坐標(biāo)。

進(jìn)一步，所述數(shù)據(jù)處理和控制系統(tǒng)包括第一工控機(jī)、第二工控機(jī)，第二工控機(jī)內(nèi)置圖像處理器和帶電作業(yè)動作序列庫，所述帶電作業(yè)動作序列庫中預(yù)先存儲有各項(xiàng)帶電作業(yè)對應(yīng)的動作序列數(shù)據(jù)；所述攝像機(jī)采集的作業(yè)場景圖像發(fā)送給第二工控機(jī)，圖像處理器對作業(yè)場景圖像進(jìn)行處理后獲的械臂末端位置和作業(yè)目標(biāo)位置在同一參考坐標(biāo)系下的坐標(biāo)之差，第二工控機(jī)根據(jù)械臂末端位置和作業(yè)目標(biāo)位置在同一參考坐標(biāo)系下的坐標(biāo)之差以及具體帶電作業(yè)所對應(yīng)的動作序列解算出機(jī)械臂每個關(guān)節(jié)的角度期望值，并將所述角度期望值數(shù)據(jù)發(fā)送給第一工控機(jī)；第一工控機(jī)根據(jù)所述角度期望值控制機(jī)械臂動作。

進(jìn)一步，所述絕緣斗臂車上設(shè)置有控制室，所述數(shù)據(jù)處理和控制系統(tǒng)包括第一工控機(jī)、第二工控機(jī)、顯示屏和主操作手，第二工控機(jī)內(nèi)置圖像處理器，顯示屏和主操作手位于控制室內(nèi)；主操作手與機(jī)械臂為主從操作關(guān)系，通過改變主操作手的姿態(tài)控制機(jī)械臂運(yùn)動；所述攝像機(jī)采集的作業(yè)場景圖像發(fā)送給第二工控機(jī)，圖像處理器對作業(yè)場景圖像進(jìn)行處理后獲的3D虛擬作業(yè)場景，并送顯示器顯示。

進(jìn)一步，所述機(jī)械臂或者主操作手為六自由度機(jī)構(gòu)，包括基座，旋轉(zhuǎn)軸方向與基座平面垂直的腰關(guān)節(jié)，與腰關(guān)節(jié)連接的肩關(guān)節(jié)，與肩關(guān)節(jié)連接的大臂，與大臂連接的肘關(guān)節(jié)，與肘關(guān)節(jié)連接的小臂，與小臂連接的腕關(guān)節(jié)，腕關(guān)節(jié)由三個旋轉(zhuǎn)關(guān)節(jié)組成，分別為腕俯仰關(guān)節(jié)、腕搖擺關(guān)節(jié)和腕旋轉(zhuǎn)關(guān)節(jié)；所述六自由度機(jī)構(gòu)中各個關(guān)節(jié)均具有相應(yīng)的正交旋轉(zhuǎn)編碼器和伺服驅(qū)動電機(jī)，正交旋轉(zhuǎn)編碼器用于采集各個關(guān)節(jié)的角度數(shù)據(jù)，伺服驅(qū)動電機(jī)用于控制各關(guān)節(jié)的運(yùn)動；第一工控機(jī)根據(jù)機(jī)械臂各關(guān)節(jié)角度的期望值，通過控制伺服驅(qū)動電機(jī)控制按機(jī)械臂各關(guān)節(jié)運(yùn)動。

進(jìn)一步，各機(jī)械臂間之間分配有優(yōu)先級，第一機(jī)械臂設(shè)置為第一優(yōu)先級，第二機(jī)械臂設(shè)置為第二優(yōu)先級，輔助機(jī)械臂的優(yōu)先級在其需要抓取執(zhí)行工器具時設(shè)置為第一優(yōu)先級，在不需要抓取執(zhí)行工器具時，設(shè)置為第三優(yōu)先級，優(yōu)先級最高的機(jī)械臂在執(zhí)行任務(wù)過程中不受約束，優(yōu)先級較低的機(jī)械臂在執(zhí)行任務(wù)過程中主動規(guī)避與優(yōu)先級最高的機(jī)械臂發(fā)生避碰。

本發(fā)明與現(xiàn)有技術(shù)相比，其顯著優(yōu)點(diǎn)在于，該控制系統(tǒng)是基于雙目視覺結(jié)合坐標(biāo)變換的方法自主式地控制機(jī)械臂完成配電線路帶電作業(yè)的任務(wù)，減輕作業(yè)人員的勞動強(qiáng)度，提高了安全性。另外，相較于常規(guī)的雙機(jī)械臂帶電作業(yè)人增加了輔助作業(yè)機(jī)械臂，它可以與雙機(jī)械臂進(jìn)行協(xié)同作業(yè)，完成測量和監(jiān)測的任務(wù)，并且機(jī)械臂之間可以共享圖像信息。通過雙機(jī)械臂與輔助機(jī)械臂的協(xié)同作業(yè)，能夠完成一些以往較為難完成的作業(yè)任務(wù)，如：更換跌落式熔斷器，更換隔離刀閘等等，提高了作業(yè)的可靠性與便捷性。

附圖說明

圖1為本發(fā)明帶電作業(yè)機(jī)器人一種實(shí)施例的整體結(jié)構(gòu)示意圖；

圖2為本發(fā)明帶電作業(yè)機(jī)器人控制系統(tǒng)組成框圖；

圖3為本發(fā)明中機(jī)器人平臺的結(jié)構(gòu)示意圖；

圖4為本發(fā)明中機(jī)械臂的結(jié)構(gòu)示意圖；

圖5為本發(fā)明中機(jī)械臂連桿坐標(biāo)系示意圖；

圖6為本發(fā)明中機(jī)械臂控制原理框圖。

具體實(shí)施方式

容易理解，依據(jù)本發(fā)明的技術(shù)方案，在不變更本發(fā)明的實(shí)質(zhì)精神的情況下，本領(lǐng)域的一般技術(shù)人員可以想象出本發(fā)明基于雙機(jī)械臂和輔助臂的帶電作業(yè)機(jī)器人控制系統(tǒng)的多種實(shí)施方式。因此，以下具體實(shí)施方式和附圖僅是對本發(fā)明的技術(shù)方案的示例性說明，而不應(yīng)當(dāng)視為本發(fā)明的全部或者視為對本發(fā)明技術(shù)方案的限制或限定。

基于雙機(jī)械臂和輔助臂的帶電作業(yè)機(jī)器人

結(jié)合附圖，帶電作業(yè)機(jī)器人包括絕緣斗臂車1、控制室2、伸縮臂3、機(jī)器人平臺4。其中，絕緣斗臂車1上架設(shè)控制室2和伸縮臂3，伸縮臂3末端連接機(jī)器人平臺4，機(jī)器人平臺4與控制室2之間采用光纖以太網(wǎng)通信或者無線網(wǎng)絡(luò)通信。

絕緣斗臂車1可供操作人員駕駛，從而將機(jī)器人平臺4運(yùn)輸?shù)阶鳂I(yè)現(xiàn)場。絕緣斗臂車1上裝有支撐腿，支撐腿可以展開，從而將絕緣斗臂車1與地面穩(wěn)固支撐。絕緣斗臂車1上裝有發(fā)電機(jī)，從而給控制室2及伸縮臂3供電。

伸縮臂3設(shè)有沿伸縮方向的驅(qū)動裝置，操作人員可以通過控制驅(qū)動裝置，從而將機(jī)器人平臺4升降到作業(yè)高度。該伸縮臂3由絕緣材料制成，用于實(shí)現(xiàn)機(jī)器人平臺4與控制室2的絕緣。在本發(fā)明中，伸縮臂3可有由剪叉式升降機(jī)構(gòu)或其他機(jī)構(gòu)代替。

作為一種實(shí)施方式，控制室2中設(shè)置有第二工控機(jī)、顯示屏、第一主操作手、第二主操作手、輔助主操作手以及通信模塊等。

作為一種實(shí)施方式，機(jī)器人平臺4包括絕緣子46、第一機(jī)械臂43、第二機(jī)械臂44、輔助機(jī)械臂42、第一工控機(jī)48、雙目攝像頭45、全景攝像頭41、深度攝像頭410、蓄電池49、專用工具箱47、通信模塊。

機(jī)器人平臺4的絕緣子46用于支撐第一機(jī)械臂43、第二機(jī)械臂44、輔助機(jī)械臂42，將這三個機(jī)械臂的外殼與機(jī)器人平臺4絕緣。

蓄電池49為第一工控機(jī)48、第一機(jī)械臂43、第二機(jī)械臂44、輔助機(jī)械臂42、全景攝像頭41、雙目攝像頭45、深度攝像頭410、通信模塊供電。

作為一種實(shí)施方式，雙目攝像頭45一共有三個，分別安裝在第一機(jī)械臂43、第二機(jī)械臂44和輔助機(jī)械臂42的腕旋轉(zhuǎn)關(guān)節(jié)439上，負(fù)責(zé)采集作業(yè)場景的圖像數(shù)據(jù)，并將圖像數(shù)據(jù)發(fā)送給第二工控機(jī)。雙目攝像頭45由兩個光軸平行的工業(yè)相機(jī)組成，平行光軸之間的距離固定。

深度攝像頭410安裝在機(jī)器人平臺4正對作業(yè)場景的側(cè)面，負(fù)責(zé)采集作業(yè)場景的景深數(shù)據(jù)，將景深數(shù)據(jù)發(fā)送給第二工控機(jī)。

全景攝像頭41通過支架安裝在機(jī)器人平臺4的上方，負(fù)責(zé)采集作業(yè)場景的全景圖像數(shù)據(jù)，將圖像數(shù)據(jù)發(fā)送給第二工控機(jī)，并顯示在顯示器上，作業(yè)人員可以通過全景圖像監(jiān)控作業(yè)場景。

專用工具箱47是放置抓具、扳手等作業(yè)工具的場所。機(jī)械臂末端安裝有工具快換裝置。機(jī)械臂根據(jù)作業(yè)任務(wù)的類型到專用工具箱47中使用工具快換裝置獲取作業(yè)工具。

控制室2中第一主操作手、第二主操作手以及輔助主操作手是一種用于人工遠(yuǎn)程操作機(jī)械臂的操作裝置，他們與第一機(jī)械臂43、第二機(jī)械臂44和輔助機(jī)械臂42構(gòu)成主從操作關(guān)系。機(jī)械臂和主操作手具有相同的結(jié)構(gòu)，只是主操作手尺寸規(guī)格比機(jī)械臂小，以便于操作人員操作。機(jī)械臂和主操作手擁有六個關(guān)節(jié)，每個關(guān)節(jié)都有光電編碼器采集角度數(shù)據(jù)，各主操作手的微型控制器通過串口將六個關(guān)節(jié)的角度數(shù)據(jù)發(fā)送給第二工控機(jī)。

作為本發(fā)明一個實(shí)施例，所述機(jī)械臂為六自由度機(jī)構(gòu)，包括基座431，旋轉(zhuǎn)軸方向與基座平面垂直的腰關(guān)節(jié)432，與腰關(guān)節(jié)432連接的肩關(guān)節(jié)433，與肩關(guān)節(jié)433連接的大臂434，與大臂434連接的肘關(guān)節(jié)435，與肘關(guān)節(jié)435連接的小臂436，與小臂436連接的腕關(guān)節(jié)，腕關(guān)節(jié)由三個旋轉(zhuǎn)關(guān)節(jié)組成，分別為腕俯仰關(guān)節(jié)437、腕搖擺關(guān)節(jié)438和腕旋轉(zhuǎn)關(guān)節(jié)439；所述六自由度機(jī)構(gòu)中各個關(guān)節(jié)均具有相應(yīng)的正交旋轉(zhuǎn)編碼器31和伺服驅(qū)動電機(jī)，正交旋轉(zhuǎn)編碼器31用于采集各個關(guān)節(jié)的角度數(shù)據(jù)，伺服驅(qū)動電機(jī)用于控制各關(guān)節(jié)的運(yùn)動；第一工控機(jī)根據(jù)所述機(jī)械臂的空間路徑解算出各關(guān)節(jié)的運(yùn)動角度，控制伺服驅(qū)動電機(jī)按照所述運(yùn)動角度控制機(jī)械臂各關(guān)節(jié)運(yùn)動。

作為一種實(shí)施方式，機(jī)器人平臺4與控制室2之間的數(shù)據(jù)傳輸通過光纖有線傳輸，或者使用無線網(wǎng)絡(luò)傳輸。機(jī)器人平臺4上的通信模塊是光纖收發(fā)器，光纖收發(fā)器用于實(shí)現(xiàn)光纖中的光信號與雙絞線中的電信號的相互轉(zhuǎn)換，從而在通信上實(shí)現(xiàn)機(jī)器人平臺4與控制室2的電氣隔離。控制室2中的通信模塊是光纖收發(fā)器，光纖收發(fā)器用于實(shí)現(xiàn)光纖中的光信號與雙絞線中的電信號的相互轉(zhuǎn)換，從而在通信上實(shí)現(xiàn)機(jī)器人平臺4與控制室2的電氣隔離。

作為一種實(shí)施方式，第二工控機(jī)可以完成以下任務(wù)：

建立動作序列庫。預(yù)先將各項(xiàng)帶電作業(yè)任務(wù)分解為作用序列，組成動作序列庫，存儲在第二工控機(jī)中，用于機(jī)械臂路徑規(guī)劃。

建立作業(yè)對象模型庫。預(yù)先制作各項(xiàng)帶電作業(yè)任務(wù)所涉及的作業(yè)對象的三維模型和目標(biāo)識別模型，例如，根據(jù)電力塔桿、電線、耐張絕緣子、隔離刀閘、避雷器等器件實(shí)物，制作三維模型和目標(biāo)識別模型，用于帶電作業(yè)機(jī)器人自動識別作業(yè)對象，構(gòu)建作業(yè)場景三維虛擬場景。

建立機(jī)械臂和專用工具模型庫。預(yù)先制作機(jī)械臂和專用工具的三維模型和目標(biāo)識別模型，例如，扳手等，用于帶電作業(yè)機(jī)器人自動構(gòu)建作業(yè)場景三維虛擬場景，規(guī)劃機(jī)械臂空間路徑。

獲取圖像數(shù)據(jù)。獲取全景圖像、深度圖像和雙目圖像的數(shù)據(jù)信息。

根據(jù)圖像數(shù)據(jù)識別和跟蹤作業(yè)目標(biāo)。

獲取主操作手的角度、角速度和角加速度數(shù)據(jù)，獲取機(jī)械臂的角度、角速度和角加速度數(shù)據(jù)。

對相關(guān)圖像數(shù)據(jù)進(jìn)行處理和計算，獲取機(jī)械臂位置，獲取作業(yè)對象的位置，獲取機(jī)械臂與作業(yè)對象之間的相對位置，并根據(jù)相對位置和作業(yè)任務(wù)規(guī)劃機(jī)械臂的空間路徑。

根據(jù)圖像數(shù)據(jù)構(gòu)建作業(yè)對象三維場景，根據(jù)機(jī)械臂角度信息和作業(yè)對象三維場景獲得機(jī)械臂與作業(yè)對象的相對位置，并根據(jù)相對位置和作業(yè)任務(wù)規(guī)劃機(jī)械臂的空間路徑。

對相關(guān)圖像數(shù)據(jù)進(jìn)行處理和計算，構(gòu)建3D虛擬作業(yè)場景，送顯示器顯示，操作人員根據(jù)3D虛擬作業(yè)場景監(jiān)控作業(yè)過程。與全景圖像相比，3D虛擬作業(yè)場景綜合和深度圖像信息和雙目圖像信息，對機(jī)器臂與作業(yè)對象之間、機(jī)械臂之間、作業(yè)對象與作業(yè)環(huán)境之間的相對位置的判斷更精確，且不會存在視覺死角。因此，操作人員通過3D虛擬作業(yè)場景進(jìn)行作業(yè)監(jiān)控，操作精度更高，可以防止碰撞發(fā)生，提高了安全性。同時，3D虛擬作業(yè)場景顯示在控制室2中的顯示器上，遠(yuǎn)離機(jī)械臂作業(yè)現(xiàn)場，提高了人作業(yè)人員的人身安全。

作為一種實(shí)施方式，第一工控機(jī)可以完成以下任務(wù)：

根據(jù)第二工控機(jī)發(fā)送的主操作手各關(guān)節(jié)的角度信息，控制機(jī)械臂各關(guān)節(jié)的運(yùn)動。

獲取第二工控機(jī)發(fā)送的機(jī)械臂的空間路徑數(shù)據(jù)，根據(jù)作業(yè)任務(wù)的動作序列，解算出機(jī)械臂各關(guān)節(jié)的角度數(shù)據(jù)運(yùn)動量，并控制機(jī)械臂各關(guān)節(jié)運(yùn)動。

本發(fā)明中，第一機(jī)械臂和第二機(jī)械臂相互配合，可以模仿人的兩個手的作業(yè)順序完成帶電作業(yè)?？紤]到靈活性，可以再增加一個強(qiáng)壯的輔助機(jī)械臂，此時，輔助機(jī)械臂專司器件夾持等力道大的動作，第一機(jī)械臂和第二機(jī)械臂則進(jìn)行相關(guān)業(yè)務(wù)操作。

根據(jù)第二工控機(jī)和第一工控機(jī)完成的不同任務(wù)的組合，本發(fā)明帶電作業(yè)機(jī)器人既可以由作業(yè)人員進(jìn)行遠(yuǎn)程搖操作以完成帶電作業(yè)，又可以進(jìn)行自主帶電作業(yè)。在進(jìn)行帶電作業(yè)之前，作業(yè)人員先通過觀察全景圖像，將機(jī)器人平臺4移動至作業(yè)對象附近。

如果選擇人工遠(yuǎn)程搖操作，則由第二工控機(jī)根據(jù)數(shù)目圖像和深度圖像構(gòu)建3D虛擬作業(yè)場景并送顯示器顯示，作業(yè)人員通過3D虛擬作業(yè)場景監(jiān)控操作過程，通過主操作手控制機(jī)械臂的動作，以完成帶電作業(yè)。在此過程中，作業(yè)人員改變主操作手姿態(tài)后，主操作手中各關(guān)節(jié)的光電編碼器采集各關(guān)節(jié)角度，各主操作手的微型控制器通過串口將各關(guān)節(jié)的角度數(shù)據(jù)發(fā)送給第二工控機(jī)。第二工控機(jī)將主操作手各關(guān)節(jié)的角度數(shù)據(jù)作為機(jī)械臂各關(guān)節(jié)角度的期望值發(fā)送給第一工控機(jī)，第一工控機(jī)根據(jù)角度期望值通過伺服電機(jī)控制機(jī)械臂各關(guān)節(jié)的運(yùn)動，已完成帶電作業(yè)。

如果選擇自主作業(yè)，則由第二工控機(jī)根據(jù)雙目圖像通過坐標(biāo)系變換計算獲取作業(yè)對象和機(jī)械臂之間的相對位置關(guān)系，然后依據(jù)作業(yè)任務(wù)所對應(yīng)的動作序列進(jìn)行機(jī)械臂空間路徑規(guī)劃，解算出機(jī)械臂各關(guān)節(jié)需要轉(zhuǎn)動的角度數(shù)據(jù)作為機(jī)械臂各關(guān)節(jié)角度的期望值，并將空間路徑發(fā)送給第一工控機(jī)，第二工控機(jī)通過伺服電機(jī)控制機(jī)械臂各關(guān)節(jié)的運(yùn)動，以完成帶電作業(yè)。

帶電作業(yè)機(jī)器人控制系統(tǒng)

上述基于雙機(jī)械臂(第一機(jī)械臂和第二機(jī)械臂)和輔助機(jī)械臂的帶電作業(yè)機(jī)器人中，雙機(jī)械臂是該帶電作業(yè)機(jī)器人的主體操作部件，作業(yè)時需要直接接觸帶電線路等帶電體，并通過工控機(jī)發(fā)出的運(yùn)動指令來完成相應(yīng)動作，應(yīng)能夠?qū)崿F(xiàn)速度、位置控制。且第一機(jī)械臂和第二機(jī)械臂具有良好的協(xié)作能力。第一機(jī)械臂和第二機(jī)械臂末端需配有符合國標(biāo)的機(jī)械接口，根據(jù)帶電作業(yè)任務(wù)的不同，可以更換不同的專用作業(yè)工器具。利用這些專用工器具，帶電作業(yè)機(jī)械臂可完成拆裝螺母、拆接導(dǎo)線等作業(yè)。

機(jī)器臂采用電驅(qū)動，具有體型靈巧，自重低，持重相對高，安裝尺寸小、關(guān)節(jié)的控制驅(qū)動一體化及編程開放性好的特點(diǎn)。機(jī)械臂關(guān)節(jié)處安裝有光電編碼器測量關(guān)節(jié)轉(zhuǎn)角。使用的主機(jī)械臂與輔助機(jī)械臂均為6個自由度，由基座、腰關(guān)節(jié)、肩關(guān)節(jié)、肘關(guān)節(jié)、腕關(guān)節(jié)組成，各關(guān)節(jié)之間通過連桿連接，其中：腕關(guān)節(jié)有三個自由度，其他各有一個自由度。本實(shí)施例使用的六自由度主機(jī)械臂如圖4所示。以第一機(jī)械臂43為例，六自由度機(jī)械臂由基座431、腰關(guān)節(jié)432、肩關(guān)節(jié)433、大臂434、肘關(guān)節(jié)435、小臂436、腕關(guān)節(jié)、夾爪接口4310組成，腕關(guān)節(jié)又包括三個關(guān)節(jié)，即腕俯仰關(guān)節(jié)437、腕搖擺關(guān)節(jié)438、腕旋轉(zhuǎn)關(guān)節(jié)439。因此，腕關(guān)節(jié)有三個自由度，其他各關(guān)節(jié)各有一個自由度。機(jī)器人本身是由擠壓鋁管和關(guān)節(jié)組成的手臂?；?31是機(jī)器人的安裝位置，機(jī)器人的另一端與從機(jī)械臂專用工具箱47中取出工具相連。通過協(xié)調(diào)各個關(guān)節(jié)的運(yùn)動，機(jī)器人可隨意移動工具進(jìn)行作業(yè)。夾爪接口4310用于連接機(jī)械臂專用夾爪，夾爪用于抓起或連接機(jī)械臂專用工具箱47內(nèi)的末端工器具。

第一機(jī)械臂和第二機(jī)械臂為主要的作業(yè)執(zhí)行器，進(jìn)行裝卸螺栓螺母、接搭引線等操作；輔助機(jī)械臂與主機(jī)械臂同構(gòu)，具有更大的尺寸，主要用于協(xié)作雙機(jī)械臂進(jìn)行抓取、固定和移動隔離變壓器、跌落式熔斷器等室外輸配電設(shè)備以及夾持引線等等作用，提高作業(yè)可靠性；

三個機(jī)械臂各有一個伺服控制器，使其可以進(jìn)行位置、速度控制。機(jī)械臂之間可以進(jìn)行一定的協(xié)調(diào)作業(yè)，保證在作業(yè)時可通過最優(yōu)路徑進(jìn)行自主作業(yè)，避免發(fā)生相互碰撞或碰撞障礙物如帶電引線、配電設(shè)備等，確保機(jī)器人執(zhí)行作業(yè)任務(wù)的可靠性、安全性和穩(wěn)定性；

帶電作業(yè)機(jī)器人采用兩臺工業(yè)控制計算機(jī)(第一工控機(jī)和第二個工控機(jī))進(jìn)行信息處理和系統(tǒng)控制，具有大量可擴(kuò)展外設(shè)接口，分別與每個機(jī)械臂控制器通過以太網(wǎng)接口或串口控制。其中，第一工控機(jī)主要用于機(jī)械臂控制，第二工控機(jī)用于圖像處理，機(jī)械臂運(yùn)動學(xué)計算，坐標(biāo)系變換處理等過程。

在開始作業(yè)前需要為帶電作業(yè)機(jī)器人系統(tǒng)建立模型和生成動作序列，具體包括如下工作：

1、通過主從遙操作使機(jī)械臂移動到目標(biāo)作業(yè)區(qū)域附近，確保作業(yè)對象的目標(biāo)在雙目視覺的識別范圍內(nèi)；

2、使用D-H參數(shù)法以機(jī)械臂基座的中心點(diǎn)為原點(diǎn)建立基坐標(biāo)系，在機(jī)械臂的每一個關(guān)節(jié)處建立連桿坐標(biāo)系，并在專用工具處建立工具坐標(biāo)系。

基坐標(biāo)系為參考坐標(biāo)系，從基座開始變換到肩關(guān)節(jié)，然后到肘關(guān)節(jié)，最后到末端的腕關(guān)節(jié)。A_i為連桿坐標(biāo)系i-1到連桿坐標(biāo)系i之間的連桿變換矩陣。

其中，θ_i,α_i,a_i和d_i分別為D-H模型參數(shù)中的關(guān)節(jié)角、扭轉(zhuǎn)角、連桿長度和連桿偏移量。

3、以雙目攝像機(jī)中兩相機(jī)連線的中垂點(diǎn)為原點(diǎn)建立雙目坐標(biāo)系，為方便進(jìn)行平移坐標(biāo)變換到腕旋轉(zhuǎn)關(guān)節(jié)坐標(biāo)系，令雙目坐標(biāo)系的三個軸的方向與腕旋轉(zhuǎn)關(guān)節(jié)坐標(biāo)系的三個軸的方向相同；

4、通過人工選擇或雙目視覺識別作業(yè)任務(wù)。根據(jù)帶電作業(yè)機(jī)器人自主作業(yè)方法生成動作序列，它包含機(jī)械臂自主作業(yè)時每一步的路徑及動作順序；

機(jī)械臂的自主作業(yè)控制步驟及如下：

步驟一、確定機(jī)械臂末端當(dāng)前的位姿

1-1：使用D-H參數(shù)法以機(jī)械臂基座的中心點(diǎn)為原點(diǎn)建立基坐標(biāo)系，在機(jī)械臂的每一個關(guān)節(jié)處建立連桿坐標(biāo)系，并在機(jī)械臂末端執(zhí)行工器具處建立工具坐標(biāo)系；以雙目攝像頭連線的中垂點(diǎn)為原點(diǎn)建立雙目坐標(biāo)系；

1-2：由機(jī)械臂各關(guān)節(jié)處的編碼器采集每個關(guān)節(jié)當(dāng)前的角度數(shù)據(jù)并發(fā)送給第二工控機(jī)，第二工控機(jī)根據(jù)關(guān)節(jié)角度確定機(jī)械臂基座到末端執(zhí)行工器具間的總連桿變換矩陣，根據(jù)當(dāng)前所連接的末端專用作業(yè)工器具的長度獲取工器具末端在工具坐標(biāo)系下的坐標(biāo)

設(shè)T_t為基座到機(jī)械臂工具坐標(biāo)系的變換矩陣，A_t為工具坐標(biāo)系相對于腕旋轉(zhuǎn)關(guān)節(jié)坐標(biāo)系下的坐標(biāo)變換矩陣。因此，機(jī)械臂基座到末端末端執(zhí)行工器具間的總連桿變換矩陣為:

其中為旋轉(zhuǎn)變換矩陣，與機(jī)械臂的關(guān)節(jié)角度θ有關(guān)。其中，n為法向矢量，o為方向矢量，a為接近矢量?？傔B桿變換矩陣右上方的3*1矩陣為平移變換矩陣，即為當(dāng)前機(jī)械臂工具坐標(biāo)系原點(diǎn)在基坐標(biāo)系下的坐標(biāo)。計算右上方的3*1的矩陣P_t即為機(jī)械臂末端在基坐標(biāo)系下的坐標(biāo)。

步驟二、獲取作業(yè)目標(biāo)的位置

2-1：通過模板匹配的方法獲取作業(yè)目標(biāo)在左、右攝像機(jī)圖像中的位置，并根據(jù)立體視覺三維測量原理計算獲得目標(biāo)在雙目坐標(biāo)系下的坐標(biāo)x_m，y_m，z_m為由雙目視覺通過三維測量圖像處理后得到的目標(biāo)作業(yè)位置在雙目坐標(biāo)系下的坐標(biāo)。

2-2：將目標(biāo)在雙目坐標(biāo)系下的坐標(biāo)左乘平移坐標(biāo)變換矩陣A_m得到目標(biāo)在機(jī)械臂腕旋轉(zhuǎn)關(guān)節(jié)坐標(biāo)系下的坐標(biāo)，再左乘腕旋轉(zhuǎn)關(guān)節(jié)坐標(biāo)系到基坐標(biāo)系的連桿變換矩陣T₆得到目標(biāo)在基坐標(biāo)系下的坐標(biāo)。

因此，雙目坐標(biāo)系相對于基坐標(biāo)系的坐標(biāo)變換矩陣T_m為：T_m＝T₆A_m，其中A_m為雙目坐標(biāo)系相對于腕旋轉(zhuǎn)關(guān)節(jié)坐標(biāo)系的坐標(biāo)變換矩陣，由雙目相機(jī)在機(jī)械臂上的安裝位置決定。

計算其中，T_m，分別由前述計算得到，為左邊的兩個矩陣相乘獲得。R_m為3*3的旋轉(zhuǎn)變換矩陣。右上角的矩陣P_m為3*1的平移變換矩陣，即為機(jī)械臂工具坐標(biāo)系原點(diǎn)在基坐標(biāo)系下的坐標(biāo)；

步驟三、進(jìn)行機(jī)械臂空間路徑規(guī)劃

3-1：第二工控機(jī)計算機(jī)械臂末端位置與作業(yè)目標(biāo)位置在同一基坐標(biāo)系下的坐標(biāo)之差；

3-2：第二工控機(jī)利用機(jī)械臂的運(yùn)動學(xué)逆解計算方法進(jìn)行運(yùn)動軌跡規(guī)劃。即通過比較以下兩式的辦法解得機(jī)械臂每個關(guān)節(jié)角位移θ_i的所有解；

根據(jù)機(jī)械臂工作情況，通過結(jié)合虛擬現(xiàn)實(shí)技術(shù)的模擬避障要求求得θ_i的最優(yōu)唯一解。

3-3、第二工控機(jī)將解得的關(guān)節(jié)位移角度發(fā)送給第一工控機(jī)，第一工控機(jī)將角度信號發(fā)送給機(jī)械臂內(nèi)部的伺服控制器，控制機(jī)械臂各關(guān)節(jié)運(yùn)動至目標(biāo)作業(yè)點(diǎn)；

3-4、到達(dá)目標(biāo)作業(yè)點(diǎn)后，第一工控機(jī)繼續(xù)根據(jù)動作序列控制末端作業(yè)工器具完成作業(yè)動作。

步驟四、在完成動作后返回步驟一繼續(xù)移動到下一位置。若完成所有動作指令，機(jī)械臂復(fù)位到初始位置。

本發(fā)明中的帶電作業(yè)機(jī)器人采用雙機(jī)械臂加輔助臂協(xié)同工作的方式，增加了靈活性和容錯率的同時提高了空間復(fù)雜程度和發(fā)生沖突的幾率。其協(xié)同工作方法主要體現(xiàn)在：

1、各機(jī)械臂間的防碰撞方式：采用結(jié)合虛擬現(xiàn)實(shí)技術(shù)的優(yōu)先級自主式避碰方式。

首先，為機(jī)械臂之間分配優(yōu)先級：定義第一機(jī)械臂為主機(jī)械臂，優(yōu)先級高，第二機(jī)械臂為副機(jī)械臂，優(yōu)先級低。輔助機(jī)械臂的優(yōu)先級在其需要進(jìn)行抓取操作時設(shè)置為第一優(yōu)先級，在不需要進(jìn)行抓取，只完成測量與監(jiān)測任務(wù)時為最低的優(yōu)先級。優(yōu)先級最高的機(jī)械臂在執(zhí)行任務(wù)過程中不受約束，優(yōu)先級較低的機(jī)械臂執(zhí)行避碰操作。

在執(zhí)行任務(wù)的過程中，根據(jù)機(jī)械臂當(dāng)前位姿的測量原理結(jié)合虛擬現(xiàn)實(shí)技術(shù)計算并構(gòu)建出每個機(jī)械臂在三維虛擬場景中的模型，計算機(jī)械臂之間的最短距離并找出最短距離對應(yīng)的點(diǎn)的位置，由該點(diǎn)的位置確定需要重新調(diào)整的關(guān)節(jié)。由于虛擬現(xiàn)實(shí)場景對于實(shí)際過程中存在延遲，并且機(jī)械臂在作業(yè)運(yùn)動時存在初速度，因此需設(shè)定安全距離。如該距離小于設(shè)定的安全距離下，則對優(yōu)先級較低機(jī)械臂重新選擇運(yùn)動學(xué)逆解，并在虛擬現(xiàn)實(shí)場景中進(jìn)行驗(yàn)證是否最短距離已大于安全距離，直至兩個機(jī)械臂之間的最短距離都大于安全距離，并且三個機(jī)械臂之間距離都滿足避碰的安全距離要求。從而實(shí)現(xiàn)機(jī)械臂之間的避障控制。

2、盲區(qū)測量：在實(shí)際測量中，雙機(jī)械臂末端的雙目攝像機(jī)需要識別并跟蹤目標(biāo)作業(yè)點(diǎn)的位置，但由于其尺寸及動作的局限性，在雙目視覺測量過程中存在盲區(qū)。因此在雙機(jī)械臂雙目視覺對于處在盲區(qū)的作業(yè)目標(biāo)不能識別到的情況下，利用輔助機(jī)械臂更大更靈活的優(yōu)勢，通過主從操縱輔助機(jī)械臂上的雙目攝像機(jī)對作業(yè)目標(biāo)的位置進(jìn)行測量，從而利用獲取目標(biāo)作業(yè)位置的方法得到目標(biāo)位置在輔助機(jī)械臂基坐標(biāo)系下的位置信息。

由于雙機(jī)械臂以及輔助機(jī)械臂的基座是相對固定于機(jī)器人平臺上的，因此可以通過坐標(biāo)變換將目標(biāo)在輔助機(jī)械臂基坐標(biāo)下的坐標(biāo)轉(zhuǎn)化為雙機(jī)械臂中任一基坐標(biāo)系下的坐標(biāo)，從而協(xié)助雙機(jī)械臂對目標(biāo)進(jìn)行識別與跟蹤，實(shí)現(xiàn)信息共享。

3、作業(yè)監(jiān)測：在雙機(jī)械臂執(zhí)行主要的操作任務(wù)時，輔助機(jī)械臂可完成帶電作業(yè)任務(wù)完成情況的監(jiān)測，通過將反饋數(shù)據(jù)發(fā)送給工控機(jī)進(jìn)行處理，判斷工作任務(wù)是否執(zhí)行完畢。

當(dāng)雙機(jī)械臂在執(zhí)行裝卸螺栓螺母的任務(wù)時，由于機(jī)械臂內(nèi)部的力反饋傳感器的靈敏度不高，在執(zhí)行該工作時無法通過力反饋判斷螺栓的安裝情況。由于雙機(jī)械臂上的雙目攝像機(jī)在作業(yè)過程中無法直接測量螺栓與螺母之間的間隙，因此需要控制輔助機(jī)械臂移動到相應(yīng)位置上采集螺栓與螺母之間的間隙距離，發(fā)送給工控機(jī)判斷螺栓是否被擰緊，形成反饋控制回路，從而完成了作業(yè)工作的監(jiān)測任務(wù)。