都立立,邢傳璽,萬(wàn)志良,李聰穎

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

在水下偵察領(lǐng)域,對(duì)動(dòng)態(tài)目標(biāo)進(jìn)行跟蹤是至關(guān)重要的研究課題.傳統(tǒng)動(dòng)態(tài)目標(biāo)跟蹤技術(shù)需要將實(shí)時(shí)數(shù)據(jù)與已知先驗(yàn)數(shù)據(jù)進(jìn)行匹配才能實(shí)現(xiàn),但在實(shí)際匹配過(guò)程中容易導(dǎo)致大幅偏差.文中可在無(wú)先驗(yàn)信息[1]的情況下,即在未知水下環(huán)境中,UUV動(dòng)態(tài)目標(biāo)跟蹤在完成對(duì)自身位置定位與周圍環(huán)境建圖[2]的同時(shí),探測(cè)估計(jì)動(dòng)態(tài)目標(biāo)的軌跡與位姿,整個(gè)運(yùn)動(dòng)過(guò)程中一直對(duì)動(dòng)態(tài)目標(biāo)進(jìn)行跟蹤.前者為SLAM問(wèn)題,后者為目標(biāo)跟蹤問(wèn)題[3].

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

基于以上內(nèi)容,提出平方根容積卡爾曼濾波SLAM的UUV動(dòng)態(tài)目標(biāo)跟蹤算法.該算法將UUV自身位姿、特征地圖的估計(jì)和目標(biāo)位姿三者相互耦合構(gòu)成一個(gè)整體,確保了各個(gè)狀態(tài)之間的關(guān)聯(lián)性,并且隨機(jī)函數(shù)的均值和方差采用容積準(zhǔn)則下數(shù)值積分法計(jì)算,平方根因子對(duì)系統(tǒng)進(jìn)行更新.結(jié)果表明,提高了UUV在未知水下環(huán)境對(duì)自身定位與動(dòng)態(tài)目標(biāo)跟蹤能力.

1 同步定位地圖構(gòu)建和動(dòng)態(tài)目標(biāo)跟蹤問(wèn)題描述

1.1 SLAM問(wèn)題的概率描述

UUV在水下環(huán)境實(shí)現(xiàn)同步定位與建圖需要明確2個(gè)問(wèn)題：首先是機(jī)器人在什么地方,即定位;其次是機(jī)器人周圍環(huán)境是什么,即建圖.兩者的完成需要提取并組合側(cè)掃聲吶獲取的未知環(huán)境信息和UUV內(nèi)部的里程計(jì)和IMU單元獲取的自身位姿信息,在移動(dòng)的同時(shí)完成環(huán)境地圖的構(gòu)建并且持續(xù)的對(duì)自身的位置的預(yù)測(cè)、優(yōu)化和更新[8].圖1為SLAM系統(tǒng)結(jié)構(gòu)框圖.

圖1 SLAM系統(tǒng)結(jié)構(gòu)框圖

UUV運(yùn)動(dòng)模型、觀測(cè)模型為

(1)

Zk=H(Xk)+δk.

(2)

則SLAM數(shù)學(xué)描述為

(3)

(4)

(5)

1.2 同步定位地圖構(gòu)建和動(dòng)態(tài)目標(biāo)跟蹤問(wèn)題描述

在未知環(huán)境中,運(yùn)動(dòng)狀態(tài)下的UUV識(shí)別到目標(biāo)物并進(jìn)行跟蹤,整個(gè)過(guò)程需要將UUV自身位姿、環(huán)境地圖特征的估計(jì)和目標(biāo)位姿[9]3者建立對(duì)應(yīng)關(guān)系.因此要建立UUV和目標(biāo)物各自的運(yùn)動(dòng)方程和觀測(cè)方程,運(yùn)動(dòng)模型負(fù)責(zé)預(yù)測(cè)系統(tǒng)的狀態(tài),觀測(cè)模型則根據(jù)地圖特征點(diǎn)觀測(cè)信息和目標(biāo)物觀測(cè)信息對(duì)系統(tǒng)狀態(tài)進(jìn)行更新.圖2為UUV動(dòng)態(tài)目標(biāo)跟蹤的系統(tǒng)框圖.

該系統(tǒng)包含運(yùn)動(dòng)模型和觀測(cè)模型[9].系統(tǒng)運(yùn)動(dòng)模型為

(6)

Zk=H(Xk)+δk.

(7)

圖2 UUV動(dòng)態(tài)目標(biāo)跟蹤系統(tǒng)框圖

因此用概率估計(jì)來(lái)描述同步定位地圖構(gòu)建與動(dòng)態(tài)目標(biāo)跟蹤問(wèn)題為

(8)

(9)

2 無(wú)人水下航行器動(dòng)態(tài)目標(biāo)跟蹤算法

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

該算法對(duì)CKF算法中計(jì)算復(fù)雜的部分進(jìn)行改進(jìn),利用3階容積準(zhǔn)則選取m個(gè)相同權(quán)值的容積點(diǎn)[10]來(lái)近似計(jì)算,再利用數(shù)值積分法計(jì)算非線性方程模型的后驗(yàn)狀態(tài)估計(jì)平均值和方差,并對(duì)協(xié)方差矩陣的平方根因子進(jìn)行更新[10],減少了計(jì)算量,提高了系統(tǒng)的運(yùn)行效率和計(jì)算結(jié)果的精度.

利用三階容積準(zhǔn)則計(jì)算高斯加權(quán)積分

(10)

(11)

整個(gè)算法分為預(yù)測(cè)階段和更新階段,圖3為SRCKF-SLAM算法框圖.

2.2 基于平方根容積卡爾曼濾波SLAM的無(wú)人水下航行器動(dòng)態(tài)目標(biāo)跟蹤算法

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

圖3 SRCKF-SLAM算法框圖

圖4 SRCKF-SLAM目標(biāo)跟蹤算法流程圖

根據(jù)上述分析可得該算法的簡(jiǎn)要步驟為：

step 1 前一時(shí)刻到當(dāng)前時(shí)刻為例,喬列斯基分解誤差協(xié)方差陣Pk-1得

(12)

step 2 求解容積點(diǎn)矩陣[15]和其經(jīng)過(guò)狀態(tài)方程的容積點(diǎn)、再預(yù)測(cè)系統(tǒng)狀態(tài)

(13)

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

(14)

(15)

step 3 UUV位姿預(yù)測(cè)和誤差協(xié)方差矩陣的平方根因子預(yù)測(cè)

(16)

(17)

(18)

(19)

式中,mi表示探測(cè)到的環(huán)境特征;Rm和Rt表示誤差信息矩陣;Zmi(k|k-1)和Zt(k|k-1)表示觀測(cè)數(shù)據(jù)值;C(Zmi(k|k-1),Zmi(k|k-1))和C(Zt(k|k-1),Zt(k|k-1))表示觀測(cè)數(shù)據(jù)值對(duì)應(yīng)的誤差信息矩陣;查χ2表獲取γ.

step 5 再進(jìn)行觀測(cè)更新

(20)

(21)

step 6 計(jì)算卡爾曼增益矩陣Wk

(22)

(23)

(24)

(25)

(26)

step 7 更新系統(tǒng)狀態(tài)信息矩陣、誤差協(xié)方差平方根因子[17]

(27)

(28)

3 仿真實(shí)驗(yàn)及分析

3.1 仿真環(huán)境及參數(shù)設(shè)置

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

仿真環(huán)境2 使用AMD Ryzen 5 4600 H with Radeon Graphics處理器,3.00 GHz 主頻,16 GB 的RAM計(jì)算機(jī),在開(kāi)源仿真平臺(tái)上,建立UUV目標(biāo)跟蹤仿真實(shí)驗(yàn)空間,目標(biāo)從坐標(biāo)(0,0)開(kāi)始,沿確定的軌跡運(yùn)行,UUV參數(shù)設(shè)置和仿真環(huán)境一相同,在整個(gè)仿真實(shí)驗(yàn)過(guò)程中UUV在完成同步定位與地圖構(gòu)建的同時(shí)對(duì)目標(biāo)保持追蹤狀態(tài).

對(duì)改進(jìn)的SRCKF-SLAM-OT算法在以上2種仿真平臺(tái)中仿真,UUV完成自身定位和對(duì)環(huán)境信息建圖的同時(shí),對(duì)動(dòng)態(tài)目標(biāo)一直保持跟蹤狀態(tài),同時(shí)也和UKF-SLAM-OT、CKF-SLAM-OT這2種算法做了對(duì)比,比較這3種算法在定位精度和目標(biāo)跟蹤能力的差異程度.

3.2 仿真結(jié)果及性能分析

在2種仿真環(huán)境中,應(yīng)用改進(jìn)的SRCKF-SLAM-OT算法實(shí)現(xiàn)對(duì)動(dòng)態(tài)目標(biāo)跟蹤實(shí)驗(yàn),與目標(biāo)實(shí)際路徑進(jìn)行比較、計(jì)算均方根誤差進(jìn)一步分析算法的性能,算法仿真結(jié)果如圖5、6所示.

仿真結(jié)果表明,在降低計(jì)算復(fù)雜度和優(yōu)化改進(jìn)得到的SRCKF-SLAM-OT算法對(duì)目標(biāo)物的跟蹤軌跡基本吻合,反映出該算法實(shí)時(shí)性高和對(duì)目標(biāo)物軌跡估計(jì)精度很高,但隨著維度升高特征點(diǎn)采樣數(shù)增多,算法估計(jì)路徑和動(dòng)態(tài)目標(biāo)實(shí)際軌跡偏差會(huì)增大.

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

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

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

3.3 3種算法對(duì)比分析

在仿真平臺(tái)中,對(duì)SRCKF-SLAM-OT、UKF-SLAM-OT和CKF-SLAM-OT 3種算法分別在二維平面和三維空間中仿真,得到動(dòng)態(tài)目標(biāo)真實(shí)路徑和3種算法目標(biāo)估計(jì)路徑,如圖7、8所示.

由圖可以看到實(shí)直線軌跡、虛線軌跡、雙畫(huà)線軌跡和點(diǎn)畫(huà)線軌跡分別表示目標(biāo)物真實(shí)軌跡、UKF、CKF和SRCKF算法目標(biāo)估計(jì)軌跡.無(wú)論在二維平面還是三維空間中,3種算法的軌跡從設(shè)置的初始位置開(kāi)始,均朝目標(biāo)物運(yùn)動(dòng)方向進(jìn)行跟蹤,UKF的動(dòng)態(tài)目標(biāo)跟蹤軌跡和實(shí)際軌跡的偏差[18]最大,其次是CKF的動(dòng)態(tài)目標(biāo)跟蹤軌跡的偏差,而SRCKF算法動(dòng)態(tài)目標(biāo)跟蹤軌跡與目標(biāo)真實(shí)軌跡基本吻合,表明了該算法提高了UUV對(duì)動(dòng)態(tài)目標(biāo)物的跟蹤能力.

(a)三種算法跟蹤結(jié)果 (b)局部放大圖圖7 3種算法動(dòng)態(tài)目標(biāo)跟蹤結(jié)果(二維)

(a)三種算法三維跟蹤軌跡 (b)局部放大圖圖8 3種算法動(dòng)態(tài)目標(biāo)跟蹤結(jié)果(三維)

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

在仿真平臺(tái)中,對(duì)SRCKF-SLAM-OT、UKF-SLAM-OT和CKF-SLAM-OT 3種算法分別在二維平面和三維空間中進(jìn)行目標(biāo)在x和y方向的誤差對(duì)比分析,如圖9所示.

(a)目標(biāo)在x方向誤差對(duì)比(二維) (b)目標(biāo)在y方向誤差對(duì)比(二維)

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

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

(2) 均方根誤差分析

在仿真平臺(tái)中,對(duì)SRCKF-SLAM-OT、UKF-SLAM-OT和CKF-SLAM-OT 3種算法分別在二維平面和三維空間中,計(jì)算位置均方根和速度均方根來(lái)評(píng)價(jià)三種算法的估計(jì)效果,如圖10所示.

(29)

(30)

由圖可以看出無(wú)論在二維平面還是三維空間中,位置均方根誤差和速度均方根誤差[20]均有SRCKF-SLAM-OT算法均方根誤差最小,其次是CKF-SLAM-OT算法均方根誤差,UKF-SLAM-OT算法均方根誤差最大.表明了采用改進(jìn)的SRCKF-SLAM-OT算法明顯提升了對(duì)動(dòng)態(tài)目標(biāo)估計(jì)的精度.

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

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

4 結(jié)語(yǔ)

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

改進(jìn)的SRCKF-SLAM-OT算法在隨著系統(tǒng)維度的升高,對(duì)目標(biāo)估計(jì)軌跡與目標(biāo)實(shí)際軌跡偏差增大的問(wèn)題,將是今后研究的重點(diǎn).