本發(fā)明屬于水下運(yùn)動(dòng)目標(biāo)的姿態(tài)估計(jì)及控制領(lǐng)域，尤其涉及一種連續(xù)沖擊載荷影響下的水下潛器運(yùn)動(dòng)姿態(tài)估計(jì)及控制技術(shù)。
背景技術(shù)：
：作為發(fā)射平臺(tái)的水下潛器必須滿足苛刻的平衡狀態(tài)才能為被發(fā)射的水下航行體提供良好的初始狀態(tài)。但是被發(fā)射的水下航行體一旦點(diǎn)火，水下潛器受到劇烈沖擊，引起升沉、縱傾、橫傾等一系列運(yùn)動(dòng)和姿態(tài)變化，使其平衡狀態(tài)遭到破壞，這直接影響所發(fā)射的水下航行體的發(fā)射狀態(tài)和精度。因此，如何對(duì)連續(xù)沖擊載荷環(huán)境下的水下潛器姿態(tài)進(jìn)行估計(jì)和控制，為其所發(fā)射的水下航行體提供良好的初始狀態(tài)，是一個(gè)值得研究的問(wèn)題。技術(shù)實(shí)現(xiàn)要素：本發(fā)明通過(guò)分析水下潛器的運(yùn)動(dòng)響應(yīng)情況，基于其六自由度運(yùn)動(dòng)方程和動(dòng)力學(xué)方程，結(jié)合水下潛器水下狀態(tài)的平衡方程，建立水下潛器低速定深航行運(yùn)動(dòng)仿真模型，考慮連續(xù)發(fā)射水下航行體時(shí)產(chǎn)生的連續(xù)沖擊載荷，添加發(fā)射水下航行體的反沖力和水錘效應(yīng)模型，對(duì)連續(xù)沖擊載荷下的水下潛器運(yùn)動(dòng)姿態(tài)進(jìn)行估計(jì)，并建立相應(yīng)的水下潛器深度與縱傾的pid姿態(tài)控制方法。建立固連于大地的地面坐標(biāo)系ox0y0z0，原點(diǎn)o置于運(yùn)動(dòng)時(shí)刻潛器浮心在水面上的投影點(diǎn)，ox0z0坐標(biāo)平面與水面重合，y0軸垂直向上。再建立原點(diǎn)位于浮心的潛器坐標(biāo)系oxyz，ox軸指向潛器頭部，oy軸在潛器縱對(duì)稱面內(nèi)與ox軸垂直，oz軸垂直于oxy平面，指向按右手系確定。速度坐標(biāo)系原點(diǎn)與潛器坐標(biāo)系原點(diǎn)重合，速度坐標(biāo)系ox'軸與原點(diǎn)處潛器的速度矢量方向重合，oy'軸位于潛器的縱對(duì)稱面內(nèi)，垂直于ox'軸并指向上方，oz'軸與ox'y'z'平面垂直，指向按右手系確定。地面坐標(biāo)系、速度坐標(biāo)系和潛器坐標(biāo)系示意圖如附圖1所示。潛器在空間的位置由潛器浮心在地面坐標(biāo)系內(nèi)的坐標(biāo)給出，潛器在空間的姿態(tài)由三個(gè)歐拉角確定。這三個(gè)歐拉角的定義為：將潛器坐標(biāo)系平移至與其原點(diǎn)與地面坐標(biāo)系原點(diǎn)重合的位置，再?gòu)脑撐恢瞄_始，依次繞oy、oz、ox軸旋轉(zhuǎn)三個(gè)角度使得兩個(gè)坐標(biāo)系完全重合，我們將三個(gè)角度的組合稱為歐拉角，并將這三個(gè)角度用于表示潛器的空間姿態(tài)，分別稱為偏航角、俯仰角和橫滾角。由地面系ox0y0z0到潛器系oxyz的轉(zhuǎn)換矩陣為反之，由潛器系oxyz到地面系ox0y0z0的轉(zhuǎn)換矩陣為的轉(zhuǎn)置，即潛器運(yùn)動(dòng)的攻角與側(cè)滑角由潛器坐標(biāo)系與速度坐標(biāo)系之間的相對(duì)位置定義。速度軸ox'在潛器縱對(duì)稱面oxy內(nèi)的投影與潛器縱軸ox之間的夾角，稱為攻角，記為α；速度軸ox'與潛器縱對(duì)稱面oxy平面的夾角稱為側(cè)滑角，記為β。水下潛器漂浮于水面任一水線或者潛伏于水下保持平衡的充分必要條件為：(1)水下潛器受到的重力等于所受到的浮力；(2)重力與浮力的作用線在同一條鉛垂線上。水下潛器漂浮于水面或者潛伏于在水中的姿態(tài)稱為水下潛器的浮態(tài)，對(duì)于每一種浮態(tài)，上述平衡條件均可表述為平衡方程。本發(fā)明基于平滑方程建立水下潛器運(yùn)動(dòng)方程模型。用φ、θ分別表示潛器水下狀態(tài)的橫傾和縱傾角，橫傾右上為正，縱傾艇艏向上為正。水下潛器處于無(wú)橫傾、無(wú)縱傾(φ＝0,θ＝0)的潛伏狀態(tài)稱為正浮狀態(tài)，其平衡方程為：式中δ↓——水下狀態(tài)的水下潛器排水量，簡(jiǎn)稱水下排水量；——水下狀態(tài)的水密艇體排體積；xg↓,yg↓——水下狀態(tài)時(shí)的水下潛器重心的g的橫縱坐標(biāo)；xb↓,yb↓——水下狀態(tài)時(shí)的水下潛器浮心的b的橫縱坐標(biāo)；↓——表示水下狀態(tài)。水下潛器處于無(wú)橫傾(φ＝0)、有縱傾(θ≠0)的潛浮狀態(tài)稱為縱傾狀態(tài)，其平衡方程為：式中θ為水下潛器水下狀態(tài)的縱傾角，首傾為正。水下潛器處于有橫傾(φ≠0)、無(wú)縱傾(θ＝0)的潛伏狀態(tài)稱為橫傾狀態(tài)，其平衡方程為：潛器處于任意狀態(tài)時(shí)，其平衡方程為：由水下潛器處于水下狀態(tài)時(shí)滿足的平衡方程可以看出，水下狀態(tài)的潛器不具有自行調(diào)整平衡的能力，因此，潛器在水下狀態(tài)時(shí)受到的重力和浮力有微小的變化就會(huì)導(dǎo)致潛器平衡狀態(tài)的破壞，并會(huì)引起潛器的下潛或者上浮。根據(jù)動(dòng)量定理和動(dòng)量矩定理，基于潛器的運(yùn)動(dòng)參數(shù)與受力之間的關(guān)系，得到潛器的水下空間運(yùn)動(dòng)方程組為：式中，為潛器浮心處的線加速度在三個(gè)坐標(biāo)軸上的分量；為潛器角加速度在三個(gè)坐標(biāo)軸上的分量；vox,voy,voz為潛器浮心處的線速度在三個(gè)坐標(biāo)軸上的分量；ωx,ωy,ωz為潛器角速度在三個(gè)坐標(biāo)軸上的分量；amλ為慣性矩陣，即其中：m為潛器質(zhì)量；λij,i,j＝1,2,...,6，為附加質(zhì)量；xc,yc,zc，為潛器質(zhì)心位置；ji,j,i,j＝x,y,z，為潛器轉(zhuǎn)動(dòng)慣量、慣性矩；amλ矩陣中的各個(gè)參數(shù)可根據(jù)潛器參數(shù)得到，為已知常數(shù)量；avω為速度矩陣，即afm為力矩陣，即其中：xb,yb,zb為浮力在潛器坐標(biāo)系下的三個(gè)坐標(biāo)軸分量；xg,yg,zg為重力在潛器坐標(biāo)系下的三個(gè)坐標(biāo)軸分量；mgx,mgy,mgz為重力矩在潛器坐標(biāo)系下的三個(gè)坐標(biāo)軸分量；xαμ,yαμ,zαμ為粘性位置力在潛器坐標(biāo)系下的三個(gè)坐標(biāo)軸分量；mαμx,mαμy,mαμz為粘性位置力矩在潛器坐標(biāo)系下的三個(gè)坐標(biāo)軸分量；yωμ,zωμ為粘性阻尼力在潛器坐標(biāo)系下的y和z坐標(biāo)軸分量；mωμx,mωμy,mωμz為粘性阻尼力矩在潛器坐標(biāo)系下的三個(gè)坐標(biāo)軸分量；tx,ty,tz為推力矢量在潛器坐標(biāo)系下的三個(gè)坐標(biāo)軸分量；mty,mtz為推力矢量產(chǎn)生的力矩在潛器坐標(biāo)系下的y和z坐標(biāo)軸分量。下面分別給出各個(gè)力的表達(dá)式。潛器浮力大小為b＝ρvg(6)式中，v為潛器的排水體積，ρ為水密度。浮力作用方向沿地面坐標(biāo)系的oy0軸正向，其在潛器坐標(biāo)系內(nèi)的三個(gè)坐標(biāo)軸分量為潛器重力的大小為g＝mg(8)m為潛器質(zhì)量。水下潛器在執(zhí)行發(fā)射水下航行體等特定任務(wù)時(shí)，一般作無(wú)縱傾的等速直線定深的垂直面運(yùn)動(dòng)，此時(shí)速度vx＝v0＝常數(shù)，沖角α、縱傾角θ、潛伏角ψ、z向速度vz等皆為0，即α＝θ＝ψ＝vz＝0。當(dāng)水下潛器執(zhí)行發(fā)射任務(wù)時(shí)，由于被發(fā)射水下航行體的質(zhì)量較大，對(duì)于備用的水下航行體一般設(shè)有專門的目標(biāo)補(bǔ)重水艙來(lái)進(jìn)行替換。當(dāng)水下航行體發(fā)射以后，直接在發(fā)射管中注水補(bǔ)重進(jìn)行替換。注水后還存有少量的質(zhì)量差值和縱傾力矩，可以采用浮力調(diào)整水艙和首尾縱傾調(diào)整水艙進(jìn)行均衡。通過(guò)調(diào)節(jié)浮力調(diào)整水艙的注排水量和補(bǔ)償靜載差來(lái)對(duì)水下潛器姿態(tài)進(jìn)行調(diào)整，此時(shí)重力g＝mg替換為：式中m為水下潛器在空氣中的質(zhì)量；δp為發(fā)射水下航行體時(shí)產(chǎn)生的靜載差；δwadjust為注排水量；vadjust為注排水速度；tp為發(fā)射水下航行體的時(shí)間點(diǎn)；水下潛器通過(guò)調(diào)整水艙的注排水量來(lái)平衡靜載差，注排水量δwadjust滿足時(shí)間t的線性關(guān)系：δwadjust＝-δp(t-tp)/vadjust,(tp≤t≤δp/vadjust+tp)(10)重力作用方向沿地面坐標(biāo)系的oy0軸負(fù)向，其在潛器坐標(biāo)系下的三個(gè)分量為重力矩在潛器坐標(biāo)系下的三個(gè)分量為在需要對(duì)航行姿態(tài)進(jìn)行估計(jì)的有限的時(shí)間及空間范圍內(nèi)，粘性阻尼力和粘性位置力可看做一個(gè)常數(shù)，其數(shù)值可根據(jù)水下潛器的相關(guān)參數(shù)進(jìn)行靈活設(shè)定。在垂直發(fā)射水下航行體時(shí)，對(duì)作為發(fā)射平臺(tái)的水下潛器的平衡造成沖擊的主要因素為推力，推力包括兩個(gè)方面，一方面是水下航行體出筒后點(diǎn)火發(fā)射時(shí)對(duì)水下潛器產(chǎn)生的發(fā)射反力，另一方面是水下潛器離筒時(shí)海水倒灌產(chǎn)生的振幅不斷震蕩的水錘壓力，這兩個(gè)方面的力均會(huì)使水下潛器產(chǎn)生下沉運(yùn)動(dòng)；從作用時(shí)間看來(lái)前者會(huì)產(chǎn)生巨大的瞬時(shí)作用力，不會(huì)使水下潛器產(chǎn)生后期的位移，而后者是負(fù)浮力的疊加，將對(duì)水下潛器的位移與姿態(tài)產(chǎn)生長(zhǎng)期的影響。當(dāng)所發(fā)射的水下航行體出筒后，點(diǎn)火發(fā)射的瞬間會(huì)對(duì)水下潛器發(fā)射位置處的甲板產(chǎn)生強(qiáng)大的發(fā)射反沖力，方向沿航行體尾部豎直向下。該力作用時(shí)間很短，并隨著遠(yuǎn)離筒口而逐漸減小。水下潛器垂直發(fā)射水下航行體時(shí)，筒內(nèi)的高溫高壓氣體隨著水下航行體的完全出水，逐漸從筒內(nèi)溢出，推動(dòng)筒口周圍的海水向周圍運(yùn)動(dòng)，由于其運(yùn)動(dòng)慣性，筒內(nèi)的壓力會(huì)逐漸減小，直到低于所在海域的流體靜壓，此時(shí)海水倒灌，加上流體的重力作用，海水像“水錘”一樣砸向筒底，從而形成水錘壓力。在發(fā)射水下航行體時(shí)刻及后續(xù)一定時(shí)間內(nèi)，發(fā)射反沖力可通過(guò)水下潛器的壓力傳感器測(cè)量得到。壓力傳感器每隔一定的時(shí)間間隔，采集發(fā)射反沖力數(shù)據(jù)。水錘壓力的計(jì)算公式為：p＝ρ·c·v(13)其中ρ為海水密度，v為發(fā)射筒內(nèi)涌入海水到達(dá)發(fā)射筒底的水流速度，c為水錘壓縮波傳播速度，該壓力值通過(guò)在垂直發(fā)射過(guò)程中采用流體動(dòng)力學(xué)方法(cfd)進(jìn)行計(jì)算。在發(fā)射水下航行體前，水下潛器的初始位置可通過(guò)定位裝置測(cè)量得到，因此在發(fā)射前其運(yùn)動(dòng)參數(shù)是已知的。發(fā)射水下潛器后，在各個(gè)離散時(shí)刻，基于壓力傳感器采集的數(shù)據(jù)得到發(fā)射反沖力，根據(jù)式(13)計(jì)算水錘壓力，并將水下潛器的運(yùn)動(dòng)方程組進(jìn)行離散化，采用迭代的方式對(duì)每個(gè)時(shí)刻的各個(gè)運(yùn)動(dòng)參數(shù)進(jìn)行近似求解，從而得到各個(gè)運(yùn)動(dòng)參數(shù)的估計(jì)值。為使發(fā)射水下航行體后的水下潛器處于動(dòng)力平衡狀態(tài)，需要操縱升降舵對(duì)其姿態(tài)進(jìn)行控制，控制方式采用pid方式來(lái)對(duì)其深度與縱傾進(jìn)行控制。在每個(gè)離散時(shí)刻，根據(jù)水下空間運(yùn)動(dòng)方程組估計(jì)得到當(dāng)前水下潛器運(yùn)動(dòng)參數(shù)，并根據(jù)運(yùn)動(dòng)參數(shù)計(jì)算其姿態(tài)角和傾角變化率，利用水下潛器上的深度計(jì)測(cè)量該時(shí)刻其所處的深度，將計(jì)算得到的姿態(tài)角和傾角變化率、測(cè)量得到的深度與設(shè)定的目標(biāo)姿態(tài)角、傾角變化率和深度進(jìn)行比較，利用pid算法計(jì)算達(dá)到設(shè)定的姿態(tài)角和深度所需要的指令控制舵角，具體公式是：式中：δh，δv為指令控制舵角，hd為設(shè)定的潛器位置，ψd，θd為設(shè)定的潛器姿態(tài)角，h為測(cè)量到的潛器深度，ψ,θ為計(jì)算得到的潛器姿態(tài)角，ωz和ωy為傾角變化率，kh，kvy，kθ，kψ，為pid控制參數(shù)。指令控制舵角的飽和舵角為30°。潛器姿態(tài)角與運(yùn)動(dòng)參數(shù)之間的表達(dá)式為：傾角變化率ωz和ωy通過(guò)計(jì)算傾角θ和ψ的時(shí)間變化率得到。根據(jù)pid算法計(jì)算得出的指令控制舵角，操縱升降舵對(duì)水下潛器的姿態(tài)進(jìn)行控制。綜上所示，本發(fā)明的具體步驟為：首先建立表示水下潛器運(yùn)動(dòng)參數(shù)與受力關(guān)系的水下空間運(yùn)動(dòng)方程組，水下潛器所受力包括浮力、粘性位置力、粘性阻尼力、重力和推力，其中浮力、粘性位置力和粘性阻尼力為常數(shù)，重力通過(guò)浮力調(diào)整水艙的注排水量來(lái)計(jì)算得到，推力包括發(fā)射反沖力和水錘壓力，發(fā)射反沖力通過(guò)水下潛器的壓力傳感器測(cè)量得到，水錘壓力根據(jù)公式p＝ρ·c·v，采用流體動(dòng)力學(xué)方法進(jìn)行計(jì)算，其中p為水錘壓力，ρ為海水密度，v為發(fā)射筒內(nèi)涌入海水到達(dá)發(fā)射筒底的水流速度，c為水錘壓縮波傳播速度；利用水下空間運(yùn)動(dòng)方程組對(duì)水下潛器的運(yùn)動(dòng)參數(shù)進(jìn)行估計(jì)：將水下潛器的運(yùn)動(dòng)方程組進(jìn)行離散化，對(duì)微分方程進(jìn)行近似迭代求解，得到各個(gè)運(yùn)動(dòng)參數(shù)的估計(jì)值；在每個(gè)離散時(shí)刻，根據(jù)當(dāng)前時(shí)刻水下潛器運(yùn)動(dòng)參數(shù)估計(jì)值，計(jì)算其潛器姿態(tài)角和傾角變化率，利用水下潛器上的深度計(jì)測(cè)量該時(shí)刻其所處的深度h，將計(jì)算得到的姿態(tài)角、傾角變化率和測(cè)量得到的深度與設(shè)定的姿態(tài)角、傾角變化率和深度進(jìn)行比較，利用pid算法計(jì)算達(dá)到設(shè)定姿態(tài)角和深度所需要的指令控制舵角，具體公式是：式中：δh，δv為指令控制舵角，hd為設(shè)定的潛器位置，ψd，θd為設(shè)定的潛器姿態(tài)角，h為測(cè)量到的潛器深度，ψ,θ為計(jì)算得到的潛器姿態(tài)角，ωz和ωy為傾角變化率，kh，kvy，kθ，kψ，為pid控制參數(shù)。根據(jù)pid算法計(jì)算得出的指令控制舵角，操縱升降舵對(duì)水下潛器的姿態(tài)進(jìn)行控制。在某參數(shù)設(shè)置情況下，其pid控制器參數(shù)如表1所示。表1pid控制器參數(shù)控制器深度控制器縱傾控制器航向控制器調(diào)節(jié)系數(shù)0.30.60.5一階導(dǎo)數(shù)調(diào)節(jié)系數(shù)0.20.50.2附圖說(shuō)明圖1是地面坐標(biāo)系、速度坐標(biāo)系及水下潛器坐標(biāo)系示意圖；圖2是俯仰角曲線估計(jì)結(jié)果；圖3是縱傾角曲線估計(jì)結(jié)果。具體實(shí)施方式假設(shè)2000s時(shí)水下航行體發(fā)射，重浮力調(diào)節(jié)水艙開始排水，高壓燃?xì)忾_始涌出，隨后海水倒灌，第2001s水下航行體施加動(dòng)力載荷，發(fā)射反力作用于水下潛器，在水錘壓力模塊和發(fā)射反力作用下對(duì)水下潛器姿態(tài)進(jìn)行了估計(jì)。其中，以俯仰角的估計(jì)結(jié)果如附圖2所示，縱傾角的估計(jì)結(jié)果如附圖3所示。從圖中可以看出在水下潛器發(fā)射單發(fā)水下航行體瞬間，受發(fā)射反力作用發(fā)生上下?lián)u擺，隨即重浮力調(diào)節(jié)水艙以及深度控制系統(tǒng)開始工作，在控制系統(tǒng)的調(diào)節(jié)下，水下潛器在發(fā)射單發(fā)水下航行體大約1000s時(shí)間后，恢復(fù)到初始深度，進(jìn)入正常航行狀態(tài)。附圖3所示為水下航行體在不同速度情況下，水下潛器縱傾角的變化情況。vsub為水下航行體速度。由圖中可以看出，在水下航行體出筒后，水下潛器在水錘壓力作用下開始下潛，并在點(diǎn)火發(fā)射后，較大的發(fā)射反沖力使得水下潛器下傾速度進(jìn)一步加快，隨后水錘壓力逐漸衰減，水下潛器受回復(fù)力矩以及控制舵角作用，艏傾角開始變小，并在慣性作用下開始產(chǎn)生艉傾，并在長(zhǎng)時(shí)間內(nèi)以逐漸衰減的振幅來(lái)回震蕩，在發(fā)射后200s左右全部穩(wěn)定在一定范圍內(nèi)。與深度變化曲線一致，水下潛器的航速越大，控制效果越好，傾角震蕩周期越短，恢復(fù)初始狀態(tài)所需時(shí)間越短。當(dāng)前第1頁(yè)12