配電線路維護(hù)機(jī)器人虛擬現(xiàn)實(shí)仿真系統(tǒng)設(shè)計(jì)

陸懷谷，楊 光，李光彥，郭 毓

(1. 國(guó)網(wǎng)江蘇省電力有限公司常州供電分公司，江蘇 常州 213000；2. 南京理工大學(xué)自動(dòng)化學(xué)院，江蘇 南京 210094)

1 引言

在科技高速發(fā)展的今天，人類在高危環(huán)境下的工作將被機(jī)器人逐步代替[1]。配電線路維護(hù)帶電作業(yè)機(jī)器人正是在此背景下立項(xiàng)研發(fā)的[2]。

在配電線路維護(hù)機(jī)器人帶電作業(yè)過程中，機(jī)械臂的作業(yè)任務(wù)復(fù)雜，對(duì)其控制精度要求亦高[3]。利用機(jī)器人進(jìn)行高危帶電作業(yè)，需要操作人員提前對(duì)機(jī)械臂的作業(yè)任務(wù)進(jìn)行分析，并對(duì)機(jī)械臂的運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃，以保證運(yùn)動(dòng)平穩(wěn)、安全[4]。同時(shí)，由于機(jī)器人進(jìn)行配電線路維護(hù)作業(yè)時(shí)處于高空，地面操作人員無法通過肉眼直接觀察到作業(yè)場(chǎng)景信息，對(duì)操控機(jī)械臂作業(yè)帶來很大困難[5]。

目前，虛擬現(xiàn)實(shí)技術(shù)發(fā)展?jié)u漸成熟，正被越來越廣泛地應(yīng)用于機(jī)器人的遠(yuǎn)程控制中[6，7]。虛擬現(xiàn)實(shí)是由計(jì)算機(jī)生成的一種多元信息融合的可交互三維環(huán)境，用戶通過這種計(jì)算機(jī)仿真系統(tǒng)，能夠置身于一個(gè)虛擬的三維空間中，獲得與實(shí)際場(chǎng)景非常相似的臨場(chǎng)感[8，9]。日本MEL實(shí)驗(yàn)室研發(fā)了一個(gè)預(yù)測(cè)環(huán)境系統(tǒng)，操作者操作虛擬環(huán)境中的機(jī)器人與操作實(shí)際機(jī)器人的感覺很相似，操作者除了能體驗(yàn)逼真的視覺畫面之外，還可以感受到力覺反饋[10]。文獻(xiàn)[11]提出的虛擬現(xiàn)實(shí)機(jī)器人實(shí)操教學(xué)系統(tǒng)，通過將虛擬現(xiàn)實(shí)引擎Unity3D構(gòu)建的機(jī)器人平臺(tái)與六軸串聯(lián)機(jī)器人結(jié)合，達(dá)到了六軸串聯(lián)機(jī)器人示教器實(shí)操教學(xué)仿真的目的。

本文將虛擬現(xiàn)實(shí)技術(shù)與機(jī)器人運(yùn)動(dòng)學(xué)模型相結(jié)合，研究機(jī)械臂軌跡規(guī)劃及三維顯示方法，開發(fā)了配電線路維護(hù)機(jī)器人虛擬現(xiàn)實(shí)仿真軟件，通過遙顯示界面可使操作者能夠更好地把握全局場(chǎng)景，更自然地實(shí)現(xiàn)人機(jī)協(xié)作，使機(jī)器人能夠在復(fù)雜的非結(jié)構(gòu)化場(chǎng)景下安全作業(yè)，順利完成配電線路維護(hù)任務(wù)。

2 虛擬現(xiàn)實(shí)軟件平臺(tái)搭建

為了實(shí)現(xiàn)配電作業(yè)機(jī)器人遠(yuǎn)程控制及路徑規(guī)劃功能，首先需要搭建機(jī)械臂模型和虛擬作業(yè)環(huán)境，實(shí)現(xiàn)機(jī)械臂與軟件操控平臺(tái)的實(shí)時(shí)數(shù)據(jù)通訊。

2.1 軟件平臺(tái)總體設(shè)計(jì)

系統(tǒng)主要由控制模塊、通信及信息處理模塊、顯示模塊組成，如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)圖

圖1中各部分功能如下所示：

1)通信及信息處理模塊

采用SOCKET TCP連接機(jī)器人作業(yè)端及虛擬現(xiàn)實(shí)顯示端，將通過服務(wù)器獲取的機(jī)器人關(guān)節(jié)姿態(tài)信息發(fā)送至數(shù)據(jù)層，用于虛擬現(xiàn)實(shí)環(huán)境中機(jī)械臂姿態(tài)同步，同時(shí)使虛擬環(huán)境中的場(chǎng)景與真實(shí)場(chǎng)景保持一致。操作人員可以在虛擬現(xiàn)實(shí)環(huán)境下調(diào)整虛擬機(jī)械臂的姿態(tài)，再將姿態(tài)關(guān)節(jié)信息發(fā)送至機(jī)器人服務(wù)器，用于實(shí)體機(jī)械臂軌跡規(guī)劃。

2)控制模塊

運(yùn)動(dòng)學(xué)解算：為了使機(jī)械臂完成配電線路維護(hù)任務(wù)，需建立機(jī)械臂運(yùn)動(dòng)學(xué)模型。通過處理傳感器信息獲取作業(yè)目標(biāo)位姿，并通過機(jī)械臂逆運(yùn)動(dòng)學(xué)可求解到達(dá)目標(biāo)位姿的機(jī)械臂關(guān)節(jié)角，從而實(shí)現(xiàn)機(jī)械臂路徑點(diǎn)的解算。

軌跡規(guī)劃：為了實(shí)現(xiàn)機(jī)械臂末端到達(dá)指定點(diǎn)或者按照規(guī)劃的路徑運(yùn)動(dòng)的功能，需對(duì)機(jī)械臂進(jìn)行軌跡規(guī)劃。

3)顯示模塊

場(chǎng)景運(yùn)動(dòng)控制：為了實(shí)現(xiàn)離線訓(xùn)練仿真和實(shí)時(shí)在線控制功能，在Unity3D引擎中完成對(duì)機(jī)械臂、工器具等其它電力設(shè)備動(dòng)態(tài)信息的管理和顯示。

危險(xiǎn)警示：機(jī)械臂同時(shí)碰觸兩相裸線時(shí)可能發(fā)生短路事故，為防止誤操作，當(dāng)機(jī)械臂觸碰一相電時(shí)，將該機(jī)械臂突出顯示為高亮的顏色，提示操作者關(guān)注。

場(chǎng)景渲染：完成場(chǎng)景遍歷和渲染，為操作者呈現(xiàn)一個(gè)具有良好沉浸性的畫面，使操作者通過簡(jiǎn)單操作就能夠順利完成機(jī)械臂運(yùn)動(dòng)仿真或者遙控機(jī)械臂作業(yè)的任務(wù)，實(shí)現(xiàn)人機(jī)共融。

2.2 場(chǎng)景三維建模

為了對(duì)帶電作業(yè)現(xiàn)場(chǎng)進(jìn)行重構(gòu)，根據(jù)作業(yè)現(xiàn)場(chǎng)各個(gè)部件在整個(gè)場(chǎng)景中分布情況以及各部件的外形尺寸和機(jī)械連接關(guān)系，通過3D MAX軟件進(jìn)行帶電作業(yè)場(chǎng)景模型庫搭建，并將模型庫導(dǎo)入U(xiǎn)nity3D中重建帶電作業(yè)場(chǎng)景。

帶電作業(yè)場(chǎng)景主要由電線桿塔、三相高壓線、單個(gè)元器件等組成。為了對(duì)帶電作業(yè)現(xiàn)場(chǎng)進(jìn)行重構(gòu)，需要掌握各個(gè)部件在整個(gè)場(chǎng)景中排布以及各部件的外形尺寸和機(jī)械連接關(guān)系。在建模的過程中，需要對(duì)照各個(gè)部件實(shí)體及其機(jī)械手冊(cè)的參數(shù)，在統(tǒng)一的尺度標(biāo)尺下根據(jù)1：1比例完成建模。部分設(shè)備三維模型如圖2所示。

圖2 帶電作業(yè)場(chǎng)景建模

帶電作業(yè)場(chǎng)景的模型一般為異形件，模型曲面由大量的三角形構(gòu)成，導(dǎo)致用Unity3D加載整個(gè)場(chǎng)景速度過慢。本文選用Polygen Crunch減面工具對(duì)模型減小三角形面片，選中需要簡(jiǎn)化的模型后，通過鼠標(biāo)拖拽拉動(dòng)條來調(diào)整簡(jiǎn)化程度，在保證加載速度的時(shí)候不至于使場(chǎng)景過于失真，圖3展示了絕緣子減面后的效果。

圖3 絕緣子減面效果圖

3 機(jī)械臂運(yùn)動(dòng)規(guī)劃

通過對(duì)機(jī)械臂的運(yùn)動(dòng)學(xué)仿真，可以在虛擬現(xiàn)實(shí)環(huán)境中控制機(jī)械臂運(yùn)動(dòng)，模擬作業(yè)任務(wù)，保證配電線路作業(yè)過程中機(jī)械臂操作的精準(zhǔn)性，同時(shí)可以避免機(jī)械臂誤操作及觸電事故的發(fā)生。

3.1 機(jī)械臂運(yùn)動(dòng)學(xué)建模

機(jī)械臂運(yùn)動(dòng)是以末端移動(dòng)與關(guān)節(jié)旋轉(zhuǎn)的形式實(shí)現(xiàn)的，對(duì)機(jī)械臂運(yùn)動(dòng)學(xué)分析，需要對(duì)機(jī)械臂系統(tǒng)進(jìn)行運(yùn)動(dòng)學(xué)建模，即獲取機(jī)械臂末端的位姿信息，分析各關(guān)節(jié)的位姿坐標(biāo)系變換關(guān)系，建立機(jī)械臂各關(guān)節(jié)角度與末端位姿的對(duì)應(yīng)關(guān)系[12]。

本文中采用優(yōu)傲機(jī)械臂UR5作為作業(yè)機(jī)械臂，如圖4所示，該機(jī)械臂由基座、6根連桿和4個(gè)旋轉(zhuǎn)關(guān)節(jié)組成，其肩關(guān)節(jié)有2個(gè)自由度，肘關(guān)節(jié)有1個(gè)自由度和腕關(guān)節(jié)有3個(gè)自由度。根據(jù)UR5的D-H參數(shù)，且連桿位姿的坐標(biāo)變換遵循“由左到右”的原則，根據(jù)機(jī)械臂連桿坐標(biāo)系及其變換求解運(yùn)動(dòng)方程，建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型。

圖4 UR機(jī)器人與連桿坐標(biāo)系的建立

連桿的位姿變換矩陣為

Ai=Rot(zi，θi)Trans(0，0，di)Trans(ai，0，0)Rot(xi，αi) (1)

(2)

對(duì)于連桿參數(shù)固定的機(jī)械臂，當(dāng)輸入各個(gè)關(guān)節(jié)角度時(shí)可得到末端執(zhí)行器的位姿。

3.2 機(jī)械臂逆運(yùn)動(dòng)學(xué)分析

機(jī)械臂逆運(yùn)動(dòng)學(xué)求解采用代數(shù)解析法實(shí)現(xiàn)，可以避免迭代誤差的問題，求解快速、實(shí)時(shí)性高。由于機(jī)械臂運(yùn)動(dòng)學(xué)逆解不唯一，需根據(jù)實(shí)際機(jī)械臂各關(guān)節(jié)的運(yùn)動(dòng)范圍以及應(yīng)用場(chǎng)景增加相應(yīng)的限制條件，以求取符合實(shí)際應(yīng)用的最優(yōu)解。UR5機(jī)械臂連桿參數(shù)(α，d，a)及機(jī)械臂末端執(zhí)行器位姿的齊次變換矩陣T已知，可以求出機(jī)械臂各個(gè)關(guān)節(jié)的關(guān)節(jié)角θ1～θ6。

由式(2)，得到

(3)

將等式兩端同時(shí)展開，根據(jù)等式兩邊矩陣對(duì)應(yīng)位置元素相等，可分別求出機(jī)械臂各個(gè)關(guān)節(jié)的關(guān)節(jié)角θ1，θ5，θ6其中當(dāng)θ5為0°或180°時(shí)，機(jī)械臂處于奇異狀態(tài)。此時(shí)，關(guān)節(jié)軸4和6重合或相反，無法求出具體的θ4。在奇異點(diǎn)時(shí)，θ4是任意的，再計(jì)算對(duì)應(yīng)的θ6，此時(shí)θ4和θ6有無窮多組解，運(yùn)動(dòng)學(xué)逆解的結(jié)果也有無窮多個(gè)。

對(duì)式(3)進(jìn)一步變換得

(4)

將等式兩端同時(shí)展開，根據(jù)等式兩邊矩陣對(duì)應(yīng)位置元素相等，可分別求出機(jī)械臂各個(gè)關(guān)節(jié)的關(guān)節(jié)角θ3，θ2，θ4。

根據(jù)上述步驟可知反向求解機(jī)械臂各關(guān)節(jié)角度的結(jié)果是多解的。當(dāng)θ5不為0°或180°時(shí)，UR5機(jī)器人的逆運(yùn)動(dòng)學(xué)解共有8組，可根據(jù)作業(yè)環(huán)境限制等實(shí)際情況進(jìn)行選擇，排除那些因場(chǎng)景或機(jī)械臂結(jié)構(gòu)限制不可行的解，再考慮相關(guān)性能指標(biāo)求出最優(yōu)解。

3.3 關(guān)節(jié)空間的插值方法

為了使機(jī)械臂能夠完成特定動(dòng)作，可以利用遙操作桿調(diào)節(jié)機(jī)械臂各關(guān)節(jié)角，改變機(jī)械臂的位姿使其末端到達(dá)目標(biāo)路徑點(diǎn)；再對(duì)每個(gè)關(guān)節(jié)進(jìn)行插值運(yùn)算，獲得關(guān)節(jié)信息；最后將各關(guān)節(jié)的信息輸出至運(yùn)動(dòng)控制模塊。通過設(shè)計(jì)合理的插值方法，可以保證機(jī)器人運(yùn)動(dòng)過程中關(guān)節(jié)速度、加速度的連續(xù)性，提高機(jī)器人的運(yùn)動(dòng)精度，減少抖動(dòng)帶來的沖擊損害。

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(5)

對(duì)上式求導(dǎo)可得

(6)

可知速度和加速度均光滑連續(xù)。點(diǎn)到點(diǎn)的運(yùn)動(dòng)中一般默認(rèn)起始時(shí)刻t0=0，當(dāng)t0≠0時(shí)令t-t0→t即可，根據(jù)起始狀態(tài)得到五次多項(xiàng)式的邊界約束條件為

(7)

可以得到多項(xiàng)式參數(shù)的解為

(8)

4 仿真與分析

4.1 插值控制系統(tǒng)仿真

為了驗(yàn)證五次多項(xiàng)式插值法對(duì)機(jī)械臂運(yùn)動(dòng)規(guī)劃的效果，設(shè)計(jì)機(jī)械臂取避雷器的路徑過程，設(shè)置路徑上的起始點(diǎn)、中間過程路徑和目標(biāo)點(diǎn)，初始點(diǎn)是機(jī)械臂末端初始位置，中間點(diǎn)1是機(jī)械臂接近桿塔的位置，中間點(diǎn)2選在靠近避雷器的附近點(diǎn)，終點(diǎn)為避雷器目標(biāo)位置點(diǎn)?？刂茩C(jī)械臂作業(yè)場(chǎng)景如圖5所示。

圖5 規(guī)劃取避雷器路徑圖

選取實(shí)際作業(yè)過程中機(jī)械臂路徑的起始點(diǎn)、中間點(diǎn)1、中間點(diǎn)2和目標(biāo)點(diǎn)，選取的路徑點(diǎn)機(jī)械臂各關(guān)節(jié)角度數(shù)據(jù)不變，得到各關(guān)節(jié)角度、關(guān)節(jié)角速度隨時(shí)間變化的曲線圖如下圖6所示。

圖6 關(guān)節(jié)五次多項(xiàng)式插值

從圖6(a)中可以看出，關(guān)節(jié)角度隨時(shí)間變化的曲線處處連續(xù)，而且處處可導(dǎo)，關(guān)節(jié)的位置、速度以及加速度對(duì)時(shí)間的曲線均光滑連續(xù)，且在指定時(shí)刻到達(dá)目標(biāo)狀態(tài)，表明插值算法具有良好的軌跡精度，極大地減小了運(yùn)動(dòng)過程中的沖擊。經(jīng)過選取的中間點(diǎn)，能夠完成指定的路徑和相應(yīng)的任務(wù)。從圖6(b)可以看出角速度隨時(shí)間變化的曲線是連續(xù)的，在選取的路徑點(diǎn)處，機(jī)械臂的角速度為0，之后從0連續(xù)變到下一個(gè)路徑點(diǎn)。從實(shí)際機(jī)械臂的運(yùn)動(dòng)狀態(tài)看，機(jī)臂運(yùn)動(dòng)到指定路徑點(diǎn)時(shí)，機(jī)械臂各關(guān)節(jié)角速度減速為0，之后各關(guān)節(jié)開始加速。可見利用五次多項(xiàng)式插值能夠很好地完成相應(yīng)的規(guī)劃任務(wù)，在虛擬現(xiàn)實(shí)環(huán)境下可以很好地實(shí)現(xiàn)遠(yuǎn)程控制機(jī)器人的作業(yè)需求。

4.2 逆運(yùn)動(dòng)學(xué)求解控制系統(tǒng)仿真

為了驗(yàn)證機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的準(zhǔn)確性，設(shè)計(jì)機(jī)械臂末端按圓周運(yùn)動(dòng)，通過虛擬現(xiàn)實(shí)系統(tǒng)仿真運(yùn)動(dòng)過程。

根據(jù)機(jī)械臂逆運(yùn)動(dòng)學(xué)求解機(jī)械臂關(guān)節(jié)角度如圖7(a)所示。從圖中可以看出，各關(guān)節(jié)角變換平緩，未出現(xiàn)奇異，所有關(guān)節(jié)角均在允許的運(yùn)動(dòng)范圍。圓周運(yùn)動(dòng)軌跡規(guī)劃下機(jī)器人各關(guān)節(jié)角速度的曲線變化如圖7(b)所示?？梢钥闯鰴C(jī)械臂各關(guān)鍵角速度變化平穩(wěn)，且沒有角速度過大的問題。

圖7 圓周運(yùn)動(dòng)關(guān)節(jié)信息圖

為了更好地展示機(jī)械臂逆運(yùn)動(dòng)學(xué)規(guī)劃的效果，實(shí)驗(yàn)中讓機(jī)械臂手爪末端夾持一個(gè)黑色水筆，通過機(jī)械臂畫下圖形驗(yàn)證圓周運(yùn)動(dòng)過程中機(jī)械臂運(yùn)動(dòng)軌跡及平穩(wěn)性，如下圖8所示，實(shí)驗(yàn)結(jié)果表明，實(shí)際機(jī)械臂能夠平穩(wěn)地完成末端圓周運(yùn)動(dòng)。

圖8 圓周運(yùn)動(dòng)實(shí)驗(yàn)圖

5 結(jié)語

本文以配電線路維護(hù)機(jī)器人項(xiàng)目為背景，設(shè)計(jì)了一種配電線路維護(hù)機(jī)器人虛擬現(xiàn)實(shí)仿真系統(tǒng)，將虛擬現(xiàn)實(shí)技術(shù)與機(jī)器人路徑規(guī)劃相結(jié)合。通過仿真證明系統(tǒng)的虛擬現(xiàn)實(shí)模塊可以直觀、安全和高效地顯示三維作業(yè)場(chǎng)景，實(shí)時(shí)同步機(jī)械臂動(dòng)作，同時(shí)系統(tǒng)的插值軌跡控制控制模式和逆運(yùn)動(dòng)學(xué)求解軌跡控制模式均能良好的實(shí)現(xiàn)軌跡規(guī)劃，為配電線路維護(hù)機(jī)器人完成帶電作業(yè)任務(wù)奠定基礎(chǔ)。