[發明專利]一種基于偏振光/慣性/視覺組合導航的水下同步定位與建圖方法

有效

• 申請號: 202110620366.9
• 申請日: 2021-06-03
• 公開（公告）號: CN113739795B
• 公開（公告）日: 2023-10-20
• 發明（設計）人: 夏琳琳;劉瑞敏;蒙躍;張晶晶
• 申請（專利權）人: 東北電力大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20;G01C21/32
• 代理公司: 吉林省中玖專利代理有限公司 22219
• 代理人: 李泉宏

1.一種基于偏振光/慣性/視覺組合導航的水下同步定位與建圖方法，其特征在于，該方法包括以下五個步驟：

步驟1：選取水下潛航器的姿態、速度及位置作為系統狀態，建立潛航器的運動學模型；

步驟2：通過偏振傳感器的定向算法，采用偏振態的最小二乘法計算太陽矢量，并確定太陽位置；

步驟3：通過拓撲節點識別算法，利用圖像偏振信息，分析視覺場景特征提取和匹配問題；

步驟4：根據慣性導航系統的速度輸出，建立速度慣性導航量測模型和偏振光非線性量測模型；

步驟5：整合上述步驟，通過數據融合建立組合導航系統模型，構建環境地圖。

2.一種基于偏振光/慣性/視覺組合導航的水下同步定位與建圖方法，其特征在于，步驟1中所建立潛航器的運動學模型的非線性方程為：

上式，及分別為潛航器的位置橫坐標、位置縱坐標、天頂處坐標及航向坐標，u為控制輸入向量，w為零均值高斯分布的過程噪聲向量。

3.一種基于偏振光/慣性/視覺組合導航的水下同步定位與建圖方法，其特征在于，步驟2中通過偏振傳感器的定向算法，采用偏振態的最小二乘法計算太陽矢量，并確定太陽位置；

上式，表示潛航器上探測器方向的航向角，表示元素角度差，為太陽位置的估計值；

由此根據估計值確定太陽位置：

上式，為太陽位置，t為測量時間，arcdist(·)為弧長。

4.一種基于偏振光/慣性/視覺組合導航的水下同步定位與建圖方法，其特征在于，步驟3：構建視覺場景環境特征地圖的步驟具體如下：

1)根據識別圖像的位置信息，確定目標樣本，即各節點中心對應區域的圖像樣本，并標注標號，在導航拓撲圖中，有n個節點共包含m個圖像樣本，各個節點的對應圖像目標記為：

上式，I_i為第i個圖像樣本的特征矢量，y_i∈[1,2,…,n]為節點的目標標號；

2)根據機器學習中大邊界近鄰算法，對相鄰拓撲節點中目標樣本進行訓練，得到新度量矩陣M，使相同標號的目標樣本更加緊湊、不同標號的目標樣本更加松散；

3)定義任意兩目標樣本間的距離度量為：

d_M(I_i,I_i+j)＝(I_i-I_i+j)^TM(I_i-I_i+j)

上式，d_M為在重構后的拓撲節點空間中兩節點的歐式距離，d_M(I_i,I_i+j)與度量矩陣呈線性關系，I_i和I_i+j為目標近鄰樣本；

4)利用度量矩陣M求解如下目標函數的最優解：

令y_il∈{0,1}表示樣本的特征向量是否屬于相同節點，若屬于即為1，否則為0；λ表示權重參數，ε_ijl為非目標樣本總數

5)由上述步驟得到拓撲圖對運動環境的表達和度量，完成構建視覺場景環境特征地圖。

5.一種基于偏振光/慣性/視覺組合導航的水下同步定位與建圖方法，其特征在于，步驟4：建立速度慣性導航量測和非線性狀態偏振光量測的組合模型如下：

組合導航系統的狀態模型為：

上式，F_IMU(t)、F_polar(t)表示狀態系數矩陣，G_IMU(t)、G_polar(t)表示系統噪聲驅動矩陣，W_IMU(t)、W_polar(t)表示系統噪聲矩陣；

組合導航系統的量測方程為：

上式，H_IMU(t)、H_polar(t)表示量測方程的非線性函數，U_IMU(t)、U_polar(t)分別表示慣性傳感器和偏振光傳感器的量測噪聲。

6.一種基于偏振光/慣性/視覺組合導航的水下同步定位與建圖方法，其特征在于，步驟5：通過AUKF-SLAM算法進行數據融合，建立組合導航系統模型，構建環境地圖的具體步驟如下：

1)在估計時變噪聲統計時，應逐漸忘記舊的數據，但是UKF算法不具備噪聲統計量的變化，所以改進該算法可以在未知和時變噪聲統計參數的情況下，使用衰減記憶加權指數法設計時變噪聲統計估計器，選取加權系數：

β_i＝β_i-1b,0b1,

可以寫成：

β_i＝d_kb^i-1,i＝1,…,k，

因此，衰減記憶時變噪聲統計估計器的遞推公式表述為：

上式，為系統噪聲協方差，為觀測噪聲協方差，K為卡爾曼增益，W_i為均值和協方差的權重，滿足UKF序列；

2)AUKF算法通過引入衰減因子公式計算自適應因子系數λ_i，然后利用該系數校正可預測項的協方差P_k|k-1，從而抑制濾波器的發散，確保濾波器的收斂性，其收斂條件為：

對于衰減因子公式計算出自適應因子系數λ_i：

上式，λ₀為初始值，tr(·)表示矩陣跟蹤，S為可調系數，為殘差序列；

該算法中因子λ_i值越大，產生的信息比例就越小，導致殘差向量效應v_k越顯著。

3)降噪之后，采用最近鄰數據關聯算法，對獲取的姿態、位置、速度及航向等進行數據關聯；若導航拓撲圖中的節點目標樣本相鄰，則進行合并，即關聯成功，此時潛航器將進行一次時間更新和量測更新；否則，將新的度量作為新特征添加到當前狀態向量，即進行狀態增廣，然后繼續執行相鄰目標樣本的合并即數據關聯，操作如下：

上式，是新特征向量。

經過以上的改進，對濾波器進行時間更新和量測更新：

時間更新：

量測更新：

上式，及分別為系統先前的狀態向量、協方差向量及k-1時刻狀態的測量向量，x_i,k∣k-1、z_i,k∣k-1為Sigma點采樣得到的值，W_i^(m)和W_i^(c)分別為系統均值和協方差的權值，Q_k-1為系統噪聲的協方差，和均為系統量測更新中獲取的觀測協方差，R為觀測噪聲的協方差，K_k為濾波器增益矩陣，和分別為系統當前的狀態向量和協方差向量。