張博研, 李鈺強(qiáng), 吳 晗, 杜錦華

(1.西安交通大學(xué)電氣工程學(xué)院, 陜西 西安 710049; 2.云南電網(wǎng)有限責(zé)任公司昆明供電局, 云南 昆明 650000)

1 引言

隨著中國(guó)社會(huì)經(jīng)濟(jì)不斷發(fā)展,社會(huì)年耗電量逐年擴(kuò)大,到2020年中國(guó)年發(fā)電量已達(dá)到77 790.6億kW·h,這既顯示了我國(guó)電力網(wǎng)越發(fā)龐大與復(fù)雜,也表現(xiàn)了整個(gè)社會(huì)對(duì)電力能源越發(fā)的依賴,使得國(guó)家對(duì)電力供應(yīng)穩(wěn)定性提出了更高的要求[1]。如此,帶電作業(yè)顯得極為關(guān)鍵?？v觀歷史,帶電作業(yè)人員事故頻發(fā),我國(guó)在1995年之后很長(zhǎng)一段時(shí)間內(nèi)限制了帶電作業(yè),但由此造成了供電可靠性下降和社會(huì)經(jīng)濟(jì)損失,也反映出供電穩(wěn)定性與帶電作業(yè)安全性之間巨大不可協(xié)調(diào)的矛盾沖突,而恰巧機(jī)器人及人工智能的蓬勃發(fā)展為此提供了一個(gè)極佳的解決方案,即半智能或智能機(jī)器替代人進(jìn)入危險(xiǎn)環(huán)境作業(yè),由人員遙控操作或者機(jī)器自主操作完成帶電檢修,既保障了電力系統(tǒng)穩(wěn)定性,又提高了帶電作業(yè)的安全性。

國(guó)內(nèi)對(duì)帶電作業(yè)機(jī)器人研究開(kāi)始于20世紀(jì)九十年代,直到2002年3月,國(guó)內(nèi)才誕生第一臺(tái)具有自主知識(shí)產(chǎn)權(quán)的帶電作業(yè)樣機(jī),它僅適用于10 kV以下電壓,由工作人員于絕緣斗內(nèi)近距離操作控制[2,3]。之后,經(jīng)過(guò)不斷研究,至2012年,我國(guó)已經(jīng)發(fā)展了多代帶電作業(yè)機(jī)器人,從只能通過(guò)鍵盤(pán)操作機(jī)械臂,到實(shí)現(xiàn)主從控制機(jī)械臂;由自主研發(fā)電機(jī)驅(qū)動(dòng)到實(shí)現(xiàn)液壓驅(qū)動(dòng),不斷推陳出新。

本文在眾人已有的研究基礎(chǔ)上,針對(duì)帶電作業(yè)機(jī)器人的廣泛應(yīng)用前景,著手于機(jī)械臂緊固螺絲作業(yè)場(chǎng)景進(jìn)行研究,主要基于實(shí)驗(yàn)室PROBOT Anno機(jī)械臂,構(gòu)建工作流程設(shè)計(jì),針對(duì)現(xiàn)有研究中實(shí)時(shí)性差和缺少自主作業(yè)等弊端,旨在提高作業(yè)的智能性、自主性,擴(kuò)展帶電作業(yè)人工智能(Artificial Intelligence,AI)化思路。相對(duì)于人工帶電作業(yè)和其他機(jī)器帶電作業(yè)方式存在的弊端,本文研究的意義和創(chuàng)新點(diǎn)主要體現(xiàn)在以下幾個(gè)方面:

(1)減少人的因素

因?yàn)槿斯し且淮维F(xiàn)場(chǎng)操作,由于攝像頭傳輸?shù)漠?huà)面不一定能反映真實(shí)機(jī)械臂操作反饋,遙控操作的方式不一定準(zhǔn)確可靠,故通過(guò)視覺(jué)伺服反饋?zhàn)灾饕?guī)劃運(yùn)動(dòng)軌跡,控制機(jī)械臂自主到達(dá)目標(biāo)位置,完成作業(yè)任務(wù)。

(2)探討了機(jī)器自主智能檢修的一種可能

通過(guò)視覺(jué)等傳感器,加持AI算法使得機(jī)械臂可以自主追蹤、定位檢修目標(biāo),完成程序任務(wù)。這種方式僅僅需要修改、加持定位目標(biāo)以及加載檢修程序,通過(guò)加載更換末端執(zhí)行器功能模塊,便能在諸多類(lèi)似環(huán)境下持續(xù)應(yīng)用,前景大有可為。

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

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

因力矩控制較位置控制復(fù)雜,本文采用運(yùn)動(dòng)學(xué)理念,忽略動(dòng)力學(xué)屬性,建立機(jī)械臂運(yùn)動(dòng)模型,利用關(guān)節(jié)角度變量輸入控制運(yùn)動(dòng)位置和方向。運(yùn)動(dòng)學(xué)最常用的建模方法是Denavit and Hartenberg參數(shù)法,簡(jiǎn)稱(chēng)D-H法,這是純粹以建立連桿坐標(biāo)系組合,達(dá)到模擬機(jī)械臂的數(shù)學(xué)方法,并通過(guò)相關(guān)函數(shù)能夠極快將計(jì)算得到的D-H參數(shù)轉(zhuǎn)換為末端位姿,進(jìn)行相應(yīng)處理。

本文基于精鋒微控PROBOT Anno機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)建模,這是一款面向教育科研的六軸開(kāi)源桌面型機(jī)械臂。采用標(biāo)準(zhǔn)D-H參數(shù)法(Standard Denavit and Hartenberg,SDH),首先建立連桿與關(guān)節(jié)的編號(hào),對(duì)于連桿,基座為桿0,之后順延至末端連桿(本文至桿6);對(duì)于關(guān)節(jié),連桿i的近基端關(guān)節(jié)為關(guān)節(jié)i,遠(yuǎn)基端關(guān)節(jié)為關(guān)節(jié)i+1,遵循近基端關(guān)節(jié)與連桿標(biāo)號(hào)一致原則。在此基礎(chǔ)上建立連桿坐標(biāo)系,SDH將桿i坐標(biāo)系固定在連桿遠(yuǎn)基端,由于關(guān)節(jié)皆為Revolute類(lèi)型,坐標(biāo)系其Z軸方向保持與關(guān)節(jié)i+1軸線同步,作為其驅(qū)動(dòng)軸,并遵循右手螺旋定則,共建立7個(gè)坐標(biāo)系。建模結(jié)果如圖1所示。

圖1 PROBOT Anno機(jī)械臂連桿坐標(biāo)系Fig.1 PROBOT Anno linkage coordinate system

通常使用一個(gè)齊次變換矩陣T,來(lái)表示兩個(gè)坐標(biāo)系之間的位置位姿變換映射關(guān)系。變換矩陣T由一個(gè)3×3的旋轉(zhuǎn)矩陣R和一個(gè)3×1的位移矩陣P構(gòu)成,一共含有12個(gè)必要參數(shù)。對(duì)于一般的機(jī)械臂來(lái)說(shuō),變換矩陣T,只需對(duì)4個(gè)參數(shù)進(jìn)行研究,便可解析得到矩陣,前提是滿足以下兩個(gè)約束[4]:

(1)坐標(biāo)系O1的X軸垂直于Oi-1的Z軸。

(2)坐標(biāo)系O1的X軸相交于Oi-1的Z軸。

這4個(gè)參數(shù)分別是d、θ、a、α,其具體含義見(jiàn)表1。

表1 參數(shù)含義Tab.1 Parameter meaning

因此坐標(biāo)系{i}到{i-1}的齊次變換矩陣,可以通過(guò)對(duì)應(yīng)的變換步驟,整理解析轉(zhuǎn)換為通用數(shù)學(xué)形式,表示為:

(1)

2.2 正向運(yùn)動(dòng)學(xué)齊次變換求解

正向運(yùn)動(dòng)學(xué)(Forward Kinematics,FK),又名直接運(yùn)動(dòng)學(xué),是將關(guān)節(jié)變量空間變換到笛卡爾坐標(biāo)系空間的數(shù)學(xué)變換。在運(yùn)動(dòng)學(xué)的位置、速度、突變等諸多信息中,FK通常關(guān)注的是位置分析,即本文所需的通過(guò)給定關(guān)節(jié)變量,求解末端位置姿態(tài),所以在本文中FK可等效于上文中提到的坐標(biāo)變換矩陣i-1Ti,對(duì)于PROBOT Anno機(jī)械臂來(lái)說(shuō)FK即綜合變換矩陣0T6。

0T6=0T11T22T33T44T55T6

(2)

將PROBOT Anno機(jī)械臂的SDH參數(shù)代入式(2),并化簡(jiǎn)得到式(3):

(3)

式中,tij為化簡(jiǎn)得到的代數(shù)式,以矩陣第一項(xiàng)為例:

其中, si為sinθi的縮寫(xiě);ci為cosθi的縮寫(xiě)。

由于篇幅有限,不能展示出其余的各項(xiàng)。匯總得到PROBOT Anno機(jī)械臂運(yùn)動(dòng)學(xué)正解,且是唯一的。

2.3 逆向運(yùn)動(dòng)學(xué)迭代求解

逆向運(yùn)動(dòng)學(xué)(Inverse Kinematics,IK)即將笛卡爾空間坐標(biāo)轉(zhuǎn)換至關(guān)節(jié)變量空間,是從已知的末端位置信息出發(fā),獲得機(jī)械臂關(guān)節(jié)變量的數(shù)學(xué)變換。在機(jī)械臂運(yùn)動(dòng)控制的總體過(guò)程中,一般在關(guān)節(jié)變量空間靠關(guān)節(jié)變量來(lái)驅(qū)動(dòng)機(jī)械運(yùn)動(dòng),在笛卡爾坐標(biāo)系規(guī)定的空間中,規(guī)劃?rùn)C(jī)械臂終端運(yùn)動(dòng)的路徑與軌跡,所以求解逆運(yùn)動(dòng)學(xué)是非常有必要的。目前對(duì)于IK問(wèn)題,仍然還沒(méi)有標(biāo)準(zhǔn)和常規(guī)應(yīng)用求解方法,但可以使用相關(guān)的分析方法和數(shù)值解法得到逆解,例如解耦、逆變換、迭代、螺旋代數(shù)、雙重矩陣、雙重四元數(shù)等。由于PROBOT Anno機(jī)械臂后三軸不僅改變末端執(zhí)行器的姿態(tài),同時(shí)也改變了其位置,因此該款機(jī)械臂違背了Pieper 準(zhǔn)則,不能采用正解矩陣的逆變換求解,所以將采用迭代法的原理[5]。

組合式(2)和式(3)的表達(dá)式,展示如式(4),它是一組非線性代數(shù)方程,迭代法求解的便是它的一組解。最常用的方法便是Newton-Raphson法。

(4)

δT=T-T*

(5)

(6)

(7)

對(duì)式(7)進(jìn)行一階泰勒展開(kāi)得到式(8):

(8)

(9)

(10)

(11)

由上得迭代一般步驟:

(1)設(shè)置初始猜想值。

(2)計(jì)算殘差,判斷誤差,是否停止迭代。

(3)計(jì)算迭代因子,更新初始值。

(4)返回步驟(2)。

(5)輸出迭代結(jié)果和其他迭代信息。

3 機(jī)械臂目標(biāo)識(shí)別與定位

目標(biāo)識(shí)別是通過(guò)攝像頭或者其他方式獲得環(huán)境圖片信息,通過(guò)圖像識(shí)別算法模塊進(jìn)行處理,提取目標(biāo)三維位置和其他相關(guān)信息,并對(duì)得到的信息類(lèi)編碼處理,使輸出信息與下一模塊端口匹配。圖像處理的前提是得到所需要的圖像,在這個(gè)過(guò)程中,一般使用透視相機(jī),它的作用是將三維世界以透視投影的方式二維化。在相機(jī)成像中,像素坐標(biāo)是必不可少的。因此需要得到像素坐標(biāo)系上某點(diǎn)的齊次形式q=(u,v,1)T與世界坐標(biāo)系點(diǎn)P=(Xw,Yw,Zw,1)T之間的關(guān)系。影響其轉(zhuǎn)換關(guān)系的主要因素為相機(jī)參數(shù)。在確定參數(shù)后可以簡(jiǎn)單地將轉(zhuǎn)換公式寫(xiě)作:

(12)

3.1 基于顏色空間解析目標(biāo)信息

顏色空間是一個(gè)三維度空間,包含了所有可能的三色刺激值,涵蓋了顏色和亮度的概念。計(jì)算機(jī)視覺(jué)發(fā)展至今,已經(jīng)形成了諸多顏色空間,常用顏色空間見(jiàn)表2。機(jī)械臂作業(yè)環(huán)境背景亮度可控,故選用RGB顏色空間進(jìn)行后續(xù)其他圖像處理操作即可,并預(yù)先將圖像顏色空間RGB分離開(kāi),使得相關(guān)操作僅對(duì)單一灰度處理,簡(jiǎn)化算法。本節(jié)主要采用了平滑、邊緣檢測(cè)、直線檢測(cè)等操作。

表2 常用顏色空間Tab.2 Common color space

圖2 處理之中值濾波效果Fig.2 Median filtering effect

邊緣檢測(cè)主要為了得到圖像中各組成部分的輪廓。一般過(guò)程即處理灰度圖中,圖像梯度變化極大的像素點(diǎn)點(diǎn)集即是輪廓。邊緣檢測(cè)成熟的算法也有很多,本文采用的是經(jīng)典的Canny 邊緣檢測(cè)算法。該算法通過(guò)濾波、增強(qiáng)、檢測(cè)三步完成邊緣檢測(cè),濾波可通過(guò)本節(jié)前述方法實(shí)現(xiàn),增強(qiáng)即是利用Sobel算子計(jì)算梯度幅值和方向,檢測(cè)即是利用滯后雙閾值完成強(qiáng)弱邊緣篩選和輪廓跟蹤。

直線檢測(cè)主要在邊緣檢測(cè)的基礎(chǔ)上,得到目標(biāo)輪廓直線位置和方向?qū)傩?為螺絲緊固作業(yè)的末端執(zhí)行器與目標(biāo)對(duì)焦獲取信息。常用Hough 變換來(lái)做處理,處理效果如圖3所示,圖3(a)為直線檢測(cè)圖,穿過(guò)目標(biāo)輪廓的直線即為檢測(cè)出來(lái)的目標(biāo)邊緣直線擴(kuò)展顯示,圖3(b)同步顯示了直線的角度(theta)和其他檢測(cè)信息,需要注意,這里的角度0是以垂直線為初始,順時(shí)針擴(kuò)大。

圖3 Hough 變換處理效果及其信息終端顯示Fig.3 Hough transform processing effect and its information terminal display

3.2 目標(biāo)識(shí)別綜合仿真

ROS即Robot Operating System,是一個(gè)開(kāi)源框架系統(tǒng),OpenCV即Open source Computer Vision library,是一個(gè)跨平臺(tái)的開(kāi)源視覺(jué)庫(kù)[6]。本節(jié)在Ubuntu18.04操作系統(tǒng)環(huán)境下,將ROS和OpenCV結(jié)合,對(duì)傳遞的圖像或相機(jī)采集到的圖像,按圖4所示流程編寫(xiě)程序進(jìn)行處理,識(shí)別出目標(biāo)的位置和偏轉(zhuǎn)角度等信息。

圖4 圖像處理簡(jiǎn)要程序流程Fig.4 Image processing brief program flow

通過(guò)編譯運(yùn)行,識(shí)別效果通過(guò)rqt工具展示如圖5所示,其中展示了算法實(shí)時(shí)處理圖像數(shù)據(jù),以矩形框選目標(biāo),并以黑色填充可視化邊緣信息,輸出得到目標(biāo)在世界坐標(biāo)系下位置以及偏轉(zhuǎn)設(shè)定基準(zhǔn)線的角度,并將關(guān)鍵信息基于話題通信方式傳送出去,等待被其他程序訂閱處理。

圖5 以rqt命令展示的圖像識(shí)別效果Fig.5 Image recognition effect shown by rqt command

隨后將圖像識(shí)別模塊應(yīng)用在Simulink中,其處理效果展示如圖6所示,其中圖6(a)為整體展示,圖6(b)為局部放大圖。由于實(shí)際物體受于相機(jī)像素和環(huán)境噪聲影響,此處以仿真世界環(huán)境作為識(shí)別源代替。從圖6可以看到螺絲模型目標(biāo)被矩形圈出,并以黑色點(diǎn)標(biāo)出目標(biāo)中心位置。

圖6 Simulink圖像識(shí)別效果Fig.6 Simulink image recognition effect

同時(shí)根據(jù)螺絲模型的創(chuàng)造信息和圖像識(shí)別出的位置信息對(duì)比見(jiàn)表3,結(jié)果差值在毫米級(jí)別,證明了該模塊處理圖像的可行性。

表3 圖像識(shí)別信息與實(shí)際比對(duì)Tab.3 Comparison of image recognition information and actual position

經(jīng)過(guò)對(duì)算法的分析,誤差來(lái)源的一大原因是,獲取目標(biāo)的原始圖像分辨率不夠高,僅有800×800,導(dǎo)致在對(duì)灰度圖處理的過(guò)程中損失了很多信息,且算法采用先橫向后縱向處理邏輯,使得y維度損失高于x維度,實(shí)際造成y維度誤差大于x。

4 運(yùn)動(dòng)路徑規(guī)劃

一般來(lái)說(shuō),機(jī)械臂的運(yùn)動(dòng)規(guī)劃,涵蓋兩個(gè)方面,即路徑和軌跡,探討的是路程最短、時(shí)間最優(yōu)、功耗最低等問(wèn)題。路徑,指的是在兩個(gè)維度點(diǎn)之間,避開(kāi)不可穿越的維度壁障,尋到的一條道路,它是空間維度點(diǎn)的集合;軌跡可以看作是路徑的接續(xù),只有先得到路徑維度集合,作為給定參數(shù),才能得到帶有時(shí)間維度的軌跡,在這個(gè)過(guò)程中通常也規(guī)范了機(jī)械臂的速度和加速度等約束。

4.1 基于A*算法實(shí)現(xiàn)最短路程設(shè)計(jì)

路徑規(guī)劃算法自提出起,便不斷發(fā)展,形成多種常用算法,隨著技術(shù)進(jìn)步逐漸與其他領(lǐng)域結(jié)合,衍生了多種多樣的規(guī)劃算法,如基于軸突延遲的尖峰神經(jīng)網(wǎng)絡(luò)路徑算法和隨機(jī)蛙跳等[7,8]。

就普通的小康家庭來(lái)說(shuō)，其實(shí)大體就分為四塊：衣食住行、保險(xiǎn)、儲(chǔ)蓄備用、投資創(chuàng)富。目前對(duì)中國(guó)的居民來(lái)說(shuō)，主要有以下大類(lèi)資產(chǎn)種類(lèi)：股票、債券、貨幣、房地產(chǎn)、商品。每類(lèi)資產(chǎn)又可以細(xì)分為很多子范疇，股票是股份有限公司在籌集資本時(shí)向出資人發(fā)行的股份憑證，是社會(huì)化大生產(chǎn)的產(chǎn)物。股票又可分為大盤(pán)股/小盤(pán)股；股票在交易市場(chǎng)上作為交易對(duì)象，同商品一樣，有自己的市場(chǎng)行情和市場(chǎng)價(jià)格。由于股票價(jià)格要受到諸如公司經(jīng)營(yíng)狀況、供求關(guān)系、銀行利率、大眾心理等多種因素的影響，其波動(dòng)有很大的不確定性。正是這種不確定性，有可能使股票投資者遭受損失。價(jià)格波動(dòng)的不確定性越大，投資風(fēng)險(xiǎn)也越大。因此，股票是一種高風(fēng)險(xiǎn)的金融產(chǎn)品。

在螺絲緊固作業(yè)中,目標(biāo)點(diǎn)為螺絲,其三維空間狀態(tài)基本是固定的,加上忽略風(fēng)力,以架空線、絕緣子和桿塔為主的障礙也基本是固定的,所以可采用靜態(tài)規(guī)劃算法。在靜態(tài)算法中尤以A*為突出代表,因它自帶的靈活性優(yōu)勢(shì),能廣泛快速運(yùn)用于不同情境中,故本文中也采取A*作為PROBOT Anno機(jī)械臂的路徑規(guī)劃器。

A*作為一種成功的人工智能啟發(fā)式算法,它的核心在于由Dijkstra算法(靠近源心的結(jié)點(diǎn))和BFS(Breath First Search)算法(靠近靶子的結(jié)點(diǎn))的信息塊結(jié)合起來(lái)的啟發(fā)式函數(shù)f(n)[9]。

f(n)=g(n)+h(n)

(13)

式中,f(n)為任意節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的最小估價(jià);g(n)為源節(jié)點(diǎn)到任意節(jié)點(diǎn)n的代價(jià);h(n)為目標(biāo)節(jié)點(diǎn)和任意節(jié)點(diǎn)n之間的成本。

啟發(fā)式函數(shù)f(n)對(duì)于A*算法有著極大的影響,h(n)越準(zhǔn)確,算法搜索速度越快,最理想的情況下,h(n)和g(n)大小完美匹配,那么沿所尋到路徑,f(n)的值將基本不變,也就是說(shuō),A*算法尋路相當(dāng)于尋找f(n)低值的聯(lián)合體。除了啟發(fā)式函數(shù)外,A*算法還具備兩個(gè)關(guān)鍵組分,分別是開(kāi)放列表和關(guān)閉列表,前者用來(lái)存放等待檢查評(píng)估代價(jià)的子節(jié)點(diǎn),通常是父節(jié)點(diǎn)周?chē)噜徦泄?jié)點(diǎn);后者用來(lái)存放不需要再次檢查評(píng)估的節(jié)點(diǎn),通常是從開(kāi)放列表中刪除的節(jié)點(diǎn)。通用程序?qū)崿F(xiàn)流程如圖7所示,可以看到,整個(gè)程序執(zhí)行循環(huán)搜索之前,就已計(jì)算完空間各節(jié)點(diǎn)對(duì)于目標(biāo)的h(n)。這里采用這種形式的目的有兩個(gè),一是加快循環(huán)速度;二是便于修改h(n)或?yàn)槠湓黾訖?quán)重等附加約束。

圖7 A*算法實(shí)現(xiàn)流程Fig.7 A* algorithm implementation flow chart

三維路徑規(guī)劃的實(shí)現(xiàn)仿真如圖8所示,由于三維空間展示復(fù)雜度遠(yuǎn)大于二維,為便于展示尋路過(guò)程與效果,網(wǎng)格大小選擇10×10×10,起點(diǎn)、終點(diǎn)、空間障礙物通過(guò)代碼進(jìn)行初始化給定。在圖8中方塊、空心球、星號(hào)的中心位置信息即為該立方體網(wǎng)格索引信息,其中不同灰度的立方體分別代表起點(diǎn)、終點(diǎn)、路徑和障礙物,空心球代表開(kāi)放搜索列表,黑色星號(hào)代表關(guān)閉列表和開(kāi)放搜索確定的代價(jià)最小子節(jié)點(diǎn)。

圖8 A*算法三維度Matlab仿真實(shí)現(xiàn)Fig.8 A* algorithm three dimensional Matlab simulation implementation

4.2 梯形軌跡規(guī)劃設(shè)計(jì)

軌跡規(guī)劃,主要包含兩大類(lèi)——關(guān)節(jié)空間軌跡規(guī)劃和笛卡爾空間軌跡規(guī)劃。本文中軌跡規(guī)劃的實(shí)現(xiàn),采用了偽笛卡爾空間軌跡規(guī)劃,是通過(guò)在依靠路徑規(guī)劃得到的三維空間路徑離散點(diǎn)集的前提下,直接將每?jī)蓚€(gè)離散點(diǎn)之間視為存在一條離散直線段,進(jìn)而采取固定時(shí)間對(duì)其進(jìn)行插值,插值完成后,將其三維坐標(biāo)通過(guò)逆運(yùn)動(dòng)學(xué)模塊,變換至關(guān)節(jié)變量空間,這里與笛卡爾空間軌跡規(guī)劃不同的是,不再進(jìn)行關(guān)節(jié)空間軌跡詳細(xì)規(guī)劃,而是依靠前節(jié)逆運(yùn)動(dòng)學(xué)迭代法所闡述的原理,即期望值向著初始值靠近。而固定時(shí)間插值點(diǎn)維度坐標(biāo)的選取,常用的方法有梯形和S曲線速度分布,本文采用梯形分布。梯形分布較S曲線精度稍弱,但運(yùn)動(dòng)速度更快[10]。

梯形速度分布將兩點(diǎn)之間,每個(gè)維度的速度分布劃為勻加速(T0～Ta)、勻速(Ta～Tb)、勻減速(Tb～T)三個(gè)階段,如圖9所示?；谠摲N分配方式,可以看到位置隨著時(shí)間雖然呈現(xiàn)非線性變化,但是變化程度是平滑的。

圖9 時(shí)間T內(nèi)某一維度梯形分布軌跡位置x、速度v、加速度a隨時(shí)間變化圖Fig.9 Change of position, velocity and acceleration of a trapezoidal trajectory in a certain dimension with time T

根據(jù)圖9并結(jié)合已知參數(shù),可得到如下三個(gè)關(guān)系公式,即位置時(shí)間關(guān)系表達(dá)式(14)、速度時(shí)間關(guān)系表達(dá)式(15)、加速度時(shí)間關(guān)系表達(dá)式(16)。通過(guò)這三個(gè)表達(dá)式和相應(yīng)輸入?yún)?shù),便可規(guī)劃出空間兩點(diǎn)之間各維度運(yùn)行平滑的速度分配軌跡。

(14)

(15)

(16)

式中,x0、x1分別為起始和終止位置;t0為開(kāi)始時(shí)間;Ta為加速、減速時(shí)間;vm為最大速度。

4.3 碰撞檢測(cè)功能設(shè)計(jì)

本文設(shè)計(jì)的機(jī)械臂緊固螺絲作業(yè)中,需要自主規(guī)劃路徑,避開(kāi)障礙物,使得機(jī)械臂末端執(zhí)行器能夠順利完成作業(yè)任務(wù)。在這之中,避免機(jī)械臂與障礙物之間,以及避免機(jī)械臂自身連桿發(fā)生碰撞是非常關(guān)鍵的一段設(shè)計(jì)環(huán)節(jié),所以對(duì)相關(guān)部分進(jìn)行碰撞檢測(cè)和碰撞預(yù)防是驅(qū)動(dòng)機(jī)械臂路徑規(guī)劃和軌跡跟蹤的重要前提[11]。由于本文設(shè)計(jì)應(yīng)用的螺絲緊固桿塔作業(yè)場(chǎng)景具有可重復(fù)性,且抽象到仿真世界的環(huán)境障礙信息是固定的、已知的,故采用靜態(tài)檢測(cè)的理念。因?yàn)榭梢灶A(yù)先對(duì)機(jī)械臂和環(huán)境障礙信息進(jìn)行幾何外形層次的簡(jiǎn)化處理,對(duì)此可采用層次包絡(luò)盒方法,即是對(duì)機(jī)械臂各連桿和環(huán)境障礙基于常用空間規(guī)則幾何形狀做包絡(luò)抽象處理,使得碰撞檢測(cè)問(wèn)題聚焦為空間幾何形狀是否發(fā)生重疊。常用的包絡(luò)方法有球體、柱體、AABB、OBB等。

對(duì)于自體碰撞處理,通過(guò)將機(jī)械臂的基礎(chǔ)連桿和連桿一的空間信息處理成自體障礙信息,將其固化在前文尋路算法的處理程序中,使得算法得到的末端執(zhí)行器路徑避開(kāi)自體連桿空間,達(dá)到避免碰撞的目的。處理結(jié)果如圖10所示。

圖10 機(jī)械臂自體碰撞區(qū)域轉(zhuǎn)化為障礙包絡(luò)處理Fig.10 Auto-collision area of robotic arm transformed into an obstacle envelope processing

二是機(jī)械臂與環(huán)境障礙之間的碰撞處理。此處對(duì)已經(jīng)簡(jiǎn)化過(guò)的環(huán)境信息,利用長(zhǎng)方體(AABB)再次進(jìn)行包絡(luò)處理,本文中由于抽象建立的仿真障礙信息位置的特殊性,碰撞處理忽略其Y維度信息,關(guān)注其體截面中心和X、Z軸信息。對(duì)于PROBOT Anno機(jī)械臂來(lái)說(shuō),末端執(zhí)行器由于已經(jīng)進(jìn)行路徑自主避障規(guī)劃處理,所以末端部分不再需要碰撞檢測(cè)處理,此處僅考慮連桿三、四、五,對(duì)這幾個(gè)連桿再次簡(jiǎn)化,將連桿末端抽象成一個(gè)點(diǎn),通過(guò)齊次變換矩陣計(jì)算該點(diǎn)的X、Z軸信息,與障礙物包絡(luò)后的截面中心X、Z軸信息進(jìn)行差處理,確保差值大于一定值,從而避免碰撞發(fā)生。根據(jù)以上的思路編寫(xiě)碰撞檢測(cè)算法函數(shù),仿真驗(yàn)證結(jié)果如圖11所示。

圖11 防碰撞模塊啟用前后對(duì)比圖Fig.11 Comparison before and after anti-collision module is enabled

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

綜合第3、第4節(jié)所述模塊,將機(jī)械臂運(yùn)動(dòng)學(xué)、圖像識(shí)別、運(yùn)動(dòng)規(guī)劃、碰撞檢測(cè)等銜接起來(lái),得到一套基于PROBOT Anno機(jī)械臂自主緊固螺絲作業(yè)抽象設(shè)計(jì),并用Simulink在仿真環(huán)境中展現(xiàn)出來(lái)。最后,為了驗(yàn)證整體設(shè)計(jì)的有效性,將其與實(shí)物連接,進(jìn)行相關(guān)部分驗(yàn)證。

基于前文介紹的工作,綜合作業(yè)實(shí)現(xiàn)設(shè)計(jì)思路如下:

(1)架構(gòu)世界笛卡爾坐標(biāo)系,以PROBOT Anno機(jī)械臂基點(diǎn)為原點(diǎn),其他坐標(biāo)系處理結(jié)果最終轉(zhuǎn)換到此系,在此系內(nèi)進(jìn)行規(guī)劃和顯示工作。

(2)對(duì)各部分參數(shù)進(jìn)行初始化,獲得或給定機(jī)械臂末端起始基礎(chǔ)位姿。

(3)通過(guò)攝像頭或者其他方式獲得環(huán)境信息,通過(guò)圖像識(shí)別算法模塊進(jìn)行處理,提取目標(biāo)三維位置和其他相關(guān)信息。

(4)通過(guò)以A*算法為基礎(chǔ)編寫(xiě)路徑規(guī)劃模塊,自主規(guī)劃出機(jī)械臂末端的空間無(wú)碰撞路徑點(diǎn)集,之后對(duì)空間路徑離散點(diǎn)之間進(jìn)行梯形速度分配規(guī)劃。

(5)用BFGS(Broyden-Fletcher-Goldfarb-Shanno)迭代法進(jìn)行逆運(yùn)動(dòng)學(xué)求解,驅(qū)動(dòng)機(jī)械臂完成軌跡點(diǎn)跟蹤,完成作業(yè)任務(wù)。

流程中,數(shù)據(jù)信息的傳送是基于陣列形式的一次傳輸,故沒(méi)有循環(huán)控制。在整個(gè)設(shè)計(jì)思路中,對(duì)環(huán)境信息、機(jī)械臂力學(xué)參數(shù)及帶電作業(yè)程序都進(jìn)行了簡(jiǎn)化、忽略和抽象處理,將目光更多著眼于緊固螺絲作業(yè)整體流程的功能實(shí)現(xiàn)。作出帶電操作緊固螺絲作業(yè)整體規(guī)劃設(shè)計(jì)流程如圖12所示。

圖12 基于PROBOT Anno機(jī)械臂緊固螺絲作業(yè)仿真實(shí)現(xiàn)簡(jiǎn)要設(shè)計(jì)流程Fig.12 Simple design process realized based on PROBOT Anno arm fastening screw operation simulation

由于Matlab軟件上已具有和ROS通信的能力,并且實(shí)驗(yàn)室所擁有的PROBOT Anno機(jī)械臂是在ROS下通過(guò)輸入關(guān)節(jié)信息給ROBCELL控制器達(dá)到控制驅(qū)動(dòng)電機(jī)目標(biāo)的。這就給予了實(shí)物驗(yàn)證極大的便利,本文通過(guò)ROS Master通信機(jī)制,將Matlab算法規(guī)劃得到關(guān)節(jié)變量以話題的形式發(fā)布出去,繼而在ROS里面通過(guò)終端訂閱處理,再傳遞給機(jī)械臂控制臺(tái),驅(qū)動(dòng)機(jī)械臂關(guān)節(jié)軸完成軌跡跟蹤。同樣基于此,可在Ubuntu系統(tǒng)中,將ROS+OpenCV+USB攝像頭得到的圖像處理結(jié)果信息發(fā)布給Matlab,使得Matlab專(zhuān)注于運(yùn)動(dòng)規(guī)劃算法設(shè)計(jì)。

緊接著在Simulink中搭建仿真模型,并將仿真結(jié)果以可視化三維圖動(dòng)態(tài)展示出來(lái),整體仿真規(guī)劃效果如圖13所示,其中①表示末端初始位置,“°”是規(guī)劃的路徑,②是簡(jiǎn)化模擬的環(huán)境障礙,③是抽象簡(jiǎn)化的目標(biāo)螺絲。由圖13中能夠看到,仿真極好地避開(kāi)了障礙物,提前規(guī)劃出一條無(wú)碰撞的路徑。

圖13 PROBOT Anno機(jī)械臂緊固螺絲作業(yè)可視化仿真Fig.13 Visual simulation of PROBOT Anno arm fastening screws

整個(gè)作業(yè)任務(wù),末端軌跡是分段平滑的直線段組合,直線段連接角度皆為鈍角,整個(gè)空間軌跡沒(méi)有出現(xiàn)瞬時(shí)的尖峰干擾,符合設(shè)計(jì)預(yù)期,如圖14所示。另外由圖15展示的各關(guān)節(jié)角度θ時(shí)間變化圖,可以看到,整個(gè)運(yùn)動(dòng)過(guò)程中,角度變化是平滑的,確保了運(yùn)動(dòng)的精確定位和機(jī)械臂電機(jī)的穩(wěn)定運(yùn)行,驗(yàn)證了設(shè)計(jì)模型的正確性。

圖14 機(jī)械臂緊固螺絲作業(yè)過(guò)程末端空間軌跡Fig.14 Space trajectory at end of screw tightening process of manipulator

圖15 PROBOT Anno機(jī)械臂緊固螺絲作業(yè)過(guò)程中各關(guān)節(jié)軸角度變化Fig.15 Angle of each joint axis changes during the fastening screw operation of PROBOT Anno robot arm

隨后將仿真文件中的可視化更換為ROS通信模塊,將Matlab與ROS時(shí)控機(jī)械臂結(jié)合起來(lái),完成機(jī)械臂實(shí)物運(yùn)動(dòng)規(guī)劃控制,結(jié)果展示如圖16所示,機(jī)械臂能夠很好自主越過(guò)白色塊狀障礙物,其與障礙物距離極近,但是卻未發(fā)生碰撞,極大證明了目標(biāo)識(shí)別、運(yùn)動(dòng)規(guī)劃和碰撞檢測(cè)的正確性。

圖16 PROBOT Anno機(jī)械臂緊固螺絲作業(yè)過(guò)程中各關(guān)節(jié)軸角度變化Fig.16 Angle of each joint axis changes during the fastening screw operation of PROBOT Anno robot arm

6 結(jié)論

本文主要對(duì)機(jī)械臂帶電緊固螺絲自主作業(yè)進(jìn)行了研究,針對(duì)現(xiàn)有研究中帶電緊固螺絲作業(yè)存在的問(wèn)題,例如人工作業(yè)風(fēng)險(xiǎn)大和機(jī)械作業(yè)缺乏自主性、實(shí)時(shí)性差等問(wèn)題,利用實(shí)驗(yàn)室現(xiàn)有PROBOT Anno機(jī)械臂,根據(jù)帶電作業(yè)緊固螺絲作業(yè)規(guī)劃研究的架設(shè)要求,基于前人已有的研究基礎(chǔ),抽象相關(guān)作業(yè)環(huán)境,簡(jiǎn)化基本作業(yè)操作,對(duì)PROBOT Anno機(jī)械臂實(shí)物進(jìn)行了深入了解,同時(shí)創(chuàng)新性地加入了目標(biāo)識(shí)別、路徑規(guī)劃等功能,實(shí)現(xiàn)了自主作業(yè)。

本文首先詳細(xì)分析了運(yùn)動(dòng)學(xué)相關(guān)原理,包括機(jī)械建模、坐標(biāo)變換、正逆求解等。隨后綜合機(jī)械臂手冊(cè)數(shù)據(jù)規(guī)劃了機(jī)械臂末端運(yùn)行空間,并利用包絡(luò)模型簡(jiǎn)化機(jī)械臂和抽象障礙,設(shè)計(jì)出異體簡(jiǎn)化碰撞檢測(cè)功能模塊。選用啟發(fā)式A*算法,自主規(guī)劃出最短路徑,利用相關(guān)模塊完成梯形速度分布軌跡插補(bǔ)。同時(shí)在Ubuntu18.04環(huán)境下利用OpenCV實(shí)現(xiàn)了實(shí)物檢測(cè)處理。最后將各功能模塊在Simulink平臺(tái)結(jié)合,完成總體作業(yè)仿真實(shí)現(xiàn)。最后通過(guò)ROS通信機(jī)制,在Ubuntu18.04和Matlab2021a之間建立信息傳輸通道,使得Matlab仿真模塊能夠控制PROBOT Anno機(jī)械臂,進(jìn)行算法實(shí)物驗(yàn)證?？傮w來(lái)看,仿真和實(shí)物的實(shí)現(xiàn)效果證明了緊固螺絲自主作業(yè)設(shè)計(jì)的可行性和有效性。

本文下一步的工作內(nèi)容將集中在拓展機(jī)械臂功能,例如增加帶電巡檢功能,實(shí)現(xiàn)巡檢-診斷-修復(fù)工作流程,填補(bǔ)現(xiàn)有研究的空缺[12]。