[發明專利]基于水下環境特征的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 | 代理人: | 蔣昱 |
| 地址: | 210096 *** | 國省代碼: | 江蘇;32 |
| 權利要求書: | 查看更多 | 說明書: | 查看更多 |
| 摘要: | |||
| 搜索關鍵詞: | 基于 水下 環境 特征 auv 魯棒 vbhiakf slam 導航 方法 | ||
1.基于水下環境特征的AUV魯棒VBHIAKF-SLAM導航方法,包括,其特征在于:
(1)構建水下三維空間環境下的EKF-SLAM系統模型;
步驟(1)的具體方法如下:
(1-1)預測階段;
根據AUV的運動學特性,基于EKF-SLAM系統的狀態方程表示為:
式中,為系統的狀態向量,為預測階段的載體狀態,為地圖向量,為載體在當前時刻相對于前一時刻的位姿變化量,Vv是時間不相關的過程噪聲向量,其中,表示為:
式中,分別是AUV在全局坐標系下的位置和航向角;
若用代表第i個特征在全局坐標系下的位置,則地圖向量表示為:
因此,狀態方程被擴展為:
式中,分別為載體在當前時刻相對于前一時刻的位姿變化量;
(1-2)量測更新階段
設當前時刻傳感器測量到一個已經存儲在地圖中估計值為的特征點,則由坐標系間的變量關系可構建出其量測方程為:
若量測z與地圖中的第i個特征點正確關聯,則SLAM的Kalman濾波估計為:
S=HPa,k/k-1HT+R
W=Pa,k/k-1HTS-1
Pa,k=Pa,k/k-1-WSWT
其中,量測信息的雅可比矩陣H可以由下式獲得:
在這里,
(1-3)狀態擴充階段
當某個特征被第一次觀測到時,需要通過狀態擴充的過程將其加入到地圖中,使地圖隨載體航行區域的擴大實現自動擴展,其實現過程為:
首先將觀測值z轉換成全局直角坐標系下的特征點位置,其轉換函數為:
因此,擴充后的狀態函數由下式給出:
其中,其對應的協方差矩陣為:
式中,Pv、Pm、Pvm分別是載體的協方差矩陣,地圖的協方差矩陣和載體與地圖間的協方差矩陣,而雅可比矩陣表述為:
其中,雅可比矩陣和由下式求得:
故,協方差矩陣進一步表示為:
最終,通過上述公式將新觀測到的特征點擴充到狀態向量中,從而實現地圖的擴充和構建;(2)利用變分貝葉斯參數對量測噪聲方差進行估計;
步驟(2)的具體方法如下:
利用變分貝葉斯參數,估計量測噪聲協方差矩陣為:
其中,μ和η是Inverse-Gamma概率密度分布參數,其具體表示為:
μk,i=μk-1,i+0.5
式中,i=1,…,d,d為量測向量的維數;
(3)采用Huber魯棒估計器對加權殘差向量的每一個維度進行檢測,并針對不同閾值范圍內的維度賦予不同的權重值;
步驟(3)的具體方法如下:
若假定:
則系統模型表述為:
mk=GkXk+ξk
根據廣義極大似然估計方法進行求解,可以得到相應的迭代收斂解為:
式中,上標j表示迭代次數,迭代初始化
根據最終收斂的狀態所對應的ψ值得相應的狀態誤差協方差矩陣為:
這里,Ψ為權重函數,通過如下公式求出:
式中,γ為調節因子,ek,i表示第i維的歸一化殘差;
由于矩陣Gk特殊的結構形式,通過應用矩陣求逆引理,進一步將狀態估計過程轉換為更一般的形式;
首先將權值矩陣Ψ作分塊化處理,用Ψx和Ψy分別代表狀態預測殘差和量測預測殘差:
其中,0m×n表示m×n維零值矩陣;
經過推導整理,預測協方差和量測協方差矩陣表示為:
(4)多次迭代測量更新環節,根據迭代后的量測值對估計誤差逐步修正。
2.根據權利要求1所述的基于水下環境特征的AUV魯棒VBHIAKF-SLAM導航方法,其特征在于:步驟(4)的具體方法如下:
由上述推導可得,系統狀態的估計值和對應的協方差矩陣表示為:
式中,Kk為濾波增益,
迭代結束后的即為系統最終的狀態估計值和協方差矩陣,這里,n=1,2…N為迭代次數,設置為5。
該專利技術資料僅供研究查看技術是否侵權等信息,商用須獲得專利權人授權。該專利全部權利屬于東南大學,未經東南大學許可,擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作,請聯系【客服】
本文鏈接:http://www.szxzyx.cn/pat/books/202210044863.3/1.html,轉載請聲明來源鉆瓜專利網。





