陳　棟，鄭　賓，郭華玲

(中北大學(xué)儀器科學(xué)與動(dòng)態(tài)測(cè)試教育部重點(diǎn)實(shí)驗(yàn)室，山西 太原 030051)

?

水下姿態(tài)測(cè)試系統(tǒng)運(yùn)動(dòng)的位置算法研究

陳棟，鄭賓，郭華玲

(中北大學(xué)儀器科學(xué)與動(dòng)態(tài)測(cè)試教育部重點(diǎn)實(shí)驗(yàn)室，山西 太原 030051)

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

關(guān)鍵詞：水下運(yùn)動(dòng)；姿態(tài)測(cè)試；位置解算

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

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

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

2載體的運(yùn)動(dòng)過(guò)程分析及軌跡解算

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

圖1　載體的運(yùn)動(dòng)過(guò)程示意圖

對(duì)整個(gè)運(yùn)動(dòng)過(guò)程分成三個(gè)階段來(lái)分析。

2.1產(chǎn)品在布放裝置中運(yùn)動(dòng)階段

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

圖2　受力分析圖

設(shè)置圖中發(fā)射傾角θ=30°，產(chǎn)品受力分析情況如圖2所示。忽略空氣阻力的影響，產(chǎn)品在布放裝置[5]中運(yùn)動(dòng)近似二維運(yùn)動(dòng)， 默認(rèn)產(chǎn)品的軸線為X軸，垂直于軸線為Z軸圖2中，產(chǎn)品受到豎直向下的重力mg，布放裝置沿著Z軸對(duì)產(chǎn)品的支持力Fn，布放裝置沿著X軸方向?qū)Ξa(chǎn)品的滑動(dòng)摩擦力Fs，由于產(chǎn)品做初速度，加速度恒定的直線運(yùn)動(dòng)，所以將外力分別沿著X軸和Z軸分解，根據(jù)牛頓第二定律F=ma來(lái)求解加速度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為滑動(dòng)摩擦力，根據(jù)高中知識(shí)，滑動(dòng)摩擦力等于最大靜摩擦力，默認(rèn)在整個(gè)運(yùn)動(dòng)過(guò)程中，摩擦力恒定不變。利用之前記錄的產(chǎn)品初始滑動(dòng)角度α，可以得到：

mgsinα=Fs.

(2)

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

a=gsinθ-gsinα.

(3)

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

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

(4)

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

(5)

為了使得整個(gè)運(yùn)動(dòng)前后銜接起來(lái)，在這里我們需要將水平和豎直方向組成的坐標(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軸與地理北方向的夾角，方向順時(shí)針為正；俯仰角θ為產(chǎn)品縱軸X軸與當(dāng)?shù)厮矫娴膴A角，方向是水平面上偏為正；橫滾角λ為產(chǎn)品橫軸Y軸與當(dāng)?shù)厮矫鎶A角，方向是水平面上偏為正。

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

圖3中，航向角ψ的大小可以由測(cè)試系統(tǒng)測(cè)得，那么將產(chǎn)品水平放置時(shí)的軸向X軸分解到同一個(gè)水平面內(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，加速度恒定的勻加速運(yùn)動(dòng)，那么三軸的位移Sx、Sy、Sz分別為：

(9)

(10)

(11)

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

(12)

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

(13)

(14)

(15)

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

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

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

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

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

(16)

(17)

(18)

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

圖5　產(chǎn)品在空中運(yùn)動(dòng)位移圖

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

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

圖6　每一小段運(yùn)動(dòng)位移分解圖

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

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

(19)

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

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

(20)

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

(21)

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

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ù)上面的四個(gè)公式可以依次求得產(chǎn)品在各個(gè)方向的運(yùn)動(dòng)軌跡。在這里由于深度傳感器沒(méi)裝，所以沒(méi)法得到深度數(shù)據(jù)，這里只對(duì)水下部分的算法做上述設(shè)計(jì)，這部分算法的驗(yàn)證還需要后期進(jìn)一步來(lái)研究。

在產(chǎn)品軌跡的解算中，我們將運(yùn)動(dòng)過(guò)程分為了3個(gè)部分，每個(gè)部分分別計(jì)算出了在X、Y和Z軸方向的軌跡，因?yàn)槲覀冊(cè)诮馑阒羞x擇的是同樣的參考系，所以最后將這部分的軌跡加起來(lái)，就可以得到產(chǎn)品整個(gè)運(yùn)動(dòng)過(guò)程的軌跡。

3結(jié)束語(yǔ)

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

參考文獻(xiàn)

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

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

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

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

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

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

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

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

[9]施闖,趙齊樂(lè),李敏,等.北斗衛(wèi)星導(dǎo)航系統(tǒng)的精密定軌與定位研究[J].中國(guó)科學(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

中圖分類號(hào)：TP202

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

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

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

收稿日期：2015-09-23