本發(fā)明屬于水聲定位技術(shù)領(lǐng)域，具體涉及一種基于單信標(biāo)測(cè)距定位雙精度加權(quán)融合的航向誤差補(bǔ)償方法。

背景技術(shù)：

目前對(duì)于水上目標(biāo)的定位通常采用衛(wèi)星技術(shù)為主的技術(shù)手段(gps系統(tǒng)、伽利略系統(tǒng)、北斗系統(tǒng)等)，輔以慣性及其它定位技術(shù)。當(dāng)目標(biāo)處于水下時(shí)，由于水介質(zhì)對(duì)無(wú)線電波的強(qiáng)烈吸收作用，限制了衛(wèi)星定位的應(yīng)用。此時(shí)，以聲波作為信息載體的水聲定位技術(shù)成為主要選擇，不但能完成對(duì)目標(biāo)的定位、導(dǎo)航，還能作為慣性定位導(dǎo)航技術(shù)的有效輔助校準(zhǔn)手段。

水聲定位技術(shù)最先應(yīng)用于軍事，后由于海洋開(kāi)發(fā)、勘探、資源開(kāi)采的需求逐步應(yīng)用于各類(lèi)商用、民用工程。它能夠提供海底勘查設(shè)備如rov(remotelyoperatedvehicle)和auv(autonomousunderwatervehicle)等重要的定位、導(dǎo)航和通信支撐。通過(guò)在水面工作船只、水下移動(dòng)平臺(tái)以及作業(yè)海區(qū)上加裝和布放聲學(xué)定位設(shè)備，可實(shí)現(xiàn)水面對(duì)水下目標(biāo)位置的實(shí)時(shí)監(jiān)控、水面與水下平臺(tái)的信息交互，是海洋科學(xué)考察、海洋資源勘探、海洋資源開(kāi)發(fā)、深海空間站建設(shè)等工程的必備手段。

基于單信標(biāo)測(cè)距的定位方法需要利用實(shí)際布放的單只信標(biāo)的地理位置和目標(biāo)運(yùn)動(dòng)速度。信標(biāo)的地理位置可以通過(guò)水面試驗(yàn)船預(yù)先標(biāo)定得到，該地理位置屬于大地坐標(biāo)系。目標(biāo)運(yùn)動(dòng)速度通過(guò)加速度計(jì)或者多普勒測(cè)速儀測(cè)量得到，為載體坐標(biāo)系下的觀測(cè)量。水下目標(biāo)的定位導(dǎo)航常采用大地坐標(biāo)系，所以需要通過(guò)坐標(biāo)旋轉(zhuǎn)矩陣將載體坐標(biāo)系下的運(yùn)動(dòng)速度轉(zhuǎn)換為大地坐標(biāo)系下的運(yùn)動(dòng)速度。坐標(biāo)旋轉(zhuǎn)矩陣由目標(biāo)姿態(tài)角組成，姿態(tài)角的誤差會(huì)傳遞給虛擬信標(biāo)的位置，進(jìn)而影響定位結(jié)果，而其中影響最大的便是目標(biāo)航向角。為了解決上述問(wèn)題，本發(fā)明提出了一種基于單信標(biāo)測(cè)距定位雙精度加權(quán)融合的航向誤差補(bǔ)償方法。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于提供一種克服目標(biāo)航向角誤差對(duì)坐標(biāo)旋轉(zhuǎn)矩陣的影響，提高單信標(biāo)測(cè)距定位系統(tǒng)對(duì)水下目標(biāo)的定位精度的基于單信標(biāo)測(cè)距定位雙精度加權(quán)融合的航向誤差補(bǔ)償方法。

本發(fā)明的目的通過(guò)如下技術(shù)方案來(lái)實(shí)現(xiàn)：

基于單信標(biāo)測(cè)距定位雙精度加權(quán)融合的航向誤差補(bǔ)償方法，包括以下步驟：

(1)在2個(gè)信標(biāo)的公共作用區(qū)域，分別獲得2個(gè)信標(biāo)對(duì)水下目標(biāo)的單信標(biāo)測(cè)距定位結(jié)果。

(1.1)當(dāng)水下載體位于xbn處時(shí)，利用第1～n-1個(gè)測(cè)距周期的測(cè)距信息和載體運(yùn)動(dòng)參數(shù)，基于真實(shí)信標(biāo)at構(gòu)建n個(gè)虛擬信標(biāo)；

(1.2)獲得信標(biāo)xt1和水下目標(biāo)的單信標(biāo)測(cè)距定位結(jié)果x1：

對(duì)于信標(biāo)xt1，虛擬信標(biāo)vt1i＝(xt1i,yt1i,zt1i)^t與真實(shí)信標(biāo)xt1的關(guān)系滿(mǎn)足：

利用虛擬信標(biāo)vt1的位置，第i個(gè)測(cè)距周期對(duì)應(yīng)的測(cè)距方程可以寫(xiě)成：

||xb1n-vt1i||＝r1i

采用高斯牛頓法，得到基于虛擬信標(biāo)vt1得到的單標(biāo)定位結(jié)果為x1。

(1.3)獲得xt2和水下目標(biāo)的單信標(biāo)測(cè)距定位結(jié)果x2：

.對(duì)于信標(biāo)xt2，虛擬信標(biāo)vt2i＝(xt2i,yt2i,zt2i)^t與真實(shí)信標(biāo)xt2的關(guān)系滿(mǎn)足：

利用虛擬信標(biāo)vt2的位置，第i個(gè)測(cè)距周期對(duì)應(yīng)的測(cè)距方程可以寫(xiě)成：

||xb1n-vt2i||＝r2i

采用高斯牛頓法，得到基于虛擬信標(biāo)vt2得到的單標(biāo)定位結(jié)果為x2。

(2)根據(jù)目標(biāo)姿態(tài)誤差對(duì)目標(biāo)運(yùn)動(dòng)速度的影響，計(jì)算目標(biāo)航向角誤差對(duì)單信標(biāo)測(cè)距定位結(jié)果的影響，確定水下目標(biāo)聲學(xué)雙精度加權(quán)融合時(shí)的權(quán)系數(shù)。

(2.1)雙信標(biāo)定位結(jié)果進(jìn)行雙精度加權(quán)融合后的定位結(jié)果可用下面的公式表示：

x12＝ω1x1+ω2x2

其中，x1為單信標(biāo)xt1的定位結(jié)果，x2為單信標(biāo)xt2的定位結(jié)果，x12為雙信標(biāo)定位結(jié)果加權(quán)融合后的定位結(jié)果，ω1和ω2為加權(quán)融合的權(quán)系數(shù)；

(2.2)得到均方誤差

根據(jù)最小均方誤差的理論，權(quán)系數(shù)ω1和ω2選取時(shí)，須使得目標(biāo)最終的定位結(jié)果x12和目標(biāo)真實(shí)位置xb的均方誤差最?。?/p>

其中，為雙信標(biāo)定位結(jié)果加權(quán)融合后定位結(jié)果的均方誤差；

由于兩個(gè)信標(biāo)對(duì)目標(biāo)的單標(biāo)定位結(jié)果是獨(dú)立的，并且權(quán)系數(shù)須滿(mǎn)足無(wú)偏性要求，進(jìn)步一得到均方誤差

(2.3)用拉格朗日乘子法求解時(shí)構(gòu)造的代價(jià)函數(shù)為：

(2.4)確定則加權(quán)融合的權(quán)系數(shù)ω1和ω2：

滿(mǎn)足下式時(shí)，雙信標(biāo)定位結(jié)果加權(quán)融合后的定位結(jié)果精度最高：

加權(quán)融合的權(quán)系數(shù)ω1和ω2可以描述為：

(3)根據(jù)2個(gè)信標(biāo)各自的單信標(biāo)測(cè)距定位結(jié)果，利用選定的權(quán)系數(shù)計(jì)算得到雙精度加權(quán)融合后的定位結(jié)果。

雙信標(biāo)定位結(jié)果進(jìn)行雙精度加權(quán)融合后的定位結(jié)果可用下面的公式表示：

x12＝ω1x1+ω2x2

特別地：

所述的在2個(gè)信標(biāo)的公共作用區(qū)域，分別獲得2個(gè)信標(biāo)對(duì)水下目標(biāo)的單信標(biāo)測(cè)距定位結(jié)果的方法為：當(dāng)目標(biāo)工作于2個(gè)信標(biāo)的公共作用區(qū)域時(shí)，根據(jù)基于虛擬測(cè)距信標(biāo)的單信標(biāo)測(cè)距定位方法，可以分別獲得兩個(gè)信標(biāo)xt1和xt2對(duì)水下目標(biāo)的單信標(biāo)測(cè)距定位結(jié)果x1和x2，這兩個(gè)定位結(jié)果是獨(dú)立的。

所述的根據(jù)目標(biāo)姿態(tài)誤差對(duì)目標(biāo)運(yùn)動(dòng)速度的影響，計(jì)算目標(biāo)航向角誤差對(duì)單信標(biāo)測(cè)距定位結(jié)果的影響，確定水下目標(biāo)聲學(xué)雙精度加權(quán)融合時(shí)的權(quán)系數(shù)的方法為：坐標(biāo)旋轉(zhuǎn)矩陣由目標(biāo)姿態(tài)角組成，姿態(tài)角的誤差會(huì)傳遞給虛擬信標(biāo)的位置，進(jìn)而影響定位結(jié)果，而其中影響最大的便是目標(biāo)航向角。存在航向誤差時(shí)，水平定位誤差呈對(duì)稱(chēng)分布，信標(biāo)處定位誤差最小，以信標(biāo)為中心向外輻射，距離信標(biāo)越遠(yuǎn)的區(qū)域定位誤差越大。

所述的根據(jù)2個(gè)信標(biāo)各自的單信標(biāo)測(cè)距定位結(jié)果，利用選定的權(quán)系數(shù)計(jì)算得到雙精度加權(quán)融合后的定位結(jié)果的方法為：對(duì)獨(dú)立的2個(gè)單信標(biāo)測(cè)距定位結(jié)果，結(jié)合目標(biāo)航向誤差引起的定位誤差空間分布特性確定的權(quán)系數(shù)，基于雙精度加權(quán)融合原理計(jì)算得到航向誤差補(bǔ)償后的定位結(jié)果。

本發(fā)明的有益效果在于：

在2個(gè)信標(biāo)的公共作用區(qū)域，根據(jù)基于虛擬測(cè)距信標(biāo)的單信標(biāo)測(cè)距定位方法，分別獲得兩個(gè)信標(biāo)對(duì)水下目標(biāo)的單信標(biāo)測(cè)距定位結(jié)果；根據(jù)航向角誤差對(duì)水平定位結(jié)果的影響，確定水下目標(biāo)聲學(xué)雙精度加權(quán)融合時(shí)的權(quán)系數(shù)；基于雙精度加權(quán)融合原理計(jì)算得到航向誤差補(bǔ)償后的定位結(jié)果。該發(fā)明解決了目標(biāo)航向誤差對(duì)坐標(biāo)旋轉(zhuǎn)矩陣和單信標(biāo)測(cè)距定位結(jié)果的影響，提高了單信標(biāo)測(cè)距定位系統(tǒng)的定位精度。

附圖說(shuō)明

圖1為信標(biāo)位于目標(biāo)航跡異側(cè)的定位結(jié)果；

圖2為信標(biāo)位于目標(biāo)航跡異側(cè)的定位誤差。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明的具體實(shí)施方式做進(jìn)一步說(shuō)明：

基于單信標(biāo)測(cè)距定位雙精度加權(quán)融合的航向誤差補(bǔ)償方法，其特征在于，包括以下步驟：

(1)在2個(gè)信標(biāo)的公共作用區(qū)域，分別獲得2個(gè)信標(biāo)對(duì)水下目標(biāo)的單信標(biāo)測(cè)距定位結(jié)果；

(2)根據(jù)目標(biāo)姿態(tài)誤差對(duì)目標(biāo)運(yùn)動(dòng)速度的影響，計(jì)算目標(biāo)航向角誤差對(duì)單信標(biāo)測(cè)距定位結(jié)果的影響，確定水下目標(biāo)聲學(xué)雙精度加權(quán)融合時(shí)的權(quán)系數(shù)；

(3)根據(jù)2個(gè)信標(biāo)各自的單信標(biāo)測(cè)距定位結(jié)果，利用選定的權(quán)系數(shù)計(jì)算得到雙精度加權(quán)融合后的定位結(jié)果。

步驟(1)的具體實(shí)現(xiàn)方法：

(1.1)當(dāng)水下載體位于xbn處時(shí)，利用第1～n-1個(gè)測(cè)距周期的測(cè)距信息和載體運(yùn)動(dòng)參數(shù)，基于真實(shí)信標(biāo)at構(gòu)建n個(gè)虛擬信標(biāo)；

(1.2)獲得信標(biāo)xt1和水下目標(biāo)的單信標(biāo)測(cè)距定位結(jié)果x1：

對(duì)于信標(biāo)xt1，虛擬信標(biāo)vt1i＝(xt1i,yt1i,zt1i)^t與真實(shí)信標(biāo)xt1的關(guān)系滿(mǎn)足：

利用虛擬信標(biāo)vt1的位置，第i個(gè)測(cè)距周期對(duì)應(yīng)的測(cè)距方程可以寫(xiě)成：

||xb1n-vt1i||＝r1i

采用高斯牛頓法，得到基于虛擬信標(biāo)vt1得到的單標(biāo)定位結(jié)果為x1。

(1.3)獲得xt2和水下目標(biāo)的單信標(biāo)測(cè)距定位結(jié)果x2：

.對(duì)于信標(biāo)xt2，虛擬信標(biāo)vt2i＝(xt2i,yt2i,zt2i)^t與真實(shí)信標(biāo)xt2的關(guān)系滿(mǎn)足：

利用虛擬信標(biāo)vt2的位置，第i個(gè)測(cè)距周期對(duì)應(yīng)的測(cè)距方程可以寫(xiě)成：

||xb1n-vt2i||＝r2i

采用高斯牛頓法，得到基于虛擬信標(biāo)vt2得到的單標(biāo)定位結(jié)果為x2。

步驟(2)的具體實(shí)現(xiàn)方法：

(2.1)那么雙信標(biāo)定位結(jié)果進(jìn)行雙精度加權(quán)融合后的定位結(jié)果可用下面的公式表示：

x12＝ω1x1+ω2x2(1)

其中，x1為單信標(biāo)xt1的定位結(jié)果，x2為單信標(biāo)xt2的定位結(jié)果，x12為雙信標(biāo)定位結(jié)果加權(quán)融合后的定位結(jié)果，ω1和ω2為加權(quán)融合的權(quán)系數(shù)。

(2.2)加權(quán)融合的權(quán)系數(shù)ω1和ω2對(duì)目標(biāo)最終的定位結(jié)果x12影響很大，根據(jù)最小均方誤差的理論，權(quán)系數(shù)ω1和ω2選取時(shí)，須使得目標(biāo)最終的定位結(jié)果x12和目標(biāo)真實(shí)位置xb的均方誤差最?。?/p>

其中，為雙信標(biāo)定位結(jié)果加權(quán)融合后定位結(jié)果的均方誤差。

由于兩個(gè)信標(biāo)對(duì)目標(biāo)的單標(biāo)定位結(jié)果是獨(dú)立的，并且權(quán)系數(shù)須滿(mǎn)足無(wú)偏性要求，進(jìn)步一得到均方誤差

(2.3)加權(quán)融合的權(quán)系數(shù)ω1和ω2選取，可以描述為一個(gè)雙信標(biāo)定位結(jié)果加權(quán)融合后的定位結(jié)果約束下的最優(yōu)化的問(wèn)題，用拉格朗日乘子法求解時(shí)構(gòu)造的代價(jià)函數(shù)為：

(2.4)則加權(quán)融合的權(quán)系數(shù)ω1和ω2滿(mǎn)足下式時(shí)，雙信標(biāo)定位結(jié)果加權(quán)融合后的定位結(jié)果精度最高。

根據(jù)目標(biāo)姿態(tài)誤差影響分析的結(jié)果可知，存在固定航向誤差時(shí)，水平定位誤差呈對(duì)稱(chēng)分布，信標(biāo)處定位誤差最小，以信標(biāo)為中心向外輻射，距離信標(biāo)越遠(yuǎn)的區(qū)域定位誤差越大。因而，加權(quán)融合的權(quán)系數(shù)ω1和ω2可以描述為：

步驟(3)的具體實(shí)現(xiàn)方法：

雙信標(biāo)定位結(jié)果進(jìn)行雙精度加權(quán)融合后的定位結(jié)果可用下面的公式表示：

x12＝ω1x1+ω2x2。

實(shí)施例：

對(duì)本發(fā)明提供的基于單信標(biāo)測(cè)距定位雙精度加權(quán)融合的航向誤差補(bǔ)償方法進(jìn)行仿真分析。

布放在海底的兩個(gè)聲信標(biāo)位置分別為(-1500,-1500,3600)m和(1500,1500,3600)m，信標(biāo)位于目標(biāo)航跡的異側(cè)。水下目標(biāo)在恒定深度3700m作前向速度1m/s的勻速直線運(yùn)動(dòng)，以航向90°航行3000m。測(cè)距周期為20s，虛擬信標(biāo)個(gè)數(shù)為15個(gè)。對(duì)距離測(cè)量添加0.15m為標(biāo)準(zhǔn)差的隨機(jī)誤差，對(duì)目標(biāo)前向速度添加3‰·v+0.002m/s為標(biāo)準(zhǔn)差的隨機(jī)誤差，對(duì)目標(biāo)右向速度添加0.002m/s為標(biāo)準(zhǔn)差的隨機(jī)誤差，對(duì)目標(biāo)航向角添加0.1°為標(biāo)準(zhǔn)差的隨機(jī)誤差，添加+1°的固定誤差。當(dāng)目標(biāo)初始位置為(-1500,0)m時(shí)，不同信標(biāo)對(duì)應(yīng)的單信標(biāo)測(cè)距定位結(jié)果和雙精度加權(quán)融合后的定位結(jié)果如圖1所示。從圖中可以看出，單信標(biāo)定位結(jié)果偏離真實(shí)航跡的程度都隨著與信標(biāo)距離的增大而增大，且真實(shí)航跡距信標(biāo)距離最小處兩端的單信標(biāo)定位結(jié)果偏離真實(shí)航跡的方向相反。加權(quán)融合后的定位結(jié)果明顯優(yōu)于任何一個(gè)信標(biāo)的單信標(biāo)測(cè)距定位結(jié)果。不同信標(biāo)對(duì)應(yīng)的單信標(biāo)測(cè)距定位誤差和雙精度加權(quán)融合后的定位誤差如圖2所示。從圖中可以看出，加權(quán)融合后的定位誤差明顯小于任何一個(gè)信標(biāo)的單信標(biāo)測(cè)距定位誤差，定位誤差隨著目標(biāo)至兩個(gè)信標(biāo)距離差的減小而減小，定位誤差從20m減小至0m。

以上所述僅為本發(fā)明的優(yōu)選實(shí)施例而已，并不用于限制本發(fā)明，對(duì)于本領(lǐng)域的技術(shù)人員來(lái)說(shuō)，本發(fā)明可以有各種更改和變化。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。