單領(lǐng)航者相對(duì)位移測(cè)量的多自主水下航行器協(xié)同導(dǎo)航

李聞白，劉明雍，張立川，劉富檣

(西北工業(yè)大學(xué) 航海學(xué)院，陜西 西安710072)

0 引言

隨著人工智能理論、傳感器技術(shù)和微計(jì)算機(jī)技術(shù)等的不斷成熟和發(fā)展，具有感知、思維和動(dòng)作能力的自主水下航行器(AUV)在軍事和民用領(lǐng)域得到了廣泛的應(yīng)用，如水下情報(bào)搜集、遠(yuǎn)程水下攻擊、深海資源勘測(cè)和海底光纜鋪設(shè)等。就當(dāng)前的技術(shù)而言，水下導(dǎo)航問題仍然是發(fā)展AUV 所面臨的主要挑戰(zhàn)之一［1］。導(dǎo)航系統(tǒng)必須提供遠(yuǎn)距離及長時(shí)間范圍內(nèi)的精確定位、速度及姿態(tài)信息，受AUV 自身的體積、質(zhì)量、能源的限制及水介質(zhì)的特殊性、隱蔽性等因素的影響，實(shí)現(xiàn)AUV 的精確導(dǎo)航是一項(xiàng)艱巨的任務(wù)［2］。

長基線(LBL)、短基線(SBL)和超短基線(USBL)系統(tǒng)作為三種傳統(tǒng)的水下導(dǎo)航方法，已被廣泛應(yīng)用于潛艇和AUV 的實(shí)時(shí)導(dǎo)航與定位。這些系統(tǒng)的定位精度較高，但其中的一系列不足之處也是明顯存在的［3-4］。以常用的LBL 為例，布放、標(biāo)定和回收水下傳感器基陣需耗費(fèi)大量的時(shí)間、人力和物力，使用成本較高;此外，針對(duì)AUV 執(zhí)行任務(wù)區(qū)域的不同，上述布放和回收工作需重復(fù)進(jìn)行，制約了AUV 的活動(dòng)范圍。SBL 和USBL 需要母船支持，減少了AUV 的靈活性和隱蔽性。

出于以上考慮，研究人員提出了利用多個(gè)AUV間的相互合作實(shí)現(xiàn)協(xié)同定位的導(dǎo)航方法［5］。它使得導(dǎo)航信息能夠分享，且擺脫了基陣(母船)的束縛，使用區(qū)域靈活，成本較低。通常多AUV 的協(xié)同導(dǎo)航采用主從式(Leader-Follower)，主AUV(領(lǐng)航者，Leader)裝備有高精度的導(dǎo)航通訊設(shè)備(如GPS、磁羅盤、多普勒速度儀、深度計(jì)、水聽器等)，從AUV(跟隨者，F(xiàn)ollower)配備的傳感器精度相對(duì)較低。利用AUV 間的水聲通訊，可以測(cè)量出各個(gè)不同時(shí)刻主、從AUV 間的相對(duì)位置信息，AUV 在各采樣周期內(nèi)的相對(duì)位移由自身的航位推算獲得。根據(jù)上述信息并結(jié)合AUV 的運(yùn)動(dòng)學(xué)方程，利用擴(kuò)展的Kalman濾波(EKF)方法就可以求取各從AUV 在不同時(shí)刻的定位估計(jì)。

為將系統(tǒng)復(fù)雜度及對(duì)量測(cè)手段的要求減到最小程度，人們提出了用一個(gè)領(lǐng)航者，僅利用距離信息的協(xié)同導(dǎo)航方法［6］。Bahr［7］等研究了單領(lǐng)航者AUV的協(xié)同導(dǎo)航，給出了基于極小化代價(jià)函數(shù)的CN 導(dǎo)航算法(CN-Algorithm)，但該算法對(duì)代價(jià)函數(shù)中Kullback-Leibler 距離(KL divergence)的求解過于復(fù)雜，沒有考慮定位解算的實(shí)時(shí)性。Gadre［8］等通過單AUV 領(lǐng)航者與單固定信標(biāo)相結(jié)合的導(dǎo)航方法來提高定位精度，給出了基于EKF 的協(xié)同導(dǎo)航算法，但固定信標(biāo)的引入制約了AUV 的活動(dòng)范圍，同時(shí)減少了隱蔽性。Baccou［9］等采用領(lǐng)航者、跟隨者AUV 間的適當(dāng)機(jī)動(dòng)來解決單個(gè)距離信息求解不充分的問題，通過設(shè)定初始化機(jī)動(dòng)路徑，利用Levenberg-Marquardt 方法(LM-Algorithm)優(yōu)化協(xié)同導(dǎo)航EKF 算法中的濾波初值，并以此提高定位精度。但是，LM優(yōu)化算法的收斂性與自身迭代初值的選取密切相關(guān)，不適當(dāng)?shù)某踔低鶎?dǎo)致其收斂到錯(cuò)誤結(jié)果，因此在實(shí)施過程中至少需要選取50 組以上的不同初值進(jìn)行迭代試驗(yàn)，利用統(tǒng)計(jì)方法篩選出最終的正確結(jié)果，這將使得算法的復(fù)雜性大為增加，從而降低導(dǎo)航算法的實(shí)時(shí)性與穩(wěn)定性。

本文針對(duì)上述研究中存在的問題，結(jié)合單領(lǐng)航者AUV 協(xié)同導(dǎo)航系統(tǒng)的定位原理和運(yùn)動(dòng)學(xué)模型，基于相對(duì)位移測(cè)量、航位推算信息及相鄰時(shí)刻主、從AUV 間相對(duì)位置的幾何關(guān)系，提出了一種多AUV協(xié)同導(dǎo)航算法。該算法的原理直觀、簡捷，具有較高的定位精度，同時(shí)兼顧了導(dǎo)航解算的實(shí)時(shí)性要求。仿真結(jié)果驗(yàn)證了該算法的可行性和有效性。

1 數(shù)學(xué)模型

主、從AUV 均配備有各自的導(dǎo)航系統(tǒng)，其中包括磁羅盤、多普勒速度儀和水聽器等傳感器。如圖1所示，tABk時(shí)刻AUV A、AUV B 間的距離為rAB(tABk)，其中k 表示A、B 間的第k 次數(shù)據(jù)傳輸;A、表示tAkB時(shí)刻AUV A 的推位B 間的航位推算信息在tAkB時(shí)刻實(shí)現(xiàn)交換，pADR(tAkB)=導(dǎo)航位置坐標(biāo)，由A 的航位推算系統(tǒng)獲得。

理論分析可知，由(1)式可以確定AUV B 的位置坐標(biāo)(xB1，yB1)，而依次累加ΔpB12，ΔpB23…即可求得后續(xù)時(shí)刻AUV B 的位置坐標(biāo)更新。進(jìn)一步，方程組(1)存在唯一解的必要條件是向量組［xA1，yA1］T，和［x3A+Δx1B3，y3A+Δy1B3］T線性獨(dú)立。也就是說，AUV A 和AUV B 不能以相同的速度沿同一方向平行航行，否則協(xié)同導(dǎo)航問題將無法求解(此時(shí)協(xié)同導(dǎo)航系統(tǒng)全局不可觀測(cè))，這與已有的研究結(jié)果是吻合的［8］。

在實(shí)際應(yīng)用中，利用代數(shù)方法或數(shù)值算法通過(1)式直接求解(xB1，yB1)是不被采用的，這將導(dǎo)致(xB1，yB1)敏感地依賴于系統(tǒng)的輸入(當(dāng)系統(tǒng)近似不可觀測(cè)時(shí)尤為顯著)從而失去意義。因此，設(shè)計(jì)適當(dāng)?shù)腅KF 導(dǎo)航算法是可行途徑，在下一節(jié)中具體展開討論。

圖1 主從式多AUV 協(xié)同導(dǎo)航系統(tǒng)Fig.1 Leader-Follower cooperative navigation system of multiple AUVs

2 協(xié)同導(dǎo)航算法

2.1 狀態(tài)估計(jì)

記pAB=pB-pA表示AUV A、AUV B 間的相對(duì)位置，其一階導(dǎo)數(shù)為=vAB，設(shè)pABlc表示主、從AUV在時(shí)刻的最新一次相對(duì)位置數(shù)據(jù)傳輸，則協(xié)同導(dǎo)航系統(tǒng)的狀態(tài)方程可以寫為

式中，ηv是具有零均值的高斯白噪聲，var ηv=diag設(shè)系統(tǒng)的狀態(tài)更新時(shí)間為Ttu，則與(2)式相對(duì)應(yīng)的離散時(shí)間狀態(tài)方程為(“-”表示先驗(yàn)估計(jì))

式中，系統(tǒng)噪聲

2.2 量測(cè)方程與方差更新

主AUV A 可近似連續(xù)地獲得自身的航位推算定位信息，并經(jīng)由水聲通訊周期性地獲取從AUV B的航位推算位置。由于漂移誤差的存在，上述航位推算結(jié)果的定位誤差將隨時(shí)間的推移持續(xù)增長，其增長特性服從高斯白噪聲過程［10］

利用連續(xù)相鄰時(shí)刻主、從AUV 的相對(duì)位移信息，可以建立系統(tǒng)的位移差量測(cè)方程

結(jié)合誤差模型(5)可得(6)式的量測(cè)誤差分布

相應(yīng)的位移差量測(cè)矩陣為

位移差量測(cè)噪聲矩陣為

此外，主、從AUV 間的相對(duì)速度量測(cè)矩陣為

速度量測(cè)噪聲矩陣為

式中，σ2hz，σ2vt分別表示水平與垂直方向的測(cè)速誤差。

下面討論濾波方差的更新。由于狀態(tài)向量中已經(jīng)包含了主、從AUV 在tABk時(shí)刻的最新一次相對(duì)位置數(shù)據(jù)傳輸pABlc，因此結(jié)合位移差量測(cè)方程的構(gòu)造形式可知，此時(shí)濾波方差的更新過程具有比經(jīng)典的EKF 算法中更為簡潔的形式。設(shè)Pk(-)表示k 時(shí)刻濾波方差的先驗(yàn)估計(jì)，ΔpABk為k 時(shí)刻新獲得的位移差量測(cè)值，則濾波方差的更新可以分為兩步(“+”表示后驗(yàn)估計(jì)):

其中方括號(hào)部分表示與Pk(+)中相對(duì)應(yīng)的子矩陣。

2.3 量測(cè)數(shù)據(jù)的選取

考慮到水下通訊環(huán)境的復(fù)雜性，在相對(duì)位置量測(cè)數(shù)據(jù)被用于濾波之前，應(yīng)舍棄部分錯(cuò)誤的量測(cè)值以確保Kalman 濾波器具有較快的收斂性。為此，可以利用先驗(yàn)方差矩陣Pk(-)來判斷tABk時(shí)刻所獲得的pAB(tABk)是否為一“好”的量測(cè)值。Pk(-)的主對(duì)角項(xiàng)描述了狀態(tài)變量估計(jì)誤差的方差。對(duì)于狀態(tài)向量s(tABk)=［(pAB(tABk))T(vAB(tABk))T主對(duì)角線的前兩項(xiàng)分別是主、從AUV 的相對(duì)位置坐標(biāo)ΔxAkB，ΔyAkB估計(jì)誤差的方差σ2Δx(tAkB)，σ2Δy(tAkB).由此，可以定義一個(gè)描述時(shí)刻相對(duì)位置估計(jì)不確定性的圓域，其半徑為

此時(shí)，如果量測(cè)值pAB(tABk)滿足

則認(rèn)為pAB(tABk)是有效數(shù)據(jù)，否則應(yīng)當(dāng)予以舍棄。(14)式中的εe是可以根據(jù)不同的實(shí)際情況加以適當(dāng)選取的正常數(shù)。判據(jù)(14)式可作為EKF 導(dǎo)航算法中的一個(gè)子模塊加以使用。

3 可觀測(cè)性分析

可觀測(cè)性作為動(dòng)態(tài)系統(tǒng)的重要性質(zhì)之一，描述了系統(tǒng)的狀態(tài)量能否經(jīng)由量測(cè)數(shù)據(jù)加以求解。由前述討論可知，當(dāng)主、從AUV 以相同的速度沿同一方向平行航行時(shí)，協(xié)同導(dǎo)航系統(tǒng)全局不可觀測(cè)。本節(jié)進(jìn)一步研究單領(lǐng)航者協(xié)同導(dǎo)航系統(tǒng)的局部可觀測(cè)性，這在實(shí)際應(yīng)用中更具有普遍意義［11］(在大多數(shù)情形下，系統(tǒng)的全局可觀測(cè)性條件是難以獲得的)。

定義tAkB時(shí)刻關(guān)于相對(duì)位置量測(cè)量rAB，ΔpAB的向量函數(shù)

經(jīng)過一個(gè)采樣周期Ttu=tk+1-tk，并結(jié)合(15)式可得

即

直接計(jì)算映射(16)式的Jacobi 行列式可得

因此，協(xié)同導(dǎo)航系統(tǒng)局部可觀測(cè)的條件是Jh［k，k+1］≠0，即

分析(19)式知，如果導(dǎo)航系統(tǒng)局部可觀測(cè)，當(dāng)且僅當(dāng)相鄰的兩個(gè)相對(duì)位置量測(cè)值pAB(tABk)和pAB(tABk+1)線性獨(dú)立。利用(19)式可以方便地檢測(cè)系統(tǒng)的局部可觀測(cè)性:設(shè)δ0是一給定的正常數(shù)，pAB(tAkB)，pAB(tAkB+1)是相鄰的兩個(gè)量測(cè)值，如果

則系統(tǒng)不可觀測(cè)或近似不可觀測(cè)，此時(shí)需要調(diào)整主、從AUV 的運(yùn)動(dòng)軌跡以保持系統(tǒng)的可觀測(cè)性，并以此保證定位解算的正確性。

4 仿真結(jié)果

為檢驗(yàn)文中協(xié)同導(dǎo)航算法的有效性和實(shí)用性，進(jìn)行仿真分析研究。如圖2所示，AUV1(領(lǐng)航者)由坐標(biāo)原點(diǎn)出發(fā)沿直線航行，AUV2、AUV3 首先按斜線航行一段距離，然后與AUV1 保持平行航行。AUV1 的航速為2 m/s，AUV2 和AUV3 的航速為1 m/s，傳感器采樣周期為1 s.速度信息由多普勒速度傳感器測(cè)量，取σ2v=(0.1 m/s)2的零均值高斯白噪聲;航向角信息由陀螺羅經(jīng)測(cè)量，取σ2φ=(0.5°)2的零均值高斯白噪聲;AUV 間的相對(duì)位置量測(cè)噪聲取σ2r=(0.1 m)2的零均值高斯白噪聲。

圖2 AUV 的仿真運(yùn)動(dòng)路徑Fig.2 The simulated trajectories of AUVs

圖3給出了從AUV 的EKF 協(xié)同導(dǎo)航算法軌跡。由圖3可知，EKF 協(xié)同導(dǎo)航算法軌跡與真實(shí)軌跡(見圖2)有較好的吻合性，且在全航程中保持穩(wěn)定。圖4分別給出了AUV2、AUV3 的協(xié)同導(dǎo)航定位誤差?？梢钥吹?，在整個(gè)航程中，AUV2、AUV3 的定位誤差中值約為2.5 m，單一方向的最大定位誤差不超過5 m，具有較高的導(dǎo)航定位精度。

為進(jìn)一步檢驗(yàn)文中導(dǎo)航算法的性能，將文獻(xiàn)［7］中的CN 導(dǎo)航算法、文獻(xiàn)［9］中的LM-EKF 導(dǎo)航算法與文中方法進(jìn)行比較(見表1)，三種算法的仿真均采用圖2所示的運(yùn)動(dòng)路徑和本節(jié)所給定的初始條件。

圖3 從AUV 的協(xié)同導(dǎo)航軌跡Fig.3 Cooperative navigation trajectories of follower AUVs

圖4 從AUV 的定位估計(jì)誤差Fig.4 Localization errors of follower AUVs

表1 三種協(xié)同導(dǎo)航算法的性能比較Tab.1 Performance comparison of three cooperative navigation algorithms

由表1可以看出，在相同仿真時(shí)間內(nèi)，文中的導(dǎo)航算法實(shí)現(xiàn)了與CN 算法相當(dāng)?shù)膶?dǎo)航定位精度，但所耗費(fèi)的CPU 計(jì)算時(shí)間僅為后者的58%.LM-EKF方法首先采用LM 算法優(yōu)化了EKF 濾波器的初值，因而其定位精度有所提高。但是，LM-EKF 方法所耗費(fèi)的CPU 計(jì)算時(shí)間大約是文中方法的2.86 倍，可見其在提高定位精度的同時(shí)犧牲了部分導(dǎo)航解算的實(shí)時(shí)性。相比而言，文中的導(dǎo)航算法較好地兼顧了定位精度與實(shí)時(shí)性兩方面的要求，在不太損失定位精度的同時(shí)有效提高了計(jì)算效率。

5 結(jié)論

結(jié)合單領(lǐng)航者AUV 協(xié)同導(dǎo)航系統(tǒng)的定位原理和運(yùn)動(dòng)學(xué)模型，基于相對(duì)位移測(cè)量、航位推算信息及相鄰時(shí)刻主、從AUV 間相對(duì)位置的幾何關(guān)系，提出了一種多AUV 協(xié)同導(dǎo)航算法。仿真結(jié)果表明，基于EKF 的協(xié)同導(dǎo)航算法具有較高的定位精度，且與文獻(xiàn)［7，9］中的導(dǎo)航算法相比，有效提高了定位解算的實(shí)時(shí)性。

References)

［1］ Kinsey J，Eustice R，Whitcomb L.A survey of underwater vehicle navigation:recent advances and new challenges［C］.The 7th IFAC Conference on Maneuvering and Control of Marine Craft，Lisbon，Portugal，2006:1-12.

［2］ Eustice R，Whitcomb L，Singh H，et al.Recent advances in synchronous-clock one-way-travel-time acoustic navigation ［C］.Oceans'06 MTS/IEEE Conference on Exhibition，Boston，USA，2006，1-7.

［3］ Stutters L，Liu H，Tiltman C，et al.Navigation technologies for autonomous underwater vehicles［J］.IEEE Transactions on Systems，2008，38(4):581-589.

［4］ Chandrasekhar V，Winston S，Yoo S C，et al.Localization in underwater sensor networks-survey and challenges［C］.Proceedings of the 1st ACM International Workshop on Underwater Networks，New York，2006:33-40.

［5］ Bahr A，Leonard J，F(xiàn)allon M.Cooperative localization for autonomous underwater vehicles［J］.The International Journal of Robotics Research，2009，28(6):714-728.

［6］ Baccou P，Jouvencel B，Creuze V，et al.Cooperative positioning and navigation for multiple AUV operations［C］.Oceans'01 MTS/IEEE Conference and Exhibition，Honolulu，2001:1816-1821.

［7］ Bahr A.Cooperative localization for autonomous underwater vehicles［D］.Massachusetts Institute of Technology and Woods Hole Oceanographic Institution，2009.

［8］ Gadre A.Observability analysis in navigation systems with an underwater vehicle application［D］.Virginia Polytechnic Institute and State University，2007.

［9］ Baccou P，Jouvencel B.Simulation results，post-processing experimentation and comparison results for navigation，homing and multiple vehicle operations with a new positioning method using a transponder［C］.Proceedings of the 2003 IEEE/RSJ International Conference on Intelligent Robotics and Systems，Las Vegas，2003:811-817.

［10］ 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安:西北工業(yè)大學(xué)出版社，2007.QIN Yong-yuan，ZHANG Hong-yue，WANG Shu-hua.Kalman filter and integrated navigation principles［M］.Xi'an:Northwestern Polytechnical University Press，2007.(in Chinese)

［11］ 鄭大鐘.線性系統(tǒng)理論［M］.第2 版.北京:清華大學(xué)出版社，2002.ZHENG Da-zhong.Linear system theory［M］.2nd ed.Beijing:Tsinghua University Press，2002.(in Chinese)