專利名稱：載人潛水器位置測(cè)量延時(shí)處理方法
技術(shù)領(lǐng)域：
本發(fā)明涉及載人潛水器在海洋中定位技術(shù)，具體地說是一種載人潛水器位置測(cè)量延時(shí)處理方法。
背景技術(shù)：
載人潛水器在海洋中運(yùn)動(dòng)時(shí)，需要實(shí)時(shí)確定自己目前的坐標(biāo)。目前多采用位置傳感器進(jìn)行實(shí)時(shí)測(cè)量，提供載人潛水器的位置信息。通過載人潛水器導(dǎo)航處理系統(tǒng)的計(jì)算，綜合這些位置信息，得到一個(gè)最優(yōu)的載人潛水器在海洋中的位置信息。
載人潛水器的導(dǎo)航處理系統(tǒng)在進(jìn)行載人潛水器位置估計(jì)時(shí)，最基本的可以實(shí)時(shí)測(cè)得的狀態(tài)量包括航向角θ、縱傾角ψ、水平面的速度值u，v。載人潛水器的水平面位置數(shù)據(jù)(X方向、Y方向)可以通過兩種方法獲得，一是直接方法，即利用位置傳感器(如超短基線水聲定位系統(tǒng))直接測(cè)量；二是間接方法，即根據(jù)其它傳感器測(cè)量數(shù)據(jù)(速度、航向角)來推測(cè)位置數(shù)據(jù)。其中超短基線水聲定位系統(tǒng)是在母船上安裝一個(gè)水聲接收基陣，水聲接收基陣上有5個(gè)接收換能器，按圓環(huán)型布置(相互間隔1厘米左右)，在圓環(huán)的中間有一個(gè)發(fā)射換能器，再在載人潛水器上安裝一只水聲應(yīng)答信標(biāo)；水聲接收基陣收到水聲應(yīng)答信標(biāo)的信號(hào)，就可以確定載人潛水器與母船的相對(duì)位置。
載人潛水器獲得超短基線定位系統(tǒng)的位置信息過程為首先超短基線水聲定位系統(tǒng)的發(fā)射換能器定時(shí)地發(fā)出聲脈沖詢問信號(hào)，載人潛水器上的應(yīng)答信標(biāo)接到詢問信號(hào)后，發(fā)射應(yīng)答聲脈沖信號(hào)。水聲接收基陣收到應(yīng)答信標(biāo)的脈沖信號(hào)后，超短基線數(shù)據(jù)處理系統(tǒng)計(jì)算出載人潛水器位置值，然后通過母船上的水聲通訊機(jī)將獲得的載人潛水器的位置信息發(fā)送給水下載人潛水器。這一過程由于載人潛水器與母船的距離不同，聲傳播需要幾秒甚至幾十秒的時(shí)間，因此每次載人潛水器收到的位置信息都將是過時(shí)的信息。
在現(xiàn)有技術(shù)中，無纜潛水器位置估算中，只靠間接的方法推算位置數(shù)據(jù)，不綜合超短基線的數(shù)據(jù)，超短基線的數(shù)據(jù)只作為起始數(shù)據(jù)使用。由于傳感器的數(shù)據(jù)漂移，長(zhǎng)時(shí)間的位置推算，無纜潛水器的位置數(shù)據(jù)會(huì)偏離實(shí)際的位置值。

發(fā)明內(nèi)容
為了解決載人潛水器在海洋航行中，導(dǎo)航系統(tǒng)得到的水聲定位系統(tǒng)數(shù)據(jù)延時(shí)問題，本發(fā)明的目的是提供一種載人潛水器位置測(cè)量延時(shí)處理方法。通過當(dāng)前得到的過時(shí)超短基線位置測(cè)量信息，實(shí)現(xiàn)對(duì)當(dāng)前的載人潛水器位置信息進(jìn)行優(yōu)化處理，解決位置信息滯后造成的估計(jì)誤差問題，本發(fā)明把超短基線的測(cè)量值融合到載人潛水器的位置推算中，避免長(zhǎng)時(shí)間的載人潛水器位置推算，導(dǎo)致載人潛水器位置推算數(shù)據(jù)偏離實(shí)際位置值。
為了實(shí)現(xiàn)上述目的，本發(fā)明的技術(shù)方案是把當(dāng)前得到的過時(shí)超短基線位置測(cè)量值通過卡爾曼濾波算法融合到載人潛水器的位置推算中，實(shí)現(xiàn)對(duì)當(dāng)前的載人潛水器位置信息進(jìn)行優(yōu)化處理；所述推算算法具體是假設(shè)得到超短基線水聲定位系統(tǒng)測(cè)量的載人潛水器位置信息過程中，聲波在水中的傳播時(shí)間長(zhǎng)度為t＝d*Ts，在t＝k*Ts時(shí)刻水下水聲應(yīng)答器向水聲接收基陣發(fā)出應(yīng)答信號(hào)，由于聲波在水中傳輸?shù)臅r(shí)延，載人潛水器要在t＝(k+d)*Ts才能接收到k時(shí)刻的超短基線測(cè)量的載人潛水器位置信息Xkm，Ykm；此時(shí)，在t＝(k+d)*Ts時(shí)刻，載人潛水器的位置估計(jì)為Xk+d=Xk+Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1]Ts;]]>Yk+d=Yk+Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>其中，d為時(shí)間間隔，Ts為時(shí)間計(jì)量單位；k為第k次測(cè)量；Xk，Yk為k時(shí)刻載人潛水器位置的估計(jì)，u、v是載人潛水器在水平面的兩維運(yùn)動(dòng)速度，θ是載人潛水器的航向角，ψ是載人潛水器的縱傾角；令ΔXd=Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1)]Ts;]]>ΔYd=Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>則根據(jù)k時(shí)刻超短基線測(cè)量值Xkm，Ykm和k時(shí)刻估計(jì)值Xk，Yk，應(yīng)用卡爾曼濾波公式，得到k時(shí)刻的最優(yōu)估計(jì) 則可得(k+d)時(shí)刻載人潛水器位置的最優(yōu)估計(jì)值為X^(k+d|k)=X^(k|k)+ΔXd.]]>Y^(k+d|k)=Y^(k|k)+ΔYd]]>所述(k+d)時(shí)刻載人潛水器位置的最優(yōu)估計(jì)值中X，Y每隔d*Ts秒刷新一次，其余變量如載人潛水器的運(yùn)動(dòng)速度和航向角，每隔Ts秒刷新一次；所述載人潛水器位置測(cè)量延時(shí)處理推算算法流程為先進(jìn)行數(shù)據(jù)采集，然后對(duì)潛水器X方向位置和潛水器Y方向位置進(jìn)行推算，再接收k時(shí)刻潛水器的位置值，運(yùn)用卡爾曼濾波器估算K時(shí)刻位置值，更新k+d時(shí)刻的位置值，構(gòu)成一個(gè)算法過程。
本發(fā)明具有如下優(yōu)點(diǎn)1.本發(fā)明解決了以前因超短基線的延遲造成數(shù)據(jù)無法利用的問題。
在現(xiàn)有技術(shù)中，無纜潛水器位置只根據(jù)其它傳感器數(shù)據(jù)(速度、航向角)來推測(cè)位置數(shù)據(jù)。由于測(cè)量誤差的存在，位置估計(jì)誤差呈現(xiàn)增大趨勢(shì)，并隨著時(shí)間增長(zhǎng)，最終使得位置估計(jì)誤差趨于無窮大。超短基線水聲定位系統(tǒng)直接進(jìn)行位置測(cè)量可以保證測(cè)量誤差有界，但是其測(cè)量噪聲是很大的。本發(fā)明把超短基線測(cè)量數(shù)據(jù)，通過卡爾曼濾波器融合到潛水器位置推算中，綜合了間接測(cè)量(推算)的數(shù)據(jù)平滑性和直接測(cè)量過程誤差的有界性，獲得了最佳的估計(jì)效果。
2.本發(fā)明提出的載人潛水器位置測(cè)量延時(shí)處理方法，同樣適用于其它的水下機(jī)器人，包括自治水下機(jī)器人(AUV)和遙控水下機(jī)器人(ROV)。


圖1為本發(fā)明載人潛水器位置測(cè)量延時(shí)處理算法軟件編程流程圖。
圖2為現(xiàn)有技術(shù)中傳統(tǒng)的根據(jù)速度測(cè)量值進(jìn)行位置推算結(jié)果示意圖。
圖3為現(xiàn)有技術(shù)中另一種傳統(tǒng)的直接位置測(cè)量效果圖。
圖4為本發(fā)明采用卡爾曼濾波進(jìn)行數(shù)據(jù)融合的效果圖。
具體實(shí)施例方式
下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)說明。
把當(dāng)前得到的過時(shí)超短基線位置測(cè)量值融合到載人潛水器的位置推算中，實(shí)現(xiàn)對(duì)當(dāng)前的載人潛水器位置信息進(jìn)行優(yōu)化處理；具體是假設(shè)得到超短基線水聲定位系統(tǒng)測(cè)量的載人潛水器位置信息過程中，聲波在水中的傳播時(shí)間長(zhǎng)度為t＝d*Ts，其中d為時(shí)間間隔，Ts為時(shí)間計(jì)量單位；k為第k次測(cè)量；時(shí)刻水下水聲應(yīng)答器向水聲接收基陣發(fā)出應(yīng)答信號(hào)，由于聲波在水中傳輸?shù)臅r(shí)延，載人潛水器要在t＝(k+d)*Ts才能接收到k時(shí)刻的超短基線測(cè)量的載人潛水器位置信息Xkm，Ykm；此時(shí)，在t＝(k+d)*Ts時(shí)刻，載人潛水器的位置估計(jì)為Xk+d=Xk+Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1]Ts---(1);]]>Yk+d=Yk+Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>其中，Xk，Yk為k時(shí)刻載人潛水器位置的估計(jì)，u、v是載人潛水器在水平面的兩維運(yùn)動(dòng)速度，θ是載人潛水器的航向角，ψ是載人潛水器的縱傾角。令ΔXd=Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1)]Ts---(2);]]>ΔYd=Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>則根據(jù)k時(shí)刻超短基線測(cè)量值Xkm，Ykm和k時(shí)刻估計(jì)值Xk，Yk，應(yīng)用(10)～(13)的卡爾曼濾波公式，得到k時(shí)刻的最優(yōu)估計(jì) 則可得(k+d)時(shí)刻載人潛水器位置的最優(yōu)估計(jì)值為
X^(k+d|k)=X^(k|k)+ΔXd---(3);]]>Y^(k+d|k)=Y^(k|k)+ΔYd]]>其中，X，Y每隔d*Ts秒刷新一次，其余變量如載人潛水器的運(yùn)動(dòng)速度和航向角，每隔Ts秒刷新一次；如圖1所示，本發(fā)明載人潛水器位置測(cè)量延時(shí)處理算法流程為先進(jìn)行數(shù)據(jù)采集，然后對(duì)潛水器X方向位置和潛水器Y方向位置進(jìn)行推算，再接收k時(shí)刻潛水器的位置值，運(yùn)用卡爾曼濾波器估算K時(shí)刻位置值，更新k+d時(shí)刻的位置值，構(gòu)成一個(gè)算法過程。
卡爾曼濾波是一種基于迭代和遞推過程的最優(yōu)線性估計(jì)器，它可以分解成預(yù)測(cè)、測(cè)量、校正三個(gè)子過程；設(shè)離散時(shí)間系統(tǒng)的狀態(tài)方程和輸出方程分別為x(k+1)＝Φ(k+1，k)x(k)+Γ(k+1，k)w(k) (4)；y(k)＝C(k)x(k)+v(k)其中，Φ(k+1，k)為狀態(tài)轉(zhuǎn)移矩陣，C(k)為觀測(cè)矩陣，w(k)，v(k)分別為模型噪聲和觀測(cè)噪聲，且有E[w(k)]＝0，Cov[w(k)，w(j)]＝Q(k)δkj，k，j≥0(5)；E[v(k)]＝0，Cov[w(k)，w(j)]＝R(k)δkj，k，j≥0(6)；假設(shè)根據(jù)觀測(cè)數(shù)據(jù)y＝{y(1)，y(2)，...，y(k-1)}，已經(jīng)求得k-1時(shí)刻狀態(tài)x(k-1)的最優(yōu)濾波估計(jì) 則在沒有獲得新的觀測(cè)數(shù)據(jù)y(k)以前，根據(jù)y(k-1)求得k時(shí)刻狀態(tài)x(k)的預(yù)測(cè)值為x^(k|k-1)=Φ(k,k-1)x^(k-1|k-1)---(7);]]>觀測(cè)值的預(yù)測(cè)估計(jì)為y^(k|k-1)=C(k)x^(k|k-1)---(8);]]>當(dāng)獲得新的觀測(cè)數(shù)據(jù)y(k)時(shí)，可得出由(6-5)式求得的預(yù)測(cè)誤差為y~(k|k-1)=y(k)-y^(k|k-1)---(9);]]>可以證明，當(dāng)式(6-2)(6-3)滿足時(shí)， 是一個(gè)白噪聲序列，即后來的與先前的無關(guān)，因此， 就可以視為k時(shí)刻獲得的新信息，可以用來對(duì)可狀態(tài)預(yù)測(cè)值 進(jìn)行校正，則k時(shí)刻狀態(tài)的估計(jì)值則變?yōu)閤~(k|k)=x^(k|k-1)+K(k)y~(k|k-1)---(10);]]>其中K(k)為濾波增益矩陣，它根據(jù)估計(jì)誤差的方差達(dá)到極小的準(zhǔn)則來確定的。表達(dá)式為K(k)＝P(k|k-1)CT(k)[C(k)P(k|k-1)CT(k)+R(k)]-1(11)；其中，P(k|k-1)為預(yù)測(cè)估計(jì)誤差的方差，表示為P(k|k-1)＝Φ(k，k-1)P(k-1|k-1)ΦT(k，k-1)+Q(k) (12)；P(k|k)＝[I-K(k)C(k)]P(k|k-1)[I-K(k)C(k)]T+K(k)R(k)KT(k)(13)。
在載人潛水器導(dǎo)航中，上述卡爾曼濾波算法實(shí)際上可理解為一種數(shù)據(jù)融合過程。
在現(xiàn)有技術(shù)中，無纜潛水器位置只根據(jù)其它傳感器數(shù)據(jù)(速度、航向角)來推測(cè)位置數(shù)據(jù)。由于測(cè)量誤差的存在，位置估計(jì)誤差呈現(xiàn)增大趨勢(shì)，并隨著時(shí)間增長(zhǎng)，最終使得位置估計(jì)誤差趨于無窮大(如圖2)。超短基線水聲定位系統(tǒng)直接進(jìn)行位置測(cè)量可以保證測(cè)量誤差有界，但是其測(cè)量噪聲是很大的(如圖3)。把超短基線測(cè)量數(shù)據(jù)，通過卡爾曼濾波器融合到潛水器位置推算中，綜合了間接測(cè)量(推算)的數(shù)據(jù)平滑性和直接測(cè)量過程誤差的有界性，獲得了最佳的估計(jì)效果(如圖4)。
本發(fā)明提出的載人潛水器位置測(cè)量延時(shí)處理方法，同樣適用于其它的水下機(jī)器人，包括自治水下機(jī)器人(AUV)和遙控水下機(jī)器人(ROV)。
在載人潛水器導(dǎo)航中，上述卡爾曼濾波算法實(shí)際上可理解為一種數(shù)據(jù)融合過程。
權(quán)利要求
1.一種載人潛水器位置測(cè)量延時(shí)處理方法，其特征在于把當(dāng)前得到的過時(shí)超短基線位置測(cè)量值通過卡爾曼濾波算法融合到載人潛水器的位置推算中，實(shí)現(xiàn)對(duì)當(dāng)前的載人潛水器位置信息進(jìn)行優(yōu)化處理。
2.按照權(quán)利要求1所述載人潛水器位置測(cè)量延時(shí)處理方法，其特征在于所述推算算法具體是假設(shè)得到超短基線水聲定位系統(tǒng)測(cè)量的載人潛水器位置信息過程中，聲波在水中的傳播時(shí)間長(zhǎng)度為t＝d*Ts，在t＝k*Ts時(shí)刻水下水聲應(yīng)答器向水聲接收基陣發(fā)出應(yīng)答信號(hào)，由于聲波在水中傳輸?shù)臅r(shí)延，載人潛水器要在t＝(k+d)*Ts才能接收到k時(shí)刻的超短基線測(cè)量的載人潛水器位置信息Xkm，Ykm；此時(shí)，在t＝(k+d)*Ts時(shí)刻，載人潛水器的位置估計(jì)為Xk+d=Xk+Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1)]Ts;]]>Yk+d=Yk+Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>其中，d為時(shí)間間隔，Ts為時(shí)間計(jì)量單位；k為第k次測(cè)量；Xk，Yk為k時(shí)刻載人潛水器位置的估計(jì)，u、v是載人潛水器在水平面的兩維運(yùn)動(dòng)速度，θ是載人潛水器的航向角，φ是載人潛水器的縱傾角；令ΔXd=Σi=1d[u(k+i-1)cosθ(k+i-1)cosψ(k+i-1)-v(k+i-1)sin(k+i-1)cosψ(k+i-1)]Ts;]]>ΔYd=Σi=1d[u(k+i-1)sinθ(k+i-1)cosψ(k+i-1)+v(k+i-1)cosθ(k+i-1)cosψ(k+i-1)]Ts]]>則根據(jù)k時(shí)刻超短基線測(cè)量值Xkm，Ykm和k時(shí)刻估計(jì)值Xk，Yk，應(yīng)用卡爾曼濾波公式，得到k時(shí)刻的最優(yōu)估計(jì)X^(k|k),Y^(k|k),]]>則可得(k+d)時(shí)刻載人潛水器位置的最優(yōu)估計(jì)值為X^(k+d|k)=X^(k|k)+ΔXd.]]>Y^(k+d|k)=Y^(k|k)+ΔYd]]>
3.按照權(quán)利要求2所述載人潛水器位置測(cè)量延時(shí)處理方法，其特征在于所述(k+d)時(shí)刻載人潛水器位置的最優(yōu)估計(jì)值中X，Y每隔d*Ts秒刷新一次，其余變量如載人潛水器的運(yùn)動(dòng)速度和航向角，每隔Ts秒刷新一次。
4.按照權(quán)利要求1所述載人潛水器位置測(cè)量延時(shí)處理方法，其特征在于所述載人潛水器位置測(cè)量延時(shí)處理推算算法流程為先進(jìn)行數(shù)據(jù)采集，然后對(duì)潛水器X方向位置和潛水器Y方向位置進(jìn)行推算，再接收k時(shí)刻潛水器的位置值，運(yùn)用卡爾曼濾波器估算K時(shí)刻位置值，更新k+d時(shí)刻的位置值，構(gòu)成一個(gè)算法過程。
全文摘要
本發(fā)明涉及載人潛水器在海洋中定位技術(shù)，具體地說是一種載人潛水器位置測(cè)量延時(shí)處理方法。本發(fā)明是把當(dāng)前得到的過時(shí)超短基線位置測(cè)量值通過卡爾曼濾波算法融合到載人潛水器的位置推算中，實(shí)現(xiàn)對(duì)當(dāng)前的載人潛水器位置信息進(jìn)行優(yōu)化處理。本發(fā)明通過當(dāng)前得到的過時(shí)超短基線位置測(cè)量信息，實(shí)現(xiàn)對(duì)當(dāng)前的載人潛水器位置信息進(jìn)行優(yōu)化處理，解決位置信息滯后造成的估計(jì)誤差問題，本發(fā)明把超短基線的測(cè)量值融合到載人潛水器的位置推算中，避免長(zhǎng)時(shí)間的載人潛水器位置推算，導(dǎo)致載人潛水器位置推算數(shù)據(jù)偏離實(shí)際位置值。
文檔編號(hào)G01C21/00GK1719198SQ20041002092
公開日2006年1月11日 申請(qǐng)日期2004年7月7日 優(yōu)先權(quán)日2004年7月7日
發(fā)明者于開洋 申請(qǐng)人:中國(guó)科學(xué)院沈陽自動(dòng)化研究所