[發明專利]基于水下環境特征的AUV魯棒VBHIAKF-SLAM導航方法

有效

• 申請號: 202210044863.3
• 申請日: 2022-01-14
• 公開（公告）號: CN114370879B
• 公開（公告）日: 2023-03-10
• 發明（設計）人: 王立輝;翟鴻啟
• 申請（專利權）人: 東南大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06F17/11;G06F17/15;G06F17/16;G06F17/18
• 代理公司: 南京眾聯專利代理有限公司 32206
• 代理人: 蔣昱

1.基于水下環境特征的AUV魯棒VBHIAKF-SLAM導航方法，包括，其特征在于：

(1)構建水下三維空間環境下的EKF-SLAM系統模型；

步驟(1)的具體方法如下：

(1-1)預測階段；

根據AUV的運動學特性，基于EKF-SLAM系統的狀態方程表示為：

式中，為系統的狀態向量，為預測階段的載體狀態，為地圖向量，為載體在當前時刻相對于前一時刻的位姿變化量，V_v是時間不相關的過程噪聲向量，其中，表示為：

式中，分別是AUV在全局坐標系下的位置和航向角；

若用代表第i個特征在全局坐標系下的位置，則地圖向量表示為：

因此，狀態方程被擴展為：

式中，分別為載體在當前時刻相對于前一時刻的位姿變化量；

(1-2)量測更新階段

設當前時刻傳感器測量到一個已經存儲在地圖中估計值為的特征點，則由坐標系間的變量關系可構建出其量測方程為：

若量測z與地圖中的第i個特征點正確關聯，則SLAM的Kalman濾波估計為：

S＝HP_a,k/k-1H^T+R

W＝P_a,k/k-1H^TS^-1

P_a,k＝P_a,k/k-1-WSW^T

其中，量測信息的雅可比矩陣H可以由下式獲得：

在這里，

(1-3)狀態擴充階段

當某個特征被第一次觀測到時，需要通過狀態擴充的過程將其加入到地圖中，使地圖隨載體航行區域的擴大實現自動擴展，其實現過程為：

首先將觀測值z轉換成全局直角坐標系下的特征點位置，其轉換函數為：

因此，擴充后的狀態函數由下式給出：

其中，其對應的協方差矩陣為：

式中，P_v、P_m、P_vm分別是載體的協方差矩陣，地圖的協方差矩陣和載體與地圖間的協方差矩陣，而雅可比矩陣表述為：

其中，雅可比矩陣和由下式求得：

故，協方差矩陣進一步表示為：

最終，通過上述公式將新觀測到的特征點擴充到狀態向量中，從而實現地圖的擴充和構建；(2)利用變分貝葉斯參數對量測噪聲方差進行估計；

步驟(2)的具體方法如下：

利用變分貝葉斯參數，估計量測噪聲協方差矩陣為：

其中，μ和η是Inverse-Gamma概率密度分布參數，其具體表示為：

μ_k,i＝μ_k-1,i+0.5

式中，i＝1,…,d，d為量測向量的維數；

(3)采用Huber魯棒估計器對加權殘差向量的每一個維度進行檢測，并針對不同閾值范圍內的維度賦予不同的權重值；

步驟(3)的具體方法如下：

若假定：

則系統模型表述為：

m_k＝G_kX_k+ξ_k

根據廣義極大似然估計方法進行求解，可以得到相應的迭代收斂解為：

式中，上標j表示迭代次數，迭代初始化

根據最終收斂的狀態所對應的ψ值得相應的狀態誤差協方差矩陣為：

這里，Ψ為權重函數，通過如下公式求出：

式中，γ為調節因子，e_k,i表示第i維的歸一化殘差；

由于矩陣G_k特殊的結構形式，通過應用矩陣求逆引理，進一步將狀態估計過程轉換為更一般的形式；

首先將權值矩陣Ψ作分塊化處理，用Ψ_x和Ψ_y分別代表狀態預測殘差和量測預測殘差：

其中，0_m×n表示m×n維零值矩陣；

經過推導整理，預測協方差和量測協方差矩陣表示為：

(4)多次迭代測量更新環節，根據迭代后的量測值對估計誤差逐步修正。

2.根據權利要求1所述的基于水下環境特征的AUV魯棒VBHIAKF-SLAM導航方法，其特征在于：步驟(4)的具體方法如下：

由上述推導可得，系統狀態的估計值和對應的協方差矩陣表示為：

式中，K_k為濾波增益，

迭代結束后的即為系統最終的狀態估計值和協方差矩陣，這里，n＝1,2…N為迭代次數，設置為5。