陳　棟，鄭　賓，郭華玲

(中北大學(xué)儀器科學(xué)與動態(tài)測試教育部重點實驗室，山西 太原 030051)

?

水下姿態(tài)測試系統(tǒng)運動的位置算法研究

陳棟，鄭賓，郭華玲

(中北大學(xué)儀器科學(xué)與動態(tài)測試教育部重點實驗室，山西 太原 030051)

摘要：針對海洋探測中水下運動研究的需求，本文基于微慣導(dǎo)的水下姿態(tài)測試系統(tǒng)，介紹了該姿態(tài)測試系統(tǒng)的功能并分析其所搭載產(chǎn)品的運動情況，提出了一種針對水下運動載體的位置解算算法。

關(guān)鍵詞：水下運動；姿態(tài)測試；位置解算

當(dāng)今世界，海洋不僅是物流運輸業(yè)的重要通道，更是一個有著富饒資源的寶庫。隨著對海洋[1]關(guān)注度的提高，將高科技應(yīng)用到水下裝備中是勢在必行的。而為了更好地評價水下裝備的性能，就需對水下裝備運動中的姿態(tài)及運動軌跡進(jìn)行研究。

1測試系統(tǒng)的功能

水下姿態(tài)測試系統(tǒng)[4]可以測試并存儲載體運動過程中的三軸X、Y、Z的線加速度、三軸磁場強(qiáng)度和三軸角速度；內(nèi)部自帶的慣性傳感部分可以根據(jù)測試參數(shù)自行解算出測試系統(tǒng)[2]的姿態(tài)角：俯仰角、橫滾角和航向角。該測試系統(tǒng)具有體積小，精度高，功耗低的優(yōu)點，可以安裝在空間有限的各種載體或設(shè)備中，實現(xiàn)水下長時間工作，配置的數(shù)據(jù)采集存儲模塊可以收集保存數(shù)據(jù)，以備實驗結(jié)束后回傳數(shù)據(jù)，進(jìn)行數(shù)據(jù)處理和分析。

2載體的運動過程分析及軌跡解算

某實驗過程中，測試系統(tǒng)所搭載的產(chǎn)品搭載在載體上，利用測試系統(tǒng)來測試載體的運動姿態(tài)[5，6]和解算運動軌跡。載體的運動過程示意圖如圖1所示。

圖1　載體的運動過程示意圖

對整個運動過程分成三個階段來分析。

2.1產(chǎn)品在布放裝置中運動階段

產(chǎn)品被裝在保護(hù)殼體中，產(chǎn)品發(fā)射入水需要一定的角度，軌道升到一定角度后打開定位器，產(chǎn)品開始受到合外力而開始順著布放裝置向下滑動，滑出布放裝置，做直線運動，記下此時的傾角θ的值。

圖2　受力分析圖

設(shè)置圖中發(fā)射傾角θ=30°，產(chǎn)品受力分析情況如圖2所示。忽略空氣阻力的影響，產(chǎn)品在布放裝置[5]中運動近似二維運動， 默認(rèn)產(chǎn)品的軸線為X軸，垂直于軸線為Z軸圖2中，產(chǎn)品受到豎直向下的重力mg，布放裝置沿著Z軸對產(chǎn)品的支持力Fn，布放裝置沿著X軸方向?qū)Ξa(chǎn)品的滑動摩擦力Fs，由于產(chǎn)品做初速度，加速度恒定的直線運動，所以將外力分別沿著X軸和Z軸分解，根據(jù)牛頓第二定律F=ma來求解加速度a。我們將重力分別分解為Gx和Gz，其中Gx=mgsinθ，Gz=mgcosθ。Z軸受力平衡，則X軸有：

F合=mgsin-Fs，從而我們可以得到產(chǎn)品的加速度，方向沿著X軸的正方向：

a=gsinθ-Fs/m.

(1)

在公式(1)中，F(xiàn)s為滑動摩擦力，根據(jù)高中知識，滑動摩擦力等于最大靜摩擦力，默認(rèn)在整個運動過程中，摩擦力恒定不變。利用之前記錄的產(chǎn)品初始滑動角度α，可以得到：

mgsinα=Fs.

(2)

所以根據(jù)公式(1)(2)得：

a=gsinθ-gsinα.

(3)

公式(3)中θ和α皆為產(chǎn)品的俯仰角，可以測試系統(tǒng)測得。由于我們需要研究產(chǎn)品的水平方向和豎直方向的軌跡，所以這里需要將加速度a分解成水平和豎直方向的兩個分量ax和ay，那么產(chǎn)品沿水平方向做初速度為0，加速度為ax的勻加速運動，沿豎直方向做初速度為0，加速度az的勻加速運動：

ax=(gsinθ-gcosθ)cosθ.

(4)

ay=(gsinθ-gcosθ)sinθ.

(5)

為了使得整個運動前后銜接起來，在這里我們需要將水平和豎直方向組成的坐標(biāo)系變換到導(dǎo)航坐標(biāo)系中，這里采用常用的ENU導(dǎo)航坐標(biāo)系[1](即東北天笛卡爾坐標(biāo)系[9])：規(guī)定在當(dāng)?shù)厮矫妫乩頄|為水平X1軸方向，地理北為水平Y(jié)1軸方向，天為垂直于地面的Z1軸，姿態(tài)角分別為：航向角ψ為產(chǎn)品縱軸X軸與地理北方向的夾角，方向順時針為正；俯仰角θ為產(chǎn)品縱軸X軸與當(dāng)?shù)厮矫娴膴A角，方向是水平面上偏為正；橫滾角λ為產(chǎn)品橫軸Y軸與當(dāng)?shù)厮矫鎶A角，方向是水平面上偏為正。

圖3　加速度坐標(biāo)變換示意圖

圖3中，航向角ψ的大小可以由測試系統(tǒng)測得，那么將產(chǎn)品水平放置時的軸向X軸分解到同一個水平面內(nèi)的X1軸和Y1軸，加速度坐標(biāo)變換示意圖如圖3所示。

那么導(dǎo)航坐標(biāo)系的三軸加速度分別為：

ax=(gsinθ-gsinα)cosθsinψ.

(6)

ay=(gsinθ-gsinα)cosθcosψ.

(7)

az=(gsinθ-gsinα)sinθ.

(8)

所以可知產(chǎn)品在導(dǎo)航坐標(biāo)系的三軸內(nèi)做初速度為0，加速度恒定的勻加速運動，那么三軸的位移Sx、Sy、Sz分別為：

(9)

(10)

(11)

產(chǎn)品在布放裝置中的運動距離為L，產(chǎn)品離開布放裝置之前，做勻加速直線運動。根據(jù)公式(12)可以計算離開布放架時的速度，

(12)

式中，vt為t時刻的速度，a為加速度，s為位移。根據(jù)上述角度參數(shù)和公式(12)我們可以得到產(chǎn)品離開布放架時三軸的速度：

(13)

(14)

(15)

至此，我們得到了產(chǎn)品在離開布放裝置之前的在導(dǎo)航坐標(biāo)系三軸軌跡和三軸線速度。然后通過實驗數(shù)據(jù)的解算[2，3]來驗證算法的正確性，產(chǎn)品的各項初始參數(shù)為：θ=30°，ψ=45°，α=10°，L=2 m，經(jīng)過計算得到如圖4所示的在布放裝置中運動的三個軸位移圖。當(dāng)產(chǎn)品離開布放裝置時，X1軸速度為2.2 m/s，Y1軸速度為2.2 m/s，Z1軸的速度為1.8 m/s。

圖4　產(chǎn)品在布放裝置中運動的位移

2.2產(chǎn)品離開布放裝置入水前的空中的運動過程

產(chǎn)品在空中的運動過程中，如果不考慮空氣阻力的影響，那么物體在空中只受重力，產(chǎn)品做勻加速運動，但是由于產(chǎn)品的重心和形心不重合，所以產(chǎn)品在運動過程中會有轉(zhuǎn)動，轉(zhuǎn)動角度可以由測試系統(tǒng)的輸出數(shù)據(jù)得到。

物體在空中做勻加速運動，X1、Y1和Z1軸方向的初始速度分別為產(chǎn)品離開布放架時的速度，水平方向不受力，所以物體在水平方向(X1和Y1軸)做勻速直線運動；豎直方向受重力，加速度為g，在豎直方向(Z1軸)做勻加速直線運動。從而我們可以得到產(chǎn)品在導(dǎo)航坐標(biāo)系中的位移分別為：

(16)

(17)

(18)

當(dāng)產(chǎn)品離開布放裝置時，產(chǎn)品的速度分為：X軸方向速度是2.2m/s，Y軸方向速度是2.2m/s，Z軸方向的速度是1.8m/s?？罩械倪\動位移可以通過上面的公式測試得到。圖5是空中三個軸向的位移圖，從圖5中可以看出在空中的運動高速為2m，X和Y軸運動距離均為1m，與實際情況相符。

圖5　產(chǎn)品在空中運動位移圖

2.3產(chǎn)品入水后軌跡的解算

當(dāng)產(chǎn)品入水以后，由于水中環(huán)境復(fù)雜，產(chǎn)品受到多個力的作用做復(fù)雜運動，我們還是通過解算導(dǎo)航坐標(biāo)系三軸的位移來分析。這里采用微元法[8]將運動過程分解為多個小段，以MTi[7]的采樣周期每10ms當(dāng)作一個小段來分析，在這段時間內(nèi)產(chǎn)品近似做直線運動。位移分解圖如圖6所示，為了和前面的區(qū)分開，這里定義一些新的變量：L為入水深度值，ΔL為深度值的變化量，λ為產(chǎn)品的俯仰角，W水平為產(chǎn)品在水平方向的位移。

圖6　每一小段運動位移分解圖

圖6中，當(dāng)入水深度增加ΔL，俯仰角為λ時，水平方向的位移增量為：

W水平=ΔL×cot(λ) .

(19)

得到水平方向的位移增量后，通過航向角，將位移增加量分解到導(dǎo)航坐標(biāo)系的X1和Y1軸中，得到：

Wx1增=ΔL×cot(λ)×sinψ.

(20)

Wy1增=ΔL×cot(λ)×cosψ.

(21)

當(dāng)產(chǎn)品入水深度增加ΔL，通過俯仰角和航向角，可以得到X1和Y1軸的位移增量，入水深度從0開始，當(dāng)深度為0時，X1軸和Y1軸的位移增量也為零，每隔10 ms，得到一個深度增量，相應(yīng)的得到一個X1和Y1軸的位移增量，從而得到產(chǎn)品在水中的總位移。在t時刻的三個方向的位移可以由下面公式求得：

Wx1(t)=(L(t)-L(t-0.01))×

cot(λ)×sinψ+Wx1(t-0.0) .

(22)

Wy1(t)=(L(t)-L(t-0.01))×

cot(λ)×cosψ+Wy1(t-0.0) .

(23)

Wz1(t)=L(t) .

(24)

Wx1(0)=Wy1(0)=Wz1(0)=0 .

(25)

根據(jù)上面的四個公式可以依次求得產(chǎn)品在各個方向的運動軌跡。在這里由于深度傳感器沒裝，所以沒法得到深度數(shù)據(jù)，這里只對水下部分的算法做上述設(shè)計，這部分算法的驗證還需要后期進(jìn)一步來研究。

在產(chǎn)品軌跡的解算中，我們將運動過程分為了3個部分，每個部分分別計算出了在X、Y和Z軸方向的軌跡，因為我們在解算中選擇的是同樣的參考系，所以最后將這部分的軌跡加起來，就可以得到產(chǎn)品整個運動過程的軌跡。

3結(jié)束語

本文在對產(chǎn)品的運動過程進(jìn)行了受力分析的基礎(chǔ)上，分析了產(chǎn)品搭載的測試系統(tǒng)的位置的計算；在matlab環(huán)境下，根據(jù)水上部分的解算數(shù)據(jù)進(jìn)行了部分仿真還原。結(jié)果顯示符合實際情況，證明了算法的合理性，但是，水下情況還需要今后進(jìn)一步來研究。

參考文獻(xiàn)

[1]李江玲.海洋戰(zhàn)略與愛國情懷[J].民生周刊(學(xué)術(shù)版)，2013(7):100-103.

[2]郭晶晶.常用姿態(tài)測試技術(shù)的綜述[J].山西電子技術(shù),2013(5):87-89.

[3]劉浩,韓晶.MATLAB R2014a完全自學(xué)一本通(暢銷升級版)[M].北京:電子工業(yè)出版社,2014.

[4]張文棟.存儲測試系統(tǒng)的設(shè)計理論及其應(yīng)用[M].北京:高等教育出版社,2002.

[5]李向楠.水下姿態(tài)解算的算法研究[D].太原：中北大學(xué),2014.

[6]杜振宇,石庚辰.彈體飛行姿態(tài)測量方法探討[J].探測與控制學(xué)報,2002(1):53-56+60.

[7]MTI User Manual//www.xsens.com.

[8]曹弋,楊忠振,左忠義.基于經(jīng)典力學(xué)與有限元的交通事故再現(xiàn)分析法[J].中國安全科學(xué)學(xué)報,2014(10):42-47.

[9]施闖,趙齊樂,李敏,等.北斗衛(wèi)星導(dǎo)航系統(tǒng)的精密定軌與定位研究[J].中國科學(xué):地球科學(xué),2012(6):854-861.

Research on the Algorithm of Moving Position for Underwater Attitude Measurement System

Chen Dong, Zheng Bin, Guo Hualing

(KeyLabofInstrumentationScience&DynamicMeasurement,MinistryofEducation,NorthUniversityofChina,TaiyuanShanxi030051,China)

Abstract:For the needs of underwater motion research in marine exploration, based on the underwater attitude test system of micro inertial navigation, the paper describes the function of this system and the movement of the product equipped with the system. A solution algorithm for the underwater location of moving carrier is proposed.

Key words:underwater sports; attitude test; location solution

中圖分類號：TP202

文獻(xiàn)標(biāo)識碼：A

文章編號：1674- 4578(2016)01- 0080- 03

作者簡介：陳棟(1988- )，男，山西呂梁人，碩士研究生，研究方向：動態(tài)測試與智能儀器。

收稿日期：2015-09-23