基于單信標(biāo)的水下移動(dòng)目標(biāo)定位算法
【專利摘要】基于單信標(biāo)的水下移動(dòng)目標(biāo)定位算法，其特征在于：采用一個(gè)水上移動(dòng)信標(biāo)來(lái)進(jìn)行水下移動(dòng)目標(biāo)的定位；所述移動(dòng)信標(biāo)配備有衛(wèi)星定位接收器，水下移動(dòng)目標(biāo)配備有水平姿態(tài)傳感器、航速傳感器和垂直方位傳感器。通過此技術(shù)可以獲得水下目標(biāo)與信標(biāo)的直線距離，同時(shí)根據(jù)水下目標(biāo)(自身攜帶的傳感器獲得水下目標(biāo)的一些姿態(tài)、速度、深度測(cè)量值)。在距離與這些測(cè)量值的基礎(chǔ)上通過建立系統(tǒng)模型、求解初始狀態(tài)、狀態(tài)可觀性分析、狀態(tài)方程離散化、卡爾曼濾波等步驟最終獲得水下目標(biāo)的位置估計(jì)值。
【專利說明】
基于單信標(biāo)的水下移動(dòng)目標(biāo)定位算法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于定位導(dǎo)航技術(shù)領(lǐng)域，設(shè)及一種水下移動(dòng)目標(biāo)的定位技術(shù)，具體的說，設(shè) 及一種基于單信標(biāo)的水下移動(dòng)目標(biāo)定位算法。
【背景技術(shù)】
[0002] 水下定位導(dǎo)航技術(shù)是海洋科學(xué)考察、海洋資源勘探、海洋資源開發(fā)、深?？臻g站建 設(shè)等工程的必備手段，而水下電磁波信號(hào)衰減快，陸地上的GI^信號(hào)無(wú)法直接應(yīng)用于水下目 標(biāo)定位，所W水聲定位技術(shù)是目前水下定位系統(tǒng)的主要研究方向。
[0003] 傳統(tǒng)上，依據(jù)水下定位信標(biāo)之間的距離，我們將水下定位系統(tǒng)分3種類型：長(zhǎng)基線 定位系統(tǒng)化化）、短基線定位系統(tǒng)（S化）W及超短基線定位系統(tǒng)(USBL)。傳統(tǒng)方法放置的基 元多，操作復(fù)雜，成本高昂。多個(gè)航行器可W接收導(dǎo)航信息速率隨水中航行器數(shù)目增加而減 少，而且導(dǎo)航區(qū)域限制在一定的范圍內(nèi)，因此傳統(tǒng)方法實(shí)現(xiàn)有界誤差的導(dǎo)航缺乏可擴(kuò)展性。 目前最先進(jìn)、準(zhǔn)確的慣性定位系統(tǒng)（1]1日的1日1化￥1肖日1:;[0]18731日1]1,^5)也有大約每小時(shí)幾 公里的偏移率。在INS中增加一個(gè)多普勒聲響(Doppler-Velocity Sonar，DVS)用于測(cè)量航 行器相對(duì)于地球的速度，可W將該誤差減少到航行距離的0.01%。但是只有潛艇和大型無(wú) 人水下航行器(UUVs)配備INS和DVS技術(shù)，較小的航行器受空間和能源限制無(wú)法裝備。大部 分小型航行器使用的是最基本的導(dǎo)航推算技術(shù)。運(yùn)種技術(shù)需要航行器定期到水面更新位置 信息，運(yùn)樣就加大了航行器的工作負(fù)擔(dān)。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的在于針對(duì)小型水下移動(dòng)目標(biāo)，提供一種導(dǎo)航效果好、定位準(zhǔn)確的基 于單信標(biāo)的水下移動(dòng)目標(biāo)的定位算法。
[0005] 本發(fā)明的技術(shù)方案為:基于單信標(biāo)的水下移動(dòng)目標(biāo)定位算法，采用一個(gè)水上移動(dòng) 信標(biāo)來(lái)進(jìn)行水下移動(dòng)目標(biāo)的定位;移動(dòng)信標(biāo)配備有衛(wèi)星定位接收器，水下移動(dòng)目標(biāo)配備有 水平姿態(tài)傳感器、航速傳感器和垂直方位傳感器，包括W下步驟：
[0006] A、建立水下坐標(biāo)系，建立水下移動(dòng)目標(biāo)的運(yùn)動(dòng)數(shù)學(xué)模型：
[0007]
其中，X，y，z為水下移動(dòng)目標(biāo)的位置，V為水下移動(dòng)目標(biāo)的 運(yùn)動(dòng)速度，Ve為洋流速度;Θ為水下移動(dòng)目標(biāo)的航向角，0為水下移動(dòng)目標(biāo)的俯仰角(水下移 動(dòng)目標(biāo)航行方向與水平方向的夾角，可通過水平姿態(tài)傳感器得到）；
[000引B、獲得水下移動(dòng)目標(biāo)的初始位置；
[0009] C、建立水下移動(dòng)目標(biāo)的狀態(tài)模型，并定義觀測(cè)模型：
[0010] 假定洋流的流速V。為恒定值，為一個(gè)常值;航向角Θ和航速V測(cè)量已經(jīng)過濾波誤差 為零;距離r測(cè)量噪聲為高斯白噪聲；
[0011] x、y、z分別對(duì)應(yīng)Ξ維坐標(biāo)系下X軸、y軸和z軸的狀態(tài)分量；
[0016] 定義移動(dòng)信標(biāo)的坐標(biāo)為(xb，yb，0)，得到水下移動(dòng)目標(biāo)與移動(dòng)信標(biāo)之間的距離測(cè)量 值：
Z為水下移動(dòng)目標(biāo)所處的水深；
[0017] 定義觀測(cè)模型為
[0018] 得到狀態(tài)方程
[0019] D、對(duì)系統(tǒng)模型進(jìn)行可觀性分析：
[0020] 令U = V,將B轉(zhuǎn)換為:./|'=[sin6^cos?/?cos9cos口slru/?0 0 0]^
[0021 ]用
代替;r，h2 = z;
[0022] 將航向角作為測(cè)量值，h3 = 0;
[0023] hi的零階李導(dǎo)數(shù)為：
[0024]
[0025] 其中zc為Z方向洋流對(duì)航行器作用的位移。
[0026] hi的一階李導(dǎo)數(shù)為：
[0037]取出0中前Ξ行構(gòu)成矩陣化，如果化滿秩說明系統(tǒng)中所有的位置量可觀。
[003引對(duì)化進(jìn)行行變換和列變換，得到：
[0039]
[0040] 得到化不滿秩的幾種情況
[0041 ]根據(jù)可觀性分析，得到如下結(jié)論：
[0042] (1)一般情況下，系統(tǒng)可觀；
[0043] (2)航行器在過巧自平面上運(yùn)動(dòng)時(shí)，系統(tǒng)不可觀；
[0044] (3)當(dāng)
時(shí)，即航行器的航向過航行器與信標(biāo)之間連線時(shí)，系統(tǒng)不可觀；
[0045] E、對(duì)系統(tǒng)模型進(jìn)行離散化處理：
[0046] ①狀態(tài)模型離散化：
[0047] 系統(tǒng)方程的狀態(tài)轉(zhuǎn)移矩陣夫；：
[004引其泰勒展開為：
[0049]
[0050] 由于A的二階W上項(xiàng)都為0,〇(*，切)可^改寫為：
[0化1 ]
[0052] X(t)可W表示為W下的形式：
[0化3]
[0054]定義離散化的周期為1，假定U前后的采樣值相差不大，可得到離散化狀態(tài)模型： [0化5] Xk = AXk-i+BUk-i+wk-i
[0化6] 離散化后，可得到新的A，B矩陣，其中A=巫（t) |t=T=i [0057]即：
[0化引
[0化9]②觀察模型離散化：
[0060] 系
將其線性化，得到：
[0061]
[0062] 為間隔離散化變?yōu)椋?br>[0063]
[0064] 測(cè)量值有距離r和深度Z，由觀測(cè)向量
，得到化矩陣為
[0069] 得到系統(tǒng)的離散化觀測(cè)模型為Zk =化Xk+vk;
[0070] F、對(duì)系統(tǒng)進(jìn)行卡爾曼濾波處理：
[0071] ①先驗(yàn)估計(jì)：
[0072]
[0073] ②更新協(xié)方差P:
[0074] 巧-='作I / + 0，其中Q為wk的協(xié)方差矩陣；
[0075] ③計(jì)算先驗(yàn)估計(jì)對(duì)應(yīng)的距離：
[0076]
[0077] ④計(jì)算Η矩陣；
[007引
[0079]⑤計(jì)算卡爾曼增益，其中R是vk的協(xié)方差矩陣：
[0085] Xk即為估計(jì)的水下移動(dòng)目標(biāo)的移動(dòng)軌跡。
[0086] 優(yōu)選的是:獲得水下移動(dòng)目標(biāo)初始位置的方法為：
[0087] 假定信標(biāo)為固定，且W信標(biāo)所在位置為原點(diǎn)，建立坐標(biāo)系；
[008引（1)假定水下移動(dòng)目標(biāo)的初始位置:記為Po,此時(shí)水下移動(dòng)目標(biāo)與y軸件的夾角記 為巧:，水下移動(dòng)目標(biāo)與信標(biāo)之間的距離為ri，時(shí)刻記為ti;
[0089] (2)忽略洋流流速，根據(jù)水下移動(dòng)目標(biāo)的運(yùn)動(dòng)速度，獲得t2時(shí)刻水下移動(dòng)目標(biāo)的航 位推算向量記為曰，從而獲得t2時(shí)刻水下移動(dòng)目標(biāo)的理想位置，記為Pm;
[0090] (3)考慮洋流流速，獲得t2時(shí)刻水下移動(dòng)目標(biāo)的真實(shí)位置，記為Pi，此時(shí)水下移動(dòng)目 標(biāo)與y軸間的夾角記為錠，水下移動(dòng)目標(biāo)與信標(biāo)之間的距離為η，時(shí)刻記為t2;
[0091 ] (4)比較Pi與Pm之間的位置，獲得洋流向量b;
[0092] (5)從Pi點(diǎn)開始，根據(jù)水下移動(dòng)目標(biāo)的運(yùn)動(dòng)速度，獲得t3時(shí)刻水下移動(dòng)目標(biāo)的航位 推算向量記為C，加上(4)得到的洋流向量b，獲得t3時(shí)刻水下移動(dòng)目標(biāo)的理想位置，記為P3， 此時(shí)水下移動(dòng)目標(biāo)與信標(biāo)之間的距離為Γ4;
[0093] (6)由信標(biāo)與航行器之間的測(cè)距信息，獲得t3時(shí)刻水下移動(dòng)目標(biāo)與信標(biāo)之間的距 離η，其中P2為根據(jù)測(cè)距信息任意選的一個(gè)位置；
[0094] (7)計(jì)算η與Γ4之間的差，記為。
[00巧](8)將閩和巧分別由0~360°遍歷，得到一系列的r值，在所有的r值中，找到最小的 r值對(duì)應(yīng)的位置Po和洋流向量b，則此時(shí)Po為最接近水下移動(dòng)目標(biāo)初始位置的初始狀態(tài)。
[0096] 優(yōu)選的是:假定信標(biāo)為固定，且W信標(biāo)所在位置為原點(diǎn)，將水下移動(dòng)目標(biāo)所在的位 置置于第一象限，建立坐標(biāo)系，將約和巧2分別由0~90°遍歷。
[0097] 本發(fā)明的有益效果為：
[0098] 本發(fā)明的算法是建立在單信標(biāo)水聲定位的基礎(chǔ)上，該定位技術(shù)可應(yīng)用于體型較小 且成本不高的水下航行器，可W解決傳統(tǒng)定位方法中布設(shè)回收困難、成本高昂，W及航位推 算受到未知洋流影響導(dǎo)致誤差發(fā)散的缺點(diǎn)。
[0099] 在此定位技術(shù)的基礎(chǔ)上提出了一種水下移動(dòng)目標(biāo)的定位算法，通過此技術(shù)可W獲 得水下目標(biāo)與信標(biāo)的直線距離(在未考慮到一些干擾的基礎(chǔ)上），同時(shí)根據(jù)水下目標(biāo)(航行 器）自身攜帶的傳感器獲得水下目標(biāo)的一些姿態(tài)、速度、深度測(cè)量值。在距離與運(yùn)些測(cè)量值 的基礎(chǔ)上通過建立系統(tǒng)模型、求解初始狀態(tài)、狀態(tài)可觀性分析、狀態(tài)方程離散化、卡爾曼濾 波等步驟最終獲得水下目標(biāo)的位置估計(jì)值。
[0100] 其中信標(biāo)可移動(dòng)使得本方法的導(dǎo)航區(qū)域擴(kuò)大，Ξ維模型的建立也減少了深度值估 計(jì)的誤差。該算法經(jīng)過仿真驗(yàn)證，定位導(dǎo)航效果良好，能夠比較準(zhǔn)確的獲得水下目標(biāo)的位置 軌跡，完成導(dǎo)航。
[0101] 單信標(biāo)定位技術(shù)只需要單個(gè)信標(biāo)，用于標(biāo)定信標(biāo)支持的區(qū)域內(nèi)航行器與已知信標(biāo) 的相對(duì)距離，信標(biāo)所做的工作只是廣播水聲信號(hào)，同時(shí)將其真實(shí)的地理位置編碼到水聲信 號(hào)中。各航行器可W計(jì)算自身的地理位置，而不需要信標(biāo)系統(tǒng)進(jìn)行計(jì)算，其支持的航行器的 數(shù)量不受限制。該技術(shù)的基礎(chǔ)就是航位推算技術(shù)，通過測(cè)量的距離對(duì)位置誤差進(jìn)行糾正，彌 補(bǔ)了定期更新位置信息的缺點(diǎn)，提高了航行器的續(xù)航能力。單個(gè)信標(biāo)布設(shè)和回收遠(yuǎn)遠(yuǎn)比多 個(gè)信標(biāo)方便，而且不存在多信標(biāo)相對(duì)位置的限制。
【附圖說明】
[0102] 圖1為本系統(tǒng)在坐標(biāo)系下模型圖。
[0103] 圖2為水下移動(dòng)目標(biāo)初始位置遍歷原理圖。
[0104] 圖3為水下移動(dòng)目標(biāo)初始位置遍歷流程圖。 圖4為水下移動(dòng)目標(biāo)航向角和俯仰角示意圖。
【具體實(shí)施方式】
[0105] W下結(jié)合附圖對(duì)本發(fā)明的【具體實(shí)施方式】進(jìn)行進(jìn)一步的描述。
[0106] 基于單信標(biāo)的水下移動(dòng)目標(biāo)定位算法，采用一個(gè)水上移動(dòng)信標(biāo)來(lái)進(jìn)行水下移動(dòng)目 標(biāo)的定位;用于標(biāo)定信標(biāo)支持的區(qū)域內(nèi)航行器與信標(biāo)的相對(duì)距離。移動(dòng)信標(biāo)配備有衛(wèi)星定 位接收器，水聲換能器及同步時(shí)鐘系統(tǒng)，通過全球定位系統(tǒng)獲得信標(biāo)的位置信息，其位置信 息與時(shí)鐘信息編碼到水聲信號(hào)中，信標(biāo)的工作是廣播水聲信號(hào)。
[0107] 水下移動(dòng)目標(biāo)配備有水平姿態(tài)傳感器、航速傳感器和垂直方位傳感器，水平姿態(tài) 傳感器可W得到水下移動(dòng)目標(biāo)航行的俯仰角信息，垂直方位傳感器可W得到水下移動(dòng)目標(biāo) 的水深位置信息，航速傳感器可W獲得水下移動(dòng)目標(biāo)的航速信息。
[0108] 如圖1所示，P(x，y)對(duì)應(yīng)水下移動(dòng)目標(biāo)的位置，對(duì)水下移動(dòng)目標(biāo)定位的算法包括W 下步驟：
[0109] A、建立水下坐標(biāo)系，信標(biāo)為移動(dòng)目標(biāo)，建立水下移動(dòng)目標(biāo)的運(yùn)動(dòng)數(shù)學(xué)模型：
[0110]
其中，x，y，z為水下移動(dòng)目標(biāo)的位置，V為水下移動(dòng)目標(biāo)的 航速，Ve為洋流速度；Θ為水下移動(dòng)目標(biāo)的航向角，0為水下移動(dòng)目標(biāo)的俯仰角(水下移動(dòng)目 標(biāo)航行方向與水平方向的夾角，可通過水平姿態(tài)傳感器得到）；
[0111] B、獲得水下移動(dòng)目標(biāo)的初始位置。
[0112] 由水聲測(cè)距方法可W獲得信標(biāo)與水下航行器的初始距離值，但是只有距離難W確 定水下航行器的初始位置，因此需要采用遍歷的方法來(lái)求得水下航行器的真實(shí)初始位置。 由于Z方向的位置信息可W通過測(cè)量得到，因此可將Ξ維的遍歷簡(jiǎn)化成二維的遍歷W減少 計(jì)算量。遍歷的基本原理是:假定一個(gè)具體的角度，在此角度下根據(jù)距離獲得航行器的初始 位置，利用航位推算與洋流信息推算出下一時(shí)刻航行器在此角度下的位置信息，由此位置 信息求得與信標(biāo)的距離，與真實(shí)時(shí)刻的距離信息作比較，在所有角度中尋找一個(gè)最接近的 值，那么運(yùn)個(gè)角度就是所估計(jì)的角度，便可W得到航行器的初始位置信息。具體方法為：
[0113] 如圖2和圖3所示，假定信標(biāo)為固定，且W信標(biāo)所在位置為原點(diǎn)，建立坐標(biāo)系；
[0114] (1)假定水下移動(dòng)目標(biāo)的初始位置:記為Po,此時(shí)水下移動(dòng)目標(biāo)與y軸件的夾角記 為約，水下移動(dòng)目標(biāo)與信標(biāo)之間的距離為ri，時(shí)刻記為ti;
[0115] (2)忽略洋流流速，根據(jù)水下移動(dòng)目標(biāo)的運(yùn)動(dòng)速度，獲得t2時(shí)刻水下移動(dòng)目標(biāo)的航 位推算向量記為曰，從而獲得t2時(shí)刻水下移動(dòng)目標(biāo)的理想位置，記為Pm;
[0116] (3)考慮洋流流速，獲得t2時(shí)刻水下移動(dòng)目標(biāo)的真實(shí)位置，記為Pi，此時(shí)水下移動(dòng)目 標(biāo)與y軸間的夾角記為巧，水下移動(dòng)目標(biāo)與信標(biāo)之間的距離為?，時(shí)刻記為t2;
[0117] (4)比較Pi與Pm之間的位置，獲得洋流向量b;
[011引（5)從Pi點(diǎn)開始，根據(jù)水下移動(dòng)目標(biāo)的運(yùn)動(dòng)速度，獲得t3時(shí)刻水下移動(dòng)目標(biāo)的航位 推算向量記為C，加上(4)得到的洋流向量b，獲得t3時(shí)刻水下移動(dòng)目標(biāo)的理想位置，記為P3， 此時(shí)水下移動(dòng)目標(biāo)與信標(biāo)之間的距離為Γ4;
[0119] (6)由信標(biāo)與航行器之間的測(cè)距信息，獲得t3時(shí)刻水下移動(dòng)目標(biāo)與信標(biāo)之間的距 離η，其中P2為根據(jù)測(cè)距信息任意選的一個(gè)位置；
[0120] (7)計(jì)算η與Γ4之間的差，記為r;
[0121] (8)將灼和媽分別由0~360°遍歷，得到一系列的r值，r值的個(gè)數(shù)與所取的遍歷分 度有關(guān)。在所有的r值中，找到最小的r值對(duì)應(yīng)的位置Po和洋流向量b，則此時(shí)Po為最接近水下 移動(dòng)目柄;初始位置的初始狀態(tài)。
[0122] 在0~360°遍歷的過程中，產(chǎn)生的所有的點(diǎn)集中在W坐標(biāo)系原點(diǎn)為中屯、，Wri為半 徑的圓周上，遍歷過程中會(huì)產(chǎn)生兩個(gè)最小值，受誤差和精度的影響，查找到的最小值可能會(huì) 落在真實(shí)位置的對(duì)稱點(diǎn)上，因此，為了節(jié)省計(jì)算量，假定信標(biāo)為固定，且W信標(biāo)所在位置為 原點(diǎn)，將水下移動(dòng)目標(biāo)所在的位置置于第一象限，建立坐標(biāo)系，將約和錢分別由0~90°遍 歷。
[0123] C、建立水下移動(dòng)目標(biāo)的狀態(tài)模型，并定義觀測(cè)模型：
[0124] 假定洋流的流速V。為恒定值，為一個(gè)常值;航向角Θ和航速V測(cè)量已經(jīng)過濾波誤差 為零;距離r測(cè)量噪聲為高斯白噪聲;在上述假設(shè)基礎(chǔ)下，建立狀態(tài)模型，其中需要估計(jì)的量 有兩個(gè)，洋流的流速V。和航行器的當(dāng)前位置P。
[0125] x、y、z分別對(duì)應(yīng)Ξ維坐標(biāo)系下X軸、y軸和Z軸的狀態(tài)分量；
[0126] 其中，令狀態(tài)量
[0127] 乂 =先1： + 及化；
[012引對(duì)X求導(dǎo)數(shù)
由于V。為恒定值，?ζ,，戶。均為0;
[0129] 因此，
巧W，得到W下A和Β，
[0130] 定義移動(dòng)信標(biāo)的坐標(biāo)為(xb，yb，0)，得到水下移動(dòng)目標(biāo)與移動(dòng)信標(biāo)之間的距離測(cè)量 值：
[0131]
;z為水下移動(dòng)目標(biāo)所處的水深；
[0132] 定義觀測(cè)模型為
，
[0133] 得到狀態(tài)方程
[0134] D、對(duì)系統(tǒng)模型進(jìn)行可觀性分析:為了得到真實(shí)的航行軌跡，必須進(jìn)行卡爾曼濾波， 而進(jìn)行卡爾曼濾波的條件便是狀態(tài)可觀。由于上述系統(tǒng)為一個(gè)非線性系統(tǒng)，對(duì)于一個(gè)非線 性系統(tǒng)，進(jìn)行可觀性分析的方法便是李導(dǎo)數(shù)法。
[013引令U=V,將B轉(zhuǎn)換為：J^j=[sm如os婷 c;os''0cos0 細(xì)療 0 0 of
[0136] 月
杉代替:r，h2 = z;
[0137] 將航向角作為測(cè)量值，h3 = Θ;
[013引hi的零階李導(dǎo)數(shù)為：
[0139]
[0140] 其中zc為Z方向洋流對(duì)航行器作用的位移。
[0141] hi的一階李導(dǎo)數(shù)為：
[0150] 得到系統(tǒng)模型的觀測(cè)矩陣0:
[0151]
[0152] 取出0中前Ξ行構(gòu)成矩陣化，如果化滿秩說明系統(tǒng)中所有的位置量可觀。
[0153] 對(duì)化進(jìn)行行變換和列變換，得到：
[0154]
[0155] 得到化不滿秩的幾種情筑
[0156] 在運(yùn)幾種情況下航行器的位置不可觀。
[0157] Ε、對(duì)系統(tǒng)模型進(jìn)行離散化處理:為了進(jìn)行卡爾曼濾波，獲得相關(guān)參數(shù)，需要對(duì)狀態(tài) 方程中的狀態(tài)模型和觀測(cè)模型就行離散化處理。
[0158] ①狀態(tài)模型離散化：
[0159] 系統(tǒng)方程的狀態(tài)轉(zhuǎn)移矩陣為曲;
[0160] 其泰勒展開為：
[0161]
[0162] 由于A的二階W上項(xiàng)都為0,〇(*，切)可^改寫為：
[0163]
[0164] X(t)可W表示為W下的形式：
[01 化]
[0166] 定義離散化的周期為1，假定U前后的采樣值相差不大，可得到離散化狀態(tài)模型：
[0167] Xk = AXk-i+BUk-i+wk-i
[016引離散化時(shí)，可得到新的A，B矩陣，其中A=巫（t) |t=T=i
[0169] 良p:
[0170]
[0171] ②觀察模型離散化：
[0172]對(duì)
將其線性化，得到：
[0176]巧慢值有距離r和深度Z，由觀測(cè)向量f，得至帖矩陣為 Z
[0181] 得到系統(tǒng)的離散化觀測(cè)模型為Zk =化Xk+vk;
[0182] F、對(duì)系統(tǒng)進(jìn)行卡爾曼濾波處理:在對(duì)系統(tǒng)進(jìn)行可觀性分析W及離散化處理后，便 可W進(jìn)行卡爾曼濾波，使得狀態(tài)向量X中的各項(xiàng)值逐漸逼近真實(shí)值。具體的卡爾曼濾波步驟 如下：
[0183] ①先驗(yàn)估計(jì)：
[0184] 玄杞+公巧-
[01化]②更新協(xié)方差P:
[0186] 帶二進(jìn)1-^:' +谷，其中Q為wk的協(xié)方差矩陣；
[0187] ③計(jì)算先驗(yàn)估計(jì)對(duì)應(yīng)的距離：
[0191] ⑤計(jì)算卡爾曼增益，其中R是Vk的協(xié)方差矩陣：
[0192]
[0193] ⑥后驗(yàn)估計(jì)：
[0194]
[01巧]⑦更新協(xié)方差：
[0196]
[0197] Xk即為估計(jì)的水下移動(dòng)目標(biāo)的移動(dòng)軌跡(狀態(tài)量的離散化形式），Xk中包括所要估 計(jì)的航行器的位置量(x，y，z)和要估計(jì)的洋流分量速度(Vex，Vw，Vcz)。
【主權(quán)項(xiàng)】
1.基于單信標(biāo)的水下移動(dòng)目標(biāo)定位算法，其特征在于:采用一個(gè)水上移動(dòng)信標(biāo)來(lái)進(jìn)行 水下移動(dòng)目標(biāo)的定位;所述移動(dòng)信標(biāo)配備有衛(wèi)星定位接收器，水下移動(dòng)目標(biāo)配備有水平姿 態(tài)傳感器、航速傳感器和垂直方位傳感器，包括以下步驟： A、 建立水下坐標(biāo)系，建立水下移動(dòng)目標(biāo)的運(yùn)動(dòng)數(shù)學(xué)模型：，其中，X，y，Z為水下移動(dòng)目標(biāo)的位置，V為水下移動(dòng)目標(biāo)的運(yùn)動(dòng)速 度，Vc為洋流速度;Θ為水下移動(dòng)目標(biāo)的航向角，r為水下移動(dòng)目標(biāo)的俯仰角； B、 獲得水下移動(dòng)目標(biāo)的初始位置； C、 建立水下移動(dòng)目標(biāo)的狀態(tài)模型，并定義觀測(cè)模型： 假定洋流的流速V。為恒定值，為一個(gè)常值;航向角Θ和航速V測(cè)量已經(jīng)過濾波誤差為零； 距離r測(cè)量噪聲為高斯白噪聲； X、y、z分別對(duì)應(yīng)三維坐標(biāo)系下X軸、y軸和z軸的狀態(tài)分量；定義移動(dòng)信標(biāo)的坐標(biāo)為(Xb，yb，〇)，得到水下移動(dòng)目標(biāo)與移動(dòng)信標(biāo)之間的距離測(cè)量值：#為水下移動(dòng)目標(biāo)所處的水深；D、 對(duì)系統(tǒng)模型進(jìn)行可觀性分析： 令U = V，將B轉(zhuǎn)換為： 用來(lái)代替r，h2 = z，將航向角作為測(cè)量值，h3 = 9，求出hi，h2和h3 的各階李導(dǎo)數(shù)及梯度，得到系統(tǒng)模型的觀測(cè)矩陣〇，取出0中前三行構(gòu)成矩陣0:，如果⑴滿秩說明系統(tǒng)中所有的位置量可觀； E、 對(duì)系統(tǒng)模型進(jìn)行離散化處理： ①狀態(tài)模型離散化： 系統(tǒng)方程的狀態(tài)轉(zhuǎn)移矩陣為其泰勒展開為：由于A的二階以上項(xiàng)都為0,〇(1如)可以改寫為：x(t)可以表示為以下的形式：定義離散化的周期為1，假定U前后的采樣值相差不大，可得到離散化狀態(tài)模型： Xk=AXk-ι+BUk-ι+wk-1 離散化后，可得到新的A，B矩陣，其中②觀察模型離散化：進(jìn)行泰勒展開：將其線性化，得到：以1為間隔離散化變?yōu)椋簻y(cè)量值有距離r和深度z，由觀測(cè)向量，得到Hk矩陣為得到系統(tǒng)的離散化觀測(cè)模型為Zk = HkXk+vk; F.對(duì)系統(tǒng)進(jìn)行卡爾曼濾波處理： ① 先驗(yàn)估計(jì)： ② 更新協(xié)方差P:if =+ β，其中Q為概的協(xié)方差矩陣； ③ 計(jì)算先驗(yàn)估計(jì)對(duì)應(yīng)的距離：④ 計(jì)算Η矩陣：⑤ 計(jì)算卡爾曼增益，其中R是vk的協(xié)方差矩陣：⑥ 后驗(yàn)估計(jì)：⑦ 更新協(xié)方差：2.如權(quán)利要求1所述的基于單信標(biāo)的水下移動(dòng)目標(biāo)定位算法，其特征在于:所述獲得水 下移動(dòng)目標(biāo)初始位置的方法為： 假定信標(biāo)為固定，且以信標(biāo)所在位置為原點(diǎn)，建立坐標(biāo)系； (1) 假定水下移動(dòng)目標(biāo)的初始位置:記為P〇，此時(shí)水下移動(dòng)目標(biāo)與y軸件的夾角記為約， 水下移動(dòng)目標(biāo)與信標(biāo)之間的距離為η，時(shí)刻記為t; (2) 忽略洋流流速，根據(jù)水下移動(dòng)目標(biāo)的運(yùn)動(dòng)速度，獲得t2時(shí)刻水下移動(dòng)目標(biāo)的航位推 算向量記為a，從而獲得〖 2時(shí)刻水下移動(dòng)目標(biāo)的理想位置，記為Pm; (3) 考慮洋流流速，獲得^時(shí)刻水下移動(dòng)目標(biāo)的真實(shí)位置，記為Pi，此時(shí)水下移動(dòng)目標(biāo)與 y軸間的夾角記為約，水下移動(dòng)目標(biāo)與信標(biāo)之間的距離為r2,時(shí)刻記為t2; (4) 比較丹與?(《之間的位置，獲得洋流向量b; (5) WPi點(diǎn)開始，根據(jù)水下移動(dòng)目標(biāo)的運(yùn)動(dòng)速度，獲得t3時(shí)刻水下移動(dòng)目標(biāo)的航位推算 向量記為c，加上(4)得到的洋流向量b，獲得t 3時(shí)刻水下移動(dòng)目標(biāo)的理想位置，記為P3,此時(shí) 水下移動(dòng)目標(biāo)與信標(biāo)之間的距離為Γ4; (6) 由信標(biāo)與航行器之間的測(cè)距信息，獲得t3時(shí)刻水下移動(dòng)目標(biāo)與信標(biāo)之間的距離r3， 其中P 2為根據(jù)測(cè)距信息任意選的一個(gè)位置； (7) 計(jì)算Γ3與Γ4之間的差，記為r; (8) 將的和愁分別由0~360°遍歷，得到一系列的r值，在所有的r值中，找到最小的r值對(duì) 應(yīng)的位置P〇和洋流向量b，則此時(shí)Ρο為最接近水下移動(dòng)目標(biāo)初始位置的初始狀態(tài)。3.如權(quán)利要求2所述的基于單信標(biāo)的水下移動(dòng)目標(biāo)定位算法，其特征在于:假定信標(biāo)為 固定，且以信標(biāo)所在位置為原點(diǎn)，將水下移動(dòng)目標(biāo)所在的位置置于第一象限，建立坐標(biāo)系， 將約和_分別由0~90°遍歷。
【文檔編號(hào)】G01C21/12GK105823480SQ201610158157
【公開日】2016年8月3日
【申請(qǐng)日】2016年3月18日
【發(fā)明人】周琳, 趙慶勝, 鹿鑫, 遲書凱
【申請(qǐng)人】中國(guó)海洋大學(xué)