[發(fā)明專利]基于水下環(huán)境特征的AUV魯棒VBHIAKF-SLAM導航方法

有效

• 申請?zhí)枺?/td>: 202210044863.3
• 申請日: 2022-01-14
• 公開（公告）號: CN114370879B
• 公開（公告）日: 2023-03-10
• 發(fā)明（設(shè)計）人: 王立輝;翟鴻啟
• 申請（專利權(quán)）人: 東南大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06F17/11;G06F17/15;G06F17/16;G06F17/18
• 代理公司: 南京眾聯(lián)專利代理有限公司 32206
• 代理人: 蔣昱

本發(fā)明提出了基于水下環(huán)境特征的AUV魯棒VBHIAKF?SLAM導航方法，包括步驟：1、構(gòu)建水下三維空間環(huán)境下的EKF?SLAM系統(tǒng)模型；2、利用變分貝葉斯參數(shù)對量測噪聲協(xié)方差進行估計；3、采用Huber魯棒估計器對加權(quán)殘差向量的每一個維度進行檢測，并針對不同閾值范圍內(nèi)的維度賦予不同的權(quán)重值；4、多次迭代測量更新環(huán)節(jié)，根據(jù)迭代后的量測值對狀態(tài)估計誤差值進行逐步修正。該方法可以適用于量測噪聲時變且量測信息中出現(xiàn)連續(xù)野值點時的環(huán)境，有效提高水下航行器的定位精度，改善對環(huán)境干擾的適用性和魯棒性。

技術(shù)領(lǐng)域

本發(fā)明屬于基于特征的水下SLAM導航技術(shù)領(lǐng)域，具體為基于水下環(huán)境特征的AUV魯棒VBHIAKF-SLAM導航方法。

背景技術(shù)

高精度導航定位技術(shù)是水下航行器順利完成航行探測及作業(yè)任務(wù)的重要保障。地球物理場導航方法因具有隱蔽性好、自主性強、誤差不隨時間累積等優(yōu)點，近年來已成為水下導航領(lǐng)域的研究熱點。但是，物理場輔助導航實現(xiàn)高精度導航定位的前提是獲取高精度的信息豐富的先驗環(huán)境信息圖，而先驗物理場圖的獲取在有些場景下是很難實現(xiàn)的。SLAM定位技術(shù)無需先驗地圖，僅通過自身攜帶的傳感器不斷感知外界環(huán)境信息便可實現(xiàn)對機器人自身定位及環(huán)境特征位置的實時更新。而標準EKF-SLAM算法的濾波精度依賴于精確的系統(tǒng)模型和準確的噪聲統(tǒng)計先驗知識，AUV在水下作業(yè)時，量測噪聲的統(tǒng)計特性很難事先準確獲取，即使先驗統(tǒng)計已知，也很容易因強時變特性的內(nèi)部和外部的不確定性而改變，從而導致其濾波精度的下降，嚴重時甚至可能會引起濾波的發(fā)散。同時，根據(jù)已有的文獻，目前應(yīng)用于AUV的基于特征的水下SLAM算法大多數(shù)是基于二維平面空間環(huán)境下的。

發(fā)明內(nèi)容

為解決上述技術(shù)問題，本發(fā)明提出了基于水下環(huán)境特征的AUV魯棒VBHIAKF-SLAM導航方法，該方法首先基于二維平面模型構(gòu)建一種水下三維空間環(huán)境下的EKF-SLAM系統(tǒng)模型，并針對傳統(tǒng)EKF-SLAM模型因噪聲統(tǒng)計特性未知或時變且量測信息中出現(xiàn)連續(xù)野值點時濾波精度下降甚至發(fā)散的問題，提出一種魯棒VBHIAKF-SLAM自適應(yīng)濾波算法。

基于水下環(huán)境特征的AUV魯棒VBHIAKF-SLAM導航方法，包括：

(1)構(gòu)建水下三維空間環(huán)境下的EKF-SLAM系統(tǒng)模型；

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

(1-1)預(yù)測階段；

根據(jù)AUV的運動學特性，基于EKF-SLAM系統(tǒng)的狀態(tài)方程表示為：

式中，為系統(tǒng)的狀態(tài)向量，為預(yù)測階段的載體狀態(tài)，為地圖向量，為載體在當前時刻相對于前一時刻的位姿變化量，V_v是時間不相關(guān)的過程噪聲向量，其中，表示為：

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

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

因此，狀態(tài)方程被擴展為：

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

(1-2)量測更新階段

設(shè)當前時刻傳感器測量到一個已經(jīng)存儲在地圖中估計值為的特征點，則由坐標系間的變量關(guān)系可構(gòu)建出其量測方程為：

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

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