水下機(jī)器人動(dòng)力學(xué)分析與控制系統(tǒng)研究

王永鼎，董亞龍

（上海海洋大學(xué) 工程學(xué)院，上海 201306）

0 引言

近年來，隨著經(jīng)濟(jì)的高速發(fā)展，用于水下檢測(cè)及信息收集的水下機(jī)器人的需求不斷增加，水下機(jī)器人的研究受到廣泛關(guān)注。無人化、智能化的水下機(jī)器人將成為一種趨勢(shì)，特別是小型水下機(jī)器人，研發(fā)周期短，機(jī)動(dòng)靈活，成本低廉，常被用于河堤大壩水況監(jiān)測(cè)、河道橋墩水下信息收集，近海碼頭港口檢修以及船舶外體水下檢測(cè)等，節(jié)省了大量的人力物力財(cái)力，其市場(chǎng)前景非常廣闊，因此對(duì)于水下機(jī)器人的研究具有重大的理論意義和實(shí)際應(yīng)用意義。

水下機(jī)器人由于其強(qiáng)非線性和耦合性，運(yùn)動(dòng)參數(shù)不確定性以及工作環(huán)境復(fù)雜，易受外界干擾影響，其要在水下完成指定任務(wù)，必須具備良好完善的運(yùn)動(dòng)控制系統(tǒng)，這也要求我們首先對(duì)水下機(jī)器人進(jìn)行動(dòng)力學(xué)分析。

1 水下機(jī)器人坐標(biāo)系及坐標(biāo)變換

1.1 建立坐標(biāo)系

根據(jù)國際水池會(huì)議推薦和造船與輪機(jī)工程學(xué)會(huì)術(shù)語公報(bào)體系，參考相關(guān)資料[1]，按右手定則建立兩個(gè)坐標(biāo)系：靜坐標(biāo)系（E-XYZ）和動(dòng)坐標(biāo)系（O-X＇Y＇Z＇），如圖1所示。

靜坐標(biāo)系，也稱慣性坐標(biāo)系或地面坐標(biāo)系，以海面或水面某一點(diǎn)為原點(diǎn)，EX和EY在水平面，EZ正向指向地心。水下機(jī)器人的位置和方向描述為分別為水下機(jī)器人在靜坐標(biāo)系中的位置，分別為水下機(jī)器人對(duì)靜坐標(biāo)系的橫滾角、縱傾角和艏向角。

動(dòng)坐標(biāo)系，也稱載體坐標(biāo)系，以水下機(jī)器人的重心為原點(diǎn)，OX＇軸沿水下機(jī)器人縱軸方向，正向指向艏部，OY＇垂直于OX＇軸且正向指向右側(cè)，OZ＇垂直于OX＇軸和OY＇軸所在平面且正向指向底部。水下機(jī)器人的速度在動(dòng)坐標(biāo)系中可描述為速度矢量V=[u v w p q r]T和力（矩）矢量τ=[X Y Z K M N]T。相關(guān)參數(shù)定義如表1所示。

圖1 水下機(jī)器人坐標(biāo)系示意圖

1.2 坐標(biāo)變換

表1 水下機(jī)器人參數(shù)定義表

式中：

2 空間運(yùn)動(dòng)數(shù)學(xué)模型

水下機(jī)器人的運(yùn)動(dòng)是一種六自由度的空間運(yùn)動(dòng)，可以用動(dòng)坐標(biāo)系下的沿三個(gè)坐標(biāo)軸的直線運(yùn)動(dòng)和繞三個(gè)坐標(biāo)軸的旋轉(zhuǎn)運(yùn)動(dòng)表示[3]。根據(jù)剛體牛頓-歐拉方程，六自由度水下機(jī)器人在動(dòng)坐標(biāo)系中的空間運(yùn)動(dòng)方程可描述為[4]：

式中，M為包括附加載體的質(zhì)量MA及慣性矩陣MRB；C(v)為剛體科氏項(xiàng)和離心項(xiàng)矩陣；D(v)為流體阻尼矩陣（水動(dòng)力損失項(xiàng)）；為重力與浮力矩陣（靜力項(xiàng)）；為流體動(dòng)力和力矩項(xiàng)矩陣；為推進(jìn)器推力和力矩項(xiàng)矩陣；為水下機(jī)器人受到外部擾動(dòng)產(chǎn)生的力和力矩項(xiàng)矩陣。

水下機(jī)器人是一個(gè)非線性的動(dòng)力學(xué)系統(tǒng)，其運(yùn)動(dòng)規(guī)律較為復(fù)雜，需要求得的參數(shù)較多。而有些參數(shù)由于如技術(shù)條件、測(cè)定環(huán)境以及硬件設(shè)備等外在條件限制，較難甚至無法測(cè)定。為了研究需要，合理地對(duì)系統(tǒng)進(jìn)行適當(dāng)簡化，這里只考慮靜力（重力和浮力）、推力和水動(dòng)力對(duì)水下機(jī)器人的影響。

2.1 重力、浮力和重力矩、浮力矩

水下機(jī)器人受到的重力和在水中受到的浮力作用在同一直線上，重力總是指向地心，浮力與重力方向相反。設(shè)重心（xg，yg，zg）與動(dòng)坐標(biāo)系坐標(biāo)原點(diǎn)重合，則這兩個(gè)力及其力矩可表示為：

2.2 水動(dòng)力及其力矩

水下機(jī)器人在水中受到的水動(dòng)力主要包括水流阻力和附加質(zhì)量。水下機(jī)器人在水下作加速運(yùn)動(dòng)時(shí)，對(duì)周圍的流體會(huì)產(chǎn)生作用力，使之產(chǎn)生加速度，克服流體本身慣性需要提供額外的作用力，進(jìn)而導(dǎo)致水下機(jī)器人所受阻力增加，這種阻力增加等同于水下機(jī)器人的質(zhì)量增加，稱為附加質(zhì)量，可由試驗(yàn)求得；水流阻力與水下機(jī)器人相對(duì)于水流速度的平方成正比。

水下機(jī)器人受到的水動(dòng)力及其力矩可表示為：

式中，mu、mv、mw分別為沿X＇、Y＇、Z＇軸軸線方向產(chǎn)生的附加質(zhì)量；mp、mq、mr分別為沿X＇、Y＇、Z＇軸旋轉(zhuǎn)方向產(chǎn)生的附加質(zhì)量。

式中，ρ為水密度；Cd為無因次阻力系數(shù)，一般通過實(shí)驗(yàn)獲得，對(duì)于大多數(shù)水下機(jī)器人，一般取0.8～1.0[5]；SX'、SY'、SZ'分別為水下機(jī)器人垂直于X＇、Y＇、Z＇軸的橫斷面面積；（ur、vr、wr）為水流相對(duì)于機(jī)器人的速度；（KX'、KY'、KZ'）為阻力矩系數(shù)，可由試驗(yàn)求得。

2.3 推力及其力矩

水下機(jī)器人所受推力及其力矩與推進(jìn)器的布置、水下機(jī)器人的結(jié)構(gòu)、尺寸、線型以及運(yùn)動(dòng)要求有關(guān)。這里給定推進(jìn)器的布置，如圖2所示。水下機(jī)器人在O＇X＇Y＇平面有兩個(gè)推進(jìn)器T1、T2，它們相對(duì)于X＇軸對(duì)稱布置，可實(shí)現(xiàn)沿X＇軸的平移以及繞Z＇軸的旋轉(zhuǎn)運(yùn)動(dòng)；在O＇Y＇Z＇平面有兩個(gè)推進(jìn)器T3、T4，它們相對(duì)于Y＇軸對(duì)稱布置，可實(shí)現(xiàn)沿Y＇軸的平移以及繞X＇軸的旋轉(zhuǎn)運(yùn)動(dòng)；在O＇X＇Z＇平面有兩個(gè)推進(jìn)器T5、T6，它們相對(duì)于Z＇軸對(duì)稱布置，可實(shí)現(xiàn)沿Z＇軸的平移以及繞Y＇軸的旋轉(zhuǎn)運(yùn)動(dòng)；由此，可實(shí)現(xiàn)水下機(jī)器人進(jìn)退、潛浮、橫移、橫搖、縱傾和轉(zhuǎn)艏六個(gè)自由度運(yùn)動(dòng)。水下機(jī)器人主要使用定距螺旋槳，單個(gè)定距螺旋槳的推力可由式(7)表示：

圖2 推進(jìn)器的布置

式中，F(xiàn)Ti為第i個(gè)推進(jìn)器產(chǎn)生的推力；ni為第i個(gè)推進(jìn)器的螺旋槳的轉(zhuǎn)速；Di為螺旋槳直徑；KTi為第i個(gè)推進(jìn)器的推力系數(shù)。

所以，水下機(jī)器人推進(jìn)器推力及其力矩項(xiàng)矩陣可表示為：

式中，RTi為第i個(gè)推進(jìn)器和與之平行的坐標(biāo)軸的垂直距離。

2.4 建立動(dòng)力學(xué)模型

基于以上受力分析，并對(duì)水下機(jī)器人水動(dòng)力學(xué)模型適當(dāng)簡化。不考慮水動(dòng)力損失項(xiàng)及外部干擾力，重心與動(dòng)坐標(biāo)系坐標(biāo)原點(diǎn)重合，即Xg=Yg=Zg=0?？傻盟聶C(jī)器人動(dòng)力學(xué)方程：

式中：

將式(3)～式(8)和式(10)、式(11)代入式(9)，得水下機(jī)器人空間動(dòng)力學(xué)方程：

通過以上分析并根據(jù)水下機(jī)器人的實(shí)際情況求解水下機(jī)器人動(dòng)力學(xué)方程，可得其在動(dòng)坐標(biāo)系下的運(yùn)動(dòng)規(guī)律（u、v、w、p、q、r），通過坐標(biāo)轉(zhuǎn)換關(guān)系式(1)和式(2)可得其在靜坐標(biāo)系下的運(yùn)動(dòng)規(guī)律

3 控制系統(tǒng)設(shè)計(jì)

在以上水下機(jī)器人受力分析的基礎(chǔ)上，進(jìn)行水下機(jī)器人控制系統(tǒng)研究。水下機(jī)器人是一個(gè)復(fù)雜的非線性系統(tǒng)，通過螺旋槳推進(jìn)器實(shí)現(xiàn)在水中的運(yùn)動(dòng)，需要設(shè)計(jì)合理的控制器進(jìn)行水下機(jī)器人的運(yùn)動(dòng)控制，以保證水下機(jī)器人在水中運(yùn)動(dòng)的平穩(wěn)性和準(zhǔn)確性。以模糊集合論、模糊語言變量和模糊邏輯推理為基礎(chǔ)的模糊邏輯控制[6～8]，通過模糊推理和模糊決策實(shí)現(xiàn)對(duì)復(fù)雜系統(tǒng)的控制，具有相當(dāng)好的魯棒性、適應(yīng)性和容錯(cuò)性。模糊控制系統(tǒng)的結(jié)構(gòu)圖如圖3所示。

圖3 模糊控制系統(tǒng)的結(jié)構(gòu)框圖

以水下機(jī)器人位置偏差ΔL和速度偏差ΔV作為輸入量，以推進(jìn)器所需提供的推力U作為輸出量，對(duì)水下機(jī)器人進(jìn)行控制，通過檢測(cè)裝置將機(jī)器人的狀態(tài)反饋到輸入端，形成一個(gè)閉環(huán)反饋系統(tǒng)。其中模糊推力方法可以采用Mandani或Sugeuo兩種類型，根據(jù)Mandani[9]所提的設(shè)計(jì)方法，將輸入量ΔL、ΔV和輸出量U論域范圍設(shè)定在[-6,6]，將輸入量的實(shí)際范圍離散化，并將其表達(dá)成論域區(qū)間的變量。模糊化后的輸入輸出模糊變量集合均為{NB,NM,NS,ZO,PS,PM,PB}，集合中的變量表示{負(fù)大，負(fù)中，負(fù)小，零，正小，正中，正大}。選擇三角函數(shù)作為輸入輸出變量的隸屬度函數(shù)，輸入變量ΔV與輸出變量U的隸屬度函數(shù)和輸入變量ΔL的隸屬度函數(shù)一樣，如圖4所示。

根據(jù)水下機(jī)器人在水下的狀態(tài)分析，當(dāng)水下機(jī)器人位置偏差ΔL和速度偏差ΔV均為負(fù)大（NB）時(shí)，水下機(jī)器人偏離目標(biāo)軌跡并且距離目標(biāo)軌跡越行越遠(yuǎn)，此時(shí)應(yīng)調(diào)節(jié)控制量為正大（PB），糾正水下機(jī)器人的實(shí)際軌跡，使其向目標(biāo)軌跡靠攏；當(dāng)水下機(jī)器人位置偏差ΔL為負(fù)大（NB），而速度偏差ΔV均為正大（LB）時(shí)，水下機(jī)器人偏離目標(biāo)軌跡但距離目標(biāo)軌跡越行越近，此時(shí)將控制量調(diào)節(jié)為正?。≒S），即需要較小的調(diào)節(jié)量就可以是說下機(jī)器人按既定軌跡行進(jìn)。依此可以得出如表2所示的模糊控制規(guī)則表。圖5為輸入輸出的關(guān)系三維曲線圖。

圖4 輸入變量ΔL的隸屬度函數(shù)

表2 模糊控制規(guī)則表

圖5 輸入輸出關(guān)系三維曲線圖

4 仿真分析

根據(jù)以上水下機(jī)器人動(dòng)力學(xué)分析以及建立的控制系統(tǒng)，通過MATLAB中的Simulink模塊[10]對(duì)水下機(jī)器人進(jìn)行仿真實(shí)驗(yàn)。水下機(jī)器人簡化為直徑0.2m，長0.4m的均質(zhì)柱體，且重力、浮力作用可使其保持水平狀態(tài)，無橫傾或縱傾，浮心與重心重合。

設(shè)目標(biāo)在初始位置進(jìn)行浮潛運(yùn)動(dòng)，參考輸入深度為4m，水下機(jī)器人的預(yù)定軌跡依據(jù)直接瞄準(zhǔn)法[11]進(jìn)行預(yù)設(shè)，采樣周期0.1s，仿真時(shí)長50s。仿真得到水下機(jī)器人速度和運(yùn)動(dòng)響應(yīng)曲線，如圖6～圖8所示。

圖6 速度偏差曲線

圖7 位置偏差曲線

圖8 控制變量變化曲線

從圖6、圖7中可以看出，水下機(jī)器人的位置偏差和速度偏差在開始時(shí)均有變化，通過控制變量的調(diào)節(jié)，如圖8所示，在t=15s時(shí)，它們的曲線趨近平穩(wěn)最終趨向于零，表明水下機(jī)器人的實(shí)際軌跡最終趨向于理論軌跡，速度趨于穩(wěn)定，系統(tǒng)最終穩(wěn)定，并經(jīng)多次仿真，得到具有類似的結(jié)果，即在經(jīng)歷開始變化后，通過控制變量的調(diào)節(jié)，系統(tǒng)最終穩(wěn)定。仿真結(jié)果表明，該控制方法可

【】【】

行，能夠使水下機(jī)器人在運(yùn)動(dòng)過程中保證其穩(wěn)定性與準(zhǔn)確性。

5 結(jié)束語

水下機(jī)器人的運(yùn)動(dòng)控制技術(shù)是機(jī)器人智能化的核心之一，動(dòng)力學(xué)分析是研究運(yùn)動(dòng)控制的基礎(chǔ)。本文首先建立了水下機(jī)器人坐標(biāo)系，對(duì)水下機(jī)器人進(jìn)行了動(dòng)力學(xué)分析，其次建立了水下機(jī)器人6自由度運(yùn)動(dòng)學(xué)模型，描述了水下機(jī)器人的運(yùn)動(dòng)規(guī)律，最后設(shè)計(jì)了一種水下機(jī)器人模糊邏輯控制器，并通過計(jì)算機(jī)仿真實(shí)驗(yàn)驗(yàn)證了控制算法的正確性和有效性。從仿真結(jié)果來看，設(shè)計(jì)的控制器使水下機(jī)器人具有了較好的適應(yīng)性和穩(wěn)定性，進(jìn)而驗(yàn)證了動(dòng)力學(xué)模型的正確性以及控制方法的可行性，同時(shí)也為水下機(jī)器人水下相關(guān)運(yùn)動(dòng)的研究奠定了基礎(chǔ)。

[1]廖華麗,蔡慶安,徐衛(wèi),劉曉磊.自治水下機(jī)器人運(yùn)動(dòng)控制的動(dòng)力學(xué)分析[J].機(jī)電一體化,2008,(3):58-60,63.

[2]朱大奇,胡震.水下機(jī)器人故障診斷與容錯(cuò)控制技術(shù)[M].北京:國防工業(yè)出版社,2012.2:39-40.

[3]蔣新松,封錫盛,王棣棠.水下機(jī)器人[M].沈陽:遼寧科學(xué)技術(shù)出版社,2000:12-17.

[4]De Souza, E.C. Maruyama, N..Intelligent UUVs: Some issues on ROV dynamic positioning[J].IEEE transactions on aerospace and electronic systems,2007,43(1):214-226.

[5]邊宇樞,高志慧,贠超.6自由度水下機(jī)器人動(dòng)力學(xué)分析與運(yùn)動(dòng)控制[J].機(jī)械工程學(xué)報(bào),2007,43(7):87-92.

[6]Zadeh L A.Fuzzy sets[J].Information and Control,1965,8(3):338-353.

[7]Cang Ye Yung, N.H.C. Danwei Wang.A fuzzy controller with supervised learning assisted reinforcement learning algorithm for obstacle avoidance[J].Systems, Man and Cybernetics, Part B,IEEE Transactions on,2003,33(1):17-27.

[8]Yang, S.X. Hao Li Meng, M.Q.-H. Liu, P.X..An embedded fuzzy controller for a behavior- based mobile robot with guaranteed performance[J].Fuzzy Systems, IEEE Transactions on,2004,12(4):436-446.

[9]Mandani, E, H, Assilian, S.An experiment in linguistic synthesis with a fuzzy logic controller[J].International Journal of Man-Machine Studies,1975,1(7):1-13.

[10]黃忠霖,黃京.控制系統(tǒng)MATLAB計(jì)算及仿真[M].北京:國防工業(yè)出版社,2009:105-110.

[11]《魚雷力學(xué)》編著組.魚雷力學(xué)[M].北京:國防工業(yè)出版社,1992.