基于最優(yōu)鄰居引導(dǎo)螢火蟲(chóng)移動(dòng)的粒子濾波算法

盧敏 ， 陳菘 ， 張敏 ,2

（1. 江西理工大學(xué)理學(xué)院，江西 贛州 341000；2. 嘉興學(xué)院數(shù)理與信息工程學(xué)院，浙江 嘉興 314001）

0 引 言

二戰(zhàn)時(shí)期，Von Neumann 提出了蒙特卡羅方法(Monte Carlo Method)，該方法的主要思想是從概率分布中隨機(jī)抽樣并用這些抽樣值來(lái)近似表示原分布[1]。 貝葉斯估計(jì)結(jié)合先驗(yàn)概率函數(shù)與觀測(cè)似然函數(shù)，利用貝葉斯公式來(lái)求解估計(jì)問(wèn)題[2]。粒子濾波算法使用遞歸貝葉斯估計(jì)和蒙特卡羅方法實(shí)現(xiàn)了對(duì)估計(jì)問(wèn)題的求解，將系統(tǒng)狀態(tài)的處理轉(zhuǎn)化成序列的估計(jì)問(wèn)題[3]。 由于粒子濾波算法可以處理任意狀態(tài)空間描述的模型[4]，因此獲得了廣泛的應(yīng)用。除用于估計(jì)問(wèn)題外，粒子濾波算法在視覺(jué)跟蹤、航空導(dǎo)航、通信與信號(hào)處理、手勢(shì)識(shí)別、故障檢查等[5-8]領(lǐng)域也發(fā)揮著重要作用。

粒子濾波算法需要從后驗(yàn)分布中抽樣，并用這些抽樣值求解后驗(yàn)分布的期望、 方差等數(shù)字特征。但通常情況是不知道分布函數(shù)的具體解析式，無(wú)法直接采樣，為此引入了提議分布。 提議分布形式已知且與分布函數(shù)近似，對(duì)提議分布的采樣間接實(shí)現(xiàn)了對(duì)后驗(yàn)分布的采樣。最優(yōu)提議分布融入了當(dāng)前觀測(cè)數(shù)據(jù)，但實(shí)際采樣中無(wú)法實(shí)現(xiàn)，因此用狀態(tài)轉(zhuǎn)移函數(shù)作為次優(yōu)提議分布。次優(yōu)提議分布導(dǎo)致了粒子退化問(wèn)題（Particles Degeneracy）[9]，當(dāng)似然函數(shù)位于先驗(yàn)概率分布尾部時(shí)，退化問(wèn)題更為嚴(yán)重。 粒子退化現(xiàn)象產(chǎn)生了大量低權(quán)重粒子，低權(quán)重粒子無(wú)助于后驗(yàn)分布的估計(jì)，浪費(fèi)了大量的計(jì)算時(shí)間。 退化現(xiàn)象的存在使得少量粒子無(wú)法保證估計(jì)的精度，需要采集更多粒子實(shí)現(xiàn)對(duì)狀態(tài)的估計(jì)。 Douceta 通過(guò)數(shù)學(xué)方法證明了若干輪迭代后粒子退化現(xiàn)象發(fā)生的必然性[10]，在自舉濾波器[11]（Bootstrap Filter）提出以后，退化問(wèn)題得以解決。 自舉濾波器根據(jù)權(quán)值大小復(fù)制粒子，用大權(quán)值粒子替換小權(quán)值粒子。 這種機(jī)制導(dǎo)致粒子種類銳減，出現(xiàn)了粒子多樣性缺失的新問(wèn)題。

解決粒子多樣性缺失的方法有改進(jìn)重采樣算法或改進(jìn)提議分布[12]。 蔡登禹等用遺傳算法替代了粒子濾波的重采樣過(guò)程，結(jié)合變異、交叉等步驟，提高了濾波的精度[13]。 但是這種方法參數(shù)較多，參數(shù)的設(shè)置有很強(qiáng)的隨機(jī)性。張威虎等將粒子群優(yōu)化算法引入到粒子濾波過(guò)程中，通過(guò)蝴蝶個(gè)體香味濃度的高低和吸引半徑引導(dǎo)種群向最優(yōu)個(gè)體附近移動(dòng)[14]，該算法運(yùn)行時(shí)間較短，跟蹤誤差較小，但存在個(gè)體被多次吸引和移動(dòng)的問(wèn)題。本文將螢火蟲(chóng)算法引入到粒子濾波中，把粒子比作螢火蟲(chóng)個(gè)體，粒子的權(quán)值比作螢火蟲(chóng)發(fā)光的亮度。 為避免文獻(xiàn)[14]粒子在最優(yōu)個(gè)體值附近震蕩的問(wèn)題，本文引入遞減函數(shù)[15]，該函數(shù)使吸引度隨著迭代次數(shù)的增加而不斷減小。由于螢火蟲(chóng)算法中每個(gè)螢火蟲(chóng)都要與其他螢火蟲(chóng)個(gè)體進(jìn)行對(duì)比，亮度低于其他個(gè)體時(shí)均需要進(jìn)行移動(dòng)尋優(yōu)操作，當(dāng)種群數(shù)目過(guò)多時(shí)影響算法的運(yùn)行效率與收斂速度。 為減少算法的復(fù)雜度，本文利用最優(yōu)鄰居引導(dǎo)螢火蟲(chóng)個(gè)體的移動(dòng)。設(shè)置半徑參數(shù)將螢火蟲(chóng)種群進(jìn)行劃分，個(gè)體向所屬種群中亮度最高的螢火蟲(chóng)移動(dòng)完成位置更新，不同種群的個(gè)體再向最優(yōu)種群的最亮個(gè)體移動(dòng)并分布在最亮個(gè)體周圍。此操作加快了迭代尋優(yōu)的速度并保證了螢火蟲(chóng)種群的多樣性，種群的多樣性提高了濾波的準(zhǔn)確度。 最后實(shí)驗(yàn)驗(yàn)證本文改進(jìn)的算法在跟蹤誤差、跟蹤精度方面優(yōu)于粒子濾波算法。

1 粒子濾波模型

1.1 模型分析

非線性系統(tǒng)的狀態(tài)模型可以用式（1）和式（2）表示：

其中，xk與zk分別表示k 時(shí)刻的狀態(tài)量與觀測(cè)量；ω 與v 分別表示過(guò)程噪聲和測(cè)量噪聲；f 與h 分別表示狀態(tài)轉(zhuǎn)移函數(shù)與測(cè)量函數(shù)。 圖1 是式（1）和式（2）的狀態(tài)空間模型。

圖1 狀態(tài)空間模型

由圖1 可以看出： ①狀態(tài) xk與狀態(tài)xk-1相關(guān)，與 xk-2，xk-3，…等狀態(tài)不相關(guān)。 ②zk與狀態(tài) xk有關(guān)，與 zk-1，zk-2，…不相關(guān)，與 xk-1，xk-2，…也不相關(guān)。

在 k-1 時(shí)刻，p（x0∶k-1｜z1∶k-1）已知，根據(jù)式（1），并利用 Chapman-Kolmogorov 方程，由 p（xk-1｜z1∶k-1）得到 p（xk｜z1∶k-1）：

當(dāng)最新時(shí)刻觀測(cè)值到來(lái)時(shí)，利用最新時(shí)刻的觀測(cè)值 zk修正 p（xk｜z1∶k-1）。

其中，p（zk｜z1∶k-1）為歸一化常數(shù)。

引入提議分布后，權(quán)重更新公式為：

通過(guò)從提議分布中采樣獲得樣本， 并根據(jù)式（5）計(jì)算樣本權(quán)重即實(shí)現(xiàn)了粒子濾波算法。圖2 為粒子濾波過(guò)程的示意圖。

圖2 粒子濾波過(guò)程

1.2 重采樣算法

重采樣基于逆變換的思想，粒子歸一化權(quán)值與總權(quán)值的比例關(guān)系決定了粒子被復(fù)制的概率。目前常采用的重采樣算法有多項(xiàng)式重采樣、殘差重采樣和系統(tǒng)重采樣[16]。 用式（6）衡量有效粒子數(shù)退化的程度：

其中,vap（w）是所有粒子權(quán)重的方差。

式（6）計(jì)算復(fù)雜，通常采用另一種形式來(lái)近似表示：

其中，wi是粒子的歸一化權(quán)值。 完全退化情況下權(quán)重值完全集中在某個(gè)粒子上，此時(shí)Neff值為1。最佳情況是所有粒子權(quán)重值相等 （重采樣后所有粒子權(quán)重值相等，為 1/N），此時(shí) Neff=N。 使用式（7）來(lái)衡量粒子退化的程度，當(dāng)有效粒子數(shù)小于設(shè)定的閾值時(shí)需要進(jìn)行重采樣。 重采樣過(guò)程如下：首先產(chǎn)生一個(gè)[0，1]之間均勻分布的隨機(jī)數(shù)ui，然后計(jì)算粒子的累計(jì)權(quán)值概率之和時(shí)，則粒子xm被選中復(fù)制一次，重復(fù)N 次獲得所需的樣本數(shù)。

2 群智能優(yōu)化理論

2.1 螢火蟲(chóng)算法

群智能算法 （Swarm Intelligence Algorithm）屬于自然啟發(fā)式算法，可以更好地解決大規(guī)模復(fù)雜非線性問(wèn)題。 該算法主要模擬了昆蟲(chóng)、鳥(niǎo)群等動(dòng)物的聚集行為，群體中的成員通過(guò)協(xié)同操作不斷優(yōu)化搜尋的方向從而達(dá)到求最優(yōu)解的目的。目前常見(jiàn)的群智能算法有蟻群算法、蛙跳算法和螢火蟲(chóng)算法等。

螢火蟲(chóng)算法有2 個(gè)版本，firefly 算法和glowworm算法[17]。 效果類似、功能相近，本文采用 firefly 算法。 在firefly 算法中，螢火蟲(chóng)依靠自身所發(fā)光的亮度與對(duì)其他螢火蟲(chóng)個(gè)體的吸引度大小完成位置更新。 亮度與吸引度均與距離有關(guān)， 隨著距離的增加亮度與吸引度均減小。 位置更新的原則是亮度低的螢火蟲(chóng)會(huì)被亮度高的螢火蟲(chóng)吸引，亮度公式定義如下：

其中，L0表示個(gè)體自身的亮度；γ∈[0.1，2]為光強(qiáng)系數(shù)，γ；rij表示距離，公式如下：

螢火蟲(chóng)之間的吸引度βij定義為：

其中，β0為螢火蟲(chóng)的最大吸引度，通常取[0.8，1]。

位置更新公式定義如下：

其中，rand 是介于 0 到 1 間的隨機(jī)數(shù)；α 是步長(zhǎng)因子；β 是式（10）中計(jì)算得到的相對(duì)吸引度。

2.2 改進(jìn)算法

由式（8）和式（10）可知，螢火蟲(chóng)的亮度和吸引度大小與距離負(fù)相關(guān)。迭代前期螢火蟲(chóng)個(gè)體間距離遠(yuǎn)，吸引度小，導(dǎo)致位置更新步驟中移動(dòng)的距離短，需要進(jìn)行多次迭代才能移動(dòng)到最優(yōu)個(gè)體附近。迭代后期螢火蟲(chóng)個(gè)體間距離近，相互間的吸引度增大導(dǎo)致每次移動(dòng)的距離大，容易出現(xiàn)在最優(yōu)值附近反復(fù)震蕩的問(wèn)題。 另一方面，粒子濾波過(guò)程中次優(yōu)提議分布 p（xk｜xk-1）忽略了最新時(shí)刻的觀測(cè)值數(shù)據(jù)，導(dǎo)致了粒子退化現(xiàn)象的產(chǎn)生。因此本文主要從以下三個(gè)方面對(duì)螢火蟲(chóng)算法進(jìn)行改進(jìn)：

1） 在螢火蟲(chóng)相對(duì)亮度的計(jì)算公式中引入最新的觀測(cè)值數(shù)據(jù)。 修改后的亮度計(jì)算公式如下：

其中，R 表示測(cè)量噪聲方差；zpre表示由式（1）得到的預(yù)測(cè)值；z 表示由式（2）得到的觀測(cè)值。 L 值越大說(shuō)明該螢火蟲(chóng)個(gè)體亮度高，位置優(yōu)，對(duì)應(yīng)的粒子權(quán)值大，越接近真實(shí)的位置。

2）在螢火蟲(chóng)吸引公式中引入遞減函數(shù)δ，該函數(shù)控制吸引度的更新。 迭代初期δ 值應(yīng)比較大，加強(qiáng)算法的全局搜索能力，有利于低亮度的螢火蟲(chóng)個(gè)體迅速收斂到高亮度個(gè)體附近。迭代后期δ 值應(yīng)比較小，提高算法的局部搜索能力，避免低亮度螢火蟲(chóng)個(gè)體在高亮度螢火蟲(chóng)個(gè)體附近反復(fù)震蕩，無(wú)法收斂。 基于上述思路，δ 函數(shù)設(shè)置如下所示：

其中，k 為迭代次數(shù)， 一般設(shè)置為30 到100 之間，改進(jìn)后的吸引度公式如式（14）所示：

3）當(dāng)螢火蟲(chóng)種群規(guī)模N 很大時(shí)，個(gè)體在迭代尋優(yōu)的過(guò)程中需要與其他螢火蟲(chóng)個(gè)體兩兩比較然后進(jìn)行移動(dòng)。這個(gè)比較過(guò)程增加了運(yùn)算的復(fù)雜度，為此本文使用最優(yōu)鄰居引導(dǎo)螢火蟲(chóng)個(gè)體的移動(dòng)，有效減少了移動(dòng)的次數(shù)。 用圖3 中的圓形表示每個(gè)螢火蟲(chóng)個(gè)體，空心圓形代表比中心螢火蟲(chóng)亮度低的螢火蟲(chóng)個(gè)體，實(shí)心圓形代表比中心螢火蟲(chóng)亮度高的螢火蟲(chóng)個(gè)體，黑色實(shí)心圓形是亮度最高的螢火蟲(chóng)個(gè)體。

圖3 最優(yōu)鄰居吸引模型

首先需要確定中心螢火蟲(chóng)的半徑，計(jì)算過(guò)程如下：

式（15）通過(guò)計(jì)算中心螢火蟲(chóng)與其余螢火蟲(chóng)距離的平均值得到螢火蟲(chóng)i 的半徑，然后計(jì)算所有螢火蟲(chóng)個(gè)體 j（j≠i）與螢火蟲(chóng) i 的距離 dij。 如果 dij≤rand×diave，那么 j 確定為中心螢火蟲(chóng) i 的鄰居，不滿足式 （15）， 則該螢火蟲(chóng)個(gè)體不是中心螢火蟲(chóng)的鄰居。 以下分兩種情況討論：

情況1：螢火蟲(chóng)i 不存在鄰居時(shí)，即中心螢火蟲(chóng)為遠(yuǎn)離群體的獨(dú)立個(gè)體，此時(shí)i 直接向全局最優(yōu)處的螢火蟲(chóng)個(gè)體進(jìn)行移動(dòng)。 公式如下：

式（16）中： β 為吸引度大小，可由式（14）計(jì)算得到；Bk為全局最優(yōu)螢火蟲(chóng)個(gè)體的位置。

情況2：螢火蟲(chóng)i 存在鄰居時(shí)，由式（12）分別確定螢火蟲(chóng)i 和其鄰居螢火蟲(chóng)的亮度，并找出亮度最優(yōu)的個(gè)體。 若螢火蟲(chóng)i 比所有鄰居螢火蟲(chóng)亮度都高，說(shuō)明該螢火蟲(chóng)個(gè)體權(quán)重大，此輪過(guò)程不進(jìn)行移動(dòng)。 若鄰居螢火蟲(chóng)存在最亮個(gè)體，則螢火蟲(chóng)i 依據(jù)式（17）向最優(yōu)鄰居進(jìn)行移動(dòng)：

式（17）中：xk為鄰居中最亮螢火蟲(chóng)個(gè)體的位置。

綜上，本文改進(jìn)后的粒子濾波算法流程如下：

3 算法仿真

3.1 非線性方程模型及參數(shù)設(shè)置

實(shí)驗(yàn)環(huán)境如下：硬件配置為Intel i7 處理器，8 GB內(nèi)存，軟件配置為Matlab R2016b。

本實(shí)驗(yàn)相關(guān)參數(shù)設(shè)置如下：粒子數(shù)目為60，跟蹤時(shí)長(zhǎng)T 為50，步長(zhǎng)因子α 為常數(shù)，本文取0.3。最大吸引度β0為0.9，迭代次數(shù)k 為50。 初始種群個(gè)體滿足均值為0.1，方差為2 的高斯分布，量測(cè)噪聲與過(guò)程噪聲服從均值為0， 方差為1 的高斯分布。實(shí)驗(yàn)中所采用的非線性狀態(tài)模型與量測(cè)模型來(lái)自文獻(xiàn)[18]，如式（18）和式（19）所示：

式（18）和式（19）是一個(gè)非線性非高斯方程，后驗(yàn)分布具有多個(gè)峰值。 x（k）與 z（k）分別表示 k 時(shí)刻的狀態(tài)值與量測(cè)值，wk與vk是均值為0，方差分別為R=1 與Q=1 的高斯白噪聲。

3.2 實(shí)驗(yàn)結(jié)果分析

圖4、圖5 分別是兩種算法實(shí)驗(yàn)誤差的對(duì)比與跟蹤效果。

圖4 誤差分布

圖5 狀態(tài)估計(jì)

分析圖4 可得：在粒子數(shù)目與跟蹤時(shí)長(zhǎng)均相同的條件下，相比于粒子濾波算法，本文改進(jìn)算法誤差分布的峰值得到了有效降低，大誤差現(xiàn)象得到了一定程度的改善。這主要是改進(jìn)算法在亮度公式的計(jì)算中利用了最新時(shí)刻觀測(cè)值，相比于粒子濾波算法的次優(yōu)提議分布誤差得到了減小。從圖5 可以看出，本文改進(jìn)算法跟蹤效果有了顯著提高，大部分時(shí)刻都圍繞在真實(shí)值附近，而粒子濾波算法估計(jì)值與真實(shí)值之間出現(xiàn)了較大的偏差。這主要是本文改進(jìn)算法不對(duì)低權(quán)重粒子進(jìn)行舍棄，低權(quán)重粒子不斷向高權(quán)重粒子移動(dòng)從而提高了跟蹤的準(zhǔn)確性。

為了定量評(píng)價(jià)本文改進(jìn)算法的性能，采用均方根誤差（Root Mean Square Error, RMSE）作為標(biāo)準(zhǔn)計(jì)算真實(shí)值與預(yù)測(cè)值的偏差。均方根誤差RSME 公式如下所示：

取過(guò)程噪聲方差R 分別為1 和0.1，粒子數(shù)目分別為50 和100， 其余參數(shù)詳見(jiàn)3.1 節(jié)參數(shù)設(shè)置。為了排除實(shí)驗(yàn)的隨機(jī)性，減少偶然誤差，表1 的數(shù)據(jù)是在進(jìn)行100 次實(shí)驗(yàn)的基礎(chǔ)上取平均值得到的。

表1 均方根誤差數(shù)據(jù)

分析表1，當(dāng)噪聲方差相同時(shí)，隨著采樣粒子數(shù)目的增加，粒子濾波算法與本文改進(jìn)算法的均方根誤差均相應(yīng)減小。 以R=0.1 為例，粒子濾波算法的誤差隨著采樣數(shù)目的增加減小了6.7%， 本文改進(jìn)算法的誤差減小了9.2%。 增加采樣粒子數(shù)目可以減小狀態(tài)估計(jì)的誤差，但與之伴隨的是計(jì)算時(shí)間的增加，影響了算法的時(shí)效性。 在噪聲與粒子數(shù)目均相同的條件下，本文改進(jìn)算法誤差小于粒子濾波算法誤差，驗(yàn)證了改進(jìn)算法的性能。分析原因，粒子濾波重采樣過(guò)程對(duì)低權(quán)重粒子直接舍棄，高權(quán)重粒子的大量復(fù)制降低了有效粒子數(shù)目。而本文改進(jìn)算法對(duì)低權(quán)重粒子進(jìn)行移動(dòng)尋優(yōu)操作，使其分布在真實(shí)后驗(yàn)概率分布值附近，這個(gè)過(guò)程中沒(méi)有舍棄低權(quán)重粒子，降低了估計(jì)狀態(tài)與真實(shí)狀態(tài)之間的誤差。

3.3 二維目標(biāo)跟蹤模型

對(duì)于平面內(nèi)做勻速運(yùn)動(dòng)的載體， 可以使用位置、速度信息來(lái)描述其運(yùn)動(dòng)狀態(tài)。本文使用的狀態(tài)變量為X（k）=[xp（k），xv（k），yp（k），yv（k）]T。 其中，xp（k）、 xv（k）為載體水平方向的位置與速度，yp（k）、yv（k）為載體豎直方向的位置與速度。載體的狀態(tài)運(yùn)動(dòng)方程如式（21）所示：

其中，A 為狀態(tài)轉(zhuǎn)移矩陣，W（k）為系統(tǒng)模型噪聲。

其中，ax和 ay是服從的高斯白噪聲。

觀測(cè)站可以使用激光、無(wú)線電、紅外等儀器對(duì)運(yùn)動(dòng)載體的狀態(tài)X（k）進(jìn)行探測(cè)，探測(cè)得到的距離用 Z（k）來(lái)表示，探測(cè)噪聲用 V（k）來(lái)表示。 系統(tǒng)的觀測(cè)方程如式（22）所示：

3.4 仿真結(jié)果分析

圖6、圖7 是兩種算法的跟蹤效果圖與誤差對(duì)比，圖8 是時(shí)間消耗對(duì)比，圖9 是方差為1 的高斯噪聲。

圖6 跟蹤效果對(duì)比

圖7 誤差對(duì)比

圖8 時(shí)間對(duì)比

圖9 方差為1 時(shí)的高斯噪聲

圖6、圖7 是對(duì)二維平面內(nèi)的運(yùn)動(dòng)載體跟蹤了100 次所得出的。 對(duì)比來(lái)看，本文改進(jìn)算法的跟蹤效果優(yōu)于粒子濾波算法。 隨著跟蹤時(shí)長(zhǎng)的增加，粒子濾波算法誤差不斷增加， 而本文算法跟蹤誤差小于粒子濾波算法， 在20 到30 時(shí)刻之間誤差改善效果更為明顯。 但另一方面，本文算法的運(yùn)算時(shí)間明顯大于粒子濾波算法。 如圖8 所示，本文算法的時(shí)間消耗是粒子濾波的2～3 倍。 為了減小跟蹤的誤差， 這種以增加運(yùn)算時(shí)間來(lái)?yè)Q取跟蹤準(zhǔn)確性的操作是必然的。 重采樣算法只是簡(jiǎn)單地選中并復(fù)制粒子，造成了粒子種類的減少。 本文算法不對(duì)粒子進(jìn)行選擇與復(fù)制， 通過(guò)粒子的移動(dòng)來(lái)保證粒子種類的多樣性， 因此提高了跟蹤準(zhǔn)確性并減小了跟蹤誤差。

4 結(jié) 論

本文將螢火蟲(chóng)算法引入到粒子濾波過(guò)程中，改進(jìn)了螢火蟲(chóng)算法中相對(duì)亮度與吸引度的計(jì)算方法，在相對(duì)亮度的計(jì)算過(guò)程中利用了當(dāng)前時(shí)刻的觀測(cè)值數(shù)據(jù)。 在吸引度的計(jì)算中引入遞減函數(shù)，粒子在向最優(yōu)值靠近的過(guò)程中避免了在最優(yōu)值附近震蕩。 利用最優(yōu)個(gè)體指引螢火蟲(chóng)的移動(dòng)，降低了算法的復(fù)雜度， 最后通過(guò)仿真實(shí)驗(yàn)驗(yàn)證改進(jìn)算法的性能。 結(jié)果表明：

1） 粒子濾波算法提議分布的選取沒(méi)有融合當(dāng)前時(shí)刻的觀測(cè)值，當(dāng)似然函數(shù)遠(yuǎn)離提議分布的峰值時(shí)，無(wú)法保證狀態(tài)估計(jì)的準(zhǔn)確性。 本文改進(jìn)算法融合了當(dāng)前時(shí)刻的觀測(cè)值，提高了估計(jì)的精度。

2） 重采樣過(guò)程用大權(quán)重粒子替代小權(quán)重粒子，解決了粒子退化問(wèn)題，但是造成了樣本的匱乏。本文改進(jìn)算法中小權(quán)重粒子通過(guò)一定的規(guī)則向大權(quán)重粒子移動(dòng)， 從而分布在大權(quán)重粒子的周圍，保證了樣本的多樣性，從而減小了估計(jì)的誤差。

3） 通過(guò)對(duì)非線性過(guò)程的仿真得出本文改進(jìn)算法可以保證估計(jì)的精度， 具有一定的實(shí)用價(jià)值，下一步將研究改進(jìn)算法在智能跟蹤、導(dǎo)航等場(chǎng)景的具體應(yīng)用。