馬仲航， 張執(zhí)南

(上海交通大學 機械與動力工程學院， 上海 200240)

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

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

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

1 空中遙操機械臂動力學模型構(gòu)建

空中遙操機械臂采用分級控制策略來處理無人機與機械臂之間的擾動.基于該思路，首先研究七自由度機械臂的質(zhì)心采集方法，將機械臂質(zhì)心的變動作為機械臂對無人機的擾動，以簡化空中遙操機械臂的動力學模型.

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

七自由度機械臂的各臂節(jié)之間依靠轉(zhuǎn)動副連接，由Hatenberg機器人位姿描述方法[6]，可獲得每一臂節(jié)質(zhì)心坐標系相對于世界坐標系的位姿關(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方向的距離.動力鏈的位姿變換關(guān)系如下：

(2)

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

(3)

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

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

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

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

1.2 多施翼無人機遙操機械臂動力學模型

圖3 空中遙操機械臂坐標系示意圖

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

(4)

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

(5)

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

(6)

式中：Bf為無人機在機體坐標系下的升力.定義向量m的3個分量分別代表沿機體坐標系的3個坐標軸方向的扭矩.

A=

(7)

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

(8)

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

(9)

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

wωpla=RBωpla

(10)

wpC=wppla+RBpC

(11)

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

(12)

式中：Bv為體軸系下三軸線速度.按文獻[12]的約簡方法，可由前文所推導的無人機動力學模型分析得出.將機械臂的重力變換至機體坐標系，并由Newton第2定律可得

(13)

式中：Cf為機體坐標系中機械臂對掛載點的作用力.由掛載點力矩平衡，則有

(14)

式中：BpL為掛載點在無人機機體坐標系中的位置向量.

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

2.1 建模流程

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

2.2 算法設計與實現(xiàn)

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

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

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

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

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

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

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

3.1 無人機定位誤差對抓取操作的影響

分析無人機定位誤差對機械臂抓取操作的影響，并提出一種簡便方法以消除定位誤差對抓取操作的干擾.

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

圖7 定位誤差示意圖

3.2 算法的設計與實現(xiàn)

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

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

3.3 仿真結(jié)果

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

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

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

圖10 抓手實際位置與理想位置對比圖

4 結(jié)論

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

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

致謝本文為上海交通大學機械與動力工程學院第四屆研究生項目式實踐課程獲獎成果.感謝小組成員馬昭、呂石磊、任書峰、李曉凱為本文研究提供的機械臂模型.