基于條件數(shù)的多自主水下航行器協(xié)同定位系統(tǒng)可觀測度分析

馬朋，張福斌，徐德民，劉書強

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

基于條件數(shù)的多自主水下航行器協(xié)同定位系統(tǒng)可觀測度分析

馬朋，張福斌，徐德民，劉書強

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

針對基于移動長基線的多自主水下航行器（MAUV）協(xié)同定位系統(tǒng)，建立了MAUV的2D運動學模型與距離量測方程。利用基于Lie導數(shù)的非線性系統(tǒng)可觀測性理論，得到協(xié)同定位系統(tǒng)可觀測矩陣及MAUV編隊的不可觀測航行路徑。利用奇異值分解（SVD）理論獲得系統(tǒng)可觀測矩陣的條件數(shù)，建立了協(xié)同定位系統(tǒng)可觀測度的評價函數(shù)，并在不同MAUV航行隊形對系統(tǒng)可觀測度進行仿真分析。結果表明，系統(tǒng)可觀測度隨著MAUV航行隊形的不同而變化，從而為通過變換隊形來提高協(xié)同定位性能提供了有效的參考依據(jù)。

信息處理技術；多自主水下航行器；協(xié)同定位；條件數(shù)；可觀測度

0　引言

可觀測性的概念是Kalman在20世紀60年代初為解決線性系統(tǒng)的問題而引入，其反映了系統(tǒng)利用有限時間內的觀測量確定系統(tǒng)狀態(tài)的能力［1］。通常使用可觀測矩陣的秩來判斷系統(tǒng)的可觀測性，若可觀測矩陣滿秩，則系統(tǒng)是完全可觀測的，否則是不完全可觀測的。

在多自主水下航行器（MAUV）協(xié)同定位過程中，定位系統(tǒng)的可觀測性直接關系到系統(tǒng)狀態(tài)量能否被估計得出，因此對協(xié)同定位系統(tǒng)進行可觀測性分析具有重要的意義。文獻［2-3］在基于單信標測距的自主水下航行器（AUV）協(xié)同定位系統(tǒng)中，利用線性時變系統(tǒng)可觀測性理論分析了協(xié)同定位系統(tǒng)的可觀測性，并給出幾種AUV在2D和3D運動學模型下的不可觀測航行路徑。文獻［4］利用基于Lie導數(shù)的非線性系統(tǒng)可觀測性理論，分析了多機器人協(xié)同定位系統(tǒng)的可觀測性，得出滿足定位系統(tǒng)局部弱可觀測的條件。文獻［5-6］討論了基于單領航者的MAUV協(xié)同定位系統(tǒng)的可觀測性，并給出幾種可有效提高系統(tǒng)可觀測性的MAUV航行路徑。文獻［7-9］同樣利用Lie導數(shù)分別討論了運動控制輸入量對多機器人和MAUV協(xié)同定位系統(tǒng)可觀測性的影響。

無論對于線性系統(tǒng)還是非線性系統(tǒng)，在定性地得到系統(tǒng)是否可觀測結論的前提下，往往還需要定量地分析系統(tǒng)及系統(tǒng)狀態(tài)的可觀測度。可觀測度反映了觀測模型對系統(tǒng)狀態(tài)變化的敏感程度，與系統(tǒng)狀態(tài)估計所能達到的精度密切相關［10］。主要的分析方法有，利用估計誤差協(xié)方差矩陣的特征值大小衡量可觀測性的強弱［11］和利用可觀測矩陣的條件數(shù)來分析系統(tǒng)可觀測度［12-13］。目前，這兩種方法已廣泛應用于慣性導航［14-15］及航空航天導航［16-17］系統(tǒng)性能分析中。

在基于移動長基線（MLBL）的MAUV協(xié)同定位過程中［18］，MAUV航行路徑與編隊隊形的變化將造成系統(tǒng)量測向量結構的變化，從而會對協(xié)同定位系統(tǒng)的可觀測性產生影響。為了定量地衡量分析這種影響，本文在利用Lie導數(shù)分析得到系統(tǒng)非線性可觀測矩陣的基礎上，依據(jù)可觀測矩陣的條件數(shù)建立了定位系統(tǒng)可觀測度的評價函數(shù)，并分析討論了協(xié)同定位系統(tǒng)可觀測度在不同MAUV隊形下的變化，為MAUV協(xié)同定位過程中的航行隊形選擇提供參考依據(jù)。

1　MAUV協(xié)同定位系統(tǒng)與模型

MAUV協(xié)同定位是在MAUV協(xié)作基礎上提出和發(fā)展的一種新的定位方式，系統(tǒng)中各AUV在未知環(huán)境中互為信標，利用水聲通信技術，交換共享各自位置與狀態(tài)信息，并與相對位置觀測量進行融合，從而獲得更高的整體定位精度。

在基于移動長基線的MAUV協(xié)同定位系統(tǒng)中，利用兩個領航AUV組成移動長基線，其中領航AUV1和AUV2均裝備高精度導航設備與深度傳感器，跟隨AUV裝備低精度導航設備與深度傳感器，領航AUV1和AUV2利用水聲信號以單向廣播方式定時發(fā)送航行狀態(tài)信息（包括時間、經(jīng)度、緯度、深度、航向、速度等），如圖1所示。跟隨AUV則利用接收到的水聲信號解算與領航AUV間相對位置關系，依靠相應的定位算法，實現(xiàn)自身位置狀態(tài)的實時更新。

圖1　MAUV協(xié)同定位系統(tǒng)Fig.1 Cooperative localization system of MAUV

由于AUV深度信息可由深度傳感器直接測得，并且測量精度較高，為簡化運動學模型，可不考慮AUV深度，將3D運動模型通過AUV間的深度差在水平面內投影轉化為2D運動學模型。

定義t時刻跟隨AUV的狀態(tài)向量為X（t）=［x（t），y（t），φ（t）］T，假定AUV做定深航行，此時跟隨AUV的2D運動學模型為

式中：u1（t）為AUV航行速度；φ（t）為AUV航向角。

定義t時刻領航AUV的狀態(tài)向量分別為X1（t）=［x1（t），y1（t），φ1（t）］T，X2（t）=［x2（t），y2（t），φ2（t）］T，則協(xié)同定位系統(tǒng)的距離量測方程為

2　MAUV協(xié)同定位系統(tǒng)可觀測性

為了便于分析MAUV協(xié)同定位系統(tǒng)的可觀測性，利用Lie導數(shù)求解非線性系統(tǒng)的可觀測矩陣，選取量測向量函數(shù)

結合AUV非線性運動模型（1）式，則量測函數(shù)hk（X）沿向量f的各階Lie導數(shù)為

式中：f=［cos（φ（t））sin（φ（t））0］T；Δ為梯度運算函數(shù)，定義為

根據(jù)微分幾何理論［19］，非線性系統(tǒng)的可觀測矩陣為

從而，利用（3）式～（6）式可得MAUV協(xié)同定位系統(tǒng)可觀測矩陣為

假定領航、跟隨AUV以定速航行，且跟隨AUV的航向角速度為0.當滿足以下3種情況時，系統(tǒng)可觀測矩陣O1不滿秩，即rank（O1）＜3，此時MAUV協(xié)同定位系統(tǒng)為不可觀測的。

分析這3種情況，可以發(fā)現(xiàn)，當領航、跟隨AUV以相同航向角沿同一直線航行時，協(xié)同定位系統(tǒng)的兩量測向量共線，從而使得可觀測矩陣（7）式行向量線性相關，系統(tǒng)可觀測矩陣O1變?yōu)槠娈惥仃嚒４藭r，MAUV協(xié)同定位系統(tǒng)是不可觀測的。

3　MAUV協(xié)同定位系統(tǒng)可觀測度分析

3.1條件數(shù)與可觀測度

設有線性方程組Ax=b，其中A為系數(shù)矩陣，b為已知向量，x為待求解向量。假定系數(shù)矩陣A和向量b存在小擾動δA和δb，此時解向量x擾動為x+δx，則擾動后的方程組為

對（8）式兩端取范數(shù)可證明得［20］：當δA=0，δb≠0時，有；當δA≠0，δb=0時，有

定義κ（A）=‖A-1‖‖A‖為矩陣A的條件數(shù)，可以看出條件數(shù)κ（A）反映了解向量x隨系統(tǒng)擾動變化的敏感程度，是衡量線性方程數(shù)值穩(wěn)定性的重要指標，矩陣的條件數(shù)越大，矩陣越接近病態(tài)。在矩陣理論中，利用奇異值分解（SVD）法求解條件數(shù)可得

式中：σmax（A）和σmin（A）分別為矩陣A的最大奇異值和最小奇異值。

考慮（1）式和（3）式組成的非線性協(xié)同定位系統(tǒng)，由于其觀測空間是由（4）式生成的包含狀態(tài)變量和觀測變量在內的最小線性空間，且關于Lie導數(shù)是封閉的，可以利用（9）式定義非線性系統(tǒng)的可觀測度為

因而，根據(jù)矩陣條件數(shù)的定義知0≤C（O）≤1，可以說可觀測度C（O）越接近于0，矩陣O將越趨于病態(tài)，當C（O）=0時，矩陣O為奇異矩陣，系統(tǒng)不可觀測。反之，C（O）越接近于1，系統(tǒng)擾動對解向量誤差的影響越小，系統(tǒng)狀態(tài)估計效果越好。

3.2協(xié)同定位系統(tǒng)可觀測度

根據(jù)上文研究可知，量測向量結構的改變將引起協(xié)同定位系統(tǒng)可觀測矩陣奇異性的變化，進而影響系統(tǒng)可觀測度的變化，而造成系統(tǒng)量測向量結構改變的主要因素則包括AUV間相對位置關系與航向角。

為了便于分析說明這些影響，以領航AUV1為坐標原點，兩領航AUV組成的基線為橫坐標，基線距離固定為l，且將領航與跟隨AUV位置用極坐標表示，如圖2所示。

圖2　協(xié)同定位系統(tǒng)坐標轉換示意圖Fig.2 Coordinate transformation of cooperative localization system

結合上文定義，將AUV位置從直角坐標轉換到極坐標下的關系為

式中：l為領航AUV間的距離，為固定常值；ρ為跟隨AUV到領航AUV1間的距離；θ為跟隨AUV與領航AUV1連線與領航AUV基線間的夾角。

定義l與ρ有如下比例關系：

則系統(tǒng)可觀測矩陣（7）式可表示為

從而，MAUV協(xié)同定位系統(tǒng)可觀測度為

3.3仿真研究

利用Matlab對MAUV協(xié)同定位系統(tǒng)的可觀測度進行仿真研究，并分兩種情況進行討論分析。

1）假設領航與跟隨AUV航向角固定且相等，兩領航AUV間的距離為l=500，夾角θ的變化范圍為-180°～180°，比值λ從0開始逐漸增大。從而，協(xié)同定位系統(tǒng)可觀測度C（O1）的變化如圖3所示。

從圖3中可以看出，無論AUV航向角為0°還是90°，協(xié)同定位系統(tǒng)可觀測度分布趨勢相同。當θ≈45°，時，定位系統(tǒng)可觀測度C（O1）最大。此時根據(jù)三角形幾何理論可證明，跟隨AUV與兩領航AUV連線間的夾角β≈45°.從而可知，在AUV航向角固定的情況下，當跟隨AUV處于以兩領航AUV連線為直徑的圓上且到兩領航AUV間距離相等時，協(xié)同定位系統(tǒng)可觀測度最高。

圖3　協(xié)同定位系統(tǒng)可觀測度分布圖Fig.3 Distribution diagram of observabilities of cooperative localization system

2）假設領航與跟隨AUV相對位置關系固定，航向角時刻保持相同，且跟隨AUV與兩領航AUV間距離相等，兩領航AUV間的距離為l=500，航向角φ的變化范圍為-120°～120°。從而，協(xié)同定位系統(tǒng)可觀測度C（O1）的變化如圖4和表1所示。

圖4　協(xié)同定位系統(tǒng)可觀測度變化曲線Fig.4 Variational curves of observability of cooperative localization system

表1　協(xié)同定位系統(tǒng)可觀測度對比Tab.1 Comparison of observabilities for cooperative localization system

表1所示為協(xié)同定位系統(tǒng)可觀測度隨夾角θ的變化，假定領航與跟隨AUV航向角相同且固定，則在-45°～45°的變化范圍內，夾角θ越大，協(xié)同定位系統(tǒng)可觀測度越高。圖4所示為協(xié)同定位系統(tǒng)可觀測度隨航向角φ的變化，從圖中可以看出，在領航與跟隨AUV間相對位置固定的情況下，協(xié)同定位系統(tǒng)可觀測度隨航向角變化的趨勢相同，隨著AUV航行方向與領航AUV基線間的夾角增大，協(xié)同定位系統(tǒng)可觀測度增高，并在±90°處達到最大。而當AUV的航向角φ=0°，夾角θ=0°時，如圖4（a）中圓圈所標示，協(xié)同定位系統(tǒng)可觀測度為0，此時系統(tǒng)是不可觀測的，此結論與上文利用可觀測矩陣的秩所得分析結果相同。

4　結論

本文通過建立基于移動長基線的MAUV協(xié)同定位系統(tǒng)模型，利用Lie導數(shù)求解獲得了定位系統(tǒng)非線性可觀測矩陣，并證明得當領航、跟隨AUV以相同航向角沿同一直線進行定速航行時，MAUV協(xié)同定位系統(tǒng)是不可觀測的。在此基礎上，利用SVD理論獲得定位系統(tǒng)可觀測矩陣的條件數(shù)，建立了系統(tǒng)可觀測度的評價函數(shù)，通過系列仿真得到系統(tǒng)可觀測度的分布圖，并討論了協(xié)同定位系統(tǒng)可觀測度在不同MAUV隊形下的變化，從而實現(xiàn)了對定位系統(tǒng)可觀測性的定量分析。

（

）

［1］邢光謙.量測系統(tǒng)的能觀度和狀態(tài)估計精度［J］.自動化學報，1985，11（2）：152-158. XING Guang-qian.Degree of observability and accuracy of state estimate for general measurement system［J］.Acta Automatica Sinica，1985，11（2）：152-158.（in Chinese）

［2］Gadre A，Stilwell D.Underwater navigation in the presence of unknown currents based on range measurements from a single location［C］∥Proceedings of the 2005 American Control Conference.Portland.US：IEEE，2005：656-661.

［3］Lee P，Jun B，Lim Y.Review on underwater navigation system based on range measurements from one reference［C］∥OCEANS 2008-MTS/IEEE KobeTechno-Ocean.Kobe，Japan：MTS/ IEEE，2008：1-5.

［4］Zhou X，Roumeliotis S.Robot-to-robot relative pose estimation from range measurements［J］.IEEE Transactions on Robots，2008，24（6）：1379-1393.

［5］Antonelli G，Arrichiello F，Chiaverini S，et al.Observability analysis of relative localization for AUVs based on ranging and depth measurements［C］∥2010 IEEE International Conference on Robotics and Automation（ICRA）.San Francisco，US：IEEE，2010：4276-4281.

［6］Antonelli G，Caiti A，Calare V，et al.Designing behaviors to improve observability for relative localization of AUVs［C］∥IEEE International Conference on Robotics and Auto-mation（ICRA）. Anchorage，US：IEEE，2010：4270-4275.

［7］Martinelli A，Siegwart R.Observability analysis for mobile robot localization［C］∥2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.Anchorage，US：IEEE，2005：1471-1476.

［8］房新鵬，嚴衛(wèi)生，李俊兵.基于距離量測的主從式AUV協(xié)同定位系統(tǒng)可觀測性研究［J］.西北工業(yè)大學學報，2012，30（4）：547-552. FANG Xin-peng，YAN Wei-sheng，LI Jun-bing.An effective observability analysis for the leader-follower autonomous underwater vehicles（AUVs）cooperative localization based on range measurements［J］.Journal of Northwestern Polytechnical University，2012，30（4）：547-552.（in Chinese）

［9］Sharma R，Beard R，Taylor C，et al.Graph-based observability analysis of bearing-only cooperative localization［J］.IEEE Transactions on Robots，2012，28（2）：522-529.

［10］常曉華，崔平遠，王曉明，等.基于條件數(shù)的能觀性度量方法及在自主導航系統(tǒng)中的應用［J］.宇航學報，2010，31（5）：1331-1337. CHANG Xiao-hua，CUI Ping-yuan，WANG Xiao-ming，et al.A condition number-based observability analysis method and its application in autonomous navigation system［J］.Journal of Astronautics，2010，31（5）：1331-1337.（in Chinese）

［11］Ham F，Brown R.Observability，eigenvalues，and Kalman filtering［J］.IEEE Transactions on Aerospace and Electronic Systems，1983，19（2）：269-273.

［12］Arrichiello F，Antonelli G，Aguiar A，et al.Observability metric of the relative localization for AUVs based on range and depth measurements：theory and experi-ments［C］∥2011 IEEE/RSJ International ConferenceonIntelligentRobotsandSystems（ICIRS）.San Francisco，US：IEEE/RSJ，2011：3166-3171.

［13］Arrichiello F，Antonelli G，Aguiar A，et al.An observability metric for underwater vehicle localization using range measurements［J］.Sensors，2013，13（12）：16191-16215.

［14］Long R，Qin Y，Jia J.Observable degree analysis of SINS initial alignment based on singular value decomposition［C］∥IEEE International Symposium on Knowledge Acquisition and Modeling Workshop.Wuhan，China：IEEE，2008：444-448.

［15］馬艷紅，胡軍.基于SVD理論的可觀測度分析方法的幾個反例［J］.中國慣性技術學報，2008，16（4）：448-452. MA Yan-hong，HU Jun.Counterexamples for degree of observability analysis method based on SVD theory［J］.Journal of Chinese Inertial Technology，2008，16（4）：448-452.（in Chinese）

［16］Maessen D，Gill E.Relative state estimation and observability analysis for formation flying satellites［J］.Journal of Guidance，Control，and Dynamics，2012，35（1）：321-326.

［17］Li M，Wang D，Huang X.Study on the observability analysis based on the trace of error covariance matrix for spacecraft autonomous navigation［C］∥IEEE International Conference on Control and Automation（ICCA）.Hangzhou，China：IEEE，2013：95-98.

［18］Vaganay J，Leonard J，Curcio J，et al.Experimental validation of the moving long baseline navigation con-cept［C］∥IEEE/ OES Autonomous Underwater Vehicles.US：IEEE/OES，2004：59-65.

［19］Isidori A.非線性控制系統(tǒng)［M］.第3版.王奔，莊圣賢，譯.北京：電子工業(yè)出版社，2005. Isidori A.Nonlinear control system［M］.3rd ed.WANG Beng，ZHUANG Sheng-xian，translated.Beijing：Publishing House of Electronics Industry，2005.（in Chinese）

［20］張賢達.矩陣分析與應用［M］.北京：清華大學出版社，2004. ZHANG Xian-da.Matrix analysis and applications［M］.Beijing：Tsinghua University Press，2004.（in Chinese）

Observability Analysis of Cooperative Localization System for MAUV Based on Condition Number

MA Peng，ZHANG Fu-bin，XU De-min，LIU Shu-qiang
（School of Marine Science and Technology，Northwestern Polytechnical University，Xi'an 710072，Shaanxi，China）

For the multiple autonomous underwater vehicle（MAUV）cooperative localization system based on the moving long baseline，the 2D kinematic models and range measurement equations of MAUV are established.The observability theory of nonlinear system based on Lie derivatives is used to derive the observability matrix of the cooperative localization system and describe the unobservable trajectories of MAUV system.The evaluation function for the observability metric of the localization system is given using the condition number of observability matrix obtained from singular value decomposition（SVD）theory.Finally，a series of simulation experiments are designed to analyze the system observability under the different MAUV formations.The results show that the system observability change with the MAUV formation，which provides an effective reference to improve the performance of cooperative localization system by transforming the MAUV formation.

information processing technology；multiple autonomous underwater vehicle；cooperative localization；condition number；observability

TP391

A

1000-1093（2015）01-0138-06

10.3969/j.issn.1000-1093.2015.01.020

2014-03-24

國家自然科學基金項目（61273333）

馬朋（1987—），男，博士研究生。E-mail：sxhy_mapeng@126.com；張福斌（1972—），男，副教授，碩士生導師。E-mail：zhangfb@nwpu.edu.cn