湯錦慧 伍發(fā)元 辛建波 鄭曉東 薛霜思

(1.國家電網(wǎng) 江西省電力有限公司 電力科學研究院,南昌 330096;2.國家電網(wǎng) 江西省電力有限公司,南昌 330096;3.西安交通大學 電氣工程學院,西安 710049)

變電站是電力系統(tǒng)的重要組成部分,站內(nèi)變壓器、電容器等高低壓電力設備在故障時會引起局部過熱現(xiàn)象,如果處理不及時會進一步造成火災,從而產(chǎn)生巨大的經(jīng)濟損失[1-2].若能對變電站的關(guān)鍵設備及時降溫或滅火,不僅會大大降低變電站火災風險,而且能對其安全運行提供更好的保障.變電站傳統(tǒng)消防手段以固定式噴淋為主,但由于安裝位置比較固定,所以其覆蓋面及靈活性不足,無法對超溫點或起火點進行準確作業(yè),所以不能滿足變電站防災減災的實際需求.

由于機械臂操作靈活性且工作穩(wěn)定性好,所以以其為核心執(zhí)行單元的智能消防系統(tǒng)可以實現(xiàn)精確的消防作業(yè).機械臂精準控制一直以來都是工業(yè)機器人技術(shù)領(lǐng)域的研究熱點,可以分為機械臂位姿控制與末端執(zhí)行器控制兩類問題.德納維和哈登伯格首先提出了機械臂Denavit-Hartenberg(D-H)建模法[3].這一方法從剛體運動學和幾何學角度出發(fā),主要從正向運動學計算與逆向運動學控制兩個方面,比較系統(tǒng)地闡述了機械臂位姿控制方法.其中,逆向運動學控制問題是機械臂運動控制的核心,其在滿足機械臂實體一系列約束條件下,追求控制規(guī)劃的時間最優(yōu),或部分時間與能量最優(yōu).為了實現(xiàn)控制規(guī)劃最優(yōu)[4-5],支持向量機、神經(jīng)網(wǎng)絡等一些智能優(yōu)化算法被引入到機械臂控制中[6],不僅極大地提高了規(guī)劃效率,而且推動了機械臂建模理論的完善[7-8].除了對機械臂傳統(tǒng)控制算法進行優(yōu)化,一些學者對人類手臂的控制進行了建模[9-11],并在此基礎上研究了模擬神經(jīng)回路與激素調(diào)節(jié)的機械臂控制方法[12]；此外,采用新型材料制造的柔性機械臂建模與控制也是研究焦點之一[13-14].近年來,機械臂控制技術(shù)取得了較大進展[15-16],且在航天器維修[17]、空間站管理[18]、仿生控制[19]、醫(yī)療[20]等工作中得到了進一步應用.但如果將機械臂直接用于變電站智慧消防系統(tǒng)中,則在實際運行中存在的累積誤差、機械結(jié)構(gòu)塑性形變等一系列問題,會直接影響機械臂的控制精度,進而會導致變電站消防系統(tǒng)運行效率降低.

針對上述問題,本文提出了一種高精度機械臂運動學建模方法.首先將機械臂結(jié)構(gòu)簡化為剛體連桿模型,基于機械臂D-H 建模法,以數(shù)學表達式描述機械臂在空間中的狀態(tài)(位姿)與運動(軌跡)；其次,對實際場景所采用的四自由度機械臂進行了正向與逆向運動學分析,并提出一種利用機械臂連桿模型幾何約束關(guān)系,直接求解四自由度機械臂逆運動學問題的算法,最后的實驗結(jié)果驗證了所建模型的精確性.

1 機械臂建?；A

機械臂運動學建模旨在將復雜的機械臂空間結(jié)構(gòu)抽象為一系列參數(shù),以運動學方法描述機械臂連桿、關(guān)節(jié)和末端執(zhí)行器的位姿,進而采用數(shù)學計算方式對機械臂的運動進行解算,實現(xiàn)機械臂的運動軌跡規(guī)劃與位姿控制.

D-H 機械臂建模法[3]是一種以剛體運動學為基礎,在三維空間坐標系下對機械臂抽象化建模的系統(tǒng)方法.這一方法通過限制原點位置與X軸方向,用4個參數(shù)即可表達關(guān)節(jié)之間六自由度的坐標變換.對最一般情況的n(n≥1)關(guān)節(jié)機械臂進行研究.自基座到末端執(zhí)行器,對關(guān)節(jié)進行編號為Axis1,Axis2,Axis3,…,Axisi-1,Axisi,…,Axisn,其中i∈[1,n]；對連桿進行編號為link1,link2,link3,…,linki-1,linki,…,linkn,其中i∈[1,n].此外,機械臂基座記為link0.關(guān)節(jié)結(jié)構(gòu)分為兩種：旋轉(zhuǎn)關(guān)節(jié)與滑動關(guān)節(jié),對應的運動軸線分別被稱作旋轉(zhuǎn)軸和平移軸,旋轉(zhuǎn)軸是旋轉(zhuǎn)關(guān)節(jié)旋轉(zhuǎn)所圍繞的軸線,平移軸是滑動關(guān)節(jié)滑動所沿的直線.

截取Axisi-1、linki-1與Axisi部分進行研究.連桿linki-1用兩個參數(shù)表示：連桿長度a i-1與連桿轉(zhuǎn)角αi-1.關(guān)節(jié)Axisi(Axisi-1)用兩個參數(shù)表示：關(guān)節(jié)轉(zhuǎn)角θi(θi-1)與連桿偏移d i(d i-1).4種參數(shù)定義如下：

(1)連桿長度：兩個關(guān)節(jié)軸線之間的距離；

(2)連桿轉(zhuǎn)角：某關(guān)節(jié)軸線相對于前一關(guān)節(jié)軸線繞其公共法線旋轉(zhuǎn)的角度；

(3)關(guān)節(jié)轉(zhuǎn)角：某關(guān)節(jié)與后一關(guān)節(jié)公共法線,和此關(guān)節(jié)與前一關(guān)節(jié)公共法線,繞此關(guān)節(jié)軸線旋轉(zhuǎn)的角度；

(4)連桿偏移：某關(guān)節(jié)與后一關(guān)節(jié)公共法線,和此關(guān)節(jié)與前一關(guān)節(jié)公共法線,沿此關(guān)節(jié)軸線方向的距離.

上述4種參數(shù)即為機械臂的D-H 參數(shù).此外,常引入?yún)?shù)σi(σi-1),用以區(qū)分關(guān)節(jié)i(i-1)是旋轉(zhuǎn)關(guān)節(jié)或是滑動關(guān)節(jié).

為將上述參數(shù)以數(shù)學表達式形式表示,在機械臂各關(guān)節(jié)處建立三維笛卡爾直角坐標系O1X1Y1Z1,O2X2Y2Z2,…,O i-1X i-1Y i-1Z i-1,O i X i Y i Z i,…,O n X n Y n Z n,其中i∈[1,n].此外,機械臂基座坐標系O0X0Y0Z0可視為機械臂的公共坐標系,統(tǒng)一記為OXYZ.以O i X i Y i Z i為例,說明坐標系建立方法：Z i為關(guān)節(jié)i軸線方向；X i為Z i-1軸線與Z i軸線的公共法線,正方向為Z i-1指向Z i,最后由右手螺旋法則確定Y i方向.特別的,當相鄰兩關(guān)節(jié)的Z軸平行,則有無數(shù)條公垂線,可挑選與前一關(guān)節(jié)公垂線共線的一條作為X軸.

2 運動學計算求解與控制

2.1 機械臂運動學計算求解方法

機械臂正向運動學計算是指已知機械臂全部關(guān)節(jié)運動位矢(旋轉(zhuǎn)角度及方向),解算機械臂末端位置與姿態(tài).機械臂正向運動學計算有且僅有唯一解,求解過程如下.

基于參數(shù)在笛卡爾直角坐標系中的定義,可以進一步求解O i-1X i-1Y i-1Z i-1與O i X i Y i Z i相鄰兩坐標系間的變換關(guān)系.由坐標系O i-1X i-1Y i-1Z i-1轉(zhuǎn)換到坐標系O i X i Y i Z i的轉(zhuǎn)換矩陣見式(1).

其中：R(z,θi)和R(x,αi)表征旋轉(zhuǎn)變換,T(0,0,d i)和T(a i,0,0)表征平移變換,具體見式(2)～(5).

求解機械臂末端位置和姿態(tài),就是求解機械臂末端連桿linkn在基座坐標系OXYZ下的空間姿態(tài).取機械臂末端連桿linkn上一質(zhì)點P n(x n,y n,z n),將其在O n X n Y n Z n下的坐標依次變換到O n-1X n-1Y n-1Z n-1、O n-2X n-2Y n-2Z n-2、O n-3X n-3Y n-3Z n-3…坐標系中,最終坐標將變換到O0X0Y0Z0坐標系,即OXYZ坐標系中,如此便推導出linkn上質(zhì)點P在機械臂公共參考系中坐標P(x,y,z)的表達式,見式(6).

其中,

方程(7)稱作機械臂運動學方程.只要任取末端連桿上兩個不同質(zhì)點,用上述方法求解其在公共參考系中的坐標表示,即可表示出機械臂末端位姿.

2.2 機械臂逆向運動學控制

機械臂逆向運動學控制是指,已知機械臂末端執(zhí)行器位姿(其上任一點在公共坐標系下的坐標),求解機械臂全部關(guān)節(jié)運動位矢.

機械臂逆向運動學控制問題就是已知A,對運動學方程(7)中的進行求解,對應于每個合理存在的A,可能有多組解集合.

方程的解空間轉(zhuǎn)化為

變量v一般有嚴格的限制條件：當v i表示旋轉(zhuǎn)角度時,要根據(jù)機械臂i關(guān)節(jié)實際可以旋轉(zhuǎn)的角度范圍設定限制條件；當v i表示移動距離時,要根據(jù)機械臂i關(guān)節(jié)實際可以平移的范圍設定限制條件.這樣,機械臂逆運動學控制問題,轉(zhuǎn)化成為在n維有限解空間內(nèi)的方程求解問題.

上述方程的求解可以從方程兩側(cè)的矩陣特征入手,A可表示為式(11).

其中：x,y,z為機械臂末端位置坐標,即坐標系O n X n Y n Z n原點O n在OXYZ坐標系下的坐標,t ij(i,j∈[1,3])為表征末端連桿linkn偏轉(zhuǎn)的常數(shù).

f(v)可表示為式(12)形式.

其中：M ij(i∈[1,3],j∈[1,4])為含有機械臂D-H參數(shù)多項式.由于兩矩陣相等,因此矩陣中的元素對應相等,由此可以得到12個方程組成的方程組.當機械臂自由度小于12時,逆運動學控制問題為解不定方程組問題；當自由度大于12時,由于r ij多項式中含有三角函數(shù),逆運動學控制問題為解超越方程組問題,可能有多組解存在.

此處從三自由度機械臂逆運動學解算開始討論.三自由度機械臂逆向運動學控制問題為方程(13).方程(13)兩側(cè)左乘得方程(14).

方程(14)共有3個未知數(shù)v1、v2、v3,而上式左側(cè)化簡展開后得到的4×4矩陣中(1,4)、(2,4)、(3,4)位置的多項式元素恰不含不易得知的t ij,此時只需使得兩側(cè)矩陣中(1,4)、(2,4)、(3,4)位置的元素對應相等即可列出3個方程對問題進行求解.機械臂自由度＞3 時,可以采用迭代方法求一組近似解

3 四自由度機械臂運動學建模

3.1 四自由度機械臂D-H 模型建立

本文所研究的機械臂共含有6個舵機,其中2個舵機負責輸出滅火泵組相關(guān)動作,對機械臂末端位置和仰角控制有貢獻的舵機為底座部位旋轉(zhuǎn)軸線垂直水平面的舵機和臂體鉸接處3個旋轉(zhuǎn)軸線平行于水平面的舵機,因此可以將其視為四自由度機械臂.

在實驗中所用機械臂實體采用伺服電機配諧波減速機結(jié)構(gòu),最大轉(zhuǎn)速為2 500 r/min,各軸可做正負角度運動.水平軸最大運動范圍為±180°,軸線平行于水平面各軸運動范圍為±170°,末端軸運動范圍為±360°,實體圖如圖1所示.

圖1 六自由度機械臂實體

在機械臂所處的水平地面上建立笛卡爾直角坐標系OXYZ,水平地面為OXY平面,Z軸垂直于OXY平面,與關(guān)節(jié)1處舵機轉(zhuǎn)軸共線.之后依次在4個關(guān)節(jié)處按照要求建立坐標系.機械臂連桿模型如圖2所示,且其D-H 參數(shù)列表見表1.

圖2 四自由度機械臂連桿模型

表1 機械臂D-H 參數(shù)表

3.2 四自由度機械臂正向運動建模

根據(jù)D-H 參數(shù)列表可得各相鄰坐標系轉(zhuǎn)換矩陣,見式(15)～(19).

將上述4個矩陣依次相乘即可得到機械臂正向運動方程為：

3.3 四自由度機械臂逆向運動解算

由于四自由度機械臂逆向運動學解算矩陣解法十分繁瑣,算法無法簡潔地編寫成代碼嵌入機械臂上位機控制程序中,故此處擯棄繁復的矩陣算法,從逆向運動學控制的定義入手,提出一種由機械臂連桿模型幾何約束關(guān)系直接求解逆運動學問題的算法.標注有機械臂參數(shù)的模型如圖3所示.

圖3 標注有逆運動學計算變量的連桿模型

四自由度機械臂逆運動學計算轉(zhuǎn)化為以下幾何問題：已知機械臂末端P點位置為P(x,y,z),仰角為φ(link4與OXY面夾角為φ),求解θ1,θ2,θ3,θ4.

首先,可以十分容易計算θ1

由于link2、link3和link4運動過程中始終處于同一平面,且關(guān)節(jié)旋轉(zhuǎn)角度值與坐標系選擇無關(guān),因此可將機械臂連桿模型投影到二維平面求解.在OXY平面建立W軸與X軸呈θ1,平面OWZ即為link2、link3和link4運動的平面,機械臂末端點P在OWZ平面坐標系中位置為P(w,z),link4與W 軸夾角等于φ,其中w=

問題進一步簡化為：平面內(nèi)已知機械臂末端P點位置為P(w,z),仰角為φ,求解θ2,θ3,θ4.

由幾何約束關(guān)系可得方程組(22).

此方程組的解表示為方程(23).

其中

θ2,θ3,θ4滿足實際角度約束條件.

綜上所述,算法的計算機程序流程如下：

1)計算θ1和w；

2)計算m,n,t1,t2；

3)判斷是否滿足式(22)的兩個條件,

兩條件都滿足執(zhí)行步驟3),只要有一個不滿足則無解,即機械臂理論上無法到達該位姿；

4)計算式(23)中兩個正弦取值

兩式均取值在[-1,1]范圍內(nèi),舍去超出的值.

5)用式(24)計算滿足如下條件的θ2,θ3,θ4,

此時(θ1,θ2,θ3,θ4)可能有多組解.

根據(jù)機械臂各關(guān)節(jié)實際可以到達的角度范圍,對解取舍,最終結(jié)果可能無解、有一組解或有多組解.這種幾何算法適用于大多數(shù)四自由度以下(包括四自由度)機械臂的逆運動學計算.

4 實驗結(jié)果

根據(jù)表1的D-H 參數(shù),建立機械臂連桿模型[21],初始狀態(tài)如圖4所示.θ1限制在[-90°,90°]間變化,θ2,θ3,θ4三個參數(shù)均限制在[-170°,170°]之間變化.

圖4 機械臂初始狀態(tài)

利用仿真實驗對逆運動學解算結(jié)果進行驗證.假設機械臂末端預期達到位姿是：末端坐標為(236,0,186),末端執(zhí)行器仰角為0°.則使用嵌入在機械臂控制程序的逆運動學解算結(jié)果為(θ1,θ2,θ3,θ4)=(0°,66.93°,-70.91°,3.98°).將數(shù)據(jù)代入到MATLAB建立好的模型中進行正向運動學計算,機械臂此時位姿如圖5所示,可求解機械臂正向運動學方程如式(33)所示.

圖5 在(0°,66.93°,-70.91°,3.98°)狀態(tài)機械臂位姿

因此末端坐標為(236.5,0,185.6),這說明本文提出的逆運動學解算方法是精確有效的.

5 結(jié)論

為了應對變電站智能消防前端執(zhí)行機構(gòu)靈活性不足、定位精確度低等問題,本文研究了面向變電站智能消防機械臂高精度運動學模型的建立方法.首先使用D-H 建模法完成了四自由度機械臂正向與逆向運動學模型構(gòu)建,并得到了正向運動學轉(zhuǎn)換矩陣,以及逆向運動學機械臂末端位姿與關(guān)節(jié)轉(zhuǎn)角的關(guān)系方程式,接著提出了一種利用機械臂連桿模型幾何約束關(guān)系,直接求解四自由度機械臂逆運動學問題的算法,可以在解算前判斷機械臂能否到達預期位姿,而且計算過程回避了復雜的矩陣運算,可高效精確地解決四自由度機械臂逆運動學控制問題.最后,本文搭建了機械臂的三維模型,并完成了仿真實驗,實驗結(jié)果驗證了模型的精確性.