王志博，孫剛

(復旦大學力學與工程科學系，上海200433)

?

常壓潛水裝具運動建模與求解

王志博，孫剛

(復旦大學力學與工程科學系，上海200433)

摘要:針對指導潛水裝具的水下操縱控制和性能設計的問題，建立潛水裝具的水下運動與控制動力學模型，根據(jù)潛水裝具的布置特點建立可動質量、附加質量、水動力和控制力的表述關系式。編制仿真計算程序，對典型水下運動進行計算機仿真。結合動力學模型迭代優(yōu)化計算，優(yōu)化總布置參數(shù)，并給出優(yōu)化計算結果。通過對直航、回轉、關節(jié)運動等典型運動狀態(tài)的建模計算，顯示選定的設計參數(shù)使得裝具具有良好的控制能力，同時具有直航穩(wěn)定性能和回轉性能，可完成水下各種作業(yè)活動。

關鍵詞:常壓潛水裝具;運動建模;控制參數(shù);數(shù)值仿真;多體運動;附加質量

對水下環(huán)境中使用的ROV和AUV等水下探測和作業(yè)裝備，建立合理和完備的運動控制模型不僅有助于水下的作業(yè)活動，而且增強潛水器應對突發(fā)情況的能力。Fossen［1］在其專著中詳細闡述了控制理論和動力學模型應用于水下探測設備的原理和方法，Chin［2］建立了適用于控制一種多推進器的ROV控制模型，Eng等［3］采用系統(tǒng)辨識的方法確認了控制系統(tǒng)必備的流體動力系數(shù)矩陣，減少了流體性能試驗周期。應當針對不同外形、總體布置和任務需求的水下ROV和AUV，基于空間運動控制方程應建立不同的控制仿真平臺［4－9］。常壓潛水裝atmospheric diving suit(ADS)(又稱為硬質潛水服)不同于常規(guī)的水下探測作業(yè)裝備［10］，裝具作為一種人型載人系統(tǒng)，在潛水時內部保持常壓，人員在內部操縱安裝在人形裝具腰間的推進器在水下航行，也可依靠自身的人力在水下完成慢速移動，訓練有素的裝具操作員利用人力輕松活動輕質鋁合金關節(jié)，操作人形裝具手臂端的工具與設備，靈活高效地完成水下作業(yè)。由于裝具需要攜帶供氧、供電、通訊、作業(yè)裝備等下潛，裝配浮力塊調節(jié)壓載平衡浮力。裝具頭部安裝了照明燈具和球形的視窗供操作員觀察，四肢為一組活動關節(jié)組裝具有多個空間自由度的四肢可供人員驅動完成水下作業(yè)，同時胸腔內有控制各系統(tǒng)的操作面板也可供人員操作，浮力包背在人形裝具的背部，腰間懸掛著左右4組推進單元用于上下前后組合推進活動。為減少水面波浪對裝具的影響，設置了中繼站，潛水裝具的動力源由母船通過高強度鎧裝牽纜傳遞給中繼站，中繼站將電力和信號通過柔性臍帶纜傳遞給水下作業(yè)的裝具。本研究結合潛水裝具的外形可變、活動空間自由度和運動穩(wěn)定性等特殊要求，對Chin等［1－2］建立的控制模型進行了修改，引入了裝具活動時的質量矩陣與運動姿勢的變換關系，推導可預報裝具姿態(tài)變化對裝具慣性性能進行預報的數(shù)值方法，編制了動力學預報程序，評估初步設計參數(shù)進行運動穩(wěn)定性能。

1　常壓潛水裝具的動力學模型

本文引入一種廣泛應用的描述水下機器人運動的動力學模型［1］，根據(jù)常壓潛水裝具的水下運動屬性和常壓潛水裝具的水動力分布特點修改了水動力項以及控制方程的系數(shù)項和右端項，并增加了流體動力計算預報模塊，對裝具的可動質量編制了模擬分析算法。動力學模型的表達式為

式中:M=MR+MA為質量慣性矩陣MR與附加質量矩陣MA的和;C包括科里奧利力與向心力;D水動力矩也是一個對角陣，忽略柔性纜的作用力矩;上述系數(shù)矩陣M、C、D均為6×6的矩陣。g是重力和浮力形成的恢復力矩;τ是所有控制的力矩的合力矩，對于潛水裝具而言包括裝具腰間的兩臺推進器和裝具中人的活動力。，v1為線速度，v2為角速度。為裝具的位置和姿態(tài)角。

體軸系和慣性系的歐拉轉換矩陣為

其中J1、J2分別為

為了便于簡化計算模型，慣性系的原點設定為在中繼站幾何中心處，體軸系的原點建立在裝具的重心處，XG=YG=ZG=0，從而使得MR其中第一個3×3的子塊變?yōu)閷蔷仃?，?個3×3的子塊非對角元素Ixy、Ixz和Iyz與對角元素相比變得較小，降低了矩陣求解的奇異性。附加質量矩陣為對角矩陣，采用Hess－Smith面元方法求解，由于裝具在水下做低速運動，水動力耦合影響作用較小，在動力學模型中不予考慮，即附加質量矩陣的非對角元素均為零。對質量矩陣做上述簡化對水下活動速度小于0.5 m/s的裝具而言具有足夠的精度。質量矩陣表述成為如下形式:

C(v)項包含了科氏力和向心力等作用力，是質量和附加質量對隨體坐標系的旋轉效應，C(v)表達式為

其中

D是水動力阻尼力矩陣，對于非流線型的潛水裝具而言，形成水動力的成因包括表面摩擦力、渦流脫落形成的非定常作用力等。實際上隨著航行速度的變化，潛水裝具受到的水動力將呈現(xiàn)出強非線性，并且相互耦合作用的效應顯著，尤其是作用的力矩與潛水裝具的航速增加其非線性和耦合效應更加顯著。對于低速范圍內的水動力而言，潛水裝具的水動力可采用二次函數(shù)近似如下:

在裝具的隨體系中，裝具的恢復力向量g(η)表達式為

由于隨體系中xG=yG=zG=0，上式還可進一步簡化。定義u1、u2為水平推進器對稱布置在裝具的腰部，u3、u4為垂向推進器同樣對稱布置在推進器的腰部，裝具的4只推進器發(fā)出的推力和力矩可表述為

式中:u為電信號驅動4只推進器發(fā)出的推力向量，為推進器發(fā)出的有效推力;l2為對稱布置的水平推進器與隨體坐標系的原點的垂向距離;l3為水平推進器與裝具對稱面的距離。值得注意的是，裝具并沒有安裝可以控制側向移動和滾轉自由度的推進器，這是由于人形裝具的回轉阻尼力較小，易通過調節(jié)水平推進器的輸入電流發(fā)出的推力差實現(xiàn)轉動，從而使裝具轉向克服側向來流造成的影響，由于裝具自身的扶正力矩大，不考慮推進器對存在滾轉角的裝具扶正控制作用。

2　裝具運動計算建模

圖1顯示了裝具的總體布置，針對其構型及運動特點，對潛水裝具的動力學模型建立仿真計算模型，并給出相關模型系數(shù)的測定和計算，通過構造動力學數(shù)值計算模型，模擬裝具在典型海流作用情況下的運動。按照動力學模型將隨體坐標系的原點建立在重心處，通過對裝具的計算機虛擬總裝配完成了各項配件的總裝后計算得到了裝具的質量屬性包括裝具的質量和轉動慣量分別為m=598 kg，Ixx=159.9 kg·m2，Iyy=165.2 kg·m2，Izz=59.5 kg·m2，Ixy=17.1 kg·m2，Ixy=55.3 kg·m2。

圖1　常壓潛水裝具的總布置Fig.1 Configuration of atmospheric diving suit

對直立行走姿態(tài)裝具的附加質量阻尼采用Hess－Smith方法求解，對完成裝配的裝具CAD模型進行適當簡化，去掉裝具的小尺寸構件如天線、夾具、燈具，對復雜外形進行適當簡化，而后在裝具的簡化CAD外形布置面元。計算得到如下的直立行走狀態(tài)對應的附加質量阻尼:X˙u=358 kg，Y˙v=385 kg，Z˙w=146 kg，K˙p=40.9 kg·m2，M˙q=41.9 kg· m2，N˙r=2.6 kg·m2。

通過浮力材料和搭載設備的優(yōu)化布置，進行重心、重量和浮心、浮力的計算，得到搭載潛航員的裝具的標準水下凈浮力5 kg，也可根據(jù)水下作業(yè)任務和海流環(huán)境的不同調整重量使得水下重量略大于浮力，增強裝具運動穩(wěn)定性，此外由于潛航員的四肢活動形成了可動重量恢復力向量是小范圍內可變的，在水下運動計算模擬時應考慮潛航員的水下活動形成可動重量，及造成的質量特征矩陣的變化。這里還應當建立質量矩陣、附加質量阻尼和慣性矩與裝具姿態(tài)的關系，根據(jù)裝具在水下的四肢運動速度低、運動幅度小，大幅度的運動姿勢需要與推進器推力相互配合，克服扶正力矩矢量的作用等特點。對裝具進行水下作業(yè)活動時的質量矩陣與運動姿勢建立如下關系:

式中:mi表示裝具的可動質量(i=1，2，3，…)表示可動質量的質心在體軸系中的位置。i表示位于關節(jié)處的隨體坐標系，伴隨裝具四肢的運動，可動質量的質心位置隨時間而變化，由于裝具的活動關節(jié)的自由度限定了四肢的運動范圍和運動路徑，故可動質量的質心在關節(jié)銜接處的隨體坐標系的空間運動軌跡可利用體軸系和關節(jié)隨體坐標系原點矢徑Ri=與各活動關節(jié)的坐標變換矩陣J1換算至體軸系中:

決定裝具附加質量阻尼的構型，主要包括裝具的大體積浮力背包、軀干、頭盔、上肢、下肢等主要部件，下肢僅可做小幅度轉動，其動力學影響可忽略，對裝具上肢的相對運動姿態(tài)可采用高效率的Hess－Smith方法自動劃分三角形的面元，實時計算附加質量阻尼矩陣，便于實時求解動力學方程組。在裝具上肢活動的肩關節(jié)和肘關節(jié)處對稱的建立4個隨體坐標。初步設計中裝具做直立姿態(tài)前進時，浮心與重心的相對位置設定為:xB=0，yB=－0.013 m，zB=0.031m。裝具在水中具有5°前傾角，由于裝具低速運動，恢復力矩的值是決定裝具運動穩(wěn)定性和水下活動能力和人員舒適性的關鍵因素，也涉及到裝具的安全性。因此在設計中增加了可調節(jié)位置和可拋棄的重塊，從而可調節(jié)扶正力矩。體軸系中，推進器安裝位置為l2=0.12 m，l3=0.385 m。

裝具的水動力系數(shù)采用了水池試驗測定［11］，由于裝具的直立行走工況最為常用，以裝具直立行走的姿態(tài)為水池測試對象，制作1∶1的水池試驗模型，進行了拖曳試驗和操縱性能試驗，通過對試驗數(shù)據(jù)的回歸整理，測試獲得了相關的水動力系數(shù)為Xu=0.032，Xu|u|=0.762，Yv=0.872，Yv|v|=1.323，Zw=0.355，Zw|w|=0.018 3，Kp=0.023，Kp|p|=0.001，Mq=0.267，Mq|q|=0.019，Nr=0.069，Nr|r|=0.003。

裝具的水下運動姿態(tài)變化對水動力阻尼系數(shù)有較大影響。本研究僅對直立航行姿態(tài)進行了測試，還需要對主要部件相對運動形成的水動力阻尼進行估算，裝具的四肢為柱形和錐形，對如雙臂相對于裝具的主體的運動形成的水動力阻尼力采用圓柱水動力阻尼計算。

其中

Dj中的部件水動力系數(shù)取低雷諾數(shù)范圍內的圓柱運動阻尼系數(shù)。對關節(jié)隨體坐標系中的相對運動的關節(jié)在運動方向的投影面積進行計算。與常規(guī)潛水器的運動性能和穩(wěn)定性能的評估方法不同，本研究利用該鈍體的水池測試獲得流體動力系數(shù)以及裝具的質量和阻尼特性，展開為上述形式，代入動力學計算模型，計算評估裝具的水下直線運動穩(wěn)定性能，航行方向穩(wěn)定性能以及姿態(tài)等靜穩(wěn)定性能，并評估裝具的動穩(wěn)定性。對裝具完成相關的水下作業(yè)活動所應當具備的活動能力，如俯身作業(yè)、翻身等典型運動進行計算。

3　裝具典型運動狀態(tài)動力學計算

裝具的水下運動受到潛航員實時控制，在裝具的設計階段對裝具水下運動是否可控，是否符合人機工程要求，以及總體布置參數(shù)的選擇設計是否合理等需要進行驗證。選擇已有的控制和分析策略對設計參數(shù)進行評估顯得尤為重要。在裝具的初步設計階段，設定了航速、海流、活動范圍等目標量，利用動力學模型評估與之相關的設計參數(shù)如推進器布置、恢復力矩、空間活動軌跡的設計是否處于合理的范圍，動力學計算流程框圖如圖2所示。

圖2　仿真計算流程Fig.2 Computational flow chart

運用Matlab Simulink工具箱編制了程序求解式(1)，在給定裝具的初始位置和姿態(tài)的情況下，以速度為未知變量時方程組為一階微分方程組，控制方程組的求解采用了四階龍格庫塔法進行求解。求解過程中需要調用各項系數(shù)矩陣和右端項。分別對質量屬性矩陣、慣性力矩陣、水動力矩陣、恢復力矩陣、控制力矩陣分別編制了對應的求解子程序，對推進器匹配、面元劃分等定義為兩個計算模塊，計算輸出的量包括裝具重心的軌跡、姿態(tài)角、運動速度等未知變量。

六自由度模型的計算模塊包括:

1)質量屬性矩陣的計算

根據(jù)裝具的總體布置可得到裝具的質量M1和慣性矩矩陣M2，利用式(7)求解質量屬性矩陣，包括質量矩陣MR與附加質量矩陣MA兩部分，質量矩陣含有可動質量，應用式(8)求解可動質量矩陣，通過指定可動質量的質心在關節(jié)銜接處的隨體坐標系的空間運動軌跡曲線預先給定關節(jié)處的局部坐標系下裝具在水下的四肢活動姿態(tài)隨時間的變化軌跡，將質心軌跡的時間歷程線性插值，計算給定的時間步長對應的質心位置，應用式(10)通過坐標變換到隨體坐標系中獲得質量和轉動慣量。

2)附加質量矩陣的計算

式(9)包括了所需求解的附加質量，由于裝具為鈍體外形，利用Hess－smith算法計算得到的附加質量矩陣中非對角線元素的計算結果往往具有較大的誤差，本研究僅計算附加質量矩陣對角線的元素，在裝具外形發(fā)生變化后需要對修改后的裝具外形表面劃分面元，通過對三維造型軟件進行二次開發(fā)，在給出了四肢活動軌跡后，可確定某一時刻的裝配體姿態(tài)，導出裝配體的外表面，并對相交曲面進行布爾運算，自動修復后，完成表面面元的劃分，輸出面元劃分文件后代入附加質量計算模塊進行計算。

3)水動力阻尼矩陣計算

裝具在水下進行低速運動，根據(jù)表3給出的水流阻力和力矩系數(shù)，計算水流阻尼力矩，利用式(11)、(12)所示的二次函數(shù)計算可動部件造成的水流阻力的影響。

4)旋轉效應矩陣的計算

包括了相對于初始重心位置處的質量分布形成的質量慣性矩，由于可動質量距離重心較遠，形成的慣性矩的變化量較大，通過質心的變化造成的慣性矩可利用造型軟件計算后代入式(4)中計算慣性力矩。

5)推進控制模塊設計及計算

本設計中需要對布置在腰間的四部推進器發(fā)出的推力進行合理的匹配，才能形成對裝具的合理控制力和矩，利用式(6)計算所需的推力匹配關系。

6)恢復力矩的設計求解

恢復力矩需要給定裝具的重量和浮力，重心和浮心的相對位置，根據(jù)式(5)計算恢復力矩設定裝配調整參數(shù)。

上述各模塊的計算流程見圖3。

圖3　運動模型的求解流程Fig.3 Solving schemes of kinematics model

3.1裝具的直線航行

裝具近海底以0.5 kn的航速直線航行時，僅開啟兩只水平推進器發(fā)出同等大小的推力，17.25 kg的推力克服水平來流的阻力實現(xiàn)水平直線前進，通過求解動力學方程組，獲得了裝具在直線前進的過程中航行速度、俯仰角度隨航行時間的變化規(guī)律。通過求解動力學方程驗證裝具在直線航行時的運動穩(wěn)定性能。

圖4　裝具水下直航軌跡Fig.4 Straight line walking trajectory of ADS

圖5　裝具俯仰角與航行速度Fig.5 Pitch and cruise velocity of ADS

3.2裝具回轉運動

腰部安裝的兩只水平推進器在進入穩(wěn)定的直線航行狀態(tài)后，調整兩只推進器的推力大小，增加右側推進器推力為23.5 kg和25.5 kg，降低左側的推進器推力為11 kg和9 kg，形成的航行軌跡如圖6所示，對應的滾轉角和俯仰角如圖7所示。

圖6　裝具回轉運動軌跡Fig.6 Full turn trajectory of ADS

圖7　裝具回轉運動姿態(tài)角Fig.7 Full turn phase angle of ADS

3.3裝具邊行走邊雙臂周向擺動

以直立姿態(tài)為基本姿態(tài)，取隨體坐標系x1y1z1和x2y2z2為左右肩關節(jié)的隨體坐標系，該隨體坐標系原點與體軸系的矢徑:，左右肩關節(jié)的隨體坐標系原點與體軸x0的夾角為φ01，02=67.5°，與體軸y0的夾角為θ01，02=13.2°，肘關節(jié)相對肩關節(jié)附連運動。假定由潛航員體力驅動裝具使得雙臂繞隨體系的原點為不動點做角速度為ω=1°/s的緩慢轉動。

裝具同時發(fā)出推力克服水平來流的阻力做0.5 kn的前進運動。通過求解前述建立的動力學方程組，可知裝具仍然具有一定的直線保持性，但裝具的姿態(tài)角軌跡運動情況如圖8所示。由于雙臂的微速旋轉運動形成的擾動作用力和力矩，對縱傾角的波動有一定的改善。當裝具靜止于水中時，潛航員仍然做上述運動，那么裝具會發(fā)生緩慢的自轉運動，并伴有增加前傾角度的趨勢，姿態(tài)角如圖9所示。

圖8　有航速時對姿態(tài)和速度的影響Fig.8 Effect of arm swing on gesture of cruising ADS at given speed

圖9　無航速時對姿態(tài)和速度的影響Fig.9 Effect of arm swing on gesture of stationary ADS

4　結論

結合剛體動力學模型和多體運動學模型建立了用于計算潛水裝具的動力學模型，針對裝具的水下運動具有多體運動的特性，著重細致的建立了動力學系統(tǒng)中的質量矩陣模型和附加阻尼矩陣模型。并對多體運動情況下的動態(tài)附加質量阻尼和動態(tài)質量特性進行了計算，通過對直航、回轉、關節(jié)運動等典型運動狀態(tài)的建模計算得出如下結論:

1)首次建立包含多體動運動特性模型的完善裝具運動動力學模型，對裝具的水下水動力預報采用了快速水動力預報模塊，應用于裝具水下運動性能的設計參數(shù)的設計評估預報。

2)建立可動質量沿著裝具軀體運動造成的重心、質量特性的變化模型。首次結合多體動力學的數(shù)學模型描述裝具運動。

3)結合裝具的低速活動四肢頻繁操作運動并需精確定位等特點，以及慣性力矩和重力浮力等靜力起主要影響的作用的特點，首次對外形的變化引

起的附加質量采用CAD曲面處理、網(wǎng)格自動劃分Hess－Smith快速預報方法進行預報附加質量力。

在水池試驗和文獻給出了詳盡的水動力參數(shù)、總體預裝給出的運動和控制特征參數(shù)的基礎上，將上述兩個運動模型整合，形成實時預報的程序模塊，可對裝具設計過程中給出總體裝配性能參數(shù)進行初步的分析評價，對水下直航、回轉、自轉等典型的運動進行預報，對運動性能建立了評估的平臺，給出了典型的仿真計算結果。

參考文獻:

［1］FOSSEN T I.Guidance and control of ocean vehicles［M］.2nd ed.New York:John Wiley＆Sons Ltd，1994:25－46.

［2］CHIN C S，LAU M W S，LOW E，et al.Software for modeling and simulation of a remotely operated vehicle(ROV)［J］.International Journal of Simulation Modeling，2006，5(3):114－125.

［3］ENG Y H.Identification of hydrodynamic terms for underwater robotic vehicle［R］.Master First Year Report，NTU，Robotic Research Center，Mechanical and Aerospace Engineering，2007.

［4］戴學豐，邊信黔.6自由度水下機器人軌跡控制仿真研究［J］.系統(tǒng)仿真學報，2001，13(3):368－369.DAI Xuefeng，BIAN Xinqian.Simulation on trajectory control of a 6－DOF underwater vehicle［J］.Journal of System Simulation，2001，13(3):368－369.

［5］段斐.微小型水下機器人運動仿真研究［D］.哈爾濱:哈爾濱工程大學，2012:29－34.DUAN Fei.Research on motion simulation of a small underwater vehicle［D］.Harbin:Harbin Engineering University，2012:29－34.

［6］范士波.深海作業(yè)型ROV水動力試驗及運動控制技術研究［D］.上海:上海交通大學，2013:90－95.FAN Shibo.Hydrodynamics test and research on motion control for deep sea work－class remotely operated vehicle［D］.Shanghai:Shanghai Jiaotong University，2013:90－95.

［7］王波，蘇玉民，秦再白.微小型水下機器人操縱性能與運動仿真研究［J］.系統(tǒng)仿真學報，2009，21(13):4149－4152，4158.WANG Bo，SU Yumin，QIN Zaibai.Research on maneuverability and simulation of small autonomous underwater vehicle［J］.Journal of System Simulation，2009，21(13):4149－4152，4158.

［8］王冠達，葛彤，趙敏，等.ROV近底行走裝置水平面動力學建模及穩(wěn)定性分析［J］.船舶工程，2015，37(1):95－99.WANG Guanda，GE Tong，ZHAO Min，et al.Dynamic modeling and stability analysis of sled device of ROV in horizontal plane［J］.Ship Engineering，2015，37(1):95－99.

［9］劉可峰，連璉，曹俊亮，等.基于模糊PID方法的ROV操縱控制仿真［J］.江蘇科技大學學報:自然科學版，2015，29(1):70－75.LIU Kefeng，LIAN Lian，CAO Junliang，et al.Simulation on manoeuvre and control of ROV based on fuzzy PID algorithm ［J］.Journal of Jiangsu University of Science and Technology:Natural Science Edition，2015，29(1):70－75.

［10］嚴似松，黃根余.單人常壓潛水服運動阻力試驗研究［J］.海洋工程，1990，8(2):44－49.YAN Shisong，HUANG Genyu.An experimental research on atmosphere diving suit’s resistance［J］.The Ocean Engineering，1990，8(2):44－49.

［11］崔維成，葉聰，顧繼紅.國際援潛救生裝備體系現(xiàn)狀與發(fā)展趨勢［J］.船舶力學，2008，12(5):830－844.CUI Weicheng，YE Cong，GU Jihong.Current status and development trend of international submarine rescue system ［J］.Journal of Ship Mechanics，2008，12(5):830－844.

Kinematic modeling of an atmospheric diving suit

WANG Zhibo，SUN Gang
(Mechanics and Engineering Science Department，F(xiàn)udan University，Shanghai 200433，China)

Abstract:The underwater performance of an atmospheric diving suit(ADS)is entirely dependent on its kinematic properties and design.A kinematic control model of an ADS was designed and fabricated.The relationship between movable mass，added mass，hydrodynamic forces，and control forces was investigated.Typical six－degree movements were simulated using an iterative optimization algorithm.The design parameters were optimized for typical movements including straight line，turning，and joint movements.The appropriate control parameters were identified for controlling the ADS in all types of underwater operations.

Keywords:atmospheric diving suit(ADS);kinematic modeling;control parameters;numerical simulation;multibody motion;added mass

通信作者:孫剛，E－mail:Gang_sun@ fudan.edu.cn.

作者簡介:王志博(1983－)，男，工程師，博士;孫剛(1966－)，男，教授，博士.

收稿日期:2014－10－12.網(wǎng)絡出版時間:2015－12－21.

中圖分類號:TP242

文獻標志碼:A

文章編號:1006－7043(2016)01－0132－07

doi:10.11990/jheu.201410027

網(wǎng)絡出版地址:http://www.cnki.net/kcms/detail/23.1390.u.20151221.1509.006.html