本發(fā)明涉及深水機(jī)器人技術(shù)領(lǐng)域，尤其涉及一種深水機(jī)器人多模型EKF組合導(dǎo)航裝置及方法。

背景技術(shù)：

深水機(jī)器人航行深度可達(dá)數(shù)千米。在水下航行過程中，導(dǎo)航誤差會(huì)隨著路程和時(shí)間的增加而逐漸增大。當(dāng)深水機(jī)器人航行一段距離后，需要對其位置進(jìn)行修正，以提高導(dǎo)航精度。

當(dāng)前對深水機(jī)器人進(jìn)行位置修正的方法主要是：通過聲學(xué)定位系統(tǒng)輸出的定位信息對深水機(jī)器人位置進(jìn)行直接修正。但這種方法存在一定的缺陷：

1)無法消除聲學(xué)傳播時(shí)延引起的定位誤差；

2)水下機(jī)器人標(biāo)定存在偏差，會(huì)導(dǎo)致導(dǎo)航偏差隨時(shí)間累積；

3)深水機(jī)器人航行軌跡不平滑；

4)無法有效地克服野值的干擾。

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

為了克服現(xiàn)有方法的不足，本發(fā)明要解決的技術(shù)問題是提供一種深水機(jī)器人多模型EKF組合導(dǎo)航裝置及方法，使得航行軌跡平滑且定位誤差小。

本發(fā)明為實(shí)現(xiàn)上述目的所采用的技術(shù)方案是：一種深水機(jī)器人多模型EKF組合導(dǎo)航裝置，包括：

測距儀，安裝于深水機(jī)器人殼體外部，連接深水機(jī)器人內(nèi)部的導(dǎo)航計(jì)算機(jī)，利用聲信號測量深水機(jī)器人到各個(gè)信標(biāo)的距離；

信標(biāo)，至少三個(gè)，布設(shè)于海底，坐標(biāo)為已知；

傳感器，包括航向傳感器、航速傳感器和深度傳感器，均安裝于深水機(jī)器人中并連接導(dǎo)航計(jì)算機(jī)，分別用于測量深水機(jī)器人的航向、航速和所處深度。

導(dǎo)航計(jì)算機(jī)，用于采集測距儀和傳感器的輸出信號，并采用多模型EKF組合導(dǎo)航方法實(shí)時(shí)計(jì)算深水機(jī)器人當(dāng)前時(shí)刻的位置。

所述測距儀按固定周期進(jìn)行測距，周期在3秒～60秒之間，量測數(shù)據(jù)內(nèi)容包括信標(biāo)的編號，信號雙程傳播時(shí)間，并將時(shí)間量測轉(zhuǎn)化為距離量測。

所述導(dǎo)航計(jì)算機(jī)按固定周期采集數(shù)據(jù)，周期在0.2秒～2秒之間。

當(dāng)導(dǎo)航計(jì)算機(jī)采集到測距儀的距離量測數(shù)據(jù)時(shí)，采用EKF算法估計(jì)深水機(jī)器人的位置；

判斷導(dǎo)航計(jì)算機(jī)采集到的距離量測數(shù)據(jù)是否同時(shí)滿足數(shù)量和質(zhì)量上的要求；如果導(dǎo)航計(jì)算機(jī)采集到的距離量測數(shù)據(jù)同時(shí)滿足數(shù)量和質(zhì)量上的要求，則通過空間位置幾何解算獲得深水機(jī)器人的初始位置，并設(shè)置深水機(jī)器人處于組合導(dǎo)航階段，并將此空間解算位置設(shè)置為深水機(jī)器人可靠的初始值；如果不滿足要求，則此時(shí)深水機(jī)器人的導(dǎo)航狀態(tài)處于組合導(dǎo)航初期階段，組合導(dǎo)航初期階段目的是通過空間位置解算確定深水機(jī)器人的初始位置；

在組合導(dǎo)航階段，判斷距離量測數(shù)據(jù)是否有效，如果距離量測數(shù)據(jù)無效，則此量測值為野值，舍棄該距離量測數(shù)據(jù)，返回通過航位推算方法估計(jì)深水機(jī)器人的位置步驟；如果距離量測數(shù)據(jù)有效，則首先確定深水機(jī)器人運(yùn)動(dòng)系統(tǒng)的模型集，然后根據(jù)距離量測數(shù)據(jù)采用多模型EKF算法對深水機(jī)器人的位置進(jìn)行在線修正。

所述深水機(jī)器人依據(jù)航向傳感器和航速傳感器的量測值，通過航位推算方法估計(jì)深水機(jī)器人的位置，具體為：

v_e＝v_ksinθ_k (3)

v_n＝v_kcosθ_k (4)

式中，(x₁₀,x₂₀)為深水機(jī)器人的初始點(diǎn)，Δt為導(dǎo)航計(jì)算機(jī)的采集周期，(x_1t,x_2t)為推算航位，θ_k為航向傳感器量測值，v_k為航速傳感器量測值，v_e為深水機(jī)器人向東速度，v_n為深水機(jī)器人向北速度。

所述當(dāng)導(dǎo)航計(jì)算機(jī)采集到測距儀的距離量測數(shù)據(jù)時(shí)，采用EKF算法估計(jì)深水機(jī)器人的位置，包括以下步驟：

步驟1：設(shè)置AUV的初始狀態(tài)為X_0/0和初始協(xié)方差矩陣為P_0/0，默認(rèn)為深水機(jī)器人剛剛開始執(zhí)行任務(wù)時(shí)候的經(jīng)緯度位置；

步驟2：時(shí)間更新：通過以下時(shí)間更新方程，更新深水機(jī)器人的狀態(tài)X_k+1/k和協(xié)方差矩陣P_k+1/k：

X_k+1/k＝A·X_k/k+B·u_k (5)

P_k+1/k＝A·P_k/k·A^T+B·Q_k·B^T (6)

其中，u_k是由航向傳感器量測值θ_k和航速傳感器量測值v_k計(jì)算而來，u_k＝[v_e v_n]^T，A為狀態(tài)轉(zhuǎn)移矩陣，A^T為矩陣A的轉(zhuǎn)置，B為控制輸入矩陣，B^T為B的轉(zhuǎn)置，X_k/k為量測更新之后深水機(jī)器人的狀態(tài)，P_k/k為量測更新之后深水機(jī)器人的狀態(tài)協(xié)方差矩陣，X_k+1/k為深水機(jī)器人的一步預(yù)測狀態(tài)，P_k+1/k為深水機(jī)器人的一步預(yù)測狀態(tài)協(xié)方差矩陣；

步驟3：量測更新：當(dāng)深水機(jī)器人獲得測距儀的量測數(shù)據(jù)時(shí)，則采用量測更新方程矯正深水機(jī)器人的狀態(tài)X_k+1/k和協(xié)方差矩陣P_k+1/k；

K_k+1＝P_k+1/k·H_k+1^T·(H_k+1·P_k+1/k·H_k+1^T+R_k+1)^-1 (7)

X_k+1/k+1＝X_k+1/k+K_k+1·(y-y_k+1) (8)

P_k+1/k+1＝(I-K_k+1·H_k+1)·P_k+1/k (9)

其中，P_k+1/k為深水機(jī)器人的一步預(yù)測狀態(tài)協(xié)方差矩陣，H_k+1為量測矩陣，R_k+1為量測協(xié)方差矩陣，K_k+1為卡爾曼增益矩陣，y為實(shí)際量測值，y_k+1為預(yù)測量測值，I為單位陣，X_k+1/k為深水機(jī)器人的一步預(yù)測狀態(tài)，X_k+1/k+1為k+1時(shí)刻狀態(tài)更新值，P_k+1/k+1為k+1時(shí)刻狀態(tài)協(xié)方差矩陣更新值。

所述判斷導(dǎo)航計(jì)算機(jī)采集到的距離量測數(shù)據(jù)是否同時(shí)滿足數(shù)量和質(zhì)量上的要求，具體為：

在下一個(gè)測距周期到來時(shí)，當(dāng)一個(gè)測距周期內(nèi)的測距數(shù)據(jù)數(shù)量大于或等于3時(shí)，對深水機(jī)器人水平面坐標(biāo)進(jìn)行解算，解算公式如下：

Ax＝v (10)

式中，

為第i號信標(biāo)的三維坐標(biāo)，(x₁,x₂,x₃)為深水機(jī)器人水平面坐標(biāo)和深度，為第i號信標(biāo)到坐標(biāo)系原點(diǎn)的水平間距，為第i號信標(biāo)到深水機(jī)器人的水平距離，其中R_i表示第i號信標(biāo)的單程傳播距離；

將深水機(jī)器人水平面坐標(biāo)與所述推算航位作差，然后求取均方根誤差，即：

如果e≤Bias，則量測數(shù)據(jù)質(zhì)量達(dá)到要求，并令空間位置解算標(biāo)志變量N＝N+1，N的初始值為0，直到N≥4時(shí)，則量測數(shù)據(jù)數(shù)量和質(zhì)量同時(shí)達(dá)到要求，則設(shè)置此時(shí)導(dǎo)航狀態(tài)為組合導(dǎo)航階段，并確定當(dāng)前深水機(jī)器人水平面的初始位置是(x₁,x₂)；如果e＞Bias并且N＜4，則設(shè)置N＝0，其中，Bias范圍為1米～100米，Bias是預(yù)設(shè)值。

所述在組合導(dǎo)航階段，判斷距離量測數(shù)據(jù)是否有效，具體為：

v(k+1)＝y(tǒng)-y_k+1/k (12)

v(k+1)^T·S_k^-1·v(k+1)≤γ (13)

其中，y是實(shí)際量測值，y_k+1/k是預(yù)測量測值，v(k+1)是新息，S_k是新息協(xié)方差矩陣，γ是閾值，γ的建議值為9.2。

判斷公式(13)是否成立，如果成立，則使用當(dāng)前測量值y；否則，此量測值為野值，無效，給予舍棄。

所述確定深水機(jī)器人運(yùn)動(dòng)系統(tǒng)的模型集，根據(jù)系統(tǒng)過程噪聲Q的范圍，確定合理的過程噪聲最小協(xié)方差矩陣Q_min和過程噪聲最大協(xié)方差矩陣Q_max，Q_min≤Q≤Q_max，取N組運(yùn)動(dòng)模型，模型集的確定方法是：

為每一個(gè)Q_i建立一個(gè)EKF濾波器，每一個(gè)濾波器獨(dú)立估計(jì)深水機(jī)器人的狀態(tài)，i表示模型索引，N是預(yù)設(shè)值，表示模型的個(gè)數(shù)，當(dāng)水下機(jī)器人的最大速度小于或等于3m/s時(shí)，N取值為10。

所述根據(jù)距離量測數(shù)據(jù)采用多模型EKF算法對深水機(jī)器人的位置進(jìn)行在線修正，包括以下步驟：

步驟1：對每一個(gè)模型對應(yīng)的濾波器進(jìn)行時(shí)間更新，得到X_k+1/k，

X_i,k+1/k＝A·X_i,k/k+B·u_k (15)

P_i,k+1/k＝A·P_i,k/k·A^T+B·Q_i,k·B^T (16)

y_i,k+1/k＝h(X_i,k+1/k) (17)

X_i,k+1/k+1＝X_i,k+1/k+K_i,k+1·(y-y_i,k+1/k) (19)

P_i,k+1/k+1＝(I-K_i,k+1·H_i,k+1)·P_i,k+1/k (20)

其中，A是狀態(tài)轉(zhuǎn)移矩陣，X_i,k/k是第i個(gè)模型對應(yīng)的狀態(tài)，B是速度控制輸入矩陣，u_k是速度矢量，X_i,k+1/k表示第i個(gè)模型對應(yīng)的預(yù)測狀態(tài)，P_i,k+1/k表示第i個(gè)模型對應(yīng)的預(yù)測狀態(tài)協(xié)方差矩陣，P_i,k/k表示量測更新之后第i個(gè)模型對應(yīng)的狀態(tài)協(xié)方差矩陣，Q_i,k表示第i個(gè)模型對應(yīng)的過程噪聲協(xié)方差矩陣，h(·)表示量測方程，y_i,k+1/k表示第i個(gè)模型對應(yīng)的預(yù)測量測值，K_i,k+1表示第i個(gè)模型對應(yīng)的卡爾曼增益，H_i,k+1表示第i個(gè)模型對應(yīng)的線性化之后量測矩陣，R_k+1是量測協(xié)方差矩陣，y是量測值，X_i,k+1/k+1表示第i個(gè)模型對應(yīng)的狀態(tài)更新值，P_i,k+1/k+1表示第i個(gè)模型對應(yīng)的狀態(tài)協(xié)方差矩陣更新值，I表示單位陣；

步驟2：當(dāng)檢測到距離量測信息時(shí)，計(jì)算深水機(jī)器人到信標(biāo)的距離；并且依據(jù)量測，計(jì)算每一個(gè)模型對應(yīng)的權(quán)重：

v_i(k+1)＝y(tǒng)-y_i,k+1/k,i＝1,...,N (21)

S_k+1＝H·P_k+1/k·H^T+R_k+1 (22)

其中，y是量測值，y_i,k+1/k表示第i個(gè)模型對應(yīng)的預(yù)測量測值，v_i(k+1)表示第i個(gè)模型對應(yīng)的新息，R_k+1是量測協(xié)方差矩陣，H表示線性化之后的量測矩陣，S_k+1表示新息協(xié)方差矩陣，P_k+1/k是預(yù)測狀態(tài)協(xié)方差矩陣，e(i)是權(quán)重，β(i)是歸一化權(quán)重；

步驟3：更新深水機(jī)器人的狀態(tài)和協(xié)方差矩陣：

X_k+1/k+1＝∑β(i)X_i,k+1/k+1 (25)

P_k+1/k+1＝∑β(i)P_i,k+1/k+1 (26)

其中，β(i)是歸一化權(quán)重，X_i,k+1/k+1是第i個(gè)模型量測對應(yīng)的狀態(tài)更新值，P_i,k+1/k+1是第i個(gè)模型量測對應(yīng)的狀態(tài)協(xié)方差矩陣更新值，X_k+1/k+1是我們所需要估計(jì)的狀態(tài)，P_k+1/k+1是我們所需要估計(jì)的狀態(tài)協(xié)方差矩陣；

則X_k+1/k+1便是所求的深水機(jī)器人此時(shí)此刻的狀態(tài)，P_k+1/k+1是對應(yīng)的協(xié)方差矩陣。

本發(fā)明具有以下優(yōu)點(diǎn)：

1.本發(fā)明穩(wěn)定可靠，修正結(jié)果準(zhǔn)確。本發(fā)明設(shè)計(jì)了確定初始位置算法和測距波門算法，保證了初始值和測量值的可靠、有效，位置估計(jì)能夠穩(wěn)定收斂，結(jié)果準(zhǔn)確。

2.本發(fā)明所需裝置簡單，繼承性好。該方法所需裝置僅僅是幾臺(tái)信標(biāo)，一臺(tái)測距儀和一臺(tái)計(jì)算機(jī)，無需其它輔助裝置，測距儀安裝簡便，修正算法程序移植性好，可方便移植到各個(gè)深水機(jī)器人。

3.本發(fā)明應(yīng)用范圍廣。本發(fā)明不但可以應(yīng)用于深水機(jī)器人，還可以用于其它海洋相關(guān)設(shè)備，可適用于全海深導(dǎo)航。

附圖說明

圖1是本發(fā)明的深水機(jī)器人導(dǎo)航傳感器配置圖；

圖2組合導(dǎo)航算法流程圖；

圖3測距儀獲得量測數(shù)據(jù)示意圖；

圖4多模型EKF算法流程圖。

具體實(shí)施方式

下面結(jié)合附圖及實(shí)施例對本發(fā)明做進(jìn)一步的詳細(xì)說明。

本發(fā)明由不少于3臺(tái)的信標(biāo)和深水機(jī)器人組成。深水機(jī)器人導(dǎo)航傳感器配置如圖1所示，由測距儀、航向傳感器、航速傳感器、深度傳感器組成。

測距儀，安裝于深水機(jī)器人殼體外部，連接深水機(jī)器人內(nèi)部的導(dǎo)航計(jì)算機(jī)，利用聲信號測量深水機(jī)器人到各個(gè)信標(biāo)的距離；

信標(biāo)，至少三個(gè)，布設(shè)于海底，坐標(biāo)為已知；

傳感器，包括航向傳感器、航速傳感器和深度傳感器，均安裝于深水機(jī)器人中并連接導(dǎo)航計(jì)算機(jī)，分別用于測量深水機(jī)器人的航向、航速和所處深度。

導(dǎo)航計(jì)算機(jī)，用于采集測距儀和傳感器的輸出信號，并采用多模型EKF組合導(dǎo)航方法實(shí)時(shí)計(jì)算深水機(jī)器人當(dāng)前時(shí)刻的位置。

所述測距儀按固定周期進(jìn)行測距，周期在3秒～60秒之間，量測數(shù)據(jù)內(nèi)容包括信標(biāo)的編號，信號雙程傳播時(shí)間，并將時(shí)間量測轉(zhuǎn)化為距離量測。

所述導(dǎo)航計(jì)算機(jī)按固定周期采集數(shù)據(jù)，周期在0.2秒～2秒之間。

多模型EKF組合導(dǎo)航算法如圖2所示，深水機(jī)器人下水后，導(dǎo)航計(jì)算機(jī)對測距儀、航向傳感器、航速傳感器和深度傳感器進(jìn)行數(shù)據(jù)采集；首先深水機(jī)器人依據(jù)航向傳感器和航速傳感器的量測值，通過航位推算方法估計(jì)深水機(jī)器人的初始位置(初始位置不準(zhǔn)確沒有關(guān)系，在隨后的濾波過程中，其導(dǎo)航精度不斷提高)。

然后開啟組合導(dǎo)航系統(tǒng)，深水機(jī)器人依據(jù)慣性導(dǎo)航系統(tǒng)和航位推算算法不斷對自身進(jìn)行定位；同時(shí)通過長基線系統(tǒng)周期性地以廣播方式向四周發(fā)射信號。

深水機(jī)器人依據(jù)航向傳感器和航速傳感器的量測值，通過航位推算方法估計(jì)深水機(jī)器人的位置；當(dāng)導(dǎo)航計(jì)算機(jī)采集到測距儀的距離量測數(shù)據(jù)時(shí)，采用EKF算法估計(jì)深水機(jī)器人的位置；

如果深水機(jī)器人沒有收到測距量測信號，則繼續(xù)依據(jù)慣性導(dǎo)航系統(tǒng)進(jìn)行航位推算；如果收到測距量測信號，則進(jìn)入組合導(dǎo)航初期階段。此外，在檢測到量測信號時(shí)，需要將對量測數(shù)據(jù)進(jìn)行修正，如圖3所示，因?yàn)橐话闱闆r下深水機(jī)器人在運(yùn)動(dòng)，則深水機(jī)器人在其發(fā)射信號與檢測到信號時(shí)，并不是在同一個(gè)位置，因此需要對其修正。修正方法如下：

假設(shè)深水機(jī)器人在t₀時(shí)刻發(fā)射信號，在t₁時(shí)刻聲信標(biāo)m檢測到信號，并給深水機(jī)器人回饋一個(gè)信號，深水機(jī)器人在t₂時(shí)刻檢測到源自信標(biāo)m的信號，則

判斷導(dǎo)航計(jì)算機(jī)采集到的距離量測數(shù)據(jù)是否同時(shí)滿足數(shù)量和質(zhì)量上的要求；如果導(dǎo)航計(jì)算機(jī)采集到的距離量測數(shù)據(jù)同時(shí)滿足數(shù)量和質(zhì)量上的要求，則通過空間位置幾何解算獲得深水機(jī)器人的初始位置，并設(shè)置深水機(jī)器人處于組合導(dǎo)航階段，并將此空間解算位置設(shè)置為深水機(jī)器人可靠的初始值；如果不滿足要求，則此時(shí)深水機(jī)器人的導(dǎo)航狀態(tài)處于組合導(dǎo)航初期階段，組合導(dǎo)航初期階段目的是通過空間位置解算確定深水機(jī)器人的初始位置；

在組合導(dǎo)航初期階段，深水機(jī)器人一方面進(jìn)行航位推算估計(jì)自身的位置，一方面則計(jì)算空間位置解算的個(gè)數(shù)與質(zhì)量。首先如果在一個(gè)測距采樣周期內(nèi)，有量測更新，則采用EKF更新深水機(jī)器人的位置。如果在一個(gè)測距采樣周期內(nèi)，有大于或者等于3個(gè)量測信號，則可以通過空間位置解算，求解深水機(jī)器人的位置。如果在連續(xù)4個(gè)測距采樣周期內(nèi)，均可以獲得深水機(jī)器人的空間解算位置，那么則設(shè)置深水機(jī)器人處于組合導(dǎo)航階段。

在組合導(dǎo)航階段，深水機(jī)器人通過航位推算預(yù)測自身的位置，獲得量測數(shù)據(jù)后，采用多模型EKF算法矯正自身的位置，提高導(dǎo)航精度；同時(shí)記錄空間位置解算點(diǎn)，用于離線分析。多模型EKF算法流程圖如圖4所示。

在組合導(dǎo)航階段，判斷距離量測數(shù)據(jù)是否有效，如果距離量測數(shù)據(jù)無效，則此量測值為野值，舍棄該距離量測數(shù)據(jù)，返回通過航位推算方法估計(jì)深水機(jī)器人的位置步驟；如果距離量測數(shù)據(jù)有效，則首先確定深水機(jī)器人運(yùn)動(dòng)系統(tǒng)的模型集，然后根據(jù)距離量測數(shù)據(jù)采用多模型EKF算法對深水機(jī)器人的位置進(jìn)行在線修正。

所述深水機(jī)器人依據(jù)航向傳感器和航速傳感器的量測值，通過航位推算方法估計(jì)深水機(jī)器人的位置，具體為：

v_e＝v_ksinθ_k (3)

v_n＝v_kcosθ_k (4)

式中，(x₁₀,x₂₀)為深水機(jī)器人的初始點(diǎn)，Δt為導(dǎo)航計(jì)算機(jī)的采集周期，(x_1t,x_2t)為推算航位，θ_k為航向傳感器量測值，v_k為航速傳感器量測值，v_e為深水機(jī)器人向東速度，v_n為深水機(jī)器人向北速度。

所述當(dāng)導(dǎo)航計(jì)算機(jī)采集到測距儀的距離量測數(shù)據(jù)時(shí)，采用EKF算法估計(jì)深水機(jī)器人的位置，包括以下步驟：

步驟1：設(shè)置AUV的初始狀態(tài)為X_0/0和初始協(xié)方差矩陣為P_0/0，默認(rèn)為深水機(jī)器人剛剛開始執(zhí)行任務(wù)時(shí)候的經(jīng)緯度位置；

步驟2：時(shí)間更新：通過以下時(shí)間更新方程，更新深水機(jī)器人的狀態(tài)X_k+1/k和協(xié)方差矩陣P_k+1/k：

X_k+1/k＝A·X_k/k+B·u_k (5)

P_k+1/k＝A·P_k/k·A^T+B·Q_k·B^T (6)

其中，u_k是由航向傳感器量測值θ_k和航速傳感器量測值v_k計(jì)算而來，u_k＝[v_e v_n]^T，A為狀態(tài)轉(zhuǎn)移矩陣，A^T為矩陣A的轉(zhuǎn)置，B為控制輸入矩陣，B^T為B的轉(zhuǎn)置，X_k/k為量測更新之后深水機(jī)器人的狀態(tài)，P_k/k為量測更新之后深水機(jī)器人的狀態(tài)協(xié)方差矩陣，X_k+1/k為深水機(jī)器人的一步預(yù)測狀態(tài)，P_k+1/k為深水機(jī)器人的一步預(yù)測狀態(tài)協(xié)方差矩陣；

步驟3：量測更新：當(dāng)深水機(jī)器人獲得測距儀的量測數(shù)據(jù)時(shí)，則采用量測更新方程矯正深水機(jī)器人的狀態(tài)X_k+1/k和協(xié)方差矩陣P_k+1/k；

X_k+1/k+1＝X_k+1/k+K_k+1·(y-y_k+1) (8)

P_k+1/k+1＝(I-K_k+1·H_k+1)·P_k+1/k (9)其中，P_k+1/k為深水機(jī)器人的一步預(yù)測狀態(tài)協(xié)方差矩陣，H_k+1為量測矩陣，R_k+1為量測協(xié)方差矩陣，K_k+1為卡爾曼增益矩陣，y為實(shí)際量測值，y_k+1為預(yù)測量測值，I為單位陣，X_k+1/k為深水機(jī)器人的一步預(yù)測狀態(tài)，X_k+1/k+1為k+1時(shí)刻狀態(tài)更新值，P_k+1/k+1為k+1時(shí)刻狀態(tài)協(xié)方差矩陣更新值。

所述判斷導(dǎo)航計(jì)算機(jī)采集到的距離量測數(shù)據(jù)是否同時(shí)滿足數(shù)量和質(zhì)量上的要求，具體為：

在下一個(gè)測距周期到來時(shí)，當(dāng)一個(gè)測距周期內(nèi)的測距數(shù)據(jù)數(shù)量大于或等于3時(shí)，對深水機(jī)器人水平面坐標(biāo)進(jìn)行解算，解算公式如下：

Ax＝v (10)

式中，

為第i號信標(biāo)的三維坐標(biāo)，(x₁,x₂,x₃)為深水機(jī)器人水平面坐標(biāo)和深度，為第i號信標(biāo)到坐標(biāo)系原點(diǎn)的水平間距，為第i號信標(biāo)到深水機(jī)器人的水平距離，其中R_i表示第i號信標(biāo)的單程傳播距離；

將深水機(jī)器人水平面坐標(biāo)與所述推算航位作差，然后求取均方根誤差，即：

如果e≤Bias，則量測數(shù)據(jù)質(zhì)量達(dá)到要求，并令空間位置解算標(biāo)志變量N＝N+1，N的初始值為0，直到N≥4時(shí)，則量測數(shù)據(jù)數(shù)量和質(zhì)量同時(shí)達(dá)到要求，則設(shè)置此時(shí)導(dǎo)航狀態(tài)為組合導(dǎo)航階段，并確定當(dāng)前深水機(jī)器人水平面的初始位置是(x₁,x₂)；如果e＞Bias并且N＜4，則設(shè)置N＝0，其中，Bias范圍為1米～100米，Bias是預(yù)設(shè)值。

所述在組合導(dǎo)航階段，判斷距離量測數(shù)據(jù)是否有效，具體為：

v(k+1)＝y(tǒng)-y_k+1/k (12)

v(k+1)^T·S_k^-1·v(k+1)≤γ (13)

其中，y是實(shí)際量測值，y_k+1/k是預(yù)測量測值，v(k+1)是新息，S_k是新息協(xié)方差矩陣，γ是閾值，γ的建議值為9.2。

判斷公式(13)是否成立，如果成立，則使用當(dāng)前測量值y；否則，此量測值為野值，無效，給予舍棄。

所述確定深水機(jī)器人運(yùn)動(dòng)系統(tǒng)的模型集，根據(jù)系統(tǒng)過程噪聲Q的范圍，確定合理的過程噪聲最小協(xié)方差矩陣Q_min和過程噪聲最大協(xié)方差矩陣Q_max，Q_min≤Q≤Q_max，取N組運(yùn)動(dòng)模型，模型集的確定方法是：

為每一個(gè)Q_i建立一個(gè)EKF濾波器，每一個(gè)濾波器獨(dú)立估計(jì)深水機(jī)器人的狀態(tài)，i表示模型索引，N是預(yù)設(shè)值，表示模型的個(gè)數(shù)，當(dāng)水下機(jī)器人的最大速度小于或等于3m/s時(shí)，N取值為10。

所述根據(jù)距離量測數(shù)據(jù)采用多模型EKF算法對深水機(jī)器人的位置進(jìn)行在線修正，包括以下步驟：

步驟1：對每一個(gè)模型對應(yīng)的濾波器進(jìn)行時(shí)間更新，得到X_k+1/k，

X_i,k+1/k＝A·X_i,k/k+B·u_k (15)

P_i,k+1/k＝A·P_i,k/k·A^T+B·Q_i,k·B^T (16)

y_i,k+1/k＝h(X_i,k+1/k) (17)

X_i,k+1/k+1＝X_i,k+1/k+K_i,k+1·(y-y_i,k+1/k) (19)

P_i,k+1/k+1＝(I-K_i,k+1·H_i,k+1)·P_i,k+1/k (20)

其中，A是狀態(tài)轉(zhuǎn)移矩陣，X_i,k/k是第i個(gè)模型對應(yīng)的狀態(tài)，B是速度控制輸入矩陣，u_k是速度矢量，X_i,k+1/k表示第i個(gè)模型對應(yīng)的預(yù)測狀態(tài)，P_i,k+1/k表示第i個(gè)模型對應(yīng)的預(yù)測狀態(tài)協(xié)方差矩陣，P_i,k/k表示量測更新之后第i個(gè)模型對應(yīng)的狀態(tài)協(xié)方差矩陣，Q_i,k表示第i個(gè)模型對應(yīng)的過程噪聲協(xié)方差矩陣，h(·)表示量測方程，y_i,k+1/k表示第i個(gè)模型對應(yīng)的預(yù)測量測值，K_i,k+1表示第i個(gè)模型對應(yīng)的卡爾曼增益，H_i,k+1表示第i個(gè)模型對應(yīng)的線性化之后量測矩陣，R_k+1是量測協(xié)方差矩陣，y是量測值，X_i,k+1/k+1表示第i個(gè)模型對應(yīng)的狀態(tài)更新值，P_i,k+1/k+1表示第i個(gè)模型對應(yīng)的狀態(tài)協(xié)方差矩陣更新值，I表示單位陣；

步驟2：當(dāng)檢測到距離量測信息時(shí)，計(jì)算深水機(jī)器人到信標(biāo)的距離；并且依據(jù)量測，計(jì)算每一個(gè)模型對應(yīng)的權(quán)重：

v_i(k+1)＝y(tǒng)-y_i,k+1/k,i＝1,...,N (21)

S_k+1＝H·P_k+1/k·H^T+R_k+1 (22)

其中，y是量測值，y_i,k+1/k表示第i個(gè)模型對應(yīng)的預(yù)測量測值，v_i(k+1)表示第i個(gè)模型對應(yīng)的新息，R_k+1是量測協(xié)方差矩陣，H表示線性化之后的量測矩陣，S_k+1表示新息協(xié)方差矩陣，P_k+1/k是預(yù)測狀態(tài)協(xié)方差矩陣，e(i)是權(quán)重，β(i)是歸一化權(quán)重；

步驟3：更新深水機(jī)器人的狀態(tài)和協(xié)方差矩陣：

X_k+1/k+1＝∑β(i)X_i,k+1/k+1 (25)

P_k+1/k+1＝∑β(i)P_i,k+1/k+1 (26)

其中，β(i)是歸一化權(quán)重，X_i,k+1/k+1是第i個(gè)模型量測對應(yīng)的狀態(tài)更新值，P_i,k+1/k+1是第i個(gè)模型量測對應(yīng)的狀態(tài)協(xié)方差矩陣更新值，X_k+1/k+1是我們所需要估計(jì)的狀態(tài)，P_k+1/k+1是我們所需要估計(jì)的狀態(tài)協(xié)方差矩陣；

則X_k+1/k+1便是所求的深水機(jī)器人此時(shí)此刻的狀態(tài)，P_k+1/k+1是對應(yīng)的協(xié)方差矩陣。

表1變量以及變量介紹