程梓洋,王德權(quán),張 南

(1.大連工業(yè)大學機械工程與自動化學院,大連 116034;2.遼寧對外經(jīng)貿(mào)學院大數(shù)據(jù)研究院,大連 116052)

0 引言

工業(yè)機器人作為提高生產(chǎn)和工作效率的利器在我國制造業(yè)中已經(jīng)得到了廣泛的應用。作為一種機電一體化的可編程裝置,當傳統(tǒng)的在線編程漸漸無法跟上生產(chǎn)腳步時,離線編程開始有了廣泛的利用空間。

目前,離線編程仿真平臺在國外有兩種,一種是如機器人4大家族(ABB、KUKA、FANUC、安川),他們均具有自己的離線編程軟件;另一種是通用型的離線編程軟件,基本上是由CAD軟件公司(如達索、UG等)等第三方軟件開發(fā)公司開發(fā)[1]。國內(nèi)哈爾濱工業(yè)大學和上海交通大學大學合作在Qt平臺上開發(fā)了可視化交互工業(yè)機器人離線編程與仿真系統(tǒng)[2]。并且專門設計了專用的機器人編程語言。上海交通大學在個人計算機開發(fā)了工業(yè)機器人離線編程與仿真系統(tǒng)。使用OpenGL完成三維圖形的建模,在對機器人運動路徑自動規(guī)劃技術(shù)和離線編程技術(shù)研究的基礎上實現(xiàn)了仿真的功能[2]。這些團隊開發(fā)的系統(tǒng)存在版權(quán)與安全、支持語言范圍小、機器人控制不完善等問題。

在對主流離線編程平臺研究后發(fā)現(xiàn)在離線編程部分,由于大部分語句邏輯或指令都很相似,不同的只是書寫形式,所以采用AST解析的思想將解析過程分為前后端,后端AST結(jié)構(gòu)通用。這一調(diào)整使得離線編程平臺支持的語言可以通過對解析前端進行靈活的調(diào)整以達到解析語言范圍的廣泛擴展。從而奠定了通用機器人離線編程平臺程序解析的基礎。另外,由于機器人公司的離線編程平臺大部分都是直接套用其原本用來控制實際機器人的算法來進行仿真,而這些算法都是不開源的。所以,本文依據(jù)動力學原理,構(gòu)造出一套通用的機器人控制模型,并利用三維圖形引擎OpenSceneGraph與Bullet物理引擎來實現(xiàn)對機器人的運動控制與仿真從而真正實現(xiàn)通用機器人離線編程平臺。

1 系統(tǒng)模塊設計

機器人離線編程仿真平臺的功能總結(jié)起來就是根據(jù)用戶編寫的機器人程序代碼,來操控機器人做出代碼規(guī)定的動作[3]。從流程上來看,平臺先是對機器人程序指令進行解析,而后根據(jù)解析得到的結(jié)果來操控機器人的運動。因此,平臺需要以下兩大模塊:

(1)提供對機器人編程語言的編輯與解析功能的解析模塊[4]。

(2)提供對機器人運動的控制并繪制機器人圖形信息的機器人仿真模塊。

對整個平臺的設計也主要圍繞著這兩大模塊的設計來進行。

1.1 解析模塊設計

解析模塊的代碼編輯功能主要的設計目標是能完整的顯示代碼、提供語法高亮并能記錄函數(shù)。

由于存儲程序的文件是一個特定格式的文本形式的文件。并且程序的解析實際上就是將構(gòu)成邏輯的程序代碼轉(zhuǎn)化為控制機器人運動的數(shù)據(jù)。因此解析模塊的解析功能設計的目標為:

(1)讀取文件,并將文件轉(zhuǎn)化為數(shù)據(jù)流。

(2)解析數(shù)據(jù)流并得到可以用來控制機器人運動的數(shù)據(jù)。

實現(xiàn)第一個目標的為解析的前端,對解析語言范圍的擴展由前端來實現(xiàn)。實現(xiàn)第二個目標的為解析的后端,后端不會輕易有大的變化,這是因為大部分離線編程語言的邏輯類似,只是表現(xiàn)形式不同[5]。

1.2 機器人仿真模塊設計

平臺關(guān)于機器人仿真模塊的設計主要分為兩部分,一部分是對機器人運動控制的模塊的設計,另一部分對三維圖形的形式繪制機器人的圖形模塊的設計。

機器人的運動控制模塊主要實現(xiàn)三項功能,分別是正向控制、逆向控制與運動軌跡規(guī)劃。

(1)正向控制。機器人正向控制功能是在給定機器人各個關(guān)節(jié)的廣義坐標的情況下,計算出機器人各個連桿的位置與姿態(tài)。

(2)逆向控制。機器人的逆向控制功能則是在給定機器人末端連桿的位置與姿態(tài)計算出機器人的廣義坐標與機器人各個連桿的位置與姿態(tài)。

(3)軌跡規(guī)劃。運動軌跡規(guī)劃可以理解為給定機器人末端的運動曲線與運動速度等運動參數(shù),使機器人末端連桿按照該軌跡與運動參數(shù)進行特定的運動。

圖形模塊主要是利用三維引擎實時繪制機器人,并處理用戶與場景的交互。

2 系統(tǒng)模塊實現(xiàn)

2.1 解析模塊實現(xiàn)

解析模塊中的代碼編輯模塊的實現(xiàn)利用了Qt的組件,在其基礎上進行派生。實現(xiàn)效果如圖1所示。

圖1 解析界面圖

解析模塊中程序語言解析模塊的實現(xiàn)的主要思想是抽象語法樹(AST)思想。AST是一種數(shù)據(jù)結(jié)構(gòu),在對程序代碼的解析過程中AST作為一種中間數(shù)據(jù)結(jié)構(gòu)連接其前端的機器人語言與后端的程序開發(fā)語言。之所以選用AST作為解析功能的主要思想是由于AST的抽象程度剛剛好符合對工業(yè)機器人編程語言解析的需求。由于大部分機器人編程語言的結(jié)構(gòu)與語法都很類似,因此作為描述語言的數(shù)據(jù)結(jié)構(gòu),AST就可以直接或經(jīng)過簡單的擴展后復用。而復用正是實現(xiàn)對多種語言解析的基礎。通過將不同語言轉(zhuǎn)換為同種的AST,而后對AST進行迭代解析,從而達到對程序代碼解析的目的。要實現(xiàn)以上描述的功能,需要分兩步。

步驟1:AST構(gòu)造過程的實現(xiàn)。該部分被稱為解析功能的前端部分[6]。主要利用Flex與Bison對程序代碼進行詞法分析與語法分析[7]。在Bison對程序代碼進行語法分析動作時插入構(gòu)造AST的動作。由于Flex與Bison是根據(jù)特定的規(guī)則文件來構(gòu)造AST[8],所以當需要擴展語言解析的范圍時候,只需要為新的語言提供新的Flex與Bison規(guī)則文件即可[9]。

步驟2:對AST求解。該部分稱為解析功能的后端部分,由于機器人編程的程序信息與結(jié)構(gòu)具有很大的相似性,所以解析功能的后端可以復用,以此達到語言解析通用的目的。AST的求解時將程序代碼中聲明的程序信息寫入上下文管理器與運行管理器中。上下文管理器負責管理程序中出現(xiàn)的用戶聲明變量信息、函數(shù)信息與數(shù)據(jù)結(jié)構(gòu)信息等,運行管理器負責管理系統(tǒng)內(nèi)置的變量信息、函數(shù)信息與數(shù)據(jù)結(jié)構(gòu)信息,并負責記錄某些跳轉(zhuǎn)語句的入口等程序運行需要的位置信息;再利用寫入的信息執(zhí)行代碼的主程序,得出可以控制機器人運行的指令數(shù)據(jù)[10]。解析功能實現(xiàn)流程如圖2所示。

圖2 解析流程圖

最終給定一段Rapid機器人程序與一段包含KRL語句程序用解析模塊解析,最終得到結(jié)果如圖3所示。

圖3 解析結(jié)果圖

2.2 機器人仿真模塊實現(xiàn)

機器人仿真模塊的三維圖形部分主要利用Osg三維引擎嵌入Qt的三維組件中來實現(xiàn),并會通過機器人的圖形參數(shù)來繪制出三維的機器人圖形。實現(xiàn)效果如圖4所示。

圖4 三維界面圖

機器人運動控制是基于動力學的運動控制,模擬的是機器人在實際運動的動力學與運動學狀態(tài)。對機器人運動控制的實現(xiàn)分為以下3部分[11]:

(1)正向控制。機器人的正向控制是給定機器人各個關(guān)節(jié)的廣義坐標、廣義速度、廣義加速度與一些機器人逆向控制是已知目標位置與運動狀態(tài)求解出機器人內(nèi)部各個部件應施加的力。首先,剛體部件在空間移動所受的力可以根據(jù)如式(1)所得到的[2]:

(1)

其次剛體在空間旋轉(zhuǎn)所受到的力矩可以根據(jù)如式(2)所得到的:

(2)

由于機器人是一個整體,父級部件的運動會對子級部件的運動狀態(tài)造成影響。因此在逆向控制中先要求解出各個部件之間的相對運動狀態(tài)。也就是計算出各個部件與其父部件之間的相對位置、速度與加速度。隨后根據(jù)式(1)和式(2)計算出每個部件所受的力與力矩,最后計算出在機器人的每個關(guān)節(jié)處應該施加多大的力與力矩。其流程如圖5所示。

圖5 機器人正向控制流程圖

(2)逆向控制實現(xiàn)。機器人本質(zhì)是剛體與剛體之間通過各種約束連接而構(gòu)造出的一個機械裝置,對其物理狀態(tài)(力矩與力)與運動狀態(tài)(速度與加速度)的求解可以視作一個線性互補問題(LCP)的求解[11]。

對機器人的正向控制就是利用PGS算法求解機器人運動方程的結(jié)果改變執(zhí)行對象狀態(tài)的過程。

給定LCP方程:

Ax+b≥0

(3)

x≥0

(4)

xT(Ax+b)=0

(5)

利用高斯賽德爾(guass-seidel)迭代法將矩陣A分割為A=D-L-U可以得到式(6):

(D-L)xk+1+b-Uxk≥0

(6)

xk+1≥0

(7)

(xk+1)T[(D-L)xk+1+b-Uxk]=0

(8)

根據(jù)PGS(projected gauss-seidel)可以得出式(9):

(xk+1)i=max(0,((D-L)-1Uxk-(D-L)-1b)i)

(9)

式(23)即為PGS在求解機器人運動狀態(tài)中的主要應用形式。

對于一個機器人內(nèi)部,影響其運動中狀態(tài)的約束主要可以分為兩類,分別是保證剛體與剛體之間不發(fā)生穿透的接觸約束與根據(jù)機器人內(nèi)部運動副形成的關(guān)節(jié)約束。對機器人運動方程的求解就是利用PGS方法來求解機器人內(nèi)部的這兩種約束。

由于機器人的運動依賴的是指令序列中的指令數(shù)據(jù)。指令數(shù)據(jù)會告訴機器人TCP移動的目標位置與目標姿態(tài),這其實就相當于給機器人在目標位置處增加了一個自由度為0的關(guān)節(jié)約束。對機器人控制就是改變該約束的位置與姿態(tài)。

根據(jù)牛頓第二定律可以知道剛體運動狀態(tài)的改變是力在作用。

根據(jù)歐拉方法:

f(t+Δt)=f(t)+Δtf′(t)

(10)

可以在已知t時刻剛體廣義速度與廣義位置的情況下求得在t+Δt時刻物體的廣義速度與位置:

st+Δt=st+ΔtSut+Δt

(11)

ut+Δt=ut+ΔtM-1f

(12)

式中:u代表剛體的廣義速度,其中包含剛體的質(zhì)心速度v與剛體角速度ω;s代表剛體的廣義位置,其中包括質(zhì)心位置r與質(zhì)心坐標系的姿態(tài)q;f表示機器人內(nèi)部所有剛體部件受到的廣義力,其中包含每個剛體部件力fext與力矩τext;M是由剛體質(zhì)量與慣量張量組成的矩陣,S是姿態(tài)的導數(shù)與角速度的關(guān)系矩陣,表現(xiàn)為:

(13)

式中:Q是由四元數(shù)構(gòu)成的矩陣,對于任意四元數(shù)qi=a+bi+cj+dk,都可以將其轉(zhuǎn)換為矩陣:

(14)

由式(11)與式(12)可以得知剛體的廣義位置可以有剛體的廣義速度求得,剛體的廣義速度可以由剛體所受到的廣義力求得。

在機器人內(nèi)部的各個剛體部件,主要受到兩種力的影響,一種是外力比如重力,另一種是剛體內(nèi)部兩種約束提供的約束力。

由于約束力在機器人運動中屬于內(nèi)力,所以不做功,用數(shù)學語言描述為:

(15)

把機器人內(nèi)部部件的約束方程簡記為:

C(s)=0

(16)

對其求導:

(17)

式中:Jc稱之為Jacobian矩陣,根據(jù)式(15)與式(17)可以得到約束力可以是Jacobian矩陣線性組合后的結(jié)果,即:

(18)

所以式(12)就可改寫為:

(19)

將其進行整理,Jjoint簡寫為Jj,Jcontact簡寫為Jc,λjoint簡寫為λj,λcontact簡寫為λc后可以得到:

(20)

因此在已知關(guān)節(jié)約束種類的情況下,機器人的運動控制方程就轉(zhuǎn)換為對機器人內(nèi)部約束力的求解。加上指令數(shù)據(jù)施加給機器人一個外部固定約束的一個LCP求解的問題,將式(9)帶入式(20)即可完成對機器人運動控制方程的求解,得到機器人內(nèi)部部件所收到的合力。再根據(jù)式(19)與式(11)得到機器人當前時刻的運動狀態(tài)。

綜上所述,對于機器人正向控制而言,首先需要根據(jù)機器人的關(guān)節(jié)構(gòu)造機器人整體的Jacobian矩陣。而后根據(jù)機器人各個部件的屬性構(gòu)造機器人的M矩陣,最后根據(jù)式(20)與指令數(shù)據(jù)求解出機器人內(nèi)各個部件所受到的力與力矩。正向控制流程如圖6所示。

圖6 機器人逆向控制流程圖

(3)軌跡規(guī)劃實現(xiàn)。軌跡規(guī)劃是給定機器人末端的運行軌跡與運行速度等參數(shù),計算機器人各個關(guān)節(jié)的廣義速度與加速度。軌跡規(guī)劃的實現(xiàn)使用了軌跡約束,所謂的軌跡約束就是利用機器人逆向控制的算法在機器人末端新增加一個末端約束連桿,為這個末端約束連桿施加一個沿著軌跡移動的力或者力矩,利用正向控制的方法設定末端約束連桿的廣義坐標等運動參數(shù)從而實現(xiàn)機器人的軌跡移動[12]。圖7展示了機器實現(xiàn)圓弧軌跡運動的實現(xiàn)結(jié)果。

圖7 機器人軌跡運行圖

3 實驗與結(jié)論

最終給定Rapid程序代碼并進行離線仿真實驗,得到結(jié)果如圖8所示。

圖8 離線編程實驗結(jié)果圖

實驗結(jié)果表明,平臺能夠滿足機器人離線編程仿真的基本需求,符合目標預期。

4 總結(jié)

為解決國內(nèi)離線編程平臺支持范圍小,機器人控制功能不完善等問題。文中基于AST思想開發(fā)程序代碼的解析功能,利用一系列動力學算法來實現(xiàn)機器人的控制功能,并利用Qt、bullet、Osg、Flex與Bison等框架實現(xiàn)了該系統(tǒng)。該系統(tǒng)既能通過簡單的修改前端代碼從而實現(xiàn)對多種代碼的解析,又能利用動力學控制的方法完善機器人的控制,解決國內(nèi)離線編程平臺現(xiàn)存的部分問題。