湯奇榮，鄧振強(qiáng)，李英浩，陳 迪

(同濟(jì)大學(xué) 機(jī)械與能源工程學(xué)院，機(jī)器人技術(shù)與多體系統(tǒng)實(shí)驗(yàn)室，上海 201804)

0 引 言

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

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

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

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

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

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

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

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

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

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

2 水下機(jī)器人-機(jī)械手系統(tǒng)滑?？刂?/h2>

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

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

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

2.2 水下機(jī)器人-機(jī)械手系統(tǒng)滑?？刂破髟O(shè)計(jì)，

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

同時(shí)選取線(xiàn)性滑模面如下式：

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

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

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

其中：

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

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

則有

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

推導(dǎo)得到水下機(jī)器人-機(jī)械手系統(tǒng)的滑?？刂坡珊?，建立UVMS滑模控制系統(tǒng)，控制系統(tǒng)結(jié)構(gòu)如圖2所示。以水下機(jī)器人-機(jī)械手系統(tǒng)的期望軌跡為輸入，通過(guò)指數(shù)趨近滑?？刂破鹘馑愠鰪V義控制力作為UVMS系統(tǒng)的控制輸入，控制UVMS系統(tǒng)的運(yùn)動(dòng)。

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

3 仿真研究

3.1 仿真對(duì)象

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

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

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

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

其中：

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

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

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

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

3.2 仿真結(jié)果

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

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

圖5 UVMS艇體空間運(yùn)動(dòng)軌跡跟蹤Fig. 5 Trajectory tracking of UVMS in 3-dimensional space

圖6 UVMS首向運(yùn)動(dòng)軌跡跟蹤Fig. 6 Sway motion tracking of UVMS

圖7 UVMS側(cè)向運(yùn)動(dòng)軌跡跟蹤Fig. 7 Surge motion tracking of UVMS

圖8 UVMS縱向運(yùn)動(dòng)軌跡跟蹤Fig. 8 Heave motion tracking of UVMS

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

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

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

4 結(jié) 語(yǔ)

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