基于指數趨近滑模控制的水下機器人-機械手系統軌跡跟蹤

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

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

0 引 言

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

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

1 水下機器人-機械手系統建模

水下機器人-機械手系統的坐標系由固定坐標系E-XYZ 、艇體坐標系以及機械手關節坐標系Oi-xiyizi組成，如圖1所示，其中艇體坐標系原點與UVMS艇體質心重合。

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

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

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

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

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

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

2 水下機器人-機械手系統滑模控制

2.1 滑模變結構控制

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

實現滑模變結構控制的過程主要包括確定系統滑動模態到達條件、選取滑模面以及求取系統滑模控制律，建立滑模控制器。

2.2 水下機器人-機械手系統滑模控制器設計，

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

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

其中： s 為控制系統滑模面矢量； c為系統滑模面參數矩陣，具體形式分別為

其中 s1,s2,···,sN為系統子滑模面， c1,c2,···,cN為滑模面參數。

為了改善滑模面動態品質，減少系統抖振，基于式（7）指數趨近律設計水下機器人-機械手系統的滑模控制律。

其中：

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

為了使系統滿足滑動模態到達條件且具有一定穩定性，需要滿足Lyapunov穩定性條件。選取系統Lyapunov函數為：

則有

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

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

圖2 基于指數趨近律的UVMS滑模控制系統Fig. 2 SMC of UVMS based on exponential reaching law

3 仿真研究

3.1 仿真對象

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

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

根據課題組研發的一款UVMS系統，水下機器人-機械手系統質量參數及水下機械手D-H參數分別如表1和表2所示，機械手D-H坐標系如圖4所示。

在系統的軌跡跟蹤控制仿真中，設定UVMS的期望軌跡為

其中：

表1 水下機器人-機械手系統質量參數Tab. 1 Mass parameters of UVMS

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

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

艇體質心期望軌跡為水下三維螺旋下潛運動，回轉周期為50 s，艇體在下潛過程中調整姿態，且水下機械手在下潛過程中展開，UVMS系統初始位置向量為。

3.2 仿真結果

根據前文所設計的指數趨近滑模控制器以及仿真對象的物理參數，在Matlab環境中進行水下機器人-機械手系統的運動軌跡跟蹤控制仿真，其中水下機器人-機械手系統艇體質心的空間運動軌跡跟蹤結果如圖5所示。

水下機器人-機械手系統艇體姿態控制仿真結果以及機械手關節控制仿真結果分別如圖9和圖10所示，其控制要達到的艇體姿態為，以及機械手關節旋轉。

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

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

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

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

圖9 UVMS姿態角控制Fig. 9 Control of attitude angles in UVMS

圖10 UVMS機械手關節角度控制Fig. 10 Control of manipulator joints in UVMS

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

4 結 語

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