基于隨機(jī)信標(biāo)的因子圖同時(shí)定位構(gòu)圖方法

王 凱，寧云暉，鄧福建，翟國(guó)威，劉 猛

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

0 引 言

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

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

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

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

1.1 融合架構(gòu)

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

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

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

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

基于最大后驗(yàn)概率估計(jì)，可得無人艇和信標(biāo)位置估計(jì)的表達(dá)式如下：

1.2 因子節(jié)點(diǎn)設(shè)計(jì)

USV運(yùn)動(dòng)學(xué)模型為：

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

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

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

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

記

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

則式(6)可整理如下：

tk時(shí)刻水聲測(cè)量的水面艦船與第i個(gè)信標(biāo)的距離為：

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

定義信標(biāo)量測(cè)因子節(jié)點(diǎn)模型為：

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

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

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

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

式中，各符號(hào)定義如下：

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

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

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

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

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

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

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

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

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

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

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

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

圖2 圓形運(yùn)動(dòng)軌跡對(duì)比圖Fig. 2 Comparison chart of circular trajectory

圖3 圓形運(yùn)動(dòng)軌跡下X軸方向估計(jì)誤差Fig. 3 Estimation error of x-axis direction in circular trajectory

圖4 圓形運(yùn)動(dòng)軌跡下Y軸方向估計(jì)誤差Fig. 4 Estimation error of y-axis direction in circular trajectory

圖5 蛇形運(yùn)動(dòng)軌跡對(duì)比圖Fig. 5 Comparison chart of serpentine trajectory

圖6 蛇形運(yùn)動(dòng)軌跡下X軸方向估計(jì)誤差Fig. 6 Estimation error of x-axis direction in serpentine trajectory

4 結(jié) 語

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

圖7 蛇形運(yùn)動(dòng)軌跡下Y軸方向估計(jì)誤差Fig. 7 Estimation error of y-axis direction in serpentine trajectory

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

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