基于MATLAB的配網(wǎng)帶電作業(yè)機(jī)器人運(yùn)動(dòng)學(xué)和作業(yè)路徑仿真分析

孫霄偉，任書(shū)楠，徐善軍，杜婧，路菲，張黎明，胡益菲

（1.北京國(guó)電富通科技發(fā)展有限責(zé)任公司，北京 100070；2.國(guó)網(wǎng)電力科學(xué)研究院有限公司，南京 211106；3.國(guó)網(wǎng)天津市電力公司,天津 300010）

0引言

隨著人工智能技術(shù)的發(fā)展，配網(wǎng)線路帶電作業(yè)逐步使用機(jī)器人替代工人完成危險(xiǎn)的作業(yè)任務(wù)。應(yīng)用機(jī)器人可以有效降低作業(yè)風(fēng)險(xiǎn)、保護(hù)作業(yè)人員，對(duì)于保障電網(wǎng)運(yùn)行安全具有重要意義?，F(xiàn)階段配網(wǎng)帶電作業(yè)機(jī)器人分為全自主和人機(jī)協(xié)同兩種方式。其中，全自主方式是用絕緣斗臂車(chē)將機(jī)器人送至配網(wǎng)線路附近，由機(jī)器人獨(dú)立完成全部作業(yè)任務(wù)，作業(yè)人員在地面監(jiān)督并遙控解決問(wèn)題。

人機(jī)協(xié)同是用絕緣斗臂車(chē)將機(jī)器人和作業(yè)工人一同送至配網(wǎng)線路附近，由機(jī)器人完成較危險(xiǎn)的直接接觸帶電線路的動(dòng)作，作業(yè)工人協(xié)助完成危險(xiǎn)性較低的動(dòng)作，如圖1所示。對(duì)于帶電作業(yè)這種危險(xiǎn)性很高的作業(yè)任務(wù)，全自主一定是未來(lái)發(fā)展的方向，但鑒于配網(wǎng)線路作業(yè)環(huán)境復(fù)雜和現(xiàn)階段技術(shù)水平的局限，人機(jī)協(xié)同對(duì)于這種復(fù)雜的作業(yè)任務(wù)適應(yīng)性更好，具有相對(duì)更廣泛的應(yīng)用前景。選擇人機(jī)協(xié)同的作業(yè)方式，作業(yè)人員和機(jī)器人共處在一個(gè)有限的空間，對(duì)機(jī)器人本身靈活性、安全性和作業(yè)路徑的規(guī)劃水平相比全自主方式要求更高。本文主要從人機(jī)協(xié)同方式的帶電作業(yè)機(jī)器人入手，通過(guò)仿真分析為規(guī)避作業(yè)風(fēng)險(xiǎn)和提高作業(yè)效率提供理論依據(jù)。

圖1 人機(jī)協(xié)同機(jī)器人圖

1 背景

配網(wǎng)線路復(fù)雜、危險(xiǎn)的作業(yè)環(huán)境要求機(jī)器人在有限的空間內(nèi)既能夠快速將末端工具送至指定的作業(yè)位置，又要靈活避開(kāi)周?chē)植嫉母邏壕€路和旁邊配合工作的作業(yè)工人。根據(jù)這些要求，選擇UR10作為配網(wǎng)帶電作業(yè)機(jī)器人的作業(yè)主體。UR10是一款工業(yè)中廣泛應(yīng)用的六軸關(guān)節(jié)型電動(dòng)操作臂，雖然其末端承載力只有98 N，但它能夠?qū)崿F(xiàn)6個(gè)關(guān)節(jié)360°旋轉(zhuǎn)，相比普通的六軸機(jī)器人更加靈活高效，非常適合這種空間有限、需要人機(jī)緊密配合的作業(yè)任務(wù)。但這種機(jī)器人負(fù)載較小、關(guān)節(jié)容易保護(hù)，為了保證作業(yè)流程順暢，有必要對(duì)機(jī)器人的關(guān)鍵作業(yè)動(dòng)作進(jìn)行仿真分析。

對(duì)于工業(yè)機(jī)器人而言，位姿數(shù)據(jù)和路徑規(guī)劃問(wèn)題都是影響機(jī)器人作業(yè)效果的重要問(wèn)題，這些問(wèn)題最終都會(huì)轉(zhuǎn)化成數(shù)學(xué)算法由機(jī)器人內(nèi)部計(jì)算機(jī)完成，并由內(nèi)部計(jì)算機(jī)控制各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)實(shí)現(xiàn)。工業(yè)機(jī)器人的仿真分析就是利用計(jì)算機(jī)軟件構(gòu)建機(jī)器人的簡(jiǎn)化數(shù)學(xué)模型，借助數(shù)學(xué)計(jì)算，模擬機(jī)器人的動(dòng)作過(guò)程，分析可能影響作業(yè)過(guò)程和效果的重要問(wèn)題。

本文使用MATLAB機(jī)器人工具箱為機(jī)器人建立數(shù)學(xué)模型，以配網(wǎng)帶電作業(yè)過(guò)程中幾個(gè)關(guān)鍵路點(diǎn)為例，對(duì)這些作業(yè)動(dòng)作進(jìn)行仿真分析，及時(shí)發(fā)現(xiàn)問(wèn)題、規(guī)避風(fēng)險(xiǎn)，保證機(jī)器人能夠安全高效地完成作業(yè)任務(wù)。

2 機(jī)器人數(shù)學(xué)模型的建立

UR10機(jī)器人數(shù)學(xué)模型的采用標(biāo)準(zhǔn)D-H坐標(biāo)變換建模方法。根據(jù)建立的簡(jiǎn)化模型，通過(guò)齊次矩陣計(jì)算各個(gè)連桿的位置姿態(tài)。

標(biāo)準(zhǔn)D-H坐標(biāo)變換法對(duì)UR10機(jī)器人連桿參數(shù)定義如下：從基座至末端的連桿編號(hào)從1#～6#，zi配置在關(guān)節(jié)i的軸線上，xi位于zi-1和zi的公垂線上；連桿長(zhǎng)度ai-1為沿xi軸從zi-1移動(dòng)到zi的距離；連桿扭轉(zhuǎn)角αi-1為繞xi軸從zi-1旋轉(zhuǎn)到zi的角度；連桿偏移di為沿zi軸從xi-1移動(dòng)到xi的距離；關(guān)節(jié)角θi為繞zi軸從xi-1旋轉(zhuǎn)到xi的角度。

根據(jù)UR10機(jī)器人各連桿間的平移和轉(zhuǎn)動(dòng)變換關(guān)系，可以得出UR10機(jī)器人模型如圖2所示。并且列出它標(biāo)準(zhǔn)D-H坐標(biāo)系參數(shù)表，如表1所示。

表1 UR10標(biāo)準(zhǔn)D-H坐標(biāo)系參數(shù)表

圖2 UR10機(jī)器人坐標(biāo)系變換圖

3 機(jī)器人運(yùn)動(dòng)學(xué)計(jì)算

由于機(jī)器人的結(jié)構(gòu)尺寸是已知的，只要已知各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的角度，可以計(jì)算出機(jī)器人末端的最終位置和姿態(tài)。這個(gè)計(jì)算過(guò)程就是運(yùn)動(dòng)學(xué)正解。反之，已知末端最終位置和姿態(tài)，也可以推導(dǎo)出機(jī)器人的各關(guān)節(jié)角度值，這個(gè)計(jì)算過(guò)程就是運(yùn)動(dòng)學(xué)逆解。計(jì)算正解和逆解是用來(lái)確定機(jī)器人末端到達(dá)位置的基礎(chǔ)問(wèn)題。

3.1 運(yùn)動(dòng)學(xué)正解

運(yùn)動(dòng)學(xué)正解求解是已知機(jī)器人各個(gè)關(guān)節(jié)角q，求機(jī)器人末端坐標(biāo)系相對(duì)于基座坐標(biāo)系的位姿矩陣T。對(duì)于UR10這樣的六軸機(jī)器人，位姿矩陣是一個(gè)4×4齊次矩陣。將上文寫(xiě)出的UR10標(biāo)準(zhǔn)D-H坐標(biāo)系參數(shù)θi、di、ai、αi，分別代入計(jì)算公式可以計(jì)算UR10機(jī)器人相鄰連桿間的齊次變換矩陣Ti-1和Ti。

UR10機(jī)器人的姿態(tài)角在示教器上可以用旋轉(zhuǎn)向量和RPY角兩種方式顯示出來(lái)。本文用旋轉(zhuǎn)矩陣轉(zhuǎn)換為RPY角公式進(jìn)行計(jì)算驗(yàn)證。RPY角可以看作機(jī)器人末端坐標(biāo)系圍繞基坐標(biāo)系旋轉(zhuǎn)而成，其中x6繞x0軸旋轉(zhuǎn)Rx，y6繞y0軸旋轉(zhuǎn)Ry，z6繞z0軸旋轉(zhuǎn)Rz。

RPY角計(jì)算公式如下：

3.2 運(yùn)動(dòng)學(xué)逆解

運(yùn)動(dòng)學(xué)逆解求解是已知機(jī)器人的位姿矩陣T，求關(guān)節(jié)角q=［θ1θ2θ3θ4θ5θ6］。運(yùn)動(dòng)學(xué)正解求解是唯一且相對(duì)容易，運(yùn)動(dòng)學(xué)逆解求解往往有多解且更為復(fù)雜。但運(yùn)動(dòng)學(xué)逆解求解對(duì)于找出奇異點(diǎn)和路徑規(guī)劃有重要意義。這里采用解析法進(jìn)行計(jì)算。

根據(jù)正運(yùn)動(dòng)求解式（9）變換行列式如下：

根據(jù)行列式相等的性質(zhì)計(jì)算θ3、θ2、θ4。

3.2.4 計(jì)算θ3

根據(jù)上文計(jì)算出的結(jié)果，假設(shè)：

3.3 實(shí)驗(yàn)驗(yàn)證

通過(guò)VNC與機(jī)器人系統(tǒng)通信, 讀取系統(tǒng)內(nèi)部有關(guān)機(jī)器人末端執(zhí)行器的x、y、z值與關(guān)節(jié)角度信息，可以驗(yàn)證上文建立的模型和計(jì)算公式的正確性。

首先，選取機(jī)器人舉起作業(yè)工具至作業(yè)點(diǎn)過(guò)程的3個(gè)路點(diǎn)為例。從示教器上讀取它們的關(guān)節(jié)角數(shù)據(jù)θ1～θ6，將其轉(zhuǎn)換為弧度值后，改寫(xiě)為關(guān)節(jié)角矩陣，然后輸入U(xiǎn)R10的標(biāo)準(zhǔn)D-H坐標(biāo)系參數(shù)，用MATLAB機(jī)器人工具箱中的SerialLink命令構(gòu)建UR10的數(shù)學(xué)模型，用fkine命令計(jì)算模型的末端坐標(biāo)系的位姿矩陣T。

求得路點(diǎn)1位姿矩陣：

上述計(jì)算結(jié)果與機(jī)器人系統(tǒng)顯示數(shù)據(jù)，如圖3所示基本吻合，可以確認(rèn)所建立機(jī)器人模型、各個(gè)建模參數(shù)和計(jì)算公式的正確性。為進(jìn)一步引導(dǎo)機(jī)器人到達(dá)目標(biāo)位置提供了理論基礎(chǔ)。

圖3 機(jī)器人關(guān)節(jié)角和路點(diǎn)參數(shù)

4 激光雷達(dá)定位

配網(wǎng)帶電作業(yè)機(jī)器人的作業(yè)目標(biāo)位置和作業(yè)姿態(tài)等關(guān)鍵信息是由激光雷達(dá)根據(jù)作業(yè)現(xiàn)場(chǎng)環(huán)境確定。這種方法可以在作業(yè)過(guò)程中實(shí)時(shí)探知作業(yè)區(qū)域的空間三維環(huán)境，并獲取高壓線路的空間位置坐標(biāo)。相比于視覺(jué)定位技術(shù)，激光雷達(dá)定位效率更高，結(jié)果更準(zhǔn)確，受環(huán)境影響更小，能夠快速、準(zhǔn)確地完成目標(biāo)位置計(jì)算，為機(jī)械臂的運(yùn)動(dòng)規(guī)劃提供有力數(shù)據(jù)支撐。

人機(jī)協(xié)同配網(wǎng)帶電作業(yè)機(jī)器人采用一臺(tái)固定在轉(zhuǎn)臺(tái)上的SICK TIM561單線激光雷達(dá)掃描定位。該激光雷達(dá)可在270°扇面范圍內(nèi)輸出激光脈沖測(cè)量點(diǎn)的距離和對(duì)應(yīng)角度值，同時(shí)激光雷達(dá)會(huì)圍繞轉(zhuǎn)臺(tái)固定軸線反復(fù)旋轉(zhuǎn)，用不同的視角對(duì)配網(wǎng)線路進(jìn)行反復(fù)掃描測(cè)量，通過(guò)計(jì)算雷達(dá)、機(jī)器人和末端工具各坐標(biāo)系之間的旋轉(zhuǎn)矩陣，可以掌握各坐標(biāo)系之間的變換關(guān)系，將激光雷達(dá)所有的掃描數(shù)據(jù)統(tǒng)一在固定的公共坐標(biāo)系下，最終形成空間點(diǎn)云圖，如圖4所示，用來(lái)引導(dǎo)機(jī)械臂準(zhǔn)確到達(dá)線路下方指定的位置完成作業(yè)任務(wù)。

圖4 激光雷達(dá)點(diǎn)云圖

5 路徑規(guī)劃

機(jī)器人的路徑規(guī)劃是根據(jù)作業(yè)任務(wù)要求和激光雷達(dá)傳回的目標(biāo)位置，確定機(jī)器人的操作順序和動(dòng)作過(guò)程，來(lái)實(shí)現(xiàn)機(jī)器人各關(guān)節(jié)轉(zhuǎn)動(dòng)角度、角速度和角加速度的連續(xù)光滑運(yùn)動(dòng)，提高機(jī)器人運(yùn)動(dòng)軌跡的精度和運(yùn)動(dòng)狀態(tài)的穩(wěn)定性，避免由于速度突變?cè)斐蓹C(jī)器人關(guān)節(jié)的磨損和振動(dòng)沖擊。路徑規(guī)劃對(duì)于提高機(jī)器人作業(yè)質(zhì)量、工作效率和延長(zhǎng)機(jī)器人使用壽命都有重要的意義。

本文以取出工具到作業(yè)位置的路徑為例，使用MATLAB機(jī)器人工具箱中的jtraj函數(shù)生成兩段連續(xù)的作業(yè)路徑。通過(guò)此函數(shù)同時(shí)可以計(jì)算出該段路徑上6個(gè)關(guān)節(jié)的角度值如圖5所示，角速度值如圖6所示，角加速度值如圖7所示。

圖5 機(jī)器人各關(guān)節(jié)角度的變化曲線

圖6 機(jī)器人各關(guān)節(jié)角速度的變化曲線

圖7 機(jī)器人各關(guān)節(jié)角加速度的變化曲線

通過(guò)fkine函數(shù)，用得出的關(guān)節(jié)角度值可以進(jìn)一步計(jì)算出機(jī)器人末端坐標(biāo)系在空間位置變化曲線。通過(guò)jacob0函數(shù)可以進(jìn)一步計(jì)算出機(jī)械臂末端坐標(biāo)系位置變化如圖8所示，線速度值如圖9所示，角速度值如圖10所示。

圖8 機(jī)器人末端坐標(biāo)系的位置變化曲線

圖9 機(jī)器人末端坐標(biāo)系的線速度變化曲線

圖10 機(jī)器人末端坐標(biāo)系的角速度的變化曲線

通過(guò)曲線可以進(jìn)一步分析所設(shè)路點(diǎn)設(shè)置是否合理。通過(guò)圖示曲線可以看出從起始路點(diǎn)到目標(biāo)位置：q1關(guān)節(jié)轉(zhuǎn)動(dòng)最大角度1.6172 rad；q4關(guān)節(jié)轉(zhuǎn)動(dòng)最小角度-4.4155 rad；q4關(guān)節(jié)轉(zhuǎn)動(dòng)最大角速度5.7709 rad/s；q6關(guān)節(jié)轉(zhuǎn)動(dòng)最小角速度-4.8642 rad/s；q4關(guān)節(jié)最大角加速度17.7701 rad/s2；q4關(guān)節(jié)最小角加速度-17.7701 rad/s2。

從上述幾個(gè)圖表可以看出，機(jī)器人末端到作業(yè)位置的整段路徑，機(jī)器人末端的運(yùn)動(dòng)軌跡和機(jī)器人關(guān)節(jié)角度變化曲線較為平滑，末端坐標(biāo)系的速度和角速度隨時(shí)間的變化比較平滑，其中關(guān)節(jié)4的相對(duì)瞬時(shí)速度和加速度較大，對(duì)機(jī)械臂的關(guān)節(jié)沖擊較大。

6 工程應(yīng)用驗(yàn)證

通過(guò)操作配網(wǎng)帶電作業(yè)機(jī)器人在實(shí)驗(yàn)室的模擬線路運(yùn)行整個(gè)作業(yè)流程來(lái)驗(yàn)證仿真結(jié)果和實(shí)際作業(yè)效果，如圖11所示。配網(wǎng)帶電作業(yè)機(jī)器人從取剝線工具開(kāi)始，然后攜帶剝線工具到達(dá)作業(yè)準(zhǔn)備位置，然后激光雷達(dá)掃描上方模擬線路，根據(jù)激光雷達(dá)提供的數(shù)據(jù)，機(jī)器人到達(dá)作業(yè)位置完成剝線任務(wù)，然后將剝線工具放回工具架，再在另一個(gè)工具架上取下接線工具，在剛才剝線相同位置完成接線任務(wù)，整個(gè)作業(yè)過(guò)程動(dòng)作流暢，沒(méi)有因關(guān)節(jié)速度和加速度變化發(fā)生關(guān)節(jié)保護(hù)，因此可以判定，規(guī)劃的作業(yè)路徑可以滿(mǎn)足實(shí)際作業(yè)的需要。

圖11 機(jī)器人模擬線路作業(yè)

完成實(shí)驗(yàn)室的驗(yàn)證之后，在選定的10 kV配網(wǎng)實(shí)際線路，有專(zhuān)業(yè)作業(yè)人員操作斗臂車(chē)配合下，完成完整的剝線、接線全流程作業(yè)，進(jìn)一步驗(yàn)證配網(wǎng)帶電作業(yè)機(jī)器人在實(shí)際工作環(huán)境的作業(yè)效果。經(jīng)過(guò)1 a多的數(shù)百次試點(diǎn)作業(yè)，配網(wǎng)帶電作業(yè)機(jī)器人可以順利完成全流程作業(yè)項(xiàng)目，整個(gè)機(jī)器人系統(tǒng)運(yùn)行流暢可以滿(mǎn)足要求。

7 結(jié)語(yǔ)

本文通過(guò)MATLAB機(jī)器人工具箱建立UR10機(jī)器人的數(shù)學(xué)模型。通過(guò)數(shù)學(xué)計(jì)算對(duì)機(jī)器人進(jìn)行帶電作業(yè)的關(guān)鍵路點(diǎn)進(jìn)行正逆運(yùn)動(dòng)學(xué)分析，在運(yùn)動(dòng)學(xué)計(jì)算的基礎(chǔ)上進(jìn)一步進(jìn)行作業(yè)路徑規(guī)劃，通過(guò)對(duì)實(shí)驗(yàn)室模擬環(huán)境對(duì)該分析方法進(jìn)行確認(rèn)，并最終在實(shí)際線路完成了該項(xiàng)作業(yè)任務(wù)。

實(shí)踐證明，仿真分析對(duì)于使用機(jī)器人完成這種特殊作業(yè)任務(wù)具有重要的意義：1）使用仿真軟件為此類(lèi)從事此類(lèi)高風(fēng)險(xiǎn)、環(huán)境復(fù)雜作業(yè)任務(wù)的機(jī)器人建立數(shù)學(xué)模型，通過(guò)模型分析作業(yè)路徑，有助于及時(shí)發(fā)現(xiàn)作業(yè)路徑設(shè)置的問(wèn)題，避免不合理的路點(diǎn)造成機(jī)器人關(guān)節(jié)磨損，影響設(shè)備使用壽命；2）通過(guò)仿真分析可以找到能夠完成作業(yè)任務(wù)的合理位置和最佳路徑，保證整體作業(yè)過(guò)程流暢高效；3）通過(guò)對(duì)機(jī)械臂模型的深入研究，有助于利用機(jī)器人替代更多人工操作，進(jìn)一步降低作業(yè)人員作業(yè)風(fēng)險(xiǎn)和勞動(dòng)強(qiáng)度。