基于隨機信標的因子圖同時定位構(gòu)圖方法

王 凱，寧云暉，鄧福建，翟國威，劉 猛

(1. 天津航海儀器研究所，天津 300131；2. 海軍研究院，北京 100161)

0 引 言

隨著未來作戰(zhàn)系統(tǒng)的信息化程度不斷提高，水面無人艇（USV）因具有機動靈活、可替代人開展危險作業(yè)等特點，被廣泛用于執(zhí)行戰(zhàn)場偵察、情報收集、反水雷等高危險任務(wù)作業(yè)[1-2]。為了更好實現(xiàn)水面無人艇的作業(yè)，精確的導(dǎo)航定位也至關(guān)重要。目前，常用的導(dǎo)航方法有慣導(dǎo)、衛(wèi)導(dǎo)、長基線水聲定位、多普勒計程儀等，其中慣導(dǎo)誤差隨時間呈發(fā)散趨勢，衛(wèi)導(dǎo)信號脆弱，易受攻擊和壓制，信標導(dǎo)航由于無需衛(wèi)星信號即可實現(xiàn)精確定位而成為導(dǎo)航領(lǐng)域新的研究熱點。如文獻[3]采用單信標布置在水面船上，并利用衛(wèi)導(dǎo)數(shù)據(jù)對信標進行定位，水下無人航行器借助裝備的長基線系統(tǒng)測量相對于信標的距離，根據(jù)DVL和角度傳感器進行航位推算，分別研究了EKF和粒子濾波算法的導(dǎo)航定位性能，驗證了粒子濾波具有更高的定位精度；文獻[4]研究了如何在給定機器人運動軌跡時最優(yōu)布置信標位置，提高導(dǎo)航定位精度?；谝阎艠宋恢玫膶?dǎo)航算法簡單，導(dǎo)航誤差小，具有良好的收斂性，但需要提前標定信標位置，花費大量時間和人力物力，特別是在戰(zhàn)時或執(zhí)行緊急任務(wù)時，無法滿足對及時性的要求，限制了信標導(dǎo)航的應(yīng)用范圍。為減少系統(tǒng)啟動時間，有專家學(xué)者提出基于隨機信標的導(dǎo)航定位算法，為在一定區(qū)域內(nèi)執(zhí)行任務(wù)的無人艇通過飛機或母船布放一些信標，而不需要標定信標位置，實現(xiàn)導(dǎo)航定位和環(huán)境構(gòu)圖[5]。

目前，關(guān)于采用因子圖算法進行同時定位構(gòu)圖的研究很多。如文獻[6]采用滑動窗因子圖基于地圖上潛在路標位置進行三維路標地圖和全部導(dǎo)航狀態(tài)的非線性優(yōu)化估計，提高衛(wèi)星拒止環(huán)境下定位和路標位置估計精度；針對無人作戰(zhàn)平臺在室內(nèi)等復(fù)雜環(huán)境下的高精度定位問題，蔣小強等[7]采用因子圖模型對WLAN和單目視覺進行數(shù)據(jù)融合，從而獲得精確的定位和地圖信息；文獻[8]研究了VO和IMU融合因子圖模型，實現(xiàn)自動駕駛系統(tǒng)的即時定位和地圖構(gòu)建；Vadim Indelman等[9]把因子圖應(yīng)用于多源導(dǎo)航信息融合系統(tǒng)，將慣導(dǎo)、GPS和視覺傳感器進行組合，實現(xiàn)導(dǎo)航定位和環(huán)境地圖的構(gòu)建；文獻[10]將因子圖模型應(yīng)用于超寬帶/INS組合導(dǎo)航中，實現(xiàn)無人飛行器的室內(nèi)定位，相比于傳統(tǒng)濾波算法提高了定位精度。但如何采用水聲信息同時進行導(dǎo)航定位和信標位置估計的研究還較少。

本文提出一種基于隨機信標的因子圖同時定位構(gòu)圖算法，利用無人艇上裝備的DVL和羅經(jīng)測量無人艇移動速度和航向，通過測量無人艇相對于信標的距離，在無需標定信標位置的前提下，實現(xiàn)無人艇和信標的位置估計。

1 因子圖模型架構(gòu)設(shè)計

1.1 融合架構(gòu)

為簡化模型，減小計算量，將水深視為固定值，在母船布放無人艇時測定。在構(gòu)建基于隨機信標的因子圖同時定位構(gòu)圖模型中，變量節(jié)點包括無人艇的位置和航向、隨機信標的位置，量測因子節(jié)點包括DVL測量的速度信息、羅經(jīng)測量的航向信息和無人艇相對于信標的距離信息。設(shè)計如圖1所示因子圖模型架構(gòu)。

圖1 基于隨機信標的因子圖同時定位構(gòu)圖模型架構(gòu)Fig. 1 Simultaneous localization and mapping model architectureof factor graph based on random beacon

不同時刻之間的無人艇狀態(tài)變量節(jié)點由DVL和羅經(jīng)量測信息組合成的因子節(jié)點fvφ連接，無人艇狀態(tài)變量節(jié)點和路標位置變量節(jié)點經(jīng)信標因子節(jié)點連接。USV狀態(tài)變量節(jié)點包括無人艇的位置和航向，即xk=，定義從初始時刻t0到當前時刻tk的無人艇狀態(tài)變量節(jié)點集合為，路標位置變量節(jié)點集合定位為；定義從初始時刻t1到當前時刻tk測量的無人艇和信標距離的集合，式中m表示從時刻t1到時刻tk感知到的路標總數(shù)量。

根據(jù)圖1描述的各個變量間的概率密度函數(shù)關(guān)系，可知USV和信標位置變量的后驗概率密度函數(shù)如下：

基于最大后驗概率估計，可得無人艇和信標位置估計的表達式如下：

1.2 因子節(jié)點設(shè)計

USV運動學(xué)模型為：

式中：Px，Py表示位置；φ表示航向；vx，vy表示速度；ω表示轉(zhuǎn)向角速度。對式（3）進行離散化，可得根據(jù)DVL和羅經(jīng)量測進行狀態(tài)更新的離散化形式：

式中：xk為tk時刻真實狀態(tài)變量；xk?1為tk?1時刻真實狀態(tài)變量；T為離散時間間隔；vk?1為tk?1時刻真實速度；ωk?1為tk?1時刻真實轉(zhuǎn)向角速度。記tk時刻的狀態(tài)估計為，狀態(tài)估計誤差為δxk，則，記tk?1時刻的狀態(tài)估計為，狀態(tài)估計誤差為δxk?1，則為tk?1時刻DVL測量的速度，則，式中wvk?1為DVL量測噪聲，通?？梢暈楦咚拱自肼?，方差為σvk?1；為tk?1時刻羅經(jīng)測量的航向，則表示航向測量誤差；ωk?1為tk?1時刻真實轉(zhuǎn)向角速度，為tk?1時刻羅經(jīng)測量的轉(zhuǎn)向角速度，則，wωk?1為羅經(jīng)角速度測量噪聲，方差為σωk?1。

考慮DVL和羅經(jīng)存在測量誤差，記Pk?1為tk?1時刻狀態(tài)估計誤差的協(xié)方差矩陣，則定義DVL和羅經(jīng)因子節(jié)點模型為：

將式(5)在估計位置處作泰勒展開，忽略二次及以上項，可得：

記

DVL和羅經(jīng)的噪聲協(xié)方差矩陣為：

則式(6)可整理如下：

tk時刻水聲測量的水面艦船與第i個信標的距離為：

式中，wd為水聲測量噪聲，通常視為高斯白噪聲，方差為σd，記Rki為tk時刻測量的第i個信標的誤差協(xié)方差矩陣，則

定義信標量測因子節(jié)點模型為：

將式(9)在估計位置處作泰勒展開，忽略二次及以上項，記第i個信標的估計位置為，信標位置估計誤差為δli，可得：

將荸薺用大量清水洗去泥土，用去皮刀除去荸薺表面的褐色表皮，將去皮后的荸薺1 kg放置在5 L的玻璃容器中，加無菌水至沒過荸薺，浸泡5 min后濾除水分(浸泡時間過長，鮮切荸薺細胞嚴重失水)，反復(fù)浸泡清洗三次，濾干水分備用，該種去皮后的荸薺即為鮮切荸薺。

2 因子圖同時定位構(gòu)圖算法

根據(jù)上述分析，將式(2)在狀態(tài)變量和信標位置估計值處作泰勒展開并保留一次項，記變量節(jié)點估計誤差集合為δX=[δl1···δx1δx2···]T，并將泰勒展開式作標準化處理，然后構(gòu)造全局雅克比矩陣J和殘差項b，求解如下方程：

式中，各符號定義如下：

因子圖同時定位構(gòu)圖算法：

2) 當接收到DVL和羅經(jīng)測量信息時，創(chuàng)建因子節(jié)點fvφ并插入因子圖G中，同時創(chuàng)建變量節(jié)點xk插入變量節(jié)點X中；當接收到信標量測因子節(jié)點后，創(chuàng)建因子節(jié)點插入圖G中，創(chuàng)建變量節(jié)點xk和lm插 入變量節(jié)點X中；

3) 提取受新插入因子節(jié)點影響的子圖Gnew；

4) 根據(jù)量測因子節(jié)點計算變量節(jié)點的初始估計值；

5) 在變量節(jié)點估計值處線性化各因子節(jié)點建立雅克比矩陣和殘差向量；

6) 計算變量節(jié)點估計誤差并進行修正；

7) 計算殘差向量，若大于設(shè)定閾值，則轉(zhuǎn)步驟5)進行迭代，否則，完成當前導(dǎo)航狀態(tài)估計，轉(zhuǎn)步驟2),等待新量測因子的插入。

3 數(shù)值仿真驗證與分析

設(shè)定仿真區(qū)域1 000 m×1 000 m，隨機布放12個信標，采用DVL和羅經(jīng)測量速度和航向，測速誤差為0.2 m/s，刻度系數(shù)誤差3%，航向測量誤差10°/h，距離測量誤差5 m，位置初始誤差1 m，初始航向誤差1°。

設(shè)置圓形和蛇形2種運動軌跡進行數(shù)值仿真驗證，將因子圖算法與EKF濾波算法進行比較，仿真結(jié)果如圖2～圖7所示。

因子圖算法、EKF算法導(dǎo)航定位誤差和信標位置估計誤差如表1和表2所示。

從圖2～圖7及表1和表2可以看出，基于隨機信標，在圓形軌跡和蛇形軌跡2種情況下因子圖定位精度和信標位置估計精度均優(yōu)于傳統(tǒng)EKF濾波算法；圓形軌跡和蛇形軌跡仿真驗證中，因子圖算法相比EKF濾波算法定位精度分別提高37.6%和14.6%，平均信標位置估計誤差分別提高14.1%和13.1%。由理論分析可知，因子圖算法通過利用所有時刻量測信息，采用非線性最小二乘優(yōu)化算法進行導(dǎo)航狀態(tài)估計，EKF濾波算法只考慮當前時刻量測信息，采用非線性濾波算法進行導(dǎo)航狀態(tài)和信標位置估計，利用量測信息不充分且具有系統(tǒng)線性化誤差，因此，因子圖算法估計精度應(yīng)優(yōu)于EKF濾波算法，與數(shù)值仿真驗證結(jié)果一致。

圖2 圓形運動軌跡對比圖Fig. 2 Comparison chart of circular trajectory

圖3 圓形運動軌跡下X軸方向估計誤差Fig. 3 Estimation error of x-axis direction in circular trajectory

圖4 圓形運動軌跡下Y軸方向估計誤差Fig. 4 Estimation error of y-axis direction in circular trajectory

圖5 蛇形運動軌跡對比圖Fig. 5 Comparison chart of serpentine trajectory

圖6 蛇形運動軌跡下X軸方向估計誤差Fig. 6 Estimation error of x-axis direction in serpentine trajectory

4 結(jié) 語

本文針對戰(zhàn)時或執(zhí)行緊急任務(wù)時的導(dǎo)航定位問題，研究了一種基于隨機信標的因子圖同時定位構(gòu)圖算法，建立了DVL和羅經(jīng)因子節(jié)點、信標因子節(jié)點數(shù)學(xué)模型，構(gòu)建了因子圖架構(gòu)，同時對水面無人艇和信標的位置進行估計，并對該算法開展了數(shù)值仿真試驗驗證。仿真結(jié)果表明：因子圖算法克服傳統(tǒng)水聲定位必須提前標定信標位置的缺點，且比EKF濾波算法具有更好的定位精度和信標位置估計精度，為快速實現(xiàn)信標導(dǎo)航提供了一種導(dǎo)航方法。但是，由于本文只進行了理論分析與數(shù)值仿真，還有待進一步開展實船試驗進行驗證和完善。

圖7 蛇形運動軌跡下Y軸方向估計誤差Fig. 7 Estimation error of y-axis direction in serpentine trajectory

表1 因子圖算法、EKF算法定位誤差Tab. 1 Location error of factor graph and EKF

表2 因子圖算法、EKF算法信標位置誤差Tab. 2 Beacon location error of factor graph and EKF