本發(fā)明涉及配網(wǎng)帶電作業(yè)領(lǐng)域，具體的說，是一種涉及高壓帶電作業(yè)機器人的輔助機械臂控制器及其控制方法，主要用于與機械臂配合使用，完成配電帶電作業(yè)中的更換橫擔、變壓器等重載作業(yè)任務(wù)。
背景技術(shù)：
：為了提高帶電作業(yè)的自動化水平和安全性，減輕操作人員的勞動強度和強電磁場對操作人員的人身威脅，從80年代起許多國家都先后開展了帶電作業(yè)機器人的研究，如日本、西班牙、美國、加拿大、法國等國家先后開展了對帶電作業(yè)機器人的研究。2002年我國也進行了高壓帶電作業(yè)機器人產(chǎn)品化樣機的研制。帶電作業(yè)機器人經(jīng)過多年的研究與實踐，完成了實驗室樣機的研制，通過大量的現(xiàn)場試驗，驗證了機器人在配電網(wǎng)帶電作業(yè)中的需求與價值，同時也驗證了研究方案的可行性，設(shè)計的合理性。但機器人作業(yè)內(nèi)容較為單一，大都是采用雙機械臂主從控制方式，不能涵蓋全部配電網(wǎng)帶電作業(yè)項目，機器人不能在復(fù)雜線路環(huán)境下開展作業(yè)，因此與配電網(wǎng)帶電作業(yè)的生產(chǎn)實際存在差距。因此需要研制大持重/自重比輔助機械臂，為機器人增加輔助大持重輔助機械臂，從而完成更換橫擔、變壓器等重載作業(yè)任務(wù)。大持重液壓輔助機械臂研究一直也是機器人研究的熱點之一，目前國際上通常的工業(yè)機器人載荷比在1：10以下。受線路環(huán)境及絕緣斗臂車的約束，要求頂部工作臺整體重量不能過大，目前一般在500～600Kg。根據(jù)研究經(jīng)驗，輔助機械臂應(yīng)限制在100Kg以內(nèi)，因此要求輔助機械臂應(yīng)達到3:1左右的高載荷比，并且保證系統(tǒng)具有足夠的剛度，以保證末端控制精度和穩(wěn)定性?，F(xiàn)有的輔助機械臂結(jié)構(gòu)簡單，控制方式大都采用液壓閥手動控制，不帶有位置傳感器，所以不能進行位置精確控制。技術(shù)實現(xiàn)要素：為解決現(xiàn)有技術(shù)存在的不足，本發(fā)明公開了一種新型輔助機械臂的控制系統(tǒng)及其控制方法，該控制系統(tǒng)采用主從控制方式代替人的手臂完成10kV配電線路更換橫擔、更換變壓器等重載作業(yè)任務(wù)。為實現(xiàn)上述目的，本發(fā)明的具體方案如下：大持重液壓輔助機械臂的控制系統(tǒng)，包括主操縱桿和主控制器,所述的主控制器的輸入端連接模擬量采集模塊和數(shù)字量采集模塊，其輸出端設(shè)有模擬量輸出模塊，所述的主操縱桿與模擬量采集模塊、數(shù)字量采集模塊相連，所述的模擬量輸出模塊輸出的模擬量通過伺服放大器放大后傳送給多個液壓伺服閥，所述的多個液壓伺服閥控制輔助機械臂運動,且輔助機械臂的回轉(zhuǎn)油缸上的限位傳感器采集的信號反饋給主控制器,輔助機械臂的擺動油缸上的直線位移傳感器采集信號反饋給主控制器，主控制其再根據(jù)反饋信號對輔助機械臂進行精確控制。進一步的，所述的輔助機械臂，包括底座、大臂、前臂和手爪，在所述的底座上設(shè)有一個液壓擺動油缸，液壓擺動油缸驅(qū)動大臂底座在水平面內(nèi)180度旋轉(zhuǎn)，所述的大臂底座與大臂通過關(guān)節(jié)II相連，且大臂底座上鉸接一個驅(qū)動大臂做俯仰運動的俯仰油缸I，所述的大臂的末端通過關(guān)節(jié)III鉸接與其垂直的前臂，所述的前臂在俯仰油缸II的驅(qū)動下做俯仰運動；在前臂的末端通過關(guān)節(jié)IV連接一個手爪。進一步的，所述主操縱桿有X、Y兩個運動自由度，能同時進行X、Y方向的運動控制信號輸出；主操縱桿X自由度控制輔助機械臂的關(guān)節(jié)II；主操縱桿Y自由度控制輔助機械臂的關(guān)節(jié)III，從而確保機械臂末端的水平前后與垂直上下運動。進一步的，所述液壓擺動油缸上端與大臂底座連接，所述的擺動油缸能在水平面內(nèi)0～180度范圍內(nèi)旋轉(zhuǎn)。進一步的，所述大臂底座一端與液壓擺動油缸連接，另一端與大臂相連，且俯仰油缸I底座固定在大臂底座上；俯仰油缸I的活塞桿驅(qū)動大臂做俯仰運動。進一步的，所述俯仰油缸I帶有直線位移傳感器I，其通過伺服閥精確控制大臂的俯仰運動。進一步的，所述俯仰油缸II帶有直線位移傳感器II，其通過伺服閥精確控制前臂的俯仰運動。利用所述的控制系統(tǒng)對輔助機械臂的大臂、前臂的擺動角度的控制方法，如下：主操縱桿的模擬量數(shù)據(jù)輸出作為控制指令輸入給主控制器；主控制器控制機械臂運動；機械臂的運動位移通過直線位移傳感器反饋給主控制器，直線位移傳感器I、直線位移傳感器II的采集的數(shù)據(jù)進入主控制器后經(jīng)過姿態(tài)解析算法計算出伺服閥驅(qū)動數(shù)據(jù)分別驅(qū)動伺服閥I、伺服閥II使關(guān)節(jié)II、關(guān)節(jié)III運動到角度θ1、θ2，主控制器通過控制伺服閥的流量來控制液壓執(zhí)行器的動作位移，最終實現(xiàn)對機械臂的位置伺服控制。所述的姿態(tài)解析算法具體如下:設(shè)大臂長度為a1、前臂長度為a2，關(guān)節(jié)II、關(guān)節(jié)III的關(guān)節(jié)軸平行，關(guān)節(jié)II的旋轉(zhuǎn)角度為θ1；關(guān)節(jié)III的旋轉(zhuǎn)角度為θ2；以基座為坐標系為{A}；關(guān)節(jié)III所在的坐標為坐標系為{B}；先求解機械臂末端在基座坐標系{A}中的位置，然后再求解θ1、θ2的大小。具體如下：步驟(1)設(shè)機械臂末端在坐標系{A}中的位置為步驟(2)由已知條件求得機械臂末端在坐標系{B}中的位置為步驟(3)為關(guān)節(jié)II、關(guān)節(jié)III的大臂底座、大臂、前臂坐標系建立3×3奇次變換矩陣表示與前一個部件坐標系的關(guān)系；由已知條件可知其中：步驟(4)由步驟(1)、(2)、(3)可得：步驟(5)求得：步驟(6)求得機械臂末端在坐標系{A}中的位置根據(jù)機械臂桿長a1、a2和機械臂末端在坐標系{A}中的位置，即可求出相應(yīng)的θ1和θ2，解得：上述的均表示中間參數(shù)。本發(fā)明的有益效果：1.本發(fā)明自行設(shè)計的大持重液壓輔助機械臂控制系統(tǒng)經(jīng)實驗驗證，代替人的手臂完成10kV配電帶電更換橫擔、變壓器重載任務(wù)。2.輔助機械臂控制系統(tǒng)可以實現(xiàn)大臂俯仰和前臂俯仰兩軸聯(lián)動，末端可以進行世界坐標系運動。3.控制系統(tǒng)設(shè)計了一種新型的控制算法，算法實現(xiàn)簡單，避免采用復(fù)雜的運動學正反解算法。4.本系統(tǒng)主控制器采用成熟的PLC控制器，其抗干擾能力強，故障率低，并且兼容C語言編程，可以進行復(fù)雜的浮點運算功能，如：算術(shù)運算、邏輯運算和三角函數(shù)。附圖說明圖1是本發(fā)明輔助機械臂結(jié)構(gòu)圖；圖2是本發(fā)明結(jié)構(gòu)原理總框圖；圖3是本發(fā)明平面兩自由度串聯(lián)機械臂結(jié)構(gòu)圖；圖4是本發(fā)明輔助機械臂控制流程圖；圖5(1)、圖5(2)、圖5(3)、圖5(4)是本發(fā)明主操縱桿示意圖；圖中:1底座，2擺動油缸，3大臂底座，4大臂油缸，5大臂，6前臂油缸，7前臂，8手爪，9關(guān)節(jié)I，10關(guān)節(jié)II，11關(guān)節(jié)III，12關(guān)節(jié)IV。具體實施方式：下面結(jié)合附圖對本發(fā)明進行詳細說明：本發(fā)明所控制的機械臂包括底座、大臂、前臂和手爪，在所述的底座上設(shè)有一個液壓擺動油缸，液壓擺動油缸驅(qū)動大臂底座在水平面內(nèi)180度旋轉(zhuǎn)，所述的大臂底座與大臂通過關(guān)節(jié)II10相連，且大臂底座上鉸接一個驅(qū)動大臂做俯仰運動的俯仰油缸I，所述的大臂的末端通過關(guān)節(jié)III11鉸接與其垂直的前臂，所述的前臂在俯仰油缸II的驅(qū)動下做俯仰運動；在前臂的末端通過關(guān)節(jié)IV12連接一個手爪。具體的參見圖1，本發(fā)明的控制系統(tǒng)針對的帶電作業(yè)用輔助機械臂，包括底座1，擺動油缸2，大臂底座3，大臂油缸4，大臂5，前臂油缸6，前臂7，手爪8。擺動油缸2通過螺釘連接到底座1上，大臂底座3通過螺釘一端連接和擺動油缸2連接在一起，另一端連接和大臂5相連接，大臂油缸4通過螺釘一端連接到大臂油缸座3，另一端連接在大臂5上，前臂7通過螺釘和大臂5連接在一起，前臂油缸6通過螺釘一端連接在大臂5上，另一端連接在前臂7上，手爪8通過螺釘連接在前臂7上。其中關(guān)節(jié)I9指的是擺動油缸2與大臂底座3的旋轉(zhuǎn)關(guān)節(jié)；所述的大臂底座與大臂通過關(guān)節(jié)II10相連；所述的大臂的末端通過關(guān)節(jié)III11鉸接與其垂直的前臂；前臂的末端通過關(guān)節(jié)IV12連接一個手爪。進一步的，液壓擺動油缸上端與大臂底座連接，所述的擺動油缸能在水平面內(nèi)0～180度范圍內(nèi)旋轉(zhuǎn)。進一步的，大臂底座一端與液壓擺動油缸連接，另一端與大臂相連，且俯仰油缸I底座固定在大臂底座上；俯仰油缸I的活塞桿驅(qū)動大臂做俯仰運動。進一步的，俯仰油缸I帶有直線位移傳感器I，其通過伺服閥精確控制大臂的俯仰運動。進一步的，俯仰油缸II帶有直線位移傳感器II，其通過伺服閥精確控制前臂的俯仰運動。針對上面所述的機械臂，其控制系統(tǒng)如圖2所示，本發(fā)明中新型輔助機械臂控制系統(tǒng)，包括主操縱桿、PLC主控制器、回轉(zhuǎn)油缸伺服閥、大臂油缸伺服閥、前臂油缸伺服閥、手爪旋轉(zhuǎn)油缸伺服閥、手爪開合油缸伺服閥、回轉(zhuǎn)油缸、大臂油缸、前臂油缸、手爪旋轉(zhuǎn)油缸、手爪開合油缸、傳感器系統(tǒng)。主操縱桿的模擬量數(shù)據(jù)輸出作為控制指令輸入給PLC主控制器，同時機械臂關(guān)節(jié)的運動位移通過高精度直線傳感器反饋給PLC主控制器，兩路模擬量進入主控制器后經(jīng)過姿態(tài)解析算法計算出伺服閥驅(qū)動數(shù)據(jù)，PLC主控制器通過控制伺服閥的流量來控制液壓執(zhí)行器的動作位移，最終實現(xiàn)對機械臂的位置伺服控制。所述主操縱桿有X、Y兩個運動自由度，能同時進行X、Y方向的運動控制信號輸出，數(shù)據(jù)信號為-10V～10V的模擬電壓。操縱桿X自由度控制輔助機械臂的基座關(guān)節(jié)左右旋轉(zhuǎn)；操縱桿Y自由度控制輔助機械臂大臂和前臂的兩軸聯(lián)動，從而確保機械臂末端的水平前后與垂直上下運動，具體運動形式由扳機開關(guān)S4控制。當扳機S4摁下時，Y自由度控制輔助機械臂末端作水平前后運動；當扳機S4松開時，Y自由度控制輔助機械臂末端作垂直上下運動。按鈕S1還可以控制機械臂的單軸運動，當選擇單軸運動時時，扳機開關(guān)S4摁下，則大臂作單軸旋轉(zhuǎn)運動，S4不摁下，則前臂作單軸旋轉(zhuǎn)運動。PLC主控制器采用基恩士PLCKV-N24DT型，運行速度10000步/ms，可以采用梯形圖+腳本(支持C語言模式)編程。模擬量采集模塊采用1個四路模擬量輸入擴展模塊KV-NC4AD，采集電壓范圍0～10V，轉(zhuǎn)換速度80μs/ch。模擬量輸出模塊采用3個兩路模擬量輸出擴展模塊KV-NC2DA，，輸出電壓范圍0～10V，轉(zhuǎn)換速度80μs/ch。主操縱桿有X、Y兩個運動自由度，數(shù)據(jù)信號為0～10V的模擬電壓，并具有S1、S2、S3、S4的數(shù)字開關(guān)。直線位移傳感器選用非接觸式磁致伸縮位移傳感器BTL6-A500-E2/E28-KA，供電電壓10～30V，輸出電壓范圍0～10V。液壓伺服閥采用中船重工CSDY1射流管伺服閥，供電電流0～20ma。參見圖3，為大臂和前臂運動的分析簡圖，由兩個關(guān)節(jié)組成，大臂和前臂長度分別a1和a2，兩旋轉(zhuǎn)關(guān)節(jié)軸平行，關(guān)節(jié)II運動范圍θ1為85°～95°，關(guān)節(jié)III運動范圍θ2為75°～135°，以基座為坐標系為{A}；關(guān)節(jié)III所在的坐標為坐標系為{B}；先求解機械臂末端在基座坐標系{A}中的位置，然后再求解θ1、θ2的大小。具體的設(shè)計參數(shù)如表1所示。表1平面兩自由度串聯(lián)機械臂連桿的設(shè)計參數(shù)iθ關(guān)節(jié)變量角度連桿長度液壓缸參數(shù)1θ185°～95°1250mm380mm(行程100mm)2θ275°～135°1800mm680mm(行程400mm)求解機械臂末端在坐標系{A}中的位置過程如下：(1)設(shè)機械臂末端在坐標系{A}中的位置為(2)由已知條件可求得p點在坐標系{B}中的位置為(3)由已知條件可知其中：(4)由(1)、(2)、(3)可得：(5)所以：給出機械臂末端在坐標系{A}中的位置根據(jù)機械臂桿長a1、a2和公式(5)即可求出相應(yīng)的θ1和θ2，解得：復(fù)位時給定機器人末端位置：參見圖4，大持重液壓輔助機械臂處于自動運動方式時，按照事先編寫好的運動控制程序自動循環(huán)執(zhí)行，直到按下控制面板上的控制按鍵。總控系統(tǒng)操作例程，系統(tǒng)上電按鈕按下，下一步系統(tǒng)復(fù)位，讀取線性傳感器數(shù)據(jù)，接收控制指令，如果控制指令是單軸運動，根據(jù)機械臂單軸運動θ1和θ2數(shù)據(jù)計算機械臂末端位置(x,y),如果兩軸聯(lián)動，根據(jù)機械臂末端坐標(x,y)通過方程組(1)計算θ1和θ2。執(zhí)行運行程序完成后，返回讀取線性傳感器數(shù)據(jù)循環(huán)運行。參見圖5(1)-圖5(4)，該操縱桿有X、Y兩個運動自由度，能同時進行X、Y方向的運動控制信號輸出，數(shù)據(jù)信號為-10V～10V的模擬電壓。操縱桿X自由度控制輔助機械臂的基座關(guān)節(jié)左右旋轉(zhuǎn)；操縱桿Y自由度控制輔助機械臂大臂和前臂的兩軸聯(lián)動，從而確保機械臂末端的水平前后與垂直上下運動，具體運動形式由扳機開關(guān)S4控制。當扳機S4摁下時，Y自由度控制輔助機械臂末端作水平前后運動；當扳機S4松開時，Y自由度控制輔助機械臂末端作垂直上下運動。按鈕S1還可以控制機械臂的單軸運動，當選擇單軸運動時時，扳機開關(guān)S4摁下，則大臂作單軸旋轉(zhuǎn)運動，S4不摁下是，前臂作單軸旋轉(zhuǎn)運動。本申請中，PLC主控制器采用基恩士PLCKV-N24DT型，運行速度10000步/ms，可以采用梯形圖+腳本(支持C語言模式)編程。所述模擬量采集模塊采用1個四路模擬量輸入擴展模塊KV-NC4AD，采集電壓范圍0～10V，轉(zhuǎn)換速度80μs/ch。所述模擬量輸出模塊采用3個兩路模擬量輸出擴展模塊KV-NC2DA，，輸出電壓范圍0～10V，轉(zhuǎn)換速度80μs/ch。主操縱桿有X、Y兩個運動自由度，數(shù)據(jù)信號為0～10V的模擬電壓，并具有S1、S2、S3、S4的數(shù)字開關(guān)。直線位移傳感器選用非接觸式磁致伸縮位移傳感器BTL6-A500-E2/E28-KA，供電電壓10～30V，輸出電壓范圍0～10V。液壓伺服閥采用中船重工CSDY1射流管伺服閥，供電電流0～20ma。上述雖然結(jié)合附圖對本發(fā)明的具體實施方式進行了描述，但并非對本發(fā)明保護范圍的限制，所屬領(lǐng)域技術(shù)人員應(yīng)該明白，在本發(fā)明的技術(shù)方案的基礎(chǔ)上，本領(lǐng)域技術(shù)人員不需要付出創(chuàng)造性勞動即可做出的各種修改或變形仍在本發(fā)明的保護范圍以內(nèi)。當前第1頁1 2 3