馬仲航， 張執(zhí)南

(上海交通大學(xué) 機(jī)械與動(dòng)力工程學(xué)院， 上海 200240)

高空作業(yè)，如橋梁檢修、風(fēng)電機(jī)組維護(hù)具有較高的危險(xiǎn)性.利用無(wú)人機(jī)(UAV)結(jié)合機(jī)械臂替代人工完成高空作業(yè)任務(wù)不僅可以大幅降低發(fā)生重大事故的風(fēng)險(xiǎn)，還具有廣闊的市場(chǎng)應(yīng)用空間.因此，無(wú)人機(jī)結(jié)合機(jī)械臂的研究獲得了國(guó)內(nèi)外的廣泛關(guān)注[1].

目前，相關(guān)研究主要集中在空中抓取、空中搬運(yùn)與空中遙操3個(gè)方面.雖然上述研究組成形式類似，但側(cè)重點(diǎn)各有不同.空中抓取指無(wú)人機(jī)在動(dòng)態(tài)飛行過(guò)程中，利用機(jī)械臂抓取特定目標(biāo)[2]；空中搬運(yùn)指利用單自由度的抓手或繩索實(shí)現(xiàn)目標(biāo)物體的空中運(yùn)輸[3]；空中遙操指利用遙控方法控制無(wú)人機(jī)以及機(jī)載的多自由度手臂，進(jìn)而實(shí)現(xiàn)空中特定任務(wù)的操作[4].空中遙操機(jī)械臂是基于空中遙操理念設(shè)計(jì)的六旋翼無(wú)人機(jī)和七自由度機(jī)械臂集成的空中機(jī)器人系統(tǒng)，通過(guò)分級(jí)控制將機(jī)械臂的運(yùn)動(dòng)作為無(wú)人機(jī)的外部擾動(dòng)加以補(bǔ)償，以實(shí)現(xiàn)復(fù)雜環(huán)境下無(wú)人機(jī)與多自由度機(jī)械臂的精細(xì)化操作.然而，為提高無(wú)人機(jī)遙操抓取的穩(wěn)定性，還需設(shè)計(jì)和驗(yàn)證空中遙操機(jī)械臂的控制算法.

無(wú)人機(jī)控制算法驗(yàn)證是無(wú)人機(jī)試飛前最為重要的工作.仿真是驗(yàn)證無(wú)人機(jī)控制算法的主要方法.目前，大多數(shù)仿真驗(yàn)證方法主要利用MATLAB等軟件進(jìn)行數(shù)值仿真[5]，缺乏實(shí)時(shí)的可視化顯示，無(wú)法直觀地獲取無(wú)人機(jī)的實(shí)時(shí)狀態(tài).本文基于Virtual Robot Experimentation Platform (V-REP)軟件的二次開(kāi)發(fā)，研制一套應(yīng)用于多施翼無(wú)人機(jī)遙操機(jī)械臂的多功能仿真實(shí)驗(yàn)平臺(tái)，實(shí)現(xiàn)對(duì)無(wú)人機(jī)-機(jī)械臂聯(lián)合動(dòng)力學(xué)的可視化仿真；提出一種基于質(zhì)心實(shí)時(shí)采集的無(wú)人機(jī)增穩(wěn)算法，在簡(jiǎn)化動(dòng)力學(xué)模型的同時(shí)實(shí)現(xiàn)無(wú)人機(jī)對(duì)機(jī)械臂擾動(dòng)的補(bǔ)償；在抓取作業(yè)階段，基于機(jī)器人逆運(yùn)動(dòng)學(xué)，實(shí)現(xiàn)機(jī)械臂對(duì)無(wú)人機(jī)擾動(dòng)的隔離，高質(zhì)量地完成增穩(wěn)抓取任務(wù).

1 空中遙操機(jī)械臂動(dòng)力學(xué)模型構(gòu)建

空中遙操機(jī)械臂采用分級(jí)控制策略來(lái)處理無(wú)人機(jī)與機(jī)械臂之間的擾動(dòng).基于該思路，首先研究七自由度機(jī)械臂的質(zhì)心采集方法，將機(jī)械臂質(zhì)心的變動(dòng)作為機(jī)械臂對(duì)無(wú)人機(jī)的擾動(dòng)，以簡(jiǎn)化空中遙操機(jī)械臂的動(dòng)力學(xué)模型.

1.1 實(shí)時(shí)質(zhì)心跟蹤采集

七自由度機(jī)械臂的各臂節(jié)之間依靠轉(zhuǎn)動(dòng)副連接，由Hatenberg機(jī)器人位姿描述方法[6]，可獲得每一臂節(jié)質(zhì)心坐標(biāo)系相對(duì)于世界坐標(biāo)系的位姿關(guān)系.各臂節(jié)間的位姿轉(zhuǎn)換矩陣如下[7]：

(1)

式中：βi為從xi-1到xi繞zi的旋轉(zhuǎn)角；αi-1為從zi-1到zi繞xi-1的旋轉(zhuǎn)角；di為從xi-1到xi沿zi方向的距離.動(dòng)力鏈的位姿變換關(guān)系如下：

(2)

根據(jù)文獻(xiàn)[7]的推導(dǎo)結(jié)果，多體系統(tǒng)質(zhì)心求解的結(jié)果如下：

(3)

式中：xio、yio、zio分別表示第i鏈節(jié)質(zhì)心相對(duì)于機(jī)器人基礎(chǔ)坐標(biāo)系的坐標(biāo)；xcom、ycom、zcom分別表示多體系統(tǒng)的質(zhì)心相對(duì)于機(jī)器人基礎(chǔ)坐標(biāo)系的坐標(biāo).需要指出的是，在下文的討論中機(jī)器人基礎(chǔ)坐標(biāo)系即為無(wú)人機(jī)機(jī)體坐標(biāo)系.

在V-REP軟件中利用Lua語(yǔ)言編寫(xiě)腳本實(shí)現(xiàn)質(zhì)心的跟蹤功能.利用Threaded腳本的循環(huán)執(zhí)行特性可以保證跟蹤動(dòng)力鏈質(zhì)心的實(shí)時(shí)性，利用其提供的庫(kù)函數(shù)可以降低編程實(shí)現(xiàn)的難度.該算法實(shí)現(xiàn)的流程圖如圖1所示.其中，調(diào)用V-REP庫(kù)函數(shù)simGetShapeMassAndInertia()可實(shí)現(xiàn)對(duì)模型質(zhì)量和轉(zhuǎn)動(dòng)慣量的獲??；調(diào)用V-REP庫(kù)函數(shù)simGetObjectPosition()可訪問(wèn)物體的空間位置；調(diào)用simGetObjectMatrix()可以獲得任意兩物體之間的位姿轉(zhuǎn)換矩陣.多施翼無(wú)人機(jī)遙操機(jī)械臂實(shí)物及其仿真場(chǎng)景如圖2所示.

圖1 質(zhì)心跟蹤算法框圖

圖2 質(zhì)心位置實(shí)時(shí)跟蹤顯示

1.2 多施翼無(wú)人機(jī)遙操機(jī)械臂動(dòng)力學(xué)模型

圖3 空中遙操機(jī)械臂坐標(biāo)系示意圖

按所定義的Euler角順序，可建立從機(jī)體坐標(biāo)系至世界坐標(biāo)系的轉(zhuǎn)換矩陣R：

(4)

根據(jù)文獻(xiàn)[8]，六旋翼無(wú)人機(jī)的每個(gè)螺旋槳均能產(chǎn)生升力Fi和力矩Mi，且均與轉(zhuǎn)速γi有關(guān)，其關(guān)系式為

(5)

式中：kF為升力系數(shù)；kM為力矩系數(shù).根據(jù)文獻(xiàn)[9]，將無(wú)人機(jī)的總升力表示為在世界坐標(biāo)系下沿坐標(biāo)軸的分力:

(6)

式中：Bf為無(wú)人機(jī)在機(jī)體坐標(biāo)系下的升力.定義向量m的3個(gè)分量分別代表沿機(jī)體坐標(biāo)系的3個(gè)坐標(biāo)軸方向的扭矩.

A=

(7)

式中：l為六旋翼無(wú)人機(jī)電機(jī)至機(jī)體質(zhì)心的距離.由Newton第2定律可知

(8)

式中：mpla為無(wú)人機(jī)的質(zhì)量；g為世界坐標(biāo)系中的重力加速度；wez為世界坐標(biāo)系中代表重力方向的單位向量.由Newton-Euler方程，在機(jī)體坐標(biāo)系下有

(9)

式中：Jpla為六旋翼無(wú)人機(jī)的轉(zhuǎn)動(dòng)慣性矩陣.根據(jù)文獻(xiàn)[10]，分析式(4)表達(dá)的轉(zhuǎn)換矩陣R可建立無(wú)人機(jī)在機(jī)體坐標(biāo)系下的角速度Bωpla和世界坐標(biāo)系下的角速度wωpla之間的關(guān)系為

wωpla=RBωpla

(10)

wpC=wppla+RBpC

(11)

由于遙操作業(yè)過(guò)程集中在無(wú)人機(jī)懸停的準(zhǔn)靜態(tài)階段，針對(duì)該階段，對(duì)機(jī)械臂的運(yùn)動(dòng)分析可以簡(jiǎn)化為對(duì)其質(zhì)心的運(yùn)動(dòng)分析.根據(jù)文獻(xiàn)[11]，機(jī)械臂質(zhì)心在無(wú)人機(jī)機(jī)體坐標(biāo)系下的絕對(duì)速度BvC為

(12)

式中：Bv為體軸系下三軸線速度.按文獻(xiàn)[12]的約簡(jiǎn)方法，可由前文所推導(dǎo)的無(wú)人機(jī)動(dòng)力學(xué)模型分析得出.將機(jī)械臂的重力變換至機(jī)體坐標(biāo)系，并由Newton第2定律可得

(13)

式中：Cf為機(jī)體坐標(biāo)系中機(jī)械臂對(duì)掛載點(diǎn)的作用力.由掛載點(diǎn)力矩平衡，則有

(14)

式中：BpL為掛載點(diǎn)在無(wú)人機(jī)機(jī)體坐標(biāo)系中的位置向量.

2 基于質(zhì)心采集的無(wú)人機(jī)增穩(wěn)算法仿真實(shí)現(xiàn)

2.1 建模流程

在V-REP軟件中，系統(tǒng)動(dòng)力學(xué)模型的建立遵循以下流程[13]：① 導(dǎo)入STL格式的機(jī)械臂裝配體；② 將導(dǎo)入的機(jī)械臂裝配體拆分為單個(gè)零件，并將這些零件轉(zhuǎn)化為物理引擎可識(shí)別的殼體；③ 對(duì)每個(gè)零件的殼體配置動(dòng)力學(xué)參數(shù)；④ 按照相對(duì)位置關(guān)系配置關(guān)節(jié)、力傳感器，形成動(dòng)力鏈；⑤ 利用V-REP軟件兼容的Lua語(yǔ)言編輯器編寫(xiě)嵌入式腳本，進(jìn)行二次開(kāi)發(fā).

2.2 算法設(shè)計(jì)與實(shí)現(xiàn)

程序架構(gòu)分為主程序和質(zhì)心采集程序模塊，其程序算法實(shí)現(xiàn)的流程圖如圖4所示.首先，利用simTubeOpen()建立V-REP軟件內(nèi)部各程序之間的通訊信道；然后，利用V-REP軟件自帶的Lua庫(kù)函數(shù)，直接獲得仿真場(chǎng)景中模型的動(dòng)力學(xué)參數(shù)信息.利用simGetObjectVelocity()函數(shù)，獲得無(wú)人機(jī)在慣性坐標(biāo)系下的線速度wvC與角速度wωpla；利用simGetObjectPosition()函數(shù)，獲得無(wú)人機(jī)在慣性坐標(biāo)系下的位置信息wppla；利用simGetObjectOrientation()函數(shù)， 獲得無(wú)人機(jī)在慣性坐標(biāo)系下的位姿矩陣.按照這些信息求解方程組，以求得加載在無(wú)人機(jī)電機(jī)位置的力與扭矩.在世界坐標(biāo)系下，利用simAddForceAndTorque()函數(shù)將所求得的力與扭矩施加在六旋翼無(wú)人機(jī)上.

圖4 基于質(zhì)心采集的無(wú)人機(jī)增穩(wěn)算法程序框圖

圖5 質(zhì)心增穩(wěn)算法效果對(duì)比圖

圖6 應(yīng)用質(zhì)心增穩(wěn)算法前后無(wú)人機(jī)的航跡對(duì)比圖

2.3 仿真結(jié)果與對(duì)比

首先，在V-REP軟件可視化仿真環(huán)境中進(jìn)行直觀地對(duì)比.在仿真開(kāi)始后第5、7、9 s時(shí)刻分別截取是否應(yīng)用基于質(zhì)心采集的無(wú)人機(jī)增穩(wěn)算法的場(chǎng)景圖像，如圖5所示.其中：CA代表應(yīng)用質(zhì)心增穩(wěn)算法所獲得的航跡；NCA代表未采用質(zhì)心增穩(wěn)算法所獲得的航跡.然后，利用V-REP與MATLAB軟件的聯(lián)合仿真實(shí)現(xiàn)兩種情況下無(wú)人機(jī)航跡的對(duì)比.利用Sockets包實(shí)現(xiàn)V-REP與MATLAB軟件的實(shí)時(shí)通信，其航跡對(duì)比如圖6所示.由圖6可見(jiàn)，應(yīng)用質(zhì)心增穩(wěn)算法后，無(wú)人機(jī)的飛行狀態(tài)穩(wěn)定，質(zhì)心增穩(wěn)算法效果顯著.

3 多施翼無(wú)人機(jī)抓取增穩(wěn)算法仿真實(shí)現(xiàn)

3.1 無(wú)人機(jī)定位誤差對(duì)抓取操作的影響

分析無(wú)人機(jī)定位誤差對(duì)機(jī)械臂抓取操作的影響，并提出一種簡(jiǎn)便方法以消除定位誤差對(duì)抓取操作的干擾.

在室外作業(yè)環(huán)境下，受全球定位系統(tǒng)(GPS)的定位精度限制，機(jī)載電腦位置信息與真實(shí)情況存在誤差[14].為簡(jiǎn)化問(wèn)題，假設(shè)陀螺儀等姿態(tài)測(cè)量設(shè)備無(wú)誤差.定義O1w為機(jī)載電腦中1號(hào)位姿的位置信息，rO1w為無(wú)人機(jī)真實(shí)位置信息；rt為目標(biāo)物體的真實(shí)位置，t為目標(biāo)物體在機(jī)載電腦中的位置.當(dāng)無(wú)人機(jī)飛臨工作區(qū)時(shí)，操作員遙操機(jī)械抓手靠近目標(biāo)物體.根據(jù)機(jī)械臂轉(zhuǎn)角信息可以確定目標(biāo)物體與無(wú)人機(jī)的相對(duì)位置.當(dāng)真實(shí)環(huán)境中無(wú)人機(jī)在風(fēng)力作用下變?yōu)槲蛔?時(shí)，機(jī)載電腦中無(wú)人機(jī)根據(jù)姿態(tài)測(cè)量設(shè)備信息也做出相同的位姿變化，如圖7所示.無(wú)論是實(shí)際情況還是機(jī)載電腦中的仿真情況，目標(biāo)物體相對(duì)于無(wú)人機(jī)的位置關(guān)系一致，故無(wú)人機(jī)定位誤差對(duì)抓取操作的影響可以通過(guò)操作員的操作方法消除.

圖7 定位誤差示意圖

3.2 算法的設(shè)計(jì)與實(shí)現(xiàn)

空中抓取增穩(wěn)算法實(shí)現(xiàn)流程如圖8所示，其中DLS法表示阻尼最小二乘法.仿真開(kāi)始，在無(wú)人機(jī)操作員發(fā)出懸停指令后，機(jī)載電腦讀取此時(shí)無(wú)人機(jī)位姿信息與機(jī)械臂各關(guān)節(jié)轉(zhuǎn)角信息.確定目標(biāo)物體與機(jī)載坐標(biāo)系的相對(duì)位置關(guān)系后，機(jī)載電腦循環(huán)讀取陀螺儀和加速度計(jì)的信息，根據(jù)三軸平動(dòng)、轉(zhuǎn)動(dòng)加速度積分確定位姿變化；接著，將動(dòng)力鏈兩端位置信息輸入運(yùn)動(dòng)學(xué)逆解求解器，解算出七自由度機(jī)械臂各關(guān)節(jié)轉(zhuǎn)角信息.至此，干擾抓取作業(yè)的無(wú)人機(jī)擾動(dòng)被隔離.其中，風(fēng)力擾動(dòng)利用simAddForceAndTorque() 函數(shù)施加在無(wú)人機(jī)本體上，在世界坐標(biāo)系y軸正方向施加正弦信號(hào)力，在世界坐標(biāo)系x軸正方向施加隨機(jī)信號(hào)力.

圖8 抓取增穩(wěn)算法框圖

3.3 仿真結(jié)果

無(wú)人機(jī)抓取增穩(wěn)算法的可視化仿真結(jié)果如圖9所示.無(wú)人機(jī)受氣流擾動(dòng)時(shí)，機(jī)械臂末端抓手可以牢牢抓住杯子，并可保證杯子中的小球不掉落.

建立V-REP與MATLAB軟件的Sockets通信后，繪制三維散點(diǎn)圖，如圖10(a)所示.其中，點(diǎn)的顏色代表杯子實(shí)際位置與杯子理想位置的距離d.經(jīng)過(guò)抓取增穩(wěn)算法補(bǔ)償后，機(jī)械臂抓手的最大位置偏差值為0.1 m，相比于無(wú)人機(jī)位置最大偏差值0.4 m有顯著減小，如圖10(b)所示.其中，E為距離偏差；EUAV為無(wú)人機(jī)距離偏差值；Em為機(jī)械抓手距離偏差值.由圖10可見(jiàn)，抓取增穩(wěn)算法的效果顯著.

圖9 抓取增穩(wěn)算法的可視化仿真結(jié)果

圖10 抓手實(shí)際位置與理想位置對(duì)比圖

4 結(jié)論

(1) 基于V-REP軟件的二次開(kāi)發(fā)，研制了一套多功能仿真實(shí)驗(yàn)平臺(tái)，實(shí)現(xiàn)了無(wú)人機(jī)-機(jī)械臂動(dòng)力學(xué)可視化仿真操作，在仿真層面提出了一套完整的空中抓取解決方案.

(2) 基于整機(jī)仿真平臺(tái)，開(kāi)發(fā)驗(yàn)證了2種算法，即基于質(zhì)心實(shí)時(shí)采集的無(wú)人機(jī)增穩(wěn)算法，其創(chuàng)新點(diǎn)是將機(jī)械臂對(duì)無(wú)人機(jī)的擾動(dòng)簡(jiǎn)化為機(jī)械臂質(zhì)心對(duì)無(wú)人機(jī)的擾動(dòng)，針對(duì)準(zhǔn)靜態(tài)作業(yè)模式從無(wú)人機(jī)層面進(jìn)行增穩(wěn)；基于逆運(yùn)動(dòng)學(xué)的抓取增穩(wěn)算法，其創(chuàng)新點(diǎn)是將運(yùn)動(dòng)學(xué)逆解算法應(yīng)用于無(wú)人機(jī)增穩(wěn)抓取操作中，從機(jī)械臂層面進(jìn)行增穩(wěn).通過(guò)逐級(jí)增穩(wěn)的方式，實(shí)現(xiàn)了無(wú)人機(jī)空中抓取作業(yè)的精細(xì)化操作.

致謝本文為上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院第四屆研究生項(xiàng)目式實(shí)踐課程獲獎(jiǎng)成果.感謝小組成員馬昭、呂石磊、任書(shū)峰、李曉凱為本文研究提供的機(jī)械臂模型.