多施翼無人機遙操機械臂多功能仿真實驗平臺的設計與實現

馬仲航， 張執南

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

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

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

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

1 空中遙操機械臂動力學模型構建

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

1.1 實時質心跟蹤采集

七自由度機械臂的各臂節之間依靠轉動副連接，由Hatenberg機器人位姿描述方法[6]，可獲得每一臂節質心坐標系相對于世界坐標系的位姿關系.各臂節間的位姿轉換矩陣如下[7]：

(1)

式中：βi為從xi-1到xi繞zi的旋轉角；αi-1為從zi-1到zi繞xi-1的旋轉角；di為從xi-1到xi沿zi方向的距離.動力鏈的位姿變換關系如下：

(2)

根據文獻[7]的推導結果，多體系統質心求解的結果如下：

(3)

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

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

圖1 質心跟蹤算法框圖

圖2 質心位置實時跟蹤顯示

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

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

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

(4)

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

(5)

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

(6)

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

A=

(7)

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

(8)

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

(9)

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

wωpla=RBωpla

(10)

wpC=wppla+RBpC

(11)

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

(12)

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

(13)

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

(14)

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

2 基于質心采集的無人機增穩算法仿真實現

2.1 建模流程

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

2.2 算法設計與實現

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

圖4 基于質心采集的無人機增穩算法程序框圖

圖5 質心增穩算法效果對比圖

圖6 應用質心增穩算法前后無人機的航跡對比圖

2.3 仿真結果與對比

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

3 多施翼無人機抓取增穩算法仿真實現

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

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

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

圖7 定位誤差示意圖

3.2 算法的設計與實現

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

圖8 抓取增穩算法框圖

3.3 仿真結果

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

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

圖9 抓取增穩算法的可視化仿真結果

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

4 結論

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

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

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