高壓輸電線路巡檢機器人運動控制研究

高壓輸電線路巡檢機器人運動控制研究

1腳手架結(jié)構(gòu)的設計作為能源輸送的動脈，高壓供電線路的安全穩(wěn)定對生產(chǎn)和生活非常重要。為了保證能源的安全傳輸，我們必須確保高壓供電線路在運行時的安全穩(wěn)定運行。高壓線路的監(jiān)測方法主要是手動監(jiān)測。現(xiàn)場輸電線的分布復雜性使得很難進行監(jiān)控。人工監(jiān)控存在許多不便，一些巡邏任務不完成，安全隱患。自動駕駛機器人旨在克服這些障礙，彌補人工駕駛的不足。輸電線路的巡檢任務主要是檢查線路是否存在缺陷及桿塔、導線、避雷線、絕緣子和金具的狀態(tài)等.機器人要完成巡檢任務,不僅應該能在輸電線上自由快速地行走,還應該能夠自主的跨越諸如防震錘、耐張線夾、懸垂線夾等各種障礙.為了跨越這些障礙,我們將巡線機器人的機構(gòu)設計成三體結(jié)構(gòu),如圖1所示,前后兩個單體設計成手臂結(jié)構(gòu),中間單體是機器人的本體.在越障時,始終有兩個單體懸掛在線路上,這樣可以保證機器人運行的穩(wěn)定性.單體的驅(qū)動輪保證機器人可以在線路上行走,它設計成中間開合方式,在遇到障礙時,通過開合關(guān)節(jié)可以打開行走輪,使其從線路上脫開,從而越過障礙.每個單體都有一個平移關(guān)節(jié),以保證在越障時手臂可以根據(jù)要求進行伸縮.單體之間有兩個連接關(guān)節(jié),擺臂關(guān)節(jié)可使手臂在水平方向上旋轉(zhuǎn),轉(zhuǎn)臂關(guān)節(jié)是為了保證兩臂可以完成前伸抱線的動作.這兩個關(guān)節(jié)通過抱閘控制設計成剛性柔性可轉(zhuǎn)換關(guān)節(jié),在越障時使之為剛性,可以完成規(guī)劃的動作;在正常行走時使之變?yōu)槿嵝?這樣在重力的作用下,3個單體就可以自主調(diào)節(jié),使3個行走輪較好地適應線路的形狀,保證了機器人運行的穩(wěn)定性.要使此機構(gòu)在線路上運行,就要對其進行運動學分析.巡線機器人跟普通的工業(yè)機器人不同,它是懸掛在輸電線路上的,當機器人一個單體運動時,在重力的作用下,另兩個單體也會隨之運動,以使機器人整體達到平衡.可見此系統(tǒng)是一個受約束系統(tǒng),重力平衡是該系統(tǒng)的約束條件.本文就是分析在這種約束條件下的逆運動學的求解方法.封閉形式的逆運動學解只是特定類型的結(jié)構(gòu)簡單的工業(yè)機器人才可能得到,對普通機器人來講,很難直接求得其逆運動學的封閉解.求解系統(tǒng)的逆運動學,通常有兩種方法,一種為轉(zhuǎn)置Jacobian矩陣法,另一種為基于優(yōu)化的方法.轉(zhuǎn)置Jacobian矩陣法求解方便,計算量小,但它要求必須能夠求出系統(tǒng)的Jacobian矩陣.在基于優(yōu)化的方法中,循環(huán)坐標下降法(Cyclic-CoordinateDescent,CCD)是一種循環(huán)迭代方法,其原理簡單,對系統(tǒng)沒有特殊要求,而且它對系統(tǒng)在奇異位姿附近時的求解也是非常有效的.在本文中,由于機器人運動學方程中含有約束,Jacobian矩陣不容易計算,所以采用的是基于CCD的迭代算法.本文根據(jù)機器人運動的具體過程分為兩種基本情況分別進行討論,一種情況是前臂脫線,本體和后臂掛線時,對其進行了模型的近似與簡化,經(jīng)分析,其約束是對懸點的重力總力矩為0.另一種情況是本體脫線,兩臂掛線時,其約束有并聯(lián)約束(幾何約束)和力學約束兩種.幾何約束是運動維持兩懸點的位置不變,力學約束是對過兩懸點的鉛垂面的重力的總力矩為0.在找到了約束條件之后,利用微分扭轉(zhuǎn)法求得了運動學方程,然后結(jié)合末端位姿要求建立方程組.為了求得逆運動學的數(shù)值解,采用了優(yōu)化方法和理論求解相結(jié)合的方式,得到了迭代CCD算法,并對實際問題進行了求解仿真,驗證了機構(gòu)設計的可行性.2巡線機器人的越障控制三單體巡線機器人在正常行走時,把單體間連接變?yōu)槿嵝?3個單體在重力的作用下,自主地適應高壓電線的形狀,需要控制的只是3個行走電機的同步問題,而不需要對機器人進行運動學分析與運動控制.巡線機器人在越障(包括防震錘、耐張線夾、懸垂線等)時,主要可分3步完成:首先本體和后臂抱線停止,前臂脫線、下移、前伸、抱線;然后前后臂抱線停止,中間本體脫線,前后臂驅(qū)動機器人前行、抱線停止,本體前伸抱線;然后前臂和本體抱線停止,后臂脫線,機器人前行、停止、后臂抱線,完成越障.在此過程中,機器人的基本姿態(tài)有2種:一種是中間本體和一只臂掛線,另一只臂脫線運動;另一種是兩臂掛線,中間本體脫線運動.所有的越障運動都可以由這兩個基本姿態(tài)完成,本文重點對這兩種姿態(tài)下巡線機器人的運動學進行分析.2.1工作原理分析在越障時,行走輪不改變手臂末端的位姿,而開合關(guān)節(jié)只是保證脫線抱線動作能夠順利進行,也不會改變手臂末端的位姿,所以可以把開合關(guān)節(jié)和行走輪作為抓手處理,不作為運動關(guān)節(jié)考慮.在這種情況下,此系統(tǒng)是一個并聯(lián)串聯(lián)機構(gòu),對其進行力學分析仍然比較復雜,為了進一步簡化分析模型,在前臂運動時,松開本體和后臂的連接關(guān)節(jié)抱閘,使其變?yōu)槿嵝?這樣在分析時如果不考慮連接關(guān)節(jié)的阻力的話,就可以只對前臂和本體進行力學分析,而不考慮后臂的影響,進而求取運動約束,進行前臂的運動學分析.如圖2所示,機器人運動的基坐標系應該建立在相對于電線不動的本體懸點處,而工具坐標系應該建立在作為抓手處理的前臂輪處.在懸點處,由于前臂其他關(guān)節(jié)的運動,使得機器人在懸點處有兩個自由旋轉(zhuǎn)關(guān)節(jié),即機器人可以繞x軸和y軸自由旋轉(zhuǎn).2.1.1改進的前向運動變換方程在運動分析時,采用微分扭轉(zhuǎn)(twist)的分析方法,這種方法的優(yōu)點在于不需要在每個關(guān)節(jié)都建立坐標系和尋找機器人的D-H參數(shù),而只需要找到每個關(guān)節(jié)的微分扭轉(zhuǎn)ξ.對旋轉(zhuǎn)關(guān)節(jié)對平移關(guān)節(jié),其中ω為旋轉(zhuǎn)關(guān)節(jié)的旋轉(zhuǎn)軸方向矢量,q為旋轉(zhuǎn)軸上的任意一點坐標矢量,而v為平移關(guān)節(jié)的平移軸方向矢量.然后利用微分扭轉(zhuǎn)指數(shù)公式,就可得到工具坐標的前向運動變換矩陣gst(θ)=e?ξ1θ1e?ξ2θ2∧e?ξnθngst(0)?(1)其中g(shù)st(θ)為關(guān)于關(guān)節(jié)變量θ從基坐標系到工具坐標系的齊次變換矩陣,gst(0)為關(guān)于關(guān)節(jié)變量都為0時的齊次變換矩陣,ξi為各關(guān)節(jié)的微分扭轉(zhuǎn),θi為各個關(guān)節(jié)變量值,運算符“∧”為從6維向量到4×4矩陣的運算.對于如圖2所示的巡線機器人簡化模型,可得各個微分扭轉(zhuǎn)為ξ1=,ξ2=,ξ3=,ξ4=[-d100001],ξ5=[0-(l1+l2)d1+d2100],ξ6=.而初始態(tài)的齊次變換陣為gst(0)=[1000010-(d1+d2)001l3+l4-l1-l20001].2.1.2運動約束條件前臂脫線之后,系統(tǒng)要保持平衡,機器人重心應在懸點正下方,即在懸點處重力力矩為0,則有∑?Gi×?Li=0其中Li為重力Gi的力臂.不失一般性,假設機器人的每段連接臂質(zhì)量分別都是均勻的,即重心位于在其幾何中心上,則各重心的力臂矢量為[?L11]=e?ξ1θ1e?ξ2θ2[α11],[?L21]=e?ξ1θ1e?ξ2θ2e?ξ3θ3[α21],[?L31]=e?ξ1θ1e?ξ2θ2e?ξ3θ3e?ξ4θ4[α31],[?L41]=e?ξ1θ1e?ξ2θ2e?ξ3θ3e?ξ4θ4e?ξ5θ5[α41],[?L51]=e?ξ1θ1e?ξ2θ2e?ξ3θ3e?ξ4θ4e?ξ5θ5e?ξ6θ6[α51],[?L61]=e?ξ1θ1e?ξ2θ2e?ξ3θ3e?ξ4θ4e?ξ5θ5e?ξ6θ6[α61].其中α1=[00-l12]Τ,α2=[00-l1-l22]Τ,α3=[0-d1-d22-l1-l22]Τ,α4=[0-d1-d2-l1-l2+l32]Τ,α5=[0-d1-d2-l1-l2+l3+l42]Τ,α6=[0-d1-d2-l1-l2+l3+l4]Τ.而重力G為Gi=[00mig]Τ=migΤ?(i=1,2,?,6).另取?L′i使得即則有∑m1g×e?ω1θ1e?ω2θ2?L′i=g×e?ω1θ1e?ω2θ2∑mi?L′i=0.對于R=e?ω1θ1e?ω2θ2∈SΟ(3)應有R(w×v)=(Rw)×(Rv),對上式則有(e?ω1θ1e?ω2θ2)-1×∑mi?L′i=(e-?ω2θ2e-?ω1θ1)×∑mi?L′i=0.即[-c1s2s1c1c2]×∑mi?L′i=0.(2)式(2)即為自由關(guān)節(jié)與受控關(guān)節(jié)的運動約束條件,L?′i是θ3,θ4,θ5,θ6的函數(shù),與θ1,θ2無關(guān).則若則有{θ2=arctan(-t1t3),θ1=arctan(t2t12+t32).(3)2.1.3基于迭代cd算法的逆運動學求解算法從基坐標系到工具坐標系的齊次變換矩陣為gst(θ)=eξ^1θ1eξ^2θ2eξ^3θ3eξ^4θ4eξ^5θ5eξ^6θ6gst(0)此系統(tǒng)雖然有6個活動關(guān)節(jié),但由于有兩個是受約束關(guān)節(jié),是一個欠自由度系統(tǒng),所以工具末端的位姿并不是任意的,也就是如果給定完全確知的末端位姿,其逆運動學不一定有解.而實際上機器人在運動過程中末端位姿并不是嚴格確定的,對末端的要求包括對末端位置的要求和末端行走輪輪槽的取向的要求.這是因為機器人在越障的過程中,只需要保證前輪的位置不與障礙物發(fā)生碰撞,至于其姿態(tài)沒有特殊的要求,而在掛線的過程中,只需要保證前輪的輪槽和電線在一個平面內(nèi)即可,而對其具體姿態(tài)不要求確知.可見確知量為輪槽的取向s和輪的位置pd,其中輪槽的取向應在工具坐標系的yoz平面內(nèi),即與x軸垂直.若齊次變換陣末端目標陣,則應有s·n=0,p=pd而由于θ1和θ2具有如式(2)的約束關(guān)系,在進行逆運動學求解時,關(guān)節(jié)變量θi之間具有很強的耦合關(guān)系,很難直接求得關(guān)節(jié)變量的理論值.所以在進行逆運動學求解時,需要利用優(yōu)化的方法求解.在此采用迭代CCD法來進行計算,它采用理論求解和數(shù)值運算相結(jié)合的方式,提高了運算效率.CCD算法的求解思想是把多元優(yōu)化問題轉(zhuǎn)化為一元函數(shù)的優(yōu)化問題,是在保持其他關(guān)節(jié)參量不變的前提下,允許一個關(guān)節(jié)參量可變,來求解使得誤差范數(shù)最小的這個可變關(guān)節(jié)參量的值,然后更換可變關(guān)節(jié),同樣用這種方法計算,如此不斷更替循環(huán)可變關(guān)節(jié),直至得解.對旋轉(zhuǎn)關(guān)節(jié),所求的關(guān)節(jié)參量滿足{(k1-k2)sinθi+k3cosθi=0,(k1-k2)cosθi-k3sinθi＜0?(4)其中k1=wp(ipd?ωi)(ipθ?ωi)+w0[(n?ωi)(nθ?ωi)+(o?ωi)(oθ?ωi)+(a?ωi)(aθ?ωi)],k2=wp(ipd?ipθ)+w0(n?nθ+o?oθ+a?aθ),k3=ωi?(wp(ipθ×ipd)+w0(n×nθ+o×oθ+a×aθ)).其中wp和w0為末端位置和姿態(tài)的權(quán)值,ipd=pd-pi,ipθ=pθ-pi,pi為第i個關(guān)節(jié)原點.對平移關(guān)節(jié),所求的關(guān)節(jié)參量滿足θi=(ipd-ipθ)?ωi.(5)CCD算法是用來解決關(guān)節(jié)間無約束的串聯(lián)機器人的逆運動學的.而在此運動模型中,由于θ1和θ2是不能直接控制的,而且是受θ3,θ4,θ5,θ6約束的,所以不可能實現(xiàn)保持其他關(guān)節(jié)量不變而只改變一個關(guān)節(jié),所以CCD法不能直接運用.為了求解可控關(guān)節(jié)量,可以先假定受約束關(guān)節(jié)保持不變,利用CCD法求解可變關(guān)節(jié)量,然后再根據(jù)約束求出關(guān)節(jié)改變后的受約束關(guān)節(jié)的值,再用CCD法求解另一個可控關(guān)節(jié)量,此過程循環(huán)迭代,最終可得到逆運動學的解.對此機器人運動模型,迭代CCD法求解逆運動學的具體過程為:先由式(3),求得當其他關(guān)節(jié)量均為0時的θ1和θ2,然后保持θ1,θ2,θ4,θ5,θ6的值不變,代入式(1),由式(4)或(5)求得θ3;將所求的θ3代入式(3),求得θ1和θ2,保持θ1,θ2,θ3,θ5,θ6不變,代入式(1),由式(4)或(5)求得θ4;依此循環(huán)進行下去,直至誤差范數(shù)∥eξ^1θ1eξ^2θ2eξ^3θ3eξ^4θ4eξ^5θ5eξ^6θ6g(θ)-gd∥＜ε?其中誤差范數(shù)定義為∥gst(θ)-gd∥2=∥pθ-p∥2+(nθ?n-1)2.CCD法是收斂的,由下一節(jié)的實驗結(jié)果可知這種迭代CCD算法也是收斂的.而這種循環(huán)迭代的坐標下降法,與其他的優(yōu)化算法(如遺傳算法)比起來,運算量大大減小,運算效率得到了提高,通過下節(jié)的實驗結(jié)果可以看出,算法的收斂速度也是比較快的,適合于實時運動控制.2.2基于fpga的逆運動學中間本體脫線之后,前后兩臂要把本體托住,則中間連接必須都變成剛性.這種位姿下機器人各運動關(guān)節(jié)既受到力學上的約束也受到幾何上的約束.在力學約束上,機器人的重心要保持在兩個懸點的連線的正下方,在幾何約束上,由于兩個懸點在電線上是固定不動的,在關(guān)節(jié)在運動時必須是在保持兩懸點間距離不變的前提下進行的.這種情況下,機器人機構(gòu)是一個并串聯(lián)機構(gòu),則關(guān)節(jié)變量間所存在的并聯(lián)約束就是它們的幾何約束.如圖3所示,對于并聯(lián)約束,從前臂關(guān)節(jié)得到的齊次變換矩陣為gs1t(θ1)=eξ^11θ11eξ^12θ12eξ^13θ13eξ^14θ14eξ^15θ15eξ^6θ6gs1t(0).(6)從后臂關(guān)節(jié)得到的齊次變換矩陣為gs2t(θ2)=eξ^21θ21eξ^22θ22eξ^23θ23eξ^24θ24eξ^25θ25eξ^6θ6gs2t(0).(7)由于機構(gòu)的并聯(lián)關(guān)系,從前后臂得到的變換矩陣應該只相差一個齊次變換陣,所以其約束表達式為對于力學約束,重力關(guān)于兩個懸點確定的直線軸axis的總力矩應為0,因此有∑Gi×L?i=0.其中L?i為重力作用點到axis軸所確定的鉛垂面的法矢量.實際上此力學約束所給出的只有一個方程,限于篇幅,此方程的推導過程和具體表達式不再贅述,可設其表示為類似于2.1的末端位姿矩陣要求,利用s和pd進行逆運動學的求解.在這里,各關(guān)節(jié)變量之間是一種更強的耦合關(guān)系,很難把這種耦合去掉,不能直接求得理論解.若采用遺傳算法、神經(jīng)網(wǎng)絡等方法,由于此處為高維空間的求解,算法的收斂速度慢,復雜度高.此處仍采用CCD迭代法進行求解.與2.1不同的是,它屬于并聯(lián)機構(gòu),所以不能滿足CCD法所要求的一個關(guān)節(jié)改變而其他關(guān)節(jié)不變的條件,所以迭代CCD法不能直接運用.為了對此種情況下的機器人進行逆運動學求解,可以將CCD法進行進一步的改變,先不考慮力學約束和并聯(lián)約束,分別從兩只手臂出發(fā)運用CCD法進行求解,再由約束求解受約束關(guān)節(jié)的值,這樣循環(huán)迭代,直到得到逆運動學的解.其具體過程為:先固定θ11～θ15,θ21～θ25為當前值不變,代入式(6)和(7),用CCD法求解出θ6,再由約束(8)(9)求得θ11,θ12,θ21,θ22,再分別用CCD法求解出θ13～θ15和θ23～θ25,再由約束求出θ11,θ12,θ21,θ22,然后再用CCD法求得θ6.依此進行下去,直至‖g(θ)-gd‖2+‖g(θ2)-gd‖2<ε.為了驗證迭代CCD法對逆運動學求解的有效性,對實際中的巡線機器人進行了前臂的逆運動學的數(shù)值求解計