基于MHE的多UUV協(xié)同定位方法

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

(武漢第二船舶設計研究所，湖北 武漢 430064)

基于MHE的多UUV協(xié)同定位方法

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

(武漢第二船舶設計研究所，湖北 武漢 430064)

針對多水下自治機器人（UUV）協(xié)同定位模型中的非線性問題，本文提出利用滾動時域估計（MHE）方法對協(xié)同定位狀態(tài)空間模型進行最優(yōu)狀態(tài)估計，通過仿真試驗，證明了該方法的可行性。

多水下自治機器人；協(xié)同定位；滾動時域估計

0 引 言

隨著海洋開發(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é)同定位（Cooperative Localization，CL）”。多UUV協(xié)同定位具有下列優(yōu)勢：能夠充分利用系統(tǒng)中某些UUV的高精度導航信息，從而使得裝備低精度導航設備的UUV可以提高自身的導航精度[3]；在多UUV系統(tǒng)中，部分UUV具有有界定位誤差的導航能力，通過協(xié)同定位實現(xiàn)定位信息共享，可以使得系統(tǒng)中每個UUV都具有誤差有界的定位能力；當某些UUV由于傳感器或環(huán)境因素喪失獨立導航能力時，協(xié)同定位可以在一定程度上恢復這些平臺的定位能力。

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

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

近年來，隨著滾動優(yōu)化理論的不斷發(fā)展以及計算機計算能力的提升，基于滾動優(yōu)化原理的滾動時域估計方法受到越來越多的關注并在許多領域獲得成功應用。MHE的基本原理是在系統(tǒng)運行過程中選取一個固定時長的移動時間段作為估計窗口；然后建立性能指標函數(shù)，主要包括估計時域初始時刻狀態(tài)估計誤差、時域內(nèi)量測噪聲以及狀態(tài)噪聲，將系統(tǒng)狀態(tài)硬性約束作為優(yōu)化約束條件引入；最后以性能指標函數(shù)為目標函數(shù)，求解約束優(yōu)化問題得到系統(tǒng)最優(yōu)解。隨著時間推移，估計窗口也相應的向前推移，為了保證估計窗口的時長不變，將舊數(shù)據(jù)移出估計窗口，加入新數(shù)據(jù)，再次執(zhí)行優(yōu)化算法，從而實現(xiàn)動態(tài)優(yōu)化[10]。

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

1.1 狀態(tài)方程

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

則上述3個UUV協(xié)同定位系統(tǒng)的狀態(tài)方程為：

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

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

將i=1，2，3時，3個UUV的狀態(tài)方程聯(lián)合起來，則兩主一從協(xié)同定位系統(tǒng)的線性狀態(tài)方程定義為：

1.2 量測方程

假設UUV1在tk時刻與從UUV進行協(xié)同定位，則UUV 3在tk時刻接收到的相對觀測信息包括UUV 1在tk時刻的位置信息為以及兩UUV之間的水聲測量距離。則根據(jù)主UUV1、從UUV3之間的幾何位置關系有：

按照上述推導，多UUV協(xié)同定位系統(tǒng)狀態(tài)空間模型為：

2 基于MHE的協(xié)同定位濾波方法

如圖1所示，在主從式多UUV協(xié)同定位系統(tǒng)中，若主UUV個數(shù)較少（為了節(jié)約成本，一般不超過2個），則無論多個主UUV如何輪替對從UUV進行協(xié)同定位，從UUV在2次信息交流之間必然有一段時間需要進行無量測信息更新的協(xié)同定位，如果主UUV個數(shù)極少同時從UUV個數(shù)較多（10個左右），那么航推時間會相應增長，這對于一般慣性導航定位的濾波周期（通常為毫秒級）而言比較大，為了更好地將2個不同階段的協(xié)同定位結(jié)合起來，本文提出一種基于CKF的滾動時域方法，其基本思路是建立2種不同處理周期、不同處理模式的狀態(tài)估計方法分別去完成無量測更新協(xié)同定位以及有量測更新協(xié)同定位，后一階段計算的初始條件需要用到前一階段的結(jié)果以及相對觀測信息，然后將有量測更新協(xié)同定位的結(jié)果作為無量測更新協(xié)同定位的初始條件代入下一次定位循環(huán)。如圖1所示，從UUV在2次協(xié)同定位之間需要進行一段時間的無量測更新協(xié)同定位，這階段的定位精度會對協(xié)同定位精度產(chǎn)生較大影響，為了分析方便，我們假設主UUV 1與從UUV2在tk時刻系統(tǒng)進行協(xié)同定位，為了更好地實現(xiàn)從UUV的無量測更新階段自身定位，保證無量測更新階段定位誤差的精度，本文提出利用在無量測更新階段采用CKF非線性濾波方法對協(xié)同定位問題進行處理。

2.1 無量測更新階段CKF濾波

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

估計k時刻的狀態(tài)預測值：

估計k時刻的狀態(tài)誤差協(xié)方差預測值：

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

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

估計卡爾曼增益：

k時刻狀態(tài)估計值：

k時刻狀態(tài)誤差協(xié)方差估計值：

2.2 有量測更新階段MHE狀態(tài)估計

如圖1所示，假設tk、ts、tm時刻主UUV 1與從UUV 2存在水聲通信，即量測信息實現(xiàn)更新，則在兩相鄰量測時刻之間，從UUV會進行基于UKF的無量測更新協(xié)同定位，假設，并且，取為MHE的估計時域長度，量測更新MHE協(xié)同定位性能指標函數(shù)可以寫為：

式中：下標k代表tk時刻的量；下標s代表ts時刻的量；，，，為ti時刻的系統(tǒng)噪聲，量測噪聲，系統(tǒng)噪聲協(xié)方差矩(陣，量測)噪聲協(xié)方差矩陣，具體形式見第2節(jié)；為到達代價函數(shù)，它代表的是階段系統(tǒng)狀態(tài)對于MHE當前估計時域的影響，到達代價函數(shù)形式為：

3 仿真試驗分析

為了分析影響多UUV協(xié)同定位誤差的主要因素以及驗證MHE方法對協(xié)同定位精度的改進，本文將利用Matlab軟件編寫相應算法程序?qū)嶒灁?shù)據(jù)進行離線處理以驗證算法的有效性。試驗采用2個主UUV輪替對從UUV進行協(xié)同定位，在沒有通信丟包情況下，每個主UUV與從UUV的通信周期為5 s，由于是兩主UUV輪替與從UUV進行協(xié)同定位，因此在沒有通信丟包情況下，從UUV 2～3 s即會收到某個主UUV的相對定位信息。濾波中設定距離測量噪聲方差為，從UUV采用M IMU/TAM組合系統(tǒng)結(jié)合實現(xiàn)自身定位，其中M IMU中微機械陀螺常值漂移為10°/h，加速度計常√值漂移為0.01 m/s2，加速度計隨機誤差，磁強計測量噪聲為10–8T，磁航向測量誤差小于0.5°。仿真時長為2 000 s，離散時間間隔T設為1 s。

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

圖3為從UUV導航軌跡，從圖中可看出，從UUV利用MHE狀態(tài)估計方法產(chǎn)生的協(xié)同導航軌跡與真實軌跡基本重合，而利用自身導航傳感器進行的航推（DR）定位軌跡與真實軌跡相差較大，證明了基于MHE的協(xié)同定位方法的有效性，圖4為具體的定位誤差比較圖，采用MHE方法比直接采用CKF方法定位協(xié)同精度要有所提升，證明了方法的可行性。

4 結(jié) 語

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

[ 1 ]許真珍, 封錫盛. 多UUV協(xié)作系統(tǒng)的研究現(xiàn)狀與發(fā)展[J]. 機器人, 2007, 29(2): 186–192.

[ 2 ]BACCOU P, JOUVENCEL B, CREUZE V. Single beacon acoustic for AUV navigation[C]// International Coference on Advanced Robotics, Budapest ICAR, 2001:1253–1260.

[ 3 ]李聞白, 劉明雍, 張立川, 等. 單領航者相對位移測量的多自主水下航行器協(xié)同導航[J]. 兵工學報, 2011, 32(8):1002–1007.

[ 4 ]BAHR A, W ALTER M R, LEONARD J J. Consistent cooperative localization. in robotics and automation[C]// IEEE International Conference on Robotics and Automation(ICRA'09), 2009: 3415–3422.

[ 5 ]BAHR A. Cooperative Localization for Autonomous Underwater Vehicles [D]. M IT, 2009.

[ 6 ]AGUIARY A, ALMEIDAY J, BAYATY M, et al. Cooperative autonomous marine vehicle motion control in the scope of the EU GREX project: theory and practice[C]// Oceans 2009-Europe. IEEE, 2009: 1–10.

[ 7 ]DIBA M. Strategies in tracking and localization of distributed underwater systems [D]. San Diego, University of California.2010.

[ 8 ]張立川, 劉明雍, 徐德民, 等. 基于水聲傳播延遲的主從式多無人水下航行器協(xié)同導航定位研究[J]. 兵工學報, 2009,30(12): 1674–1678.

[ 9 ]穆華. 多運動平臺協(xié)同導航的分散式算法研究[D]. 長沙: 國防科學技術大學, 2010

[10]RAO C V, RAWLINGS J B, MAYNE D Q. Constraint state estimation for nonlinear discrete-time system s: stability and moving horizon approximations [J]. IEEE Transactions on Automatic Control, 2003, 48(2): 246–258.

The cooperation localization method of MUUVs based on MHE

YANG Jian, LUO Tao, WEI Shi-le, WANG Ya-bo, WANG Hong-hua
(Wuhan Second Ship Design and Research Institute, Wuhan 430064, China)

Aiming at the non-liner problem of the cooperation localization model of MUUVs, the paper proposed a Moving Horizon Estimation method to get the optimization eatate estimation of cooperation localization, the theoretical analysis and experimental results proved the high accuracy and availability of the proposed method.

MUUVs；cooperation localization；MHE

TB566

A

1672–7649(2017)12–0081–05

10.3404/j.issn.1672–7649.2017.12.017

2017–07–26

楊建(1987–)，男，工程師，主要從事艦船導航、多UUV協(xié)同定位研究。