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

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


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