楊 建，羅 濤，魏世樂(lè)，王亞波，王紅華

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

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

楊 建，羅 濤，魏世樂(lè)，王亞波，王紅華

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

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

多水下自治機(jī)器人；協(xié)同定位；滾動(dòng)時(shí)域估計(jì)

0 引 言

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

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

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

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

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

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

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

1.1 狀態(tài)方程

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

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

若假設(shè)各UUV的運(yùn)動(dòng)相互之間無(wú)影響，則可以將式（4）分解為3個(gè)UUV的方程：

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

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

1.2 量測(cè)方程

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

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

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

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

2.1 無(wú)量測(cè)更新階段CKF濾波

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

估計(jì)k時(shí)刻的狀態(tài)預(yù)測(cè)值：

估計(jì)k時(shí)刻的狀態(tài)誤差協(xié)方差預(yù)測(cè)值：

通過(guò)2.2節(jié)定義的觀測(cè)方程傳播Cubature點(diǎn)：

估計(jì)自相關(guān)協(xié)方差陣：

估計(jì)卡爾曼增益：

k時(shí)刻狀態(tài)估計(jì)值：

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

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

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

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

3 仿真試驗(yàn)分析

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

主、從UUV運(yùn)動(dòng)軌跡見圖2，三UUV近似為直線運(yùn)動(dòng)，從UUV運(yùn)動(dòng)軌跡在兩主UUV之間。

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

4 結(jié) 語(yǔ)

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

[ 1 ]許真珍, 封錫盛. 多UUV協(xié)作系統(tǒng)的研究現(xiàn)狀與發(fā)展[J]. 機(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 ]李聞白, 劉明雍, 張立川, 等. 單領(lǐng)航者相對(duì)位移測(cè)量的多自主水下航行器協(xié)同導(dǎo)航[J]. 兵工學(xué)報(bào), 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 ]張立川, 劉明雍, 徐德民, 等. 基于水聲傳播延遲的主從式多無(wú)人水下航行器協(xié)同導(dǎo)航定位研究[J]. 兵工學(xué)報(bào), 2009,30(12): 1674–1678.

[ 9 ]穆華. 多運(yùn)動(dòng)平臺(tái)協(xié)同導(dǎo)航的分散式算法研究[D]. 長(zhǎng)沙: 國(guó)防科學(xué)技術(shù)大學(xué), 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–)，男，工程師，主要從事艦船導(dǎo)航、多UUV協(xié)同定位研究。