基于CKF-SLAM改進的無人水下航行器動態目標跟蹤算法研究

都立立,邢傳璽,萬志良,李聰穎

(云南民族大學 電氣信息工程學院,云南 昆明 650500)

在水下偵察領域,對動態目標進行跟蹤是至關重要的研究課題.傳統動態目標跟蹤技術需要將實時數據與已知先驗數據進行匹配才能實現,但在實際匹配過程中容易導致大幅偏差.文中可在無先驗信息[1]的情況下,即在未知水下環境中,UUV動態目標跟蹤在完成對自身位置定位與周圍環境建圖[2]的同時,探測估計動態目標的軌跡與位姿,整個運動過程中一直對動態目標進行跟蹤.前者為SLAM問題,后者為目標跟蹤問題[3].

目前在動態目標跟蹤的問題上,主要集中在基于卡爾曼濾波類[4]方法.擴展卡爾曼濾波(extended Kalman filter,EKF)[5]SLAM算法,是一種最小均方差估計方法,將非線性系統利用泰勒級數一階展開線性化,再進行線性卡爾曼濾波.EKF-SLAM算法簡單易于實現并且在未知環境特征點較少時,UUV實際軌跡可以逼近理想的軌跡,但當未知環境中的特征點增加時,UUV實際軌跡和理想軌跡偏差較大,因此該算法只適合應用在環境特征點少的小區域環境.無跡卡爾曼濾波(unscented Kalman filter,UKF)[6]SLAM算法,該算法利用高斯隨機變量來近似表達狀態分布,用一組確定性采樣得到的Sigma點來逼近,提高變量的平均值和方差,精度逼近二階泰勒.UKF-SLAM算法在精度上有所提高,計算復雜度上因無須計算雅克比矩陣,因此更容易實現,但同樣不適用于龐大復雜的環境.容積卡爾曼濾波(cubature Kalman filter,CKF)[7]SLAM算法,該算法是在三階球面-容積變換規則下,提取容積樣本點集,用來描述后驗狀態變量的統計特征,并由先驗狀態估計平均值和方差.CKF-SLAM算法的容積樣本點集的權重比非負,保證在計算高斯積分時濾波性能和數值穩定,但也存在算法實時性不高、計算量大以及在面對動態目標物跟蹤精度較低的問題.

基于以上內容,提出平方根容積卡爾曼濾波SLAM的UUV動態目標跟蹤算法.該算法將UUV自身位姿、特征地圖的估計和目標位姿三者相互耦合構成一個整體,確保了各個狀態之間的關聯性,并且隨機函數的均值和方差采用容積準則下數值積分法計算,平方根因子對系統進行更新.結果表明,提高了UUV在未知水下環境對自身定位與動態目標跟蹤能力.

1 同步定位地圖構建和動態目標跟蹤問題描述

1.1 SLAM問題的概率描述

UUV在水下環境實現同步定位與建圖需要明確2個問題：首先是機器人在什么地方,即定位;其次是機器人周圍環境是什么,即建圖.兩者的完成需要提取并組合側掃聲吶獲取的未知環境信息和UUV內部的里程計和IMU單元獲取的自身位姿信息,在移動的同時完成環境地圖的構建并且持續的對自身的位置的預測、優化和更新[8].圖1為SLAM系統結構框圖.

圖1 SLAM系統結構框圖

UUV運動模型、觀測模型為

(1)

Zk=H(Xk)+δk.

(2)

則SLAM數學描述為

(3)

(4)

(5)

1.2 同步定位地圖構建和動態目標跟蹤問題描述

在未知環境中,運動狀態下的UUV識別到目標物并進行跟蹤,整個過程需要將UUV自身位姿、環境地圖特征的估計和目標位姿[9]3者建立對應關系.因此要建立UUV和目標物各自的運動方程和觀測方程,運動模型負責預測系統的狀態,觀測模型則根據地圖特征點觀測信息和目標物觀測信息對系統狀態進行更新.圖2為UUV動態目標跟蹤的系統框圖.

該系統包含運動模型和觀測模型[9].系統運動模型為

(6)

Zk=H(Xk)+δk.

(7)

圖2 UUV動態目標跟蹤系統框圖

因此用概率估計來描述同步定位地圖構建與動態目標跟蹤問題為

(8)

(9)

2 無人水下航行器動態目標跟蹤算法

2.1 平方根容積卡爾曼濾波SLAM算法

該算法對CKF算法中計算復雜的部分進行改進,利用3階容積準則選取m個相同權值的容積點[10]來近似計算,再利用數值積分法計算非線性方程模型的后驗狀態估計平均值和方差,并對協方差矩陣的平方根因子進行更新[10],減少了計算量,提高了系統的運行效率和計算結果的精度.

利用三階容積準則計算高斯加權積分

(10)

(11)

整個算法分為預測階段和更新階段,圖3為SRCKF-SLAM算法框圖.

2.2 基于平方根容積卡爾曼濾波SLAM的無人水下航行器動態目標跟蹤算法

該算法流程圖如圖4所示,處理過程分為3步：預測、數據關聯和更新[11].預測階段將誤差協方差矩陣分解,計算容積點信息矩陣和先驗概率估計,并對當前時刻系統狀態和誤差協方差矩陣的平方根因子預測[12],最終推算UUV和目標的位姿信息.數據關聯包含3種關系：外部傳感器單元獲取的信息和UUV自身傳感器單元獲取數據[13]之間關系;UUV自身位姿狀態和環境地圖特征之間關系;環境地圖特征數據之間的關系.同時數據關聯階段解決兩方面問題：分別計算出UUV和目標的觀測值,并和其實際觀測值比較;篩選和剔除可能存在的偽觀測值.更新階段是在觀測信息通過數據關聯環節后計算觀測向量后驗概率和更新系統狀態信息、誤差協方差平方根因子[14].

圖3 SRCKF-SLAM算法框圖

圖4 SRCKF-SLAM目標跟蹤算法流程圖

根據上述分析可得該算法的簡要步驟為：

step 1 前一時刻到當前時刻為例,喬列斯基分解誤差協方差陣Pk-1得

(12)

step 2 求解容積點矩陣[15]和其經過狀態方程的容積點、再預測系統狀態

(13)

式中,(i=1,2,…,m,m=2n).

(14)

(15)

step 3 UUV位姿預測和誤差協方差矩陣的平方根因子預測

(16)

(17)

(18)

(19)

式中,mi表示探測到的環境特征;Rm和Rt表示誤差信息矩陣;Zmi(k|k-1)和Zt(k|k-1)表示觀測數據值;C(Zmi(k|k-1),Zmi(k|k-1))和C(Zt(k|k-1),Zt(k|k-1))表示觀測數據值對應的誤差信息矩陣;查χ2表獲取γ.

step 5 再進行觀測更新

(20)

(21)

step 6 計算卡爾曼增益矩陣Wk

(22)

(23)

(24)

(25)

(26)

step 7 更新系統狀態信息矩陣、誤差協方差平方根因子[17]

(27)

(28)

3 仿真實驗及分析

3.1 仿真環境及參數設置

仿真環境1 使用AMD Ryzen 5 4600H with Radeon Graphics處理器,3.00 GHz 主頻,16 GB 的RAM計算機,目標初始位置設為xp=[0,0,0,0],循環次數N=200,蒙特卡洛次數MC=300,濾波器個數為1,系統初始狀態的方差和均值為P_0=diag([1×105,10,1×105,10]),x=[100,15,100,15],量測模型設置為sigma_r(1)=100,sigma_b(1)=80×e-3,這里僅說明了在二維平面的參數.UUV速度和控制速度分別為 3 m/s、0.3 m/s,轉角誤差為 2 rad,輪間距為 4 m.

仿真環境2 使用AMD Ryzen 5 4600 H with Radeon Graphics處理器,3.00 GHz 主頻,16 GB 的RAM計算機,在開源仿真平臺上,建立UUV目標跟蹤仿真實驗空間,目標從坐標(0,0)開始,沿確定的軌跡運行,UUV參數設置和仿真環境一相同,在整個仿真實驗過程中UUV在完成同步定位與地圖構建的同時對目標保持追蹤狀態.

對改進的SRCKF-SLAM-OT算法在以上2種仿真平臺中仿真,UUV完成自身定位和對環境信息建圖的同時,對動態目標一直保持跟蹤狀態,同時也和UKF-SLAM-OT、CKF-SLAM-OT這2種算法做了對比,比較這3種算法在定位精度和目標跟蹤能力的差異程度.

3.2 仿真結果及性能分析

在2種仿真環境中,應用改進的SRCKF-SLAM-OT算法實現對動態目標跟蹤實驗,與目標實際路徑進行比較、計算均方根誤差進一步分析算法的性能,算法仿真結果如圖5、6所示.

仿真結果表明,在降低計算復雜度和優化改進得到的SRCKF-SLAM-OT算法對目標物的跟蹤軌跡基本吻合,反映出該算法實時性高和對目標物軌跡估計精度很高,但隨著維度升高特征點采樣數增多,算法估計路徑和動態目標實際軌跡偏差會增大.

(a)路徑跟蹤(二維) (b)路徑跟蹤(三維) (c)RMS位置分析(二維)

(d)RMS位置分析(三維) (e)RMS速率分析(二維) (f)RMS速率分析(二維)圖5 仿真環境1跟蹤軌跡和均方根誤差

(a)跟蹤軌跡 (b)均方根誤差分析圖6 仿真環境2跟蹤軌跡和均方根誤差

3.3 3種算法對比分析

在仿真平臺中,對SRCKF-SLAM-OT、UKF-SLAM-OT和CKF-SLAM-OT 3種算法分別在二維平面和三維空間中仿真,得到動態目標真實路徑和3種算法目標估計路徑,如圖7、8所示.

由圖可以看到實直線軌跡、虛線軌跡、雙畫線軌跡和點畫線軌跡分別表示目標物真實軌跡、UKF、CKF和SRCKF算法目標估計軌跡.無論在二維平面還是三維空間中,3種算法的軌跡從設置的初始位置開始,均朝目標物運動方向進行跟蹤,UKF的動態目標跟蹤軌跡和實際軌跡的偏差[18]最大,其次是CKF的動態目標跟蹤軌跡的偏差,而SRCKF算法動態目標跟蹤軌跡與目標真實軌跡基本吻合,表明了該算法提高了UUV對動態目標物的跟蹤能力.

(a)三種算法跟蹤結果 (b)局部放大圖圖7 3種算法動態目標跟蹤結果(二維)

(a)三種算法三維跟蹤軌跡 (b)局部放大圖圖8 3種算法動態目標跟蹤結果(三維)

(1) 定位與跟蹤誤差分析

在仿真平臺中,對SRCKF-SLAM-OT、UKF-SLAM-OT和CKF-SLAM-OT 3種算法分別在二維平面和三維空間中進行目標在x和y方向的誤差對比分析,如圖9所示.

(a)目標在x方向誤差對比(二維) (b)目標在y方向誤差對比(二維)

(c)目標在x方向誤差對比(三維) (d)目標在x方向誤差對比(三維)圖9 在x、y方向誤差對比

由圖可以看到無論在二維平面還是三維空間中,在x方向和y方向上均有SRCKF-SLAM-OT算法誤差最小,其次是CKF-SLAM-OT算法誤差,UKF-SLAM-OT算法誤差[19]最大.表明了采用改進的SRCKF-SLAM-OT算法,提高了UUV自身定位精度和對動態目標跟蹤能力.

(2) 均方根誤差分析

在仿真平臺中,對SRCKF-SLAM-OT、UKF-SLAM-OT和CKF-SLAM-OT 3種算法分別在二維平面和三維空間中,計算位置均方根和速度均方根來評價三種算法的估計效果,如圖10所示.

(29)

(30)

由圖可以看出無論在二維平面還是三維空間中,位置均方根誤差和速度均方根誤差[20]均有SRCKF-SLAM-OT算法均方根誤差最小,其次是CKF-SLAM-OT算法均方根誤差,UKF-SLAM-OT算法均方根誤差最大.表明了采用改進的SRCKF-SLAM-OT算法明顯提升了對動態目標估計的精度.

(a)位置均方根誤差分析(二維) (b)速度均方根誤差分析(二維)

(c) 速度均方根誤差分析(三維) (d) 位置均方根誤差分析(三維)圖10 位置、速度的均方根誤差對比

4 結語

為解決容積卡爾曼濾波SLAM算法存在的算法實時性不高、計算量大以及對動態目標物跟蹤精度較低的問題,提出SRCKF-SLAM的無人水下航行器動態目標跟蹤算法.該算法將UUV、環境地圖特征和動態目標物三者構成一個整體,更具體的是對UUV位姿的估計、環境地圖特征的估計和目標位姿的估計這三者互相耦合.建立UUV與動態目標各自的運動模型和觀測模型,得到的信息經過預測階段、數據關聯階段和更新階段,并在更新階段傳遞系統協方差矩陣的平方根因子.在3種算法跟蹤軌跡仿真、定位與跟蹤誤差分析和均方根誤差分析結果中,一致表明采用SRCKF-SLAM-OT算法,減小了計算復雜度,提高了UUV自身定位精度和動態目標跟蹤能力,在水下偵察領域應用意義巨大.

改進的SRCKF-SLAM-OT算法在隨著系統維度的升高,對目標估計軌跡與目標實際軌跡偏差增大的問題,將是今后研究的重點.