本發(fā)明屬于導(dǎo)航領(lǐng)域，特別是涉及一種基于5階ssrckf的sins/dvl組合導(dǎo)航濾波方法。

背景技術(shù)：

捷聯(lián)慣性導(dǎo)航系統(tǒng)能實(shí)時(shí)測(cè)量系統(tǒng)所處的位置、當(dāng)前速度和姿態(tài)等導(dǎo)航信息，并能在短時(shí)間內(nèi)保持高精度，具有多輸出信號(hào)、強(qiáng)隱蔽性、強(qiáng)自主式等優(yōu)點(diǎn)，其系統(tǒng)噪聲較低，但作為一種導(dǎo)航信息具有連續(xù)性的系統(tǒng)，系統(tǒng)誤差會(huì)隨時(shí)間積累，若不采取外界導(dǎo)航信息校正，其精度將會(huì)快速下降而導(dǎo)致無(wú)法使用。

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

發(fā)明目的：為了提高auv組合導(dǎo)航的精度及穩(wěn)定性，并且考慮到實(shí)時(shí)的計(jì)算量，本發(fā)明提出了一種基于5階ssrckf的sins/dvl組合導(dǎo)航濾波方法。

技術(shù)方案：一種基于5階ssrckf的sins/dvl組合導(dǎo)航濾波方法，包括如下步驟：

1)建立sins的非線性誤差模型：在sins中選取東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，以水下航行器自身建立載體坐標(biāo)系，并給定實(shí)際計(jì)算導(dǎo)航坐標(biāo)系，建立慣性器件陀螺儀和速度計(jì)的誤差模型，并求解系統(tǒng)的姿態(tài)誤差方程、位置誤差方程；

2)建立dvl的非線性誤差模型：針對(duì)dvl誤差中的刻度誤差以及隨機(jī)測(cè)量誤差，以一階馬爾科夫過(guò)程來(lái)擬合誤差方程；

3)根據(jù)步驟1和步驟2分別得到的sins非線性誤差模型和dvl非線性誤差模型建立組合導(dǎo)航濾波模型：選取sins的三維姿態(tài)角誤差、速度誤差的前兩維、位置誤差的前兩維，三維陀螺常值漂移和加速度計(jì)的水平方向的兩維常值誤差的12維狀態(tài)變量，選取dvl的刻度系數(shù)誤差和隨機(jī)常值測(cè)量誤差作為狀態(tài)量；然后根據(jù)步驟1和步驟2分別得到的sins非線性誤差模型和dvl非線性誤差模型共同建立組合導(dǎo)航的狀態(tài)方程；以dvl和sins的東向、北向速度分量的差值作為觀測(cè)量,建立sins/dvl組合導(dǎo)航系統(tǒng)的觀測(cè)方程；

4)建立5階ssrckf濾波模型，輸出導(dǎo)航系統(tǒng)參數(shù)：利用5階ssrckf對(duì)sins和dvl輸出的信息進(jìn)行濾波、融合，得到導(dǎo)航參數(shù)的最優(yōu)估計(jì)并修正速度、位置和姿態(tài)矩陣，得到精確的導(dǎo)航信息。

進(jìn)一步的，所述步驟1)中建立sins非線性誤差模型的具體步驟包括：

選取東-北-天坐標(biāo)系為sins的導(dǎo)航坐標(biāo)系并記為n系，載體系選用右-前-上記為b系，sins解算的導(dǎo)航坐標(biāo)系記為n′系；系統(tǒng)真實(shí)姿態(tài)角為真實(shí)速度真實(shí)地理坐標(biāo)pr＝[l,λ,h]^t；記陀螺和加速度計(jì)的隨機(jī)常值誤差分別為和陀螺和加速度計(jì)對(duì)應(yīng)的擾動(dòng)白噪聲分別為和則陀螺儀誤差加速度計(jì)誤差陀螺儀和加表的測(cè)量值為和和分別為地球自轉(zhuǎn)角速度和導(dǎo)航坐標(biāo)系相對(duì)地球坐標(biāo)系的旋轉(zhuǎn)角速度；vn和ve分別表示載體的運(yùn)動(dòng)速度的北向分量和東向分量；rn和re分別為卯酉圈半徑和子午圈半徑；

建立sins的速度、姿態(tài)和位置誤差方程，具體如下：

速度方程為：

姿態(tài)誤差方程為：

其中：

位置誤差方程為：

進(jìn)一步的，所述步驟2)中建立dvl誤差模型的具體步驟包括：

設(shè)定dvl刻度誤差為δkc，隨機(jī)測(cè)量誤差為δvdvl,dvl的實(shí)際測(cè)量值為dvl的理想速度為vdvl。

dvl的速度方程為：

定義βv，βk分別表示多普勒測(cè)速儀隨機(jī)測(cè)量誤差和刻度系數(shù)誤差的相關(guān)時(shí)間；wv，wk為其對(duì)應(yīng)的高斯白噪聲；則一階馬爾科夫過(guò)程擬合的誤差方程表示為：

進(jìn)一步的，所述步驟3)中建立組合導(dǎo)航濾波模型的具體步驟包括：

對(duì)于sins，忽略高度通道的狀態(tài)量，即選取以下12維狀態(tài)量：

對(duì)于dvl選取刻度系數(shù)誤差和隨機(jī)常值測(cè)量誤差為狀態(tài)量為：xdvl＝[δkcx,δkcy,δvdvlx,δvdvly]，共同構(gòu)成sins/dvl組合導(dǎo)航系統(tǒng)的非線性狀態(tài)方程，并將的非線性狀態(tài)方程簡(jiǎn)記為：

選取導(dǎo)航坐標(biāo)系下捷聯(lián)解算速度和dvl速度的前兩維的差值為sins/dvl組合導(dǎo)航系統(tǒng)的觀測(cè)量。

記sins解算的速度在n系下的投影為即速度真值vⁿ與速度誤差值的和，方程如下：

記dvl在載體系b下的直接輸出的速度為為實(shí)際測(cè)量值在載體系下的表示；sins解算的姿態(tài)變換矩陣為實(shí)際計(jì)算坐標(biāo)系偏離理想坐標(biāo)系的誤差矩陣記為將dvl的輸出速度轉(zhuǎn)換到導(dǎo)航坐標(biāo)系上為則根據(jù)dvl的速度方程得：

式中：vⁿ為導(dǎo)航坐標(biāo)系下速度的真實(shí)值，δkc為dvl的刻度系數(shù)誤差，δvdvl為dvl的隨機(jī)測(cè)量誤差，因此量測(cè)方程為：

將此量測(cè)方程簡(jiǎn)記為：

z＝h(x,t)+u(t)

式中u(t)為系統(tǒng)的觀測(cè)噪聲，并假設(shè)為零均值高斯白噪聲；

以采樣周期ts作為濾波周期，并以ts為步長(zhǎng)，對(duì)狀態(tài)方程和量測(cè)方程進(jìn)行離散化，得到系統(tǒng)的非線性濾波模型，并簡(jiǎn)記為：

式中：wk為隨機(jī)系統(tǒng)噪聲并且wk～n(0,qk)，qk為噪聲矩陣；uk為隨機(jī)觀測(cè)噪聲并且uk～n(0,rk)，rk為觀測(cè)噪聲矩陣。

進(jìn)一步的，所述步驟4)中建立5階ssrckf濾波模型的具體步驟包括：

5.1基于ssr容積采樣規(guī)則：高斯濾波積分歸納成

其中

采用基于ssr容積采樣規(guī)則，求解球面積分s(r)，對(duì)于擁有n維狀態(tài)量的系統(tǒng)，求解過(guò)程如下：取一組m維向量ai＝[ai1,ai2,ai3,…，ain]^t,i＝1,2,…，m+1，并將向量ai的中點(diǎn)投影到球面un得到序列則有

選用[ξi,ωi]表示濾波器的采樣容積點(diǎn)，其中則得到基于ssr規(guī)則下5階非線性多維積分為：

其中：

f(ξi)＝f(ξi)+f(-ξi)

f(ωi)＝f(ωi)+f(-ωi)；

5.2時(shí)間更新過(guò)程：估計(jì)方差pk-1/k-1通過(guò)喬列斯基(cholesky)分解得：pk-1/k-1＝sk-1/k-1s^tk-1/k-1；根據(jù)基于ssr容積規(guī)則選取容積點(diǎn)集合[ξi,ωi]，計(jì)算容積點(diǎn)：通過(guò)狀態(tài)方程來(lái)傳導(dǎo)容積點(diǎn)：x^*i,k/k-1＝f(xi,k-1/k-1)，因此k時(shí)刻的一步狀態(tài)預(yù)測(cè)和一步預(yù)測(cè)方差為：

5.3量測(cè)過(guò)程更新流程：通過(guò)量測(cè)方程傳導(dǎo)容積點(diǎn)：zi,k/k-1＝h(xi,k/k-1)，k時(shí)刻的量測(cè)量z的預(yù)測(cè)以及濾波增益矩陣為：

因此k時(shí)刻的狀態(tài)估計(jì)和均方差估計(jì)為：

有益效果：本發(fā)明采用非線性sins和dvl的組合導(dǎo)航系統(tǒng)。sins不需外部輸入信息、又不向外發(fā)射能量的自主式、隱蔽性強(qiáng)的導(dǎo)航系統(tǒng)，能夠提供位置、航向等導(dǎo)航信息，但其導(dǎo)航誤差隨時(shí)間累積。dvl能在波束打到水底的情況下，為載體提供高精度的對(duì)地速度，使用便捷。因此將sins和dvl兩種導(dǎo)航系統(tǒng)組合起來(lái)，并針對(duì)具有非線性特性的sins不同運(yùn)動(dòng)狀態(tài)設(shè)計(jì)不同的組合導(dǎo)航方法。當(dāng)sins系統(tǒng)誤差模型為非線性時(shí)，擴(kuò)展卡爾曼濾波(extendedkalmanfiltering，ekf)中采用泰勒展開(kāi)及一階截?cái)鄷?huì)導(dǎo)致很大的線性化誤差，從而造成ekf濾波精度低以及必須計(jì)算雅克比矩陣的不足，采用ckf，可克服提高濾波算法的精度、穩(wěn)定性。針對(duì)高階系統(tǒng)，提出了五階ssrckf在ckf的基礎(chǔ)上可以顯著的減少計(jì)算量，提高系統(tǒng)的實(shí)時(shí)性。

附圖說(shuō)明

圖1是本發(fā)明流程圖；

圖2是本發(fā)明中建立5階ssrckf的流程圖。

具體實(shí)施方式

下面將結(jié)合附圖，對(duì)本發(fā)明的實(shí)施案例進(jìn)行詳細(xì)的描述；

如圖1所示，本發(fā)明所述的基于5階ssrckf(sphericalsimplex-radialcubaturekalmanfilter,球面最簡(jiǎn)相徑容積卡爾曼濾波)的sins/dvl組合導(dǎo)航濾波方法，包含以下步驟：

步驟1：建立sins(strap-downinertialnavigationsystem，捷聯(lián)慣導(dǎo)系統(tǒng))非線性誤差模型：在sins中選取東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系(n)，以水下航行器(autonomousunderwatervehicle，auv)自身建立載體坐標(biāo)系(b)，并給定實(shí)際計(jì)算導(dǎo)航坐標(biāo)系(n′)，建立慣性器件陀螺儀和速度計(jì)的誤差模型，并求解系統(tǒng)的姿態(tài)誤差方程、位置誤差方程；

選取東-北-天坐標(biāo)系為導(dǎo)航坐標(biāo)系記為n系，載體系選用右-前-上記為b系，捷聯(lián)解算的導(dǎo)航坐標(biāo)系記為n′系；系統(tǒng)真實(shí)姿態(tài)角為真實(shí)速度真實(shí)地理坐標(biāo)pr＝[l,λ,h]^t；記陀螺和加速度計(jì)的隨機(jī)常值誤差分別為和陀螺和加速度計(jì)對(duì)應(yīng)的擾動(dòng)白噪聲分別為和則陀螺儀誤差加速度計(jì)誤差陀螺儀和加表的測(cè)量值為和和分別為地球自轉(zhuǎn)角速度和導(dǎo)航坐標(biāo)系相對(duì)地球坐標(biāo)系的旋轉(zhuǎn)角速度；vn和ve分別表示載體的運(yùn)動(dòng)速度的北向分量和東向分量；rn和re分別為卯酉圈半徑和子午圈半徑。建立sins系統(tǒng)的速度、姿態(tài)和位置誤差方程，具體如下：

速度方程為：

姿態(tài)誤差方程為：

其中：

位置誤差方程為：

步驟2：dvl的非線性誤差模型的建立：dvl(dopplervelocitylog，多普勒計(jì)程儀)根據(jù)多普勒效應(yīng)，利用超生換能器發(fā)射的超聲波來(lái)測(cè)量載體速度的儀器，多普勒計(jì)程儀測(cè)量的速度誤差不隨時(shí)間而累積，具有一定的實(shí)時(shí)性和自主性。當(dāng)載體相對(duì)水底的距離在多普勒計(jì)程儀有效量測(cè)范圍內(nèi)時(shí)，多普勒計(jì)程儀測(cè)得的為載體相對(duì)水底的速度，但當(dāng)多普勒計(jì)程儀工作在有效測(cè)量范圍外時(shí)，多普勒計(jì)程儀測(cè)得的為載體相對(duì)于水流的速度。多普勒測(cè)速儀利用多普勒效應(yīng)，由安裝在載體上的發(fā)射器發(fā)射并返回的超聲波頻移來(lái)計(jì)算載體運(yùn)動(dòng)速度。其誤差主要包括刻度誤差以及隨機(jī)測(cè)量誤差，以一階馬爾科夫過(guò)程來(lái)擬合誤差方程；具體如下：

假設(shè)dvl的刻度誤差為δkc，隨機(jī)測(cè)量誤差為δvdvl，dvl實(shí)際測(cè)量值為dvl的刻度系數(shù)誤差為δkc；dvl的理想速度為vdvl；隨機(jī)測(cè)量誤差為δvdvl。

因此其速度方程可以表示為：

定義βv，βk分別表示dvl隨機(jī)測(cè)量誤差和刻度系數(shù)誤差的相關(guān)時(shí)間；wv，wk為其對(duì)應(yīng)的高斯白噪聲。則一階馬爾科夫過(guò)程擬合的誤差方程表示為：

步驟3：組合導(dǎo)航濾波模型的建立：根據(jù)步驟1和步驟2分別得到的sins非線性誤差模型和dvl非線性誤差模型建立組合導(dǎo)航濾波模型，針對(duì)sins系統(tǒng)，選取三維姿態(tài)角誤差、速度誤差的前兩維、位置誤差的前兩維，三維陀螺常值漂移和加速度計(jì)的水平方向的兩維常值誤差構(gòu)成12維狀態(tài)變量；對(duì)于dvl系統(tǒng)選取刻度系數(shù)誤差和隨機(jī)常值測(cè)量誤差作為狀態(tài)量，建立組合導(dǎo)航的狀態(tài)方程。以dvl東向、北向的速度作為觀測(cè)量建立系統(tǒng)的觀測(cè)方程；具體如下：

對(duì)于sins系統(tǒng)，忽略高度通道的狀態(tài)量，即選取以下12維狀態(tài)量：

對(duì)于dvl選取刻度系數(shù)誤差和隨機(jī)常值測(cè)量誤差為狀態(tài)量為：xdvl＝[δkcx,δkcy,δvdvlx,δvdvly]，共同構(gòu)成sins/dvl組合導(dǎo)航系統(tǒng)的非線性狀態(tài)方程，并將的非線性狀態(tài)方程簡(jiǎn)記為：

選取導(dǎo)航坐標(biāo)系下捷聯(lián)解算速度和多普勒測(cè)速儀速度的前兩維的差值為sins/dvl組合導(dǎo)航系統(tǒng)的觀測(cè)量。

記sins解算的速度在n系下的投影為即速度真值vⁿ與速度誤差值的和，方程如下：

記dvl在載體系b下的直接輸出的速度為為實(shí)際測(cè)量值在載體系下的表示，導(dǎo)航解算的姿態(tài)變換矩陣為實(shí)際計(jì)算坐標(biāo)系偏離理想坐標(biāo)系的誤差矩陣記為將dvl的輸出速度轉(zhuǎn)換到導(dǎo)航坐標(biāo)系上為則根據(jù)dvl的速度方程可得：

式中：vⁿ為導(dǎo)航坐標(biāo)系下速度的真實(shí)值，δkc為dvl的刻度系數(shù)誤差，δvdvl為dvl的隨機(jī)測(cè)量誤差，因此量測(cè)方程為：

將此量測(cè)方程簡(jiǎn)記為：

z＝h(x,t)+u(t)

式中u(t)為系統(tǒng)的觀測(cè)噪聲，并假設(shè)為零均值高斯白噪聲。

以采樣周期ts作為濾波周期，并以ts為步長(zhǎng)，對(duì)系統(tǒng)的狀態(tài)方程和量測(cè)方程進(jìn)行離散化，得到系統(tǒng)的非線性濾波模型，并簡(jiǎn)記為：

式中：wk為隨機(jī)系統(tǒng)噪聲并且wk～n(0,qk)，qk為系統(tǒng)的噪聲矩陣；uk為系統(tǒng)的隨機(jī)觀測(cè)噪聲并且uk～n(0,rk)，rk為系統(tǒng)的觀測(cè)噪聲矩陣。

步驟4：5階球ssrckf濾波模型的建立，輸出導(dǎo)航系統(tǒng)參數(shù)：ssr(sphericalsimplex-radial，基于球面最短相徑)采樣規(guī)則的ckf(cubaturekalmanfilter，容積卡爾曼濾波)相比于同階標(biāo)準(zhǔn)容積卡爾曼濾波采樣點(diǎn)數(shù)目顯著降低，濾波效率更高，精度也高于標(biāo)準(zhǔn)ckf。利用5階ssrckf對(duì)sins和dvl輸出的信息進(jìn)行濾波、融合，得到導(dǎo)航參數(shù)的最優(yōu)估計(jì)并修正速度、位置和姿態(tài)矩陣，得到精確的導(dǎo)航信息。其步驟包括時(shí)間更新過(guò)程及量測(cè)過(guò)程更新過(guò)程。如圖2所示，具體如下：

1)ssr容積采樣規(guī)則：高斯濾波積分可歸納成其中

采用ssr容積采樣規(guī)則，求解球面積分s(r)，對(duì)于擁有n維狀態(tài)量的系統(tǒng)，求解過(guò)程如下：取一組m維向量ai＝[ai1,ai2,ai3,…，ain]^t,i＝1,2,…，m+1，并將向量ai的中點(diǎn)投影到球面un得到序列則有

選用[ξi,ωi]表示濾波器的采樣容積點(diǎn)，其中則得到ssr規(guī)則下五階非線性多維積分為：

其中：

f(ξi)＝f(ξi)+f(-ξi)

f(ωi)＝f(ωi)+f(-ωi)

5.2時(shí)間更新過(guò)程：估計(jì)方差pk-1/k-1通過(guò)cholesky矩陣進(jìn)行分解得：pk-1/k-1＝sk-1/k-1s^tk-1/k-1。根據(jù)ssr容積規(guī)則選取容積點(diǎn)集合[ξi,ωi]，因此容積點(diǎn)為：通過(guò)狀態(tài)方程來(lái)傳導(dǎo)容積點(diǎn)：x^*i,k/k-1＝f(xi,k-1/k-1)，因此k時(shí)刻的一步狀態(tài)預(yù)測(cè)和一步預(yù)測(cè)方差為：

5.3量測(cè)過(guò)程更新流程：通過(guò)量測(cè)方程傳導(dǎo)容積點(diǎn)：zi,k/k-1＝h(xi,k/k-1)，k時(shí)刻的量測(cè)量z的預(yù)測(cè)以及濾波增益矩陣為：

因此k時(shí)刻的狀態(tài)估計(jì)和均方差估計(jì)為：

本發(fā)明以水下航行器為研究對(duì)象，建立了捷聯(lián)慣性導(dǎo)航非線性誤差模型，并以多普勒測(cè)速儀作為輔助建立了組合導(dǎo)航模型；采用ssr規(guī)則選取ckf的容積點(diǎn)，并建立5階ssrckf的非線性濾波模型；本發(fā)明采用ssr規(guī)則對(duì)標(biāo)準(zhǔn)ckf濾波方法進(jìn)行改進(jìn)，在一定程度上減少了ckf濾波方法的容積采樣點(diǎn)，在高階系統(tǒng)中效果顯著，解決了傳統(tǒng)ckf算法在高階系統(tǒng)中容積采樣點(diǎn)的數(shù)目太大導(dǎo)致算法的實(shí)時(shí)性不好和效率不高的問(wèn)題。