基于CKF的多UUV協同定位方法?

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

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

1 引言

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

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

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

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

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

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

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

2 多UUV協同定位狀態空間模型

2.1 狀態方程

假設多UUV群體中有N個主UUV，M個從UUV，為了研究的方便，先考慮2個主UUV對1個從UUV進行協同定位的情況，定義tk時刻系統狀態 為Xk=(，，)T，單 個UUV狀 態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時刻的航向信息，根據UUV的運動特性，定義Vik、wik分別表示第i個UUV在tk時刻的自身傳感器測量的前向合成速度以及航向角速度，δt為采樣周期，則第i個UUV的運動學方程可表示為

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

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

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

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

2.2 量測方程

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

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

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

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

3 基于CKF的協同定位濾波方法

3.1 時間更新

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

其中n為UUV個數，x?k-1|k-1為k-1時刻系統狀態估計值，且定義以下變量：

通過2.1節定義的狀態方程（7）傳播Cubature點：

3.2 量測更新

通過Cholesky分解Pk|k-1：

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

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

估計自相關協方差陣：

估計卡爾曼增益：

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

4 仿真試驗分析

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

和本文理論分析的情況一樣，試驗采用兩個主UUV輪替對從UUV進行協同定位，在沒有通信丟包情況下，每個主UUV與從UUV的通信周期為5s，由于是兩主UUV輪替與從UUV進行協同定位，因此在沒有通信丟包情況下，從UUV 2s～3s即會收到某個主UUV的相對定位信息。濾波中設定距離測量噪聲方差為=(10m)2，從UUV采用MIMU/TAM組合系統結合實現自身定位，其中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產生的協同導航軌跡與真實軌跡基本重合，而利用自身導航傳感器進行的航推（DR）定位軌跡與真實軌跡相差較大，證明了基于CKF的協同定位方法的有效性，圖5為具體的定位誤差比較圖。

5 結語

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

［1］許真珍，封錫盛.多UUV協作系統的研究現狀與發展［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］李聞白，劉明雍，張立川等.單領航者相對位移測量的多自主水下航行器協同導航［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］蔣榮欣，田翔，謝立，陳耀武.一種多機器人編隊協同定位的方法［J］.哈爾濱工業大學學報，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］張立川，劉明雍，徐德民等.基于水聲傳播延遲的主從式多無人水下航行器協同導航定位研究［J］.兵工學報，2009，30（12）：1674-1678.

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

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