基于指數(shù)趨近滑模控制的水下機器人-機械手系統(tǒng)軌跡跟蹤

湯奇榮，鄧振強，李英浩，陳 迪

(同濟大學 機械與能源工程學院，機器人技術(shù)與多體系統(tǒng)實驗室，上海 201804)

0 引 言

近年來，隨著對海洋開發(fā)的深入，水下機器人的應(yīng)用逐漸引起各行業(yè)的重視。水下機器人-機械手系統(tǒng)（Underwater Vehicle-Manipulator System，UVMS）是一種可以在水下進行觀測和自主作業(yè)的自動化裝備，在海底科學考察、資源勘探、管道鋪設(shè)、近海養(yǎng)殖等方面具有很大的潛在應(yīng)用價值[1]。UVMS系統(tǒng)由水下機器人艇體和水下作業(yè)機械手兩部分組成，通過水下機器人艇體和水下機械手關(guān)節(jié)的運動共同完成需要執(zhí)行的作業(yè)任務(wù)。UVMS具有非線性、強耦合、時變和高維等動力學特點[2]，同時UVMS系統(tǒng)在水下作業(yè)時為節(jié)省能量，通常采用欠驅(qū)動的工作方式[3]。這些情況的存在會使水下機器人-機械手系統(tǒng)的控制變得非常困難。因此，研究水下機器人-機械手系統(tǒng)的控制技術(shù)是實現(xiàn)UVMS自主作業(yè)的關(guān)鍵之一。UVMS軌跡跟蹤控制的目標是選擇合適的控制策略，并設(shè)計有效的控制律，使其能夠跟蹤系統(tǒng)的預(yù)定軌跡，并保證跟蹤誤差滿足一定要求。目前國內(nèi)外在水下機器人-機械手系統(tǒng)上采用的控制方法主要有PID控制方法、自適應(yīng)控制方法、神經(jīng)網(wǎng)絡(luò)控制方法以及滑模控制方法等[4]。PID控制方法在實際工程中已經(jīng)被廣泛應(yīng)用，但由于PID參數(shù)需要適應(yīng)模型參數(shù)的變化，而UVMS模型具有很大不確定性，因此PID控制方法不適用于對UVMS的控制。自適應(yīng)控制通常適用于模型參數(shù)不確定性嚴重、參數(shù)變化緩慢和自由度較少的場合，但是穩(wěn)定性較差[5]。神經(jīng)網(wǎng)絡(luò)控制具有很好的學習能力和適應(yīng)能力，不依賴精確的數(shù)學模型，但神經(jīng)網(wǎng)絡(luò)控制需要大量的訓練樣本來提高控制精度[6]。而滑模變結(jié)構(gòu)控制具有響應(yīng)快、魯棒性高等優(yōu)點，被廣泛應(yīng)用于非線性機器人的控制[7]。因此，滑模變結(jié)構(gòu)控制比較適用于海洋工作環(huán)境下的UVMS。Bin Xu等[8]提出了一種水下機器人-機械手系統(tǒng)軌跡跟蹤滑模控制器，并根據(jù)模糊邏輯控制方法實現(xiàn)了增益的協(xié)調(diào)變化，在攜帶五自由度機械手的UVMS系統(tǒng)仿真實驗中驗證了這種控制策略的有效性。Taira等[9]研究了在參數(shù)不確定情況下水下機器人-機械手操縱系統(tǒng)的控制方案，開發(fā)了自適應(yīng)控制器和滑模控制器，并將其應(yīng)用于UVMS的穩(wěn)定控制。但滑模控制在本質(zhì)上的不連續(xù)開關(guān)特性會引起系統(tǒng)的抖振，因此需要削弱系統(tǒng)抖振。目前，國內(nèi)外在減少滑模控制的抖振方面也有許多研究，主要方法有趨近律方法，觀測器方法，智能控制方法，動態(tài)滑模方法等[10]。其中趨近律方法可以調(diào)整系統(tǒng)滑模點趨近滑模面的速度，從而有效地減小系統(tǒng)慣性引起的抖振[11]。

為了提高系統(tǒng)響應(yīng)速度和控制精度，同時減少系統(tǒng)抖振，本文基于指數(shù)趨近律設(shè)計了水下機器人-機械手系統(tǒng)的滑模控制器，用于實現(xiàn)對UVMS系統(tǒng)的軌跡跟蹤，并通過仿真驗證該控制策略的有效性。

1 水下機器人-機械手系統(tǒng)建模

水下機器人-機械手系統(tǒng)的坐標系由固定坐標系E-XYZ 、艇體坐標系以及機械手關(guān)節(jié)坐標系Oi-xiyizi組成，如圖1所示，其中艇體坐標系原點與UVMS艇體質(zhì)心重合。

水下機器人-機械手系統(tǒng)由水下機器人艇體和水下機械手兩部分組成，其廣義坐標和廣義控制力分別定義為：

圖1 水下機器人-機械手系統(tǒng)坐標系Fig. 1 Coordinate system of UVMS

其中： ξ=[ηTqT]T∈?6+n為水下機器人-機械手系統(tǒng)廣義位置向量，包括水下機器人艇體的位置、姿態(tài)以及機械手關(guān)節(jié)變量。向量以及表示水下機器人艇體重心在固定坐標系中的位置坐標，以及分別表示水下機器人艇體的橫滾角，縱傾角以及首向角。而其中向量 q=[q1···qn]T∈?n為機械手關(guān)節(jié)角度變量。同時 τ∈?(6+n)為系統(tǒng)廣義控制力項，包括水下機器人艇體所受的廣義推力和水下機械手關(guān)節(jié)驅(qū)動力。

根據(jù)式（2）第二類拉格朗日方程建立水下機器人-機械手系統(tǒng)的動力學方程，即

其中： L 為拉格朗日函數(shù)， L=T-V ，其中 T為系統(tǒng)動能， V 為系統(tǒng)勢能。 Q為保守主動力對應(yīng)的廣義力，主要包括廣義控制力以及系統(tǒng)所受恢復(fù)力矩和水阻力。通過推導可得系統(tǒng)整體動力學方程為：

其中： M(ξ)∈ ?(6+n)×(6+n)為UVMS系統(tǒng)的慣性矩陣，C(ξ,ξ˙)ξ˙∈ ?(6+n)為向心力與科里奧利力項，D(ξ˙)ξ˙∈ ?(6+n)為阻力項，由于UVMS的重心和浮心不重合會產(chǎn)生恢復(fù)力矩，為水下機器人-機械手系統(tǒng)所受重力和浮力的合力矩產(chǎn)生的恢復(fù)力項。

2 水下機器人-機械手系統(tǒng)滑模控制

2.1 滑模變結(jié)構(gòu)控制

滑模變結(jié)構(gòu)控制的基本原理是控制器將系統(tǒng)當前狀態(tài)或狀態(tài)誤差限定到滑模面并使之穩(wěn)定在滑模面上[11]。由于滑模運動特性預(yù)先按照要求設(shè)計，同時給定的相軌跡與控制對象參數(shù)以及外部干擾變化無關(guān)，因而滑模控制對參數(shù)變化及擾動不靈敏，系統(tǒng)具有極強的魯棒性。但在實際的控制系統(tǒng)中，由于系統(tǒng)存在質(zhì)量慣性、時間延遲等因素，滑模變結(jié)構(gòu)控制在滑動模態(tài)下會存在抖振。

實現(xiàn)滑模變結(jié)構(gòu)控制的過程主要包括確定系統(tǒng)滑動模態(tài)到達條件、選取滑模面以及求取系統(tǒng)滑模控制律，建立滑模控制器。

2.2 水下機器人-機械手系統(tǒng)滑模控制器設(shè)計，

首先設(shè)定UVMS期望軌跡為 ξd(t),t∈ [0,T]，對應(yīng)速度和加速度分別為、且在整個時間段內(nèi)存在并連續(xù)。設(shè)定軌跡跟蹤誤差及軌跡跟蹤誤差變化率分別為

同時選取線性滑模面如下式：

其中： s 為控制系統(tǒng)滑模面矢量； c為系統(tǒng)滑模面參數(shù)矩陣，具體形式分別為

其中 s1,s2,···,sN為系統(tǒng)子滑模面， c1,c2,···,cN為滑模面參數(shù)。

為了改善滑模面動態(tài)品質(zhì)，減少系統(tǒng)抖振，基于式（7）指數(shù)趨近律設(shè)計水下機器人-機械手系統(tǒng)的滑模控制律。

其中：

綜合式（3）～式（7）可得水下機器人-機械手系統(tǒng)滑模控制律為：

為了使系統(tǒng)滿足滑動模態(tài)到達條件且具有一定穩(wěn)定性，需要滿足Lyapunov穩(wěn)定性條件。選取系統(tǒng)Lyapunov函數(shù)為：

則有

綜合式（7）和式（11）可得

推導得到水下機器人-機械手系統(tǒng)的滑模控制律后，建立UVMS滑模控制系統(tǒng)，控制系統(tǒng)結(jié)構(gòu)如圖2所示。以水下機器人-機械手系統(tǒng)的期望軌跡為輸入，通過指數(shù)趨近滑模控制器解算出廣義控制力作為UVMS系統(tǒng)的控制輸入，控制UVMS系統(tǒng)的運動。

圖2 基于指數(shù)趨近律的UVMS滑模控制系統(tǒng)Fig. 2 SMC of UVMS based on exponential reaching law

3 仿真研究

3.1 仿真對象

為了驗證上述滑模變結(jié)構(gòu)控制策略的有效性，在Matlab環(huán)境中建立控制仿真系統(tǒng)，對UVMS進行軌跡跟蹤控制仿真。仿真對象為攜帶三自由度機械手的水下機器人-機械手系統(tǒng)，水下機器人艇體具有6個自由度，仿真對象流線型UVMS三維模型如圖3所示。

圖3 水下機器人-機械手系統(tǒng)三維模型Fig. 3 Three dimensional model of UVMS

根據(jù)課題組研發(fā)的一款UVMS系統(tǒng)，水下機器人-機械手系統(tǒng)質(zhì)量參數(shù)及水下機械手D-H參數(shù)分別如表1和表2所示，機械手D-H坐標系如圖4所示。

在系統(tǒng)的軌跡跟蹤控制仿真中，設(shè)定UVMS的期望軌跡為

其中：

表1 水下機器人-機械手系統(tǒng)質(zhì)量參數(shù)Tab. 1 Mass parameters of UVMS

表2 水下機械手D-H參數(shù)Tab. 2 D-H parameters of manipulator in UVMS

圖4 本文研究的三自由度水下機械手D-H坐標系Fig. 4 D-H coordinate system of our 3-DOFs manipulator

艇體質(zhì)心期望軌跡為水下三維螺旋下潛運動，回轉(zhuǎn)周期為50 s，艇體在下潛過程中調(diào)整姿態(tài)，且水下機械手在下潛過程中展開，UVMS系統(tǒng)初始位置向量為。

3.2 仿真結(jié)果

根據(jù)前文所設(shè)計的指數(shù)趨近滑模控制器以及仿真對象的物理參數(shù)，在Matlab環(huán)境中進行水下機器人-機械手系統(tǒng)的運動軌跡跟蹤控制仿真，其中水下機器人-機械手系統(tǒng)艇體質(zhì)心的空間運動軌跡跟蹤結(jié)果如圖5所示。

水下機器人-機械手系統(tǒng)艇體姿態(tài)控制仿真結(jié)果以及機械手關(guān)節(jié)控制仿真結(jié)果分別如圖9和圖10所示，其控制要達到的艇體姿態(tài)為，以及機械手關(guān)節(jié)旋轉(zhuǎn)。

圖5 UVMS艇體空間運動軌跡跟蹤Fig. 5 Trajectory tracking of UVMS in 3-dimensional space

圖6 UVMS首向運動軌跡跟蹤Fig. 6 Sway motion tracking of UVMS

圖7 UVMS側(cè)向運動軌跡跟蹤Fig. 7 Surge motion tracking of UVMS

圖8 UVMS縱向運動軌跡跟蹤Fig. 8 Heave motion tracking of UVMS

圖9 UVMS姿態(tài)角控制Fig. 9 Control of attitude angles in UVMS

圖10 UVMS機械手關(guān)節(jié)角度控制Fig. 10 Control of manipulator joints in UVMS

從仿真結(jié)果看，系統(tǒng)軌跡跟蹤效果較好，滑模控制器可以比較精確地實現(xiàn)UVMS的期望軌跡，且對艇體姿態(tài)和機械手控制響應(yīng)速度較快，系統(tǒng)抖振較小。仿真的初始位置不在坐標原點，從仿真開始到定位至預(yù)定軌跡需要一定的響應(yīng)時間，而仿真結(jié)果表明滑模控制可以實現(xiàn)UVMS的快速準確定位，有利于提高UVMS的水下作業(yè)效率。

4 結(jié) 語

本文針對水下機器人-機械手系統(tǒng)的軌跡跟蹤控制問題，建立了系統(tǒng)整體動力學模型，并基于指數(shù)趨近律設(shè)計了系統(tǒng)滑模控制器用于UVMS的軌跡跟蹤控制。仿真結(jié)果表明所設(shè)計的滑模控制器總體上可以較精確地實現(xiàn)UVMS水下螺旋下潛運動的軌跡跟蹤以及水下機械運動控制，且系統(tǒng)上升時間短，響應(yīng)速度較快。水下機械手轉(zhuǎn)動時對艇體的耦合作用導致開始階段誤差大于機械手運動完成后的誤差。同時由于UVMS運動過程中x方向所受阻力會大于y方向和z方向，因此仿真結(jié)果中x方向上軌跡跟蹤誤差要大于y，z方向跟蹤誤差。本文研究的UVMS實體系統(tǒng)正在開發(fā)中，后續(xù)研究將會在實體系統(tǒng)上對所設(shè)計的控制器進行實驗驗證。