本發(fā)明涉及導(dǎo)航系統(tǒng)及導(dǎo)航傳感器技術(shù)領(lǐng)域，具體涉及一種多普勒計(jì)程儀測(cè)速誤差的修正方法。

背景技術(shù)：

在長(zhǎng)航時(shí)導(dǎo)航中，由于慣性導(dǎo)航系統(tǒng)(INS)的誤差隨時(shí)間積累，因此，需要其他傳感器來(lái)輔助來(lái)進(jìn)行組合導(dǎo)航，從而抑制INS的誤差積累。在水下潛器的長(zhǎng)航時(shí)導(dǎo)航中，由于水下環(huán)境的特殊性，無(wú)線(xiàn)電信號(hào)在水中衰減迅速，致使以GPS等為代表的全球?qū)Ш叫l(wèi)星系統(tǒng)(GNSS)無(wú)法在水下使用，需要尋找其他傳感器來(lái)實(shí)現(xiàn)水下組合導(dǎo)航。

多普勒計(jì)程儀(DVL)作為一種精確的速度傳感器，在水下組合導(dǎo)航中得到了廣泛地應(yīng)用，INS/DVL是目前實(shí)現(xiàn)水下自主導(dǎo)航的主要方式。DVL測(cè)速精度是INS/DVL組合導(dǎo)航系統(tǒng)精度的關(guān)鍵因素，在正常情況下，DVL在底跟蹤模式下測(cè)速精度可以達(dá)到當(dāng)前載體速度的1％，可以滿(mǎn)足組合導(dǎo)航精度的需要。然而，當(dāng)浪涌，或者載體的上浮、下潛運(yùn)動(dòng)等因素使載體水平姿態(tài)發(fā)生變化時(shí)，會(huì)給DVL的速度量測(cè)引入誤差，從而影響組合導(dǎo)航精度。

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

有鑒于此，本發(fā)明提供了一種多普勒計(jì)程儀測(cè)速誤差的修正方法，基于該修正方法的修正模型能夠利用慣性導(dǎo)航系統(tǒng)(INS)提供的姿態(tài)信息，修正由載體俯仰角變化引起的多普勒計(jì)程儀(DVL)的測(cè)速誤差，從而提高INS/DVL組合導(dǎo)航精度。

為了解決上述技術(shù)問(wèn)題，本發(fā)明采用的技術(shù)方案是：

利用公式(9)的修正模型對(duì)在Δt＝t₂-t₁時(shí)間內(nèi)的多普勒計(jì)程儀量測(cè)速度進(jìn)行修正：

其中V_h＝[V_hx V_hy V_hz]^T表示在Δt＝t₂-t₁時(shí)間內(nèi)載體在水平坐標(biāo)系h內(nèi)的速度，為多普勒計(jì)程儀經(jīng)測(cè)速誤差修正后輸出的速度；t₁為多普勒計(jì)程儀發(fā)射信號(hào)的時(shí)刻，t₂為多普勒計(jì)程儀接收到返回信號(hào)的時(shí)刻；V_{DVL_x}、V_{DVL_y}和V_{DVL_z}分別為多普勒計(jì)程儀量測(cè)得到的載體的右向、前向和向上的速度；θ為t₁時(shí)刻載體的俯仰角；Δθ為Δt時(shí)間內(nèi)載體俯仰角的變化量；L_y和L_z分別為多普勒計(jì)程儀安裝點(diǎn)在載體坐標(biāo)系b Y^b和Z^b軸方向上偏離坐標(biāo)系原點(diǎn)的距離；ω_θ1為t₁時(shí)刻載體的俯仰角變化速率，ω_θ2為t₂時(shí)刻載體的俯仰角變化速率。

其中，t₁時(shí)刻載體俯仰角變化速率ω_θ1、t₂時(shí)刻載體俯仰角變化速率ω_θ2、以及t₁時(shí)刻載體的俯仰角θ和t₂時(shí)刻載體的俯仰角(θ+Δθ)由載體的導(dǎo)航系統(tǒng)提供。較佳地，導(dǎo)航系統(tǒng)為SINS、INS或GNSS。

進(jìn)一步地，修正模型的建立方法為：

步驟1，將多普勒計(jì)程儀在發(fā)射信號(hào)時(shí)刻t₁和接收信號(hào)時(shí)刻t₂沿前、右、后和左四條波束方向相對(duì)于水底的速度用含有俯仰角相關(guān)參數(shù)的表達(dá)式分別進(jìn)行表達(dá)；

步驟2，依據(jù)步驟1得到的t₁和t₂時(shí)刻多普勒計(jì)程儀相對(duì)水底的速度的表達(dá)式，得到多普勒計(jì)程儀四條波束的頻移[f_D1 f_D2 f_D3 f_D4]^T和載體坐標(biāo)系b內(nèi)的速度V_b的關(guān)系：

其中C為聲波在水中的傳播速度；f₀為多普勒計(jì)程儀發(fā)射信號(hào)的頻率；α為多普勒計(jì)程儀發(fā)射信號(hào)方向與載體坐標(biāo)系OX_bY_b平面之間的夾角；V_b＝[V_bx V_by V_bz]^T為載體坐標(biāo)系b下的載體速度，V_bx、V_by和V_bz分別為載體坐標(biāo)系b下載體的右向、前向和向上的速度；

步驟3，建立載體速度與量測(cè)速度在Δt＝t₂-t₁時(shí)間內(nèi)的關(guān)系式，具體包括如下子步驟：

步驟3.1，實(shí)際采用的四波束Janus配置多普勒計(jì)程儀使用如下等式(5)計(jì)算載體速度，得到的載體速度為量測(cè)速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T：

步驟3.2，依據(jù)公式(3)，構(gòu)建如下等式(4)：

步驟3.3，聯(lián)立式(4)和式(5)，得到載體在載體坐標(biāo)系b下的速度[V_bx V_by V_bz]^T與量測(cè)速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T在Δt時(shí)間內(nèi)的關(guān)系式：

步驟4，將載體在載體坐標(biāo)系b下的速度轉(zhuǎn)換到水平坐標(biāo)系h下；

步驟5，在Δt時(shí)間內(nèi)，載體在水平坐標(biāo)系h內(nèi)的速度[V_hx V_hy V_hz]^T為：

式(9)即為多普勒計(jì)程儀測(cè)速誤差的修正模型。

其中，步驟1中t時(shí)刻多普勒計(jì)程儀沿四條波束方向相對(duì)于水底的速度表達(dá)式為：

其中t＝t₁時(shí)，ω_θi＝ω_θ1，t＝t₂時(shí)，ω_θi＝ω_θ2。

其中，步驟4中，具體的坐標(biāo)轉(zhuǎn)換方法如下：t₁時(shí)刻，水平坐標(biāo)系h到載體坐標(biāo)系b的變換矩陣為：

t₂時(shí)刻，水平坐標(biāo)系h到載體坐標(biāo)系b的變換矩陣為：

依據(jù)式(6)載體在載體坐標(biāo)系b下的速度，t₁時(shí)刻水平坐標(biāo)系h到載體坐標(biāo)系b的變換矩陣，在t₁時(shí)刻載體在水平坐標(biāo)系h下的速度為：

依據(jù)式(6)載體在載體坐標(biāo)系b下的速度，t₂時(shí)刻水平坐標(biāo)系h到載體坐標(biāo)系b的變換矩陣，在t₂時(shí)刻載體在水平坐標(biāo)系h下的速度為：

有益效果：

(1)建立了載體俯仰角變化下多普勒計(jì)程儀測(cè)速誤差的修正模型，在模型中不僅考慮了非零俯仰角對(duì)DVL測(cè)速誤差的引入，還考慮了DVL發(fā)射信號(hào)時(shí)刻和接收信號(hào)時(shí)刻的時(shí)間差內(nèi)載體俯仰角的變化量，以及俯仰角速率和DVL安裝位置的桿臂引起的桿臂效應(yīng)對(duì)DVL測(cè)速誤差的引入，建立的誤差修正模型更為準(zhǔn)確。

(2)多普勒計(jì)程儀測(cè)速誤差修正方法中利用所建立的誤差修正模型，選取導(dǎo)航系統(tǒng)提供的載體的姿態(tài)信息代入DVL測(cè)速誤差的修正模型，數(shù)據(jù)選取簡(jiǎn)單、快速，實(shí)現(xiàn)對(duì)誤差的有效修正。

(3)多普勒計(jì)程儀測(cè)速誤差修正方法應(yīng)用在INS/DVL組合導(dǎo)航系統(tǒng)中，有效提高DVL測(cè)速精度，進(jìn)而提高INS/DVL組合導(dǎo)航系統(tǒng)的精度。

附圖說(shuō)明

圖1為本發(fā)明的修正方法流程圖；

圖2為俯仰角和桿臂效應(yīng)影響下的DVL測(cè)速原理圖；

圖3為矯正前后的DVL速度誤差對(duì)比圖。

具體實(shí)施方式

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

本發(fā)明在建立修正模型時(shí)，全面考慮了俯仰角θ、DVL發(fā)射和接收信號(hào)時(shí)刻的時(shí)間差Δt內(nèi)載體俯仰角的變化量Δθ以及DVL安裝位置和載體搖擺中心間的桿臂L和載體俯仰角速率ω引起的桿臂效應(yīng)對(duì)DVL測(cè)速誤差的引入，使得建立的修正模型更為精確，完成DVL測(cè)速誤差修正后得到更為精確的導(dǎo)航結(jié)果。

本發(fā)明中的坐標(biāo)系及變量的定義：

導(dǎo)航坐標(biāo)系n：原點(diǎn)O位于載體中心，Xⁿ軸和Yⁿ軸分別在當(dāng)?shù)厮矫鎯?nèi)指向東和北，Zⁿ軸垂直于當(dāng)?shù)厮矫嬷赶蛱煜颉?/p>

載體坐標(biāo)系b：原點(diǎn)O位于載體中心，Y^b軸沿載體縱軸指向前，Z^b軸垂直于載體平面指向上，X^b與Y^b和Z^b構(gòu)成右手坐標(biāo)系。

水平坐標(biāo)系h：原點(diǎn)O位于載體中心，X^h和Y^h是X^b與Y^b在水平面上的投影，Z^h垂直于水平面指向上。

t₁：DVL發(fā)射信號(hào)的時(shí)刻；

t₂：DVL接收到返回信號(hào)的時(shí)刻；

Δt：t₁時(shí)刻到t₂時(shí)刻的時(shí)間差；

θ：載體俯仰角；

Δθ：Δt時(shí)間內(nèi)載體俯仰角的變化量；

O：載體的搖擺中心；

M：DVL的安裝位置；

L_x L_y L_z：M在載體坐標(biāo)系b內(nèi)分別在X^b、Y^b和Z^b軸方向上偏離O點(diǎn)的距離；

L：M到O點(diǎn)的直線(xiàn)距離；

ω_θ1：t₁時(shí)刻載體的俯仰角變化速率；

ω_θ2：t₂時(shí)刻載體的俯仰角變化速率；

D₁：DVL前向波束在水底的反射位置點(diǎn)；

D₂：DVL右向波束在水底的反射位置點(diǎn)；

D₃：DVL后向波束在水底的反射位置點(diǎn)；

D₄：DVL左向波束在水底的反射位置點(diǎn)；

V_bx：載體坐標(biāo)系b下的右向速度，在Δt內(nèi)設(shè)為常值；

V_by：載體坐標(biāo)系b下的前向速度，在Δt內(nèi)設(shè)為常值；

V_bz：載體坐標(biāo)系b下向上的速度，在Δt內(nèi)設(shè)為常值；

V_hx：水平坐標(biāo)系h下載體的右向速度；

V_hy：水平坐標(biāo)系h下載體的前向速度；

V_hz：水平坐標(biāo)系h下載體向上的速度；

V_{DVL_x}：DVL量測(cè)得到的載體的右向速度；

V_{DVL_y}：DVL量測(cè)得到的載體的前向速度；

V_{DVL_z}：DVL量測(cè)得到的載體向上的速度；

β：載體坐標(biāo)系b下M點(diǎn)到O點(diǎn)的連線(xiàn)與Y^b軸之間的夾角，為定值；

C：聲波在水中的傳播速度，設(shè)為常數(shù)值。

α：多普勒計(jì)程儀發(fā)射信號(hào)方向與載體坐標(biāo)系Y^b軸之間的夾角，為常數(shù)值；

f₀：DVL發(fā)射信號(hào)的頻率；

前向波束發(fā)射信號(hào)和接收信號(hào)之間的頻移；

右向波束發(fā)射信號(hào)和接收信號(hào)之間的頻移；

后向波束發(fā)射信號(hào)和接收信號(hào)之間的頻移；

左向波束發(fā)射信號(hào)和接收信號(hào)之間的頻移。

根據(jù)DVL測(cè)速原理可知，安裝在載體上的DVL測(cè)量的是載體相對(duì)地的速度，在載體無(wú)俯仰角和橫滾角變化的情況下，載體坐標(biāo)系b和水平坐標(biāo)系h重合，DVL測(cè)量的是載體坐標(biāo)系b系下的速度，然而實(shí)際上DVL測(cè)量的應(yīng)該是水平坐標(biāo)系h系下的速度，當(dāng)載體存在俯仰角和橫滾角變化情況下，載體坐標(biāo)系b和水平坐標(biāo)系h不再重合，此時(shí)如果再認(rèn)為DVL測(cè)量的是載體坐標(biāo)系b下的速度就會(huì)出現(xiàn)偏差。在原理上橫滾角對(duì)DVL測(cè)速誤差的影響和俯仰角相同，所以為了簡(jiǎn)化模型，在本發(fā)明中重點(diǎn)討論俯仰角對(duì)DVL測(cè)速誤差的影響及修正。

本發(fā)明提供了一種多普勒計(jì)程儀測(cè)速誤差的修正方法，設(shè)定在自身驅(qū)動(dòng)下，載體坐標(biāo)系b下的速度在Δt時(shí)間內(nèi)保持不變，而導(dǎo)航坐標(biāo)系n和水平坐標(biāo)系h系下的速度在俯仰角變化的影響下實(shí)時(shí)發(fā)生變化。

本發(fā)明中DVL采用詹納斯(Janus)配置，在載體坐標(biāo)系b下分別向前下、左下、后下、右下四個(gè)方向發(fā)射，四個(gè)波束相隔90度分布，圖2所示為俯仰角和桿臂效應(yīng)影響下的DVL測(cè)速原理圖。圖2為縱面剖圖，為清晰表達(dá)各夾角，只繪出對(duì)應(yīng)反射點(diǎn)為D₁和D₃的波束，橫向兩條波束及對(duì)應(yīng)的反射點(diǎn)D₂和D₄未在圖2中繪出；

在Δt時(shí)間內(nèi)俯仰角和桿臂效應(yīng)影響下的DVL測(cè)速誤差的修正模型為公式(9)，將含有誤差的DVL的量測(cè)值代入公式(9)，得到修正后的更為精確的載體速度[V_hx V_hy V_hz]^T：

其中，V_{DVL_x}為多普勒計(jì)程儀量測(cè)得到的載體的右向速度；V_{DVL_y}為多普勒計(jì)程儀量測(cè)得到的載體的前向速度；V_{DVL_z}為多普勒計(jì)程儀量測(cè)得到的載體向上的速度；θ為多普勒計(jì)程儀信號(hào)發(fā)射的時(shí)刻載體的俯仰角；Δθ為Δt時(shí)間內(nèi)載體俯仰角的變化量；L_y和L_z為M點(diǎn)在載體坐標(biāo)系b內(nèi)分別在Y^b和Z^b軸方向上偏離O點(diǎn)的距離；ω_θ1為t₁時(shí)刻載體的俯仰角變化速率，ω_θ2為t₂時(shí)刻載體的俯仰角變化速率。

多普勒計(jì)程儀測(cè)速誤差的修正模型建立方法如下：

根據(jù)四波束Janus配置DVL測(cè)速原理，DVL安裝位置即M點(diǎn)沿四條波束發(fā)射方向相對(duì)于水底的速度的表達(dá)式為：

在t₁時(shí)刻由桿臂L和俯仰角速度ω_θ1引起桿臂速度為V_Lθ1＝L·ω_θ1，速度方向如圖1所示，同理，在DVL信號(hào)接收的t₂時(shí)刻，M點(diǎn)沿四條波束返回方向相對(duì)于水底的速度的表達(dá)式為：

其中V_Lθ2＝L·ω_θ2，V_Lθ2為由桿臂L和t₂時(shí)刻載體的俯仰角變化速率ω_θ2引起桿臂速度。

根據(jù)V_trans和V_rec，得到多普勒計(jì)程儀四條波束發(fā)射信號(hào)和接收信號(hào)之間的頻移[f_D1 f_D2 f_D3 f_D4]^T和V_b之間的關(guān)系式，其中V_b＝[V_bx V_by V_bz]^T：

依據(jù)公式(3)，可以構(gòu)造如下等式(4)：

而在實(shí)際應(yīng)用中，四波束Janus配置DVL使用如下等式(5)計(jì)算載體速度，得到的載體速度為量測(cè)速度[V_{DVL_x} V_{DVL_y} V_{DVL_z}]^T：

由于f_D1、f_D2、f_D3、f_D4為量測(cè)值，即式(4)和式(5)等號(hào)的左半部分都是通過(guò)量測(cè)得到的頻移量，無(wú)論使用式(4)或式(5)計(jì)算速度，量測(cè)值f_D1、f_D2、f_D3、f_D4是不會(huì)變的，故式(4)和式(5)左半部分相等，聯(lián)立兩式并化簡(jiǎn)，可以得到V_b與V_DVL的關(guān)系式：

由式(6)可以從DVL測(cè)量值計(jì)算出Δt時(shí)間內(nèi)載體在載體坐標(biāo)系b下的速度。

當(dāng)俯仰角為θ時(shí)，t₁時(shí)刻水平坐標(biāo)系h到載體坐標(biāo)系b的變換矩陣為：

t₂時(shí)刻水平坐標(biāo)系h到載體坐標(biāo)系b的變換矩陣為：

依據(jù)式(6)求得的載體在載體坐標(biāo)系b下的速度，t₁時(shí)刻水平坐標(biāo)系h到載體坐標(biāo)系b的變換矩陣，得到在t₁時(shí)刻載體在水平坐標(biāo)系h下的速度為：

依據(jù)式(6)載體在載體坐標(biāo)系b下的速度，t₂時(shí)刻水平坐標(biāo)系h到載體坐標(biāo)系b的變換矩陣，得到在t₂時(shí)刻載體在水平坐標(biāo)系h下的速度為：

在Δt內(nèi)，載體在水平坐標(biāo)系h內(nèi)的速度[V_hx V_hy V_hz]^T近似等于t₁時(shí)刻載體在水平坐標(biāo)系h下的速度與t₂時(shí)刻載體在水平坐標(biāo)系h下的速度的平均值，表達(dá)式如下：

式(9)即為俯仰角和桿臂效應(yīng)影響下的DVL測(cè)速誤差的修正模型，將含有誤差的DVL的量測(cè)值代入式(9)，得到修正后的更為精確的載體速度，[V_hx V_hy V_hz]^T為載體在水平坐標(biāo)系h內(nèi)的速度，為DVL信號(hào)發(fā)射時(shí)刻即t₁時(shí)刻，載體在水平坐標(biāo)系h下的速度，為DVL信號(hào)接收時(shí)刻即t₂時(shí)刻，載體在水平坐標(biāo)系h下的速度。

本發(fā)明多普勒計(jì)程儀測(cè)速誤差的修正方法為：

步驟1，建立起在Δt＝t₂-t₁時(shí)間內(nèi)如公式(9)的修正模型：

步驟2，利用INS提供的載體姿態(tài)信息修正DVL測(cè)速誤差：

由于INS可以提供載體完整的姿態(tài)信息，故可以利用t₁時(shí)刻陀螺儀提供的俯仰角變化速率ω_{θ1_SINS}，t₂時(shí)刻陀螺儀提供的俯仰角變化速率ω_{θ2_SINS}，以及INS解算得到的俯仰角θ_SINS和(θ+Δθ)_SINS修正DVL測(cè)速誤差，將其代入公式(9)，得到如下公式(10)的修正模型：

步驟3，利用由公式(10)得到的V_h代替DVL的直接量測(cè)值V_DVL進(jìn)行組合導(dǎo)航，完成DVL測(cè)速誤差的修正。

橫滾角和桿臂效應(yīng)影響下的DVL測(cè)速誤差的修正模型構(gòu)建方法與步驟1類(lèi)似。本發(fā)明具體實(shí)施中更多地采用捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)來(lái)提供載體的姿態(tài)信息。在矯正效果仿真實(shí)驗(yàn)中：設(shè)L_y＝2.5m，L_z＝0.5m，其他條件如下：

(1)捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)：

陀螺常值漂移0.01°/h；隨機(jī)漂移0.01°/h

加表常值零偏10^-4g；加表隨機(jī)零偏5×10^-5g

(2)多普勒計(jì)程儀(DVL)：

測(cè)速常值誤差1cm/s；隨機(jī)誤差1cm/s；更新頻率1Hz

仿真時(shí)間設(shè)定為1200s，初始位置載體系下速度恒定為[0 2]，0-600s載體以姿態(tài)[0° 0° 45°]勻速直線(xiàn)運(yùn)動(dòng)，600-1200s在海浪的激勵(lì)下，載體開(kāi)始繞俯仰軸搖擺運(yùn)動(dòng)，搖擺運(yùn)動(dòng)模型為θ_msin(ω_θt+φ_θ)，搖擺幅值θ_m＝20°，搖擺周期T_θ＝50s，ω_θ＝2π/T_θ。

圖3所示為多普勒計(jì)程儀測(cè)速誤差修正前和修正后的對(duì)比圖，由對(duì)比圖可知本發(fā)明提出的修正方法是有效的。

綜上所述，以上僅為本發(fā)明的較佳實(shí)施例而已，并非用于限定本發(fā)明的保護(hù)范圍。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。