面向變電站智能消防機(jī)械臂的高精度運(yùn)動(dòng)學(xué)模型研究

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

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

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

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

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

1 機(jī)械臂建?；A(chǔ)

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

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

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

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

(2)連桿轉(zhuǎn)角：某關(guān)節(jié)軸線相對(duì)于前一關(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ù)即為機(jī)械臂的D-H 參數(shù).此外,常引入?yún)?shù)σi(σi-1),用以區(qū)分關(guān)節(jié)i(i-1)是旋轉(zhuǎn)關(guān)節(jié)或是滑動(dòng)關(guān)節(jié).

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

2 運(yùn)動(dòng)學(xué)計(jì)算求解與控制

2.1 機(jī)械臂運(yùn)動(dòng)學(xué)計(jì)算求解方法

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

基于參數(shù)在笛卡爾直角坐標(biāo)系中的定義,可以進(jìn)一步求解O i-1X i-1Y i-1Z i-1與O i X i Y i Z i相鄰兩坐標(biāo)系間的變換關(guān)系.由坐標(biāo)系O i-1X i-1Y i-1Z i-1轉(zhuǎn)換到坐標(biāo)系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).

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

其中,

方程(7)稱作機(jī)械臂運(yùn)動(dòng)學(xué)方程.只要任取末端連桿上兩個(gè)不同質(zhì)點(diǎn),用上述方法求解其在公共參考系中的坐標(biāo)表示,即可表示出機(jī)械臂末端位姿.

2.2 機(jī)械臂逆向運(yùn)動(dòng)學(xué)控制

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

機(jī)械臂逆向運(yùn)動(dòng)學(xué)控制問題就是已知A,對(duì)運(yùn)動(dòng)學(xué)方程(7)中的進(jìn)行求解,對(duì)應(yīng)于每個(gè)合理存在的A,可能有多組解集合.

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

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

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

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

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

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

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

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

3 四自由度機(jī)械臂運(yùn)動(dòng)學(xué)建模

3.1 四自由度機(jī)械臂D-H 模型建立

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

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

圖1 六自由度機(jī)械臂實(shí)體

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

圖2 四自由度機(jī)械臂連桿模型

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

3.2 四自由度機(jī)械臂正向運(yùn)動(dòng)建模

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

將上述4個(gè)矩陣依次相乘即可得到機(jī)械臂正向運(yùn)動(dòng)方程為：

3.3 四自由度機(jī)械臂逆向運(yùn)動(dòng)解算

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

圖3 標(biāo)注有逆運(yùn)動(dòng)學(xué)計(jì)算變量的連桿模型

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

首先,可以十分容易計(jì)算θ1

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

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

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

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

其中

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

綜上所述,算法的計(jì)算機(jī)程序流程如下：

1)計(jì)算θ1和w；

2)計(jì)算m,n,t1,t2；

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

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

4)計(jì)算式(23)中兩個(gè)正弦取值

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

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

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

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

4 實(shí)驗(yàn)結(jié)果

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

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

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

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

因此末端坐標(biāo)為(236.5,0,185.6),這說明本文提出的逆運(yùn)動(dòng)學(xué)解算方法是精確有效的.

5 結(jié)論

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