楊 建 羅 濤 魏世樂 王亞波 王紅華

（武漢第二船舶設計研究所 武漢 430064）

1 引言

隨著海洋開發(fā)技術的發(fā)展，單自治水下機器人（Unmanned Underwater Vehicle，UUV）越來越難完成復雜的軍事及民事任務，從而使得多UUV協(xié)同系統(tǒng)在海洋探索及開發(fā)、軍事作戰(zhàn)等方面擁有越來越重要的作用［1］。定位技術是協(xié)同作業(yè)的前提條件和關鍵技術。由于多UUV協(xié)同定位具有各UUV獨自定位所不具有的多種優(yōu)勢，因此多UUV協(xié)同定位正逐步成為一個熱門研究課題［2］，設計能夠提高多UUV定位精度的協(xié)同定位算法具有重要的理論價值和現(xiàn)實意義。

如果多UUV群體在定位時存在著相對觀測，那么通過一定的信息交換，就可以實現(xiàn)UUV間定位信息的共享，達到提高多UUV系統(tǒng)整體定位能力的目的，這種定位方法稱為“協(xié)同定位（Coopera?tive Localization，CL）”。多UUV協(xié)同定位具有下列優(yōu)勢：1）能夠充分利用系統(tǒng)中某些UUV的高精度導航信息，從而使得裝備低精度導航設備的UUV可以提高自身的導航精度［3］；2）在多UUV系統(tǒng)中，部分UUV具有有界定位誤差的導航能力，通過協(xié)同定位實現(xiàn)定位信息共享，可以使得系統(tǒng)中每個UUV都具有誤差有界的定位能力；3）當某些UUV由于傳感器或環(huán)境因素喪失獨立導航能力時，協(xié)同定位可以在一定程度上恢復這些平臺的定位能力。

多無人平臺協(xié)同定位的研究始于20世紀90年代，研究對象包括移動機器人、水面無人艇、無人機、衛(wèi)星、水下自治潛器等。目前，多UUV協(xié)同定位技術的研究總體上處于理論向工程實踐轉化的階段，美國麻省理工大學的John J.Leonard教授帶領的團隊在2009年前后系統(tǒng)地提出了一系列多UUV協(xié)同定位方法，并進行了工程實踐［4］，但是他們的協(xié)同定位方法存在著不足：各UUV的導航傳感器精度都比較高，協(xié)同僅僅作為一種保證定位精度的輔助手段引入，也就意味著協(xié)同定位精度與UUV自身導航定位精度差別不大。Leonard教授的博士 Alexander Bahr在他的博士論文中［5］，根據(jù)幾何學原理，提出了一種基于主定位器相對觀測確定的從潛航器定位區(qū)域，然后利用Kullback—Leibler原理，提出一種新的優(yōu)化目標函數(shù)，在上述優(yōu)化區(qū)域內(nèi)按照這種優(yōu)化目標函數(shù)值最小的原則對潛器進行定位，仿真結果證明了這種優(yōu)化方法的可行性，這種優(yōu)化方法理論比較簡單，但是由于要在大量區(qū)域內(nèi)尋優(yōu)，每一次尋優(yōu)過程都需要進行復雜的尋優(yōu)計算。

歐盟資助的多水下潛航器項目MAUVs GREX的一個研究成果是只依靠航跡推算和潛航器之間的距離測量信息，在不使用水聲定位的情況下，實現(xiàn)潛航器的相對定位［6］。Corp公司研究了地球同步衛(wèi)星協(xié)同定位的分散式算法［7］，同時該公司資助悉尼大學自主系統(tǒng)研究中心以機器人協(xié)同定位為背景，開展了相關的定位算法研究。

蔣榮欣［8］等利用主機器人上的傳感器獲得的方位以及距離信息，然后結合從機器人的自身解算出的位置及航向信息，設計了一種聯(lián)邦濾波模型，并對其進行EKF濾波處理，通過實驗證明了該方法定位精度較高，且無明顯的誤差累積效應。通過上述文獻可以看出，協(xié)同定位方法已經(jīng)在機器人領域展開了相應的研究，目前的各項研究都比較成熟，且實際中有所應用。受到陸地移動機器人的啟發(fā)，研究人員將目光投向海洋，多UUV系統(tǒng)也逐漸成為協(xié)同定位領域里新的熱點。

Mourikis等相關研究者分析了機器人協(xié)同定位精度與相關導航參數(shù)的關系［9］。對于可觀系統(tǒng)，所有機器人的穩(wěn)態(tài)定位誤差都是有界的，其誤差上界取決于機器人之間的相對測量關系、機器人運動傳感器的精度以及機器人相對測量傳感器的精度，與初始定位誤差無關。對于不可觀系統(tǒng)，機器人的定位誤差會隨時間無限增大，定位誤差上界包含兩個常值項和一個時間函數(shù)項，其中一個常值項由初始定位誤差決定，另一個常值項由機器人之間相對測量關系和機器人傳感器精度決定，時間函數(shù)項中時間的系數(shù)取決于機器人數(shù)目和航跡推算能力。

我國對多UUV協(xié)同定位技術的研究起步較晚。西北工業(yè)大學的徐德民院士團隊提出了一種利用移動長基線技術的多AUV協(xié)同定位方法，并設計了AUV協(xié)同導航系統(tǒng)的運動學模型，對主AUV的精確定位信息、主從AUV之間的距離測量信息以及從AUV的自身定位信息進行了數(shù)據(jù)融合嘗試［10～11］。國防科技大學的穆華［12］等提出了一種基于增廣信息和高斯貝葉斯分布的移動機器人協(xié)同定位算法。并且對比分析了分散式信息濾波算法和分散式經(jīng)典卡爾曼濾波算法的不同。

2 多UUV協(xié)同定位狀態(tài)空間模型

2.1 狀態(tài)方程

假設多UUV群體中有N個主UUV，M個從UUV，為了研究的方便，先考慮2個主UUV對1個從UUV進行協(xié)同定位的情況，定義tk時刻系統(tǒng)狀態(tài) 為Xk=(，，)T，單 個UUV狀 態(tài)Xik=(xik，yik，T，其中 i=1，2，3分別為三個UUV的編號，其中1，2為兩主UUV，3為從UUV，且定義L1k=(x1k，y1k)T為第i個UUV在tk時刻的位置信息，?i表示第i個UUV在tk時刻的航向信息，根據(jù)UUV的運動特性，定義Vik、wik分別表示第i個UUV在tk時刻的自身傳感器測量的前向合成速度以及航向角速度，δt為采樣周期，則第i個UUV的運動學方程可表示為

定義uik=(Vik，ωik)T為第i個UUV系統(tǒng)的輸入：

若假設各UUV的運動相互之間無影響，則可以將式（4）分解為三個UUV的方程：

結合式（1）中的UUV運動學方程，利用EKF原理可將第i個UUV的狀態(tài)方程線性化：

其中：Fik、Gik分別為線性化后的第i個UUV系統(tǒng)矩陣、系統(tǒng)噪聲激勵矩陣，具體形式為

2.2 量測方程

如圖1所示，假設UUV1在tk時刻與從UUV進行協(xié)同定位，則UUV3在tk時刻接收到的相對觀測信息包括UUV1在tk時刻的位置信息為L1k=(x1k，y1k)T以及兩UUV之間的水聲測量距離r1k。

則根據(jù)主UUV1、從UUV3之間的幾何位置關系有：

選取 L1k=(x1k，y1k)T、r1k作為tk時刻系統(tǒng)的量測信息，則量測方程可寫為

其中：v13k為tk時刻UUV1與UUV3之間的量測噪聲，利用Jacobian矩陣將式（9）線性化可得：

3 基于CKF的協(xié)同定位濾波方法

3.1 時間更新

假設k-1時刻狀態(tài)估計協(xié)方差 p(xk-1)已知，通過Cholesky分解誤差協(xié)方差Pk-1|k-1：

其中n為UUV個數(shù)，x?k-1|k-1為k-1時刻系統(tǒng)狀態(tài)估計值，且定義以下變量：

通過2.1節(jié)定義的狀態(tài)方程（7）傳播Cubature點：

3.2 量測更新

通過Cholesky分解Pk|k-1：

計算Cubature點i=1，2，...m，m=2n：

通過2.2節(jié)定義的觀測方程傳播Cubature點：

估計自相關協(xié)方差陣：

估計卡爾曼增益：

結合上述CKF時間以及量測更新，可以將通用的CKF計算流程總結為圖2。

4 仿真試驗分析

為了分析影響多UUV協(xié)同定位誤差的主要因素以及驗證CKF方法對協(xié)同定位精度的改進，本文將利用Matlab軟件編寫相應算法程序對實驗數(shù)據(jù)進行離線處理以驗證算法的有效性。

和本文理論分析的情況一樣，試驗采用兩個主UUV輪替對從UUV進行協(xié)同定位，在沒有通信丟包情況下，每個主UUV與從UUV的通信周期為5s，由于是兩主UUV輪替與從UUV進行協(xié)同定位，因此在沒有通信丟包情況下，從UUV 2s～3s即會收到某個主UUV的相對定位信息。濾波中設定距離測量噪聲方差為=(10m)2，從UUV采用MIMU/TAM組合系統(tǒng)結合實現(xiàn)自身定位，其中MIMU中微機械陀螺常值漂移為10°h，加速度計常值漂移為0.01m/s2，加速度計隨機誤差5×10-3m/s2/Hz，磁強計測量噪聲為10-8T，磁航向測量誤差小于0.5°。仿真時長為2000s，離散時間間隔T設為1s。

主、從UUV運動軌跡見圖3，三UUV近似為直線運動，從UUV運動軌跡在兩主UUV之間。

圖4為從UUV導航軌跡，從圖中可看出，從UUV利用CKF產(chǎn)生的協(xié)同導航軌跡與真實軌跡基本重合，而利用自身導航傳感器進行的航推（DR）定位軌跡與真實軌跡相差較大，證明了基于CKF的協(xié)同定位方法的有效性，圖5為具體的定位誤差比較圖。

5 結語

本文在建立多UUV協(xié)同定位方案設計基礎上，利用UUV運動學模型，建立多UUV協(xié)同定位狀態(tài)空間非線性模型，在此基礎上，提出利用CKF方法實現(xiàn)多UUV協(xié)同定位最優(yōu)狀態(tài)估計，通過試驗驗證，證明了基于CKF的多UUV協(xié)同定位方法的有效性。

［1］許真珍，封錫盛.多UUV協(xié)作系統(tǒng)的研究現(xiàn)狀與發(fā)展［J］.機器人，2007，29（2）：186-192.

［2］P.Baccou，B.Jouvencel，V.Creuze.Single Beacon Acous?tic for AUVNavigation［C］//InternationalCoference on Ad?vanced Robotics，Budapest ICAR，2001：1253-1260.

［3］李聞白，劉明雍，張立川等.單領航者相對位移測量的多自主水下航行器協(xié)同導航［J］.兵工學報，2011，32（8）：1002-1007.

［4］A.Bahr，M.R.Walter，and J.J.Leonard.Consistentcoop?erative localization.In Robotics and Automation［C］//IEEE International Conference on Robotics and Automa?tion（ICRA'09），2009：3415-3422.

［5］Bahr A.Cooperative Localization for Autonomous Under?water Vehicles［D］.MIT，2009.

［6］Aguiary A，Almeiday J，Bayaty M，etal.Cooperative au?tonomousmarine vehiclemotion control in the scope of the EU GREX project：theory and practice［C］//Oceans 2009-Europe.IEEE，2009：1-10.

［7］Diba Mirza.Strategies in Tracking and Localization of Dis?tributed Underwater Systems［D］.University of Califor?nia，San Diego.2010.

［8］蔣榮欣，田翔，謝立，陳耀武.一種多機器人編隊協(xié)同定位的方法［J］.哈爾濱工業(yè)大學學報，2010（1）：152-157.

［9］Mourikis A I，Roumeliotis S I.Performance analysis of multirobot cooperative localization［J］.Robotics，IEEE Transactionson，2006，22（4）：666-681.

［10］張立川，劉明雍，徐德民等.基于水聲傳播延遲的主從式多無人水下航行器協(xié)同導航定位研究［J］.兵工學報，2009，30（12）：1674-1678.

［11］張立川，劉明雍，徐德民等.多UUV協(xié)同導航與定位研究［J］.系統(tǒng)仿真學報，2008，20（19）：5342-5349.

［12］穆華.多運動平臺協(xié)同導航的分散式算法研究［D］.長沙：國防科學技術大學，2010.