一種DVL標定方法、系統、介質、設備及終端

2023-09-16 10:44:56 1

一種dvl標定方法、系統、介質、設備及終端
技術領域
1.本發明屬於水下組合導航技術領域，尤其涉及一種dvl標定方法、系統、介質、設備及終端。


背景技術：

2.目前，自主式水下航行器(autonomous underwater vehicle，auv)廣泛應用於國家海防建設、海洋資源勘探等軍民領域，高精度、高可靠性的自主導航能力是auv順利完成任務的前提。捷聯慣性導航系統(strapdown inertial navigation system，sins)由於結構簡單、體積小、隱蔽性強、便於與其他設備集成化設計等優點，在水下定位導航與授時(position，navigation and timing，pnt)領域受到廣泛關注，成為auv自主導航定位的重要手段。然而，sins本質上是以牛頓第二定律為基礎的積分推算系統，需要對慣性測量單元(inertial measurement unit，imu)的輸出進行積分運算，導致其導航誤差會隨著時間的推移而不斷積累。水下環境對全球導航衛星系統(global navigation satellite system，gnss)信號具有屏蔽作用，而都卜勒速度計程儀(doppler velocity log，dvl)測速精度穩定，可為sins提供實時外部速度輔助融合信息，sins/dvl組合導航已成為當前解決水下導航問題的主流方案之一。
3.dvl通過聲學都卜勒效應來測量auv相對於水底或者水流的速度，在工程實際中，由於製作工藝、外界環境、安裝偏差等條件限制，導致dvl的測速誤差難以避免。因此，在預先標定dvl誤差參數的基礎上，通過標定結果對dvl原始輸出進行實時補償以確保速度量測信息的準確性對提高sins/dvl組合導航系統的精度具有重要意義。
4.總結現有的dvl標定方法大致劃分為三類：(1)基於新興機器學習理論的標定方法，通過遺傳算法、pso、svr等智能算法對dvl誤差參數進行預測，此類方法雖然標定精度得到了提升，但是需要大量離線數據和強大的計算能力，現階段難以滿足工程應用實際需求；(2)無外界信息輔助條件下的標定方法，通過運載體速度誤差、位置誤差、加速度誤差等自身信息作為觀測量並通過最小二乘方法估計dvl誤差，此類方法雖然有助於提升隱蔽性，但是往往需要設置複雜的機動軌跡，標定的效率和精度受到一定程度影響；(3)有外界信息輔助條件下的標定方法，通過gnss等外部傳感器提供外界輔助導航信息並在此基礎上利用卡爾曼濾波、凸優化等理論標定dvl誤差，此類方法研究成果相對豐碩，體系也較為成熟，是現階段常用的標定方法，但是方法在系統的穩定性和準確性方面仍有進一步提升的空間。針對上述問題，亟待設計一種新的dvl標定方法。
5.通過上述分析，現有技術存在的問題及缺陷為：
6.(1)現有的基於機器學習理論的標定方法，需要大量的樣本且計算量龐大，難以滿足工程實際應用。
7.(2)現有的無外界信息輔助條件下的標定方法，對載體航行軌跡要求苛刻且標定的效率和精度較差。
8.(3)現有的有外界信息輔助條件下的標定方法，在系統的穩定性和準確性方面仍
有進一步提升的空間。


技術實現要素：

9.針對現有技術存在的問題，本發明提供了一種dvl標定方法、系統、介質、設備及終端，尤其涉及一種基於位置觀測信息davenport四元數方法的dvl標定方法、系統、介質、設備及終端。
10.本發明是這樣實現的，一種dvl標定方法，所述dvl標定方法包括：
11.當auv處於水面航行狀態可接收gnss信號時，利用kalman濾波最優估計方法進行sins/gnss組合導航，得到載體高精度的姿態和速度信息作為參考值；綜合dvl測量的d系下的速度值，通過所提出的標定方法估計出刻度因子誤差和安裝誤差角，並利用估計結果對dvl的原始輸出進行標定補償。
12.進一步，所述dvl標定方法包括以下步驟：
13.步驟一，構建dvl誤差模型，明確dvl測速誤差的主要來源，重點對刻度因子誤差和安裝誤差角進行分析；
14.步驟二，將sins/gnss組合導航結果作為b系參考速度，為後續標定方法的展開提供速度基準；
15.步驟三，基於都卜勒測速原理的位置觀測運算以標定刻度因子誤差，減小速度量測噪聲的不利影響；
16.步驟四，基於davenport四元數法求解位置觀測矢量方程以標定安裝誤差角，增強姿態旋轉矩陣求解過程的穩定性；
17.步驟五，根據計算結果對dvl的量測輸出進行標定。
18.進一步，所述步驟一中的dvl誤差模型的構建包括：
19.定義坐標系：選取「東-北-天：e-n-u」地理坐標係為導航坐標系，記為n系；選取「右-前-上」坐標係為載體坐標系，記為b系；dvl安裝坐標系記為d系；地心慣性坐標系記為i系；地球坐標系記為e系；計算導航坐標系記為n
′
系。
20.在理想的安裝情況下，dvl安裝坐標系d系與載體坐標系b系的坐標軸是互相一致的，安裝矩陣為單位陣i3×3，x
b-y
b-zb表示b系，x
d-y
d-zd表示d系。
21.根據dvl的工作原理，測速誤差模型表示為：
[0022][0023]
式中，表示dvl的量測輸出值，s表示刻度因子誤差，表示b繫到d系的姿態變換矩陣，vb表示載體的真實速度，表示載體系相對於導航系的旋轉角速度在載體系上的投影，表示sins與dvl之間的杆臂矢量誤差。
[0024]
進一步，所述步驟一中的dvl誤差模型的構建還包括：
[0025]
在實際工程應用中，由於sins和dvl之間通常緊密安裝且杆臂矢量直接通過測量獲得並予以補償，忽略杆臂誤差的影響，將式(1)簡化為：
[0026][0027]
對刻度因子誤差s進行估計，當b繫到d系的安裝誤差角ε為小角度時，姿態變換矩
陣(
·
×
)表示反對稱矩陣運算，定義ε＝[ε
x
,εy,εz]
t
，則：
[0028][0029]
當載體真實速度vb已知時，僅有dvl誤差為待求項，vb由sins和gnss組合導航的結果求得，dvl測速誤差模型表示為：
[0030][0031]
dvl的誤差標定轉化為求解式(4)中刻度因子誤差和安裝誤差角的問題，當求出相應未知參數後，對dvl量測輸出進行補償得出b系速度，用於進行sins/dvl水下組合導航解算。
[0032]
進一步，所述步驟三中的刻度因子誤差的標定包括：
[0033]
由於姿態旋轉矩陣的模值為1，姿態旋轉矩陣與向量相乘時，僅改變向量的方向但不改變向量的大小，故同時對式(4)兩側進行取模得：
[0034][0035]
在dvl測量得到載體d系速度的基礎上，得出：
[0036][0037]
同時對式(4)兩側進行位置運算，則式(4)的左側表示為：
[0038][0039]
式中，k表示某一離散時刻，δt表示採樣時間間隔。
[0040]
設在dvl每次採樣時間間隔δt內，sins/gnss組合導航系統解算更新n次，此時，對位置信息進行離散化處理得式(4)的右側為：
[0041][0042]
式中，
[0043]
得出基於位置信息的刻度因子誤差s表達式為：
[0044][0045]
進一步，所述步驟四中的安裝誤差角的標定包括：
[0046]
利用式(4)和式(9)，得到基於位置信息的安裝誤差角計算表達式為：
[0047]
[0048]
令：
[0049][0050]
此時，式(10)將變換為：
[0051][0052]
由於和均為已知量，dvl安裝誤差角標定問題轉化為求解兩個向量組之間旋轉矩陣的wahba問題，求解旋轉矩陣r使得目標函數f最小：
[0053][0054]
式中，ωk為量測向量對應的權值，ωk＝1。
[0055]
採用davenport四元數方法對旋轉矩陣r進行求解。
[0056]
在式(13)中，由於常數對目標函數的最小化不產生任何影響，進行線性變換後得到：
[0057][0058]
由於和與自身的內積為1且r
t
r＝i，故對式(14)展開得到：
[0059][0060]
忽略常數項，目標函數f化簡為：
[0061][0062]
由於矩陣的跡運算對目標函數不產生影響，根據矩陣跡的性質得到：
[0063][0064]
式中，
[0065]
旋轉矩陣r用四元數形式表示為：
[0066]
r＝(q
02-q
t
q)i+2qq
t-2q0[q
×
]
ꢀꢀ
(18)
[0067]
式中，q0和q分別為四元數的標量部分和矢量部分，目標函數改寫為：
[0068][0069]
根據矩陣跡的性質，對式(19)作進一步運算：
[0070][0071]
對tr([q
×
]a
t
)展開得：
[0072][0073]
令：
[0074][0075]
則式(21)化簡為：
[0076]
tr([q
×
]a
t
)＝-a
tqꢀꢀ
(23)
[0077]
將式(23)帶入式(20)得：
[0078]
min f(q)＝-(q
02-q
t
q)tr(a)-q
t
(a+a
t
)q-2q0a
tqꢀꢀ
(24)
[0079]
設tr(a)＝ρ且a+a
t
＝b，式(24)變換為：
[0080]
min f(q)＝-(q
t
(b-ρi)q+q0a
t
q+q0q
t
a+q
02
ρ)
ꢀꢀ
(25)
[0081]
根據矩陣運算法則，式(25)改寫為線性形式：
[0082][0083]
式中，q為矢量部分在前的四元數形式的姿態矩陣，d為davenport矩陣。
[0084]
目標函數f化簡為min f(q)＝-q
t
dq且q
t
q＝1，採用拉格朗日乘數法對目標函數f最小化問題進行求解，則：
[0085]
min f(q,λ)＝-q
t
dq+λ(q
t
q-1)
ꢀꢀ
(27)
[0086]
對方程進行求導得：
[0087]
dq＝λq
ꢀꢀ
(28)
[0088]
因此，q為d的對應於特徵值λ的特徵向量，此時姿態四元數目標函數f最小化問題轉化為求解矩陣d的最大特徵值對應的特徵向量的問題，在求出q的基礎上，轉換為歐拉角得到所標定出的dvl安裝誤差角。
[0089]
本發明的另一目的在於提供一種應用所述的dvl標定方法的dvl標定系統，所述dvl標定系統包括：
[0090]
模型構建模塊，用於構建dvl誤差模型；
[0091]
參考速度確定模塊，用於將sins/gnss組合導航結果作為b系參考速度；
[0092]
刻度因子誤差標定模塊，用於基於都卜勒測速原理的位置觀測運算以標定刻度因
子誤差；
[0093]
安裝誤差角標定模塊，用於基於davenport四元數法求解位置觀測矢量方程以標定安裝誤差角；
[0094]
dvl標定模塊，用於根據計算結果對dvl的量測輸出進行標定。
[0095]
本發明的另一目的在於提供一種計算機設備，所述計算機設備包括存儲器和處理器，所述存儲器存儲有電腦程式，所述電腦程式被所述處理器執行時，使得所述處理器執行所述的dvl標定方法的步驟。
[0096]
本發明的另一目的在於提供一種計算機可讀存儲介質，存儲有電腦程式，所述電腦程式被處理器執行時，使得所述處理器執行所述的dvl標定方法的步驟。
[0097]
本發明的另一目的在於提供一種信息數據處理終端，所述信息數據處理終端用於實現所述的dvl標定系統。
[0098]
結合上述的技術方案和解決的技術問題，本發明所要保護的技術方案所具備的優點及積極效果為：
[0099]
第一，針對上述現有技術存在的技術問題以及解決該問題的難度，緊密結合本發明的所要保護的技術方案以及研發過程中結果和數據等，詳細、深刻地分析本發明技術方案如何解決的技術問題，解決問題之後帶來的一些具備創造性的技術效果。具體描述如下：
[0100]
本發明提出了一種基於位置觀測信息的davenport四元數方法以實現對dvl刻度因子誤差和安裝誤差角的標定。在刻度因子標定過程中，通過基於都卜勒測速的位置運算減小了量測噪聲對測速精度的不利影響；在安裝誤差角標定過程中，通過基於davenport四元數的方法計算位置觀測矢量方程增強了旋轉矩陣求解的穩定性。利用標定後的參數結果對dvl的原始量測輸出進行補償，即可得到準確的dvl速度信息。
[0101]
本發明針對dvl刻度因子和安裝誤差角嚴重影響sins/dvl組合系統導航精度的問題，本發明提出了一種基於位置觀測信息的davenport四元數方法，並將其應用於dvl誤差標定系統；通過船載湖試試驗驗證了方法的有效性，簡單和複雜不同的機動條件下，所提標定方法在準確度和穩定性方面均有提升，具有良好的工程應用價值，並可進一步應用於組合導航領域。
[0102]
第二，把技術方案看做一個整體或者從產品的角度，本發明所要保護的技術方案具備的技術效果和優點，具體描述如下：
[0103]
本發明通過船載湖試試驗對比分析了該標定方法與其他現有方法的標定效果，結果表明，在簡單和複雜不同的機動情況下，該方法速度標定結果的穩定性和準確度更高；同時，利用標定補償後的dvl速度量測輸出進行sins/dvl組合導航，該方法得到的載體位置誤差更小。
[0104]
第三，作為本發明的權利要求的創造性輔助證據，還體現在以下幾個重要方面：
[0105]
(1)本發明的技術方案轉化後的預期收益和商業價值為：
[0106]
本發明的基於位置觀測信息davenport四元數標定方法通過技術方案轉化後，與現階段常規dvl標定方法相比，按照「先標定刻度因子誤差，再標定安裝誤差角」的思路，實現了dvl主要測速誤差的標定，有效提升了標定系統的穩定性並提高了最終標定精度
[0107]
(2)本發明的技術方案解決了人們一直渴望解決、但始終未能獲得成功的技術難題：
[0108]
長期以來，都卜勒速度計程儀作為水下運載體一種重要的測速手段，但是其測速精度很大程度上依賴於預先標定的效果，準確穩定的dvl標定是水下導航領域長期面臨的重難點問題，將都卜勒測速進行位置運算標定刻度因子誤差並通過davenport四元數方法解算姿態旋轉矩陣標定安裝誤差角，可解決長期以來始終未能解決的dvl標定問題。
[0109]
(3)本發明的技術方案克服了技術偏見：
[0110]
本發明克服了長期以來有外界信息輔助條件下同時標定刻度因子誤差和安裝誤差角的慣性思維，按照「先標定刻度因子誤差，再標定安裝誤差角」的思路，指出了實際應用中dvl速度量測噪聲對標定精度的不利影響，並通過位置運算對其進行了有效抑制，同時通過davenport四元數方法增強了姿態旋轉矩陣求解過程中系統的穩定性，最終提高了dvl的標定精度。
附圖說明
[0111]
為了更清楚地說明本發明實施例的技術方案，下面將對本發明實施例中所需要使用的附圖做簡單的介紹，顯而易見地，下面所描述的附圖僅僅是本發明的一些實施例，對於本領域普通技術人員來講，在不付出創造性勞動的前提下還可以根據這些附圖獲得其他的附圖。
[0112]
圖1是本發明實施例提供的dvl標定方法流程圖；
[0113]
圖2是本發明實施例提供的dvl安裝誤差示意圖；
[0114]
圖3是本發明實施例提供的dvl誤差標定系統結構圖；
[0115]
圖4是本發明實施例提供的試驗平臺安裝示意圖；
[0116]
圖5是本發明實施例提供的簡單機動情況下的dvl原始輸出及對應軌跡圖；
[0117]
圖6是本發明實施例提供的複雜機動情況下的dvl輸出及對應軌跡圖；
[0118]
圖7是本發明實施例提供的簡單機動時右向速度標定誤差圖；
[0119]
圖8是本發明實施例提供的簡單機動時前向速度標定誤差圖；
[0120]
圖9是本發明實施例提供的簡單機動時上向速度標定誤差圖；
[0121]
圖10是本發明實施例提供的簡單機動時組合導航位置誤差對比圖；
[0122]
圖11是本發明實施例提供的複雜機動時右向速度標定誤差圖；
[0123]
圖12是本發明實施例提供的複雜機動時前向速度標定誤差圖；
[0124]
圖13是本發明實施例提供的複雜機動時上向速度標定誤差圖；
[0125]
圖14是本發明實施例提供的複雜機動時組合導航位置誤差對比圖。
具體實施方式
[0126]
為了使本發明的目的、技術方案及優點更加清楚明白，以下結合實施例，對本發明進行進一步詳細說明。應當理解，此處所描述的具體實施例僅僅用以解釋本發明，並不用於限定本發明。
[0127]
針對現有技術存在的問題，本發明提供了一種dvl標定方法、系統、介質、設備及終端，下面結合附圖對本發明作詳細的描述。
[0128]
一、解釋說明實施例。為了使本領域技術人員充分了解本發明如何具體實現，該部分是對權利要求技術方案進行展開說明的解釋說明實施例。
[0129]
如圖1所示，本發明實施例提供的dvl標定方法包括以下步驟：
[0130]
s101，構建dvl誤差模型；
[0131]
s102，將sins/gnss組合導航結果作為b系參考速度；
[0132]
s103，基於都卜勒測速原理的位置觀測運算以標定刻度因子誤差；
[0133]
s104，基於davenport四元數法求解位置觀測矢量方程以標定安裝誤差角；
[0134]
s105，根據計算結果對dvl的量測輸出進行標定。
[0135]
在水下環境中，dvl可以通過聲學都卜勒效應來測量載體相對於水底或者水流的速度，但在工程實際中，由於製作工藝、外界環境、安裝偏差等各種條件限制，導致dvl的測速誤差難以避免。本發明在對刻度因子誤差和安裝角誤差進行分析並建模的基礎上，首先，將sins/gnss組合導航得到的載體系速度作為參考值，然後，通過都卜勒測速的位置運算來標定刻度因子誤差，進一步，利用davenport四元數方法求解位置觀測矢量方程來標定安裝角誤差，最後，利用求得的誤差參數對dvl原始量測輸出進行實時補償，從而達到保證dvl測速信息高度準確的目的。
[0136]
作為優選實施例，本發明實施例提供的dvl標定方法具體包括以下步驟：
[0137]
一、dvl的測速誤差分析與建模
[0138]
定義坐標系：選取「東-北-天(e-n-u)」地理坐標係為導航坐標系，記為n系；選取「右-前-上」坐標係為載體坐標系，記為b系；dvl安裝坐標系記為d系；地心慣性坐標系記為i系；地球坐標系記為e系；計算導航坐標系記為n
′
系。
[0139]
在理想的安裝情況下，dvl安裝坐標系d系與載體坐標系b系的坐標軸應該是互相一致的，即安裝矩陣為單位陣i3×3，但在工程實踐中，由於技術工藝限制導致安裝誤差角難以避免，載體上dvl安裝誤差示意圖如圖2所示，其中，x
b-y
b-zb表示b系，x
d-y
d-zd表示d系。
[0140]
根據dvl的工作原理，其測速誤差模型可表示為：
[0141][0142]
式中，表示dvl的量測輸出值，s表示刻度因子誤差，表示b繫到d系的姿態變換矩陣，vb表示載體的真實速度，表示載體系相對於導航系的旋轉角速度在載體系上的投影，表示sins與dvl之間的杆臂矢量誤差。
[0143]
在實際工程應用中，由於sins和dvl之間通常緊密安裝且杆臂矢量可直接通過測量獲得並予以補償，因此，本發明實施例將忽略杆臂誤差的影響，式(1)可簡化為：
[0144][0145]
需要說明的是，刻度因子誤差s並非固定值，其取值會隨著水中聲波傳播速度的改變而變化，在不同溫度、鹽度、深度等情況下會存在小範圍波動，因此，為確保後續組合導航的精度，需對s進行估計。當b繫到d系的安裝誤差角ε為小角度時，姿態變換矩陣其中，(
·
×
)表示反對稱矩陣運算，定義ε＝[ε
x
,εy,εz]
t
，則有：
[0146]
[0147]
由式(2)可知，當載體真實速度vb已知時，僅有dvl誤差為待求項，本發明實施例中，vb由sins和gnss組合導航的結果求得，此時dvl測速誤差模型可以進一步表示為：
[0148][0149]
基於上述分析，dvl的誤差標定轉化為求解式(4)中刻度因子誤差和安裝誤差角的問題，當求出相應未知參數後，對dvl量測輸出進行補償以便得出準確的b系速度用於進行sins/dvl水下組合導航解算。
[0150]
二、基於dp-p標定方法基本原理
[0151]
2.1坐標系定義
[0152]
1.慣性坐標系(o-xiyizi，i系)
[0153]
坐標原點為地心，xi軸指向地球春分點，zi軸指向地球北極且重合於地球自轉軸，yi軸位於地球赤道平面內且與xi、zi軸構成右手坐標系。慣性坐標系各坐標軸相對慣性空間的指向不發生改變。
[0154]
2.地球坐標系(o-xeyeze，e系)
[0155]
坐標原點為地心，xe軸指向地球格林尼治子午線，ze軸與i系zi軸重合，ye軸位於地球赤道平面內且與xe、ze軸構成右手坐標系。地球坐標系始終與地球固連，會隨著地球轉動。
[0156]
3.導航坐標系(o
n-x
nynzn
，n系)
[0157]
坐標原點為運動載體的質心，根據各坐標軸指向選取的不同可分為「東-北-天」坐標系和「北-東-地」坐標系。本發明實施例中，統一選取「東-北-天」坐標系記為n系。
[0158]
4.載體坐標系(o
b-xbybzb，b系)
[0159]
坐標原點為運動載體的質心，xb軸、yb軸和zb軸分別指向載體的右向、前向和上向，構成「右-前-上」右手坐標系。
[0160]
5.dvl坐標系(o
d-xdydzd，d系)
[0161]
d系與b系在理論上完全重合，但在工程實際中，由於dvl的安裝誤差導致d系與b系之間存在一定偏差。
[0162]
2.2dvl誤差標定系統結構
[0163]
在所提的dvl誤差標定系統中，當auv處於水面航行狀態可接收gnss信號時，利用kalman濾波最優估計方法進行sins/gnss組合導航，得到載體高精度的姿態和速度信息作為參考值，然後綜合dvl測量的d系下的速度值，通過標定算法估計出刻度因子誤差和安裝誤差角並對dvl輸出進行補償。dvl誤差標定系統結構如圖3所示。
[0164]
2.3刻度因子誤差的標定
[0165]
由於姿態旋轉矩陣的模值為1，即姿態旋轉矩陣與向量相乘時，只會改變向量的方向但不改變向量的大小，因此，同時對式(4)兩側進行取模可得：
[0166][0167]
此時，在dvl測量得到載體d系速度的基礎上，可以得出：
[0168][0169]
但需要指出的是，在利用式(6)計算刻度因子誤差s時，由於直接使用dvl輸出的量測值會引入量測噪聲誤差，進而降低了刻度因子誤差的標定精度。因此，為進一步消除dvl量測噪聲的影響，同時對式(4)兩側進行位置運算，此時，式(4)的左側可表示為：
[0170][0171]
式中，k表示某一離散時刻，δt表示採樣時間間隔。
[0172]
設在dvl每次採樣時間間隔δt內，sins/gnss組合導航系統解算更新n次，此時，對位置信息進行離散化處理可得式(4)的右側為：
[0173][0174]
式中，
[0175]
因此，可以得出基於位置信息的刻度因子誤差s表達式為：
[0176][0177]
2.4安裝誤差角的標定
[0178]
利用式(4)和式(9)，可以得到基於位置信息的安裝誤差角計算表達式為：
[0179][0180]
令：
[0181][0182]
此時，式(10)將變換為：
[0183][0184]
由式(12)可知，和均為已知量，因此，dvl安裝誤差角標定問題轉化為求解兩個向量組之間旋轉矩陣的wahba問題，即求解旋轉矩陣r使得目標函數f最小：
[0185][0186]
式中，ωk為量測向量對應的權值，在本發明標定問題中，ωk＝1。
[0187]
為快速準確的完成dvl安裝誤差角標定，採用davenport四元數方法對旋轉矩陣r進行求解。
[0188]
在式(13)中，由於常數對目標函數的最小化不產生任何影響，對其進行線性變換
後可以得到：
[0189][0190]
由於和與其自身的內積為1且r
t
r＝i，因此，對式(14)展開得到：
[0191][0192]
忽略常數項，目標函數f可以進一步化簡為：
[0193][0194]
由於矩陣的跡運算對目標函數不產生影響，根據矩陣跡的性質可以得到：
[0195][0196]
式中，
[0197]
旋轉矩陣r可用四元數形式表示為：
[0198]
r＝(q
02-q
t
q)i+2qq
t-2q0[q
×
]
ꢀꢀ
(18)
[0199]
式中，q0和q分別為四元數的標量部分和矢量部分，此時，目標函數可改寫為：
[0200][0201]
根據矩陣跡的性質，對式(19)作進一步運算：
[0202][0203]
對於tr([q
×
]a
t
)而言，將其展開可得：
[0204][0205]
令：
[0206][0207]
則式(21)可化簡為：
[0208]
tr([q
×
]a
t
)＝-a
tqꢀꢀ
(23)
[0209]
將式(23)帶入式(20)可得：
[0210]
min f(q)＝-(q
02-q
t
q)tr(a)-q
t
(a+a
t
)q-2q0a
tqꢀꢀ
(24)
[0211]
設tr(a)＝ρ且a+a
t
＝b，式(24)可變換為：
[0212]
min f(q)＝-(q
t
(b-ρi)q+q0a
t
q+q0q
t
a+q
02
ρ)
ꢀꢀ
(25)
[0213]
根據矩陣運算法則，式(25)可改寫為線性形式：
[0214][0215]
式中，q為矢量部分在前的四元數形式的姿態矩陣，d為davenport矩陣。
[0216]
基於上述推導，目標函數f化簡為min f(q)＝-q
t
dq且q
t
q＝1，採用拉格朗日乘數法對目標函數f最小化問題進行求解，則：
[0217]
min f(q,λ)＝-q
t
dq+λ(q
t
q-1)
ꢀꢀ
(27)
[0218]
對方程進行求導得：
[0219]
dq＝λq
ꢀꢀ
(28)
[0220]
因此，q為d的對應於特徵值λ的特徵向量。此時，姿態四元數目標函數f最小化問題轉化為求解矩陣d的最大特徵值對應的特徵向量的問題，在求出q的基礎上，將其轉換為歐拉角即為所標定出的dvl安裝誤差角。利用基於位置觀測信息的davenport四元數進行dvl誤差標定的流程如圖1所示。
[0221]
本發明實施例提供的dvl標定系統包括：
[0222]
模型構建模塊，用於構建dvl誤差模型；
[0223]
參考速度確定模塊，用於將sins/gnss組合導航結果作為b系參考速度；
[0224]
刻度因子誤差標定模塊，用於基於都卜勒測速原理的位置觀測運算以標定刻度因子誤差；
[0225]
安裝誤差角標定模塊，用於基於davenport四元數法求解位置觀測矢量方程以標定安裝誤差角；
[0226]
dvl標定模塊，用於根據計算結果對dvl的量測輸出進行標定。
[0227]
二、應用實施例。為了證明本發明的技術方案的創造性和技術價值，該部分是對權利要求技術方案進行具體產品上或相關技術上的應用實施例。
[0228]
本發明為實現dvl誤差的精確標定，在分析並建立dvl誤差來源及模型的基礎上，提出了一種基於位置觀測信息的davenport四元數標定方法。在本發明中，首先對刻度因子誤差進行標定，通過基於都卜勒測速原理的位置觀測運算減小了速度量測噪聲的不利影響；然後對安裝誤差角進行標定，通過基於davenport四元數方法解算位置觀測矢量方程增強了旋轉矩陣求解的穩定性。本發明所提方法在載體簡單和複雜不同的機動情況下，均能實現dvl測速誤差的準確標定，並可進一步應用於水下多傳感器組合導航領域。
[0229]
三、實施例相關效果的證據。本發明實施例在研發或者使用過程中取得了一些積極效果，和現有技術相比的確具備很大的優勢，下面內容結合試驗過程的數據、圖表等進行描述。
[0230]
試驗驗證：為驗證本發明實施例提供的dvl標定方法的可行性和有效性，位某省某市木蘭湖開展了船載湖試試驗。船載導航設備包括cy-jg90j型捷聯慣性導航系統、pa600型dvl傳感器和gnss接收機，設備的參數如表1～表2所示。試驗過程中以高精度rtk-gnss與
sins進行組合後得到的結果作為參考基準。主要試驗設備實物圖以及試驗平臺安裝概況如圖4所示。
[0231]
表1 cy-jg90j型sins的性能參數
[0232][0233]
表2 pa600型dvl的性能參數
[0234][0235][0236]
此次船載試驗共採集12000秒實測數據，其中，0～1200秒為sins初始對準階段，以滿足後期高精度組合導航的基本需求。選取初始對準階段後的兩段1200秒試驗數據對不同機動條件下dvl標定效果進行驗證，所選兩段時間內dvl的原始輸出和對應的行進軌跡如圖5～圖6所示，其中，白色圖標表示軌跡起點，灰色圖標表示軌跡終點。
[0237]
1、簡單機動情況下的性能驗證
[0238]
利用圖5所示軌跡驗證載體在簡單機動情況下的dvl標定效果。為比較方法性能，分別利用svd標定方法、kf標定方法和本發明實施例提供的標定方法進行1200秒的標定試驗，利用標定結果對dvl測量輸出進行補償，並將補償後的結果與gnss/sins組合導航得到的dvl參考速度進行對比，得到三個方向的速度誤差結果如圖7～9所示。為方便表述，上述三種標定方法分別記為svd、kf和dp-p。
[0239]
從圖7～圖9可以看出，對右向速度而言，3種標定方法補償後的誤差相對於未標定數據均有了提升，svd和kf方法誤差基本穩定在0.1m
·
s-1
以內，dp-p方法誤差基本穩定在0.08m
·
s-1
以內且更加平滑；對前向速度而言，由於試驗載體行進過程中主要為前向速度且速度量測值不可避免地受外界噪聲影響，所以誤差結果波動較大，但相比於未標定和kf方法標定誤差，其他標定方法所得誤差更小；對上向速度而言，由於kf標定方法的天向通道發散，導致最終標定結果不可信，svd和dp-p方法標定誤差效果基本相當。總的來說，由於簡單
機動情況下的試驗載體近似作勻速直線運動，kf方法中部分狀態量的可觀測度較弱，因此導致kf方法的標定效果大幅下降，而其他標定方法的速度誤差均能基本穩定在0.1m
·
s-1
以內，能夠滿足後續組合導航的要求。
[0240]
為量化比較方法效果，選取1200秒內速度的最大絕對誤差和平均絕對誤差(mean absolute error，mae)作為性能評判指標，mae定義如式(29)所示，最終計算結果如表3所示。從中可以看出，當試驗載體進行簡單機動時，kf方法的標定結果不夠理想，僅有右向速度誤差優於未標定值，而svd和dp-p方法的速度標定誤差在各個方向上均優於未標定值，達到了速度標定的目的。
[0241][0242]
式中，n表示試驗時長，x(t)、分別表示t時刻速度的參考值和估計值。
[0243]
表3簡單機動時不同標定方法速度誤差標定結果
[0244][0245]
為進一步比較方法效果，利用svd和dp-p方法標定補償後的dvl量測輸出與sins通過標準卡爾曼濾波算法進行組合導航得到位置結果，並與kf標定方法進行對比以間接反映標定效果的優劣，試驗得到的位置誤差如圖10和表4所示。其中，位置誤差的計算公式如下：
[0246][0247]
式中，δp表示位置誤差，l表示緯度，λ表示經度。
[0248]
表4簡單機動時組合導航位置誤差的最大值與平均值(米)
[0249][0250]
由圖10可知，使用未標定的速度進行組合導航得到的軌跡發散嚴重，而通過標定技術對dvl輸出進行補償後再進行組合導航的軌跡均能較好地跟蹤參考軌跡，需要指出的是，使用kf方法得到的位置信息準確度較高，說明當試驗載體近似作勻速直線運動時，位置狀態量不同於安裝誤差角狀態量，其可觀測度並沒有受到影響。
[0251]
表4可以更加直觀地反映位置誤差結果，從中可以看出，相比於其他方法，dp-p方法的標定效果有了大幅提高且幅度均達到80％以上。試驗結果驗證了所提dp-p標定方法在試驗載體簡單機動情況下的有效性。
[0252]
2、複雜機動情況下的性能驗證
[0253]
利用圖6所示軌跡驗證載體在複雜機動情況下的dvl標定效果。利用不同標定方法
得到的dvl測量輸出與dvl參考速度進行比較，得到三個方向的速度誤差結果如圖11～13和表5所示。
[0254]
表5複雜機動時不同標定方法速度誤差標定結果
[0255][0256]
從圖11～13可以看出，當試驗載體作連續「s」形的複雜機動時，svd方法的標定效果出現了下滑，速度誤差曲線波動劇烈；kf方法中安裝誤差角狀態量的可觀測度增強，右向和前向速度的標定誤差結果明顯優於簡單機動情況，上向速度由於天向通道發散的原因，標定結果仍然不可信；dp-p方法標定精度優於其他方法。從表5可以看出，當試驗載體進行複雜機動時，與未標定的速度誤差相比，使用標定方法後的速度誤差均有不同程度的降低，但就整體而言，本發明實施例提供的dp-p方法的標定效果最佳。
[0257]
利用不同方法標定後的速度進行組合導航，得到的軌跡位置誤差如圖14和表6所示。
[0258]
表6複雜機動時組合導航位置誤差的最大值與平均值(米)
[0259][0260]
由圖14可知，使用未標定的速度進行組合導航得到的軌跡與參考軌跡存在較大偏差；使用svd方法標定補償後的速度進行組合導航，位置誤差隨著時間的推移而逐漸增大；使用kf和dp-p方法標定補償後的速度進行組合導航，1200秒內的位置誤差基本均能控制在20米以內。表6進一步說明了kf和dp-p方法在導航精度方面所存在的優勢，而相比而言，dp-p方法的精度更高，以位置誤差平均值為例，比kf方法提高了62.74％。試驗結果驗證了所提dp-p標定方法在試驗載體複雜機動情況下的有效性。
[0261]
應當注意，本發明的實施方式可以通過硬體、軟體或者軟體和硬體的結合來實現。硬體部分可以利用專用邏輯來實現；軟體部分可以存儲在存儲器中，由適當的指令執行系統，例如微處理器或者專用設計硬體來執行。本領域的普通技術人員可以理解上述的設備和方法可以使用計算機可執行指令和/或包含在處理器控制代碼中來實現，例如在諸如磁碟、cd或dvd-rom的載體介質、諸如只讀存儲器(固件)的可編程的存儲器或者諸如光學或電子信號載體的數據載體上提供了這樣的代碼。本發明的設備及其模塊可以由諸如超大規模集成電路或門陣列、諸如邏輯晶片、電晶體等的半導體、或者諸如現場可編程門陣列、可編程邏輯設備等的可編程硬體設備的硬體電路實現，也可以用由各種類型的處理器執行的軟體實現，也可以由上述硬體電路和軟體的結合例如固件來實現。
[0262]
以上所述，僅為本發明的具體實施方式，但本發明的保護範圍並不局限於此，任何
熟悉本技術領域的技術人員在本發明揭露的技術範圍內，凡在本發明的精神和原則之內所作的任何修改、等同替換和改進等，都應涵蓋在本發明的保護範圍之內。